From dccb453157f9ea389a938643ae1dd3fbc4590737 Mon Sep 17 00:00:00 2001
From: Cong Zhang <congzhan@codeaurora.org>
Date: Thu, 12 Mar 2020 11:20:07 +0800
Subject: [PATCH] arch_timer: add workaround for arm erratum 858921

If CNTVCT or CNTPCT are read while they are updated with the lower
32bits rolled-over, it is possible to read a wrong value(both AArch32 or
AArch64). So if the low 32 bits are set, we need to reread the counter.

Change-Id: Ifd9720c8ff6d0622610b8f53d029635b3130280a
Signed-off-by: Cong Zhang <congzhan@codeaurora.org>
---
 arch/arm/include/asm/arch_timer.h    | 31 ++++++++++++++++++++++++++--
 drivers/clocksource/Kconfig          | 10 +++++++++
 drivers/clocksource/arm_arch_timer.c | 21 +++++++++++++++++++
 3 files changed, 60 insertions(+), 2 deletions(-)

diff --git a/arch/arm/include/asm/arch_timer.h b/arch/arm/include/asm/arch_timer.h
index 0a8d7bba2cb0..ed63e83c35eb 100644
--- a/arch/arm/include/asm/arch_timer.h
+++ b/arch/arm/include/asm/arch_timer.h
@@ -11,6 +11,20 @@
 #include <clocksource/arm_arch_timer.h>
 
 #ifdef CONFIG_ARM_ARCH_TIMER
+
+#ifdef CONFIG_ARM_ERRATUM_858921
+DECLARE_PER_CPU(bool, timer_erratum_858921_workaround_enabled);
+static __always_inline bool erratum_858921_workaround_enabled(void)
+{
+	return this_cpu_read(timer_erratum_858921_workaround_enabled);
+}
+#else
+static __always_inline bool erratum_858921_workaround_enabled(void)
+{
+	return false;
+}
+#endif
+
 int arch_timer_arch_init(void);
 
 /*
@@ -79,12 +93,19 @@ static inline u32 arch_timer_get_cntfrq(void)
 	return val;
 }
 
+#define L32_BITS 0x00000000FFFFFFFF
 static inline u64 arch_counter_get_cntpct(void)
 {
 	u64 cval;
 
 	isb();
-	asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (cval));
+	if (erratum_858921_workaround_enabled()) {
+		do {
+			asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r"(cval));
+		} while ((cval & L32_BITS) == L32_BITS);
+	} else {
+		asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (cval));
+	}
 	return cval;
 }
 
@@ -93,7 +114,13 @@ static inline u64 arch_counter_get_cntvct(void)
 	u64 cval;
 
 	isb();
-	asm volatile("mrrc p15, 1, %Q0, %R0, c14" : "=r" (cval));
+	if (erratum_858921_workaround_enabled()) {
+		do {
+			asm volatile("mrrc p15, 1, %Q0, %R0, c14" : "=r"(cval));
+		} while ((cval & L32_BITS) == L32_BITS);
+	} else {
+		asm volatile("mrrc p15, 1, %Q0, %R0, c14" : "=r" (cval));
+	}
 	return cval;
 }
 
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 25e296785cd6..1cdd8363ec23 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -366,6 +366,16 @@ config ARM64_ERRATUM_858921
 	  The workaround will be dynamically enabled when an affected
 	  core is detected.
 
+config ARM_ERRATUM_858921
+	bool "Workaround for Cortex-A73(AArch32) erratum 858921"
+	default y
+	depends on ARM_ARCH_TIMER && ARM
+	help
+	  This option enables a workaround applicable to AArch32 Cortex-A73
+	  (all versions), whose counter may return incorrect values.
+	  The workaround will be dynamically enabled when an affected
+	  core is detected.
+
 config SUN50I_ERRATUM_UNKNOWN1
 	bool "Workaround for Allwinner A64 erratum UNKNOWN1"
 	default y
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index c7f38cf751c2..ebc5c13d8ad0 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -31,6 +31,7 @@
 #include <asm/arch_timer.h>
 #include <asm/traps.h>
 #include <asm/virt.h>
+#include <asm/cputype.h>
 
 #include <clocksource/arm_arch_timer.h>
 
@@ -193,6 +194,25 @@ struct ate_acpi_oem_info {
 	u32 oem_revision;
 };
 
+#ifdef CONFIG_ARM_ERRATUM_858921
+DEFINE_PER_CPU(bool, timer_erratum_858921_workaround_enabled);
+EXPORT_PER_CPU_SYMBOL(timer_erratum_858921_workaround_enabled);
+static void arch_timer_check_858921_workaround(void)
+{
+	unsigned int cpuid_part;
+
+	cpuid_part = read_cpuid_part();
+	if (cpuid_part == ARM_CPU_PART_CORTEX_A73 ||
+	    cpuid_part == QCOM_CPU_PART_KRYO2XX_GOLD) {
+		this_cpu_write(timer_erratum_858921_workaround_enabled, true);
+	} else {
+		this_cpu_write(timer_erratum_858921_workaround_enabled, false);
+	}
+}
+#else
+#define arch_timer_check_858921_workaround()	do { } while (0)
+#endif
+
 #ifdef CONFIG_FSL_ERRATUM_A008585
 /*
  * The number of retries is an arbitrary value well beyond the highest number
@@ -803,6 +823,7 @@ static void __arch_timer_setup(unsigned type,
 		}
 
 		arch_timer_check_ool_workaround(ate_match_local_cap_id, NULL);
+		arch_timer_check_858921_workaround();
 	} else {
 		clk->features |= CLOCK_EVT_FEAT_DYNIRQ;
 		clk->name = "arch_mem_timer";