stm32: impl. stop for stm32wb

5 files changed, 112 insertions, 22 deletions

diff --git a/embassy-stm32/CHANGELOG.md b/embassy-stm32/CHANGELOG.md
index 597cbb192..7d8e7c617 100644
--- a/embassy-stm32/CHANGELOG.md
+++ b/embassy-stm32/CHANGELOG.md
@@ -7,6 +7,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## Unreleased - ReleaseDate
 
+- feat: implement stop for stm32wb.
 - change: rework hsem and add HIL test for some chips.
 - change: stm32/eth: ethernet no longer has a hard dependency on station management, and station management can be used independently ([#4871](https://github.com/embassy-rs/embassy/pull/4871))
 - feat: allow embassy_executor::main for low power
diff --git a/embassy-stm32/src/hsem/mod.rs b/embassy-stm32/src/hsem/mod.rs
index a6b910a53..5f1ed9b09 100644
--- a/embassy-stm32/src/hsem/mod.rs
+++ b/embassy-stm32/src/hsem/mod.rs
@@ -5,6 +5,8 @@ use core::marker::PhantomData;
 use core::sync::atomic::{Ordering, compiler_fence};
 use core::task::Poll;
 
+#[cfg(all(stm32wb, feature = "low-power"))]
+use critical_section::CriticalSection;
 use embassy_hal_internal::PeripheralType;
 use embassy_sync::waitqueue::AtomicWaker;
 
@@ -78,6 +80,12 @@ impl CoreId {
     }
 }
 
+#[cfg(not(all(stm32wb, feature = "low-power")))]
+const PUB_CHANNELS: usize = 6;
+
+#[cfg(all(stm32wb, feature = "low-power"))]
+const PUB_CHANNELS: usize = 4;
+
 /// TX interrupt handler.
 pub struct HardwareSemaphoreInterruptHandler<T: Instance> {
     _phantom: PhantomData<T>,
@@ -127,7 +135,7 @@ pub struct HardwareSemaphoreChannel<'a, T: Instance> {
 }
 
 impl<'a, T: Instance> HardwareSemaphoreChannel<'a, T> {
-    pub(self) const fn new(number: u8) -> Self {
+    pub(crate) const fn new(number: u8) -> Self {
         core::assert!(number > 0 && number <= 6);
 
         Self {
@@ -151,19 +159,29 @@ impl<'a, T: Instance> HardwareSemaphoreChannel<'a, T> {
                 .ier(core_id.to_index())
                 .modify(|w| w.set_ise(self.index as usize, true));
 
-            if self.two_step_lock(process_id).is_ok() {
-                Poll::Ready(HardwareSemaphoreMutex {
-                    index: self.index,
-                    process_id: process_id,
-                    _lifetime: PhantomData,
-                })
-            } else {
-                Poll::Pending
+            match self.try_lock(process_id) {
+                Some(mutex) => Poll::Ready(mutex),
+                None => Poll::Pending,
             }
         })
         .await
     }
 
+    /// Try to lock the semaphor
+    /// The 2-step lock procedure consists in a write to lock the semaphore, followed by a read to
+    /// check if the lock has been successful, carried out from the HSEM_Rx register.
+    pub fn try_lock(&mut self, process_id: u8) -> Option<HardwareSemaphoreMutex<'a, T>> {
+        if self.two_step_lock(process_id).is_ok() {
+            Some(HardwareSemaphoreMutex {
+                index: self.index,
+                process_id: process_id,
+                _lifetime: PhantomData,
+            })
+        } else {
+            None
+        }
+    }
+
     /// Locks the semaphore.
     /// The 2-step lock procedure consists in a write to lock the semaphore, followed by a read to
     /// check if the lock has been successful, carried out from the HSEM_Rx register.
@@ -206,9 +224,9 @@ impl<'a, T: Instance> HardwareSemaphoreChannel<'a, T> {
         });
     }
 
-    /// Checks if the semaphore is locked.
-    pub fn is_semaphore_locked(&self) -> bool {
-        T::regs().r(self.index as usize).read().lock()
+    /// Return the channel number
+    pub const fn channel(&self) -> u8 {
+        self.index + 1
     }
 }
 
@@ -230,15 +248,22 @@ impl<T: Instance> HardwareSemaphore<T> {
 
     /// Get a single channel, and keep the global struct
     pub const fn channel_for<'a>(&'a mut self, number: u8) -> HardwareSemaphoreChannel<'a, T> {
+        #[cfg(all(stm32wb, feature = "low-power"))]
+        core::assert!(number != 3 && number != 4);
+
         HardwareSemaphoreChannel::new(number)
     }
 
     /// Split the global struct into channels
-    pub const fn split<'a>(self) -> [HardwareSemaphoreChannel<'a, T>; 6] {
+    ///
+    /// If using low-power mode, channels 3 and 4 will not be returned
+    pub const fn split<'a>(self) -> [HardwareSemaphoreChannel<'a, T>; PUB_CHANNELS] {
         [
             HardwareSemaphoreChannel::new(1),
             HardwareSemaphoreChannel::new(2),
+            #[cfg(not(all(stm32wb, feature = "low-power")))]
             HardwareSemaphoreChannel::new(3),
+            #[cfg(not(all(stm32wb, feature = "low-power")))]
             HardwareSemaphoreChannel::new(4),
             HardwareSemaphoreChannel::new(5),
             HardwareSemaphoreChannel::new(6),
@@ -267,6 +292,11 @@ impl<T: Instance> HardwareSemaphore<T> {
     }
 }
 
+#[cfg(all(stm32wb, feature = "low-power"))]
+pub(crate) fn init_hsem(_cs: CriticalSection) {
+    rcc::enable_and_reset::<crate::peripherals::HSEM>();
+}
+
 struct State {
     wakers: [AtomicWaker; 6],
 }
@@ -278,8 +308,8 @@ impl State {
         }
     }
 
-    const fn waker_for(&self, number: u8) -> &AtomicWaker {
-        &self.wakers[number as usize]
+    const fn waker_for(&self, index: u8) -> &AtomicWaker {
+        &self.wakers[index as usize]
     }
 }
 
diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs
index 7c3770643..ef6f1d6dc 100644
--- a/embassy-stm32/src/lib.rs
+++ b/embassy-stm32/src/lib.rs
@@ -650,6 +650,9 @@ fn init_hw(config: Config) -> Peripherals {
 
             #[cfg(feature = "low-power")]
             rtc::init_rtc(cs, config.rtc, config.min_stop_pause);
+
+            #[cfg(all(stm32wb, feature = "low-power"))]
+            hsem::init_hsem(cs);
         }
 
         p
diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index cf8f2b393..15478eed4 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -148,7 +148,7 @@ pub enum StopMode {
 }
 
 #[cfg(any(stm32l4, stm32l5, stm32u5, stm32wba, stm32wlex, stm32u0))]
-use stm32_metapac::pwr::vals::Lpms;
+use crate::pac::pwr::vals::Lpms;
 
 #[cfg(any(stm32l4, stm32l5, stm32u5, stm32wba, stm32wlex, stm32u0))]
 impl Into<Lpms> for StopMode {
@@ -242,8 +242,63 @@ impl Executor {
         }
     }
 
+    #[cfg(all(stm32wb, feature = "low-power"))]
+    fn configure_stop_stm32wb(&self, _stop_mode: StopMode) -> Result<(), ()> {
+        use core::task::Poll;
+
+        use embassy_futures::poll_once;
+
+        use crate::hsem::HardwareSemaphoreChannel;
+        use crate::pac::rcc::vals::{Smps, Sw};
+        use crate::pac::{PWR, RCC};
+
+        let sem3_mutex = match poll_once(HardwareSemaphoreChannel::<crate::peripherals::HSEM>::new(3).lock(0)) {
+            Poll::Pending => None,
+            Poll::Ready(mutex) => Some(mutex),
+        }
+        .ok_or(())?;
+
+        let sem4_mutex = HardwareSemaphoreChannel::<crate::peripherals::HSEM>::new(4).try_lock(0);
+        if let Some(sem4_mutex) = sem4_mutex {
+            if PWR.extscr().read().c2ds() {
+                drop(sem4_mutex);
+            } else {
+                return Ok(());
+            }
+        }
+
+        // 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);
+        });
+
+        drop(sem3_mutex);
+
+        Ok(())
+    }
+
     #[allow(unused_variables)]
-    fn configure_stop(&self, stop_mode: StopMode) {
+    fn configure_stop(&self, stop_mode: StopMode) -> Result<(), ()> {
+        #[cfg(all(stm32wb, feature = "low-power"))]
+        self.configure_stop_stm32wb(stop_mode)?;
+
         #[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0, stm32wba, stm32wlex))]
         crate::pac::PWR.cr1().modify(|m| m.set_lpms(stop_mode.into()));
         #[cfg(stm32h5)]
@@ -252,6 +307,8 @@ impl Executor {
             v.set_lpms(vals::Lpms::STOP);
             v.set_svos(vals::Svos::SCALE3);
         });
+
+        Ok(())
     }
 
     fn configure_pwr(&self) {
@@ -267,12 +324,11 @@ impl Executor {
         critical_section::with(|cs| {
             let stop_mode = Self::stop_mode(cs)?;
             let _ = get_driver().pause_time(cs).ok()?;
+            self.configure_stop(stop_mode).ok()?;
 
-            Some(stop_mode)
+            Some(())
         })
-        .map(|stop_mode| {
-            self.configure_stop(stop_mode);
-
+        .map(|_| {
             #[cfg(not(feature = "low-power-debug-with-sleep"))]
             Self::get_scb().set_sleepdeep();
         });
diff --git a/examples/stm32wb/Cargo.toml b/examples/stm32wb/Cargo.toml
index 783690c11..83119e3a0 100644
--- a/examples/stm32wb/Cargo.toml
+++ b/examples/stm32wb/Cargo.toml
@@ -7,7 +7,7 @@ publish = false
 
 [dependencies]
 # Change stm32wb55rg to your chip name in both dependencies, if necessary.
-embassy-stm32 = { version = "0.4.0", path = "../../embassy-stm32", features = [ "defmt", "stm32wb55rg", "time-driver-any", "memory-x", "exti"]  }
+embassy-stm32 = { version = "0.4.0", path = "../../embassy-stm32", features = [ "defmt", "stm32wb55rg", "time-driver-any", "memory-x", "exti", "low-power"]  }
 embassy-stm32-wpan = { version = "0.1.0", path = "../../embassy-stm32-wpan", features = ["defmt", "stm32wb55rg"] }
 embassy-sync = { version = "0.7.2", path = "../../embassy-sync", features = ["defmt"] }
 embassy-executor = { version = "0.9.0", path = "../../embassy-executor", features = ["arch-cortex-m", "executor-thread", "defmt"] }