Merge branch 'main' into usart

17 files changed, 677 insertions, 139 deletions

diff --git a/docs/pages/bootloader.adoc b/docs/pages/bootloader.adoc
index b0f0331aa..c010b0622 100644
--- a/docs/pages/bootloader.adoc
+++ b/docs/pages/bootloader.adoc
@@ -43,14 +43,14 @@ Partition Size~dfu~= Partition Size~active~+ Page Size~active~
 +
 All values are specified in bytes.
 
-* BOOTLOADER STATE - Where the bootloader stores the current state describing if the active and dfu partitions need to be swapped. When the new firmware has been written to the DFU partition, a magic field is written to instruct the bootloader that the partitions should be swapped. This partition must be able to store a magic field as well as the partition swap progress. The partition size given by:
+* BOOTLOADER STATE - Where the bootloader stores the current state describing if the active and dfu partitions need to be swapped. When the new firmware has been written to the DFU partition, a magic field is written to instruct the bootloader that the partitions should be swapped. This partition must be able to store a magic field as well as the partition swap progress. The partition size is given by:
 +
-Partition Size~state~ = Write Size~state~ + (2 × Partition Size~active~ / Page Size~active~)
+Partition Size~state~ = (2 × Write Size~state~) + (4 × Write Size~state~ × Partition Size~active~ / Page Size~active~)
 +
 All values are specified in bytes.
 
 The partitions for ACTIVE (+BOOTLOADER), DFU and BOOTLOADER_STATE may be placed in separate flash. The page size used by the bootloader is determined by the lowest common multiple of the ACTIVE and DFU page sizes.
-The BOOTLOADER_STATE partition must be big enough to store one word per page in the ACTIVE and DFU partitions combined.
+The BOOTLOADER_STATE partition must be big enough to store two words, plus four words per page in the ACTIVE partition.
 
 The bootloader has a platform-agnostic part, which implements the power fail safe swapping algorithm given the boundaries set by the partitions. The platform-specific part is a minimal shim that provides additional functionality such as watchdogs or supporting the nRF52 softdevice.
 
diff --git a/embassy-boot/CHANGELOG.md b/embassy-boot/CHANGELOG.md
index 8d6395357..1d41043cb 100644
--- a/embassy-boot/CHANGELOG.md
+++ b/embassy-boot/CHANGELOG.md
@@ -8,6 +8,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 <!-- next-header -->
 ## Unreleased - ReleaseDate
 
+- Fixed documentation and assertion of STATE partition size requirements
+- Added documentation for package features
+
 ## 0.6.1 - 2025-08-26
 
 - First release with changelog.
diff --git a/embassy-boot/Cargo.toml b/embassy-boot/Cargo.toml
index 8c5c1f633..754c6e5f1 100644
--- a/embassy-boot/Cargo.toml
+++ b/embassy-boot/Cargo.toml
@@ -26,6 +26,7 @@ features = ["defmt"]
 [dependencies]
 defmt = { version = "1.0.1", optional = true }
 digest = "0.10"
+document-features = "0.2.7"
 log = { version = "0.4", optional = true }
 ed25519-dalek = { version = "2", default-features = false, features = ["digest"], optional = true }
 embassy-embedded-hal = { version = "0.5.0", path = "../embassy-embedded-hal" }
@@ -45,11 +46,22 @@ critical-section = { version = "1.1.1", features = ["std"] }
 ed25519-dalek = { version = "2", default-features = false, features = ["std", "rand_core", "digest"]  }
 
 [features]
+## Use [`defmt`](https://docs.rs/defmt/latest/defmt/) for logging
 defmt = ["dep:defmt"]
+## Use log for logging
 log = ["dep:log"]
+
+## Enable for devices that set erased flash bytes to `0x00` instead of the usual `0xFF`
+flash-erase-zero = []
+
+#! ## Firmware Signing
+#! Enable one of these features to allow verification of DFU signatures with
+#! `FirmwareUpdater::verify_and_mark_updated`.
+
+## Use the `ed25519-dalek` package to verify DFU signatures.
 ed25519-dalek = ["dep:ed25519-dalek", "_verify"]
+## Use the `salty` package to verify DFU signatures.
 ed25519-salty = ["dep:salty", "_verify"]
-flash-erase-zero = []
 
 #Internal features
 _verify = []
diff --git a/embassy-boot/src/boot_loader.rs b/embassy-boot/src/boot_loader.rs
index c38940d6e..a3a307051 100644
--- a/embassy-boot/src/boot_loader.rs
+++ b/embassy-boot/src/boot_loader.rs
@@ -135,10 +135,12 @@ pub struct BootLoader<ACTIVE: NorFlash, DFU: NorFlash, STATE: NorFlash> {
     dfu: DFU,
     /// The state partition has the following format:
     /// All ranges are in multiples of WRITE_SIZE bytes.
-    /// | Range    | Description                                                                      |
-    /// | 0..1     | Magic indicating bootloader state. BOOT_MAGIC means boot, SWAP_MAGIC means swap. |
-    /// | 1..2     | Progress validity. ERASE_VALUE means valid, !ERASE_VALUE means invalid.          |
-    /// | 2..2 + N | Progress index used while swapping or reverting      
+    /// N = Active partition size divided by WRITE_SIZE.
+    /// | Range              | Description                                                                      |
+    /// | 0..1               | Magic indicating bootloader state. BOOT_MAGIC means boot, SWAP_MAGIC means swap. |
+    /// | 1..2               | Progress validity. ERASE_VALUE means valid, !ERASE_VALUE means invalid.          |
+    /// | 2..(2 + 2N)        | Progress index used while swapping                                               |
+    /// | (2 + 2N)..(2 + 4N) | Progress index used while reverting
     state: STATE,
 }
 
@@ -429,7 +431,7 @@ fn assert_partitions<ACTIVE: NorFlash, DFU: NorFlash, STATE: NorFlash>(
     assert_eq!(dfu.capacity() as u32 % page_size, 0);
     // DFU partition has to be bigger than ACTIVE partition to handle swap algorithm
     assert!(dfu.capacity() as u32 - active.capacity() as u32 >= page_size);
-    assert!(2 + 2 * (active.capacity() as u32 / page_size) <= state.capacity() as u32 / STATE::WRITE_SIZE as u32);
+    assert!(2 + 4 * (active.capacity() as u32 / page_size) <= state.capacity() as u32 / STATE::WRITE_SIZE as u32);
 }
 
 #[cfg(test)]
diff --git a/embassy-boot/src/lib.rs b/embassy-boot/src/lib.rs
index 7dc811f66..3e61d6036 100644
--- a/embassy-boot/src/lib.rs
+++ b/embassy-boot/src/lib.rs
@@ -3,6 +3,10 @@
 #![allow(unsafe_op_in_unsafe_fn)]
 #![warn(missing_docs)]
 #![doc = include_str!("../README.md")]
+
+//! ## Feature flags
+#![doc = document_features::document_features!(feature_label = r#"<span class="stab portability"><code>{feature}</code></span>"#)]
+
 mod fmt;
 
 mod boot_loader;
diff --git a/embassy-stm32/CHANGELOG.md b/embassy-stm32/CHANGELOG.md
index 5fd43744e..416590f88 100644
--- a/embassy-stm32/CHANGELOG.md
+++ b/embassy-stm32/CHANGELOG.md
@@ -8,6 +8,9 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ## Unreleased - ReleaseDate
 
 - fix: fix incorrect logic for buffered usart transmission complete.
+- feat: add poll_for methods to exti
+- 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
 - feat: Add waveform methods to ComplementaryPwm
@@ -68,6 +71,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - feat: add flash support for c0 family ([#4874](https://github.com/embassy-rs/embassy/pull/4874))
 - fix: fixing channel numbers on vbat and vddcore for adc on adc
 - adc: adding disable to vbat
+- feat: stm32/flash: add async support for h7 family
 
 ## 0.4.0 - 2025-08-26
 
diff --git a/embassy-stm32/src/adc/ringbuffered.rs b/embassy-stm32/src/adc/ringbuffered.rs
index a56f8ca0b..5437866d3 100644
--- a/embassy-stm32/src/adc/ringbuffered.rs
+++ b/embassy-stm32/src/adc/ringbuffered.rs
@@ -49,8 +49,6 @@ impl<'d, T: Instance + AnyInstance> RingBufferedAdc<'d, T> {
     }
 
     pub fn stop(&mut self) {
-        T::stop();
-
         self.ring_buf.request_pause();
 
         compiler_fence(Ordering::SeqCst);
@@ -161,7 +159,7 @@ impl<'d, T: Instance + AnyInstance> RingBufferedAdc<'d, T> {
                     return Ok(len);
                 }
                 Err(_) => {
-                    self.stop();
+                    self.ring_buf.request_pause();
 
                     return Err(OverrunError);
                 }
diff --git a/embassy-stm32/src/exti.rs b/embassy-stm32/src/exti.rs
index cb46d362c..899d5e677 100644
--- a/embassy-stm32/src/exti.rs
+++ b/embassy-stm32/src/exti.rs
@@ -7,6 +7,7 @@ use core::task::{Context, Poll};
 
 use embassy_hal_internal::{PeripheralType, impl_peripheral};
 use embassy_sync::waitqueue::AtomicWaker;
+use futures_util::FutureExt;
 
 use crate::gpio::{AnyPin, Input, Level, Pin as GpioPin, PinNumber, Pull};
 use crate::pac::EXTI;
@@ -133,7 +134,7 @@ impl<'d> ExtiInput<'d> {
     ///
     /// This returns immediately if the pin is already high.
     pub async fn wait_for_high(&mut self) {
-        let fut = ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), true, false);
+        let fut = ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), true, false, true);
         if self.is_high() {
             return;
         }
@@ -144,7 +145,7 @@ impl<'d> ExtiInput<'d> {
     ///
     /// This returns immediately if the pin is already low.
     pub async fn wait_for_low(&mut self) {
-        let fut = ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), false, true);
+        let fut = ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), false, true, true);
         if self.is_low() {
             return;
         }
@@ -155,19 +156,40 @@ impl<'d> ExtiInput<'d> {
     ///
     /// If the pin is already high, it will wait for it to go low then back high.
     pub async fn wait_for_rising_edge(&mut self) {
-        ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), true, false).await
+        ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), true, false, true).await
+    }
+
+    /// Asynchronously wait until the pin sees a rising edge.
+    ///
+    /// If the pin is already high, it will wait for it to go low then back high.
+    pub fn poll_for_rising_edge<'a>(&mut self, cx: &mut Context<'a>) {
+        let _ =
+            ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), true, false, false).poll_unpin(cx);
     }
 
     /// Asynchronously wait until the pin sees a falling edge.
     ///
     /// If the pin is already low, it will wait for it to go high then back low.
     pub async fn wait_for_falling_edge(&mut self) {
-        ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), false, true).await
+        ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), false, true, true).await
+    }
+
+    /// Asynchronously wait until the pin sees a falling edge.
+    ///
+    /// If the pin is already low, it will wait for it to go high then back low.
+    pub fn poll_for_falling_edge<'a>(&mut self, cx: &mut Context<'a>) {
+        let _ =
+            ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), false, true, false).poll_unpin(cx);
     }
 
     /// Asynchronously wait until the pin sees any edge (either rising or falling).
     pub async fn wait_for_any_edge(&mut self) {
-        ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), true, true).await
+        ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), true, true, true).await
+    }
+
+    /// Asynchronously wait until the pin sees any edge (either rising or falling).
+    pub fn poll_for_any_edge<'a>(&mut self, cx: &mut Context<'a>) {
+        let _ = ExtiInputFuture::new(self.pin.pin.pin.pin(), self.pin.pin.pin.port(), true, true, false).poll_unpin(cx);
     }
 }
 
@@ -227,11 +249,12 @@ impl<'d> embedded_hal_async::digital::Wait for ExtiInput<'d> {
 #[must_use = "futures do nothing unless you `.await` or poll them"]
 struct ExtiInputFuture<'a> {
     pin: PinNumber,
+    drop: bool,
     phantom: PhantomData<&'a mut AnyPin>,
 }
 
 impl<'a> ExtiInputFuture<'a> {
-    fn new(pin: PinNumber, port: PinNumber, rising: bool, falling: bool) -> Self {
+    fn new(pin: PinNumber, port: PinNumber, rising: bool, falling: bool, drop: bool) -> Self {
         critical_section::with(|_| {
             let pin = pin as usize;
             exticr_regs().exticr(pin / 4).modify(|w| w.set_exti(pin % 4, port));
@@ -252,6 +275,7 @@ impl<'a> ExtiInputFuture<'a> {
 
         Self {
             pin,
+            drop,
             phantom: PhantomData,
         }
     }
@@ -259,10 +283,12 @@ impl<'a> ExtiInputFuture<'a> {
 
 impl<'a> Drop for ExtiInputFuture<'a> {
     fn drop(&mut self) {
-        critical_section::with(|_| {
-            let pin = self.pin as _;
-            cpu_regs().imr(0).modify(|w| w.set_line(pin, false));
-        });
+        if self.drop {
+            critical_section::with(|_| {
+                let pin = self.pin as _;
+                cpu_regs().imr(0).modify(|w| w.set_line(pin, false));
+            });
+        }
     }
 }
 
diff --git a/embassy-stm32/src/flash/h7.rs b/embassy-stm32/src/flash/h7.rs
index 8a43cce3f..b342f4a83 100644
--- a/embassy-stm32/src/flash/h7.rs
+++ b/embassy-stm32/src/flash/h7.rs
@@ -1,10 +1,31 @@
 use core::ptr::write_volatile;
 use core::sync::atomic::{Ordering, fence};
 
+use embassy_sync::waitqueue::AtomicWaker;
+use pac::flash::regs::Sr;
+
 use super::{BANK1_REGION, FLASH_REGIONS, FlashSector, WRITE_SIZE};
 use crate::flash::Error;
 use crate::pac;
 
+static WAKER: AtomicWaker = AtomicWaker::new();
+
+pub(crate) unsafe fn on_interrupt() {
+    // Clear IRQ flags
+    pac::FLASH.bank(0).ccr().write(|w| {
+        w.set_clr_eop(true);
+        w.set_clr_operr(true);
+    });
+    if is_dual_bank() {
+        pac::FLASH.bank(1).ccr().write(|w| {
+            w.set_clr_eop(true);
+            w.set_clr_operr(true);
+        });
+    }
+
+    WAKER.wake();
+}
+
 const fn is_dual_bank() -> bool {
     FLASH_REGIONS.len() >= 2
 }
@@ -29,12 +50,68 @@ pub(crate) unsafe fn unlock() {
     }
 }
 
+pub(crate) unsafe fn enable_write() {
+    enable_blocking_write();
+}
+
+pub(crate) unsafe fn disable_write() {
+    disable_blocking_write();
+}
+
 pub(crate) unsafe fn enable_blocking_write() {
     assert_eq!(0, WRITE_SIZE % 4);
 }
 
 pub(crate) unsafe fn disable_blocking_write() {}
 
+pub(crate) async unsafe fn write(start_address: u32, buf: &[u8; WRITE_SIZE]) -> Result<(), Error> {
+    // We cannot have the write setup sequence in begin_write as it depends on the address
+    let bank = if start_address < BANK1_REGION.end() {
+        pac::FLASH.bank(0)
+    } else {
+        pac::FLASH.bank(1)
+    };
+    bank.cr().write(|w| {
+        w.set_pg(true);
+        #[cfg(flash_h7)]
+        w.set_psize(2); // 32 bits at once
+        w.set_eopie(true);
+        w.set_operrie(true);
+    });
+    cortex_m::asm::isb();
+    cortex_m::asm::dsb();
+    fence(Ordering::SeqCst);
+
+    let mut res = None;
+    let mut address = start_address;
+    for val in buf.chunks(4) {
+        write_volatile(address as *mut u32, u32::from_le_bytes(unwrap!(val.try_into())));
+        address += val.len() as u32;
+
+        res = Some(wait_ready(bank).await);
+        bank.sr().modify(|w| {
+            if w.eop() {
+                w.set_eop(true);
+            }
+        });
+        if unwrap!(res).is_err() {
+            break;
+        }
+    }
+
+    cortex_m::asm::isb();
+    cortex_m::asm::dsb();
+    fence(Ordering::SeqCst);
+
+    bank.cr().write(|w| {
+        w.set_pg(false);
+        w.set_eopie(false);
+        w.set_operrie(false);
+    });
+
+    unwrap!(res)
+}
+
 pub(crate) unsafe fn blocking_write(start_address: u32, buf: &[u8; WRITE_SIZE]) -> Result<(), Error> {
     // We cannot have the write setup sequence in begin_write as it depends on the address
     let bank = if start_address < BANK1_REGION.end() {
@@ -77,6 +154,36 @@ pub(crate) unsafe fn blocking_write(start_address: u32, buf: &[u8; WRITE_SIZE])
     unwrap!(res)
 }
 
+pub(crate) async unsafe fn erase_sector(sector: &FlashSector) -> Result<(), Error> {
+    let bank = pac::FLASH.bank(sector.bank as usize);
+    bank.cr().modify(|w| {
+        w.set_ser(true);
+        #[cfg(flash_h7)]
+        w.set_snb(sector.index_in_bank);
+        #[cfg(flash_h7ab)]
+        w.set_ssn(sector.index_in_bank);
+        w.set_eopie(true);
+        w.set_operrie(true);
+    });
+
+    bank.cr().modify(|w| {
+        w.set_start(true);
+    });
+
+    cortex_m::asm::isb();
+    cortex_m::asm::dsb();
+    fence(Ordering::SeqCst);
+
+    let ret: Result<(), Error> = wait_ready(bank).await;
+    bank.cr().modify(|w| {
+        w.set_ser(false);
+        w.set_eopie(false);
+        w.set_operrie(false);
+    });
+    bank_clear_all_err(bank);
+    ret
+}
+
 pub(crate) unsafe fn blocking_erase_sector(sector: &FlashSector) -> Result<(), Error> {
     let bank = pac::FLASH.bank(sector.bank as usize);
     bank.cr().modify(|w| {
@@ -112,46 +219,59 @@ unsafe fn bank_clear_all_err(bank: pac::flash::Bank) {
     bank.sr().modify(|_| {});
 }
 
+async fn wait_ready(bank: pac::flash::Bank) -> Result<(), Error> {
+    use core::future::poll_fn;
+    use core::task::Poll;
+
+    poll_fn(|cx| {
+        WAKER.register(cx.waker());
+
+        let sr = bank.sr().read();
+        if !sr.bsy() && !sr.qw() {
+            Poll::Ready(get_result(sr))
+        } else {
+            return Poll::Pending;
+        }
+    })
+    .await
+}
+
 unsafe fn blocking_wait_ready(bank: pac::flash::Bank) -> Result<(), Error> {
     loop {
         let sr = bank.sr().read();
 
         if !sr.bsy() && !sr.qw() {
-            if sr.wrperr() {
-                return Err(Error::Protected);
-            }
-            if sr.pgserr() {
-                error!("pgserr");
-                return Err(Error::Seq);
-            }
-            if sr.incerr() {
-                // writing to a different address when programming 256 bit word was not finished
-                error!("incerr");
-                return Err(Error::Seq);
-            }
-            if sr.crcrderr() {
-                error!("crcrderr");
-                return Err(Error::Seq);
-            }
-            if sr.operr() {
-                return Err(Error::Prog);
-            }
-            if sr.sneccerr1() {
-                // single ECC error
-                return Err(Error::Prog);
-            }
-            if sr.dbeccerr() {
-                // double ECC error
-                return Err(Error::Prog);
-            }
-            if sr.rdperr() {
-                return Err(Error::Protected);
-            }
-            if sr.rdserr() {
-                return Err(Error::Protected);
-            }
-
-            return Ok(());
+            return get_result(sr);
         }
     }
 }
+
+fn get_result(sr: Sr) -> Result<(), Error> {
+    if sr.wrperr() {
+        Err(Error::Protected)
+    } else if sr.pgserr() {
+        error!("pgserr");
+        Err(Error::Seq)
+    } else if sr.incerr() {
+        // writing to a different address when programming 256 bit word was not finished
+        error!("incerr");
+        Err(Error::Seq)
+    } else if sr.crcrderr() {
+        error!("crcrderr");
+        Err(Error::Seq)
+    } else if sr.operr() {
+        Err(Error::Prog)
+    } else if sr.sneccerr1() {
+        // single ECC error
+        Err(Error::Prog)
+    } else if sr.dbeccerr() {
+        // double ECC error
+        Err(Error::Prog)
+    } else if sr.rdperr() {
+        Err(Error::Protected)
+    } else if sr.rdserr() {
+        Err(Error::Protected)
+    } else {
+        Ok(())
+    }
+}
diff --git a/embassy-stm32/src/flash/mod.rs b/embassy-stm32/src/flash/mod.rs
index 39cd9b3a9..6211a37b7 100644
--- a/embassy-stm32/src/flash/mod.rs
+++ b/embassy-stm32/src/flash/mod.rs
@@ -1,14 +1,14 @@
 //! Flash memory (FLASH)
 use embedded_storage::nor_flash::{NorFlashError, NorFlashErrorKind};
 
-#[cfg(flash_f4)]
+#[cfg(any(flash_f4, flash_h7, flash_h7ab))]
 mod asynch;
 #[cfg(flash)]
 mod common;
 #[cfg(eeprom)]
 mod eeprom;
 
-#[cfg(flash_f4)]
+#[cfg(any(flash_f4, flash_h7, flash_h7ab))]
 pub use asynch::InterruptHandler;
 #[cfg(flash)]
 pub use common::*;
diff --git a/embassy-stm32/src/hsem/mod.rs b/embassy-stm32/src/hsem/mod.rs
index 4d3a5d68d..e62de0454 100644
--- a/embassy-stm32/src/hsem/mod.rs
+++ b/embassy-stm32/src/hsem/mod.rs
@@ -1,14 +1,22 @@
 //! Hardware Semaphore (HSEM)
 
+use core::future::poll_fn;
+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;
 
 // TODO: This code works for all HSEM implemenations except for the STM32WBA52/4/5xx MCUs.
 // Those MCUs have a different HSEM implementation (Secure semaphore lock support,
 // Privileged / unprivileged semaphore lock support, Semaphore lock protection via semaphore attribute),
 // which is not yet supported by this code.
 use crate::Peri;
-use crate::pac;
 use crate::rcc::{self, RccPeripheral};
+use crate::{interrupt, pac};
 
 /// HSEM error.
 #[derive(Debug)]
@@ -41,63 +49,152 @@ pub enum CoreId {
     Core1 = 0x8,
 }
 
-/// Get the current core id
-/// This code assume that it is only executed on a Cortex-M M0+, M4 or M7 core.
-#[inline(always)]
-pub fn get_current_coreid() -> CoreId {
-    let cpuid = unsafe { cortex_m::peripheral::CPUID::PTR.read_volatile().base.read() };
-    match (cpuid & 0x000000F0) >> 4 {
-        #[cfg(any(stm32wb, stm32wl))]
-        0x0 => CoreId::Core1,
+impl CoreId {
+    /// Get the current core id
+    /// This code assume that it is only executed on a Cortex-M M0+, M4 or M7 core.
+    pub fn current() -> Self {
+        let cpuid = unsafe { cortex_m::peripheral::CPUID::PTR.read_volatile().base.read() };
+        match (cpuid & 0x000000F0) >> 4 {
+            #[cfg(any(stm32wb, stm32wl))]
+            0x0 => CoreId::Core1,
 
-        #[cfg(not(any(stm32h745, stm32h747, stm32h755, stm32h757)))]
-        0x4 => CoreId::Core0,
+            #[cfg(not(any(stm32h745, stm32h747, stm32h755, stm32h757)))]
+            0x4 => CoreId::Core0,
 
-        #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
-        0x4 => CoreId::Core1,
+            #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
+            0x4 => CoreId::Core1,
 
-        #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
-        0x7 => CoreId::Core0,
-        _ => panic!("Unknown Cortex-M core"),
+            #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
+            0x7 => CoreId::Core0,
+            _ => panic!("Unknown Cortex-M core"),
+        }
+    }
+
+    /// Translates the core ID to an index into the interrupt registers.
+    pub fn to_index(&self) -> usize {
+        match &self {
+            CoreId::Core0 => 0,
+            #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757, stm32wb, stm32wl))]
+            CoreId::Core1 => 1,
+        }
     }
 }
 
-/// Translates the core ID to an index into the interrupt registers.
-#[inline(always)]
-fn core_id_to_index(core: CoreId) -> usize {
-    match core {
-        CoreId::Core0 => 0,
-        #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757, stm32wb, stm32wl))]
-        CoreId::Core1 => 1,
+#[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>,
+}
+
+impl<T: Instance> interrupt::typelevel::Handler<T::Interrupt> for HardwareSemaphoreInterruptHandler<T> {
+    unsafe fn on_interrupt() {
+        let core_id = CoreId::current();
+
+        for number in 0..5 {
+            if T::regs().isr(core_id.to_index()).read().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();
+            }
+        }
     }
 }
 
-/// HSEM driver
-pub struct HardwareSemaphore<'d, T: Instance> {
-    _peri: Peri<'d, T>,
+/// Hardware semaphore mutex
+pub struct HardwareSemaphoreMutex<'a, T: Instance> {
+    index: u8,
+    process_id: u8,
+    _lifetime: PhantomData<&'a mut T>,
 }
 
-impl<'d, T: Instance> HardwareSemaphore<'d, T> {
-    /// Creates a new HardwareSemaphore instance.
-    pub fn new(peripheral: Peri<'d, T>) -> Self {
-        rcc::enable_and_reset::<T>();
+impl<'a, T: Instance> Drop for HardwareSemaphoreMutex<'a, T> {
+    fn drop(&mut self) {
+        HardwareSemaphoreChannel::<'a, T> {
+            index: self.index,
+            _lifetime: PhantomData,
+        }
+        .unlock(self.process_id);
+    }
+}
+
+/// Hardware semaphore channel
+pub struct HardwareSemaphoreChannel<'a, T: Instance> {
+    index: u8,
+    _lifetime: PhantomData<&'a mut T>,
+}
+
+impl<'a, T: Instance> HardwareSemaphoreChannel<'a, T> {
+    pub(crate) const fn new(number: u8) -> Self {
+        core::assert!(number > 0 && number <= 6);
+
+        Self {
+            index: number - 1,
+            _lifetime: PhantomData,
+        }
+    }
+
+    /// 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.
+    pub async fn lock(&mut self, process_id: u8) -> HardwareSemaphoreMutex<'a, T> {
+        let core_id = CoreId::current();
+
+        poll_fn(|cx| {
+            T::state().waker_for(self.index).register(cx.waker());
+
+            compiler_fence(Ordering::SeqCst);
+
+            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),
+                None => Poll::Pending,
+            }
+        })
+        .await
+    }
 
-        HardwareSemaphore { _peri: peripheral }
+    /// 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.
-    pub fn two_step_lock(&mut self, sem_id: u8, process_id: u8) -> Result<(), HsemError> {
-        T::regs().r(sem_id as usize).write(|w| {
+    pub fn two_step_lock(&mut self, process_id: u8) -> Result<(), HsemError> {
+        T::regs().r(self.index as usize).write(|w| {
             w.set_procid(process_id);
-            w.set_coreid(get_current_coreid() as u8);
+            w.set_coreid(CoreId::current() as u8);
             w.set_lock(true);
         });
-        let reg = T::regs().r(sem_id as usize).read();
+        let reg = T::regs().r(self.index as usize).read();
         match (
             reg.lock(),
-            reg.coreid() == get_current_coreid() as u8,
+            reg.coreid() == CoreId::current() as u8,
             reg.procid() == process_id,
         ) {
             (true, true, true) => Ok(()),
@@ -108,9 +205,9 @@ impl<'d, T: Instance> HardwareSemaphore<'d, T> {
     /// Locks the semaphore.
     /// The 1-step procedure consists in a read to lock and check the semaphore in a single step,
     /// carried out from the HSEM_RLRx register.
-    pub fn one_step_lock(&mut self, sem_id: u8) -> Result<(), HsemError> {
-        let reg = T::regs().rlr(sem_id as usize).read();
-        match (reg.lock(), reg.coreid() == get_current_coreid() as u8, reg.procid()) {
+    pub fn one_step_lock(&mut self) -> Result<(), HsemError> {
+        let reg = T::regs().rlr(self.index as usize).read();
+        match (reg.lock(), reg.coreid() == CoreId::current() as u8, reg.procid()) {
             (false, true, 0) => Ok(()),
             _ => Err(HsemError::LockFailed),
         }
@@ -119,14 +216,60 @@ impl<'d, T: Instance> HardwareSemaphore<'d, T> {
     /// Unlocks the semaphore.
     /// Unlocking a semaphore is a protected process, to prevent accidental clearing by a AHB bus
     /// core ID or by a process not having the semaphore lock right.
-    pub fn unlock(&mut self, sem_id: u8, process_id: u8) {
-        T::regs().r(sem_id as usize).write(|w| {
+    pub fn unlock(&mut self, process_id: u8) {
+        T::regs().r(self.index as usize).write(|w| {
             w.set_procid(process_id);
-            w.set_coreid(get_current_coreid() as u8);
+            w.set_coreid(CoreId::current() as u8);
             w.set_lock(false);
         });
     }
 
+    /// Return the channel number
+    pub const fn channel(&self) -> u8 {
+        self.index + 1
+    }
+}
+
+/// HSEM driver
+pub struct HardwareSemaphore<T: Instance> {
+    _type: PhantomData<T>,
+}
+
+impl<T: Instance> HardwareSemaphore<T> {
+    /// Creates a new HardwareSemaphore instance.
+    pub fn new<'d>(
+        _peripheral: Peri<'d, T>,
+        _irq: impl interrupt::typelevel::Binding<T::Interrupt, HardwareSemaphoreInterruptHandler<T>> + 'd,
+    ) -> Self {
+        rcc::enable_and_reset::<T>();
+
+        HardwareSemaphore { _type: PhantomData }
+    }
+
+    /// 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
+    ///
+    /// 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),
+        ]
+    }
+
     /// Unlocks all semaphores.
     /// All semaphores locked by a COREID can be unlocked at once by using the HSEM_CR
     /// register. Write COREID and correct KEY value in HSEM_CR. All locked semaphores with a
@@ -138,11 +281,6 @@ impl<'d, T: Instance> HardwareSemaphore<'d, T> {
         });
     }
 
-    /// Checks if the semaphore is locked.
-    pub fn is_semaphore_locked(&self, sem_id: u8) -> bool {
-        T::regs().r(sem_id as usize).read().lock()
-    }
-
     /// Sets the clear (unlock) key
     pub fn set_clear_key(&mut self, key: u16) {
         T::regs().keyr().modify(|w| w.set_key(key));
@@ -152,38 +290,61 @@ impl<'d, T: Instance> HardwareSemaphore<'d, T> {
     pub fn get_clear_key(&mut self) -> u16 {
         T::regs().keyr().read().key()
     }
+}
 
-    /// Sets the interrupt enable bit for the semaphore.
-    pub fn enable_interrupt(&mut self, core_id: CoreId, sem_x: usize, enable: bool) {
-        T::regs()
-            .ier(core_id_to_index(core_id))
-            .modify(|w| w.set_ise(sem_x, enable));
+#[cfg(all(stm32wb, feature = "low-power"))]
+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;
     }
+}
 
-    /// Gets the interrupt flag for the semaphore.
-    pub fn is_interrupt_active(&mut self, core_id: CoreId, sem_x: usize) -> bool {
-        T::regs().isr(core_id_to_index(core_id)).read().isf(sem_x)
+struct State {
+    wakers: [AtomicWaker; 6],
+}
+
+impl State {
+    const fn new() -> Self {
+        Self {
+            wakers: [const { AtomicWaker::new() }; 6],
+        }
     }
 
-    /// Clears the interrupt flag for the semaphore.
-    pub fn clear_interrupt(&mut self, core_id: CoreId, sem_x: usize) {
-        T::regs()
-            .icr(core_id_to_index(core_id))
-            .write(|w| w.set_isc(sem_x, false));
+    const fn waker_for(&self, index: u8) -> &AtomicWaker {
+        &self.wakers[index as usize]
     }
 }
 
 trait SealedInstance {
     fn regs() -> pac::hsem::Hsem;
+    fn state() -> &'static State;
 }
 
 /// HSEM instance trait.
 #[allow(private_bounds)]
-pub trait Instance: SealedInstance + PeripheralType + RccPeripheral + Send + 'static {}
+pub trait Instance: SealedInstance + PeripheralType + RccPeripheral + Send + 'static {
+    /// Interrupt for this peripheral.
+    type Interrupt: interrupt::typelevel::Interrupt;
+}
 
 impl SealedInstance for crate::peripherals::HSEM {
     fn regs() -> crate::pac::hsem::Hsem {
         crate::pac::HSEM
     }
+
+    fn state() -> &'static State {
+        static STATE: State = State::new();
+        &STATE
+    }
 }
-impl Instance for crate::peripherals::HSEM {}
+
+foreach_interrupt!(
+    ($inst:ident, hsem, $block:ident, $signal_name:ident, $irq:ident) => {
+        impl Instance for crate::peripherals::$inst {
+            type Interrupt = crate::interrupt::typelevel::$irq;
+        }
+    };
+);
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..9de49c867 100644
--- a/embassy-stm32/src/low_power.rs
+++ b/embassy-stm32/src/low_power.rs
@@ -50,7 +50,7 @@ use critical_section::CriticalSection;
 use embassy_executor::*;
 
 use crate::interrupt;
-use crate::rcc::{REFCOUNT_STOP1, REFCOUNT_STOP2};
+use crate::rcc::{RCC_CONFIG, REFCOUNT_STOP1, REFCOUNT_STOP2};
 use crate::time_driver::get_driver;
 
 const THREAD_PENDER: usize = usize::MAX;
@@ -147,17 +147,17 @@ pub enum StopMode {
     Stop2,
 }
 
-#[cfg(any(stm32l4, stm32l5, stm32u5, stm32wba, stm32wlex, stm32u0))]
-use stm32_metapac::pwr::vals::Lpms;
+#[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?
         }
     }
@@ -201,7 +201,7 @@ impl Executor {
             {
                 use crate::pac::rcc::vals::Sw;
                 use crate::pac::{PWR, RCC};
-                use crate::rcc::{RCC_CONFIG, init as init_rcc};
+                use crate::rcc::init as init_rcc;
 
                 let extscr = PWR.extscr().read();
                 if extscr.c1stop2f() || extscr.c1stopf() {
@@ -237,14 +237,77 @@ 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, _cs: CriticalSection) -> 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};
+
+        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 {
+                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) {
-        #[cfg(any(stm32l4, stm32l5, stm32u5, stm32u0, stm32wba, stm32wlex))]
+    fn configure_stop(&self, _cs: CriticalSection, stop_mode: StopMode) -> Result<(), ()> {
+        #[cfg(all(stm32wb, feature = "low-power"))]
+        self.configure_stop_stm32wb(_cs)?;
+
+        #[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| {
@@ -252,6 +315,8 @@ impl Executor {
             v.set_lpms(vals::Lpms::STOP);
             v.set_svos(vals::Svos::SCALE3);
         });
+
+        Ok(())
     }
 
     fn configure_pwr(&self) {
@@ -265,14 +330,14 @@ impl Executor {
         compiler_fence(Ordering::SeqCst);
 
         critical_section::with(|cs| {
+            let _ = unsafe { RCC_CONFIG }?;
             let stop_mode = Self::stop_mode(cs)?;
-            let _ = get_driver().pause_time(cs).ok()?;
+            get_driver().pause_time(cs).ok()?;
+            self.configure_stop(cs, 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/stm32h7/src/bin/flash_async.rs b/examples/stm32h7/src/bin/flash_async.rs
new file mode 100644
index 000000000..96d1936f3
--- /dev/null
+++ b/examples/stm32h7/src/bin/flash_async.rs
@@ -0,0 +1,84 @@
+#![no_std]
+#![no_main]
+
+use defmt::{info, unwrap};
+use embassy_executor::Spawner;
+use embassy_stm32::flash::{Flash, InterruptHandler};
+use embassy_stm32::gpio::{AnyPin, Level, Output, Speed};
+use embassy_stm32::{Peri, bind_interrupts};
+use embassy_time::Timer;
+use {defmt_rtt as _, panic_probe as _};
+
+bind_interrupts!(struct Irqs {
+    FLASH => InterruptHandler;
+});
+
+#[embassy_executor::main]
+async fn main(spawner: Spawner) {
+    let p = embassy_stm32::init(Default::default());
+    info!("Hello Flash!");
+
+    let mut f = Flash::new(p.FLASH, Irqs);
+
+    // Led should blink uninterrupted during ~2sec erase operation
+    spawner.spawn(blinky(p.PB14.into()).unwrap());
+
+    // Test on bank 2 in order not to stall CPU.
+    test_flash(&mut f, 1024 * 1024, 128 * 1024).await;
+}
+
+#[embassy_executor::task]
+async fn blinky(p: Peri<'static, AnyPin>) {
+    let mut led = Output::new(p, Level::High, Speed::Low);
+
+    loop {
+        info!("high");
+        led.set_high();
+        Timer::after_millis(300).await;
+
+        info!("low");
+        led.set_low();
+        Timer::after_millis(300).await;
+    }
+}
+
+async fn test_flash<'a>(f: &mut Flash<'a>, offset: u32, size: u32) {
+    info!("Testing offset: {=u32:#X}, size: {=u32:#X}", offset, size);
+
+    info!("Reading...");
+    let mut buf = [0u8; 32];
+    unwrap!(f.blocking_read(offset, &mut buf));
+    info!("Read: {=[u8]:x}", buf);
+
+    info!("Erasing...");
+    unwrap!(f.erase(offset, offset + size).await);
+
+    info!("Reading...");
+    let mut buf = [0u8; 32];
+    unwrap!(f.blocking_read(offset, &mut buf));
+    info!("Read after erase: {=[u8]:x}", buf);
+
+    info!("Writing...");
+    unwrap!(
+        f.write(
+            offset,
+            &[
+                1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28,
+                29, 30, 31, 32
+            ]
+        )
+        .await
+    );
+
+    info!("Reading...");
+    let mut buf = [0u8; 32];
+    unwrap!(f.blocking_read(offset, &mut buf));
+    info!("Read: {=[u8]:x}", buf);
+    assert_eq!(
+        &buf[..],
+        &[
+            1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29,
+            30, 31, 32
+        ]
+    );
+}
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"] }
diff --git a/tests/stm32/Cargo.toml b/tests/stm32/Cargo.toml
index b92b47be2..8fcb6b2b4 100644
--- a/tests/stm32/Cargo.toml
+++ b/tests/stm32/Cargo.toml
@@ -31,9 +31,9 @@ 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"]
+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"]
+stm32wl55jc = ["embassy-stm32/stm32wl55jc-cm4", "not-gpdma", "rng", "chrono", "hsem"]
 stm32f091rc = ["embassy-stm32/stm32f091rc", "cm0", "not-gpdma", "chrono"]
 stm32h503rb = ["embassy-stm32/stm32h503rb", "spi-v345", "rng", "stop"]
 stm32h7s3l8 = ["embassy-stm32/stm32h7s3l8", "spi-v345", "rng", "cordic", "hash-v34"] # TODO: fdcan crashes, cryp dma hangs.
@@ -58,6 +58,7 @@ not-gpdma = []
 dac = []
 ucpd = []
 cordic = ["dep:num-traits"]
+hsem = []
 dual-bank = ["embassy-stm32/dual-bank"]
 single-bank = ["embassy-stm32/single-bank"]
 eeprom = []
@@ -224,6 +225,11 @@ name = "wpan_mac"
 path = "src/bin/wpan_mac.rs"
 required-features = [ "mac",]
 
+[[bin]]
+name = "hsem"
+path = "src/bin/hsem.rs"
+required-features = [ "hsem",]
+
 # END TESTS
 
 [profile.dev]
diff --git a/tests/stm32/src/bin/hsem.rs b/tests/stm32/src/bin/hsem.rs
new file mode 100644
index 000000000..fa69f22b2
--- /dev/null
+++ b/tests/stm32/src/bin/hsem.rs
@@ -0,0 +1,50 @@
+// required-features: hsem
+#![no_std]
+#![no_main]
+
+#[path = "../common.rs"]
+mod common;
+
+use common::*;
+use embassy_executor::Spawner;
+use embassy_stm32::bind_interrupts;
+use embassy_stm32::hsem::{HardwareSemaphore, HardwareSemaphoreInterruptHandler};
+use embassy_stm32::peripherals::HSEM;
+
+bind_interrupts!(struct Irqs{
+    HSEM => HardwareSemaphoreInterruptHandler<HSEM>;
+});
+
+#[embassy_executor::main]
+async fn main(_spawner: Spawner) {
+    let p: embassy_stm32::Peripherals = init();
+
+    let hsem = HardwareSemaphore::new(p.HSEM, Irqs);
+
+    //    if hsem.channel_for(SemaphoreNumber::Channel5).is_semaphore_locked() {
+    //        defmt::panic!("Semaphore 5 already locked!")
+    //    }
+    //
+    //    hsem.channel_for(SemaphoreNumber::Channel5).one_step_lock().unwrap();
+    //    hsem.channel_for(SemaphoreNumber::Channel1).two_step_lock(0).unwrap();
+    //
+    //    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");
+
+    let mutex = channel5.lock(0).await;
+
+    info!("Locked channel 5");
+
+    drop(mutex);
+
+    info!("Unlocked channel 5");
+
+    info!("Test OK");
+    cortex_m::asm::bkpt();
+}