4 files changed, 107 insertions, 164 deletions

diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index 964819abb..425362162 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -66,9 +66,6 @@ pub struct Executor {
     not_send: PhantomData<*mut ()>,
     scb: SCB,
     time_driver: &'static RtcDriver,
-    stop_time: embassy_time::Duration,
-    next_alarm: embassy_time::Duration,
-    wfe: u8,
 }
 
 impl Executor {
@@ -82,9 +79,6 @@ impl Executor {
                 not_send: PhantomData,
                 scb: cortex_m::Peripherals::steal().SCB,
                 time_driver: get_driver(),
-                stop_time: Duration::from_ticks(0),
-                next_alarm: Duration::from_ticks(u64::MAX),
-                wfe: 0,
             });
 
             EXECUTOR.as_mut().unwrap()
@@ -94,50 +88,8 @@ impl Executor {
     unsafe fn on_wakeup_irq(&mut self) {
         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");
-
-        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;
-        //
-        //        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);
-        //
-        //        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();
-        //        }
     }
 
     pub(self) fn stop_with_rtc(&mut self, rtc: &'static Rtc) {
diff --git a/embassy-stm32/src/rtc/mod.rs b/embassy-stm32/src/rtc/mod.rs
index a6102077a..1f1abb78d 100644
--- a/embassy-stm32/src/rtc/mod.rs
+++ b/embassy-stm32/src/rtc/mod.rs
@@ -1,6 +1,14 @@
 //! RTC peripheral abstraction
 mod datetime;
 
+#[cfg(feature = "low-power")]
+use core::cell::Cell;
+
+#[cfg(feature = "low-power")]
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+#[cfg(feature = "low-power")]
+use embassy_sync::blocking_mutex::Mutex;
+
 pub use self::datetime::{DateTime, DayOfWeek, Error as DateTimeError};
 
 /// refer to AN4759 to compare features of RTC2 and RTC3
@@ -30,9 +38,73 @@ pub enum RtcError {
     NotRunning,
 }
 
+#[cfg(feature = "low-power")]
+/// Represents an instant in time that can be substracted to compute a duration
+struct RtcInstant {
+    second: u8,
+    subsecond: u16,
+}
+
+#[cfg(feature = "low-power")]
+impl RtcInstant {
+    pub fn now() -> Self {
+        let tr = RTC::regs().tr().read();
+        let tr2 = RTC::regs().tr().read();
+        let ssr = RTC::regs().ssr().read().ss();
+        let ssr2 = RTC::regs().ssr().read().ss();
+
+        let st = bcd2_to_byte((tr.st(), tr.su()));
+        let st2 = bcd2_to_byte((tr2.st(), tr2.su()));
+
+        assert!(st == st2);
+        assert!(ssr == ssr2);
+
+        let _ = RTC::regs().dr().read();
+
+        let subsecond = ssr;
+        let second = st;
+
+        // trace!("rtc: instant now: st, ssr: {}, {}", st, ssr);
+
+        Self { second, subsecond }
+    }
+}
+
+#[cfg(feature = "low-power")]
+impl core::ops::Sub for RtcInstant {
+    type Output = embassy_time::Duration;
+
+    fn sub(self, rhs: Self) -> Self::Output {
+        use embassy_time::{Duration, TICK_HZ};
+
+        let second = if self.second < rhs.second {
+            self.second + 60
+        } else {
+            self.second
+        };
+
+        // TODO: read prescaler
+
+        let self_ticks = second as u32 * 256 + (255 - self.subsecond as u32);
+        let other_ticks = rhs.second as u32 * 256 + (255 - rhs.subsecond as u32);
+        let rtc_ticks = self_ticks - other_ticks;
+
+        //        trace!(
+        //            "rtc: instant sub: self, other, rtc ticks: {}, {}, {}",
+        //            self_ticks,
+        //            other_ticks,
+        //            rtc_ticks
+        //        );
+
+        Duration::from_ticks(((rtc_ticks * TICK_HZ as u32) / 256u32) as u64)
+    }
+}
+
 /// RTC Abstraction
 pub struct Rtc {
     rtc_config: RtcConfig,
+    #[cfg(feature = "low-power")]
+    stop_time: Mutex<CriticalSectionRawMutex, Cell<Option<RtcInstant>>>,
 }
 
 #[derive(Copy, Clone, Debug, PartialEq)]
@@ -108,8 +180,15 @@ impl Rtc {
     pub fn new(_rtc: impl Peripheral<P = RTC>, rtc_config: RtcConfig) -> Self {
         RTC::enable_peripheral_clk();
 
+        #[cfg(not(feature = "low-power"))]
         let mut rtc_struct = Self { rtc_config };
 
+        #[cfg(feature = "low-power")]
+        let mut rtc_struct = Self {
+            rtc_config,
+            stop_time: Mutex::const_new(CriticalSectionRawMutex::new(), Cell::new(None)),
+        };
+
         Self::enable();
 
         rtc_struct.configure(rtc_config);
diff --git a/embassy-stm32/src/rtc/v2.rs b/embassy-stm32/src/rtc/v2.rs
index 525dc45a2..d73e7083c 100644
--- a/embassy-stm32/src/rtc/v2.rs
+++ b/embassy-stm32/src/rtc/v2.rs
@@ -1,71 +1,12 @@
 use stm32_metapac::rtc::vals::{Init, Osel, Pol};
 
+#[cfg(feature = "low-power")]
+use super::RtcInstant;
 use super::{sealed, RtcClockSource, RtcConfig};
 use crate::pac::rtc::Rtc;
 use crate::peripherals::RTC;
 use crate::rtc::sealed::Instance;
 
-#[cfg(all(feature = "time", any(stm32wb, stm32f4)))]
-pub struct RtcInstant {
-    ssr: u16,
-    st: u8,
-}
-
-#[cfg(all(feature = "time", any(stm32wb, stm32f4)))]
-impl RtcInstant {
-    pub fn now() -> Self {
-        // TODO: read value twice
-        use crate::rtc::bcd2_to_byte;
-
-        let tr = RTC::regs().tr().read();
-        let tr2 = RTC::regs().tr().read();
-        let ssr = RTC::regs().ssr().read().ss();
-        let ssr2 = RTC::regs().ssr().read().ss();
-
-        let st = bcd2_to_byte((tr.st(), tr.su()));
-        let st2 = bcd2_to_byte((tr2.st(), tr2.su()));
-
-        assert!(st == st2);
-        assert!(ssr == ssr2);
-
-        let _ = RTC::regs().dr().read();
-
-        trace!("rtc: instant now: st, ssr: {}, {}", st, ssr);
-
-        Self { ssr, st }
-    }
-}
-
-#[cfg(all(feature = "time", any(stm32wb, stm32f4)))]
-impl core::ops::Sub for RtcInstant {
-    type Output = embassy_time::Duration;
-
-    fn sub(self, rhs: Self) -> Self::Output {
-        use embassy_time::{Duration, TICK_HZ};
-
-        let st = if self.st < rhs.st { self.st + 60 } else { self.st };
-
-        // TODO: read prescaler
-
-        let self_ticks = st as u32 * 256 + (255 - self.ssr as u32);
-        let other_ticks = rhs.st as u32 * 256 + (255 - rhs.ssr as u32);
-        let rtc_ticks = self_ticks - other_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)))
-                * TICK_HZ as u32) as u32
-                / 256u32) as u64,
-        )
-    }
-}
-
 #[allow(dead_code)]
 #[derive(Clone, Copy, Debug)]
 pub(crate) enum WakeupPrescaler {
@@ -165,16 +106,11 @@ impl super::Rtc {
 
     #[allow(dead_code)]
     #[cfg(all(feature = "time", any(stm32wb, stm32f4)))]
-    /// start the wakeup alarm and return the actual duration of the alarm
-    /// the actual duration will be the closest value possible that is less
-    /// than the requested duration.
-    ///
-    /// note: this api is exposed for testing purposes until low power is implemented.
-    /// it is not intended to be public
-    pub(crate) fn start_wakeup_alarm(&self, requested_duration: embassy_time::Duration) -> RtcInstant {
+    /// start the wakeup alarm and wtih a duration that is as close to but less than
+    /// the requested duration, and record the instant the wakeup alarm was started
+    pub(crate) fn start_wakeup_alarm(&self, requested_duration: embassy_time::Duration) {
         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;
@@ -206,47 +142,34 @@ impl super::Rtc {
 
         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()
+        critical_section::with(|cs| assert!(self.stop_time.borrow(cs).replace(Some(RtcInstant::now())).is_none()))
     }
 
     #[allow(dead_code)]
     #[cfg(all(feature = "time", any(stm32wb, stm32f4)))]
-    /// stop the wakeup alarm and return the time remaining
-    ///
-    /// 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 {
+    /// stop the wakeup alarm and return the time elapsed since `start_wakeup_alarm`
+    /// was called, otherwise none
+    pub(crate) fn stop_wakeup_alarm(&self) -> Option<embassy_time::Duration> {
+        use crate::interrupt::typelevel::Interrupt;
+
         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));
+
+            crate::pac::EXTI.pr(0).modify(|w| w.set_line(22, true));
+            crate::interrupt::typelevel::RTC_WKUP::unpend();
         });
 
-        RtcInstant::now()
+        critical_section::with(|cs| {
+            if let Some(stop_time) = self.stop_time.borrow(cs).take() {
+                Some(RtcInstant::now() - stop_time)
+            } else {
+                None
+            }
+        })
     }
 
     #[allow(dead_code)]
diff --git a/embassy-stm32/src/time_driver.rs b/embassy-stm32/src/time_driver.rs
index 56f42aa96..99d423d08 100644
--- a/embassy-stm32/src/time_driver.rs
+++ b/embassy-stm32/src/time_driver.rs
@@ -15,7 +15,7 @@ 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::rtc::Rtc;
 use crate::timer::sealed::{Basic16bitInstance as BasicInstance, GeneralPurpose16bitInstance as Instance};
 use crate::{interrupt, peripherals};
 
@@ -140,8 +140,6 @@ pub(crate) struct RtcDriver {
     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();
@@ -152,8 +150,6 @@ embassy_time::time_driver_impl!(static DRIVER: RtcDriver = RtcDriver {
     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 {
@@ -340,9 +336,8 @@ impl RtcDriver {
     /// 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);
+            if let Some(offset) = self.rtc.borrow(cs).get().unwrap().stop_wakeup_alarm() {
+                self.add_time(offset);
             }
         });
     }
@@ -361,17 +356,11 @@ impl RtcDriver {
             Err(())
         } else {
             critical_section::with(|cs| {
-                assert!(self
-                    .stop_time
+                self.rtc
                     .borrow(cs)
-                    .replace(Some(
-                        self.rtc
-                            .borrow(cs)
-                            .get()
-                            .unwrap()
-                            .start_wakeup_alarm(time_until_next_alarm)
-                    ))
-                    .is_none());
+                    .get()
+                    .unwrap()
+                    .start_wakeup_alarm(time_until_next_alarm);
             });
 
             T::regs_gp16().cr1().modify(|w| w.set_cen(false));