From 4ae44dd71231abb80a45e8dd1d23127c95d61929 Mon Sep 17 00:00:00 2001
From: Andy Ross <andrew.j.ross@intel.com>
Date: Tue, 23 Feb 2021 21:14:32 -0800
Subject: [PATCH] drivers/timer/hpet: Extend qemu workaround

Qemu when running more than one processor has a known synchronization
bug where counter values read from the HPET (notionally a single
global device) can be seen going "backwards" when read from different
CPUs.

There was a pre-existing workaround in the ISR that knew about this,
but the problem can crop up anywhere the counter value is used.  In
particular I caught it aliasing with the "max_ticks" computation in
z_clock_set_timeout(), where it would cause a rollover and the
resulting negative comparator value would result in no end of
hilarity.

Wrap all access to the counter register with a counter() inline that
(when the workaround is enabled) forces the result to be monotonic by
clamping it to a minimum of one more than the previously read value.

Signed-off-by: Andy Ross <andrew.j.ross@intel.com>
---
 drivers/timer/hpet.c | 60 ++++++++++++++++++++++++++++++--------------
 1 file changed, 41 insertions(+), 19 deletions(-)

diff --git a/drivers/timer/hpet.c b/drivers/timer/hpet.c
index 11279a4f1d68..bbd8341fa6ba 100644
--- a/drivers/timer/hpet.c
+++ b/drivers/timer/hpet.c
@@ -41,18 +41,41 @@ DEVICE_MMIO_TOPLEVEL_STATIC(hpet_regs, DT_DRV_INST(0));
 
 #define MIN_DELAY 1000
 
+/* The clock on Qemu in SMP has been observed going backwards.  This
+ * enables a workaround that forces the clock to be monotonic.
+ */
+#if defined(CONFIG_SMP) && defined(CONFIG_QEMU_TARGET)
+#define GLITCHY_EMU
+#endif
+
 static struct k_spinlock lock;
 static unsigned int max_ticks;
 static unsigned int cyc_per_tick;
 static unsigned int last_count;
 
+#ifdef GLITCHY_EMU
+volatile static int32_t last_counter;
+#endif
+
+static ALWAYS_INLINE uint32_t counter(void)
+{
+	int32_t now = MAIN_COUNTER_REG;
+#ifdef GLITCHY_EMU
+	if ((now - last_counter) < 0) {
+		now = last_counter + 1;
+	}
+	last_counter = now;
+#endif
+	return now;
+}
+
 static void hpet_isr(const void *arg)
 {
 	ARG_UNUSED(arg);
 
 	k_spinlock_key_t key = k_spin_lock(&lock);
 
-	uint32_t now = MAIN_COUNTER_REG;
+	uint32_t now = counter();
 
 #if ((DT_INST_IRQ(0, sense) & IRQ_TYPE_LEVEL) == IRQ_TYPE_LEVEL)
 	/*
@@ -63,19 +86,6 @@ static void hpet_isr(const void *arg)
 	INTR_STATUS_REG = TIMER0_INT_STS;
 #endif
 
-	if (IS_ENABLED(CONFIG_SMP) &&
-	    IS_ENABLED(CONFIG_QEMU_TARGET)) {
-		/* Qemu in SMP mode has observed the clock going
-		 * "backwards" relative to interrupts already received
-		 * on the other CPU, despite the HPET being
-		 * theoretically a global device.
-		 */
-		int32_t diff = (int32_t)(now - last_count);
-
-		if (last_count && diff < 0) {
-			now = last_count;
-		}
-	}
 	uint32_t dticks = (now - last_count) / cyc_per_tick;
 
 	last_count += dticks * cyc_per_tick;
@@ -138,10 +148,14 @@ int z_clock_driver_init(const struct device *device)
 	TIMER0_CONF_REG |= TCONF_MODE32;
 
 	max_ticks = (0x7fffffff - cyc_per_tick) / cyc_per_tick;
-	last_count = MAIN_COUNTER_REG;
+	last_count = counter();
+#ifdef GLITCHY_EMU
+	last_counter = last_count;
+	max_ticks -= 20;
+#endif
 
 	TIMER0_CONF_REG |= TCONF_INT_ENABLE;
-	TIMER0_COMPARATOR_REG = MAIN_COUNTER_REG + cyc_per_tick;
+	TIMER0_COMPARATOR_REG = counter() + cyc_per_tick;
 
 	return 0;
 }
@@ -168,7 +182,7 @@ void z_clock_set_timeout(int32_t ticks, bool idle)
 	ticks = CLAMP(ticks - 1, 0, (int32_t)max_ticks);
 
 	k_spinlock_key_t key = k_spin_lock(&lock);
-	uint32_t now = MAIN_COUNTER_REG, cyc, adj;
+	uint32_t now = counter(), cyc, adj;
 	uint32_t max_cyc = max_ticks * cyc_per_tick;
 
 	/* Round up to next tick boundary. */
@@ -198,7 +212,7 @@ uint32_t z_clock_elapsed(void)
 	}
 
 	k_spinlock_key_t key = k_spin_lock(&lock);
-	uint32_t ret = (MAIN_COUNTER_REG - last_count) / cyc_per_tick;
+	uint32_t ret = (counter() - last_count) / cyc_per_tick;
 
 	k_spin_unlock(&lock, key);
 	return ret;
@@ -206,7 +220,15 @@ uint32_t z_clock_elapsed(void)
 
 uint32_t z_timer_cycle_get_32(void)
 {
-	return MAIN_COUNTER_REG;
+#ifdef GLITCHY_EMU
+	k_spinlock_key_t key = k_spin_lock(&lock);
+	uint32_t ret = counter();
+
+	k_spin_unlock(&lock, key);
+	return ret;
+#else
+	return counter();
+#endif
 }
 
 void z_clock_idle_exit(void)