5 files changed, 362 insertions, 35 deletions

diff --git a/embassy-stm32-wpan/Cargo.toml b/embassy-stm32-wpan/Cargo.toml
index 7e562f5cc..103dedead 100644
--- a/embassy-stm32-wpan/Cargo.toml
+++ b/embassy-stm32-wpan/Cargo.toml
@@ -59,7 +59,7 @@ wb55_mac = ["dep:bitflags", "dep:embassy-net-driver", "dep:smoltcp", "smoltcp/me
 
 wba = [ "dep:stm32-bindings" ]
 wba_ble = [ "stm32-bindings/wba_wpan_mac" , "stm32-bindings/wba_wpan" ]
-wba_mac = [ "stm32-bindings/wba_wpan_ble" , "stm32-bindings/lib_wba5_linklayer15_4", "stm32-bindings/lib_wba_mac_lib" , "stm32-bindings/wba_wpan" ]
+wba_mac = [ "stm32-bindings/wba_wpan_mac", "stm32-bindings/wba_wpan_ble" , "stm32-bindings/lib_wba5_linklayer15_4", "stm32-bindings/lib_wba_mac_lib" , "stm32-bindings/wba_wpan" ]
 
 extended = []
 
diff --git a/embassy-stm32-wpan/src/wba/ll_sys_if.rs b/embassy-stm32-wpan/src/wba/ll_sys_if.rs
index 992c2a6f1..7218b69c4 100644
--- a/embassy-stm32-wpan/src/wba/ll_sys_if.rs
+++ b/embassy-stm32-wpan/src/wba/ll_sys_if.rs
@@ -1,6 +1,4 @@
-#[allow(dead_code)]
-fn test_fn() {}
-
+#![cfg(feature = "wba")]
 // /* USER CODE BEGIN Header */
 // /**
 //   ******************************************************************************
@@ -333,3 +331,86 @@ fn test_fn() {}
 // }
 // #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
index b0dab238e..273399a19 100644
--- a/embassy-stm32-wpan/src/wba/mac_sys_if.rs
+++ b/embassy-stm32-wpan/src/wba/mac_sys_if.rs
@@ -1,4 +1,6 @@
-use crate::bindings::mac::mac_baremetal_run;
+#![cfg(feature = "wba")]
+#![allow(non_snake_case)]
+
 //
 // /* USER CODE BEGIN Header */
 // /**
@@ -113,16 +115,28 @@ use crate::bindings::mac::mac_baremetal_run;
 // }
 //
 
-/**
- * @brief  Mac Layer Initialisation
- * @param  None
- * @retval None
- */
+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 extern "C" fn MacSys_Init() {
-    unsafe {
-        mac_baremetal_run();
-    }
+pub unsafe extern "C" fn MacSys_Init() {
+    util_seq::UTIL_SEQ_RegTask(TASK_MAC_LAYER_MASK, UTIL_SEQ_RFU, Some(mac::mac_baremetal_run));
 }
 
 /**
@@ -131,10 +145,8 @@ pub extern "C" fn MacSys_Init() {
  * @retval None
  */
 #[unsafe(no_mangle)]
-pub extern "C" fn MacSys_Resume() {
-    unsafe {
-        mac_baremetal_run();
-    }
+pub unsafe extern "C" fn MacSys_Resume() {
+    util_seq::UTIL_SEQ_ResumeTask(TASK_MAC_LAYER_MASK);
 }
 
 /**
@@ -143,10 +155,8 @@ pub extern "C" fn MacSys_Resume() {
  * @retval None
  */
 #[unsafe(no_mangle)]
-pub extern "C" fn MacSys_SemaphoreSet() {
-    unsafe {
-        mac_baremetal_run();
-    }
+pub unsafe extern "C" fn MacSys_SemaphoreSet() {
+    util_seq::UTIL_SEQ_SetTask(TASK_MAC_LAYER_MASK, TASK_PRIO_MAC_LAYER);
 }
 
 /**
@@ -155,11 +165,7 @@ pub extern "C" fn MacSys_SemaphoreSet() {
  * @retval None
  */
 #[unsafe(no_mangle)]
-pub extern "C" fn MacSys_SemaphoreWait() {
-    unsafe {
-        mac_baremetal_run();
-    }
-}
+pub unsafe extern "C" fn MacSys_SemaphoreWait() {}
 
 /**
  * @brief  MAC Layer set Event.
@@ -167,10 +173,8 @@ pub extern "C" fn MacSys_SemaphoreWait() {
  * @retval None
  */
 #[unsafe(no_mangle)]
-pub extern "C" fn MacSys_EventSet() {
-    unsafe {
-        mac_baremetal_run();
-    }
+pub unsafe extern "C" fn MacSys_EventSet() {
+    util_seq::UTIL_SEQ_SetEvt(EVENT_MAC_LAYER_MASK);
 }
 
 /**
@@ -179,8 +183,6 @@ pub extern "C" fn MacSys_EventSet() {
  * @retval None
  */
 #[unsafe(no_mangle)]
-pub extern "C" fn MacSys_EventWait() {
-    unsafe {
-        mac_baremetal_run();
-    }
+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
index c93b8d020..3161b578e 100644
--- a/embassy-stm32-wpan/src/wba/mod.rs
+++ b/embassy-stm32-wpan/src/wba/mod.rs
@@ -3,3 +3,4 @@ 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();
+    }
+}