8 files changed, 70 insertions, 62 deletions

diff --git a/embassy-stm32-wpan/src/ble.rs b/embassy-stm32-wpan/src/ble.rs
index 46a2f41c4..d8af861ae 100644
--- a/embassy-stm32-wpan/src/ble.rs
+++ b/embassy-stm32-wpan/src/ble.rs
@@ -1,6 +1,8 @@
+use core::mem::MaybeUninit;
+
 use embassy_stm32::ipcc::Ipcc;
 
-use crate::cmd::{CmdPacket, CmdSerial};
+use crate::cmd::CmdPacket;
 use crate::consts::TlPacketType;
 use crate::evt::EvtBox;
 use crate::tables::BleTable;
@@ -14,6 +16,11 @@ pub struct Ble;
 impl Ble {
     pub(super) fn enable() {
         unsafe {
+            // Ensure reproducible behavior
+            BLE_CMD_BUFFER
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+
             LinkedListNode::init_head(EVT_QUEUE.as_mut_ptr());
 
             TL_BLE_TABLE.as_mut_ptr().write_volatile(BleTable {
@@ -29,11 +36,8 @@ impl Ble {
 
     pub(super) fn evt_handler() {
         unsafe {
-            while !LinkedListNode::is_empty(EVT_QUEUE.as_mut_ptr()) {
-                let node_ptr = LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr());
-
-                let event = node_ptr.cast();
-                let event = EvtBox::new(event);
+            while let Some(node_ptr) = LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr()) {
+                let event = EvtBox::new(node_ptr.cast());
 
                 EVT_CHANNEL.try_send(event).unwrap();
             }
@@ -48,18 +52,11 @@ impl Ble {
         // TODO: ACL data ack to the user
     }
 
-    pub fn send_cmd(buf: &[u8]) {
+    pub fn send_cmd(opcode: u16, payload: &[u8]) {
         debug!("writing ble cmd");
 
         unsafe {
-            let pcmd_buffer: *mut CmdPacket = BLE_CMD_BUFFER.as_mut_ptr();
-            let pcmd_serial: *mut CmdSerial = &mut (*pcmd_buffer).cmdserial;
-            let pcmd_serial_buf: *mut u8 = pcmd_serial.cast();
-
-            core::ptr::copy(buf.as_ptr(), pcmd_serial_buf, buf.len());
-
-            let cmd_packet = &mut *(*TL_REF_TABLE.assume_init().ble_table).pcmd_buffer;
-            cmd_packet.cmdserial.ty = TlPacketType::BleCmd as u8;
+            CmdPacket::write_into(BLE_CMD_BUFFER.as_mut_ptr(), TlPacketType::BleCmd, opcode, payload);
         }
 
         Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_BLE_CMD_CHANNEL);
diff --git a/embassy-stm32-wpan/src/cmd.rs b/embassy-stm32-wpan/src/cmd.rs
index a023201ba..581e5019b 100644
--- a/embassy-stm32-wpan/src/cmd.rs
+++ b/embassy-stm32-wpan/src/cmd.rs
@@ -1,3 +1,6 @@
+use core::ptr;
+
+use crate::consts::TlPacketType;
 use crate::evt::{EvtPacket, EvtSerial};
 use crate::{PacketHeader, TL_EVT_HEADER_SIZE};
 
@@ -42,6 +45,22 @@ pub struct CmdPacket {
 }
 
 impl CmdPacket {
+    pub unsafe fn write_into(cmd_buf: *mut CmdPacket, packet_type: TlPacketType, cmd_code: u16, payload: &[u8]) {
+        let p_cmd_serial = &mut (*cmd_buf).cmdserial as *mut _ as *mut CmdSerialStub;
+        let p_payload = &mut (*cmd_buf).cmdserial.cmd.payload as *mut _;
+
+        ptr::write_volatile(
+            p_cmd_serial,
+            CmdSerialStub {
+                ty: packet_type as u8,
+                cmd_code: cmd_code,
+                payload_len: payload.len() as u8,
+            },
+        );
+
+        ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len());
+    }
+
     /// Writes an underlying CmdPacket into the provided buffer.
     /// Returns a number of bytes that were written.
     /// Returns an error if event kind is unknown or if provided buffer size is not enough.
diff --git a/embassy-stm32-wpan/src/mm.rs b/embassy-stm32-wpan/src/mm.rs
index 06063b89a..1ea6edeff 100644
--- a/embassy-stm32-wpan/src/mm.rs
+++ b/embassy-stm32-wpan/src/mm.rs
@@ -7,7 +7,7 @@ use crate::tables::MemManagerTable;
 use crate::unsafe_linked_list::LinkedListNode;
 use crate::{
     channels, BLE_SPARE_EVT_BUF, EVT_POOL, FREE_BUF_QUEUE, LOCAL_FREE_BUF_QUEUE, POOL_SIZE, SYS_SPARE_EVT_BUF,
-    TL_MEM_MANAGER_TABLE, TL_REF_TABLE,
+    TL_MEM_MANAGER_TABLE,
 };
 
 pub(super) struct MemoryManager;
@@ -51,13 +51,8 @@ impl MemoryManager {
     /// gives free event buffers back to CPU2 from local buffer queue
     pub fn send_free_buf() {
         unsafe {
-            while !LinkedListNode::is_empty(LOCAL_FREE_BUF_QUEUE.as_mut_ptr()) {
-                let node_ptr = LinkedListNode::remove_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr());
-
-                LinkedListNode::insert_tail(
-                    (*(*TL_REF_TABLE.as_ptr()).mem_manager_table).pevt_free_buffer_queue,
-                    node_ptr,
-                );
+            while let Some(node_ptr) = LinkedListNode::remove_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr()) {
+                LinkedListNode::insert_head(FREE_BUF_QUEUE.as_mut_ptr(), node_ptr);
             }
         }
     }
diff --git a/embassy-stm32-wpan/src/rc.rs b/embassy-stm32-wpan/src/rc.rs
index a217aa224..e67350f79 100644
--- a/embassy-stm32-wpan/src/rc.rs
+++ b/embassy-stm32-wpan/src/rc.rs
@@ -1,6 +1,6 @@
 use crate::ble::Ble;
 use crate::consts::TlPacketType;
-use crate::{shci, TlMbox, STATE};
+use crate::{TlMbox, STATE};
 
 pub struct RadioCoprocessor<'d> {
     mbox: TlMbox<'d>,
@@ -15,12 +15,12 @@ impl<'d> RadioCoprocessor<'d> {
         }
     }
 
-    pub fn write(&self, buf: &[u8]) {
+    pub fn write(&self, opcode: u16, buf: &[u8]) {
         let cmd_code = buf[0];
         let cmd = TlPacketType::try_from(cmd_code).unwrap();
 
         match &cmd {
-            TlPacketType::BleCmd => Ble::send_cmd(buf),
+            TlPacketType::BleCmd => Ble::send_cmd(opcode, buf),
             _ => todo!(),
         }
     }
@@ -30,8 +30,6 @@ impl<'d> RadioCoprocessor<'d> {
             STATE.wait().await;
 
             while let Some(evt) = self.mbox.dequeue_event() {
-                let event = evt.evt();
-
                 evt.write(&mut self.rx_buf).unwrap();
             }
 
diff --git a/embassy-stm32-wpan/src/shci.rs b/embassy-stm32-wpan/src/shci.rs
index 6e58a7156..cdf027d5e 100644
--- a/embassy-stm32-wpan/src/shci.rs
+++ b/embassy-stm32-wpan/src/shci.rs
@@ -1,8 +1,6 @@
 use core::{mem, slice};
 
-use super::cmd::CmdPacket;
-use super::consts::TlPacketType;
-use super::{sys, TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE, TL_SYS_TABLE};
+use super::{TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE};
 
 pub const SCHI_OPCODE_BLE_INIT: u16 = 0xfc66;
 
diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs
index 76f65cbd8..fb247f276 100644
--- a/embassy-stm32-wpan/src/sys.rs
+++ b/embassy-stm32-wpan/src/sys.rs
@@ -1,9 +1,9 @@
-use core::ptr;
+use core::mem::MaybeUninit;
 use core::sync::atomic::{compiler_fence, Ordering};
 
 use embassy_stm32::ipcc::Ipcc;
 
-use crate::cmd::{CmdPacket, CmdSerial, CmdSerialStub};
+use crate::cmd::{CmdPacket, CmdSerial};
 use crate::consts::TlPacketType;
 use crate::evt::{CcEvt, EvtBox, EvtSerial};
 use crate::shci::{ShciBleInitCmdParam, SCHI_OPCODE_BLE_INIT};
@@ -16,6 +16,11 @@ pub struct Sys;
 impl Sys {
     pub fn enable() {
         unsafe {
+            // Ensure reproducible behavior
+            SYS_CMD_BUF
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+
             LinkedListNode::init_head(SYSTEM_EVT_QUEUE.as_mut_ptr());
 
             TL_SYS_TABLE.as_mut_ptr().write_volatile(SysTable {
@@ -50,11 +55,8 @@ impl Sys {
 
     pub fn evt_handler() {
         unsafe {
-            while !LinkedListNode::is_empty(SYSTEM_EVT_QUEUE.as_mut_ptr()) {
-                let node_ptr = LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr());
-
-                let event = node_ptr.cast();
-                let event = EvtBox::new(event);
+            while let Some(node_ptr) = LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr()) {
+                let event = EvtBox::new(node_ptr.cast());
 
                 EVT_CHANNEL.try_send(event).unwrap();
             }
@@ -71,19 +73,7 @@ impl Sys {
 
     pub fn send_cmd(opcode: u16, payload: &[u8]) {
         unsafe {
-            let p_cmd_serial = &mut (*SYS_CMD_BUF.as_mut_ptr()).cmdserial as *mut _ as *mut CmdSerialStub;
-            let p_payload = &mut (*SYS_CMD_BUF.as_mut_ptr()).cmdserial.cmd.payload as *mut _;
-
-            ptr::write_volatile(
-                p_cmd_serial,
-                CmdSerialStub {
-                    ty: TlPacketType::SysCmd as u8,
-                    cmd_code: opcode,
-                    payload_len: payload.len() as u8,
-                },
-            );
-
-            ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len());
+            CmdPacket::write_into(SYS_CMD_BUF.as_mut_ptr(), TlPacketType::SysCmd, opcode, payload);
         }
 
         compiler_fence(Ordering::SeqCst);
diff --git a/embassy-stm32-wpan/src/unsafe_linked_list.rs b/embassy-stm32-wpan/src/unsafe_linked_list.rs
index a312178b3..5b210aac4 100644
--- a/embassy-stm32-wpan/src/unsafe_linked_list.rs
+++ b/embassy-stm32-wpan/src/unsafe_linked_list.rs
@@ -139,28 +139,36 @@ impl LinkedListNode {
     }
 
     /// Remove `list_head` and return a pointer to the `node`.
-    pub unsafe fn remove_head(mut p_list_head: *mut LinkedListNode) -> *mut LinkedListNode {
+    pub unsafe fn remove_head(mut p_list_head: *mut LinkedListNode) -> Option<*mut LinkedListNode> {
         interrupt::free(|_| {
             let list_head = ptr::read_volatile(p_list_head);
 
-            // Allowed because a removed node is not seen by another core
-            let p_node = list_head.next;
-            Self::remove_node(p_node);
+            if list_head.next == p_list_head {
+                None
+            } else {
+                // Allowed because a removed node is not seen by another core
+                let p_node = list_head.next;
+                Self::remove_node(p_node);
 
-            p_node
+                Some(p_node)
+            }
         })
     }
 
     /// Remove `list_tail` and return a pointer to the `node`.
-    pub unsafe fn remove_tail(mut p_list_tail: *mut LinkedListNode) -> *mut LinkedListNode {
+    pub unsafe fn remove_tail(mut p_list_tail: *mut LinkedListNode) -> Option<*mut LinkedListNode> {
         interrupt::free(|_| {
             let list_tail = ptr::read_volatile(p_list_tail);
 
-            // Allowed because a removed node is not seen by another core
-            let p_node = list_tail.prev;
-            Self::remove_node(p_node);
+            if list_tail.prev == p_list_tail {
+                None
+            } else {
+                // Allowed because a removed node is not seen by another core
+                let p_node = list_tail.prev;
+                Self::remove_node(p_node);
 
-            p_node
+                Some(p_node)
+            }
         })
     }
 
diff --git a/tests/stm32/src/bin/tl_mbox.rs b/tests/stm32/src/bin/tl_mbox.rs
index 5e7d11ef5..5a2309263 100644
--- a/tests/stm32/src/bin/tl_mbox.rs
+++ b/tests/stm32/src/bin/tl_mbox.rs
@@ -10,6 +10,7 @@ use common::*;
 use embassy_executor::Spawner;
 use embassy_stm32::bind_interrupts;
 use embassy_stm32::ipcc::Config;
+use embassy_stm32_wpan::ble::Ble;
 use embassy_stm32_wpan::rc::RadioCoprocessor;
 use embassy_stm32_wpan::sys::Sys;
 use embassy_stm32_wpan::TlMbox;
@@ -59,7 +60,9 @@ async fn main(_spawner: Spawner) {
 
     Sys::shci_ble_init(Default::default());
 
-    rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]);
+    // rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]);
+    Ble::send_cmd(0x0c, &[]);
+
     let response = rc.read().await;
     info!("ble reset rsp {}", response);