fix(stm32wb55): sync cpu2 via hsem when entering sleep

Author: ckrenslehner

embassy-stm32/src/low_power.rs

@@ -58,11 +58,10 @@ use core::arch::asm;
 use core::marker::PhantomData;
 use core::sync::atomic::{compiler_fence, Ordering};
 
-use cortex_m::peripheral::SCB;
-use embassy_executor::*;
-
 use crate::interrupt;
 use crate::time_driver::{get_driver, RtcDriver};
+use cortex_m::peripheral::SCB;
+use embassy_executor::*;
 
 const THREAD_PENDER: usize = usize::MAX;
 
@@ -98,10 +97,22 @@ pub(crate) unsafe fn on_wakeup_irq() {
 }
 
 /// Configure STOP mode with RTC.
+#[cfg(all(feature = "low-power", not(any(stm32wb))))]
 pub fn stop_with_rtc(rtc: &'static Rtc) {
     unsafe { EXECUTOR.as_mut().unwrap() }.stop_with_rtc(rtc)
 }
 
+/// Configure STOP mode with RTC and HSEM. The hardware semaphore is needed for configuring the coprocessor
+#[cfg(all(feature = "low-power", any(stm32wb)))]
+pub fn stop_with_rtc_and_hsem(
+    rtc: &'static Rtc,
+    hsem: &'static mut crate::hsem::HardwareSemaphore<'static, crate::peripherals::HSEM>,
+) {
+    let exec = unsafe { EXECUTOR.as_mut().unwrap() };
+    exec.set_hsem(hsem);
+    exec.stop_with_rtc(rtc)
+}
+
 /// Get whether the core is ready to enter the given stop mode.
 ///
 /// This will return false if some peripheral driver is in use that
@@ -124,10 +135,10 @@ pub enum StopMode {
     Stop2,
 }
 
-#[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0))]
+#[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0, stm32wb))]
 use stm32_metapac::pwr::vals::Lpms;
 
-#[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0))]
+#[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0, stm32wb))]
 impl Into<Lpms> for StopMode {
     fn into(self) -> Lpms {
         match self {
@@ -152,6 +163,12 @@ pub struct Executor {
     not_send: PhantomData<*mut ()>,
     scb: SCB,
     time_driver: &'static RtcDriver,
+
+    #[cfg(all(feature = "low-power", any(stm32wb)))]
+    /// Special treatment of coprocessor requires a hardware semaphore
+    hsem: embassy_sync::blocking_mutex::NoopMutex<
+        core::cell::Cell<Option<&'static mut crate::hsem::HardwareSemaphore<'static, crate::peripherals::HSEM>>>,
+    >,
 }
 
 impl Executor {
@@ -165,6 +182,8 @@ impl Executor {
                 not_send: PhantomData,
                 scb: cortex_m::Peripherals::steal().SCB,
                 time_driver: get_driver(),
+                #[cfg(all(feature = "low-power", any(stm32wb)))]
+                hsem: embassy_sync::blocking_mutex::NoopMutex::new(core::cell::Cell::default()),
             });
 
             let executor = EXECUTOR.as_mut().unwrap();
@@ -178,6 +197,15 @@ impl Executor {
         trace!("low power: resume");
     }
 
+    #[cfg(all(feature = "low-power", any(stm32wb)))]
+    pub(self) fn set_hsem(
+        &mut self,
+        hsem: &'static mut crate::hsem::HardwareSemaphore<'static, crate::peripherals::HSEM>,
+    ) {
+        assert!(self.hsem.lock(|x| x.replace(Some(hsem))).is_none());
+        trace!("low power: hsem configured");
+    }
+
     pub(self) fn stop_with_rtc(&mut self, rtc: &'static Rtc) {
         self.time_driver.set_rtc(rtc);
 
@@ -198,7 +226,17 @@ impl Executor {
 
     #[allow(unused_variables)]
     fn configure_stop(&mut self, stop_mode: StopMode) {
-        #[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0))]
+        #[cfg(all(feature = "low-power", any(stm32wb)))]
+        {
+            // Special treatment for MCUs with a coprocessor
+            unsafe {
+                self.hsem.lock_mut(|x| {
+                    crate::rcc::low_power::stm32wb_configure_clocks_enter_stop_mode(x.get_mut().as_mut().unwrap());
+                });
+            }
+        }
+
+        #[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0, stm32wb))]
         crate::pac::PWR.cr1().modify(|m| m.set_lpms(stop_mode.into()));
         #[cfg(stm32h5)]
         crate::pac::PWR.pmcr().modify(|v| {
@@ -263,6 +301,15 @@ impl Executor {
                 executor.inner.poll();
                 self.configure_pwr();
                 asm!("wfe");
+
+                #[cfg(all(feature = "low-power", any(stm32wb)))]
+                {
+                    // Special treatment for MCUs with a coprocessor
+                    self.hsem.lock_mut(|x|
+
+                    // Inentionally not calling from a critical section, as the critical section is handled internally
+                    crate::rcc::low_power::stm32wb_configure_clocks_exit_stop_mode(x.get_mut().as_mut().unwrap()));
+                }
             };
         }
     }

embassy-stm32/src/rcc/l.rs

@@ -133,6 +133,9 @@ pub const WPAN_DEFAULT: Config = Config {
     mux: super::mux::ClockMux::default(),
 };
 
+#[cfg(all(feature = "low-power", any(stm32wb)))]
+pub(super) static mut CURRENT_RCC_CONFIG: core::mem::MaybeUninit<Config> = core::mem::MaybeUninit::uninit();
+
 fn msi_enable(range: MSIRange) {
     #[cfg(any(stm32l4, stm32l5, stm32wb, stm32wl, stm32u0))]
     RCC.cr().modify(|w| {
@@ -149,6 +152,13 @@ fn msi_enable(range: MSIRange) {
 }
 
 pub(crate) unsafe fn init(config: Config) {
+    #[cfg(all(feature = "low-power", any(stm32wb)))]
+    {
+        // Store the current RCC config in to restore it when exiting low power mode.
+        // This is needed because these MCUs require a clock source switch before entering low power mode.
+        CURRENT_RCC_CONFIG = core::mem::MaybeUninit::new(config);
+    }
+
     // Switch to MSI to prevent problems with PLL configuration.
     if !RCC.cr().read().msion() {
         // Turn on MSI and configure it to 4MHz.
@@ -670,4 +680,4 @@ mod pll {
 
         PllOutput { p, q, r }
     }
-}
+}

embassy-stm32/src/rcc/low_power.rs

@@ -0,0 +1,81 @@
+use crate::hsem::{self, HardwareSemaphore};
+
+/// It is required to switch to the HSI clock before entering stop mode
+/// See an5289-how-to-build-wireless-applications-with-stm32wb-mcus-stmicroelectronics p. 23-24
+#[cfg(all(feature = "low-power", stm32wb))]
+pub(crate) fn stm32wb_configure_clocks_enter_stop_mode<T: hsem::Instance>(hsem: &mut HardwareSemaphore<T>) {
+    use crate::pac::PWR;
+    use crate::pac::{rcc::vals::Sw, RCC};
+    use hsem::SemId;
+    use stm32_metapac::rcc::vals::Smps;
+
+    critical_section::with(|_cs| {
+        // Poll Sem3 until granted
+        // TODO: Add some kind of timeout here and panic?
+        trace!("Polling Sem3 until granted");
+        while let Err(_) = hsem.one_step_lock(SemId::Id3) {}
+        trace!("Sem3 granted");
+
+        // Get Sem4
+        match hsem.one_step_lock(SemId::Id4) {
+            Ok(_) => {
+                // Sem4 granted
+                if PWR.extscr().read().c2ds() {
+                    // C2DS set - unlock Sem4
+                    hsem.unlock(SemId::Id4, None);
+                }
+            }
+            Err(_) => {
+                // Sem4 not granted
+                // Set HSION
+                RCC.cr().modify(|w| {
+                    w.set_hsion(true);
+                });
+
+                // Wait for HSIRDY
+                while !RCC.cr().read().hsirdy() {}
+
+                // Set SW to HSI
+                RCC.cfgr().modify(|w| {
+                    w.set_sw(Sw::HSI);
+                });
+
+                // Wait for SWS to report HSI
+                while !RCC.cfgr().read().sws().eq(&Sw::HSI) {}
+
+                // Set SMPSSEL to HSI
+                RCC.smpscr().modify(|w| {
+                    w.set_smpssel(Smps::HSI);
+                });
+            }
+        }
+
+        // Unlock Sem3
+        hsem.unlock(SemId::Id3, None);
+    });
+}
+
+/// __Attention__: Must not be called from within a critical section
+#[cfg(all(feature = "low-power", stm32wb))]
+pub(crate) fn stm32wb_configure_clocks_exit_stop_mode<T: hsem::Instance>(hsem: &mut HardwareSemaphore<T>) {
+    use super::{init, CURRENT_RCC_CONFIG};
+    use crate::hsem::SemId;
+
+    // Release Sem4
+    hsem.unlock(SemId::Id4, None);
+
+    // Enter the critical section afterwards
+    critical_section::with(|_cs| {
+        // Poll Sem3 until granted
+        // TODO: Add some kind of timeout here and panic?
+        while let Err(_) = hsem.one_step_lock(SemId::Id3) {}
+
+        // Restore the RCC config from before entering stop mode
+        // Diveating from The flow chart of Figure 7 in AN5289
+        // We always reconfigure the clocks as we don't know if HSE is suitable for the application
+        let config = unsafe { CURRENT_RCC_CONFIG.assume_init() };
+        unsafe { init(config) };
+
+        hsem.unlock(SemId::Id3, None);
+    });
+}

embassy-stm32/src/rcc/mod.rs

@@ -30,6 +30,9 @@ pub use hsi48::*;
 #[cfg_attr(stm32wba, path = "wba.rs")]
 mod _version;
 
+#[cfg(any(feature = "low-power", any(stm32wb)))]
+pub mod low_power;
+
 pub use _version::*;
 use stm32_metapac::RCC;