2 files changed, 16 insertions, 46 deletions

diff --git a/rtic-monotonics/src/imxrt.rs b/rtic-monotonics/src/imxrt.rs
index 97ba73f..5f9fc08 100644
--- a/rtic-monotonics/src/imxrt.rs
+++ b/rtic-monotonics/src/imxrt.rs
@@ -30,8 +30,9 @@
 //! ```
 
 use crate::{Monotonic, TimeoutError, TimerQueue};
-use atomic_polyfill::{compiler_fence, AtomicU32, Ordering};
+use atomic_polyfill::{AtomicU32, Ordering};
 pub use fugit::{self, ExtU64, ExtU64Ceil};
+use rtic_time::half_period_counter::calculate_now;
 
 use imxrt_ral as ral;
 
@@ -73,29 +74,6 @@ macro_rules! create_imxrt_gpt2_token {
     }};
 }
 
-// Credits to the `time-driver` of `embassy-stm32`.
-//
-// Clock timekeeping works with something we call "periods", which are time intervals
-// of 2^31 ticks. The Clock counter value is 32 bits, so one "overflow cycle" is 2 periods.
-//
-// A `period` count is maintained in parallel to the Timer hardware `counter`, like this:
-// - `period` and `counter` start at 0
-// - `period` is incremented on overflow (at counter value 0)
-// - `period` is incremented "midway" between overflows (at counter value 0x8000_0000)
-//
-// Therefore, when `period` is even, counter is in 0..0x7FFF_FFFF. When odd, counter is in 0x8000_0000..0xFFFF_FFFF
-// This allows for now() to return the correct value even if it races an overflow.
-//
-// To get `now()`, `period` is read first, then `counter` is read. If the counter value matches
-// the expected range for the `period` parity, we're done. If it doesn't, this means that
-// a new period start has raced us between reading `period` and `counter`, so we assume the `counter` value
-// corresponds to the next period.
-//
-// `period` is a 32bit integer, so it overflows on 2^32 * 2^31 / 1_000_000 seconds of uptime, which is 292471 years.
-fn calc_now(period: u32, counter: u32) -> u64 {
-    (u64::from(period) << 31) + u64::from(counter ^ ((period & 1) << 31))
-}
-
 macro_rules! make_timer {
     ($mono_name:ident, $timer:ident, $period:ident, $tq:ident$(, doc: ($($doc:tt)*))?) => {
         /// Timer implementing [`Monotonic`] which runs at 1 MHz.
@@ -142,7 +120,7 @@ macro_rules! make_timer {
                 );
 
                 // Reset period
-                $period.store(0, Ordering::Relaxed);
+                $period.store(0, Ordering::SeqCst);
 
                 // Prescaler
                 ral::modify_reg!(ral::gpt, gpt, PR,
@@ -231,12 +209,10 @@ macro_rules! make_timer {
             fn now() -> Self::Instant {
                 let gpt = unsafe{ $timer::instance() };
 
-                // Important: period **must** be read first.
-                let period = $period.load(Ordering::Relaxed);
-                compiler_fence(Ordering::Acquire);
-                let counter = ral::read_reg!(ral::gpt, gpt, CNT);
-
-                Self::Instant::from_ticks(calc_now(period, counter))
+                Self::Instant::from_ticks(calculate_now(
+                    $period.load(Ordering::Relaxed),
+                    || ral::read_reg!(ral::gpt, gpt, CNT)
+                ))
             }
 
             fn set_compare(instant: Self::Instant) {
diff --git a/rtic-monotonics/src/stm32.rs b/rtic-monotonics/src/stm32.rs
index 254f130..c86005e 100644
--- a/rtic-monotonics/src/stm32.rs
+++ b/rtic-monotonics/src/stm32.rs
@@ -35,8 +35,9 @@
 //! ```
 
 use crate::{Monotonic, TimeoutError, TimerQueue};
-use atomic_polyfill::{compiler_fence, AtomicU64, Ordering};
+use atomic_polyfill::{AtomicU64, Ordering};
 pub use fugit::{self, ExtU64, ExtU64Ceil};
+use rtic_time::half_period_counter::calculate_now;
 use stm32_metapac as pac;
 
 mod _generated {
@@ -166,13 +167,14 @@ macro_rules! make_timer {
                 // Since this is not the case, it should be cleared.
                 $timer.sr().modify(|r| r.set_uif(false));
 
+                $tq.initialize(Self {});
+                $overflow.store(0, Ordering::SeqCst);
+
                 // Start the counter.
                 $timer.cr1().modify(|r| {
                     r.set_cen(true);
                 });
 
-                $tq.initialize(Self {});
-
                 // SAFETY: We take full ownership of the peripheral and interrupt vector,
                 // plus we are not using any external shared resources so we won't impact
                 // basepri/source masking based critical sections.
@@ -231,18 +233,10 @@ macro_rules! make_timer {
             const TICK_PERIOD: Self::Duration = Self::Duration::from_ticks(1);
 
             fn now() -> Self::Instant {
-                // Credits to the `time-driver` of `embassy-stm32`.
-                // For more info, see the `imxrt` driver.
-                fn calc_now(period: u64, counter: $bits) -> u64 {
-                    (period << ($bits::BITS - 1)) + u64::from(counter ^ (((period & 1) as $bits) << ($bits::BITS - 1)))
-                }
-
-                // Important: period **must** be read first.
-                let period = $overflow.load(Ordering::Relaxed);
-                compiler_fence(Ordering::Acquire);
-                let counter = $timer.cnt().read().cnt();
-
-                Self::Instant::from_ticks(calc_now(period, counter))
+                Self::Instant::from_ticks(calculate_now(
+                    $overflow.load(Ordering::Relaxed),
+                    || $timer.cnt().read().cnt()
+                ))
             }
 
             fn set_compare(instant: Self::Instant) {