4 files changed, 27 insertions, 11 deletions

diff --git a/embassy-stm32/src/hsem/mod.rs b/embassy-stm32/src/hsem/mod.rs
index 5f1ed9b09..e62de0454 100644
--- a/embassy-stm32/src/hsem/mod.rs
+++ b/embassy-stm32/src/hsem/mod.rs
@@ -293,8 +293,13 @@ 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>();
+pub(crate) fn init_hsem(cs: CriticalSection) {
+    rcc::enable_and_reset_with_cs::<crate::peripherals::HSEM>(cs);
+
+    unsafe {
+        crate::rcc::REFCOUNT_STOP1 = 0;
+        crate::rcc::REFCOUNT_STOP2 = 0;
+    }
 }
 
 struct State {
diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index 15478eed4..f35eb71e4 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -147,17 +147,17 @@ pub enum StopMode {
     Stop2,
 }
 
-#[cfg(any(stm32l4, stm32l5, stm32u5, stm32wba, stm32wlex, stm32u0))]
+#[cfg(any(stm32l4, stm32l5, stm32u5, stm32wba, stm32wb, stm32wlex, stm32u0))]
 use crate::pac::pwr::vals::Lpms;
 
-#[cfg(any(stm32l4, stm32l5, stm32u5, stm32wba, stm32wlex, stm32u0))]
+#[cfg(any(stm32l4, stm32l5, stm32u5, stm32wba, stm32wb, stm32wlex, stm32u0))]
 impl Into<Lpms> for StopMode {
     fn into(self) -> Lpms {
         match self {
             StopMode::Stop1 => Lpms::STOP1,
-            #[cfg(not(stm32wba))]
+            #[cfg(not(any(stm32wb, stm32wba)))]
             StopMode::Stop2 => Lpms::STOP2,
-            #[cfg(stm32wba)]
+            #[cfg(any(stm32wb, stm32wba))]
             StopMode::Stop2 => Lpms::STOP1, // TODO: WBA has no STOP2?
         }
     }
@@ -237,13 +237,15 @@ impl Executor {
             trace!("low power: stop 1");
             Some(StopMode::Stop1)
         } else {
-            trace!("low power: not ready to stop");
+            trace!("low power: not ready to stop (refcount_stop1: {})", unsafe {
+                REFCOUNT_STOP1
+            });
             None
         }
     }
 
     #[cfg(all(stm32wb, feature = "low-power"))]
-    fn configure_stop_stm32wb(&self, _stop_mode: StopMode) -> Result<(), ()> {
+    fn configure_stop_stm32wb(&self) -> Result<(), ()> {
         use core::task::Poll;
 
         use embassy_futures::poll_once;
@@ -252,14 +254,20 @@ impl Executor {
         use crate::pac::rcc::vals::{Smps, Sw};
         use crate::pac::{PWR, RCC};
 
+        trace!("low power: trying to get sem3");
+
         let sem3_mutex = match poll_once(HardwareSemaphoreChannel::<crate::peripherals::HSEM>::new(3).lock(0)) {
             Poll::Pending => None,
             Poll::Ready(mutex) => Some(mutex),
         }
         .ok_or(())?;
 
+        trace!("low power: got sem3");
+
         let sem4_mutex = HardwareSemaphoreChannel::<crate::peripherals::HSEM>::new(4).try_lock(0);
         if let Some(sem4_mutex) = sem4_mutex {
+            trace!("low power: got sem4");
+
             if PWR.extscr().read().c2ds() {
                 drop(sem4_mutex);
             } else {
@@ -297,9 +305,9 @@ impl Executor {
     #[allow(unused_variables)]
     fn configure_stop(&self, stop_mode: StopMode) -> Result<(), ()> {
         #[cfg(all(stm32wb, feature = "low-power"))]
-        self.configure_stop_stm32wb(stop_mode)?;
+        self.configure_stop_stm32wb()?;
 
-        #[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0, stm32wba, stm32wlex))]
+        #[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0, stm32wb, stm32wba, stm32wlex))]
         crate::pac::PWR.cr1().modify(|m| m.set_lpms(stop_mode.into()));
         #[cfg(stm32h5)]
         crate::pac::PWR.pmcr().modify(|v| {
diff --git a/tests/stm32/Cargo.toml b/tests/stm32/Cargo.toml
index f5684d4df..8fcb6b2b4 100644
--- a/tests/stm32/Cargo.toml
+++ b/tests/stm32/Cargo.toml
@@ -31,7 +31,7 @@ stm32l4r5zi = ["embassy-stm32/stm32l4r5zi", "chrono", "not-gpdma", "rng", "dual-
 stm32l552ze = ["embassy-stm32/stm32l552ze", "not-gpdma", "rng", "hash", "dual-bank"]
 stm32u585ai = ["embassy-stm32/stm32u585ai", "spi-v345", "chrono", "rng", "hash", "cordic"]
 stm32u5a5zj = ["embassy-stm32/stm32u5a5zj", "spi-v345", "chrono", "rng", "hash"] # FIXME: cordic test cause it crash
-stm32wb55rg = ["embassy-stm32/stm32wb55rg", "chrono", "not-gpdma", "ble", "mac" , "rng", "hsem"]
+stm32wb55rg = ["embassy-stm32/stm32wb55rg", "chrono", "not-gpdma", "ble", "mac" , "rng", "hsem", "stop"]
 stm32wba52cg = ["embassy-stm32/stm32wba52cg", "spi-v345", "chrono", "rng", "hash"]
 stm32wl55jc = ["embassy-stm32/stm32wl55jc-cm4", "not-gpdma", "rng", "chrono", "hsem"]
 stm32f091rc = ["embassy-stm32/stm32f091rc", "cm0", "not-gpdma", "chrono"]
diff --git a/tests/stm32/src/bin/hsem.rs b/tests/stm32/src/bin/hsem.rs
index 19648997c..fa69f22b2 100644
--- a/tests/stm32/src/bin/hsem.rs
+++ b/tests/stm32/src/bin/hsem.rs
@@ -30,6 +30,9 @@ async fn main(_spawner: Spawner) {
     //
     //    hsem.channel_for(SemaphoreNumber::Channel5).unlock(0);
 
+    #[cfg(feature = "stm32wb55rg")]
+    let [_channel1, _channel2, mut channel5, _channel6] = hsem.split();
+    #[cfg(not(feature = "stm32wb55rg"))]
     let [_channel1, _channel2, _channel3, _channel4, mut channel5, _channel6] = hsem.split();
 
     info!("Locking channel 5");