From cda404731093015f84bad96675fdbfc712bc0215 Mon Sep 17 00:00:00 2001
From: xoviat <xoviat@users.noreply.github.com>
Date: Thu, 24 Aug 2023 19:29:11 -0500
Subject: [PATCH] stm32: flesh out lp executor
---
embassy-stm32/src/lib.rs | 5 +++
embassy-stm32/src/low_power.rs | 45 +++++++++++++------
embassy-stm32/src/rcc/mod.rs | 2 +
embassy-stm32/src/rtc/v2.rs | 25 ++++++++---
embassy-stm32/src/time_driver.rs | 76 ++++++++++++++++++++++++++++++++
5 files changed, 133 insertions(+), 20 deletions(-)
diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs
index 8c87ea7d5..ec8648ee4 100644
--- a/embassy-stm32/src/lib.rs
+++ b/embassy-stm32/src/lib.rs
@@ -197,6 +197,11 @@ pub fn init(config: Config) -> Peripherals {
// must be after rcc init
#[cfg(feature = "_time-driver")]
time_driver::init();
+
+ #[cfg(feature = "low-power")]
+ while !crate::rcc::low_power_ready() {
+ crate::rcc::clock_refcount_sub();
+ }
}
p
diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index 7814fa384..0d9506aaa 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -9,6 +9,7 @@ use crate::interrupt;
use crate::interrupt::typelevel::Interrupt;
use crate::pac::EXTI;
use crate::rcc::low_power_ready;
+use crate::time_driver::{pause_time, resume_time, time_until_next_alarm};
const THREAD_PENDER: usize = usize::MAX;
const THRESHOLD: Duration = Duration::from_millis(500);
@@ -16,6 +17,9 @@ const THRESHOLD: Duration = Duration::from_millis(500);
use crate::rtc::{Rtc, RtcInstant};
static mut RTC: Option<&'static Rtc> = None;
+static mut STOP_TIME: embassy_time::Duration = Duration::from_ticks(0);
+static mut NEXT_ALARM: embassy_time::Duration = Duration::from_ticks(u64::MAX);
+static mut RTC_INSTANT: Option<crate::rtc::RtcInstant> = None;
foreach_interrupt! {
(RTC, rtc, $block:ident, WKUP, $irq:ident) => {
@@ -69,13 +73,25 @@ impl Executor {
}
unsafe fn on_wakeup_irq() {
- info!("on wakeup irq");
+ trace!("on wakeup irq");
- cortex_m::asm::bkpt();
- }
+ let elapsed = RTC_INSTANT.take().unwrap() - stop_wakeup_alarm();
- fn time_until_next_alarm(&self) -> Duration {
- Duration::from_secs(3)
+ 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();
+ }
}
fn get_scb() -> SCB {
@@ -86,25 +102,28 @@ impl Executor {
trace!("configure_pwr");
if !low_power_ready() {
+ trace!("configure_pwr: low power not ready");
return;
}
- let time_until_next_alarm = self.time_until_next_alarm();
+ let time_until_next_alarm = time_until_next_alarm();
if time_until_next_alarm < THRESHOLD {
+ trace!("configure_pwr: not enough time until next alarm");
return;
}
- trace!("low power stop required");
+ unsafe {
+ NEXT_ALARM = time_until_next_alarm;
+ RTC_INSTANT = Some(start_wakeup_alarm(time_until_next_alarm))
+ };
- critical_section::with(|_| {
- trace!("executor: set wakeup alarm...");
+ // return;
- start_wakeup_alarm(time_until_next_alarm);
+ pause_time();
- trace!("low power wait for rtc ready...");
+ trace!("enter stop...");
- Self::get_scb().set_sleepdeep();
- });
+ Self::get_scb().set_sleepdeep();
}
/// Run the executor.
diff --git a/embassy-stm32/src/rcc/mod.rs b/embassy-stm32/src/rcc/mod.rs
index 3c75923e5..45a4d880d 100644
--- a/embassy-stm32/src/rcc/mod.rs
+++ b/embassy-stm32/src/rcc/mod.rs
@@ -86,6 +86,8 @@ static CLOCK_REFCOUNT: AtomicU32 = AtomicU32::new(0);
#[cfg(feature = "low-power")]
pub fn low_power_ready() -> bool {
+ trace!("clock refcount: {}", CLOCK_REFCOUNT.load(Ordering::SeqCst));
+
CLOCK_REFCOUNT.load(Ordering::SeqCst) == 0
}
diff --git a/embassy-stm32/src/rtc/v2.rs b/embassy-stm32/src/rtc/v2.rs
index bcb127ecb..197c3b8f9 100644
--- a/embassy-stm32/src/rtc/v2.rs
+++ b/embassy-stm32/src/rtc/v2.rs
@@ -30,9 +30,6 @@ impl RtcInstant {
let _ = RTC::regs().dr().read();
- trace!("ssr: {}", ssr);
- trace!("st: {}", st);
-
Self { ssr, st }
}
}
@@ -52,7 +49,12 @@ impl core::ops::Sub for RtcInstant {
let other_ticks = rhs.st as u32 * 256 + (255 - rhs.ssr as u32);
let rtc_ticks = self_ticks - other_ticks;
- trace!("self, other, rtc ticks: {}, {}, {}", self_ticks, other_ticks, rtc_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)))
@@ -174,10 +176,10 @@ impl super::Rtc {
rtc_ticks as u64 * TICK_HZ * (<WakeupPrescaler as Into<u32>>::into(prescaler) as u64) / rtc_hz,
);
- trace!("set wakeup timer for {} ms", duration.as_millis());
+ trace!("rtc: set wakeup timer for {} ms", duration.as_millis());
self.write(false, |regs| {
- regs.cr().modify(|w| w.set_wutie(true));
+ // regs.cr().modify(|w| w.set_wutie(true));
regs.cr().modify(|w| w.set_wute(false));
regs.isr().modify(|w| w.set_wutf(false));
@@ -187,6 +189,15 @@ impl super::Rtc {
regs.cr().modify(|w| w.set_wute(true));
});
+ self.write(false, |regs| {
+ regs.cr().modify(|w| w.set_wutie(false));
+
+ regs.isr().modify(|w| w.set_wutf(false));
+ crate::pac::PWR.cr1().modify(|w| w.set_cwuf(false));
+
+ regs.cr().modify(|w| w.set_wutie(true));
+ });
+
RtcInstant::now()
}
@@ -197,7 +208,7 @@ impl super::Rtc {
/// 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 {
- trace!("disable wakeup timer...");
+ trace!("rtc: stop wakeup alarm...");
self.write(false, |regs| {
regs.cr().modify(|w| w.set_wute(false));
diff --git a/embassy-stm32/src/time_driver.rs b/embassy-stm32/src/time_driver.rs
index 2622442f4..8e05346ad 100644
--- a/embassy-stm32/src/time_driver.rs
+++ b/embassy-stm32/src/time_driver.rs
@@ -259,6 +259,64 @@ impl RtcDriver {
let f: fn(*mut ()) = unsafe { mem::transmute(alarm.callback.get()) };
f(alarm.ctx.get());
}
+
+ #[cfg(feature = "low-power")]
+ /// Compute the approximate amount of time until the next alarm
+ pub(crate) fn time_until_next_alarm(&self) -> embassy_time::Duration {
+ critical_section::with(|cs| {
+ let now = self.now() + 32;
+
+ embassy_time::Duration::from_ticks(
+ self.alarms
+ .borrow(cs)
+ .iter()
+ .map(|alarm: &AlarmState| alarm.timestamp.get().saturating_sub(now))
+ .min()
+ .unwrap_or(u64::MAX),
+ )
+ })
+ }
+
+ #[cfg(feature = "low-power")]
+ /// Pause the timer
+ pub(crate) fn pause_time(&self) {
+ T::regs_gp16().cr1().modify(|w| w.set_cen(false));
+ }
+
+ #[cfg(feature = "low-power")]
+ /// Resume the timer with the given offset
+ pub(crate) fn resume_time(&self, offset: embassy_time::Duration) {
+ let offset = offset.as_ticks();
+ let cnt = T::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 };
+
+ self.period.store(period, Ordering::SeqCst);
+ T::regs_gp16().cnt().write(|w| w.set_cnt(cnt as u16));
+
+ T::regs_gp16().cr1().modify(|w| w.set_cen(true));
+ }
}
impl Driver for RtcDriver {
@@ -329,6 +387,24 @@ impl Driver for RtcDriver {
}
}
+#[cfg(feature = "low-power")]
+/// Compute the approximate amount of time until the next alarm
+pub(crate) fn time_until_next_alarm() -> embassy_time::Duration {
+ DRIVER.time_until_next_alarm()
+}
+
+#[cfg(feature = "low-power")]
+/// Pause the timer
+pub(crate) fn pause_time() {
+ DRIVER.pause_time();
+}
+
+#[cfg(feature = "low-power")]
+/// Resume the timer with the given offset
+pub(crate) fn resume_time(offset: embassy_time::Duration) {
+ DRIVER.resume_time(offset);
+}
+
pub(crate) fn init() {
DRIVER.init()
}