2 files changed, 114 insertions, 0 deletions

diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 567788664723..8a753fd5b79d 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -305,6 +305,16 @@ config ARM_ARCH_TIMER_EVTSTREAM
           This must be disabled for hardware validation purposes to detect any
           hardware anomalies of missing events.
 
+config FSL_ERRATUM_A008585
+        bool "Workaround for Freescale/NXP Erratum A-008585"
+        default y
+        depends on ARM_ARCH_TIMER && ARM64
+        help
+          This option enables a workaround for Freescale/NXP Erratum
+          A-008585 ("ARM generic timer may contain an erroneous
+          value").  The workaround will only be active if the
+          fsl,erratum-a008585 property is found in the timer node.
+
 config ARM_GLOBAL_TIMER
         bool "Support for the ARM global timer" if COMPILE_TEST
         select CLKSRC_OF if OF
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index 57700541f951..eb5fb4121ac8 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -94,6 +94,43 @@ early_param("clocksource.arm_arch_timer.evtstrm", early_evtstrm_cfg);
  * Architected system timer support.
  */
 
+#ifdef CONFIG_FSL_ERRATUM_A008585
+DEFINE_STATIC_KEY_FALSE(arch_timer_read_ool_enabled);
+EXPORT_SYMBOL_GPL(arch_timer_read_ool_enabled);
+
+static int fsl_a008585_enable = -1;
+
+static int __init early_fsl_a008585_cfg(char *buf)
+{
+        int ret;
+        bool val;
+
+        ret = strtobool(buf, &val);
+        if (ret)
+                return ret;
+
+        fsl_a008585_enable = val;
+        return 0;
+}
+early_param("clocksource.arm_arch_timer.fsl-a008585", early_fsl_a008585_cfg);
+
+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);
+#endif /* CONFIG_FSL_ERRATUM_A008585 */
+
 static __always_inline
 void arch_timer_reg_write(int access, enum arch_timer_reg reg, u32 val,
                           struct clock_event_device *clk)
@@ -243,6 +280,40 @@ static __always_inline void set_next_event(const int access, unsigned long evt,
         arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
 }
 
+#ifdef CONFIG_FSL_ERRATUM_A008585
+static __always_inline void fsl_a008585_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;
+
+        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);
+}
+
+static int fsl_a008585_set_next_event_virt(unsigned long evt,
+                                           struct clock_event_device *clk)
+{
+        fsl_a008585_set_next_event(ARCH_TIMER_VIRT_ACCESS, evt, clk);
+        return 0;
+}
+
+static int fsl_a008585_set_next_event_phys(unsigned long evt,
+                                           struct clock_event_device *clk)
+{
+        fsl_a008585_set_next_event(ARCH_TIMER_PHYS_ACCESS, evt, clk);
+        return 0;
+}
+#endif /* CONFIG_FSL_ERRATUM_A008585 */
+
 static int arch_timer_set_next_event_virt(unsigned long evt,
                                           struct clock_event_device *clk)
 {
@@ -271,6 +342,19 @@ static int arch_timer_set_next_event_phys_mem(unsigned long evt,
         return 0;
 }
 
+static void fsl_a008585_set_sne(struct clock_event_device *clk)
+{
+#ifdef CONFIG_FSL_ERRATUM_A008585
+        if (!static_branch_unlikely(&arch_timer_read_ool_enabled))
+                return;
+
+        if (arch_timer_uses_ppi == VIRT_PPI)
+                clk->set_next_event = fsl_a008585_set_next_event_virt;
+        else
+                clk->set_next_event = fsl_a008585_set_next_event_phys;
+#endif
+}
+
 static void __arch_timer_setup(unsigned type,
                                struct clock_event_device *clk)
 {
@@ -299,6 +383,8 @@ static void __arch_timer_setup(unsigned type,
                 default:
                         BUG();
                 }
+
+                fsl_a008585_set_sne(clk);
         } else {
                 clk->features |= CLOCK_EVT_FEAT_DYNIRQ;
                 clk->name = "arch_mem_timer";
@@ -515,6 +601,15 @@ static void __init arch_counter_register(unsigned type)
                         arch_timer_read_counter = arch_counter_get_cntvct;
                 else
                         arch_timer_read_counter = arch_counter_get_cntpct;
+
+#ifdef CONFIG_FSL_ERRATUM_A008585
+                /*
+                 * Don't use the vdso fastpath if errata require using
+                 * the out-of-line counter accessor.
+                 */
+                if (static_branch_unlikely(&arch_timer_read_ool_enabled))
+                        clocksource_counter.name = "arch_sys_counter_ool";
+#endif
         } else {
                 arch_timer_read_counter = arch_counter_get_cntvct_mem;
 
@@ -800,6 +895,15 @@ static int __init arch_timer_of_init(struct device_node *np)
 
         arch_timer_c3stop = !of_property_read_bool(np, "always-on");
 
+#ifdef CONFIG_FSL_ERRATUM_A008585
+        if (fsl_a008585_enable < 0)
+                fsl_a008585_enable = of_property_read_bool(np, "fsl,erratum-a008585");
+        if (fsl_a008585_enable) {
+                static_branch_enable(&arch_timer_read_ool_enabled);
+                pr_info("Enabling workaround for FSL erratum A-008585\n");
+        }
+#endif
+
         /*
          * If we cannot rely on firmware initializing the timer registers then
          * we should use the physical timers instead.