stm32/rtc: use psc to compute instants

2 files changed, 19 insertions, 38 deletions

diff --git a/embassy-stm32/src/rtc/mod.rs b/embassy-stm32/src/rtc/mod.rs
index c408b2d61..8bda0926e 100644
--- a/embassy-stm32/src/rtc/mod.rs
+++ b/embassy-stm32/src/rtc/mod.rs
@@ -48,31 +48,6 @@ struct RtcInstant {
 }
 
 #[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;
 
@@ -85,20 +60,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) as u64)
     }
 }
 
@@ -198,6 +166,19 @@ impl Rtc {
         Ok(())
     }
 
+    /// 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();
+
+        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..62b398689 100644
--- a/embassy-stm32/src/rtc/v2.rs
+++ b/embassy-stm32/src/rtc/v2.rs
@@ -110,7 +110,7 @@ impl super::Rtc {
 
         trace!("rtc: start wakeup alarm for {} ms", duration.as_millis());
 
-        critical_section::with(|cs| assert!(self.stop_time.borrow(cs).replace(Some(RtcInstant::now())).is_none()))
+        critical_section::with(|cs| assert!(self.stop_time.borrow(cs).replace(Some(self.instant())).is_none()))
     }
 
     #[cfg(feature = "low-power")]
@@ -132,7 +132,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
             }