12 files changed, 2451 insertions, 0 deletions

diff --git a/embassy-stm32-wpan/src/wba/bindings.rs b/embassy-stm32-wpan/src/wba/bindings.rs
new file mode 100644
index 000000000..d2030cfb8
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/bindings.rs
@@ -0,0 +1 @@
+pub use stm32_bindings::bindings::{mac, wba_ble_stack as ble, wba_link_layer as link_layer};
diff --git a/embassy-stm32-wpan/src/wba/linklayer_plat.rs b/embassy-stm32-wpan/src/wba/linklayer_plat.rs
new file mode 100644
index 000000000..108e84efe
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/linklayer_plat.rs
@@ -0,0 +1,913 @@
+// /* USER CODE BEGIN Header */
+// /**
+//   ******************************************************************************
+//   * @file    linklayer_plat.c
+//   * @author  MCD Application Team
+//   * @brief   Source file for the linklayer plateform adaptation layer
+//   ******************************************************************************
+//   * @attention
+//   *
+//   * Copyright (c) 2024 STMicroelectronics.
+//   * All rights reserved.
+//   *
+//   * This software is licensed under terms that can be found in the LICENSE file
+//   * in the root directory of this software component.
+//   * If no LICENSE file comes with this software, it is provided AS-IS.
+//   *
+//   ******************************************************************************
+//   */
+// /* USER CODE END Header */
+//
+// #include "stm32wbaxx_hal.h"
+// #include "stm32wbaxx_hal_conf.h"
+// #include "stm32wbaxx_ll_rcc.h"
+//
+// #include "app_common.h"
+// #include "app_conf.h"
+// #include "linklayer_plat.h"
+// #include "scm.h"
+// #include "log_module.h"
+// #if (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1)
+// #include "adc_ctrl.h"
+// #endif /* (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1) */
+//
+// #if (CFG_LPM_LEVEL != 0)
+// #include "stm32_lpm.h"
+// #include "stm32_lpm_if.h"
+// #endif /* (CFG_LPM_LEVEL != 0) */
+//
+// /* USER CODE BEGIN Includes */
+//
+// /* USER CODE END Includes */
+//
+// #define max(a,b) ((a) > (b) ? a : b)
+//
+// /* 2.4GHz RADIO ISR callbacks */
+// void (*radio_callback)(void) = NULL;
+// void (*low_isr_callback)(void) = NULL;
+//
+// /* RNG handle */
+// extern RNG_HandleTypeDef hrng;
+//
+// #if (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1)
+// /* Link Layer temperature request from background */
+// extern void ll_sys_bg_temperature_measurement(void);
+// #endif /* (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1) */
+//
+// /* Radio critical sections */
+// static uint32_t primask_bit = 0;
+// volatile int32_t prio_high_isr_counter = 0;
+// volatile int32_t prio_low_isr_counter = 0;
+// volatile int32_t prio_sys_isr_counter = 0;
+// volatile int32_t irq_counter = 0;
+// volatile uint32_t local_basepri_value = 0;
+//
+// /* Radio SW low ISR global variable */
+// volatile uint8_t radio_sw_low_isr_is_running_high_prio = 0;
+//
+// /* Radio bus clock control variables */
+// uint8_t AHB5_SwitchedOff = 0;
+// uint32_t radio_sleep_timer_val = 0;
+//
+// /* USER CODE BEGIN LINKLAYER_PLAT 0 */
+//
+// /* USER CODE END LINKLAYER_PLAT 0 */
+#![cfg(feature = "wba")]
+#![allow(clippy::missing_safety_doc)]
+
+use core::hint::spin_loop;
+use core::ptr;
+use core::sync::atomic::{AtomicBool, AtomicI32, AtomicPtr, AtomicU32, Ordering};
+
+use cortex_m::asm::{dsb, isb};
+use cortex_m::interrupt::InterruptNumber;
+use cortex_m::peripheral::NVIC;
+use cortex_m::register::basepri;
+use critical_section;
+#[cfg(feature = "defmt")]
+use defmt::trace;
+use embassy_stm32::NVIC_PRIO_BITS;
+use embassy_time::{Duration, block_for};
+
+use super::bindings::{link_layer, mac};
+
+// Missing constant from stm32-bindings - RADIO_SW_LOW interrupt number
+// For STM32WBA, this is typically RADIO_IRQ_BUSY (interrupt 43)
+const RADIO_SW_LOW_INTR_NUM: u32 = 43;
+
+type Callback = unsafe extern "C" fn();
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+#[repr(transparent)]
+struct RawInterrupt(u16);
+
+impl RawInterrupt {
+    #[inline(always)]
+    fn new(irq: u32) -> Self {
+        debug_assert!(irq <= u16::MAX as u32);
+        Self(irq as u16)
+    }
+}
+
+impl From<u32> for RawInterrupt {
+    #[inline(always)]
+    fn from(value: u32) -> Self {
+        Self::new(value)
+    }
+}
+
+unsafe impl InterruptNumber for RawInterrupt {
+    fn number(self) -> u16 {
+        self.0
+    }
+}
+
+static RADIO_CALLBACK: AtomicPtr<()> = AtomicPtr::new(ptr::null_mut());
+static LOW_ISR_CALLBACK: AtomicPtr<()> = AtomicPtr::new(ptr::null_mut());
+
+static IRQ_COUNTER: AtomicI32 = AtomicI32::new(0);
+
+static PRIO_HIGH_ISR_COUNTER: AtomicI32 = AtomicI32::new(0);
+static PRIO_LOW_ISR_COUNTER: AtomicI32 = AtomicI32::new(0);
+static PRIO_SYS_ISR_COUNTER: AtomicI32 = AtomicI32::new(0);
+static LOCAL_BASEPRI_VALUE: AtomicU32 = AtomicU32::new(0);
+
+static RADIO_SW_LOW_ISR_RUNNING_HIGH_PRIO: AtomicBool = AtomicBool::new(false);
+static AHB5_SWITCHED_OFF: AtomicBool = AtomicBool::new(false);
+static RADIO_SLEEP_TIMER_VAL: AtomicU32 = AtomicU32::new(0);
+
+static PRNG_STATE: AtomicU32 = AtomicU32::new(0);
+static PRNG_INIT: AtomicBool = AtomicBool::new(false);
+
+// Critical-section restore token for IRQ enable/disable pairing.
+// Only written when the IRQ disable counter transitions 0->1, and consumed when it transitions 1->0.
+static mut CS_RESTORE_STATE: Option<critical_section::RestoreState> = None;
+
+unsafe extern "C" {
+    static SystemCoreClock: u32;
+}
+
+#[inline(always)]
+fn read_system_core_clock() -> u32 {
+    unsafe { ptr::read_volatile(&SystemCoreClock) }
+}
+
+#[inline(always)]
+fn store_callback(slot: &AtomicPtr<()>, cb: Option<Callback>) {
+    let ptr = cb.map_or(ptr::null_mut(), |f| f as *mut ());
+    slot.store(ptr, Ordering::Release);
+}
+
+#[inline(always)]
+fn load_callback(slot: &AtomicPtr<()>) -> Option<Callback> {
+    let ptr = slot.load(Ordering::Acquire);
+    if ptr.is_null() {
+        None
+    } else {
+        Some(unsafe { core::mem::transmute::<*mut (), Callback>(ptr) })
+    }
+}
+
+#[inline(always)]
+fn priority_shift() -> u8 {
+    8 - NVIC_PRIO_BITS as u8
+}
+
+fn pack_priority(raw: u32) -> u8 {
+    let shift = priority_shift();
+    let priority_bits = NVIC_PRIO_BITS as u32;
+    let mask = if priority_bits >= 32 {
+        u32::MAX
+    } else {
+        (1u32 << priority_bits) - 1
+    };
+    let clamped = raw & mask;
+    (clamped << u32::from(shift)) as u8
+}
+
+#[inline(always)]
+fn counter_release(counter: &AtomicI32) -> bool {
+    counter.fetch_sub(1, Ordering::SeqCst) <= 1
+}
+
+#[inline(always)]
+fn counter_acquire(counter: &AtomicI32) -> bool {
+    counter.fetch_add(1, Ordering::SeqCst) == 0
+}
+
+unsafe fn nvic_enable(irq: u32) {
+    NVIC::unmask(RawInterrupt::new(irq));
+    dsb();
+    isb();
+}
+
+unsafe fn nvic_disable(irq: u32) {
+    NVIC::mask(RawInterrupt::new(irq));
+    dsb();
+    isb();
+}
+
+unsafe fn nvic_set_pending(irq: u32) {
+    NVIC::pend(RawInterrupt::new(irq));
+    dsb();
+    isb();
+}
+
+unsafe fn nvic_get_active(irq: u32) -> bool {
+    NVIC::is_active(RawInterrupt::new(irq))
+}
+
+unsafe fn nvic_set_priority(irq: u32, priority: u8) {
+    // STM32WBA is ARMv8-M, which uses byte-accessible IPR registers
+    let nvic = &*NVIC::PTR;
+    nvic.ipr[irq as usize].write(priority);
+
+    dsb();
+    isb();
+}
+
+#[inline(always)]
+fn set_basepri_max(value: u8) {
+    unsafe {
+        if basepri::read() < value {
+            basepri::write(value);
+        }
+    }
+}
+
+fn prng_next() -> u32 {
+    #[inline]
+    fn xorshift(mut x: u32) -> u32 {
+        x ^= x << 13;
+        x ^= x >> 17;
+        x ^= x << 5;
+        x
+    }
+
+    if !PRNG_INIT.load(Ordering::Acquire) {
+        let seed = unsafe {
+            let timer = link_layer::ll_intf_cmn_get_slptmr_value();
+            let core_clock = read_system_core_clock();
+            timer ^ core_clock ^ 0x6C8E_9CF5
+        };
+        PRNG_STATE.store(seed, Ordering::Relaxed);
+        PRNG_INIT.store(true, Ordering::Release);
+    }
+
+    let mut current = PRNG_STATE.load(Ordering::Relaxed);
+    loop {
+        let next = xorshift(current);
+        match PRNG_STATE.compare_exchange_weak(current, next, Ordering::AcqRel, Ordering::Relaxed) {
+            Ok(_) => return next,
+            Err(v) => current = v,
+        }
+    }
+}
+
+pub unsafe fn run_radio_high_isr() {
+    if let Some(cb) = load_callback(&RADIO_CALLBACK) {
+        cb();
+    }
+}
+
+pub unsafe fn run_radio_sw_low_isr() {
+    if let Some(cb) = load_callback(&LOW_ISR_CALLBACK) {
+        cb();
+    }
+
+    if RADIO_SW_LOW_ISR_RUNNING_HIGH_PRIO.swap(false, Ordering::AcqRel) {
+        nvic_set_priority(RADIO_SW_LOW_INTR_NUM, pack_priority(mac::RADIO_SW_LOW_INTR_PRIO));
+    }
+}
+
+// /**
+//   * @brief  Configure the necessary clock sources for the radio.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_ClockInit() {
+    //   uint32_t linklayer_slp_clk_src = LL_RCC_RADIOSLEEPSOURCE_NONE;
+    //
+    //   /* Get the Link Layer sleep timer clock source */
+    //   linklayer_slp_clk_src = LL_RCC_RADIO_GetSleepTimerClockSource();
+    //   if(linklayer_slp_clk_src == LL_RCC_RADIOSLEEPSOURCE_NONE)
+    //   {
+    //     /* If there is no clock source defined, should be selected before */
+    //     assert_param(0);
+    //   }
+    //
+    //   /* Enable AHB5ENR peripheral clock (bus CLK) */
+    //   __HAL_RCC_RADIO_CLK_ENABLE();
+    trace!("LINKLAYER_PLAT_ClockInit: get_slptmr_value");
+    let _ = link_layer::ll_intf_cmn_get_slptmr_value();
+}
+
+// /**
+//   * @brief  Link Layer active waiting loop.
+//   * @param  delay: delay in us
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_DelayUs(delay: u32) {
+    //   static uint8_t lock = 0;
+    //   uint32_t t0;
+    //   uint32_t primask_bit;
+    //
+    //   /* Enter critical section */
+    //   primask_bit= __get_PRIMASK();
+    //   __disable_irq();
+    //
+    //   if (lock == 0U)
+    //   {
+    //     /* Initialize counter */
+    //     /* Reset cycle counter to prevent overflow
+    //        As a us counter, it is assumed than even with re-entrancy,
+    //        overflow will never happen before re-initializing this counter */
+    //     DWT->CYCCNT = 0U;
+    //     /* Enable DWT by safety but should be useless (as already set) */
+    //     SET_BIT(DCB->DEMCR, DCB_DEMCR_TRCENA_Msk);
+    //     /* Enable counter */
+    //     SET_BIT(DWT->CTRL, DWT_CTRL_CYCCNTENA_Msk);
+    //   }
+    //   /* Increment 're-entrance' counter */
+    //   lock++;
+    //   /* Get starting time stamp */
+    //   t0 = DWT->CYCCNT;
+    //   /* Exit critical section */
+    //  __set_PRIMASK(primask_bit);
+    //
+    //   /* Turn us into cycles */
+    //   delay = delay * (SystemCoreClock / 1000000U);
+    //   delay += t0;
+    //
+    //   /* Busy waiting loop */
+    //   while (DWT->CYCCNT < delay)
+    //   {
+    //   };
+    //
+    //   /* Enter critical section */
+    //   primask_bit= __get_PRIMASK();
+    //   __disable_irq();
+    //   if (lock == 1U)
+    //   {
+    //     /* Disable counter */
+    //     CLEAR_BIT(DWT->CTRL, DWT_CTRL_CYCCNTENA_Msk);
+    //   }
+    //   /* Decrement 're-entrance' counter */
+    //   lock--;
+    //   /* Exit critical section */
+    //  __set_PRIMASK(primask_bit);
+    //
+    trace!("LINKLAYER_PLAT_DelayUs: delay={}", delay);
+    block_for(Duration::from_micros(u64::from(delay)));
+}
+
+// /**
+//   * @brief  Link Layer assertion API
+//   * @param  condition: conditional statement to be checked.
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_Assert(condition: u8) {
+    if condition == 0 {
+        panic!("LINKLAYER_PLAT assertion failed");
+    }
+}
+
+// /**
+//   * @brief  Enable/disable the Link Layer active clock (baseband clock).
+//   * @param  enable: boolean value to enable (1) or disable (0) the clock.
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_WaitHclkRdy() {
+    trace!("LINKLAYER_PLAT_WaitHclkRdy");
+    if AHB5_SWITCHED_OFF.swap(false, Ordering::AcqRel) {
+        let reference = RADIO_SLEEP_TIMER_VAL.load(Ordering::Acquire);
+        trace!("LINKLAYER_PLAT_WaitHclkRdy: reference={}", reference);
+        while reference == link_layer::ll_intf_cmn_get_slptmr_value() {
+            spin_loop();
+        }
+    }
+}
+
+// /**
+//   * @brief  Notify the Link Layer platform layer the system will enter in WFI
+//   *         and AHB5 clock may be turned of regarding the 2.4Ghz radio state.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_NotifyWFIEnter() {
+    //   /* Check if Radio state will allow the AHB5 clock to be cut */
+    //
+    //   /* AHB5 clock will be cut in the following cases:
+    //    * - 2.4GHz radio is not in ACTIVE mode (in SLEEP or DEEPSLEEP mode).
+    //    * - RADIOSMEN and STRADIOCLKON bits are at 0.
+    //    */
+    //   if((LL_PWR_GetRadioMode() != LL_PWR_RADIO_ACTIVE_MODE) ||
+    //      ((__HAL_RCC_RADIO_IS_CLK_SLEEP_ENABLED() == 0) && (LL_RCC_RADIO_IsEnabledSleepTimerClock() == 0)))
+    //   {
+    //     AHB5_SwitchedOff = 1;
+    //   }
+    trace!("LINKLAYER_PLAT_NotifyWFIEnter");
+    AHB5_SWITCHED_OFF.store(true, Ordering::Release);
+}
+
+// /**
+//   * @brief  Notify the Link Layer platform layer the system exited WFI and AHB5
+//   *         clock may be resynchronized as is may have been turned of during
+//   *         low power mode entry.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_NotifyWFIExit() {
+    trace!("LINKLAYER_PLAT_NotifyWFIExit");
+    //   /* Check if AHB5 clock has been turned of and needs resynchronisation */
+    if AHB5_SWITCHED_OFF.load(Ordering::Acquire) {
+        //     /* Read sleep register as earlier as possible */
+        let value = link_layer::ll_intf_cmn_get_slptmr_value();
+        RADIO_SLEEP_TIMER_VAL.store(value, Ordering::Release);
+    }
+}
+
+// /**
+//   * @brief  Active wait on bus clock readiness.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_AclkCtrl(_enable: u8) {
+    trace!("LINKLAYER_PLAT_AclkCtrl: enable={}", _enable);
+    if _enable != 0 {
+        // #if (CFG_SCM_SUPPORTED == 1)
+        //     /* SCM HSE BEGIN */
+        //     /* Polling on HSE32 activation */
+        //     SCM_HSE_WaitUntilReady();
+        //     /* Enable RADIO baseband clock (active CLK) */
+        //     HAL_RCCEx_EnableRadioBBClock();
+        //     /* SCM HSE END */
+        // #else
+        //     /* Enable RADIO baseband clock (active CLK) */
+        //     HAL_RCCEx_EnableRadioBBClock();
+        //     /* Polling on HSE32 activation */
+        //     while ( LL_RCC_HSE_IsReady() == 0);
+        // #endif /* CFG_SCM_SUPPORTED */
+        // NOTE: Add a proper assertion once a typed `Radio` peripheral exists in embassy-stm32
+        // that exposes the baseband clock enable status via RCC.
+    } else {
+        //     /* Disable RADIO baseband clock (active CLK) */
+        //     HAL_RCCEx_DisableRadioBBClock();
+    }
+}
+
+// /**
+//   * @brief  Link Layer RNG request.
+//   * @param  ptr_rnd: pointer to the variable that hosts the number.
+//   * @param  len: number of byte of anthropy to get.
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_GetRNG(ptr_rnd: *mut u8, len: u32) {
+    //   uint32_t nb_remaining_rng = len;
+    //   uint32_t generated_rng;
+    //
+    //   /* Get the requested RNGs (4 bytes by 4bytes) */
+    //   while(nb_remaining_rng >= 4)
+    //   {
+    //     generated_rng = 0;
+    //     HW_RNG_Get(1, &generated_rng);
+    //     memcpy((ptr_rnd+(len-nb_remaining_rng)), &generated_rng, 4);
+    //     nb_remaining_rng -=4;
+    //   }
+    //
+    //   /* Get the remaining number of RNGs */
+    //   if(nb_remaining_rng>0){
+    //     generated_rng = 0;
+    //     HW_RNG_Get(1, &generated_rng);
+    //     memcpy((ptr_rnd+(len-nb_remaining_rng)), &generated_rng, nb_remaining_rng);
+    //   }
+    trace!("LINKLAYER_PLAT_GetRNG: ptr_rnd={:?}, len={}", ptr_rnd, len);
+    if ptr_rnd.is_null() || len == 0 {
+        return;
+    }
+
+    for i in 0..len {
+        let byte = (prng_next() >> ((i & 3) * 8)) as u8;
+        ptr::write_volatile(ptr_rnd.add(i as usize), byte);
+    }
+}
+
+// /**
+//   * @brief  Initialize Link Layer radio high priority interrupt.
+//   * @param  intr_cb: function pointer to assign for the radio high priority ISR routine.
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_SetupRadioIT(intr_cb: Option<Callback>) {
+    trace!("LINKLAYER_PLAT_SetupRadioIT: intr_cb={:?}", intr_cb);
+    store_callback(&RADIO_CALLBACK, intr_cb);
+
+    if intr_cb.is_some() {
+        nvic_set_priority(mac::RADIO_INTR_NUM, pack_priority(mac::RADIO_INTR_PRIO_HIGH));
+        nvic_enable(mac::RADIO_INTR_NUM);
+    } else {
+        nvic_disable(mac::RADIO_INTR_NUM);
+    }
+}
+
+// /**
+//   * @brief  Initialize Link Layer SW low priority interrupt.
+//   * @param  intr_cb: function pointer to assign for the SW low priority ISR routine.
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_SetupSwLowIT(intr_cb: Option<Callback>) {
+    trace!("LINKLAYER_PLAT_SetupSwLowIT: intr_cb={:?}", intr_cb);
+    store_callback(&LOW_ISR_CALLBACK, intr_cb);
+
+    if intr_cb.is_some() {
+        nvic_set_priority(RADIO_SW_LOW_INTR_NUM, pack_priority(mac::RADIO_SW_LOW_INTR_PRIO));
+        nvic_enable(RADIO_SW_LOW_INTR_NUM);
+    } else {
+        nvic_disable(RADIO_SW_LOW_INTR_NUM);
+    }
+}
+
+// /**
+//   * @brief  Trigger the link layer SW low interrupt.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_TriggerSwLowIT(priority: u8) {
+    trace!("LINKLAYER_PLAT_TriggerSwLowIT: priority={}", priority);
+    let active = nvic_get_active(RADIO_SW_LOW_INTR_NUM);
+
+    //   /* Check if a SW low interrupt as already been raised.
+    //    * Nested call far radio low isr are not supported
+    //    **/
+    if !active {
+        let prio = if priority == 0 {
+            //     /* No nested SW low ISR, default behavior */
+            pack_priority(mac::RADIO_SW_LOW_INTR_PRIO)
+        } else {
+            pack_priority(mac::RADIO_INTR_PRIO_LOW)
+        };
+        nvic_set_priority(RADIO_SW_LOW_INTR_NUM, prio);
+    } else if priority != 0 {
+        //     /* Nested call detected */
+        //     /* No change for SW radio low interrupt priority for the moment */
+        //
+        //     if(priority != 0)
+        //     {
+        //       /* At the end of current SW radio low ISR, this pending SW low interrupt
+        //        * will run with RADIO_INTR_PRIO_LOW priority
+        //        **/
+        //       radio_sw_low_isr_is_running_high_prio = 1;
+        //     }
+        RADIO_SW_LOW_ISR_RUNNING_HIGH_PRIO.store(true, Ordering::Release);
+    }
+
+    nvic_set_pending(RADIO_SW_LOW_INTR_NUM);
+}
+
+// /**
+//   * @brief  Enable interrupts.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_EnableIRQ() {
+    trace!("LINKLAYER_PLAT_EnableIRQ");
+    //   irq_counter = max(0,irq_counter-1);
+    //
+    //   if(irq_counter == 0)
+    //   {
+    //     /* When irq_counter reaches 0, restore primask bit */
+    //     __set_PRIMASK(primask_bit);
+    //   }
+    if counter_release(&IRQ_COUNTER) {
+        // When the counter reaches zero, restore prior interrupt state using the captured token.
+        if let Some(token) = CS_RESTORE_STATE.take() {
+            critical_section::release(token);
+        }
+    }
+}
+
+// /**
+//   * @brief  Disable interrupts.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_DisableIRQ() {
+    trace!("LINKLAYER_PLAT_DisableIRQ");
+    //   if(irq_counter == 0)
+    //   {
+    //     /* Save primask bit at first interrupt disablement */
+    //     primask_bit= __get_PRIMASK();
+    //   }
+    //   __disable_irq();
+    //   irq_counter ++;
+    if counter_acquire(&IRQ_COUNTER) {
+        // Capture and disable using critical-section API on first disable.
+        CS_RESTORE_STATE = Some(critical_section::acquire());
+    }
+}
+
+// /**
+//   * @brief  Enable specific interrupt group.
+//   * @param  isr_type: mask for interrupt group to enable.
+//   *         This parameter can be one of the following:
+//   *         @arg LL_HIGH_ISR_ONLY: enable link layer high priority ISR.
+//   *         @arg LL_LOW_ISR_ONLY: enable link layer SW low priority ISR.
+//   *         @arg SYS_LOW_ISR: mask interrupts for all the other system ISR with
+//   *              lower priority that link layer SW low interrupt.
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_EnableSpecificIRQ(isr_type: u8) {
+    trace!("LINKLAYER_PLAT_EnableSpecificIRQ: isr_type={}", isr_type);
+    //   if( (isr_type & LL_HIGH_ISR_ONLY) != 0 )
+    //   {
+    //     prio_high_isr_counter--;
+    //     if(prio_high_isr_counter == 0)
+    //     {
+    //       /* When specific counter for link layer high ISR reaches 0, interrupt is enabled */
+    //       HAL_NVIC_EnableIRQ(RADIO_INTR_NUM);
+    //       /* USER CODE BEGIN LINKLAYER_PLAT_EnableSpecificIRQ_1 */
+    //
+    //       /* USER CODE END LINKLAYER_PLAT_EnableSpecificIRQ_1 */
+    //     }
+    //   }
+    //
+    //   if( (isr_type & LL_LOW_ISR_ONLY) != 0 )
+    //   {
+    //     prio_low_isr_counter--;
+    //     if(prio_low_isr_counter == 0)
+    //     {
+    //       /* When specific counter for link layer SW low ISR reaches 0, interrupt is enabled */
+    //       HAL_NVIC_EnableIRQ(RADIO_SW_LOW_INTR_NUM);
+    //     }
+    //
+    //   }
+    //
+    //   if( (isr_type & SYS_LOW_ISR) != 0 )
+    //   {
+    //     prio_sys_isr_counter--;
+    //     if(prio_sys_isr_counter == 0)
+    //     {
+    //       /* Restore basepri value */
+    //       __set_BASEPRI(local_basepri_value);
+    //     }
+    //   }
+    if (isr_type & link_layer::LL_HIGH_ISR_ONLY as u8) != 0 {
+        if counter_release(&PRIO_HIGH_ISR_COUNTER) {
+            nvic_enable(mac::RADIO_INTR_NUM);
+        }
+    }
+
+    if (isr_type & link_layer::LL_LOW_ISR_ONLY as u8) != 0 {
+        if counter_release(&PRIO_LOW_ISR_COUNTER) {
+            nvic_enable(RADIO_SW_LOW_INTR_NUM);
+        }
+    }
+
+    if (isr_type & link_layer::SYS_LOW_ISR as u8) != 0 {
+        if counter_release(&PRIO_SYS_ISR_COUNTER) {
+            let stored = LOCAL_BASEPRI_VALUE.load(Ordering::Relaxed) as u8;
+            basepri::write(stored);
+        }
+    }
+}
+
+// /**
+//   * @brief  Disable specific interrupt group.
+//   * @param  isr_type: mask for interrupt group to disable.
+//   *         This parameter can be one of the following:
+//   *         @arg LL_HIGH_ISR_ONLY: disable link layer high priority ISR.
+//   *         @arg LL_LOW_ISR_ONLY: disable link layer SW low priority ISR.
+//   *         @arg SYS_LOW_ISR: unmask interrupts for all the other system ISR with
+//   *              lower priority that link layer SW low interrupt.
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_DisableSpecificIRQ(isr_type: u8) {
+    //   if( (isr_type & LL_HIGH_ISR_ONLY) != 0 )
+    //   {
+    //     prio_high_isr_counter++;
+    //     if(prio_high_isr_counter == 1)
+    //     {
+    //       /* USER CODE BEGIN LINKLAYER_PLAT_DisableSpecificIRQ_1 */
+    //
+    //       /* USER CODE END LINKLAYER_PLAT_DisableSpecificIRQ_1 */
+    //       /* When specific counter for link layer high ISR value is 1, interrupt is disabled */
+    //       HAL_NVIC_DisableIRQ(RADIO_INTR_NUM);
+    //     }
+    //   }
+    //
+    //   if( (isr_type & LL_LOW_ISR_ONLY) != 0 )
+    //   {
+    //     prio_low_isr_counter++;
+    //     if(prio_low_isr_counter == 1)
+    //     {
+    //       /* When specific counter for link layer SW low ISR value is 1, interrupt is disabled */
+    //       HAL_NVIC_DisableIRQ(RADIO_SW_LOW_INTR_NUM);
+    //     }
+    //   }
+    //
+    //   if( (isr_type & SYS_LOW_ISR) != 0 )
+    //   {
+    //     prio_sys_isr_counter++;
+    //     if(prio_sys_isr_counter == 1)
+    //     {
+    //       /* Save basepri register value */
+    //       local_basepri_value = __get_BASEPRI();
+    //
+    //       /* Mask all other interrupts with lower priority that link layer SW low ISR */
+    //       __set_BASEPRI_MAX(RADIO_INTR_PRIO_LOW<<4);
+    //     }
+    //   }
+    trace!("LINKLAYER_PLAT_DisableSpecificIRQ: isr_type={}", isr_type);
+    if (isr_type & link_layer::LL_HIGH_ISR_ONLY as u8) != 0 {
+        if counter_acquire(&PRIO_HIGH_ISR_COUNTER) {
+            nvic_disable(mac::RADIO_INTR_NUM);
+        }
+    }
+
+    if (isr_type & link_layer::LL_LOW_ISR_ONLY as u8) != 0 {
+        if counter_acquire(&PRIO_LOW_ISR_COUNTER) {
+            nvic_disable(RADIO_SW_LOW_INTR_NUM);
+        }
+    }
+
+    if (isr_type & link_layer::SYS_LOW_ISR as u8) != 0 {
+        if counter_acquire(&PRIO_SYS_ISR_COUNTER) {
+            let current = basepri::read();
+            LOCAL_BASEPRI_VALUE.store(current.into(), Ordering::Relaxed);
+            set_basepri_max(pack_priority(mac::RADIO_INTR_PRIO_LOW));
+        }
+    }
+}
+
+// /**
+//   * @brief  Enable link layer high priority ISR only.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_EnableRadioIT() {
+    trace!("LINKLAYER_PLAT_EnableRadioIT");
+    nvic_enable(mac::RADIO_INTR_NUM);
+}
+
+// /**
+//   * @brief  Disable link layer high priority ISR only.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_DisableRadioIT() {
+    trace!("LINKLAYER_PLAT_DisableRadioIT");
+    nvic_disable(mac::RADIO_INTR_NUM);
+}
+
+// /**
+//   * @brief  Link Layer notification for radio activity start.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_StartRadioEvt() {
+    trace!("LINKLAYER_PLAT_StartRadioEvt");
+    //   __HAL_RCC_RADIO_CLK_SLEEP_ENABLE();
+    //   NVIC_SetPriority(RADIO_INTR_NUM, RADIO_INTR_PRIO_HIGH);
+    // #if (CFG_SCM_SUPPORTED == 1)
+    //   scm_notifyradiostate(SCM_RADIO_ACTIVE);
+    // #endif /* CFG_SCM_SUPPORTED */
+    nvic_set_priority(mac::RADIO_INTR_NUM, pack_priority(mac::RADIO_INTR_PRIO_HIGH));
+    nvic_enable(mac::RADIO_INTR_NUM);
+}
+
+// /**
+//   * @brief  Link Layer notification for radio activity end.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_StopRadioEvt() {
+    trace!("LINKLAYER_PLAT_StopRadioEvt");
+    // {
+    //   __HAL_RCC_RADIO_CLK_SLEEP_DISABLE();
+    //   NVIC_SetPriority(RADIO_INTR_NUM, RADIO_INTR_PRIO_LOW);
+    // #if (CFG_SCM_SUPPORTED == 1)
+    //   scm_notifyradiostate(SCM_RADIO_NOT_ACTIVE);
+    // #endif /* CFG_SCM_SUPPORTED */
+    nvic_set_priority(mac::RADIO_INTR_NUM, pack_priority(mac::RADIO_INTR_PRIO_LOW));
+}
+
+// /**
+//   * @brief  Link Layer notification for RCO calibration start.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_RCOStartClbr() {
+    trace!("LINKLAYER_PLAT_RCOStartClbr");
+    // #if (CFG_LPM_LEVEL != 0)
+    //   PWR_DisableSleepMode();
+    //   /* Disabling stop mode prevents also from entering in standby */
+    //   UTIL_LPM_SetStopMode(1U << CFG_LPM_LL_HW_RCO_CLBR, UTIL_LPM_DISABLE);
+    // #endif /* (CFG_LPM_LEVEL != 0) */
+    // #if (CFG_SCM_SUPPORTED == 1)
+    //   scm_setsystemclock(SCM_USER_LL_HW_RCO_CLBR, HSE_32MHZ);
+    //   while (LL_PWR_IsActiveFlag_VOS() == 0);
+    // #endif /* (CFG_SCM_SUPPORTED == 1) */
+}
+
+// /**
+//   * @brief  Link Layer notification for RCO calibration end.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_RCOStopClbr() {
+    trace!("LINKLAYER_PLAT_RCOStopClbr");
+    // #if (CFG_LPM_LEVEL != 0)
+    //   PWR_EnableSleepMode();
+    //   UTIL_LPM_SetStopMode(1U << CFG_LPM_LL_HW_RCO_CLBR, UTIL_LPM_ENABLE);
+    // #endif /* (CFG_LPM_LEVEL != 0) */
+    // #if (CFG_SCM_SUPPORTED == 1)
+    //   scm_setsystemclock(SCM_USER_LL_HW_RCO_CLBR, HSE_16MHZ);
+    // #endif /* (CFG_SCM_SUPPORTED == 1) */
+}
+
+// /**
+//   * @brief  Link Layer requests temperature.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_RequestTemperature() {
+    trace!("LINKLAYER_PLAT_RequestTemperature");
+    // #if (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1)
+    //   ll_sys_bg_temperature_measurement();
+    // #endif /* USE_TEMPERATURE_BASED_RADIO_CALIBRATION */
+}
+
+// /**
+//   * @brief  PHY Start calibration.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_PhyStartClbr() {
+    trace!("LINKLAYER_PLAT_PhyStartClbr");
+}
+
+// /**
+//   * @brief  PHY Stop calibration.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_PhyStopClbr() {
+    trace!("LINKLAYER_PLAT_PhyStopClbr");
+}
+
+// /**
+//  * @brief Notify the upper layer that new Link Layer timings have been applied.
+//  * @param evnt_timing[in]: Evnt_timing_t pointer to structure contains drift time , execution time and scheduling time
+//  * @retval None.
+//  */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_SCHLDR_TIMING_UPDATE_NOT(_timings: *const link_layer::Evnt_timing_t) {
+    trace!("LINKLAYER_PLAT_SCHLDR_TIMING_UPDATE_NOT: timings={:?}", _timings);
+}
+
+// /**
+//   * @brief  Get the ST company ID.
+//   * @param  None
+//   * @retval Company ID
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_GetSTCompanyID() -> u32 {
+    // STMicroelectronics Bluetooth SIG Company Identifier
+    // TODO: Pull in update from latest stm32-generated-data
+    0x0030
+}
+
+// /**
+//   * @brief  Get the Unique Device Number (UDN).
+//   * @param  None
+//   * @retval UDN
+//   */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn LINKLAYER_PLAT_GetUDN() -> u32 {
+    // Read the first 32 bits of the STM32 unique 96-bit ID
+    let uid = embassy_stm32::uid::uid();
+    u32::from_le_bytes([uid[0], uid[1], uid[2], uid[3]])
+}
diff --git a/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_cs.rs b/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_cs.rs
new file mode 100644
index 000000000..30103ba27
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_cs.rs
@@ -0,0 +1,77 @@
+use crate::bindings::link_layer::{
+    LINKLAYER_PLAT_DisableIRQ, LINKLAYER_PLAT_DisableSpecificIRQ, LINKLAYER_PLAT_EnableIRQ,
+    LINKLAYER_PLAT_EnableSpecificIRQ, LINKLAYER_PLAT_PhyStartClbr, LINKLAYER_PLAT_PhyStopClbr,
+};
+
+// /**
+//   ******************************************************************************
+//   * @file    ll_sys_cs.c
+//   * @author  MCD Application Team
+//   * @brief   Link Layer IP system interface critical sections management
+//   ******************************************************************************
+//   * @attention
+//   *
+//   * Copyright (c) 2022 STMicroelectronics.
+//   * All rights reserved.
+//   *
+//   * This software is licensed under terms that can be found in the LICENSE file
+//   * in the root directory of this software component.
+//   * If no LICENSE file comes with this software, it is provided AS-IS.
+//   *
+//   ******************************************************************************
+//   */
+//
+// #include "linklayer_plat.h"
+// #include "ll_sys.h"
+// #include <stdint.h>
+//
+/**
+ * @brief  Enable interrupts
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_enable_irq() {
+    LINKLAYER_PLAT_EnableIRQ();
+}
+//
+// /**
+//   * @brief  Disable interrupts
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_disable_irq() {
+    LINKLAYER_PLAT_DisableIRQ();
+}
+//
+// /**
+//   * @brief  Set the Current Interrupt Priority Mask.
+//   *         All interrupts with low priority level will be masked.
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_enable_specific_irq(isr_type: u8) {
+    LINKLAYER_PLAT_EnableSpecificIRQ(isr_type);
+}
+//
+// /**
+//   * @brief  Restore the previous interrupt priority level
+//   * @param  None
+//   * @retval None
+//   */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_disable_specific_irq(isr_type: u8) {
+    LINKLAYER_PLAT_DisableSpecificIRQ(isr_type);
+}
+//
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_phy_start_clbr() {
+    LINKLAYER_PLAT_PhyStartClbr();
+}
+//
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_phy_stop_clbr() {
+    LINKLAYER_PLAT_PhyStopClbr();
+}
diff --git a/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_dp_slp.rs b/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_dp_slp.rs
new file mode 100644
index 000000000..ae8223a5a
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_dp_slp.rs
@@ -0,0 +1,163 @@
+use crate::bindings::link_layer::{
+    _NULL as NULL, DPSLP_STATE_DEEP_SLEEP_DISABLE, DPSLP_STATE_DEEP_SLEEP_ENABLE, LINKLAYER_PLAT_DisableRadioIT,
+    LINKLAYER_PLAT_EnableRadioIT, LL_SYS_DP_SLP_STATE_T_LL_SYS_DP_SLP_DISABLED,
+    LL_SYS_DP_SLP_STATE_T_LL_SYS_DP_SLP_ENABLED, LL_SYS_STATUS_T_LL_SYS_ERROR, LL_SYS_STATUS_T_LL_SYS_OK,
+    OS_TIMER_PRIO_HG_PRIO_TMR, OS_TIMER_STATE_OSTIMERSTOPPED, OS_TIMER_TYPE_OS_TIMER_ONCE, SUCCESS, ble_stat_t,
+    ll_intf_cmn_le_set_dp_slp_mode, ll_sys_dp_slp_state_t, ll_sys_status_t, os_get_tmr_state, os_timer_create,
+    os_timer_id, os_timer_set_prio, os_timer_start, os_timer_stop,
+};
+
+macro_rules! LL_DP_SLP_NO_WAKEUP {
+    () => {
+        !0u32
+    };
+}
+
+macro_rules! LL_INTERNAL_TMR_US_TO_STEPS {
+    ($us:expr) => {
+        ((($us) * 4) / 125)
+    };
+}
+
+// /**
+//   ******************************************************************************
+//   * @file    ll_sys_dp_slp.c
+//   * @author  MCD Application Team
+//   * @brief   Link Layer IP system interface deep sleep management
+//   ******************************************************************************
+//   * @attention
+//   *
+//   * Copyright (c) 2022 STMicroelectronics.
+//   * All rights reserved.
+//   *
+//   * This software is licensed under terms that can be found in the LICENSE file
+//   * in the root directory of this software component.
+//   * If no LICENSE file comes with this software, it is provided AS-IS.
+//   *
+//   ******************************************************************************
+//   */
+//
+// #include "linklayer_plat.h"
+// #include "ll_sys.h"
+// #include "ll_intf_cmn.h"
+//
+// /* Link Layer deep sleep timer */
+static mut RADIO_DP_SLP_TMR_ID: os_timer_id = NULL as *mut _;
+//
+// /* Link Layer deep sleep state */
+static mut LINKLAYER_DP_SLP_STATE: ll_sys_dp_slp_state_t = LL_SYS_DP_SLP_STATE_T_LL_SYS_DP_SLP_DISABLED;
+//
+// /**
+//   * @brief  Initialize resources to handle deep sleep entry/exit
+//   * @param  None
+//   * @retval LL_SYS status
+//   */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_dp_slp_init() -> ll_sys_status_t {
+    let mut return_status: ll_sys_status_t = LL_SYS_STATUS_T_LL_SYS_ERROR;
+
+    /* Create link layer timer for handling IP DEEP SLEEP mode */
+    RADIO_DP_SLP_TMR_ID = os_timer_create(
+        Some(ll_sys_dp_slp_wakeup_evt_clbk),
+        OS_TIMER_TYPE_OS_TIMER_ONCE,
+        NULL as *mut _,
+    );
+
+    /* Set priority of deep sleep timer */
+    os_timer_set_prio(RADIO_DP_SLP_TMR_ID, OS_TIMER_PRIO_HG_PRIO_TMR);
+
+    if RADIO_DP_SLP_TMR_ID != NULL as *mut _ {
+        return_status = LL_SYS_STATUS_T_LL_SYS_OK;
+    }
+
+    return return_status;
+}
+//
+// /**
+//   * @brief  Link Layer deep sleep status getter
+//   * @param  None
+//   * @retval Link Layer deep sleep state
+//   */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_dp_slp_get_state() -> ll_sys_dp_slp_state_t {
+    return LINKLAYER_DP_SLP_STATE;
+}
+//
+// /**
+//   * @brief  The Link Layer IP enters deep sleep mode
+//   * @param  dp_slp_duration    deep sleep duration in us
+//   * @retval LL_SYS status
+//   */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_dp_slp_enter(dp_slp_duration: u32) -> ll_sys_status_t {
+    let cmd_status: ble_stat_t;
+    let os_status: i32;
+    let mut return_status: ll_sys_status_t = LL_SYS_STATUS_T_LL_SYS_ERROR;
+
+    /* Check if deep sleep timer has to be started */
+    if dp_slp_duration < LL_DP_SLP_NO_WAKEUP!() {
+        /* Start deep sleep timer */
+        os_status = os_timer_start(RADIO_DP_SLP_TMR_ID, LL_INTERNAL_TMR_US_TO_STEPS!(dp_slp_duration));
+    } else {
+        /* No timer started */
+        os_status = SUCCESS as i32;
+    }
+
+    if os_status == SUCCESS as i32 {
+        /* Switch Link Layer IP to DEEP SLEEP mode */
+        cmd_status = ll_intf_cmn_le_set_dp_slp_mode(DPSLP_STATE_DEEP_SLEEP_ENABLE as u8);
+        if cmd_status == SUCCESS {
+            LINKLAYER_DP_SLP_STATE = LL_SYS_DP_SLP_STATE_T_LL_SYS_DP_SLP_ENABLED;
+            return_status = LL_SYS_STATUS_T_LL_SYS_OK;
+        }
+    }
+
+    return return_status;
+}
+//
+// /**
+//   * @brief  The Link Layer IP exits deep sleep mode
+//   * @param  None
+//   * @retval LL_SYS status
+//   */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_dp_slp_exit() -> ll_sys_status_t {
+    let cmd_status: ble_stat_t;
+    let mut return_status: ll_sys_status_t = LL_SYS_STATUS_T_LL_SYS_ERROR;
+
+    /* Disable radio interrupt */
+    LINKLAYER_PLAT_DisableRadioIT();
+
+    if LINKLAYER_DP_SLP_STATE == LL_SYS_DP_SLP_STATE_T_LL_SYS_DP_SLP_DISABLED {
+        /* Radio not in sleep mode */
+        return_status = LL_SYS_STATUS_T_LL_SYS_OK;
+    } else {
+        /* Switch Link Layer IP to SLEEP mode (by deactivate DEEP SLEEP mode) */
+        cmd_status = ll_intf_cmn_le_set_dp_slp_mode(DPSLP_STATE_DEEP_SLEEP_DISABLE as u8);
+        if cmd_status == SUCCESS {
+            LINKLAYER_DP_SLP_STATE = LL_SYS_DP_SLP_STATE_T_LL_SYS_DP_SLP_DISABLED;
+            return_status = LL_SYS_STATUS_T_LL_SYS_OK;
+        }
+
+        /* Stop the deep sleep wake-up timer if running */
+        if os_get_tmr_state(RADIO_DP_SLP_TMR_ID) != OS_TIMER_STATE_OSTIMERSTOPPED {
+            os_timer_stop(RADIO_DP_SLP_TMR_ID);
+        }
+    }
+
+    /* Re-enable radio interrupt */
+    LINKLAYER_PLAT_EnableRadioIT();
+
+    return return_status;
+}
+
+/**
+ * @brief  Link Layer deep sleep wake-up timer callback
+ * @param  ptr_arg    pointer passed through the callback
+ * @retval LL_SYS status
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_dp_slp_wakeup_evt_clbk(_ptr_arg: *const ::core::ffi::c_void) {
+    /* Link Layer IP exits from DEEP SLEEP mode */
+    ll_sys_dp_slp_exit();
+}
diff --git a/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_intf.rs b/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_intf.rs
new file mode 100644
index 000000000..0b4b0b37f
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_intf.rs
@@ -0,0 +1,199 @@
+use crate::bindings::link_layer::{
+    Evnt_timing_t, HostStack_Process, LINKLAYER_PLAT_AclkCtrl, LINKLAYER_PLAT_Assert, LINKLAYER_PLAT_ClockInit,
+    LINKLAYER_PLAT_DelayUs, LINKLAYER_PLAT_GetRNG, LINKLAYER_PLAT_RCOStartClbr, LINKLAYER_PLAT_RCOStopClbr,
+    LINKLAYER_PLAT_RequestTemperature, LINKLAYER_PLAT_SCHLDR_TIMING_UPDATE_NOT, LINKLAYER_PLAT_SetupRadioIT,
+    LINKLAYER_PLAT_SetupSwLowIT, LINKLAYER_PLAT_StartRadioEvt, LINKLAYER_PLAT_StopRadioEvt,
+    LINKLAYER_PLAT_TriggerSwLowIT, LINKLAYER_PLAT_WaitHclkRdy, MAX_NUM_CNCRT_STAT_MCHNS, emngr_can_mcu_sleep,
+    emngr_handle_all_events, ll_sys_schedule_bg_process,
+};
+
+// /**
+//   ******************************************************************************
+//   * @file    ll_sys_intf.c
+//   * @author  MCD Application Team
+//   * @brief   Link Layer IP general system interface
+//   ******************************************************************************
+//   * @attention
+//   *
+//   * Copyright (c) 2022 STMicroelectronics.
+//   * All rights reserved.
+//   *
+//   * This software is licensed under terms that can be found in the LICENSE file
+//   * in the root directory of this software component.
+//   * If no LICENSE file comes with this software, it is provided AS-IS.
+//   *
+//   ******************************************************************************
+//   */
+// #include <stdint.h>
+//
+// #include "ll_sys.h"
+// #include "linklayer_plat.h"
+// #include "event_manager.h"
+// #include "ll_intf.h"
+//
+/**
+ * @brief  Initialize the Link Layer SoC dependencies
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_init() {
+    LINKLAYER_PLAT_ClockInit();
+}
+//
+/**
+ * @brief  Blocking delay in us
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_delay_us(delay: u32) {
+    LINKLAYER_PLAT_DelayUs(delay);
+}
+
+/**
+ * @brief  Assert checking
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_assert(condition: u8) {
+    LINKLAYER_PLAT_Assert(condition);
+}
+
+/**
+ * @brief  Radio active clock management
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_radio_ack_ctrl(enable: u8) {
+    LINKLAYER_PLAT_AclkCtrl(enable);
+}
+
+/**
+ * @brief  Link Layer waits for radio bus clock ready
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_radio_wait_for_busclkrdy() {
+    LINKLAYER_PLAT_WaitHclkRdy();
+}
+
+/**
+ * @brief  Get RNG number for the Link Layer IP
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_get_rng(ptr_rnd: *mut u8, len: u32) {
+    LINKLAYER_PLAT_GetRNG(ptr_rnd, len);
+}
+
+/**
+ * @brief  Initialize the main radio interrupt
+ * @param  intr_cb    radio interrupt callback to link with the radio IRQ
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_setup_radio_intr(intr_cb: ::core::option::Option<unsafe extern "C" fn()>) {
+    LINKLAYER_PLAT_SetupRadioIT(intr_cb);
+}
+
+/**
+ * @brief  Initialize the radio SW low interrupt
+ * @param  intr_cb    radio SW low interrupt interrupt callback to link
+ *                    with the defined interrupt vector
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_setup_radio_sw_low_intr(intr_cb: ::core::option::Option<unsafe extern "C" fn()>) {
+    LINKLAYER_PLAT_SetupSwLowIT(intr_cb);
+}
+
+/**
+ * @brief  Trigger the radio SW low interrupt
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_radio_sw_low_intr_trigger(priority: u8) {
+    LINKLAYER_PLAT_TriggerSwLowIT(priority);
+}
+
+/**
+ * @brief  Link Layer radio activity event notification
+ * @param  start      start/end of radio event
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_radio_evt_not(start: u8) {
+    if start != 0 {
+        LINKLAYER_PLAT_StartRadioEvt();
+    } else {
+        LINKLAYER_PLAT_StopRadioEvt();
+    }
+}
+
+/**
+ * @brief  Link Layer RCO calibration notification
+ * @param  start      start/end of RCO calibration
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_rco_clbr_not(start: u8) {
+    if start != 0 {
+        LINKLAYER_PLAT_RCOStartClbr();
+    } else {
+        LINKLAYER_PLAT_RCOStopClbr();
+    }
+}
+
+/**
+ * @brief  Link Layer temperature request
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_request_temperature() {
+    LINKLAYER_PLAT_RequestTemperature();
+}
+
+/**
+ * @brief  Link Layer background task pcoessing procedure
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_bg_process() {
+    if emngr_can_mcu_sleep() == 0 {
+        emngr_handle_all_events();
+
+        HostStack_Process();
+    }
+
+    if emngr_can_mcu_sleep() == 0 {
+        ll_sys_schedule_bg_process();
+    }
+}
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_schldr_timing_update_not(p_evnt_timing: *mut Evnt_timing_t) {
+    LINKLAYER_PLAT_SCHLDR_TIMING_UPDATE_NOT(p_evnt_timing);
+}
+
+/**
+ * @brief  Get the number of concurrent state machines for the Link Layer
+ * @param  None
+ * @retval Supported number of concurrent state machines
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_get_concurrent_state_machines_num() -> u8 {
+    return MAX_NUM_CNCRT_STAT_MCHNS as u8;
+}
+//
+// __WEAK void HostStack_Process(void)
+// {
+//
+// }
diff --git a/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_startup.rs b/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_startup.rs
new file mode 100644
index 000000000..074aaeafe
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/ll_sys/ll_sys_startup.rs
@@ -0,0 +1,125 @@
+use crate::bindings::link_layer::{
+    _NULL as NULL, LL_SYS_STATUS_T_LL_SYS_OK, ble_buff_hdr_p, hci_dispatch_tbl, hci_get_dis_tbl, hst_cbk, ll_intf_init,
+    ll_intf_rgstr_hst_cbk, ll_intf_rgstr_hst_cbk_ll_queue_full, ll_sys_assert, ll_sys_bg_process_init,
+    ll_sys_config_params, ll_sys_dp_slp_init, ll_sys_status_t,
+};
+use crate::bindings::mac::ST_MAC_preInit;
+// /**
+//   ******************************************************************************
+//   * @file    ll_sys_startup.c
+//   * @author  MCD Application Team
+//   * @brief   Link Layer IP system interface startup module
+//   ******************************************************************************
+//   * @attention
+//   *
+//   * Copyright (c) 2022 STMicroelectronics.
+//   * All rights reserved.
+//   *
+//   * This software is licensed under terms that can be found in the LICENSE file
+//   * in the root directory of this software component.
+//   * If no LICENSE file comes with this software, it is provided AS-IS.
+//   *
+//   ******************************************************************************
+//   */
+//
+// #include "ll_fw_config.h"
+// #include "ll_sys.h"
+// #include "ll_intf.h"
+// #include "ll_sys_startup.h"
+// #include "common_types.h"
+// #if defined(MAC)
+// #ifndef OPENTHREAD_CONFIG_FILE
+// /* Projects with MAC Layer (i.e. 15.4 except Thread) */
+// #include "st_mac_802_15_4_sap.h"
+// #endif /* OPENTHREAD_CONFIG_FILE */
+// #endif /* MAC */
+//
+
+#[allow(dead_code)]
+/**
+ * @brief  Missed HCI event flag
+ */
+static mut MISSED_HCI_EVENT_FLAG: u8 = 0;
+
+// static void ll_sys_dependencies_init(void);
+// #if SUPPORT_BLE
+
+#[cfg(feature = "wba_ble")]
+#[allow(dead_code)]
+unsafe extern "C" fn ll_sys_event_missed_cb(_ptr_evnt_hdr: ble_buff_hdr_p) {
+    MISSED_HCI_EVENT_FLAG = 1;
+}
+
+#[cfg(feature = "wba_ble")]
+/**
+ * @brief  Initialize the Link Layer IP BLE controller
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_ble_cntrl_init(host_callback: hst_cbk) {
+    let p_hci_dis_tbl: *const hci_dispatch_tbl = NULL as *const _;
+
+    hci_get_dis_tbl(&p_hci_dis_tbl as *const *const _ as *mut *const _);
+
+    ll_intf_init(p_hci_dis_tbl);
+
+    ll_intf_rgstr_hst_cbk(host_callback);
+
+    ll_intf_rgstr_hst_cbk_ll_queue_full(Some(ll_sys_event_missed_cb));
+
+    ll_sys_dependencies_init();
+}
+// #endif /* SUPPORT_BLE */
+// #if defined(MAC)
+// #ifndef OPENTHREAD_CONFIG_FILE
+#[cfg(feature = "wba_mac")]
+/**
+ * @brief  Initialize the Link Layer IP 802.15.4 MAC controller
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_mac_cntrl_init() {
+    ST_MAC_preInit();
+    ll_sys_dependencies_init();
+}
+// #endif /* OPENTHREAD_CONFIG_FILE */
+// #endif /* MAC */
+/**
+ * @brief  Start the Link Layer IP in OpenThread configuration
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_thread_init() {
+    ll_sys_dependencies_init();
+}
+
+/**
+ * @brief  Initialize the Link Layer resources for startup.
+ *         This includes: - Deep Sleep feature resources
+ *                        - Link Layer background task
+ * @param  None
+ * @retval None
+ */
+unsafe fn ll_sys_dependencies_init() {
+    static mut IS_LL_INITIALIZED: u8 = 0;
+    let dp_slp_status: ll_sys_status_t;
+
+    /* Ensure Link Layer resources are created only once */
+    if IS_LL_INITIALIZED == 1 {
+        return;
+    }
+    IS_LL_INITIALIZED = 1;
+
+    /* Deep sleep feature initialization */
+    dp_slp_status = ll_sys_dp_slp_init();
+    ll_sys_assert((dp_slp_status == LL_SYS_STATUS_T_LL_SYS_OK) as u8);
+
+    /* Background task initialization */
+    ll_sys_bg_process_init();
+
+    /* Link Layer user parameters application */
+    ll_sys_config_params();
+}
diff --git a/embassy-stm32-wpan/src/wba/ll_sys/ll_version.rs b/embassy-stm32-wpan/src/wba/ll_sys/ll_version.rs
new file mode 100644
index 000000000..a42e8cc67
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/ll_sys/ll_version.rs
@@ -0,0 +1,115 @@
+use crate::bindings::link_layer::{
+    LL_SYS_BRIEF_VERSION_MAJOR, LL_SYS_BRIEF_VERSION_MAJOR_MASK, LL_SYS_BRIEF_VERSION_MAJOR_POS,
+    LL_SYS_BRIEF_VERSION_MINOR, LL_SYS_BRIEF_VERSION_MINOR_MASK, LL_SYS_BRIEF_VERSION_MINOR_POS,
+    LL_SYS_BRIEF_VERSION_PATCH, LL_SYS_BRIEF_VERSION_PATCH_MASK, LL_SYS_BRIEF_VERSION_PATCH_POS,
+};
+
+// /**
+//   ******************************************************************************
+//   * @file    ll_version.c
+//   * @author  MCD Application Team
+//   * @brief   Link Layer version interface
+//   ******************************************************************************
+//   * @attention
+//   *
+//   * Copyright (c) 2025 STMicroelectronics.
+//   * All rights reserved.
+//   *
+//   * This software is licensed under terms that can be found in the LICENSE file
+//   * in the root directory of this software component.
+//   * If no LICENSE file comes with this software, it is provided AS-IS.
+//   *
+//   ******************************************************************************
+//   */
+//
+// /* Includes ------------------------------------------------------------------*/
+// /* Integer types */
+// #include <stdint.h>
+//
+// /* Own header file  */
+// #include "ll_version.h"
+//
+// /* Temporary header file for version tracking */
+// #include "ll_tmp_version.h"
+//
+// /* Private defines -----------------------------------------------------------*/
+// /**
+//  * @brief Magic keyword to identify the system version when debugging
+//  */
+//  #define LL_SYS_MAGIC_KEYWORD  0xDEADBEEF
+
+const LL_SYS_MAGIC_KEYWORD: u32 = 0xDEADBEEF;
+
+//
+// /* Private macros ------------------------------------------------------------*/
+// /* Macro to set a specific field value */
+// #define LL_SYS_SET_FIELD_VALUE(value, mask, pos) \
+//   (((value) << (pos)) & (mask))
+
+macro_rules! LL_SYS_SET_FIELD_VALUE {
+    ($value:expr, $mask:expr, $pos:expr) => {
+        ((($value) << ($pos)) & ($mask))
+    };
+}
+
+//
+// /* Private typedef -----------------------------------------------------------*/
+// /**
+//   * @brief Link Layer system version structure definition
+//   */
+#[allow(non_camel_case_types)]
+struct ll_sys_version_t {
+    #[allow(unused)]
+    magic_key_word: u32, /* Magic key word to identify the system version */
+    version: u32, /* System version - i.e.: short hash of latest commit */
+}
+//
+// /* Private variables ---------------------------------------------------------*/
+// /**
+//  * @brief Link Layer brief version definition
+//  */
+const LL_SYS_BRIEF_VERSION: u8 = LL_SYS_SET_FIELD_VALUE!(
+    LL_SYS_BRIEF_VERSION_MAJOR as u8,
+    LL_SYS_BRIEF_VERSION_MAJOR_MASK as u8,
+    LL_SYS_BRIEF_VERSION_MAJOR_POS as u8
+) | LL_SYS_SET_FIELD_VALUE!(
+    LL_SYS_BRIEF_VERSION_MINOR as u8,
+    LL_SYS_BRIEF_VERSION_MINOR_MASK as u8,
+    LL_SYS_BRIEF_VERSION_MINOR_POS as u8
+) | LL_SYS_SET_FIELD_VALUE!(
+    LL_SYS_BRIEF_VERSION_PATCH as u8,
+    LL_SYS_BRIEF_VERSION_PATCH_MASK as u8,
+    LL_SYS_BRIEF_VERSION_PATCH_POS as u8
+);
+//
+// /**
+//  * @brief Link Layer system version structure definition
+//  */
+const LL_SYS_SYSTEM_VERSION: ll_sys_version_t = ll_sys_version_t {
+    magic_key_word: LL_SYS_MAGIC_KEYWORD,
+    version: 0, // LL_SYS_SYSTEM_VERSION,
+};
+//
+// /**
+//  * @brief Link Layer source version structure definition
+//  */
+const LL_SYS_SOURCE_VERSION: ll_sys_version_t = ll_sys_version_t {
+    magic_key_word: LL_SYS_MAGIC_KEYWORD,
+    version: 0, // LL_SYS_SOURCE_VERSION
+};
+//
+// /* Functions Definition ------------------------------------------------------*/
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_get_brief_fw_version() -> u8 {
+    return LL_SYS_BRIEF_VERSION;
+}
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_get_system_fw_version() -> u32 {
+    return LL_SYS_SYSTEM_VERSION.version;
+}
+
+#[unsafe(no_mangle)]
+unsafe extern "C" fn ll_sys_get_source_fw_version() -> u32 {
+    return LL_SYS_SOURCE_VERSION.version;
+}
diff --git a/embassy-stm32-wpan/src/wba/ll_sys/mod.rs b/embassy-stm32-wpan/src/wba/ll_sys/mod.rs
new file mode 100644
index 000000000..45e196c96
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/ll_sys/mod.rs
@@ -0,0 +1,5 @@
+mod ll_sys_cs;
+mod ll_sys_dp_slp;
+mod ll_sys_intf;
+mod ll_sys_startup;
+mod ll_version;
diff --git a/embassy-stm32-wpan/src/wba/ll_sys_if.rs b/embassy-stm32-wpan/src/wba/ll_sys_if.rs
new file mode 100644
index 000000000..7218b69c4
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/ll_sys_if.rs
@@ -0,0 +1,416 @@
+#![cfg(feature = "wba")]
+// /* USER CODE BEGIN Header */
+// /**
+//   ******************************************************************************
+//   * @file    ll_sys_if.c
+//   * @author  MCD Application Team
+//   * @brief   Source file for initiating system
+//   ******************************************************************************
+//   * @attention
+//   *
+//   * Copyright (c) 2022 STMicroelectronics.
+//   * All rights reserved.
+//   *
+//   * This software is licensed under terms that can be found in the LICENSE file
+//   * in the root directory of this software component.
+//   * If no LICENSE file comes with this software, it is provided AS-IS.
+//   *
+//   ******************************************************************************
+//   */
+// /* USER CODE END Header */
+//
+// #include "main.h"
+// #include "app_common.h"
+// #include "app_conf.h"
+// #include "log_module.h"
+// #include "ll_intf_cmn.h"
+// #include "ll_sys.h"
+// #include "ll_sys_if.h"
+// #include "stm32_rtos.h"
+// #include "utilities_common.h"
+// #if (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1)
+// #include "temp_measurement.h"
+// #endif /* (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1) */
+// #if (CFG_LPM_STANDBY_SUPPORTED == 0)
+// extern void profile_reset(void);
+// #endif
+// /* Private defines -----------------------------------------------------------*/
+// /* Radio event scheduling method - must be set at 1 */
+// #define USE_RADIO_LOW_ISR                   (1)
+// #define NEXT_EVENT_SCHEDULING_FROM_ISR      (1)
+//
+// /* USER CODE BEGIN PD */
+//
+// /* USER CODE END PD */
+//
+// /* Private macros ------------------------------------------------------------*/
+// /* USER CODE BEGIN PM */
+//
+// /* USER CODE END PM */
+//
+// /* Private constants ---------------------------------------------------------*/
+// /* USER CODE BEGIN PC */
+//
+// /* USER CODE END PC */
+//
+// /* Private variables ---------------------------------------------------------*/
+// /* USER CODE BEGIN PV */
+//
+// /* USER CODE END PV */
+//
+// /* Global variables ----------------------------------------------------------*/
+//
+// /* USER CODE BEGIN GV */
+//
+// /* USER CODE END GV */
+//
+// /* Private functions prototypes-----------------------------------------------*/
+// #if (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1)
+// static void ll_sys_bg_temperature_measurement_init(void);
+// #endif /* USE_TEMPERATURE_BASED_RADIO_CALIBRATION */
+// static void ll_sys_sleep_clock_source_selection(void);
+// static uint8_t ll_sys_BLE_sleep_clock_accuracy_selection(void);
+// void ll_sys_reset(void);
+//
+// /* USER CODE BEGIN PFP */
+//
+// /* USER CODE END PFP */
+//
+// /* External variables --------------------------------------------------------*/
+//
+// /* USER CODE BEGIN EV */
+//
+// /* USER CODE END EV */
+//
+// /* Functions Definition ------------------------------------------------------*/
+//
+// /**
+//   * @brief  Link Layer background process initialization
+//   * @param  None
+//   * @retval None
+//   */
+// void ll_sys_bg_process_init(void)
+// {
+//   /* Register Link Layer task */
+//   UTIL_SEQ_RegTask(1U << CFG_TASK_LINK_LAYER, UTIL_SEQ_RFU, ll_sys_bg_process);
+// }
+//
+// /**
+//   * @brief  Link Layer background process next iteration scheduling
+//   * @param  None
+//   * @retval None
+//   */
+// void ll_sys_schedule_bg_process(void)
+// {
+//   UTIL_SEQ_SetTask(1U << CFG_TASK_LINK_LAYER, TASK_PRIO_LINK_LAYER);
+// }
+//
+// /**
+//   * @brief  Link Layer background process next iteration scheduling from ISR
+//   * @param  None
+//   * @retval None
+//   */
+// void ll_sys_schedule_bg_process_isr(void)
+// {
+//   UTIL_SEQ_SetTask(1U << CFG_TASK_LINK_LAYER, TASK_PRIO_LINK_LAYER);
+// }
+//
+// /**
+//   * @brief  Link Layer configuration phase before application startup.
+//   * @param  None
+//   * @retval None
+//   */
+// void ll_sys_config_params(void)
+// {
+// /* USER CODE BEGIN ll_sys_config_params_0 */
+//
+// /* USER CODE END ll_sys_config_params_0 */
+//
+//   /* Configure link layer behavior for low ISR use and next event scheduling method:
+//    * - SW low ISR is used.
+//    * - Next event is scheduled from ISR.
+//    */
+//   ll_intf_cmn_config_ll_ctx_params(USE_RADIO_LOW_ISR, NEXT_EVENT_SCHEDULING_FROM_ISR);
+//   /* Apply the selected link layer sleep timer source */
+//   ll_sys_sleep_clock_source_selection();
+//
+// /* USER CODE BEGIN ll_sys_config_params_1 */
+//
+// /* USER CODE END ll_sys_config_params_1 */
+//
+// #if (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1)
+//   /* Initialize link layer temperature measurement background task */
+//   ll_sys_bg_temperature_measurement_init();
+//
+//   /* Link layer IP uses temperature based calibration instead of periodic one */
+//   ll_intf_cmn_set_temperature_sensor_state();
+// #endif /* USE_TEMPERATURE_BASED_RADIO_CALIBRATION */
+//
+//   /* Link Layer power table */
+//   ll_intf_cmn_select_tx_power_table(CFG_RF_TX_POWER_TABLE_ID);
+//
+// #if (USE_CTE_DEGRADATION == 1u)
+//   /* Apply CTE degradation */
+//   ll_sys_apply_cte_settings ();
+// #endif /* (USE_CTE_DEGRADATION == 1u) */
+//
+// /* USER CODE BEGIN ll_sys_config_params_2 */
+//
+// /* USER CODE END ll_sys_config_params_2 */
+// }
+//
+// #if (USE_TEMPERATURE_BASED_RADIO_CALIBRATION == 1)
+//
+// /**
+//   * @brief  Link Layer temperature request background process initialization
+//   * @param  None
+//   * @retval None
+//   */
+// void ll_sys_bg_temperature_measurement_init(void)
+// {
+//   /* Register Temperature Measurement task */
+//   UTIL_SEQ_RegTask(1U << CFG_TASK_TEMP_MEAS, UTIL_SEQ_RFU, TEMPMEAS_RequestTemperatureMeasurement);
+// }
+//
+// /**
+//   * @brief  Request backroud task processing for temperature measurement
+//   * @param  None
+//   * @retval None
+//   */
+// void ll_sys_bg_temperature_measurement(void)
+// {
+//   static uint8_t initial_temperature_acquisition = 0;
+//
+//   if(initial_temperature_acquisition == 0)
+//   {
+//     TEMPMEAS_RequestTemperatureMeasurement();
+//     initial_temperature_acquisition = 1;
+//   }
+//   else
+//   {
+//     UTIL_SEQ_SetTask(1U << CFG_TASK_TEMP_MEAS, CFG_SEQ_PRIO_0);
+//   }
+// }
+//
+// #endif /* USE_TEMPERATURE_BASED_RADIO_CALIBRATION */
+//
+// uint8_t ll_sys_BLE_sleep_clock_accuracy_selection(void)
+// {
+//   uint8_t BLE_sleep_clock_accuracy = 0;
+// #if (CFG_RADIO_LSE_SLEEP_TIMER_CUSTOM_SCA_RANGE == 0)
+//   uint32_t RevID = LL_DBGMCU_GetRevisionID();
+// #endif
+//   uint32_t linklayer_slp_clk_src = LL_RCC_RADIO_GetSleepTimerClockSource();
+//
+//   if(linklayer_slp_clk_src == LL_RCC_RADIOSLEEPSOURCE_LSE)
+//   {
+//     /* LSE selected as Link Layer sleep clock source.
+//        Sleep clock accuracy is different regarding the WBA device ID and revision
+//      */
+// #if (CFG_RADIO_LSE_SLEEP_TIMER_CUSTOM_SCA_RANGE == 0)
+// #if defined(STM32WBA52xx) || defined(STM32WBA54xx) || defined(STM32WBA55xx)
+//     if(RevID == REV_ID_A)
+//     {
+//       BLE_sleep_clock_accuracy = STM32WBA5x_REV_ID_A_SCA_RANGE;
+//     }
+//     else if(RevID == REV_ID_B)
+//     {
+//       BLE_sleep_clock_accuracy = STM32WBA5x_REV_ID_B_SCA_RANGE;
+//     }
+//     else
+//     {
+//       /* Revision ID not supported, default value of 500ppm applied */
+//       BLE_sleep_clock_accuracy = STM32WBA5x_DEFAULT_SCA_RANGE;
+//     }
+// #elif defined(STM32WBA65xx)
+//     BLE_sleep_clock_accuracy = STM32WBA6x_SCA_RANGE;
+//     UNUSED(RevID);
+// #else
+//     UNUSED(RevID);
+// #endif /* defined(STM32WBA52xx) || defined(STM32WBA54xx) || defined(STM32WBA55xx) */
+// #else /* CFG_RADIO_LSE_SLEEP_TIMER_CUSTOM_SCA_RANGE */
+//     BLE_sleep_clock_accuracy = CFG_RADIO_LSE_SLEEP_TIMER_CUSTOM_SCA_RANGE;
+// #endif /* CFG_RADIO_LSE_SLEEP_TIMER_CUSTOM_SCA_RANGE */
+//   }
+//   else
+//   {
+//     /* LSE is not the Link Layer sleep clock source, sleep clock accurcay default value is 500 ppm */
+//     BLE_sleep_clock_accuracy = STM32WBA5x_DEFAULT_SCA_RANGE;
+//   }
+//
+//   return BLE_sleep_clock_accuracy;
+// }
+//
+// void ll_sys_sleep_clock_source_selection(void)
+// {
+//   uint16_t freq_value = 0;
+//   uint32_t linklayer_slp_clk_src = LL_RCC_RADIOSLEEPSOURCE_NONE;
+//
+//   linklayer_slp_clk_src = LL_RCC_RADIO_GetSleepTimerClockSource();
+//   switch(linklayer_slp_clk_src)
+//   {
+//     case LL_RCC_RADIOSLEEPSOURCE_LSE:
+//       linklayer_slp_clk_src = RTC_SLPTMR;
+//       break;
+//
+//     case LL_RCC_RADIOSLEEPSOURCE_LSI:
+//       linklayer_slp_clk_src = RCO_SLPTMR;
+//       break;
+//
+//     case LL_RCC_RADIOSLEEPSOURCE_HSE_DIV1000:
+//       linklayer_slp_clk_src = CRYSTAL_OSCILLATOR_SLPTMR;
+//       break;
+//
+//     case LL_RCC_RADIOSLEEPSOURCE_NONE:
+//       /* No Link Layer sleep clock source selected */
+//       assert_param(0);
+//       break;
+//   }
+//   ll_intf_cmn_le_select_slp_clk_src((uint8_t)linklayer_slp_clk_src, &freq_value);
+// }
+//
+// void ll_sys_reset(void)
+// {
+//   uint8_t bsca = 0;
+//   /* Link layer timings */
+//   uint8_t drift_time = DRIFT_TIME_DEFAULT;
+//   uint8_t exec_time = EXEC_TIME_DEFAULT;
+//
+// /* USER CODE BEGIN ll_sys_reset_0 */
+//
+// /* USER CODE END ll_sys_reset_0 */
+//
+//   /* Apply the selected link layer sleep timer source */
+//   ll_sys_sleep_clock_source_selection();
+//
+//   /* Configure the link layer sleep clock accuracy */
+//   bsca = ll_sys_BLE_sleep_clock_accuracy_selection();
+//   ll_intf_le_set_sleep_clock_accuracy(bsca);
+//
+//   /* Update link layer timings depending on selected configuration */
+//   if(LL_RCC_RADIO_GetSleepTimerClockSource() == LL_RCC_RADIOSLEEPSOURCE_LSI)
+//   {
+//     drift_time += DRIFT_TIME_EXTRA_LSI2;
+//     exec_time += EXEC_TIME_EXTRA_LSI2;
+//   }
+//   else
+//   {
+// #if defined(__GNUC__) && defined(DEBUG)
+//     drift_time += DRIFT_TIME_EXTRA_GCC_DEBUG;
+//     exec_time += EXEC_TIME_EXTRA_GCC_DEBUG;
+// #endif
+//   }
+//
+//   /* USER CODE BEGIN ll_sys_reset_1 */
+//
+//   /* USER CODE END ll_sys_reset_1 */
+//
+//   if((drift_time != DRIFT_TIME_DEFAULT) || (exec_time != EXEC_TIME_DEFAULT))
+//   {
+//     ll_sys_config_BLE_schldr_timings(drift_time, exec_time);
+//   }
+//   /* USER CODE BEGIN ll_sys_reset_2 */
+//
+//   /* USER CODE END ll_sys_reset_2 */
+// }
+// #if defined(STM32WBA52xx) || defined(STM32WBA54xx) || defined(STM32WBA55xx) || defined(STM32WBA65xx)
+// void ll_sys_apply_cte_settings(void)
+// {
+//   ll_intf_apply_cte_degrad_change();
+// }
+// #endif /* defined(STM32WBA52xx) || defined(STM32WBA54xx) || defined(STM32WBA55xx) || defined(STM32WBA65xx) */
+//
+// #if (CFG_LPM_STANDBY_SUPPORTED == 0)
+// void ll_sys_get_ble_profile_statistics(uint32_t* exec_time, uint32_t* drift_time, uint32_t* average_drift_time, uint8_t reset)
+// {
+//   if (reset != 0U)
+//   {
+//     profile_reset();
+//   }
+//   ll_intf_get_profile_statistics(exec_time, drift_time, average_drift_time);
+// }
+// #endif
+//
+use super::bindings::{link_layer, mac};
+use super::util_seq;
+
+const UTIL_SEQ_RFU: u32 = 0;
+const TASK_LINK_LAYER_MASK: u32 = 1 << mac::CFG_TASK_ID_T_CFG_TASK_LINK_LAYER;
+const TASK_PRIO_LINK_LAYER: u32 = mac::CFG_SEQ_PRIO_ID_T_CFG_SEQ_PRIO_0 as u32;
+
+/**
+ * @brief  Link Layer background process initialization
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn ll_sys_bg_process_init() {
+    util_seq::UTIL_SEQ_RegTask(TASK_LINK_LAYER_MASK, UTIL_SEQ_RFU, Some(link_layer::ll_sys_bg_process));
+}
+
+/**
+ * @brief  Link Layer background process next iteration scheduling
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn ll_sys_schedule_bg_process() {
+    util_seq::UTIL_SEQ_SetTask(TASK_LINK_LAYER_MASK, TASK_PRIO_LINK_LAYER);
+}
+
+/**
+ * @brief  Link Layer background process next iteration scheduling from ISR
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn ll_sys_schedule_bg_process_isr() {
+    util_seq::UTIL_SEQ_SetTask(TASK_LINK_LAYER_MASK, TASK_PRIO_LINK_LAYER);
+}
+
+/**
+ * @brief  Link Layer configuration phase before application startup.
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn ll_sys_config_params() {
+    let allow_low_isr = mac::USE_RADIO_LOW_ISR as u8;
+    let run_from_isr = mac::NEXT_EVENT_SCHEDULING_FROM_ISR as u8;
+    let _ = link_layer::ll_intf_cmn_config_ll_ctx_params(allow_low_isr, run_from_isr);
+
+    ll_sys_sleep_clock_source_selection();
+    let _ = link_layer::ll_intf_cmn_select_tx_power_table(mac::CFG_RF_TX_POWER_TABLE_ID as u8);
+}
+
+/**
+ * @brief  Reset Link Layer timing parameters to their default configuration.
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn ll_sys_reset() {
+    ll_sys_sleep_clock_source_selection();
+
+    let sleep_accuracy = ll_sys_BLE_sleep_clock_accuracy_selection();
+    let _ = link_layer::ll_intf_le_set_sleep_clock_accuracy(sleep_accuracy);
+}
+
+/// Select the sleep-clock source used by the Link Layer.
+/// Defaults to the crystal oscillator when no explicit configuration is available.
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn ll_sys_sleep_clock_source_selection() {
+    let mut frequency: u16 = 0;
+    let _ = link_layer::ll_intf_cmn_le_select_slp_clk_src(
+        link_layer::_SLPTMR_SRC_TYPE_E_CRYSTAL_OSCILLATOR_SLPTMR as u8,
+        &mut frequency as *mut u16,
+    );
+}
+
+/// Determine the BLE sleep-clock accuracy used by the stack.
+/// Returns zero when board-specific calibration data is unavailable.
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn ll_sys_BLE_sleep_clock_accuracy_selection() -> u8 {
+    // TODO: derive the board-specific sleep clock accuracy once calibration data is available.
+    0
+}
diff --git a/embassy-stm32-wpan/src/wba/mac_sys_if.rs b/embassy-stm32-wpan/src/wba/mac_sys_if.rs
new file mode 100644
index 000000000..273399a19
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/mac_sys_if.rs
@@ -0,0 +1,188 @@
+#![cfg(feature = "wba")]
+#![allow(non_snake_case)]
+
+//
+// /* USER CODE BEGIN Header */
+// /**
+//   ******************************************************************************
+//   * @file    mac_sys_if.c
+//   * @author  MCD Application Team
+//   * @brief   Source file for using MAC Layer with a RTOS
+//   ******************************************************************************
+//   * @attention
+//   *
+//   * Copyright (c) 2025 STMicroelectronics.
+//   * All rights reserved.
+//   *
+//   * This software is licensed under terms that can be found in the LICENSE file
+//   * in the root directory of this software component.
+//   * If no LICENSE file comes with this software, it is provided AS-IS.
+//   *
+//   ******************************************************************************
+//   */
+// /* USER CODE END Header */
+//
+// #include "main.h"
+// #include "app_common.h"
+// #include "app_conf.h"
+// #include "log_module.h"
+// #include "stm32_rtos.h"
+// #include "st_mac_802_15_4_sys.h"
+//
+// extern void mac_baremetal_run(void);
+//
+// /* Private defines -----------------------------------------------------------*/
+// /* USER CODE BEGIN PD */
+//
+// /* USER CODE END PD */
+//
+// /* Private macros ------------------------------------------------------------*/
+// /* USER CODE BEGIN PM */
+//
+// /* USER CODE END PM */
+//
+// /* Private variables ---------------------------------------------------------*/
+// /* USER CODE BEGIN PV */
+//
+// /* USER CODE END PV */
+//
+// /* Global variables ----------------------------------------------------------*/
+// /* USER CODE BEGIN GV */
+//
+// /* USER CODE END GV */
+//
+// /* Functions Definition ------------------------------------------------------*/
+//
+// /**
+//   * @brief  Mac Layer Initialisation
+//   * @param  None
+//   * @retval None
+//   */
+// void MacSys_Init(void)
+// {
+//   /* Register tasks */
+//   UTIL_SEQ_RegTask( TASK_MAC_LAYER, UTIL_SEQ_RFU, mac_baremetal_run);
+// }
+//
+// /**
+//   * @brief  Mac Layer Resume
+//   * @param  None
+//   * @retval None
+//   */
+// void MacSys_Resume(void)
+// {
+//   UTIL_SEQ_ResumeTask( TASK_MAC_LAYER );
+// }
+//
+// /**
+//   * @brief  MAC Layer set Task.
+//   * @param  None
+//   * @retval None
+//   */
+// void MacSys_SemaphoreSet(void)
+// {
+//   UTIL_SEQ_SetTask( TASK_MAC_LAYER, TASK_PRIO_MAC_LAYER );
+// }
+//
+// /**
+//   * @brief  MAC Layer Task wait.
+//   * @param  None
+//   * @retval None
+//   */
+// void MacSys_SemaphoreWait( void )
+// {
+//   /* Not used */
+// }
+//
+// /**
+//   * @brief  MAC Layer set Event.
+//   * @param  None
+//   * @retval None
+//   */
+// void MacSys_EventSet( void )
+// {
+//   UTIL_SEQ_SetEvt( EVENT_MAC_LAYER );
+// }
+//
+// /**
+//   * @brief  MAC Layer wait Event.
+//   * @param  None
+//   * @retval None
+//   */
+// void MacSys_EventWait( void )
+// {
+//   UTIL_SEQ_WaitEvt( EVENT_MAC_LAYER );
+// }
+//
+
+use super::util_seq;
+use crate::bindings::mac;
+
+/// Placeholder value used by the original ST middleware when registering tasks.
+const UTIL_SEQ_RFU: u32 = 0;
+
+/// Bit mask identifying the MAC layer task within the sequencer.
+const TASK_MAC_LAYER_MASK: u32 = 1 << mac::CFG_TASK_ID_T_CFG_TASK_MAC_LAYER;
+
+/// Sequencer priority assigned to the MAC layer task.
+const TASK_PRIO_MAC_LAYER: u32 = mac::CFG_SEQ_PRIO_ID_T_CFG_SEQ_PRIO_0 as u32;
+
+/// Event flag consumed by the MAC task while waiting on notifications.
+const EVENT_MAC_LAYER_MASK: u32 = 1 << 0;
+
+/// Registers the MAC bare-metal runner with the lightweight sequencer.
+///
+/// Mirrors the behaviour of the reference implementation:
+/// `UTIL_SEQ_RegTask(TASK_MAC_LAYER, UTIL_SEQ_RFU, mac_baremetal_run);`
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn MacSys_Init() {
+    util_seq::UTIL_SEQ_RegTask(TASK_MAC_LAYER_MASK, UTIL_SEQ_RFU, Some(mac::mac_baremetal_run));
+}
+
+/**
+ * @brief  Mac Layer Resume
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn MacSys_Resume() {
+    util_seq::UTIL_SEQ_ResumeTask(TASK_MAC_LAYER_MASK);
+}
+
+/**
+ * @brief  MAC Layer set Task.
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn MacSys_SemaphoreSet() {
+    util_seq::UTIL_SEQ_SetTask(TASK_MAC_LAYER_MASK, TASK_PRIO_MAC_LAYER);
+}
+
+/**
+ * @brief  MAC Layer Task wait.
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn MacSys_SemaphoreWait() {}
+
+/**
+ * @brief  MAC Layer set Event.
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn MacSys_EventSet() {
+    util_seq::UTIL_SEQ_SetEvt(EVENT_MAC_LAYER_MASK);
+}
+
+/**
+ * @brief  MAC Layer wait Event.
+ * @param  None
+ * @retval None
+ */
+#[unsafe(no_mangle)]
+pub unsafe extern "C" fn MacSys_EventWait() {
+    util_seq::UTIL_SEQ_WaitEvt(EVENT_MAC_LAYER_MASK);
+}
diff --git a/embassy-stm32-wpan/src/wba/mod.rs b/embassy-stm32-wpan/src/wba/mod.rs
new file mode 100644
index 000000000..3161b578e
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/mod.rs
@@ -0,0 +1,6 @@
+pub mod bindings;
+pub mod linklayer_plat;
+pub mod ll_sys;
+pub mod ll_sys_if;
+pub mod mac_sys_if;
+pub mod util_seq;
diff --git a/embassy-stm32-wpan/src/wba/util_seq.rs b/embassy-stm32-wpan/src/wba/util_seq.rs
new file mode 100644
index 000000000..b596df908
--- /dev/null
+++ b/embassy-stm32-wpan/src/wba/util_seq.rs
@@ -0,0 +1,243 @@
+#![cfg(feature = "wba")]
+
+use core::cell::UnsafeCell;
+use core::sync::atomic::{AtomicBool, AtomicU32, Ordering};
+
+use critical_section::with as critical;
+
+type TaskFn = unsafe extern "C" fn();
+
+const MAX_TASKS: usize = 32;
+const DEFAULT_PRIORITY: u8 = u8::MAX;
+
+struct TaskTable {
+    funcs: UnsafeCell<[Option<TaskFn>; MAX_TASKS]>,
+    priorities: UnsafeCell<[u8; MAX_TASKS]>,
+}
+
+impl TaskTable {
+    const fn new() -> Self {
+        Self {
+            funcs: UnsafeCell::new([None; MAX_TASKS]),
+            priorities: UnsafeCell::new([DEFAULT_PRIORITY; MAX_TASKS]),
+        }
+    }
+
+    unsafe fn set_task(&self, idx: usize, func: Option<TaskFn>, priority: u8) {
+        (*self.funcs.get())[idx] = func;
+        (*self.priorities.get())[idx] = priority;
+    }
+
+    unsafe fn update_priority(&self, idx: usize, priority: u8) {
+        (*self.priorities.get())[idx] = priority;
+    }
+
+    unsafe fn task(&self, idx: usize) -> Option<TaskFn> {
+        (*self.funcs.get())[idx]
+    }
+
+    unsafe fn priority(&self, idx: usize) -> u8 {
+        (*self.priorities.get())[idx]
+    }
+}
+
+unsafe impl Sync for TaskTable {}
+
+#[inline(always)]
+fn wake_event() {
+    #[cfg(target_arch = "arm")]
+    {
+        cortex_m::asm::sev();
+    }
+
+    #[cfg(not(target_arch = "arm"))]
+    {
+        // No-op on architectures without SEV support.
+    }
+}
+
+#[inline(always)]
+fn wait_event() {
+    #[cfg(target_arch = "arm")]
+    {
+        cortex_m::asm::wfe();
+    }
+
+    #[cfg(not(target_arch = "arm"))]
+    {
+        core::hint::spin_loop();
+    }
+}
+
+static TASKS: TaskTable = TaskTable::new();
+static PENDING_TASKS: AtomicU32 = AtomicU32::new(0);
+static EVENTS: AtomicU32 = AtomicU32::new(0);
+static SCHEDULING: AtomicBool = AtomicBool::new(false);
+
+fn mask_to_index(mask: u32) -> Option<usize> {
+    if mask == 0 {
+        return None;
+    }
+    let idx = mask.trailing_zeros() as usize;
+    if idx < MAX_TASKS { Some(idx) } else { None }
+}
+
+fn drain_pending_tasks() {
+    loop {
+        if SCHEDULING
+            .compare_exchange(false, true, Ordering::AcqRel, Ordering::Acquire)
+            .is_err()
+        {
+            return;
+        }
+
+        loop {
+            let next = critical(|_| select_next_task());
+            match next {
+                Some((idx, task)) => unsafe {
+                    task();
+                    // Force a fresh read of the pending bitmask after each task completion.
+                    let _ = idx;
+                },
+                None => break,
+            }
+        }
+
+        SCHEDULING.store(false, Ordering::Release);
+
+        if PENDING_TASKS.load(Ordering::Acquire) == 0 {
+            break;
+        }
+    }
+}
+
+/// Poll and execute any tasks that have been scheduled via the UTIL sequencer API.
+pub fn poll_pending_tasks() {
+    drain_pending_tasks();
+}
+
+fn select_next_task() -> Option<(usize, TaskFn)> {
+    let pending = PENDING_TASKS.load(Ordering::Acquire);
+    if pending == 0 {
+        return None;
+    }
+
+    let mut remaining = pending;
+    let mut best_idx: Option<usize> = None;
+    let mut best_priority = DEFAULT_PRIORITY;
+    let mut best_fn: Option<TaskFn> = None;
+
+    while remaining != 0 {
+        let idx = remaining.trailing_zeros() as usize;
+        remaining &= remaining - 1;
+
+        if idx >= MAX_TASKS {
+            continue;
+        }
+
+        unsafe {
+            if let Some(func) = TASKS.task(idx) {
+                let prio = TASKS.priority(idx);
+                if prio <= best_priority {
+                    if prio < best_priority || best_idx.map_or(true, |current| idx < current) {
+                        best_priority = prio;
+                        best_idx = Some(idx);
+                        best_fn = Some(func);
+                    }
+                }
+            } else {
+                PENDING_TASKS.fetch_and(!(1u32 << idx), Ordering::AcqRel);
+            }
+        }
+    }
+
+    if let (Some(idx), Some(func)) = (best_idx, best_fn) {
+        PENDING_TASKS.fetch_and(!(1u32 << idx), Ordering::AcqRel);
+        Some((idx, func))
+    } else {
+        None
+    }
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_RegTask(task_mask: u32, _flags: u32, task: Option<TaskFn>) {
+    if let Some(idx) = mask_to_index(task_mask) {
+        critical(|_| unsafe {
+            TASKS.set_task(idx, task, DEFAULT_PRIORITY);
+        });
+    }
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_UnregTask(task_mask: u32) {
+    if let Some(idx) = mask_to_index(task_mask) {
+        critical(|_| unsafe {
+            TASKS.set_task(idx, None, DEFAULT_PRIORITY);
+        });
+        PENDING_TASKS.fetch_and(!(task_mask), Ordering::AcqRel);
+    }
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_SetTask(task_mask: u32, priority: u32) {
+    let prio = (priority & 0xFF) as u8;
+
+    if let Some(idx) = mask_to_index(task_mask) {
+        let registered = critical(|_| unsafe {
+            if TASKS.task(idx).is_some() {
+                TASKS.update_priority(idx, prio);
+                true
+            } else {
+                false
+            }
+        });
+
+        if registered {
+            PENDING_TASKS.fetch_or(task_mask, Ordering::Release);
+            wake_event();
+        }
+    }
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_ResumeTask(task_mask: u32) {
+    PENDING_TASKS.fetch_or(task_mask, Ordering::Release);
+    wake_event();
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_PauseTask(task_mask: u32) {
+    PENDING_TASKS.fetch_and(!task_mask, Ordering::AcqRel);
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_SetEvt(event_mask: u32) {
+    EVENTS.fetch_or(event_mask, Ordering::Release);
+    wake_event();
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_ClrEvt(event_mask: u32) {
+    EVENTS.fetch_and(!event_mask, Ordering::AcqRel);
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_IsEvtSet(event_mask: u32) -> u32 {
+    let state = EVENTS.load(Ordering::Acquire);
+    if (state & event_mask) == event_mask { 1 } else { 0 }
+}
+
+#[unsafe(no_mangle)]
+pub extern "C" fn UTIL_SEQ_WaitEvt(event_mask: u32) {
+    loop {
+        poll_pending_tasks();
+
+        let current = EVENTS.load(Ordering::Acquire);
+        if (current & event_mask) == event_mask {
+            EVENTS.fetch_and(!event_mask, Ordering::AcqRel);
+            break;
+        }
+
+        wait_event();
+    }
+}