5 files changed, 133 insertions, 20 deletions

diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs
index 8c87ea7d5..ec8648ee4 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 7814fa384..0d9506aaa 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<crate::rtc::RtcInstant> = 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();
+
+        STOP_TIME += elapsed;
+        // let to_next = NEXT_ALARM - STOP_TIME;
+        let to_next = Duration::from_secs(3);
 
-    fn time_until_next_alarm(&self) -> Duration {
-        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 3c75923e5..45a4d880d 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 bcb127ecb..197c3b8f9 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 * (<WakeupPrescaler as Into<u32>>::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 2622442f4..8e05346ad 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()
 }