3 files changed, 196 insertions, 92 deletions

diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index cf12a1ea5..964819abb 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<Executor> = 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<crate::rtc::RtcInstant>,
+    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");
+
+        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");
 
-        self.rtc.unwrap().clear_wakeup_alarm();
+        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 197c3b8f9..525dc45a2 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 * (<WakeupPrescaler as Into<u32>>::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 8e05346ad..56f42aa96 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<CriticalSectionRawMutex, [AlarmState; ALARM_COUNT]>,
+    #[cfg(feature = "low-power")]
+    rtc: Mutex<CriticalSectionRawMutex, Cell<Option<&'static Rtc>>>,
+    #[cfg(feature = "low-power")]
+    stop_time: Mutex<CriticalSectionRawMutex, Cell<Option<RtcInstant>>>,
 }
 
 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 {
@@ -261,8 +271,14 @@ impl RtcDriver {
     }
 
     #[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() {