32 bit monotonic from STM32Lxx 16 bit timer

monotonic.rs

use core::convert::TryFrom;
use embedded_time::rate::{Extensions, Hertz};
use rtic::{rtic_monotonic::Clock, Monotonic};
use stm32l0xx_hal::{
    pac::{TIM2, TIM21, TIM22, TIM3},
    rcc::Rcc,
};

pub trait TimExt {
    fn configure(&mut self, rcc: &mut Rcc, frequency: Hertz);

    /// Try to set the compare value.
    ///
    /// Return true if it was set
    fn try_set_compare_at(&mut self, ticks: u32, overflows: u16) -> bool;
    fn count(&self) -> u16;
    fn reset_count(&self);
    fn has_overflowed(&self) -> bool;
    fn clear_irq(&self);
    fn clear_compare(&self);
    fn has_compared(&self) -> bool;
}

macro_rules! monotonic {
    ($TIM:ident: ($timXen:ident, $apbenr:ident, $timclk:ident)) => {
        impl TimExt for $TIM {
            fn configure(&mut self, rcc: &mut Rcc, frequency: Hertz) {
                rcc.rb.$apbenr.modify(|_, w| w.$timXen().set_bit());

                let timer_freq = rcc.clocks.$timclk();
                let psc = u16::try_from(timer_freq.0 / frequency.0 - 1).unwrap();

                self.psc.write(|w| w.psc().bits(psc));
                self.arr.write(|w| w.arr().bits(u16::MAX));

                // Reset the counter
                self.cnt.reset();
                // Set Update Request Source to counter overflow/underflow only
                self.cr1.modify(|_, w| w.urs().counter_only());
                // Force an update of the psc and arr registers
                self.egr.write(|w| w.ug().update());
                // Trigger interrupt on updates (so now, that's only counter overflows)
                self.dier.write(|w| w.uie().enabled());
                // Start the timer
                self.cr1.modify(|_, w| w.cen().set_bit());
            }

            /// Try to set the compare value.
            ///
            /// Return true if it was set
            fn try_set_compare_at(&mut self, ticks: u32, overflows: u16) -> bool {
                let tick_overflows = (ticks >> 16) as u16;
                let tick_ticks = (ticks & 0xFFFF) as u16;
                let timer_ticks = self.count();

                if tick_overflows < overflows
                    || (tick_overflows == overflows && tick_ticks < timer_ticks)
                {
                    // In the past, so trigger immediately
                    self.egr.write(|w| w.cc1g().trigger());
                    self.dier.modify(|_, w| w.cc1ie().enabled());
                    true
                } else if tick_overflows == overflows {
                    // In the future of the current overflow
                    self.ccr1.write(|w| w.ccr().bits(tick_ticks));
                    self.dier.modify(|_, w| w.cc1ie().enabled());
                    true
                } else {
                    // In the future after overflow
                    self.dier.modify(|_, w| w.cc1ie().disabled());
                    false
                }
            }

            fn count(&self) -> u16 {
                self.cnt.read().bits() as u16
            }

            fn reset_count(&self) {
                self.cnt.reset();
            }

            fn has_overflowed(&self) -> bool {
                self.sr.read().uif().is_update_pending()
            }

            fn has_compared(&self) -> bool {
                self.sr.read().cc1if().bit_is_set()
            }

            fn clear_irq(&self) {
                self.sr.modify(|_, w| w.uif().clear_bit());
                let _ = self.sr.read();
                let _ = self.sr.read(); // Delay 2 peripheral clocks
            }

            fn clear_compare(&self) {
                self.dier.modify(|_, w| w.cc1ie().disabled());
                self.sr.modify(|_, w| w.cc1if().clear_bit());
            }
        }
    };
}

monotonic!(TIM2: (tim2en, apb1enr, apb1_tim_clk));
// NOTE: TIM3 is in the PAC but isn't available on at least the L0x1 or L0x3 I checked the
// datasheets for. I'll leave it enabled for other devices that might have it.
monotonic!(TIM3: (tim3en, apb1enr, apb1_tim_clk));
monotonic!(TIM21: (tim21en, apb2enr, apb2_tim_clk));
monotonic!(TIM22: (tim22en, apb2enr, apb2_tim_clk));

/// Wrapper struct for the RTIC `Monotonic` timer.
///
/// Specify the timer to use and set `TICK_RATE_HZ` to the update rate that RTIC should use.
/// Internally, 16 bit timers are used but are extended to 32 bits with some logic that handles
/// overflows.
pub struct MonoTimer<TIMX, const TICK_RATE_HZ: u32> {
    timer: TIMX,
    overflows: u16,
    next_compare_value: Option<u32>,
}

impl<TIMX, const TICK_RATE_HZ: u32> MonoTimer<TIMX, TICK_RATE_HZ>
where
    TIMX: TimExt,
{
    pub fn start(mut local_timer: TIMX, rcc: &mut Rcc) -> Self {
        local_timer.configure(rcc, TICK_RATE_HZ.Hz());

        Self {
            timer: local_timer,
            overflows: 0,
            next_compare_value: None,
        }
    }
}

impl<TIMX, const TICK_RATE_HZ: u32> Clock for MonoTimer<TIMX, TICK_RATE_HZ>
where
    TIMX: TimExt,
{
    type T = u32;

    const SCALING_FACTOR: rtic::rtic_monotonic::Fraction =
        rtic::rtic_monotonic::Fraction::new(1, TICK_RATE_HZ);

    fn try_now(&self) -> Result<rtic::rtic_monotonic::Instant<Self>, rtic::time::clock::Error> {
        let counter = self.timer.count();
        let has_overflowed = self.timer.has_overflowed();

        Ok(rtic::rtic_monotonic::Instant::new(
            ((self.overflows as u32 + if has_overflowed { 1 } else { 0 }) << 16) | (counter as u32),
        ))
    }
}

impl<TIMX, const TICK_RATE_HZ: u32> Monotonic for MonoTimer<TIMX, TICK_RATE_HZ>
where
    TIMX: TimExt,
{
    const DISABLE_INTERRUPT_ON_EMPTY_QUEUE: bool = false;

    unsafe fn reset(&mut self) {
        self.timer.reset_count();
        self.timer.clear_irq();
    }

    fn set_compare(&mut self, instant: &rtic::rtic_monotonic::Instant<Self>) {
        let instant_ticks = instant.duration_since_epoch().integer();
        if !self.timer.try_set_compare_at(instant_ticks, self.overflows) {
            self.next_compare_value = Some(instant_ticks);
        }
    }

    fn clear_compare_flag(&mut self) {
        self.timer.clear_compare();
        self.next_compare_value = None;
    }

    fn on_interrupt(&mut self) {
        if self.timer.has_overflowed() {
            self.timer.clear_irq();

            self.overflows += 1;

            if let Some(compare_value) = self.next_compare_value {
                if self.timer.try_set_compare_at(compare_value, self.overflows) {
                    self.next_compare_value = None;
                }
            }
        }

        if self.timer.has_compared() {
            // Compared
            self.clear_compare_flag();
        }
    }
}