Fixed systick monotonic

10 files changed, 137 insertions, 4 deletions

diff --git a/rtic-monotonics/Cargo.toml b/rtic-monotonics/Cargo.toml
index 24448fb2..68daba40 100644
--- a/rtic-monotonics/Cargo.toml
+++ b/rtic-monotonics/Cargo.toml
@@ -1,5 +1,5 @@
 [package]
-name = "rtic-timer"
+name = "rtic-monotonics"
 version = "0.1.0"
 edition = "2021"
 
@@ -9,4 +9,4 @@ edition = "2021"
 cortex-m = { version = "0.7.6" }
 embedded-hal-async = "0.2.0-alpha.0"
 fugit = { version = "0.3.6", features = ["defmt"] }
-rtic-timer = { version = "1.0.0", path = "../rtic-timer" }
+rtic-time = { version = "1.0.0", path = "../rtic-time" }
diff --git a/rtic-monotonics/src/lib.rs b/rtic-monotonics/src/lib.rs
index 88398cad..ce30c36b 100644
--- a/rtic-monotonics/src/lib.rs
+++ b/rtic-monotonics/src/lib.rs
@@ -6,6 +6,6 @@
 #![allow(incomplete_features)]
 #![feature(async_fn_in_trait)]
 
-pub use rtic_timer::{Monotonic, TimeoutError, TimerQueue};
+pub use rtic_time::{Monotonic, TimeoutError, TimerQueue};
 
 pub mod systick_monotonic;
diff --git a/rtic-monotonics/src/systick_monotonic.rs b/rtic-monotonics/src/systick_monotonic.rs
index 491cf81c..7c713b61 100644
--- a/rtic-monotonics/src/systick_monotonic.rs
+++ b/rtic-monotonics/src/systick_monotonic.rs
@@ -1 +1,134 @@
 //! ...
+
+use super::Monotonic;
+pub use super::{TimeoutError, TimerQueue};
+use core::{
+    ops::Deref,
+    sync::atomic::{AtomicU32, Ordering},
+};
+use cortex_m::peripheral::SYST;
+use embedded_hal_async::delay::DelayUs;
+use fugit::ExtU32;
+
+/// Systick implementing `rtic_monotonic::Monotonic` which runs at a
+/// settable rate using the `TIMER_HZ` parameter.
+pub struct Systick<const TIMER_HZ: u32>;
+
+impl<const TIMER_HZ: u32> Systick<TIMER_HZ> {
+    /// Start a `Monotonic` based on SysTick.
+    ///
+    /// The `sysclk` parameter is the speed at which SysTick runs at. This value should come from
+    /// the clock generation function of the used HAL.
+    ///
+    /// Notice that the actual rate of the timer is a best approximation based on the given
+    /// `sysclk` and `TIMER_HZ`.
+    ///
+    /// Note: Give the return value to `TimerQueue::initialize()` to initialize the timer queue.
+    #[must_use]
+    pub fn start(mut systick: cortex_m::peripheral::SYST, sysclk: u32) -> Self {
+        // + TIMER_HZ / 2 provides round to nearest instead of round to 0.
+        // - 1 as the counter range is inclusive [0, reload]
+        let reload = (sysclk + TIMER_HZ / 2) / TIMER_HZ - 1;
+
+        assert!(reload <= 0x00ff_ffff);
+        assert!(reload > 0);
+
+        systick.disable_counter();
+        systick.set_clock_source(cortex_m::peripheral::syst::SystClkSource::Core);
+        systick.set_reload(reload);
+        systick.enable_interrupt();
+        systick.enable_counter();
+
+        Systick {}
+    }
+
+    fn systick() -> SYST {
+        unsafe { core::mem::transmute::<(), SYST>(()) }
+    }
+}
+
+static SYSTICK_CNT: AtomicU32 = AtomicU32::new(0);
+
+impl<const TIMER_HZ: u32> Monotonic for Systick<TIMER_HZ> {
+    type Instant = fugit::TimerInstantU32<TIMER_HZ>;
+    type Duration = fugit::TimerDurationU32<TIMER_HZ>;
+
+    const ZERO: Self::Instant = Self::Instant::from_ticks(0);
+
+    fn now() -> Self::Instant {
+        if Self::systick().has_wrapped() {
+            SYSTICK_CNT.fetch_add(1, Ordering::AcqRel);
+        }
+
+        Self::Instant::from_ticks(SYSTICK_CNT.load(Ordering::Relaxed))
+    }
+
+    fn set_compare(_: Self::Instant) {
+        // No need to do something here, we get interrupts anyway.
+    }
+
+    fn clear_compare_flag() {
+        // NOOP with SysTick interrupt
+    }
+
+    fn pend_interrupt() {
+        cortex_m::peripheral::SCB::set_pendst();
+    }
+
+    fn on_interrupt() {
+        if Self::systick().has_wrapped() {
+            SYSTICK_CNT.fetch_add(1, Ordering::AcqRel);
+        }
+    }
+
+    fn enable_timer() {}
+
+    fn disable_timer() {}
+}
+
+/// Timer queue wrapper to implement traits on
+pub struct SystickTimerQueue<const TIMER_HZ: u32>(TimerQueue<Systick<TIMER_HZ>>);
+
+impl<const TIMER_HZ: u32> SystickTimerQueue<TIMER_HZ> {
+    /// Create a new timer queue.
+    pub const fn new() -> Self {
+        Self(TimerQueue::new())
+    }
+}
+
+impl<const TIMER_HZ: u32> Deref for SystickTimerQueue<TIMER_HZ> {
+    type Target = TimerQueue<Systick<TIMER_HZ>>;
+
+    #[inline(always)]
+    fn deref(&self) -> &Self::Target {
+        &self.0
+    }
+}
+
+impl<const TIMER_HZ: u32> DelayUs for SystickTimerQueue<TIMER_HZ> {
+    type Error = core::convert::Infallible;
+
+    async fn delay_us(&mut self, us: u32) -> Result<(), Self::Error> {
+        self.delay(us.micros()).await;
+        Ok(())
+    }
+
+    async fn delay_ms(&mut self, ms: u32) -> Result<(), Self::Error> {
+        self.delay(ms.millis()).await;
+        Ok(())
+    }
+}
+
+/// Register the Systick interrupt and crate a timer queue with a specific name and speed.
+#[macro_export]
+macro_rules! make_systick_timer_queue {
+    ($timer_queue_name:ident, $systick_speed:expr) => {
+        static $timer_queue_name: SystickTimerQueue<$systick_speed> = SystickTimerQueue::new();
+
+        #[no_mangle]
+        #[allow(non_snake_case)]
+        unsafe extern "C" fn SysTick() {
+            $timer_queue_name.on_monotonic_interrupt();
+        }
+    };
+}
diff --git a/rtic-timer/.gitignore b/rtic-time/.gitignore
index c4002562..c4002562 100644
--- a/rtic-timer/.gitignore
+++ b/rtic-time/.gitignore
diff --git a/rtic-timer/CHANGELOG.md b/rtic-time/CHANGELOG.md
index e69de29b..e69de29b 100644
--- a/rtic-timer/CHANGELOG.md
+++ b/rtic-time/CHANGELOG.md
diff --git a/rtic-timer/Cargo.toml b/rtic-time/Cargo.toml
index b7b3a5fb..ea059396 100644
--- a/rtic-timer/Cargo.toml
+++ b/rtic-time/Cargo.toml
@@ -1,5 +1,5 @@
 [package]
-name = "rtic-timer"
+name = "rtic-time"
 version = "1.0.0"
 edition = "2021"
 
diff --git a/rtic-timer/rust-toolchain.toml b/rtic-time/rust-toolchain.toml
index e28b55de..e28b55de 100644
--- a/rtic-timer/rust-toolchain.toml
+++ b/rtic-time/rust-toolchain.toml
diff --git a/rtic-timer/src/lib.rs b/rtic-time/src/lib.rs
index d7faa07f..d7faa07f 100644
--- a/rtic-timer/src/lib.rs
+++ b/rtic-time/src/lib.rs
diff --git a/rtic-timer/src/linked_list.rs b/rtic-time/src/linked_list.rs
index 42ff8cb6..42ff8cb6 100644
--- a/rtic-timer/src/linked_list.rs
+++ b/rtic-time/src/linked_list.rs
diff --git a/rtic-timer/src/monotonic.rs b/rtic-time/src/monotonic.rs
index 9b3742fa..9b3742fa 100644
--- a/rtic-timer/src/monotonic.rs
+++ b/rtic-time/src/monotonic.rs