Copy DWT management and profiling features from stm32f4xx-hal (#50)

The DWT unit allows cycle count based profiling/tracing for performance
testing. The [module in the
stm32f4xx-hal](https://github.com/stm32-rs/stm32f4xx-hal/blob/master/src/dwt.rs)
is a useful tool for managing this functionality, so I'm copying it
verbatim (with very minor changes for imports) here.

Author: astapleton

examples/dwt-blinky.rs

@@ -0,0 +1,73 @@
+#![deny(unsafe_code)]
+#![deny(warnings)]
+#![no_main]
+#![no_std]
+
+mod utilities;
+
+use cortex_m_rt::entry;
+use embedded_hal::delay::DelayNs;
+use log::info;
+use stm32h5xx_hal::{
+    dwt::{ClockDuration, DwtExt},
+    pac,
+    prelude::*,
+};
+
+#[entry]
+fn main() -> ! {
+    utilities::logger::init();
+
+    let cp = cortex_m::Peripherals::take().unwrap();
+    let dp = pac::Peripherals::take().unwrap();
+
+    info!("Setup PWR...                  ");
+    let pwr = dp.PWR.constrain();
+    let pwrcfg = pwr.vos0().freeze();
+
+    // Constrain and Freeze clock
+    info!("Setup RCC...                  ");
+    let rcc = dp.RCC.constrain();
+    let ccdr = rcc.sys_ck(250.MHz()).freeze(pwrcfg, &dp.SBS);
+
+    let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA);
+    let mut led = gpioa.pa5.into_push_pull_output();
+
+    // Create a delay abstraction based on DWT cycle counter
+    let dwt = cp.DWT.constrain(cp.DCB, &ccdr.clocks);
+    let mut delay = dwt.delay();
+
+    // Create a stopwatch for maximum 9 laps
+    // Note: it starts immediately
+    let mut lap_times = [0u32; 10];
+    let mut sw = dwt.stopwatch(&mut lap_times);
+    loop {
+        // On for 1s, off for 1s.
+        led.set_high();
+        delay.delay_ms(1000);
+        sw.lap();
+        led.set_low();
+        delay.delay_ms(900);
+
+        // Also you can measure with almost clock precision
+        let cd: ClockDuration = dwt.measure(|| delay.delay_ms(100));
+        info!("Ticks: {}", cd.as_ticks()); // Should return 250MHz * 0.1s as u32
+        info!("Secs (f32): {}", cd.as_secs_f32()); // Should return ~0.1s as a f32
+        info!("Secs (f64): {}", cd.as_secs_f64()); // Should return ~0.1s as a f64
+        info!("Nanos: {}", cd.as_nanos()); // Should return 100000000ns as a u64
+
+        sw.lap();
+
+        // Get all the lap times
+        {
+            let mut lap = 1;
+            while let Some(lap_time) = sw.lap_time(lap) {
+                let _t = lap_time.as_secs_f64();
+                lap += 1;
+            }
+        }
+
+        // Reset stopwatch
+        sw.reset();
+    }
+}

src/dwt.rs

@@ -0,0 +1,266 @@
+//! Debug and trace management for profiling/tracing operations
+
+use cortex_m::peripheral::{DCB, DWT};
+use fugit::HertzU32 as Hertz;
+
+use crate::rcc::CoreClocks;
+
+pub trait DwtExt {
+    fn constrain(self, dcb: DCB, clocks: &CoreClocks) -> Dwt;
+}
+impl DwtExt for DWT {
+    /// Enable trace unit and cycle counter
+    fn constrain(mut self, mut dcb: DCB, clocks: &CoreClocks) -> Dwt {
+        dcb.enable_trace();
+        self.enable_cycle_counter();
+        Dwt {
+            dwt: self,
+            dcb,
+            clock: clocks.hclk(),
+        }
+    }
+}
+
+/// DWT (Data Watchpoint and Trace) unit
+pub struct Dwt {
+    dwt: DWT,
+    dcb: DCB,
+    clock: Hertz,
+}
+impl Dwt {
+    /// Release the dwt and dcb control
+    /// # Safety
+    /// All instances of Delay and StopWatch become invalid after this
+    pub unsafe fn release(self) -> (DWT, DCB) {
+        (self.dwt, self.dcb)
+    }
+    /// Create a delay instance
+    pub fn delay(&self) -> Delay {
+        Delay { clock: self.clock }
+    }
+    /// Create a stopwatch instance
+    /// # Arguments
+    /// * `times` - Array which will be holding the timings in ticks (max laps == times.len()-1)
+    pub fn stopwatch<'i>(&self, times: &'i mut [u32]) -> StopWatch<'i> {
+        StopWatch::new(times, self.clock)
+    }
+    /// Measure cycles it takes to execute closure `f`.
+    ///
+    /// Since DWT Cycle Counter is a 32-bit counter that wraps around to 0 on overflow,
+    /// users should be aware that `Dwt::measure` cannot correctly measure running time of
+    /// closures which take longer than `u32::MAX` cycles
+    pub fn measure<F: FnOnce()>(&self, f: F) -> ClockDuration {
+        let mut times: [u32; 2] = [0; 2];
+        let mut sw = self.stopwatch(&mut times);
+        f();
+        sw.lap().lap_time(1).unwrap()
+    }
+}
+
+#[derive(Clone, Copy)]
+pub struct Delay {
+    clock: Hertz,
+}
+impl Delay {
+    /// Delay for `ClockDuration::ticks`
+    pub fn delay(duration: ClockDuration) {
+        let ticks = duration.ticks as u64;
+        Delay::delay_ticks(DWT::cycle_count(), ticks);
+    }
+    /// Delay ticks
+    /// NOTE DCB and DWT need to be set up for this to work, so it is private
+    fn delay_ticks(mut start: u32, ticks: u64) {
+        if ticks < (core::u32::MAX / 2) as u64 {
+            // Simple delay
+            let ticks = ticks as u32;
+            while (DWT::cycle_count().wrapping_sub(start)) < ticks {}
+        } else if ticks <= core::u32::MAX as u64 {
+            // Try to avoid race conditions by limiting delay to u32::MAX / 2
+            let mut ticks = ticks as u32;
+            ticks -= core::u32::MAX / 2;
+            while (DWT::cycle_count().wrapping_sub(start)) < core::u32::MAX / 2
+            {
+            }
+            start -= core::u32::MAX / 2;
+            while (DWT::cycle_count().wrapping_sub(start)) < ticks {}
+        } else {
+            // Delay for ticks, then delay for rest * u32::MAX
+            let mut rest = (ticks >> 32) as u32;
+            let ticks = (ticks & core::u32::MAX as u64) as u32;
+            loop {
+                while (DWT::cycle_count().wrapping_sub(start)) < ticks {}
+                if rest == 0 {
+                    break;
+                }
+                rest -= 1;
+                while (DWT::cycle_count().wrapping_sub(start)) > ticks {}
+            }
+        }
+    }
+}
+
+impl embedded_hal::delay::DelayNs for Delay {
+    fn delay_ns(&mut self, ns: u32) {
+        // Convert us to ticks
+        let start = DWT::cycle_count();
+        let ticks = (ns as u64 * self.clock.raw() as u64) / 1_000_000_000;
+        Delay::delay_ticks(start, ticks);
+    }
+
+    fn delay_us(&mut self, us: u32) {
+        // Convert us to ticks
+        let start = DWT::cycle_count();
+        let ticks = (us as u64 * self.clock.raw() as u64) / 1_000_000;
+        Delay::delay_ticks(start, ticks);
+    }
+
+    fn delay_ms(&mut self, ms: u32) {
+        // Convert ms to ticks
+        let start = DWT::cycle_count();
+        let ticks = (ms as u64 * self.clock.raw() as u64) / 1_000;
+        Delay::delay_ticks(start, ticks);
+    }
+}
+
+/// Very simple stopwatch which reads from DWT Cycle Counter to record timing.
+///
+/// Since DWT Cycle Counter is a 32-bit counter that wraps around to 0 on overflow,
+/// users should be aware that `StopWatch` cannot correctly measure laps
+/// which take longer than `u32::MAX` cycles
+pub struct StopWatch<'l> {
+    times: &'l mut [u32],
+    timei: usize,
+    clock: Hertz,
+}
+impl<'l> StopWatch<'l> {
+    /// Create a new instance (Private because dwt/dcb should be set up)
+    /// # Arguments
+    /// * `times` - Array which will be holding the timings (max laps == times.len()-1)
+    /// * `clock` - The DWT cycle counters clock
+    fn new(times: &'l mut [u32], clock: Hertz) -> Self {
+        assert!(times.len() >= 2);
+        let mut sw = StopWatch {
+            times,
+            timei: 0,
+            clock,
+        };
+        sw.reset();
+        sw
+    }
+    /// Returns the numbers of laps recorded
+    pub fn lap_count(&self) -> usize {
+        self.timei
+    }
+    /// Resets recorded laps to 0 and sets 0 offset
+    pub fn reset(&mut self) {
+        self.timei = 0;
+        self.times[0] = DWT::cycle_count();
+    }
+    /// Record a new lap.
+    ///
+    /// If lap count exceeds maximum, the last lap is updated
+    pub fn lap(&mut self) -> &mut Self {
+        let c = DWT::cycle_count();
+        if self.timei < self.times.len() {
+            self.timei += 1;
+        }
+        self.times[self.timei] = c;
+        self
+    }
+    /// Calculate the time of lap n (n starting with 1).
+    ///
+    /// Returns None if `n` is out of range
+    pub fn lap_time(&self, n: usize) -> Option<ClockDuration> {
+        if (n < 1) || (self.timei < n) {
+            None
+        } else {
+            Some(ClockDuration {
+                ticks: self.times[n].wrapping_sub(self.times[n - 1]),
+                clock: self.clock,
+            })
+        }
+    }
+}
+
+/// Clock difference with capability to calculate SI units (s)
+#[derive(Clone, Copy)]
+pub struct ClockDuration {
+    ticks: u32,
+    clock: Hertz,
+}
+impl ClockDuration {
+    /// Returns ticks
+    pub fn as_ticks(self) -> u32 {
+        self.ticks
+    }
+    /// Returns calculated milliseconds as integer
+    pub fn as_millis(self) -> u64 {
+        self.ticks as u64 * 1_000 / self.clock.raw() as u64
+    }
+    /// Returns calculated microseconds as integer
+    pub fn as_micros(self) -> u64 {
+        self.ticks as u64 * 1_000_000 / self.clock.raw() as u64
+    }
+    /// Returns calculated nanoseconds as integer
+    pub fn as_nanos(self) -> u64 {
+        self.ticks as u64 * 1_000_000_000 / self.clock.raw() as u64
+    }
+    /// Return calculated seconds as 32-bit float
+    pub fn as_secs_f32(self) -> f32 {
+        self.ticks as f32 / self.clock.raw() as f32
+    }
+    /// Return calculated seconds as 64-bit float
+    pub fn as_secs_f64(self) -> f64 {
+        self.ticks as f64 / self.clock.raw() as f64
+    }
+}
+
+/// A monotonic non-decreasing timer
+///
+/// This uses the timer in the debug watch trace peripheral. This means, that if the
+/// core is stopped, the timer does not count up. This may be relevant if you are using
+/// cortex_m_semihosting::hprintln for debugging in which case the timer will be stopped
+/// while printing
+#[derive(Clone, Copy)]
+pub struct MonoTimer {
+    frequency: Hertz,
+}
+
+impl MonoTimer {
+    /// Creates a new `Monotonic` timer
+    pub fn new(mut dwt: DWT, mut dcb: DCB, clocks: &CoreClocks) -> Self {
+        dcb.enable_trace();
+        dwt.enable_cycle_counter();
+
+        // now the CYCCNT counter can't be stopped or reset
+
+        MonoTimer {
+            frequency: clocks.hclk(),
+        }
+    }
+
+    /// Returns the frequency at which the monotonic timer is operating at
+    pub fn frequency(self) -> Hertz {
+        self.frequency
+    }
+
+    /// Returns an `Instant` corresponding to "now"
+    pub fn now(self) -> Instant {
+        Instant {
+            now: DWT::cycle_count(),
+        }
+    }
+}
+
+/// A measurement of a monotonically non-decreasing clock
+#[derive(Clone, Copy)]
+pub struct Instant {
+    now: u32,
+}
+
+impl Instant {
+    /// Ticks elapsed since the `Instant` was created
+    pub fn elapsed(self) -> u32 {
+        DWT::cycle_count().wrapping_sub(self.now)
+    }
+}

src/lib.rs

@@ -76,6 +76,9 @@ pub mod delay;
 #[cfg(feature = "device-selected")]
 pub mod spi;
 
+#[cfg(feature = "device-selected")]
+pub mod dwt;
+
 #[cfg(feature = "device-selected")]
 mod sealed {
     pub trait Sealed {}

src/prelude.rs

@@ -1,6 +1,7 @@
 //! Prelude
 
 pub use crate::delay::DelayExt as _stm32h5xx_hal_delay_DelayExt;
+pub use crate::dwt::DwtExt as _stm32h5xx_hal_delay_DwtExt;
 pub use crate::gpio::GpioExt as _stm32h5xx_hal_gpio_GpioExt;
 pub use crate::i2c::I2cExt as _stm32h5xx_hal_i2c_I2cExt;
 pub use crate::icache::ICacheExt as _stm32h5xx_hal_icache_ICacheExt;