2 files changed, 42 insertions, 59 deletions

diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index 696dfe83f..36c7e2242 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -228,10 +228,13 @@ impl Executor {
 
     fn stop_mode(_cs: CriticalSection) -> Option<StopMode> {
         if unsafe { crate::rcc::REFCOUNT_STOP2 == 0 && crate::rcc::REFCOUNT_STOP1 == 0 } {
+            trace!("low power: stop 2");
             Some(StopMode::Stop2)
         } else if unsafe { crate::rcc::REFCOUNT_STOP1 == 0 } {
+            trace!("low power: stop 1");
             Some(StopMode::Stop1)
         } else {
+            trace!("low power: not ready to stop");
             None
         }
     }
@@ -258,27 +261,18 @@ impl Executor {
 
         compiler_fence(Ordering::SeqCst);
 
-        let stop_mode = critical_section::with(|cs| Self::stop_mode(cs));
-
-        if stop_mode.is_none() {
-            trace!("low power: not ready to stop");
-            return;
-        }
-
-        if get_driver().pause_time().is_err() {
-            trace!("low power: failed to pause time");
-            return;
-        }
+        critical_section::with(|cs| {
+            let stop_mode = Self::stop_mode(cs)?;
+            let _ = get_driver().pause_time(cs).ok()?;
 
-        let stop_mode = stop_mode.unwrap();
-        match stop_mode {
-            StopMode::Stop1 => trace!("low power: stop 1"),
-            StopMode::Stop2 => trace!("low power: stop 2"),
-        }
-        self.configure_stop(stop_mode);
+            Some(stop_mode)
+        })
+        .map(|stop_mode| {
+            self.configure_stop(stop_mode);
 
-        #[cfg(not(feature = "low-power-debug-with-sleep"))]
-        self.scb.set_sleepdeep();
+            #[cfg(not(feature = "low-power-debug-with-sleep"))]
+            self.scb.set_sleepdeep();
+        });
     }
 
     /// Run the executor.
diff --git a/embassy-stm32/src/time_driver.rs b/embassy-stm32/src/time_driver.rs
index bc34892ee..6d93b430a 100644
--- a/embassy-stm32/src/time_driver.rs
+++ b/embassy-stm32/src/time_driver.rs
@@ -380,13 +380,15 @@ impl RtcDriver {
     #[cfg(feature = "low-power")]
     /// Stop the wakeup alarm, if enabled, and add the appropriate offset
     fn stop_wakeup_alarm(&self, cs: CriticalSection) {
-        if let Some(offset) = self.rtc.borrow(cs).borrow_mut().as_mut().unwrap().stop_wakeup_alarm(cs) {
+        if !regs_gp16().cr1().read().cen()
+            && let Some(offset) = self.rtc.borrow(cs).borrow_mut().as_mut().unwrap().stop_wakeup_alarm(cs)
+        {
             self.add_time(offset, cs);
         }
     }
 
     /*
-        Low-power public functions: all create or require a critical section
+        Low-power public functions: all require a critical section
     */
     #[cfg(feature = "low-power")]
     pub(crate) fn set_min_stop_pause(&self, cs: CriticalSection, min_stop_pause: embassy_time::Duration) {
@@ -403,49 +405,36 @@ impl RtcDriver {
 
     #[cfg(feature = "low-power")]
     /// Pause the timer if ready; return err if not
-    pub(crate) fn pause_time(&self) -> Result<(), ()> {
-        critical_section::with(|cs| {
-            /*
-                If the wakeup timer is currently running, then we need to stop it and
-                add the elapsed time to the current time, as this will impact the result
-                of `time_until_next_alarm`.
-            */
-            self.stop_wakeup_alarm(cs);
-
-            let time_until_next_alarm = self.time_until_next_alarm(cs);
-            if time_until_next_alarm < self.min_stop_pause.borrow(cs).get() {
-                trace!(
-                    "time_until_next_alarm < self.min_stop_pause ({})",
-                    time_until_next_alarm
-                );
-                Err(())
-            } else {
-                self.rtc
-                    .borrow(cs)
-                    .borrow_mut()
-                    .as_mut()
-                    .unwrap()
-                    .start_wakeup_alarm(time_until_next_alarm, cs);
-
-                regs_gp16().cr1().modify(|w| w.set_cen(false));
-                // save the count for the timer as its lost in STOP2 for stm32wlex
-                #[cfg(stm32wlex)]
-                self.saved_count
-                    .store(regs_gp16().cnt().read().cnt() as u16, Ordering::SeqCst);
-                Ok(())
-            }
-        })
+    pub(crate) fn pause_time(&self, cs: CriticalSection) -> Result<(), ()> {
+        self.stop_wakeup_alarm(cs);
+
+        let time_until_next_alarm = self.time_until_next_alarm(cs);
+        if time_until_next_alarm < self.min_stop_pause.borrow(cs).get() {
+            trace!(
+                "time_until_next_alarm < self.min_stop_pause ({})",
+                time_until_next_alarm
+            );
+            Err(())
+        } else {
+            self.rtc
+                .borrow(cs)
+                .borrow_mut()
+                .as_mut()
+                .unwrap()
+                .start_wakeup_alarm(time_until_next_alarm, cs);
+
+            regs_gp16().cr1().modify(|w| w.set_cen(false));
+            // save the count for the timer as its lost in STOP2 for stm32wlex
+            #[cfg(stm32wlex)]
+            self.saved_count
+                .store(regs_gp16().cnt().read().cnt() as u16, Ordering::SeqCst);
+            Ok(())
+        }
     }
 
     #[cfg(feature = "low-power")]
     /// Resume the timer with the given offset
     pub(crate) fn resume_time(&self, cs: CriticalSection) {
-        if regs_gp16().cr1().read().cen() {
-            // Time isn't currently stopped
-
-            return;
-        }
-
         self.stop_wakeup_alarm(cs);
 
         regs_gp16().cr1().modify(|w| w.set_cen(true));