2 files changed, 57 insertions, 12 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..ce3ee6a03 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -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(())
     }