3 files changed, 61 insertions, 16 deletions

diff --git a/embassy-stm32/src/hsem/mod.rs b/embassy-stm32/src/hsem/mod.rs
index b5fa3c897..e5c68acbd 100644
--- a/embassy-stm32/src/hsem/mod.rs
+++ b/embassy-stm32/src/hsem/mod.rs
@@ -94,17 +94,14 @@ pub struct HardwareSemaphoreInterruptHandler<T: Instance> {
 impl<T: Instance> interrupt::typelevel::Handler<T::Interrupt> for HardwareSemaphoreInterruptHandler<T> {
     unsafe fn on_interrupt() {
         let core_id = CoreId::current();
+        let isr = T::regs().isr(core_id.to_index()).read();
 
         for number in 0..5 {
-            if T::regs().isr(core_id.to_index()).read().isf(number as usize) {
+            if isr.isf(number as usize) {
                 T::regs()
                     .icr(core_id.to_index())
                     .write(|w| w.set_isc(number as usize, true));
 
-                T::regs()
-                    .ier(core_id.to_index())
-                    .modify(|w| w.set_ise(number as usize, false));
-
                 T::state().waker_for(number).wake();
             }
         }
@@ -120,6 +117,18 @@ pub struct HardwareSemaphoreMutex<'a, T: Instance> {
 
 impl<'a, T: Instance> Drop for HardwareSemaphoreMutex<'a, T> {
     fn drop(&mut self) {
+        let core_id = CoreId::current();
+
+        T::regs()
+            .icr(core_id.to_index())
+            .write(|w| w.set_isc(self.index as usize, true));
+
+        critical_section::with(|_| {
+            T::regs()
+                .ier(core_id.to_index())
+                .modify(|w| w.set_ise(self.index as usize, false));
+        });
+
         HardwareSemaphoreChannel::<'a, T> {
             index: self.index,
             _lifetime: PhantomData,
@@ -156,9 +165,11 @@ impl<'a, T: Instance> HardwareSemaphoreChannel<'a, T> {
 
             compiler_fence(Ordering::SeqCst);
 
-            T::regs()
-                .ier(core_id.to_index())
-                .modify(|w| w.set_ise(self.index as usize, true));
+            critical_section::with(|_| {
+                T::regs()
+                    .ier(core_id.to_index())
+                    .modify(|w| w.set_ise(self.index as usize, true));
+            });
 
             match self.try_lock(process_id) {
                 Some(mutex) => Poll::Ready(mutex),
diff --git a/embassy-stm32/src/low_power.rs b/embassy-stm32/src/low_power.rs
index 2388abe3c..0cdd7789e 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -124,9 +124,9 @@ impl Into<Lpms> for StopMode {
     fn into(self) -> Lpms {
         match self {
             StopMode::Stop1 => Lpms::STOP1,
-            #[cfg(not(any(stm32wb, stm32wba)))]
+            #[cfg(not(stm32wba))]
             StopMode::Standby | StopMode::Stop2 => Lpms::STOP2,
-            #[cfg(any(stm32wb, stm32wba))]
+            #[cfg(stm32wba)]
             StopMode::Standby | StopMode::Stop2 => Lpms::STOP1, // TODO: WBA has no STOP2?
         }
     }
@@ -170,24 +170,44 @@ impl Executor {
         }
 
         critical_section::with(|cs| {
-            #[cfg(stm32wlex)]
+            #[cfg(any(stm32wlex, stm32wb))]
             {
                 let es = crate::pac::PWR.extscr().read();
+                #[cfg(stm32wlex)]
                 match (es.c1stopf(), es.c1stop2f()) {
                     (true, false) => debug!("low power: wake from STOP1"),
                     (false, true) => debug!("low power: wake from STOP2"),
                     (true, true) => debug!("low power: wake from STOP1 and STOP2 ???"),
                     (false, false) => trace!("low power: stop mode not entered"),
                 };
+
+                #[cfg(stm32wb)]
+                match (es.c1stopf(), es.c2stopf()) {
+                    (true, false) => debug!("low power: cpu1 wake from STOP"),
+                    (false, true) => debug!("low power: cpu2 wake from STOP"),
+                    (true, true) => debug!("low power: cpu1 and cpu2 wake from STOP"),
+                    (false, false) => trace!("low power: stop mode not entered"),
+                };
                 crate::pac::PWR.extscr().modify(|w| {
                     w.set_c1cssf(false);
                 });
 
-                if es.c1stop2f() || es.c1stopf() {
+                let has_stopped2 = {
+                    #[cfg(stm32wb)]
+                    {
+                        es.c2stopf()
+                    }
+
+                    #[cfg(stm32wlex)]
+                    {
+                        es.c1stop2f()
+                    }
+                };
+
+                if es.c1stopf() || has_stopped2 {
                     // when we wake from any stop mode we need to re-initialize the rcc
                     crate::rcc::init(RCC_CONFIG.unwrap());
-
-                    if es.c1stop2f() {
+                    if has_stopped2 {
                         // when we wake from STOP2, we need to re-initialize the time driver
                         get_driver().init_timer(cs);
                         // reset the refcounts for STOP2 and STOP1 (initializing the time driver will increment one of them for the timer)
@@ -276,6 +296,20 @@ impl Executor {
 
         drop(sem3_mutex);
 
+        // on PWR
+        RCC.apb1enr1().modify(|r| r.0 |= 1 << 28);
+        cortex_m::asm::dsb();
+
+        // off SMPS, on Bypass
+        PWR.cr5().modify(|r| {
+            let mut val = r.0;
+            val &= !(1 << 15); // sdeb = 0 (off SMPS)
+            val |= 1 << 14; // sdben = 1 (on Bypass)
+            r.0 = val
+        });
+
+        cortex_m::asm::delay(1000);
+
         Ok(())
     }
 
diff --git a/embassy-stm32/src/rcc/mod.rs b/embassy-stm32/src/rcc/mod.rs
index d25c922d8..753ee1290 100644
--- a/embassy-stm32/src/rcc/mod.rs
+++ b/embassy-stm32/src/rcc/mod.rs
@@ -313,7 +313,7 @@ impl RccInfo {
 
     #[allow(dead_code)]
     fn increment_minimum_stop_refcount_with_cs(&self, _cs: CriticalSection) {
-        #[cfg(all(stm32wlex, feature = "low-power"))]
+        #[cfg(all(any(stm32wlex, stm32wb), feature = "low-power"))]
         match self.stop_mode {
             StopMode::Stop1 | StopMode::Stop2 => increment_stop_refcount(_cs, StopMode::Stop2),
             _ => {}
@@ -334,7 +334,7 @@ impl RccInfo {
 
     #[allow(dead_code)]
     fn decrement_minimum_stop_refcount_with_cs(&self, _cs: CriticalSection) {
-        #[cfg(all(stm32wlex, feature = "low-power"))]
+        #[cfg(all(any(stm32wlex, stm32wb), feature = "low-power"))]
         match self.stop_mode {
             StopMode::Stop1 | StopMode::Stop2 => decrement_stop_refcount(_cs, StopMode::Stop2),
             _ => {}