6 files changed, 499 insertions, 12 deletions

diff --git a/rtic-time/CHANGELOG.md b/rtic-time/CHANGELOG.md
index 2e7bebc..cf312ac 100644
--- a/rtic-time/CHANGELOG.md
+++ b/rtic-time/CHANGELOG.md
@@ -15,6 +15,7 @@ For each category, *Added*, *Changed*, *Fixed* add new entries at the top!
 
 ### Fixed
 
+- **Soundness fix:** `TimerQueue` did not wait long enough in `Duration` based delays. Fixing this sadly required adding a `const TICK_PERIOD` to the `Monotonic` trait, which requires updating all existing implementations.
 - If the queue was non-empty and a new instant was added that was earlier than `head`, then the queue would no pend the monotonic handler. This would cause the new `head` to be dequeued at the wrong time.
 
 ## [v1.0.0] - 2023-05-31
diff --git a/rtic-time/Cargo.toml b/rtic-time/Cargo.toml
index ef7635e..a8379e4 100644
--- a/rtic-time/Cargo.toml
+++ b/rtic-time/Cargo.toml
@@ -22,5 +22,9 @@ futures-util = { version = "0.3.25", default-features = false }
 rtic-common = { version = "1.0.0", path = "../rtic-common" }
 
 [dev-dependencies]
+embedded-hal = { version = "1.0.0-rc.2" }
+embedded-hal-async = { version = "1.0.0-rc.2" }
+fugit = "0.3.7"
 parking_lot = "0.12"
 cassette = "0.2"
+cooked-waker = "5.0.0"
diff --git a/rtic-time/src/lib.rs b/rtic-time/src/lib.rs
index 0c3e349..4c89d52 100644
--- a/rtic-time/src/lib.rs
+++ b/rtic-time/src/lib.rs
@@ -181,22 +181,36 @@ impl<Mono: Monotonic> TimerQueue<Mono> {
         }
     }
 
-    /// Timeout after a specific duration.
+    /// Timeout after at least a specific duration.
     #[inline]
     pub async fn timeout_after<F: Future>(
         &self,
         duration: Mono::Duration,
         future: F,
     ) -> Result<F::Output, TimeoutError> {
-        self.timeout_at(Mono::now() + duration, future).await
+        let now = Mono::now();
+        let mut timeout = now + duration;
+        if now != timeout {
+            timeout = timeout + Mono::TICK_PERIOD;
+        }
+
+        // Wait for one period longer, because by definition timers have an uncertainty
+        // of one period, so waiting for 'at least' needs to compensate for that.
+        self.timeout_at(timeout, future).await
     }
 
-    /// Delay for some duration of time.
+    /// Delay for at least some duration of time.
     #[inline]
     pub async fn delay(&self, duration: Mono::Duration) {
         let now = Mono::now();
+        let mut timeout = now + duration;
+        if now != timeout {
+            timeout = timeout + Mono::TICK_PERIOD;
+        }
 
-        self.delay_until(now + duration).await;
+        // Wait for one period longer, because by definition timers have an uncertainty
+        // of one period, so waiting for 'at least' needs to compensate for that.
+        self.delay_until(timeout).await;
     }
 
     /// Delay to some specific time instant.
diff --git a/rtic-time/src/monotonic.rs b/rtic-time/src/monotonic.rs
index ad79bf2..0c0d6f3 100644
--- a/rtic-time/src/monotonic.rs
+++ b/rtic-time/src/monotonic.rs
@@ -10,6 +10,9 @@ pub trait Monotonic {
     /// The time at time zero.
     const ZERO: Self::Instant;
 
+    /// The duration between two timer ticks.
+    const TICK_PERIOD: Self::Duration;
+
     /// The type for instant, defining an instant in time.
     ///
     /// **Note:** In all APIs in RTIC that use instants from this monotonic, this type will be used.
@@ -65,3 +68,153 @@ pub trait Monotonic {
     /// NOTE: This may be called more than once.
     fn disable_timer() {}
 }
+
+/// Creates impl blocks for `embedded_hal::delay::DelayUs`,
+/// based on `fugit::ExtU64Ceil`.
+#[macro_export]
+macro_rules! embedded_hal_delay_impl_fugit64 {
+    ($t:ty) => {
+        impl ::embedded_hal::delay::DelayNs for $t {
+            fn delay_ns(&mut self, ns: u32) {
+                use ::fugit::ExtU64Ceil;
+
+                let now = Self::now();
+                let mut done = now + u64::from(ns).nanos_at_least();
+                if now != done {
+                    // Compensate for sub-tick uncertainty
+                    done += Self::TICK_PERIOD;
+                }
+
+                while Self::now() < done {}
+            }
+
+            fn delay_us(&mut self, us: u32) {
+                use ::fugit::ExtU64Ceil;
+
+                let now = Self::now();
+                let mut done = now + u64::from(us).micros_at_least();
+                if now != done {
+                    // Compensate for sub-tick uncertainty
+                    done += Self::TICK_PERIOD;
+                }
+
+                while Self::now() < done {}
+            }
+
+            fn delay_ms(&mut self, ms: u32) {
+                use ::fugit::ExtU64Ceil;
+
+                let now = Self::now();
+                let mut done = now + u64::from(ms).millis_at_least();
+                if now != done {
+                    // Compensate for sub-tick uncertainty
+                    done += Self::TICK_PERIOD;
+                }
+
+                while Self::now() < done {}
+            }
+        }
+    };
+}
+
+/// Creates impl blocks for `embedded_hal_async::delay::DelayUs`,
+/// based on `fugit::ExtU64Ceil`.
+#[macro_export]
+macro_rules! embedded_hal_async_delay_impl_fugit64 {
+    ($t:ty) => {
+        impl ::embedded_hal_async::delay::DelayNs for $t {
+            #[inline]
+            async fn delay_ns(&mut self, ns: u32) {
+                use ::fugit::ExtU64Ceil;
+                Self::delay(u64::from(ns).nanos_at_least()).await;
+            }
+
+            #[inline]
+            async fn delay_us(&mut self, us: u32) {
+                use ::fugit::ExtU64Ceil;
+                Self::delay(u64::from(us).micros_at_least()).await;
+            }
+
+            #[inline]
+            async fn delay_ms(&mut self, ms: u32) {
+                use ::fugit::ExtU64Ceil;
+                Self::delay(u64::from(ms).millis_at_least()).await;
+            }
+        }
+    };
+}
+
+/// Creates impl blocks for `embedded_hal::delay::DelayUs`,
+/// based on `fugit::ExtU32Ceil`.
+#[macro_export]
+macro_rules! embedded_hal_delay_impl_fugit32 {
+    ($t:ty) => {
+        impl ::embedded_hal::delay::DelayNs for $t {
+            fn delay_ns(&mut self, ns: u32) {
+                use ::fugit::ExtU32Ceil;
+
+                let now = Self::now();
+                let mut done = now + ns.nanos_at_least();
+                if now != done {
+                    // Compensate for sub-tick uncertainty
+                    done += Self::TICK_PERIOD;
+                }
+
+                while Self::now() < done {}
+            }
+
+            fn delay_us(&mut self, us: u32) {
+                use ::fugit::ExtU32Ceil;
+
+                let now = Self::now();
+                let mut done = now + us.micros_at_least();
+                if now != done {
+                    // Compensate for sub-tick uncertainty
+                    done += Self::TICK_PERIOD;
+                }
+
+                while Self::now() < done {}
+            }
+
+            fn delay_ms(&mut self, ms: u32) {
+                use ::fugit::ExtU32Ceil;
+
+                let now = Self::now();
+                let mut done = now + ms.millis_at_least();
+                if now != done {
+                    // Compensate for sub-tick uncertainty
+                    done += Self::TICK_PERIOD;
+                }
+
+                while Self::now() < done {}
+            }
+        }
+    };
+}
+
+/// Creates impl blocks for `embedded_hal_async::delay::DelayUs`,
+/// based on `fugit::ExtU32Ceil`.
+#[macro_export]
+macro_rules! embedded_hal_async_delay_impl_fugit32 {
+    ($t:ty) => {
+        impl ::embedded_hal_async::delay::DelayNs for $t {
+            #[inline]
+            async fn delay_ns(&mut self, ns: u32) {
+                use ::fugit::ExtU32Ceil;
+                Self::delay(ns.nanos_at_least()).await;
+            }
+
+            #[inline]
+            async fn delay_us(&mut self, us: u32) {
+                use ::fugit::ExtU32Ceil;
+                Self::delay(us.micros_at_least()).await;
+            }
+
+            #[inline]
+            async fn delay_ms(&mut self, ms: u32) {
+                use ::fugit::ExtU32Ceil;
+                Self::delay(ms.millis_at_least()).await;
+            }
+        }
+    };
+}
diff --git a/rtic-time/tests/delay_precision_subtick.rs b/rtic-time/tests/delay_precision_subtick.rs
new file mode 100644
index 0000000..4db889c
--- /dev/null
+++ b/rtic-time/tests/delay_precision_subtick.rs
@@ -0,0 +1,313 @@
+//! A test that verifies the sub-tick correctness of the [`TimerQueue`]'s `delay` functionality.
+//!
+//! To run this test, you need to activate the `critical-section/std` feature.
+
+use std::{
+    fmt::Debug,
+    future::Future,
+    pin::Pin,
+    sync::{
+        atomic::{AtomicBool, AtomicU64, AtomicUsize, Ordering},
+        Arc,
+    },
+    task::Context,
+    thread::sleep,
+    time::Duration,
+};
+
+use ::fugit::ExtU64Ceil;
+use cooked_waker::{IntoWaker, WakeRef};
+use parking_lot::Mutex;
+use rtic_time::{Monotonic, TimeoutError, TimerQueue};
+
+const SUBTICKS_PER_TICK: u32 = 10;
+struct SubtickTestTimer;
+static TIMER_QUEUE: TimerQueue<SubtickTestTimer> = TimerQueue::new();
+static NOW_SUBTICKS: AtomicU64 = AtomicU64::new(0);
+static COMPARE_TICKS: Mutex<Option<u64>> = Mutex::new(None);
+
+impl Monotonic for SubtickTestTimer {
+    const ZERO: Self::Instant = Self::Instant::from_ticks(0);
+    const TICK_PERIOD: Self::Duration = Self::Duration::from_ticks(1);
+
+    type Instant = fugit::Instant<u64, SUBTICKS_PER_TICK, 1000>;
+    type Duration = fugit::Duration<u64, SUBTICKS_PER_TICK, 1000>;
+
+    fn now() -> Self::Instant {
+        Self::Instant::from_ticks(
+            NOW_SUBTICKS.load(Ordering::Relaxed) / u64::from(SUBTICKS_PER_TICK),
+        )
+    }
+
+    fn set_compare(instant: Self::Instant) {
+        *COMPARE_TICKS.lock() = Some(instant.ticks());
+    }
+
+    fn clear_compare_flag() {}
+
+    fn pend_interrupt() {
+        unsafe {
+            Self::__tq().on_monotonic_interrupt();
+        }
+    }
+}
+
+impl SubtickTestTimer {
+    pub fn init() {
+        Self::__tq().initialize(Self)
+    }
+
+    pub fn tick() -> u64 {
+        let now = NOW_SUBTICKS.fetch_add(1, Ordering::Relaxed) + 1;
+        let ticks = now / u64::from(SUBTICKS_PER_TICK);
+        let subticks = now % u64::from(SUBTICKS_PER_TICK);
+
+        let compare = COMPARE_TICKS.lock();
+
+        // println!(
+        //     "ticks: {ticks}, subticks: {subticks}, compare: {:?}",
+        //     *compare
+        // );
+        if subticks == 0 && Some(ticks) == *compare {
+            unsafe {
+                Self::__tq().on_monotonic_interrupt();
+            }
+        }
+
+        subticks
+    }
+
+    pub fn forward_to_subtick(subtick: u64) {
+        assert!(subtick < u64::from(SUBTICKS_PER_TICK));
+        while Self::tick() != subtick {}
+    }
+
+    pub fn now_subticks() -> u64 {
+        NOW_SUBTICKS.load(Ordering::Relaxed)
+    }
+
+    fn __tq() -> &'static TimerQueue<Self> {
+        &TIMER_QUEUE
+    }
+
+    /// Delay for some duration of time.
+    #[inline]
+    pub async fn delay(duration: <Self as Monotonic>::Duration) {
+        Self::__tq().delay(duration).await;
+    }
+
+    /// Timeout after a specific duration.
+    #[inline]
+    pub async fn timeout_after<F: core::future::Future>(
+        duration: <Self as Monotonic>::Duration,
+        future: F,
+    ) -> Result<F::Output, TimeoutError> {
+        Self::__tq().timeout_after(duration, future).await
+    }
+}
+
+rtic_time::embedded_hal_delay_impl_fugit64!(SubtickTestTimer);
+rtic_time::embedded_hal_async_delay_impl_fugit64!(SubtickTestTimer);
+
+// A simple struct that counts the number of times it is awoken. Can't
+// be awoken by value (because that would discard the counter), so we
+// must instead wrap it in an Arc.
+#[derive(Debug, Default)]
+struct WakeCounter {
+    count: AtomicUsize,
+}
+
+impl WakeCounter {
+    fn get(&self) -> usize {
+        self.count.load(Ordering::SeqCst)
+    }
+}
+
+impl WakeRef for WakeCounter {
+    fn wake_by_ref(&self) {
+        let _prev = self.count.fetch_add(1, Ordering::SeqCst);
+    }
+}
+
+struct OnDrop<F: FnOnce()>(Option<F>);
+impl<F: FnOnce()> OnDrop<F> {
+    pub fn new(f: F) -> Self {
+        Self(Some(f))
+    }
+}
+impl<F: FnOnce()> Drop for OnDrop<F> {
+    fn drop(&mut self) {
+        (self.0.take().unwrap())();
+    }
+}
+
+macro_rules! subtick_test {
+    (@run $start:expr, $actual_duration:expr, $delay_fn:expr) => {{
+        // forward clock to $start
+        SubtickTestTimer::forward_to_subtick($start);
+
+        // call wait function
+        let delay_fn = $delay_fn;
+        let mut future = std::pin::pin!(delay_fn);
+
+        let wakecounter = Arc::new(WakeCounter::default());
+        let waker = Arc::clone(&wakecounter).into_waker();
+        let mut context = Context::from_waker(&waker);
+
+        let mut finished_after: Option<u64> = None;
+        for i in 0..10 * u64::from(SUBTICKS_PER_TICK) {
+            if Future::poll(Pin::new(&mut future), &mut context).is_ready() {
+                if finished_after.is_none() {
+                    finished_after = Some(i);
+                }
+                break;
+            };
+
+            assert_eq!(wakecounter.get(), 0);
+            SubtickTestTimer::tick();
+        }
+
+        let expected_wakeups = {
+            if $actual_duration == 0 {
+                0
+            } else {
+                1
+            }
+        };
+        assert_eq!(wakecounter.get(), expected_wakeups);
+
+        // Tick again to test that we don't get a second wake
+        SubtickTestTimer::tick();
+        assert_eq!(wakecounter.get(), expected_wakeups);
+
+        assert_eq!(
+            Some($actual_duration),
+            finished_after,
+            "Expected to wait {} ticks, but waited {:?} ticks.",
+            $actual_duration,
+            finished_after,
+        );
+    }};
+
+    (@run_blocking $start:expr, $actual_duration:expr, $delay_fn:expr) => {{
+        // forward clock to $start
+        SubtickTestTimer::forward_to_subtick($start);
+
+        let t_start = SubtickTestTimer::now_subticks();
+
+        let finished = AtomicBool::new(false);
+        std::thread::scope(|s|{
+            s.spawn(||{
+                let _finished_guard = OnDrop::new(|| finished.store(true, Ordering::Relaxed));
+                ($delay_fn)();
+            });
+            s.spawn(||{
+                sleep(Duration::from_millis(10));
+                while !finished.load(Ordering::Relaxed) {
+                    SubtickTestTimer::tick();
+                    sleep(Duration::from_millis(10));
+                }
+            });
+        });
+
+        let t_end = SubtickTestTimer::now_subticks();
+        let measured_duration = t_end - t_start;
+        assert_eq!(
+            $actual_duration,
+            measured_duration,
+            "Expected to wait {} ticks, but waited {:?} ticks.",
+            $actual_duration,
+            measured_duration,
+        );
+    }};
+
+
+
+
+    ($start:expr, $min_duration:expr, $actual_duration:expr) => {{
+        subtick_test!(@run $start, $actual_duration, async {
+            let mut timer = SubtickTestTimer;
+            embedded_hal_async::delay::DelayNs::delay_ms(&mut timer, $min_duration).await;
+        });
+        subtick_test!(@run $start, $actual_duration, async {
+            let mut timer = SubtickTestTimer;
+            embedded_hal_async::delay::DelayNs::delay_us(&mut timer, 1_000 * $min_duration).await;
+        });
+        subtick_test!(@run $start, $actual_duration, async {
+            let mut timer = SubtickTestTimer;
+            embedded_hal_async::delay::DelayNs::delay_ns(&mut timer, 1_000_000 * $min_duration).await;
+        });
+        subtick_test!(@run $start, $actual_duration, async {
+            SubtickTestTimer::delay($min_duration.millis_at_least()).await;
+        });
+        subtick_test!(@run $start, $actual_duration, async {
+            let _ = SubtickTestTimer::timeout_after($min_duration.millis_at_least(), std::future::pending::<()>()).await;
+        });
+
+        // Those are slow and unreliable; enable them when needed.
+        const ENABLE_BLOCKING_TESTS: bool = false;
+        if ENABLE_BLOCKING_TESTS {
+            subtick_test!(@run_blocking $start, $actual_duration, || {
+                let mut timer = SubtickTestTimer;
+                embedded_hal::delay::DelayNs::delay_ms(&mut timer, $min_duration);
+            });
+            subtick_test!(@run_blocking $start, $actual_duration, || {
+                let mut timer = SubtickTestTimer;
+                embedded_hal::delay::DelayNs::delay_us(&mut timer, 1_000 * $min_duration);
+            });
+            subtick_test!(@run_blocking $start, $actual_duration, || {
+                let mut timer = SubtickTestTimer;
+                embedded_hal::delay::DelayNs::delay_ns(&mut timer, 1_000_000 * $min_duration);
+            });
+        }
+    }};
+}
+
+#[test]
+fn timer_queue_subtick_precision() {
+    SubtickTestTimer::init();
+
+    // subtick_test!(a, b, c) tests the following thing:
+    //
+    // If we start at subtick a and we need to wait b subticks,
+    // then we will actually wait c subticks.
+    // The important part is that c is never smaller than b,
+    // in all cases, as that would violate the contract of
+    // embedded-hal's DelayUs.
+
+    subtick_test!(0, 0, 0);
+    subtick_test!(0, 1, 20);
+    subtick_test!(0, 10, 20);
+    subtick_test!(0, 11, 30);
+    subtick_test!(0, 12, 30);
+
+    subtick_test!(1, 0, 0);
+    subtick_test!(1, 1, 19);
+    subtick_test!(1, 10, 19);
+    subtick_test!(1, 11, 29);
+    subtick_test!(1, 12, 29);
+
+    subtick_test!(2, 0, 0);
+    subtick_test!(2, 1, 18);
+    subtick_test!(2, 10, 18);
+    subtick_test!(2, 11, 28);
+    subtick_test!(2, 12, 28);
+
+    subtick_test!(3, 0, 0);
+    subtick_test!(3, 1, 17);
+    subtick_test!(3, 10, 17);
+    subtick_test!(3, 11, 27);
+    subtick_test!(3, 12, 27);
+
+    subtick_test!(8, 0, 0);
+    subtick_test!(8, 1, 12);
+    subtick_test!(8, 10, 12);
+    subtick_test!(8, 11, 22);
+    subtick_test!(8, 12, 22);
+
+    subtick_test!(9, 0, 0);
+    subtick_test!(9, 1, 11);
+    subtick_test!(9, 10, 11);
+    subtick_test!(9, 11, 21);
+    subtick_test!(9, 12, 21);
+}
diff --git a/rtic-time/tests/timer_queue.rs b/rtic-time/tests/timer_queue.rs
index 9ad7175..8bae385 100644
--- a/rtic-time/tests/timer_queue.rs
+++ b/rtic-time/tests/timer_queue.rs
@@ -17,7 +17,7 @@ static NOW: Mutex<Option<Instant>> = Mutex::new(None);
 pub struct Duration(u64);
 
 impl Duration {
-    pub fn from_ticks(millis: u64) -> Self {
+    pub const fn from_ticks(millis: u64) -> Self {
         Self(millis)
     }
 
@@ -161,6 +161,7 @@ impl TestMono {
 
 impl Monotonic for TestMono {
     const ZERO: Self::Instant = Instant::ZERO;
+    const TICK_PERIOD: Self::Duration = Duration::from_ticks(1);
 
     type Instant = Instant;
 
@@ -210,7 +211,8 @@ fn timer_queue() {
             let elapsed = start.elapsed().as_ticks();
             println!("{total_millis} ticks delay reached after {elapsed} ticks");
 
-            if elapsed != total_millis {
+            // Expect a delay of one longer, to compensate for timer uncertainty
+            if elapsed != total_millis + 1 {
                 panic!(
                     "{total_millis} ticks delay was not on time ({elapsed} ticks passed instead)"
                 );
@@ -263,25 +265,25 @@ fn timer_queue() {
 
         if Instant::now() == 0.into() {
             // First, we want to be waiting for our 300 tick delay
-            assert_eq!(TestMono::compare(), Some(300.into()));
+            assert_eq!(TestMono::compare(), Some(301.into()));
         }
 
         if Instant::now() == 100.into() {
             // After 100 ticks, we enqueue a new delay that is supposed to last
             // until the 200-tick-mark
-            assert_eq!(TestMono::compare(), Some(200.into()));
+            assert_eq!(TestMono::compare(), Some(201.into()));
         }
 
-        if Instant::now() == 200.into() {
+        if Instant::now() == 201.into() {
             // After 200 ticks, we dequeue the 200-tick-mark delay and
             // requeue the 300 tick delay
-            assert_eq!(TestMono::compare(), Some(300.into()));
+            assert_eq!(TestMono::compare(), Some(301.into()));
         }
 
-        if Instant::now() == 300.into() {
+        if Instant::now() == 301.into() {
             // After 300 ticks, we dequeue the 300-tick-mark delay and
             // go to the 400 tick delay that is already enqueued
-            assert_eq!(TestMono::compare(), Some(400.into()));
+            assert_eq!(TestMono::compare(), Some(401.into()));
         }
     }