From b97b6d409c1b042b5d5f1b17dd2c8dfec50acdfc Mon Sep 17 00:00:00 2001 From: xoviat Date: Fri, 14 Nov 2025 14:53:09 -0600 Subject: low_power: cleanup add_time --- embassy-stm32/src/time_driver.rs | 33 +++++++-------------------------- 1 file changed, 7 insertions(+), 26 deletions(-) diff --git a/embassy-stm32/src/time_driver.rs b/embassy-stm32/src/time_driver.rs index 7db51d72e..bc34892ee 100644 --- a/embassy-stm32/src/time_driver.rs +++ b/embassy-stm32/src/time_driver.rs @@ -196,6 +196,11 @@ fn calc_now(period: u32, counter: u16) -> u64 { ((period as u64) << 15) + ((counter as u32 ^ ((period & 1) << 15)) as u64) } +#[cfg(feature = "low-power")] +fn calc_period_counter(ticks: u64) -> (u32, u16) { + (2 * (ticks >> 16) as u32 + (ticks as u16 >= 0x8000) as u32, ticks as u16) +} + struct AlarmState { timestamp: Cell, } @@ -358,34 +363,10 @@ impl RtcDriver { #[cfg(feature = "low-power")] /// Add the given offset to the current time fn add_time(&self, offset: embassy_time::Duration, cs: CriticalSection) { - let offset = offset.as_ticks(); - let cnt = 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 }; + let (period, counter) = calc_period_counter(self.now() + offset.as_ticks()); self.period.store(period, Ordering::SeqCst); - regs_gp16().cnt().write(|w| w.set_cnt(cnt as u16)); + regs_gp16().cnt().write(|w| w.set_cnt(counter)); // Now, recompute alarm let alarm = self.alarm.borrow(cs); -- cgit