5 files changed, 64 insertions, 82 deletions

diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index 65b93f8a4..7e678d323 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -6,7 +6,6 @@ use embassy_executor::*;
 
 use crate::interrupt;
 use crate::interrupt::typelevel::Interrupt;
-use crate::pac::EXTI;
 use crate::rcc::low_power_ready;
 use crate::time_driver::{get_driver, RtcDriver};
 
@@ -99,8 +98,7 @@ impl Executor {
         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));
+        rtc.enable_wakeup_line();
     }
 
     fn configure_pwr(&mut self) {
diff --git a/embassy-stm32/src/rcc/bd.rs b/embassy-stm32/src/rcc/bd.rs
index 0fc116ed8..4d8ed82aa 100644
--- a/embassy-stm32/src/rcc/bd.rs
+++ b/embassy-stm32/src/rcc/bd.rs
@@ -87,13 +87,14 @@ impl BackupDomain {
     }
 
     #[cfg(any(
-        rtc_v2f0, rtc_v2f2, rtc_v2f3, rtc_v2f4, rtc_v2f7, rtc_v2h7, rtc_v2l0, rtc_v2l1, rtc_v2l4, rtc_v2wb
+        rtc_v2f0, rtc_v2f2, rtc_v2f3, rtc_v2f4, rtc_v2f7, rtc_v2h7, rtc_v2l0, rtc_v2l1, rtc_v2l4, rtc_v2wb, rtc_v3,
+        rtc_v3u5
     ))]
     #[allow(dead_code)]
     pub fn enable_rtc() {
         let reg = Self::read();
 
-        #[cfg(any(rtc_v2h7, rtc_v2l4, rtc_v2wb))]
+        #[cfg(any(rtc_v2h7, rtc_v2l4, rtc_v2wb, rtc_v3, rtc_v3u5))]
         assert!(!reg.lsecsson(), "RTC is not compatible with LSE CSS, yet.");
 
         if !reg.rtcen() {
@@ -102,47 +103,21 @@ impl BackupDomain {
 
             Self::modify(|w| {
                 // Reset
-                #[cfg(not(any(rtc_v2l0, rtc_v2l1)))]
+                #[cfg(not(any(rtc_v2l0, rtc_v2l1, rtc_v2f2)))]
                 w.set_bdrst(false);
 
                 w.set_rtcen(true);
                 w.set_rtcsel(reg.rtcsel());
 
                 // Restore bcdr
-                #[cfg(any(rtc_v2l4, rtc_v2wb))]
+                #[cfg(any(rtc_v2l4, rtc_v2wb, rtc_v3, rtc_v3u5))]
                 w.set_lscosel(reg.lscosel());
-                #[cfg(any(rtc_v2l4, rtc_v2wb))]
+                #[cfg(any(rtc_v2l4, rtc_v2wb, rtc_v3, rtc_v3u5))]
                 w.set_lscoen(reg.lscoen());
 
                 w.set_lseon(reg.lseon());
 
-                #[cfg(any(rtc_v2f0, rtc_v2f7, rtc_v2h7, rtc_v2l4, rtc_v2wb))]
-                w.set_lsedrv(reg.lsedrv());
-                w.set_lsebyp(reg.lsebyp());
-            });
-        }
-    }
-
-    #[cfg(any(rtc_v3, rtc_v3u5))]
-    #[allow(dead_code)]
-    pub fn enable_rtc() {
-        let reg = Self::read();
-        assert!(!reg.lsecsson(), "RTC is not compatible with LSE CSS, yet.");
-
-        if !reg.rtcen() {
-            Self::modify(|w| w.set_bdrst(true));
-
-            Self::modify(|w| {
-                w.set_bdrst(false);
-
-                w.set_rtcen(true);
-                w.set_rtcsel(reg.rtcsel());
-
-                // Restore bcdr
-                w.set_lscosel(reg.lscosel());
-                w.set_lscoen(reg.lscoen());
-
-                w.set_lseon(reg.lseon());
+                #[cfg(any(rtc_v2f0, rtc_v2f7, rtc_v2h7, rtc_v2l4, rtc_v2wb, rtc_v3, rtc_v3u5))]
                 w.set_lsedrv(reg.lsedrv());
                 w.set_lsebyp(reg.lsebyp());
             });
diff --git a/embassy-stm32/src/rtc/mod.rs b/embassy-stm32/src/rtc/mod.rs
index c408b2d61..796fd7d96 100644
--- a/embassy-stm32/src/rtc/mod.rs
+++ b/embassy-stm32/src/rtc/mod.rs
@@ -47,28 +47,15 @@ struct RtcInstant {
     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(all(feature = "low-power", feature = "defmt"))]
+impl defmt::Format for RtcInstant {
+    fn format(&self, fmt: defmt::Formatter) {
+        defmt::write!(
+            fmt,
+            "{}:{}",
+            self.second,
+            RTC::regs().prer().read().prediv_s() - self.subsecond,
+        )
     }
 }
 
@@ -85,20 +72,13 @@ impl core::ops::Sub for RtcInstant {
             self.second
         };
 
-        // TODO: read prescaler
+        let psc = RTC::regs().prer().read().prediv_s() as u32;
 
-        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 self_ticks = second as u32 * (psc + 1) + (psc - self.subsecond as u32);
+        let other_ticks = rhs.second as u32 * (psc + 1) + (psc - 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)
+        Duration::from_ticks(((rtc_ticks * TICK_HZ as u32) / (psc + 1)) as u64)
     }
 }
 
@@ -198,6 +178,20 @@ impl Rtc {
         Ok(())
     }
 
+    #[cfg(feature = "low-power")]
+    /// Return the current instant.
+    fn instant(&self) -> RtcInstant {
+        let r = RTC::regs();
+        let tr = r.tr().read();
+        let subsecond = r.ssr().read().ss();
+        let second = bcd2_to_byte((tr.st(), tr.su()));
+
+        // Unlock the registers
+        r.dr().read();
+
+        RtcInstant { second, subsecond }
+    }
+
     /// Return the current datetime.
     ///
     /// # Errors
diff --git a/embassy-stm32/src/rtc/v2.rs b/embassy-stm32/src/rtc/v2.rs
index 1e0ca9b48..49f66e957 100644
--- a/embassy-stm32/src/rtc/v2.rs
+++ b/embassy-stm32/src/rtc/v2.rs
@@ -1,7 +1,5 @@
 use stm32_metapac::rtc::vals::{Init, Osel, Pol};
 
-#[cfg(feature = "low-power")]
-use super::RtcInstant;
 use super::{sealed, RtcConfig};
 use crate::pac::rtc::Rtc;
 use crate::peripherals::RTC;
@@ -86,17 +84,14 @@ impl super::Rtc {
         let rtc_ticks = requested_duration.as_ticks() * rtc_hz / TICK_HZ;
         let prescaler = WakeupPrescaler::compute_min((rtc_ticks / u16::MAX as u64) as u32);
 
-        // adjust the rtc ticks to the prescaler
+        // adjust the rtc ticks to the prescaler and subtract one rtc tick
         let rtc_ticks = rtc_ticks / (<WakeupPrescaler as Into<u32>>::into(prescaler) as u64);
         let rtc_ticks = if rtc_ticks >= u16::MAX as u64 {
             u16::MAX - 1
         } else {
             rtc_ticks as u16
-        };
-
-        let duration = Duration::from_ticks(
-            rtc_ticks as u64 * TICK_HZ * (<WakeupPrescaler as Into<u32>>::into(prescaler) as u64) / rtc_hz,
-        );
+        }
+        .saturating_sub(1);
 
         self.write(false, |regs| {
             regs.cr().modify(|w| w.set_wute(false));
@@ -104,13 +99,31 @@ impl super::Rtc {
             while !regs.isr().read().wutwf() {}
 
             regs.cr().modify(|w| w.set_wucksel(prescaler.into()));
+            regs.wutr().write(|w| w.set_wut(rtc_ticks));
             regs.cr().modify(|w| w.set_wute(true));
             regs.cr().modify(|w| w.set_wutie(true));
         });
 
-        trace!("rtc: start wakeup alarm for {} ms", duration.as_millis());
+        trace!(
+            "rtc: start wakeup alarm for {} ms (psc: {}, ticks: {}) at {}",
+            Duration::from_ticks(
+                rtc_ticks as u64 * TICK_HZ * (<WakeupPrescaler as Into<u32>>::into(prescaler) as u64) / rtc_hz,
+            )
+            .as_millis(),
+            <WakeupPrescaler as Into<u32>>::into(prescaler),
+            rtc_ticks,
+            self.instant(),
+        );
+
+        critical_section::with(|cs| assert!(self.stop_time.borrow(cs).replace(Some(self.instant())).is_none()))
+    }
+
+    #[cfg(feature = "low-power")]
+    pub(crate) fn enable_wakeup_line(&self) {
+        use crate::pac::EXTI;
 
-        critical_section::with(|cs| assert!(self.stop_time.borrow(cs).replace(Some(RtcInstant::now())).is_none()))
+        EXTI.rtsr(0).modify(|w| w.set_line(22, true));
+        EXTI.imr(0).modify(|w| w.set_line(22, true));
     }
 
     #[cfg(feature = "low-power")]
@@ -119,7 +132,7 @@ impl super::Rtc {
     pub(crate) fn stop_wakeup_alarm(&self) -> Option<embassy_time::Duration> {
         use crate::interrupt::typelevel::Interrupt;
 
-        trace!("rtc: stop wakeup alarm...");
+        trace!("rtc: stop wakeup alarm at {}", self.instant());
 
         self.write(false, |regs| {
             regs.cr().modify(|w| w.set_wutie(false));
@@ -132,7 +145,7 @@ impl super::Rtc {
 
         critical_section::with(|cs| {
             if let Some(stop_time) = self.stop_time.borrow(cs).take() {
-                Some(RtcInstant::now() - stop_time)
+                Some(self.instant() - stop_time)
             } else {
                 None
             }
diff --git a/tests/stm32/src/bin/stop.rs b/tests/stm32/src/bin/stop.rs
index 0b3f4a300..a490d7b8c 100644
--- a/tests/stm32/src/bin/stop.rs
+++ b/tests/stm32/src/bin/stop.rs
@@ -46,8 +46,10 @@ async fn async_main(_spawner: Spawner) {
 
     stop_with_rtc(rtc);
 
-    info!("Waiting 5 seconds");
-    Timer::after(Duration::from_secs(5)).await;
+    info!("Waiting...");
+    Timer::after(Duration::from_secs(2)).await;
+    info!("Waiting...");
+    Timer::after(Duration::from_secs(3)).await;
 
     info!("Test OK");
     cortex_m::asm::bkpt();