1 files changed, 31 insertions, 4 deletions

diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index 538bfa8ba9b4..57cb2f00fc07 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -159,6 +159,7 @@ u32 arch_timer_reg_read(int access, enum arch_timer_reg reg,
  * if we don't have the cp15 accessors we won't have a problem.
  */
 u64 (*arch_timer_read_counter)(void) = arch_counter_get_cntvct;
+EXPORT_SYMBOL_GPL(arch_timer_read_counter);
 
 static u64 arch_counter_read(struct clocksource *cs)
 {
@@ -218,6 +219,11 @@ static u32 notrace fsl_a008585_read_cntv_tval_el0(void)
         return __fsl_a008585_read_reg(cntv_tval_el0);
 }
 
+static u64 notrace fsl_a008585_read_cntpct_el0(void)
+{
+        return __fsl_a008585_read_reg(cntpct_el0);
+}
+
 static u64 notrace fsl_a008585_read_cntvct_el0(void)
 {
         return __fsl_a008585_read_reg(cntvct_el0);
@@ -259,6 +265,11 @@ static u32 notrace hisi_161010101_read_cntv_tval_el0(void)
         return __hisi_161010101_read_reg(cntv_tval_el0);
 }
 
+static u64 notrace hisi_161010101_read_cntpct_el0(void)
+{
+        return __hisi_161010101_read_reg(cntpct_el0);
+}
+
 static u64 notrace hisi_161010101_read_cntvct_el0(void)
 {
         return __hisi_161010101_read_reg(cntvct_el0);
@@ -289,6 +300,15 @@ static struct ate_acpi_oem_info hisi_161010101_oem_info[] = {
 #endif
 
 #ifdef CONFIG_ARM64_ERRATUM_858921
+static u64 notrace arm64_858921_read_cntpct_el0(void)
+{
+        u64 old, new;
+
+        old = read_sysreg(cntpct_el0);
+        new = read_sysreg(cntpct_el0);
+        return (((old ^ new) >> 32) & 1) ? old : new;
+}
+
 static u64 notrace arm64_858921_read_cntvct_el0(void)
 {
         u64 old, new;
@@ -310,16 +330,19 @@ static void erratum_set_next_event_tval_generic(const int access, unsigned long
                                                 struct clock_event_device *clk)
 {
         unsigned long ctrl;
-        u64 cval = evt + arch_counter_get_cntvct();
+        u64 cval;
 
         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)
+        if (access == ARCH_TIMER_PHYS_ACCESS) {
+                cval = evt + arch_counter_get_cntpct();
                 write_sysreg(cval, cntp_cval_el0);
-        else
+        } else {
+                cval = evt + arch_counter_get_cntvct();
                 write_sysreg(cval, cntv_cval_el0);
+        }
 
         arch_timer_reg_write(access, ARCH_TIMER_REG_CTRL, ctrl, clk);
 }
@@ -346,6 +369,7 @@ static const struct arch_timer_erratum_workaround ool_workarounds[] = {
                 .desc = "Freescale erratum a005858",
                 .read_cntp_tval_el0 = fsl_a008585_read_cntp_tval_el0,
                 .read_cntv_tval_el0 = fsl_a008585_read_cntv_tval_el0,
+                .read_cntpct_el0 = fsl_a008585_read_cntpct_el0,
                 .read_cntvct_el0 = fsl_a008585_read_cntvct_el0,
                 .set_next_event_phys = erratum_set_next_event_tval_phys,
                 .set_next_event_virt = erratum_set_next_event_tval_virt,
@@ -358,6 +382,7 @@ static const struct arch_timer_erratum_workaround ool_workarounds[] = {
                 .desc = "HiSilicon erratum 161010101",
                 .read_cntp_tval_el0 = hisi_161010101_read_cntp_tval_el0,
                 .read_cntv_tval_el0 = hisi_161010101_read_cntv_tval_el0,
+                .read_cntpct_el0 = hisi_161010101_read_cntpct_el0,
                 .read_cntvct_el0 = hisi_161010101_read_cntvct_el0,
                 .set_next_event_phys = erratum_set_next_event_tval_phys,
                 .set_next_event_virt = erratum_set_next_event_tval_virt,
@@ -368,6 +393,7 @@ static const struct arch_timer_erratum_workaround ool_workarounds[] = {
                 .desc = "HiSilicon erratum 161010101",
                 .read_cntp_tval_el0 = hisi_161010101_read_cntp_tval_el0,
                 .read_cntv_tval_el0 = hisi_161010101_read_cntv_tval_el0,
+                .read_cntpct_el0 = hisi_161010101_read_cntpct_el0,
                 .read_cntvct_el0 = hisi_161010101_read_cntvct_el0,
                 .set_next_event_phys = erratum_set_next_event_tval_phys,
                 .set_next_event_virt = erratum_set_next_event_tval_virt,
@@ -378,6 +404,7 @@ static const struct arch_timer_erratum_workaround ool_workarounds[] = {
                 .match_type = ate_match_local_cap_id,
                 .id = (void *)ARM64_WORKAROUND_858921,
                 .desc = "ARM erratum 858921",
+                .read_cntpct_el0 = arm64_858921_read_cntpct_el0,
                 .read_cntvct_el0 = arm64_858921_read_cntvct_el0,
         },
 #endif
@@ -901,7 +928,7 @@ static void __init arch_counter_register(unsigned type)
 
         /* Register the CP15 based counter if we have one */
         if (type & ARCH_TIMER_TYPE_CP15) {
-                if (IS_ENABLED(CONFIG_ARM64) ||
+                if ((IS_ENABLED(CONFIG_ARM64) && !is_hyp_mode_available()) ||
                     arch_timer_uses_ppi == ARCH_TIMER_VIRT_PPI)
                         arch_timer_read_counter = arch_counter_get_cntvct;
                 else