diff --git a/arch/arm64/include/asm/arch_timer.h b/arch/arm64/include/asm/arch_timer.h
index f298ce1a..57eb84e2 100755
--- a/arch/arm64/include/asm/arch_timer.h
+++ b/arch/arm64/include/asm/arch_timer.h
@@ -23,9 +23,51 @@
#include <linux/init.h>
#include <linux/types.h>
+#include <linux/stringify.h>
#include <clocksource/arm_arch_timer.h>
+#define read_sysreg(r) ({ \
+ u64 __val; \
+ asm volatile("mrs %0, " __stringify(r) : "=r" (__val)); \
+ __val; \
+})
+
+#define write_sysreg(v, r) do { \
+ u64 __val = (u64)v; \
+ asm volatile("msr " __stringify(r) ", %x0" \
+ : : "rZ" (__val)); \
+} while (0)
+
+u32 __fsl_a008585_read_cntp_tval_el0(void);
+u32 __fsl_a008585_read_cntv_tval_el0(void);
+u64 __fsl_a008585_read_cntvct_el0(void);
+
+/*
+ * The number of retries is an arbitrary value well beyond the highest number
+ * of iterations the loop has been observed to take.
+ */
+#define __fsl_a008585_read_reg(reg) ({ \
+ u64 _old, _new; \
+ int _retries = 200; \
+ \
+ do { \
+ _old = read_sysreg(reg); \
+ _new = read_sysreg(reg); \
+ _retries--; \
+ } while (unlikely(_old != _new) && _retries); \
+ \
+ WARN_ON_ONCE(!_retries); \
+ _new; \
+})
+
+#define arch_timer_reg_read_stable(reg) \
+({ \
+ u64 _val; \
+ _val = __fsl_a008585_read_##reg(); \
+ _val; \
+})
+
/*
* These register accessors are marked inline so the compiler can
* nicely work out which register we want, and chuck away the rest of
@@ -60,29 +102,23 @@ void arch_timer_reg_write_cp15(int access, enum arch_timer_reg reg, u32 val)
static __always_inline
u32 arch_timer_reg_read_cp15(int access, enum arch_timer_reg reg)
{
- u32 val;
-
if (access == ARCH_TIMER_PHYS_ACCESS) {
switch (reg) {
case ARCH_TIMER_REG_CTRL:
- asm volatile("mrs %0, cntp_ctl_el0" : "=r" (val));
- break;
+ return read_sysreg(cntp_ctl_el0);
case ARCH_TIMER_REG_TVAL:
- asm volatile("mrs %0, cntp_tval_el0" : "=r" (val));
- break;
+ return arch_timer_reg_read_stable(cntp_tval_el0);
}
} else if (access == ARCH_TIMER_VIRT_ACCESS) {
switch (reg) {
case ARCH_TIMER_REG_CTRL:
- asm volatile("mrs %0, cntv_ctl_el0" : "=r" (val));
- break;
+ return read_sysreg(cntv_ctl_el0);
case ARCH_TIMER_REG_TVAL:
- asm volatile("mrs %0, cntv_tval_el0" : "=r" (val));
- break;
+ return arch_timer_reg_read_stable(cntv_tval_el0);
}
}
- return val;
+ BUG();
}
static inline u32 arch_timer_get_cntfrq(void)
@@ -135,48 +171,11 @@ static inline void arch_timer_evtstrm_enable(int divider)
#endif
}
-#ifdef CONFIG_ARCH_SUN50I
-#define ARCH_VCNT_TRY_MAX_TIME (8)
-#define ARCH_VCNT_MAX_DELTA (8)
-static inline u64 arch_counter_get_cntvct(void)
-{
- u64 cval0;
- u64 cval1;
- u64 delta;
- u32 retry = 0;
-
- /* sun50i vcnt maybe imprecise,
- * we should try to fix this.
- */
- while (retry < ARCH_VCNT_TRY_MAX_TIME) {
- isb();
- asm volatile("mrs %0, cntvct_el0" : "=r" (cval0));
- isb();
- asm volatile("mrs %0, cntvct_el0" : "=r" (cval1));
- delta = cval1 - cval0;
- if ((cval1 >= cval0) && (delta < ARCH_VCNT_MAX_DELTA)) {
- /* read valid vcnt */
- return cval1;
- }
- /* vcnt value error, try again */
- retry++;
- }
- /* Do not warry for this, just return the last time vcnt.
- * arm64 have enabled CONFIG_CLOCKSOURCE_VALIDATE_LAST_CYCLE.
- */
- return cval1;
-}
-#else
static inline u64 arch_counter_get_cntvct(void)
{
- u64 cval;
-
isb();
- asm volatile("mrs %0, cntvct_el0" : "=r" (cval));
-
- return cval;
+ return arch_timer_reg_read_stable(cntvct_el0);
}
-#endif /* CONFIG_ARCH_SUN50I */
static inline int arch_timer_arch_init(void)
{
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index 4cf3c47e..d01a93bd 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -192,6 +192,22 @@ static __always_inline void timer_set_mode(const int access, int mode,
}
}
+u32 __fsl_a008585_read_cntp_tval_el0(void)
+{
+ return __fsl_a008585_read_reg(cntp_tval_el0);
+}
+
+u32 __fsl_a008585_read_cntv_tval_el0(void)
+{
+ return __fsl_a008585_read_reg(cntv_tval_el0);
+}
+
+u64 __fsl_a008585_read_cntvct_el0(void)
+{
+ return __fsl_a008585_read_reg(cntvct_el0);
+}
+EXPORT_SYMBOL(__fsl_a008585_read_cntvct_el0);
+
static void arch_timer_set_mode_virt(enum clock_event_mode mode,
struct clock_event_device *clk)
{
@@ -215,49 +231,23 @@ static void arch_timer_set_mode_phys_mem(enum clock_event_mode mode,
{
timer_set_mode(ARCH_TIMER_MEM_PHYS_ACCESS, mode, clk);
}
-#ifdef CONFIG_ARCH_SUN50I
-#define ARCH_TVAL_TRY_MAX_TIME (8)
-static __always_inline void set_next_event(const int access, unsigned long evt,
- struct clock_event_device *clk)
-{
- unsigned int retry = 0;
- unsigned long ctrl;
- unsigned long tval;
- ctrl = arch_timer_reg_read(access, ARCH_TIMER_REG_CTRL, clk);
- ctrl |= ARCH_TIMER_CTRL_ENABLE;
- ctrl &= ~ARCH_TIMER_CTRL_IT_MASK;
-
- /* sun50i timer maybe imprecise,
- * we should try to fix this.
- */
- while (retry < ARCH_VCNT_TRY_MAX_TIME) {
- arch_timer_reg_write(access, ARCH_TIMER_REG_TVAL, evt, clk);
- tval = arch_timer_reg_read(access, ARCH_TIMER_REG_TVAL, clk);
- if (tval <= evt) {
- /* set tval succeeded, let timer running */
- arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
- return;
- }
- /* tval set value error, try again */
- retry++;
- }
- /* set tval fail, just let timer running */
- printk("notice: set tval failed.\n");
- arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
-}
-#else
static __always_inline void set_next_event(const int access, unsigned long evt,
struct clock_event_device *clk)
{
unsigned long ctrl;
+ u64 cval = evt + arch_counter_get_cntvct();
ctrl = arch_timer_reg_read(access, ARCH_TIMER_REG_CTRL, clk);
ctrl |= ARCH_TIMER_CTRL_ENABLE;
ctrl &= ~ARCH_TIMER_CTRL_IT_MASK;
- arch_timer_reg_write(access, ARCH_TIMER_REG_TVAL, evt, clk);
+
+ if (access == ARCH_TIMER_PHYS_ACCESS)
+ write_sysreg(cval, cntp_cval_el0);
+ else if (access == ARCH_TIMER_VIRT_ACCESS)
+ write_sysreg(cval, cntv_cval_el0);
+
arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
}
-#endif /* CONFIG_ARCH_SUN50I */
static int arch_timer_set_next_event_virt(unsigned long evt,
struct clock_event_device *clk)
@@ -432,7 +422,7 @@ static cycle_t arch_counter_read_cc(const struct cyclecounter *cc)
}
static struct clocksource clocksource_counter = {
- .name = "arch_sys_counter",
+ .name = "arch_sys_counter_ool",
.rating = 400,
.read = arch_counter_read,
.mask = CLOCKSOURCE_MASK(56),