From 731f32f066aacf0ac2db95b9f66d23063aabbae6 Mon Sep 17 00:00:00 2001
From: Tyler Holmes <TylerDanielHolmes@gmail.com>
Date: Fri, 1 Jul 2022 21:40:01 -0500
Subject: [PATCH] Fix potential rollover edgecase in SysTick extended mode
 (#13)

Previously it was possible for us to have read a SYSTICK value just
before overflow, have the rollover happen & exception fire, incrementing
ROLLOVER_COUNT, resulting in a count off by a full 2**24. Now, we do two
reads of the systick counter with ROLLOVER_COUNT read between them. We
can now check if a rollover happened sometime between the two reads. If
it has, we know to use the second reading and re-read ROLLOVER_COUNT to
make sure we have the incremented value. If we didn't see any rollover,
we're happy and use the first readings of the SYSTICK and
ROLLOVER_COUNT.
---
 ep-systick/src/lib.rs | 49 ++++++++++++++++++++++++++++++-------------
 1 file changed, 35 insertions(+), 14 deletions(-)

diff --git a/ep-systick/src/lib.rs b/ep-systick/src/lib.rs
index e3c3224..94608c8 100644
--- a/ep-systick/src/lib.rs
+++ b/ep-systick/src/lib.rs
@@ -54,12 +54,18 @@
 use cortex_m::peripheral::{syst::SystClkSource, SYST};
 use embedded_profiling::{EPContainer, EPInstant, EPInstantGeneric, EPSnapshot, EmbeddedProfiler};
 
+#[cfg(feature = "extended")]
+use core::sync::atomic::{AtomicU32, Ordering};
+
 #[cfg(feature = "extended")]
 /// Tracker of `systick` cycle count overflows to extend systick's 24 bit timer
-static ROLLOVER_COUNT: core::sync::atomic::AtomicU32 = core::sync::atomic::AtomicU32::new(0);
+static ROLLOVER_COUNT: AtomicU32 = AtomicU32::new(0);
 
 /// The reload value of the [`systick`](cortex_m::peripheral::SYST) peripheral. Also is the max it can go (2**24).
 const SYSTICK_RELOAD: u32 = 0x00FF_FFFF;
+/// the resolution of [`systick`](cortex_m::peripheral::SYST), 2**24
+#[cfg(feature = "extended")]
+const SYSTICK_RESOLUTION: EPContainer = 0x0100_0000;
 
 /// [`systick`](cortex_m::peripheral::SYST) implementation of [`EmbeddedProfiler`].
 ///
@@ -97,20 +103,35 @@ impl<const FREQ: u32> SysTickProfiler<FREQ> {
 
 impl<const FREQ: u32> EmbeddedProfiler for SysTickProfiler<FREQ> {
     fn read_clock(&self) -> EPInstant {
-        let raw_ticks = SYST::get_current();
-        #[allow(unused_mut)]
-        let mut count = EPContainer::from(SYSTICK_RELOAD - raw_ticks);
+        // Read SYSTICK count and maybe account for rollovers
+        let count = {
+            #[cfg(feature = "extended")]
+            {
+                // read the clock & ROLLOVER_COUNT. We read `SYST` twice because we need to detect
+                // if we've rolled over, and if we have make sure we have the right value for ROLLOVER_COUNT.
+                let first = SYST::get_current();
+                let rollover_count: EPContainer = ROLLOVER_COUNT.load(Ordering::Acquire).into();
+                let second = SYST::get_current();
 
-        #[cfg(feature = "extended")]
-        {
-            /// the resolution of [`systick`](cortex_m::peripheral::SYST), 2**24
-            const SYSTICK_RESOLUTION: EPContainer = 16_777_216;
+                // Since the SYSTICK counter is a count down timer, check if first is larger than second
+                if first > second {
+                    // The usual case. We did not roll over between the first and second reading,
+                    // and because of that we also know we got a valid read on ROLLOVER_COUNT.
+                    rollover_count * SYSTICK_RESOLUTION + EPContainer::from(SYSTICK_RELOAD - first)
+                } else {
+                    // we rolled over sometime between the first and second read. We may or may not have
+                    // caught the right ROLLOVER_COUNT, so grab that again and then use the second reading.
+                    let rollover_count: EPContainer = ROLLOVER_COUNT.load(Ordering::Acquire).into();
+                    rollover_count * SYSTICK_RESOLUTION + EPContainer::from(SYSTICK_RELOAD - second)
+                }
+            }
 
-            // add on the number of times we've rolled over (systick resolution is 2**24)
-            let rollover_count =
-                EPContainer::from(ROLLOVER_COUNT.load(core::sync::atomic::Ordering::Acquire));
-            count += rollover_count * SYSTICK_RESOLUTION;
-        }
+            #[cfg(not(feature = "extended"))]
+            {
+                // We aren't trying to be fancy here, we don't care if this rolled over from the last read.
+                EPContainer::from(SYSTICK_RELOAD - SYST::get_current())
+            }
+        };
 
         embedded_profiling::convert_instant(EPInstantGeneric::<1, FREQ>::from_ticks(count))
     }
@@ -127,5 +148,5 @@ use cortex_m_rt::exception;
 #[exception]
 #[allow(non_snake_case)]
 fn SysTick() {
-    ROLLOVER_COUNT.fetch_add(1, core::sync::atomic::Ordering::Relaxed);
+    ROLLOVER_COUNT.fetch_add(1, Ordering::Release);
 }