diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs index cf12a1ea..964819ab 100644 --- a/embassy-stm32/src/low_power.rs +++ b/embassy-stm32/src/low_power.rs @@ -9,12 +9,11 @@ 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}; +use crate::time_driver::{get_driver, RtcDriver}; const THREAD_PENDER: usize = usize::MAX; -const THRESHOLD: Duration = Duration::from_millis(500); -use crate::rtc::{Rtc, RtcInstant}; +use crate::rtc::Rtc; static mut EXECUTOR: Option = None; @@ -27,20 +26,30 @@ foreach_interrupt! { }; } +// pub fn timer_driver_pause_time() { +// pause_time(); +// } + pub fn stop_with_rtc(rtc: &'static Rtc) { unsafe { EXECUTOR.as_mut().unwrap() }.stop_with_rtc(rtc) } -pub fn start_wakeup_alarm(requested_duration: embassy_time::Duration) -> RtcInstant { - unsafe { EXECUTOR.as_mut().unwrap() } - .rtc - .unwrap() - .start_wakeup_alarm(requested_duration) -} - -pub fn stop_wakeup_alarm() -> RtcInstant { - unsafe { EXECUTOR.as_mut().unwrap() }.rtc.unwrap().stop_wakeup_alarm() -} +// pub fn start_wakeup_alarm(requested_duration: embassy_time::Duration) { +// let rtc_instant = unsafe { EXECUTOR.as_mut().unwrap() } +// .rtc +// .unwrap() +// .start_wakeup_alarm(requested_duration); +// +// unsafe { EXECUTOR.as_mut().unwrap() }.last_stop = Some(rtc_instant); +// } +// +// pub fn set_sleepdeep() { +// unsafe { EXECUTOR.as_mut().unwrap() }.scb.set_sleepdeep(); +// } +// +// pub fn stop_wakeup_alarm() -> RtcInstant { +// unsafe { EXECUTOR.as_mut().unwrap() }.rtc.unwrap().stop_wakeup_alarm() +// } /// Thread mode executor, using WFE/SEV. /// @@ -56,15 +65,15 @@ pub struct Executor { inner: raw::Executor, not_send: PhantomData<*mut ()>, scb: SCB, - pub(self) rtc: Option<&'static Rtc>, + time_driver: &'static RtcDriver, stop_time: embassy_time::Duration, next_alarm: embassy_time::Duration, - last_stop: Option, + wfe: u8, } impl Executor { /// Create a new Executor. - pub fn new() -> &'static mut Self { + pub fn take() -> &'static mut Self { unsafe { assert!(EXECUTOR.is_none()); @@ -72,10 +81,10 @@ impl Executor { inner: raw::Executor::new(THREAD_PENDER as *mut ()), not_send: PhantomData, scb: cortex_m::Peripherals::steal().SCB, - rtc: None, + time_driver: get_driver(), stop_time: Duration::from_ticks(0), next_alarm: Duration::from_ticks(u64::MAX), - last_stop: None, + wfe: 0, }); EXECUTOR.as_mut().unwrap() @@ -83,9 +92,31 @@ impl Executor { } unsafe fn on_wakeup_irq(&mut self) { - trace!("low power: one wakekup irq"); + trace!("low power: on wakeup irq"); - self.rtc.unwrap().clear_wakeup_alarm(); + if crate::pac::RTC.isr().read().wutf() { + trace!("low power: wutf set"); + } else { + trace!("low power: wutf not set"); + } + + self.time_driver.resume_time(); + trace!("low power: resume time"); + + crate::interrupt::typelevel::RTC_WKUP::disable(); + + // cortex_m::asm::bkpt(); + + // let time_elasped = self.rtc.unwrap().stop_wakeup_alarm() - self.last_stop.take().unwrap(); + // + // trace!("low power: {} ms elapsed", time_elasped.as_millis()); + // + // resume_time(time_elasped); + // trace!("low power: resume time"); + // + // self.scb.clear_sleepdeep(); + + // cortex_m::asm::bkpt(); // Self::get_scb().set_sleeponexit(); // // return; @@ -110,48 +141,33 @@ impl Executor { } pub(self) fn stop_with_rtc(&mut self, rtc: &'static Rtc) { - assert!(self.rtc.is_none()); + trace!("low power: stop with rtc configured"); + + self.time_driver.set_rtc(rtc); crate::interrupt::typelevel::RTC_WKUP::unpend(); unsafe { crate::interrupt::typelevel::RTC_WKUP::enable() }; EXTI.rtsr(0).modify(|w| w.set_line(22, true)); EXTI.imr(0).modify(|w| w.set_line(22, true)); - - self.rtc = Some(rtc); } - fn configure_pwr(&self) { - // defeat the borrow checker - let s = unsafe { EXECUTOR.as_mut().unwrap() }; + fn configure_pwr(&mut self) { + trace!("low power: configure_pwr"); - // trace!("configure_pwr"); - // - // if !low_power_ready() { - // trace!("configure_pwr: low power not ready"); - // return; - // } - // - // 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; - // } - // - // unsafe { - // NEXT_ALARM = time_until_next_alarm; - // if RTC_INSTANT.is_none() { - // RTC_INSTANT = Some(start_wakeup_alarm(time_until_next_alarm)) - // } - // }; - // - // // return; - // - // pause_time(); - // - // trace!("enter stop..."); - // - // Self::get_scb().set_sleepdeep(); + self.scb.clear_sleepdeep(); + if !low_power_ready() { + trace!("low power: configure_pwr: low power not ready"); + return; + } + + if self.time_driver.pause_time().is_err() { + trace!("low power: configure_pwr: time driver failed to pause"); + return; + } + + trace!("low power: enter stop..."); + self.scb.set_sleepdeep(); } /// Run the executor. @@ -173,11 +189,11 @@ impl Executor { /// /// This function never returns. pub fn run(&'static mut self, init: impl FnOnce(Spawner)) -> ! { - init(self.inner.spawner()); + init(unsafe { EXECUTOR.as_mut().unwrap() }.inner.spawner()); loop { unsafe { - self.inner.poll(); + EXECUTOR.as_mut().unwrap().inner.poll(); self.configure_pwr(); asm!("wfe"); }; diff --git a/embassy-stm32/src/rtc/v2.rs b/embassy-stm32/src/rtc/v2.rs index 197c3b8f..525dc45a 100644 --- a/embassy-stm32/src/rtc/v2.rs +++ b/embassy-stm32/src/rtc/v2.rs @@ -30,6 +30,8 @@ impl RtcInstant { let _ = RTC::regs().dr().read(); + trace!("rtc: instant now: st, ssr: {}, {}", st, ssr); + Self { ssr, st } } } @@ -146,6 +148,21 @@ impl super::Rtc { } } + // pub(crate) fn clear_wakeup_alarm(&self) { + // use crate::interrupt::typelevel::Interrupt; + // + // 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)); + // crate::pac::EXTI.pr(0).modify(|w| w.set_line(22, false)); + // crate::interrupt::typelevel::RTC_WKUP::unpend(); + // + // regs.cr().modify(|w| w.set_wutie(true)); + // }); + // } + #[allow(dead_code)] #[cfg(all(feature = "time", any(stm32wb, stm32f4)))] /// start the wakeup alarm and return the actual duration of the alarm @@ -157,6 +174,7 @@ impl super::Rtc { pub(crate) fn start_wakeup_alarm(&self, requested_duration: embassy_time::Duration) -> RtcInstant { use embassy_time::{Duration, TICK_HZ}; + use crate::interrupt::typelevel::Interrupt; use crate::rcc::get_freqs; let rtc_hz = unsafe { get_freqs() }.rtc.unwrap().0 as u64; @@ -176,28 +194,40 @@ impl super::Rtc { rtc_ticks as u64 * TICK_HZ * (>::into(prescaler) as u64) / rtc_hz, ); - 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_wute(false)); regs.isr().modify(|w| w.set_wutf(false)); while !regs.isr().read().wutwf() {} regs.cr().modify(|w| w.set_wucksel(prescaler.into())); 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)); }); + trace!("rtc: start wakeup alarm for {} ms", duration.as_millis()); + + // self.write(false, |regs| { + // regs.cr().modify(|w| w.set_wutie(false)); + // + // regs.isr().modify(|w| w.set_wutf(false)); + // + // regs.cr().modify(|w| w.set_wutie(true)); + // }); + // + // trace!("rtc: clear wuf..."); + // crate::pac::PWR.cr1().modify(|w| w.set_cwuf(true)); + // while crate::pac::PWR.csr1().read().wuf() {} + // trace!("rtc: clear wuf...done"); + // + // crate::pac::PWR + // .cr1() + // .modify(|w| w.set_pdds(crate::pac::pwr::vals::Pdds::STANDBY_MODE)); + // + // // crate::pac::PWR.cr1().modify(|w| w.set_lpds(true)); + // + // // crate::pac::EXTI.pr(0).modify(|w| w.set_line(22, true)); + // crate::interrupt::typelevel::RTC_WKUP::unpend(); + RtcInstant::now() } @@ -211,6 +241,7 @@ impl super::Rtc { trace!("rtc: stop wakeup alarm..."); self.write(false, |regs| { + regs.cr().modify(|w| w.set_wutie(false)); regs.cr().modify(|w| w.set_wute(false)); regs.isr().modify(|w| w.set_wutf(false)); }); diff --git a/embassy-stm32/src/time_driver.rs b/embassy-stm32/src/time_driver.rs index 8e05346a..56f42aa9 100644 --- a/embassy-stm32/src/time_driver.rs +++ b/embassy-stm32/src/time_driver.rs @@ -14,6 +14,8 @@ use stm32_metapac::timer::regs; use crate::interrupt::typelevel::Interrupt; use crate::pac::timer::vals; use crate::rcc::sealed::RccPeripheral; +#[cfg(feature = "low-power")] +use crate::rtc::{Rtc, RtcInstant}; use crate::timer::sealed::{Basic16bitInstance as BasicInstance, GeneralPurpose16bitInstance as Instance}; use crate::{interrupt, peripherals}; @@ -130,12 +132,16 @@ impl AlarmState { } } -struct RtcDriver { +pub(crate) struct RtcDriver { /// Number of 2^15 periods elapsed since boot. period: AtomicU32, alarm_count: AtomicU8, /// Timestamp at which to fire alarm. u64::MAX if no alarm is scheduled. alarms: Mutex, + #[cfg(feature = "low-power")] + rtc: Mutex>>, + #[cfg(feature = "low-power")] + stop_time: Mutex>>, } const ALARM_STATE_NEW: AlarmState = AlarmState::new(); @@ -144,6 +150,10 @@ embassy_time::time_driver_impl!(static DRIVER: RtcDriver = RtcDriver { period: AtomicU32::new(0), alarm_count: AtomicU8::new(0), alarms: Mutex::const_new(CriticalSectionRawMutex::new(), [ALARM_STATE_NEW; ALARM_COUNT]), + #[cfg(feature = "low-power")] + rtc: Mutex::const_new(CriticalSectionRawMutex::new(), Cell::new(None)), + #[cfg(feature = "low-power")] + stop_time: Mutex::const_new(CriticalSectionRawMutex::new(), Cell::new(None)), }); impl RtcDriver { @@ -260,9 +270,15 @@ impl RtcDriver { f(alarm.ctx.get()); } + #[cfg(feature = "low-power")] + /// Set the rtc but panic if it's already been set + pub(crate) fn set_rtc(&self, rtc: &'static Rtc) { + critical_section::with(|cs| assert!(self.rtc.borrow(cs).replace(Some(rtc)).is_none())); + } + #[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 { + fn time_until_next_alarm(&self) -> embassy_time::Duration { critical_section::with(|cs| { let now = self.now() + 32; @@ -278,14 +294,8 @@ impl RtcDriver { } #[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) { + /// Add the given offset to the current time + fn add_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); @@ -315,6 +325,66 @@ impl RtcDriver { self.period.store(period, Ordering::SeqCst); T::regs_gp16().cnt().write(|w| w.set_cnt(cnt as u16)); + // Now, recompute all alarms + critical_section::with(|cs| { + for i in 0..ALARM_COUNT { + let alarm_handle = unsafe { AlarmHandle::new(i as u8) }; + let alarm = self.get_alarm(cs, alarm_handle); + + self.set_alarm(alarm_handle, alarm.timestamp.get()); + } + }) + } + + #[cfg(feature = "low-power")] + /// Stop the wakeup alarm, if enabled, and add the appropriate offset + fn stop_wakeup_alarm(&self) { + critical_section::with(|cs| { + if let Some(stop_time) = self.stop_time.borrow(cs).take() { + let current_time = self.rtc.borrow(cs).get().unwrap().stop_wakeup_alarm(); + self.add_time(current_time - stop_time); + } + }); + } + + #[cfg(feature = "low-power")] + /// Pause the timer if ready; return err if not + pub(crate) fn pause_time(&self) -> Result<(), ()> { + /* + If the wakeup timer is currently running, then we need to stop it and + add the elapsed time to the current time + */ + self.stop_wakeup_alarm(); + + let time_until_next_alarm = self.time_until_next_alarm(); + if time_until_next_alarm < embassy_time::Duration::from_millis(250) { + Err(()) + } else { + critical_section::with(|cs| { + assert!(self + .stop_time + .borrow(cs) + .replace(Some( + self.rtc + .borrow(cs) + .get() + .unwrap() + .start_wakeup_alarm(time_until_next_alarm) + )) + .is_none()); + }); + + T::regs_gp16().cr1().modify(|w| w.set_cen(false)); + + Ok(()) + } + } + + #[cfg(feature = "low-power")] + /// Resume the timer with the given offset + pub(crate) fn resume_time(&self) { + self.stop_wakeup_alarm(); + T::regs_gp16().cr1().modify(|w| w.set_cen(true)); } } @@ -388,21 +458,8 @@ 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 get_driver() -> &'static RtcDriver { + &DRIVER } pub(crate) fn init() {