From cda404731093015f84bad96675fdbfc712bc0215 Mon Sep 17 00:00:00 2001 From: xoviat Date: Thu, 24 Aug 2023 19:29:11 -0500 Subject: [PATCH] stm32: flesh out lp executor --- embassy-stm32/src/lib.rs | 5 +++ embassy-stm32/src/low_power.rs | 45 +++++++++++++------ embassy-stm32/src/rcc/mod.rs | 2 + embassy-stm32/src/rtc/v2.rs | 25 ++++++++--- embassy-stm32/src/time_driver.rs | 76 ++++++++++++++++++++++++++++++++ 5 files changed, 133 insertions(+), 20 deletions(-) diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs index 8c87ea7d..ec8648ee 100644 --- a/embassy-stm32/src/lib.rs +++ b/embassy-stm32/src/lib.rs @@ -197,6 +197,11 @@ pub fn init(config: Config) -> Peripherals { // must be after rcc init #[cfg(feature = "_time-driver")] time_driver::init(); + + #[cfg(feature = "low-power")] + while !crate::rcc::low_power_ready() { + crate::rcc::clock_refcount_sub(); + } } p diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs index 7814fa38..0d9506aa 100644 --- a/embassy-stm32/src/low_power.rs +++ b/embassy-stm32/src/low_power.rs @@ -9,6 +9,7 @@ use crate::interrupt; use crate::interrupt::typelevel::Interrupt; use crate::pac::EXTI; use crate::rcc::low_power_ready; +use crate::time_driver::{pause_time, resume_time, time_until_next_alarm}; const THREAD_PENDER: usize = usize::MAX; const THRESHOLD: Duration = Duration::from_millis(500); @@ -16,6 +17,9 @@ const THRESHOLD: Duration = Duration::from_millis(500); use crate::rtc::{Rtc, RtcInstant}; static mut RTC: Option<&'static Rtc> = None; +static mut STOP_TIME: embassy_time::Duration = Duration::from_ticks(0); +static mut NEXT_ALARM: embassy_time::Duration = Duration::from_ticks(u64::MAX); +static mut RTC_INSTANT: Option = None; foreach_interrupt! { (RTC, rtc, $block:ident, WKUP, $irq:ident) => { @@ -69,13 +73,25 @@ impl Executor { } unsafe fn on_wakeup_irq() { - info!("on wakeup irq"); + trace!("on wakeup irq"); - cortex_m::asm::bkpt(); - } + let elapsed = RTC_INSTANT.take().unwrap() - stop_wakeup_alarm(); - fn time_until_next_alarm(&self) -> Duration { - Duration::from_secs(3) + STOP_TIME += elapsed; + // let to_next = NEXT_ALARM - STOP_TIME; + let to_next = Duration::from_secs(3); + + trace!("on wakeup irq: to next: {}", to_next); + if to_next > THRESHOLD { + trace!("start wakeup alarm"); + RTC_INSTANT.replace(start_wakeup_alarm(to_next)); + + trace!("set sleeponexit"); + Self::get_scb().set_sleeponexit(); + } else { + Self::get_scb().clear_sleeponexit(); + Self::get_scb().clear_sleepdeep(); + } } fn get_scb() -> SCB { @@ -86,25 +102,28 @@ impl Executor { trace!("configure_pwr"); if !low_power_ready() { + trace!("configure_pwr: low power not ready"); return; } - let time_until_next_alarm = self.time_until_next_alarm(); + let time_until_next_alarm = time_until_next_alarm(); if time_until_next_alarm < THRESHOLD { + trace!("configure_pwr: not enough time until next alarm"); return; } - trace!("low power stop required"); + unsafe { + NEXT_ALARM = time_until_next_alarm; + RTC_INSTANT = Some(start_wakeup_alarm(time_until_next_alarm)) + }; - critical_section::with(|_| { - trace!("executor: set wakeup alarm..."); + // return; - start_wakeup_alarm(time_until_next_alarm); + pause_time(); - trace!("low power wait for rtc ready..."); + trace!("enter stop..."); - Self::get_scb().set_sleepdeep(); - }); + Self::get_scb().set_sleepdeep(); } /// Run the executor. diff --git a/embassy-stm32/src/rcc/mod.rs b/embassy-stm32/src/rcc/mod.rs index 3c75923e..45a4d880 100644 --- a/embassy-stm32/src/rcc/mod.rs +++ b/embassy-stm32/src/rcc/mod.rs @@ -86,6 +86,8 @@ static CLOCK_REFCOUNT: AtomicU32 = AtomicU32::new(0); #[cfg(feature = "low-power")] pub fn low_power_ready() -> bool { + trace!("clock refcount: {}", CLOCK_REFCOUNT.load(Ordering::SeqCst)); + CLOCK_REFCOUNT.load(Ordering::SeqCst) == 0 } diff --git a/embassy-stm32/src/rtc/v2.rs b/embassy-stm32/src/rtc/v2.rs index bcb127ec..197c3b8f 100644 --- a/embassy-stm32/src/rtc/v2.rs +++ b/embassy-stm32/src/rtc/v2.rs @@ -30,9 +30,6 @@ impl RtcInstant { let _ = RTC::regs().dr().read(); - trace!("ssr: {}", ssr); - trace!("st: {}", st); - Self { ssr, st } } } @@ -52,7 +49,12 @@ impl core::ops::Sub for RtcInstant { let other_ticks = rhs.st as u32 * 256 + (255 - rhs.ssr as u32); let rtc_ticks = self_ticks - other_ticks; - trace!("self, other, rtc ticks: {}, {}, {}", self_ticks, other_ticks, rtc_ticks); + trace!( + "rtc: instant sub: self, other, rtc ticks: {}, {}, {}", + self_ticks, + other_ticks, + rtc_ticks + ); Duration::from_ticks( ((((st as u32 * 256 + (255u32 - self.ssr as u32)) - (rhs.st as u32 * 256 + (255u32 - rhs.ssr as u32))) @@ -174,10 +176,10 @@ impl super::Rtc { rtc_ticks as u64 * TICK_HZ * (>::into(prescaler) as u64) / rtc_hz, ); - trace!("set wakeup timer for {} ms", duration.as_millis()); + trace!("rtc: set wakeup timer for {} ms", duration.as_millis()); self.write(false, |regs| { - regs.cr().modify(|w| w.set_wutie(true)); + // regs.cr().modify(|w| w.set_wutie(true)); regs.cr().modify(|w| w.set_wute(false)); regs.isr().modify(|w| w.set_wutf(false)); @@ -187,6 +189,15 @@ impl super::Rtc { regs.cr().modify(|w| w.set_wute(true)); }); + self.write(false, |regs| { + regs.cr().modify(|w| w.set_wutie(false)); + + regs.isr().modify(|w| w.set_wutf(false)); + crate::pac::PWR.cr1().modify(|w| w.set_cwuf(false)); + + regs.cr().modify(|w| w.set_wutie(true)); + }); + RtcInstant::now() } @@ -197,7 +208,7 @@ impl super::Rtc { /// note: this api is exposed for testing purposes until low power is implemented. /// it is not intended to be public pub(crate) fn stop_wakeup_alarm(&self) -> RtcInstant { - trace!("disable wakeup timer..."); + trace!("rtc: stop wakeup alarm..."); self.write(false, |regs| { regs.cr().modify(|w| w.set_wute(false)); diff --git a/embassy-stm32/src/time_driver.rs b/embassy-stm32/src/time_driver.rs index 2622442f..8e05346a 100644 --- a/embassy-stm32/src/time_driver.rs +++ b/embassy-stm32/src/time_driver.rs @@ -259,6 +259,64 @@ impl RtcDriver { let f: fn(*mut ()) = unsafe { mem::transmute(alarm.callback.get()) }; f(alarm.ctx.get()); } + + #[cfg(feature = "low-power")] + /// Compute the approximate amount of time until the next alarm + pub(crate) fn time_until_next_alarm(&self) -> embassy_time::Duration { + critical_section::with(|cs| { + let now = self.now() + 32; + + embassy_time::Duration::from_ticks( + self.alarms + .borrow(cs) + .iter() + .map(|alarm: &AlarmState| alarm.timestamp.get().saturating_sub(now)) + .min() + .unwrap_or(u64::MAX), + ) + }) + } + + #[cfg(feature = "low-power")] + /// Pause the timer + pub(crate) fn pause_time(&self) { + T::regs_gp16().cr1().modify(|w| w.set_cen(false)); + } + + #[cfg(feature = "low-power")] + /// Resume the timer with the given offset + pub(crate) fn resume_time(&self, offset: embassy_time::Duration) { + let offset = offset.as_ticks(); + let cnt = T::regs_gp16().cnt().read().cnt() as u32; + let period = self.period.load(Ordering::SeqCst); + + // Correct the race, if it exists + let period = if period & 1 == 1 && cnt < u16::MAX as u32 / 2 { + period + 1 + } else { + period + }; + + // Normalize to the full overflow + let period = (period / 2) * 2; + + // Add the offset + let period = period + 2 * (offset / u16::MAX as u64) as u32; + let cnt = cnt + (offset % u16::MAX as u64) as u32; + + let (cnt, period) = if cnt > u16::MAX as u32 { + (cnt - u16::MAX as u32, period + 2) + } else { + (cnt, period) + }; + + let period = if cnt > u16::MAX as u32 / 2 { period + 1 } else { period }; + + self.period.store(period, Ordering::SeqCst); + T::regs_gp16().cnt().write(|w| w.set_cnt(cnt as u16)); + + T::regs_gp16().cr1().modify(|w| w.set_cen(true)); + } } impl Driver for RtcDriver { @@ -329,6 +387,24 @@ impl Driver for RtcDriver { } } +#[cfg(feature = "low-power")] +/// Compute the approximate amount of time until the next alarm +pub(crate) fn time_until_next_alarm() -> embassy_time::Duration { + DRIVER.time_until_next_alarm() +} + +#[cfg(feature = "low-power")] +/// Pause the timer +pub(crate) fn pause_time() { + DRIVER.pause_time(); +} + +#[cfg(feature = "low-power")] +/// Resume the timer with the given offset +pub(crate) fn resume_time(offset: embassy_time::Duration) { + DRIVER.resume_time(offset); +} + pub(crate) fn init() { DRIVER.init() }