stm32: impl. draft cfgr pwr

2 files changed, 60 insertions, 11 deletions

diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index 3fe28b9df..7d5093b61 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -1,15 +1,33 @@
 use core::arch::asm;
 use core::marker::PhantomData;
 
+use cortex_m::peripheral::SCB;
 use embassy_executor::*;
+use embassy_time::Duration;
+
+use crate::interrupt;
+use crate::interrupt::typelevel::Interrupt;
 
 const THREAD_PENDER: usize = usize::MAX;
+const THRESHOLD: Duration = Duration::from_millis(500);
 
 use crate::rtc::{Rtc, RtcInstant};
 
 static mut RTC: Option<&'static Rtc> = None;
 
+foreach_interrupt! {
+    (RTC, rtc, $block:ident, WKUP, $irq:ident) => {
+        #[interrupt]
+        unsafe fn $irq() {
+            Executor::on_wakeup_irq();
+        }
+    };
+}
+
 pub fn stop_with_rtc(rtc: &'static Rtc) {
+    crate::interrupt::typelevel::RTC_WKUP::unpend();
+    unsafe { crate::interrupt::typelevel::RTC_WKUP::enable() };
+
     unsafe { RTC = Some(rtc) };
 }
 
@@ -45,8 +63,47 @@ impl Executor {
         }
     }
 
-    fn configure_power(&self) {
-        todo!()
+    unsafe fn on_wakeup_irq() {
+        info!("on wakeup irq");
+
+        cortex_m::asm::bkpt();
+    }
+
+    fn low_power_ready(&self) -> bool {
+        true
+    }
+
+    fn time_until_next_alarm(&self) -> Duration {
+        Duration::from_secs(3)
+    }
+
+    fn get_scb(&self) -> SCB {
+        unsafe { cortex_m::Peripherals::steal() }.SCB
+    }
+
+    fn configure_pwr(&self) {
+        trace!("low power before wfe");
+
+        if !self.low_power_ready() {
+            return;
+        }
+
+        let time_until_next_alarm = self.time_until_next_alarm();
+        if time_until_next_alarm < THRESHOLD {
+            return;
+        }
+
+        trace!("low power stop required");
+
+        critical_section::with(|_| {
+            trace!("executor: set wakeup alarm...");
+
+            start_wakeup_alarm(time_until_next_alarm);
+
+            trace!("low power wait for rtc ready...");
+
+            self.get_scb().set_sleepdeep();
+        });
     }
 
     /// Run the executor.
@@ -73,7 +130,7 @@ impl Executor {
         loop {
             unsafe {
                 self.inner.poll();
-                self.configure_power();
+                self.configure_pwr();
                 asm!("wfe");
             };
         }
diff --git a/embassy-stm32/src/rtc/v2.rs b/embassy-stm32/src/rtc/v2.rs
index f6565a721..e03c7d2a0 100644
--- a/embassy-stm32/src/rtc/v2.rs
+++ b/embassy-stm32/src/rtc/v2.rs
@@ -165,7 +165,6 @@ impl super::Rtc {
     pub(crate) fn start_wakeup_alarm(&self, requested_duration: embassy_time::Duration) -> RtcInstant {
         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;
@@ -205,9 +204,6 @@ impl super::Rtc {
             trace!("wakeup timer enabled");
         }
 
-        crate::interrupt::typelevel::RTC_WKUP::unpend();
-        unsafe { crate::interrupt::typelevel::RTC_WKUP::enable() };
-
         RtcInstant::now()
     }
 
@@ -218,10 +214,6 @@ 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 {
-        use crate::interrupt::typelevel::Interrupt;
-
-        crate::interrupt::typelevel::RTC_WKUP::disable();
-
         trace!("disable wakeup timer...");
 
         RTC::regs().cr().modify(|w| {