L0xx monotonic impl for TIM2

monotonic.rs

use rtic::{rtic_monotonic::Clock, Monotonic};
use stm32l0xx_hal::pac::TIM2;

pub struct Tim2Monotonic {
    local_timer: TIM2,
    overflows: u16,
    next_compare_value: Option<u32>,
}

impl Tim2Monotonic {
    const CLOCK_SPEED: u32 = 16_000_000;
    const CLOCK_DIVIDER: u16 = 1;
    const TIMER_SPEED: u32 = Self::CLOCK_SPEED / Self::CLOCK_DIVIDER as u32;
    const TIMER_MAX: u16 = u16::MAX;

    pub fn start(local_timer: TIM2) -> Self {
        // assert_eq!(
        //     TIM2::get_clk(&get_clocks()),
        //     Some(Self::CLOCK_SPEED.hz()),
        //     "Settings have changed. Change these numbers and also the fraction in the clock impl"
        // );

        // Set prescaler divide by the given amount
        local_timer
            .psc
            .write(|w| w.psc().bits(Self::CLOCK_DIVIDER - 1));
        // Set auto reload to max
        local_timer.arr.write(|w| w.arr().bits(Self::TIMER_MAX));
        // Reset the counter
        local_timer.cnt.reset();
        // Set Update Request Source to counter overflow/underflow only
        local_timer.cr1.modify(|_, w| w.urs().counter_only());
        // Force an update of the psc and arr registers
        local_timer.egr.write(|w| w.ug().update());
        // Trigger interrupt on updates (so now, that's only counter overflows)
        local_timer.dier.write(|w| w.uie().enabled());
        // Start the timer
        local_timer.cr1.modify(|_, w| w.cen().set_bit());

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

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

        let timer_ticks = self.local_timer.cnt.read().bits();

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

impl Clock for Tim2Monotonic {
    type T = u32;

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

    fn try_now(&self) -> Result<rtic::rtic_monotonic::Instant<Self>, rtic::time::clock::Error> {
        let counter = self.local_timer.cnt.read().bits();
        let has_overflowed = self.local_timer.sr.read().uif().is_update_pending();

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

impl Monotonic for Tim2Monotonic {
    const DISABLE_INTERRUPT_ON_EMPTY_QUEUE: bool = false;

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

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

    fn clear_compare_flag(&mut self) {
        self.local_timer.dier.modify(|_, w| w.cc1ie().disabled());
        self.local_timer.sr.modify(|_, w| w.cc1if().clear_bit());
        self.next_compare_value = None;
    }

    fn on_interrupt(&mut self) {
        let sr = self.local_timer.sr.read();

        if sr.uif().is_update_pending() {
            // Overflowed
            self.local_timer.sr.modify(|_, w| {
                // Clears timeout event
                w.uif().clear_bit()
            });
            let _ = self.local_timer.sr.read();
            let _ = self.local_timer.sr.read(); // Delay 2 peripheral clocks

            self.overflows += 1;

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

        if sr.cc1if().bit_is_set() {
            // Compared
            self.clear_compare_flag();
        }
    }
}