From 626c8028a3ceb0394ce57ba6edfb819234fa6826 Mon Sep 17 00:00:00 2001
From: astapleton <andrew@utvate.com>
Date: Fri, 4 Oct 2024 11:46:32 -0700
Subject: [PATCH] delay: Add delay module

---
 examples/blinky.rs |  20 ++++----
 src/delay.rs       | 112 +++++++++++++++++++++++++++++++++++++++++++++
 src/lib.rs         |   3 ++
 src/prelude.rs     |   2 +
 4 files changed, 128 insertions(+), 9 deletions(-)
 create mode 100644 src/delay.rs

diff --git a/examples/blinky.rs b/examples/blinky.rs
index 4772797..a37af82 100644
--- a/examples/blinky.rs
+++ b/examples/blinky.rs
@@ -4,12 +4,15 @@
 mod utilities;
 
 use cortex_m_rt::entry;
-use stm32h5xx_hal::{pac, prelude::*};
+use embedded_hal::delay::DelayNs;
+use fugit::SecsDurationU32;
+use stm32h5xx_hal::{delay::Delay, pac, prelude::*, rcc::ResetEnable};
 
 #[entry]
 fn main() -> ! {
     utilities::logger::init();
 
+    let cp = cortex_m::Peripherals::take().unwrap();
     let dp = pac::Peripherals::take().unwrap();
 
     let pwr = dp.PWR.constrain();
@@ -17,21 +20,20 @@ fn main() -> ! {
 
     // Constrain and Freeze clock
     let rcc = dp.RCC.constrain();
-    let _ccdr = rcc.sys_ck(250.MHz()).freeze(pwrcfg, &dp.SBS);
+    let ccdr = rcc.sys_ck(250.MHz()).freeze(pwrcfg, &dp.SBS);
+
+    ccdr.peripheral.GPIOA.enable();
 
     dp.GPIOA.moder().write(|w| w.mode5().output()); // output
     dp.GPIOA.pupdr().write(|w| w.pupd5().pull_up()); // pull-up
 
-    // dp.GPIOA.odr.write(|w| w.od5().set_bit());
+    let mut delay = Delay::new(cp.SYST, &ccdr.clocks);
+    let duration = SecsDurationU32::secs(1).to_millis();
 
     loop {
         dp.GPIOA.odr().write(|w| w.od5().low());
-        for _ in 0..10_000 {
-            cortex_m::asm::nop();
-        }
+        delay.delay_ns(duration);
         dp.GPIOA.odr().write(|w| w.od5().high());
-        for _ in 0..10_000 {
-            cortex_m::asm::nop();
-        }
+        delay.delay_ns(duration);
     }
 }
diff --git a/src/delay.rs b/src/delay.rs
new file mode 100644
index 0000000..507bdca
--- /dev/null
+++ b/src/delay.rs
@@ -0,0 +1,112 @@
+//! Delay providers
+//!
+//! # Examples
+//!
+//! ## Delay
+//!
+//! ```no_run
+//! let mut delay = Delay::new(core.SYST, device.clocks);
+//!
+//! delay.delay_ms(500);
+//!
+//! // Release SYST from the delay
+//! let syst = delay.free();
+//! ```
+//!
+//! # Examples
+//!
+//! - [Blinky](https://github.com/stm32-rs/stm32h7xx-hal/blob/master/examples/blinky.rs)
+
+use cortex_m::peripheral::syst::SystClkSource;
+use cortex_m::peripheral::SYST;
+use embedded_hal::delay::DelayNs;
+use fugit::SecsDurationU64;
+
+use crate::rcc::CoreClocks;
+
+const SYSTICK_HCLK_DIV: u32 = 8;
+
+pub trait DelayExt {
+    fn delay(self, clocks: &CoreClocks) -> Delay;
+}
+
+impl DelayExt for SYST {
+    fn delay(self, clocks: &CoreClocks) -> Delay {
+        Delay::new(self, clocks)
+    }
+}
+
+/// System timer (SysTick) as a delay provider
+pub struct Delay {
+    hclk_hz: u32,
+    syst: SYST,
+}
+fn calc_rvr(ns: u32, hclk: u32) -> u32 {
+    // Default is for SYSTICK to be fed by HCLK/8
+    let ticks: u64 = (SecsDurationU64::secs(1) * SYSTICK_HCLK_DIV).to_nanos();
+    ((ns as u64 * hclk as u64) / ticks) as u32
+}
+
+impl Delay {
+    /// Configures the system timer (SysTick) as a delay provider
+    pub fn new(mut syst: SYST, clocks: &CoreClocks) -> Self {
+        syst.set_clock_source(SystClkSource::External);
+
+        Delay {
+            hclk_hz: clocks.hclk().raw(),
+            syst,
+        }
+    }
+
+    /// Releases the system timer (SysTick) resource
+    pub fn free(self) -> SYST {
+        self.syst
+    }
+}
+
+impl DelayNs for Delay {
+    fn delay_ns(&mut self, ns: u32) {
+        // The SysTick Reload Value register supports values between 1 and 0x00FFFFFF.
+        const MAX_RVR: u32 = 0x00FF_FFFF;
+
+        let mut total_rvr = calc_rvr(ns, self.hclk_hz);
+
+        while total_rvr != 0 {
+            let current_rvr = if total_rvr <= MAX_RVR.into() {
+                total_rvr as u32
+            } else {
+                MAX_RVR
+            };
+
+            self.syst.set_reload(current_rvr);
+            self.syst.clear_current();
+            self.syst.enable_counter();
+
+            // Update the tracking variable while we are waiting...
+            total_rvr -= current_rvr;
+
+            while !self.syst.has_wrapped() {}
+
+            self.syst.disable_counter();
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::calc_rvr;
+    #[test]
+    fn test_calc_rvr() {
+        let rvr = calc_rvr(1000, 8_000_000);
+        assert_eq!(rvr, 1);
+
+        let rvr = calc_rvr(1000_000, 8_000_000);
+        assert_eq!(rvr, 1000);
+
+        let rvr = calc_rvr(1000_000, 10_000_000);
+        assert_eq!(rvr, 1250);
+
+        let rvr = calc_rvr(1000_000, 250_000_000);
+        assert_eq!(rvr, 31250);
+    }
+}
diff --git a/src/lib.rs b/src/lib.rs
index a72b082..2d80eaa 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -48,3 +48,6 @@ pub mod time;
 
 #[cfg(feature = "device-selected")]
 pub mod rcc;
+
+#[cfg(feature = "device-selected")]
+pub mod delay;
diff --git a/src/prelude.rs b/src/prelude.rs
index 9ab0257..57bb493 100644
--- a/src/prelude.rs
+++ b/src/prelude.rs
@@ -1,6 +1,8 @@
 //! Prelude
 
+pub use crate::delay::DelayExt as _stm32h5xx_hal_delay_DelayExt;
 pub use crate::pwr::PwrExt as _stm32h5xx_hal_pwr_PwrExt;
 pub use crate::rcc::RccExt as _stm32h5xx_hal_rcc_RccExt;
 
+pub use crate::time::U32Ext as _;
 pub use fugit::{ExtU32 as _, RateExtU32 as _};