Refactor race condition free timer helper (#850)

* Implement half_period_counter in rtic-time

* Rename compute_now to calculate_now, use it in stm32 and imxrt

* Add more tests

* Add some docs

* Fix clippy warning, add imxrt timer to monotonics tests

* Bump dependency version to make sure monotonics will build properly

* Add changelog to rtic-monotonics

* Add more docs

* Add more docs

* Finish documentation

* Fix typos

* Switch from atomic-polyfill to portable-atomic

* Some more doc fixes

* More doc fixes

* Minor doc fix

* Minor doc fix

* Fix Atomics not existing

* Fix example

* Minor example improvement

* Revert back to atomic-polyfill

* Fix cargo.toml formatting

* Remove atomic-polyfill

* Attempt to fix unused macro warning

* Remove atomics completely from half period counter

* Minor doc fix

* Doc fixes

* Doc fixes

* Remove obsolete comment

* Fix ordering in monotonic initialization sequence

10 files changed, 350 insertions, 49 deletions

diff --git a/rtic-monotonics/CHANGELOG.md b/rtic-monotonics/CHANGELOG.md
index 4dea3fc5..990da3a1 100644
--- a/rtic-monotonics/CHANGELOG.md
+++ b/rtic-monotonics/CHANGELOG.md
@@ -11,6 +11,10 @@ For each category, *Added*, *Changed*, *Fixed* add new entries at the top!
 
 - **Soundness fix:** Monotonics did not wait long enough in `Duration` based delays.
 
+### Changed
+
+- Bump `rtic-time`
+
 ## v1.3.0 - 2023-11-08
 
 ### Added
diff --git a/rtic-monotonics/Cargo.toml b/rtic-monotonics/Cargo.toml
index 4d9dc0b8..a3ab9bc0 100644
--- a/rtic-monotonics/Cargo.toml
+++ b/rtic-monotonics/Cargo.toml
@@ -27,7 +27,7 @@ features = [
 rustdoc-flags = ["--cfg", "docsrs"]
 
 [dependencies]
-rtic-time = { version = "1.0.0", path = "../rtic-time" }
+rtic-time = { version = "1.1.0", path = "../rtic-time" }
 embedded-hal = { version = "1.0.0-rc.2" }
 embedded-hal-async = { version = "1.0.0-rc.2", optional = true }
 fugit = { version = "0.3.6" }
diff --git a/rtic-monotonics/src/imxrt.rs b/rtic-monotonics/src/imxrt.rs
index 97ba73f8..5f9fc088 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 254f1302..c86005ef 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) {
diff --git a/rtic-time/CHANGELOG.md b/rtic-time/CHANGELOG.md
index cf312ac1..d2e28873 100644
--- a/rtic-time/CHANGELOG.md
+++ b/rtic-time/CHANGELOG.md
@@ -9,7 +9,8 @@ For each category, *Added*, *Changed*, *Fixed* add new entries at the top!
 
 ### Added
 
-- `should_dequeue` to the `Monotonic` trait to handle bugged timers
+- `half_period_counter` containing utilities for implementing a half-period-counter based monotonic.
+- `should_dequeue_check` to the `Monotonic` trait to handle bugged timers.
 
 ### Changed
 
diff --git a/rtic-time/Cargo.toml b/rtic-time/Cargo.toml
index a8379e4a..8d2851ac 100644
--- a/rtic-time/Cargo.toml
+++ b/rtic-time/Cargo.toml
@@ -1,6 +1,6 @@
 [package]
 name = "rtic-time"
-version = "1.0.0"
+version = "1.1.0"
 
 edition = "2021"
 authors = [
diff --git a/rtic-time/src/half_period_counter.rs b/rtic-time/src/half_period_counter.rs
new file mode 100644
index 00000000..5d3bf752
--- /dev/null
+++ b/rtic-time/src/half_period_counter.rs
@@ -0,0 +1,229 @@
+//! Utility to implement a race condition free half-period based monotonic.
+//!
+//! # Background
+//!
+//! Monotonics are continuous and never wrap (in a reasonable amount of time), while
+//! the underlying hardware usually wraps frequently and has interrupts to indicate that
+//! a wrap happened.
+//!
+//! The biggest problem when implementing a monotonic from such hardware is that there exists
+//! a non-trivial race condition while reading data from the timer. Let's assume we increment
+//! a period counter every time an overflow interrupt happens.
+//! Which should we then read first when computing the current time? The period counter or
+//! the timer value?
+//! - When reading the timer value first, an overflow interrupt could happen before we read
+//!   the period counter, causing the calculated time to be much too high
+//! - When reading the period counter first, the timer value could overflow before we
+//!   read it, causing the calculated time to be much too low
+//!
+//! The reason this is non-trivil to solve is because even critical sections do not help
+//! much - the inherent problem here is that the timer value continues to change, and there
+//! is no way to read it together with the period counter in an atomic way.
+//!
+//! # Solution
+//!
+//! This module provides utilities to solve this problem in a reliable, race-condition free way.
+//! A second interrupt must be added at the half-period mark, which effectively converts the period counter
+//! to a half-period counter. This creates one bit of overlap between the
+//! timer value and the period counter, which makes it mathematically possible to solve the
+//! race condition.
+//!
+//! The following steps have to be fulfilled to make this reliable:
+//! - The period counter gets incremented twice per period; once when the timer overflow happens and once
+//!   at the half-period mark. For example, a 16-bit timer would require the period counter to be
+//!   incremented at the values `0x0000` and `0x8000`.
+//! - The timer value and the period counter must be in sync. After the overflow interrupt
+//!   was processed, the period counter must be even, and after the half-way interrupt was
+//!   processed, the period counter must be odd.
+//! - Both the overflow interrupt and the half-way interrupt must be processed within half a
+//!   timer period. This means those interrupts should be the highest priority in the
+//!   system - disabling them for more than half a period will cause the monotonic to misbehave.
+//!
+//! If those conditions are fulfilled, the [`calculate_now`] function will reliably
+//! return the correct time value.
+//!
+//! # Why does this work?
+//!
+//! It's complicated. In essence, this one bit of overlap gets used to make
+//! it irrelevant whether the period counter was already incremented or not.
+//! For example, during the second part of the timer period, it is irrelevant if the
+//! period counter is `2` (before the interrupt) or `3` (after the interrupt) - [`calculate_now`]
+//! will yield the same result. Then half a period later, in the first part of the next timer period,
+//! it is irrelevant if the period counter is `3` or `4` - they again will yield the same result.
+//!
+//! This means that as long as we read the period counter **before** the timer value, we will
+//! always get the correct result, given that the interrupts are not delayed by more than half a period.
+//!
+//! # Example
+//!
+//! This example takes a 16-bit timer and uses a 32-bit period counter
+//! to extend the timer to 47-bit. Note that one bit gets lost because
+//! this method requires the period counter to be increased twice per period.
+//!
+//! The resulting time value is returned as a `u64`.
+//!
+//! ```rust
+//! # fn timer_stop() {}
+//! # fn timer_reset() {}
+//! # fn timer_enable_overflow_interrupt() {}
+//! # fn timer_enable_compare_interrupt(_val: u16) {}
+//! # fn timer_start() {}
+//! # fn overflow_interrupt_happened() -> bool { false }
+//! # fn compare_interrupt_happened() -> bool { false }
+//! # fn clear_overflow_interrupt() {}
+//! # fn clear_compare_interrupt() {}
+//! # fn timer_get_value() -> u16 { 0 }
+//! use core::sync::atomic::{AtomicU32, Ordering};
+//!
+//! static HALF_PERIOD_COUNTER: AtomicU32 = AtomicU32::new(0);
+//!
+//! struct MyMonotonic;
+//!
+//! impl MyMonotonic {
+//!     fn init() {
+//!         timer_stop();
+//!         timer_reset();
+//!         HALF_PERIOD_COUNTER.store(0, Ordering::SeqCst);
+//!         timer_enable_overflow_interrupt();
+//!         timer_enable_compare_interrupt(0x8000);
+//!         // Both the period counter and the timer are reset
+//!         // to zero and the interrupts are enabled.
+//!         // This means the period counter and the timer value
+//!         // are in sync, so we can now enable the timer.
+//!         timer_start();
+//!     }
+//!
+//!     fn on_interrupt() {
+//!         if overflow_interrupt_happened() {
+//!             clear_overflow_interrupt();
+//!             HALF_PERIOD_COUNTER.fetch_add(1, Ordering::Relaxed);
+//!         }
+//!         if compare_interrupt_happened() {
+//!             clear_compare_interrupt();
+//!             HALF_PERIOD_COUNTER.fetch_add(1, Ordering::Relaxed);
+//!         }
+//!     }
+//!
+//!     fn now() -> u64 {
+//!         rtic_time::half_period_counter::calculate_now(
+//!             HALF_PERIOD_COUNTER.load(Ordering::Relaxed),
+//!             || timer_get_value(),
+//!         )
+//!     }
+//! }
+//! ```
+//!
+
+use core::sync::atomic::{compiler_fence, Ordering};
+
+/// The value of the timer's count register.
+pub trait TimerValue {
+    /// Bit size of the timer. Does not need to be a multiple of `8`.
+    const BITS: u32;
+}
+macro_rules! impl_timer_value {
+    ($t:ty) => {
+        impl TimerValue for $t {
+            const BITS: u32 = Self::BITS;
+        }
+    };
+}
+impl_timer_value!(u8);
+impl_timer_value!(u16);
+impl_timer_value!(u32);
+impl_timer_value!(u64);
+
+/// Operations a type has to support
+/// in order to be used as the return value
+/// of [`calculate_now`].
+pub trait TimerOps: Copy {
+    /// All bits set to `1`.
+    const MAX: Self;
+    /// The lowest bit set to `1`.
+    const ONE: Self;
+    /// The `^` operation.
+    fn xor(self, other: Self) -> Self;
+    /// The `&` operation.
+    fn and(self, other: Self) -> Self;
+    /// The `+` operation.
+    fn add(self, other: Self) -> Self;
+    /// The `<<` operation.
+    fn left_shift(self, amount: u32) -> Self;
+}
+
+macro_rules! impl_timer_ops {
+    ($t:ty) => {
+        impl TimerOps for $t {
+            const MAX: Self = Self::MAX;
+            const ONE: Self = 1;
+
+            #[inline]
+            fn xor(self, other: Self) -> Self {
+                self ^ other
+            }
+
+            #[inline]
+            fn and(self, other: Self) -> Self {
+                self & other
+            }
+
+            #[inline]
+            fn add(self, other: Self) -> Self {
+                self + other
+            }
+
+            #[inline]
+            fn left_shift(self, amount: u32) -> Self {
+                self << amount
+            }
+        }
+    };
+}
+
+impl_timer_ops!(u16);
+impl_timer_ops!(u32);
+impl_timer_ops!(u64);
+impl_timer_ops!(u128);
+
+/// Calculates the current time from the half period counter and the timer value.
+///
+/// # Arguments
+///
+/// * `half_periods` - The period counter value. If read from an atomic, can use `Ordering::Relaxed`.
+/// * `timer_value` - A closure/function that when called produces the current timer value.
+pub fn calculate_now<P, T, F, O>(half_periods: P, timer_value: F) -> O
+where
+    T: TimerValue,
+    O: From<P> + From<T> + TimerOps,
+    F: FnOnce() -> T,
+{
+    // Important: half_period **must** be read first.
+    // Otherwise we have another mathematical race condition.
+    let half_periods = O::from(half_periods);
+    compiler_fence(Ordering::Acquire);
+    let timer_value = O::from(timer_value());
+
+    // Credits to the `time-driver` of `embassy-stm32`.
+    //
+    // Given that our clock counter value is 32 bits.
+    //
+    // 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.
+
+    let upper_half = half_periods.left_shift(T::BITS - 1);
+    let lower_half = O::ONE.left_shift(T::BITS - 1).and(upper_half);
+    upper_half.add(lower_half.xor(timer_value))
+}
diff --git a/rtic-time/src/lib.rs b/rtic-time/src/lib.rs
index 4c89d52c..6254bca0 100644
--- a/rtic-time/src/lib.rs
+++ b/rtic-time/src/lib.rs
@@ -19,6 +19,7 @@ use linked_list::{Link, LinkedList};
 pub use monotonic::Monotonic;
 use rtic_common::dropper::OnDrop;
 
+pub mod half_period_counter;
 mod linked_list;
 mod monotonic;
 
diff --git a/rtic-time/tests/half_period_time_counter.rs b/rtic-time/tests/half_period_time_counter.rs
new file mode 100644
index 00000000..ceffbea2
--- /dev/null
+++ b/rtic-time/tests/half_period_time_counter.rs
@@ -0,0 +1,95 @@
+use rtic_time::half_period_counter::calculate_now;
+
+macro_rules! do_test_u8 {
+    ($periods:literal, $counter:literal, $expected:literal) => {{
+        let periods: u32 = $periods;
+        let counter: u8 = $counter;
+        let expected: u32 = $expected;
+        let actual: u32 = calculate_now(periods, || counter);
+        assert_eq!(
+            actual, expected,
+            "Expected: ({} | {}) => {}, got: {}",
+            periods, counter, expected, actual
+        );
+    }};
+}
+
+macro_rules! do_test_u16 {
+    ($periods:literal, $counter:literal, $expected:literal) => {{
+        let periods: u16 = $periods;
+        let counter: u16 = $counter;
+        let expected: u32 = $expected;
+        let actual: u32 = calculate_now(periods, || counter);
+        assert_eq!(
+            actual, expected,
+            "Expected: ({} | {}) => {}, got: {}",
+            periods, counter, expected, actual
+        );
+    }};
+}
+
+#[test]
+fn half_period_time_counter_u8() {
+    do_test_u8!(0, 0, 0);
+    do_test_u8!(0, 1, 1);
+
+    do_test_u8!(0, 126, 126);
+    do_test_u8!(1, 126, 382); // This is why it's important to load the periods before the counter
+    do_test_u8!(0, 127, 127);
+    do_test_u8!(1, 127, 383);
+    do_test_u8!(0, 128, 128);
+    do_test_u8!(1, 128, 128);
+    do_test_u8!(0, 129, 129);
+    do_test_u8!(1, 129, 129);
+
+    do_test_u8!(1, 254, 254);
+    do_test_u8!(2, 254, 510);
+    do_test_u8!(1, 255, 255);
+    do_test_u8!(2, 255, 511);
+    do_test_u8!(1, 0, 256);
+    do_test_u8!(2, 0, 256);
+    do_test_u8!(1, 1, 257);
+    do_test_u8!(2, 1, 257);
+
+    do_test_u8!(2, 126, 382);
+    do_test_u8!(3, 126, 638);
+    do_test_u8!(2, 127, 383);
+    do_test_u8!(3, 127, 639);
+    do_test_u8!(2, 128, 384);
+    do_test_u8!(3, 128, 384);
+    do_test_u8!(2, 129, 385);
+    do_test_u8!(3, 129, 385);
+}
+
+#[test]
+fn half_period_time_counter_u16() {
+    do_test_u16!(0, 0, 0);
+    do_test_u16!(0, 1, 1);
+
+    do_test_u16!(0, 32766, 32766);
+    do_test_u16!(1, 32766, 98302); // This is why it's important to load the periods before the counter
+    do_test_u16!(0, 32767, 32767);
+    do_test_u16!(1, 32767, 98303);
+    do_test_u16!(0, 32768, 32768);
+    do_test_u16!(1, 32768, 32768);
+    do_test_u16!(0, 32769, 32769);
+    do_test_u16!(1, 32769, 32769);
+
+    do_test_u16!(1, 65534, 65534);
+    do_test_u16!(2, 65534, 131070);
+    do_test_u16!(1, 65535, 65535);
+    do_test_u16!(2, 65535, 131071);
+    do_test_u16!(1, 0, 65536);
+    do_test_u16!(2, 0, 65536);
+    do_test_u16!(1, 1, 65537);
+    do_test_u16!(2, 1, 65537);
+
+    do_test_u16!(2, 32766, 98302);
+    do_test_u16!(3, 32766, 163838);
+    do_test_u16!(2, 32767, 98303);
+    do_test_u16!(3, 32767, 163839);
+    do_test_u16!(2, 32768, 98304);
+    do_test_u16!(3, 32768, 98304);
+    do_test_u16!(2, 32769, 98305);
+    do_test_u16!(3, 32769, 98305);
+}
diff --git a/xtask/src/argument_parsing.rs b/xtask/src/argument_parsing.rs
index b4dcd904..5d9c00e5 100644
--- a/xtask/src/argument_parsing.rs
+++ b/xtask/src/argument_parsing.rs
@@ -79,6 +79,7 @@ impl Package {
                         "nrf5340-app,embedded-hal-async",
                         "nrf5340-net,embedded-hal-async",
                         "nrf9160,embedded-hal-async",
+                        "imxrt_gpt1,imxrt-ral/imxrt1062,embedded-hal-async",
                     ][..]
                 };