1 files changed, 55 insertions, 27 deletions

diff --git a/arch/arm64/include/asm/arch_timer.h b/arch/arm64/include/asm/arch_timer.h
index fbe0ca31a99c..eaa5bbe3fa87 100644
--- a/arch/arm64/include/asm/arch_timer.h
+++ b/arch/arm64/include/asm/arch_timer.h
@@ -20,13 +20,55 @@
 #define __ASM_ARCH_TIMER_H
 
 #include <asm/barrier.h>
+#include <asm/sysreg.h>
 
 #include <linux/bug.h>
 #include <linux/init.h>
+#include <linux/jump_label.h>
 #include <linux/types.h>
 
 #include <clocksource/arm_arch_timer.h>
 
+#if IS_ENABLED(CONFIG_FSL_ERRATUM_A008585)
+extern struct static_key_false arch_timer_read_ool_enabled;
+#define needs_fsl_a008585_workaround() \
+        static_branch_unlikely(&arch_timer_read_ool_enabled)
+#else
+#define needs_fsl_a008585_workaround()  false
+#endif
+
+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;                                       \
+        if (needs_fsl_a008585_workaround())             \
+                _val = __fsl_a008585_read_##reg();      \
+        else                                            \
+                _val = read_sysreg(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
@@ -38,19 +80,19 @@ void arch_timer_reg_write_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("msr cntp_ctl_el0,  %0" : : "r" (val));
+                        write_sysreg(val, cntp_ctl_el0);
                         break;
                 case ARCH_TIMER_REG_TVAL:
-                        asm volatile("msr cntp_tval_el0, %0" : : "r" (val));
+                        write_sysreg(val, cntp_tval_el0);
                         break;
                 }
         } else if (access == ARCH_TIMER_VIRT_ACCESS) {
                 switch (reg) {
                 case ARCH_TIMER_REG_CTRL:
-                        asm volatile("msr cntv_ctl_el0,  %0" : : "r" (val));
+                        write_sysreg(val, cntv_ctl_el0);
                         break;
                 case ARCH_TIMER_REG_TVAL:
-                        asm volatile("msr cntv_tval_el0, %0" : : "r" (val));
+                        write_sysreg(val, cntv_tval_el0);
                         break;
                 }
         }
@@ -61,48 +103,38 @@ 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)
 {
-        u32 val;
-        asm volatile("mrs %0,   cntfrq_el0" : "=r" (val));
-        return val;
+        return read_sysreg(cntfrq_el0);
 }
 
 static inline u32 arch_timer_get_cntkctl(void)
 {
-        u32 cntkctl;
-        asm volatile("mrs       %0, cntkctl_el1" : "=r" (cntkctl));
-        return cntkctl;
+        return read_sysreg(cntkctl_el1);
 }
 
 static inline void arch_timer_set_cntkctl(u32 cntkctl)
 {
-        asm volatile("msr       cntkctl_el1, %0" : : "r" (cntkctl));
+        write_sysreg(cntkctl, cntkctl_el1);
 }
 
 static inline u64 arch_counter_get_cntpct(void)
@@ -116,12 +148,8 @@ static inline u64 arch_counter_get_cntpct(void)
 
 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);
 }
 
 static inline int arch_timer_arch_init(void)