7 files changed, 229 insertions, 82 deletions

diff --git a/embassy-stm32-wpan/src/ble.rs b/embassy-stm32-wpan/src/ble.rs
index 19955a8a3..57348a925 100644
--- a/embassy-stm32-wpan/src/ble.rs
+++ b/embassy-stm32-wpan/src/ble.rs
@@ -1,5 +1,3 @@
-use core::mem::MaybeUninit;
-
 use embassy_stm32::ipcc::Ipcc;
 
 use crate::cmd::{CmdPacket, CmdSerial};
@@ -18,7 +16,7 @@ impl Ble {
         unsafe {
             LinkedListNode::init_head(EVT_QUEUE.as_mut_ptr());
 
-            TL_BLE_TABLE = MaybeUninit::new(BleTable {
+            TL_BLE_TABLE.as_mut_ptr().write_volatile(BleTable {
                 pcmd_buffer: BLE_CMD_BUFFER.as_mut_ptr().cast(),
                 pcs_buffer: CS_BUFFER.as_ptr().cast(),
                 pevt_queue: EVT_QUEUE.as_ptr().cast(),
@@ -31,11 +29,8 @@ impl Ble {
 
     pub(super) fn evt_handler() {
         unsafe {
-            let mut node_ptr = core::ptr::null_mut();
-            let node_ptr_ptr: *mut _ = &mut node_ptr;
-
             while !LinkedListNode::is_empty(EVT_QUEUE.as_mut_ptr()) {
-                LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr(), node_ptr_ptr);
+                let node_ptr = LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr());
 
                 let event = node_ptr.cast();
                 let event = EvtBox::new(event);
diff --git a/embassy-stm32-wpan/src/lib.rs b/embassy-stm32-wpan/src/lib.rs
index c37b67dc4..2852d6270 100644
--- a/embassy-stm32-wpan/src/lib.rs
+++ b/embassy-stm32-wpan/src/lib.rs
@@ -4,6 +4,7 @@
 pub mod fmt;
 
 use core::mem::MaybeUninit;
+use core::sync::atomic::{compiler_fence, Ordering};
 
 use cmd::CmdPacket;
 use embassy_futures::block_on;
@@ -189,33 +190,76 @@ impl<'d> TlMbox<'d> {
         into_ref!(ipcc);
 
         unsafe {
-            TL_REF_TABLE = MaybeUninit::new(RefTable {
-                device_info_table: TL_DEVICE_INFO_TABLE.as_mut_ptr(),
+            TL_REF_TABLE.as_mut_ptr().write_volatile(RefTable {
+                device_info_table: TL_DEVICE_INFO_TABLE.as_ptr(),
                 ble_table: TL_BLE_TABLE.as_ptr(),
                 thread_table: TL_THREAD_TABLE.as_ptr(),
                 sys_table: TL_SYS_TABLE.as_ptr(),
                 mem_manager_table: TL_MEM_MANAGER_TABLE.as_ptr(),
                 traces_table: TL_TRACES_TABLE.as_ptr(),
                 mac_802_15_4_table: TL_MAC_802_15_4_TABLE.as_ptr(),
+                // zigbee_table: TL_ZIGBEE_TABLE.as_ptr(),
+                // lld_tests_table: TL_LLD_TESTS_TABLE.as_ptr(),
+                // ble_lld_table: TL_BLE_LLD_TABLE.as_ptr(),
             });
 
-            TL_SYS_TABLE = MaybeUninit::zeroed();
-            TL_DEVICE_INFO_TABLE = MaybeUninit::zeroed();
-            TL_BLE_TABLE = MaybeUninit::zeroed();
-            TL_THREAD_TABLE = MaybeUninit::zeroed();
-            TL_MEM_MANAGER_TABLE = MaybeUninit::zeroed();
-            TL_TRACES_TABLE = MaybeUninit::zeroed();
-            TL_MAC_802_15_4_TABLE = MaybeUninit::zeroed();
-
-            EVT_POOL = MaybeUninit::zeroed();
-            SYS_SPARE_EVT_BUF = MaybeUninit::zeroed();
-            BLE_SPARE_EVT_BUF = MaybeUninit::zeroed();
-
-            CS_BUFFER = MaybeUninit::zeroed();
-            BLE_CMD_BUFFER = MaybeUninit::zeroed();
-            HCI_ACL_DATA_BUFFER = MaybeUninit::zeroed();
+            TL_SYS_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_DEVICE_INFO_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_BLE_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_THREAD_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_MEM_MANAGER_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+
+            TL_TRACES_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_MAC_802_15_4_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            //            TL_ZIGBEE_TABLE
+            //                .as_mut_ptr()
+            //                .write_volatile(MaybeUninit::zeroed().assume_init());
+            //            TL_LLD_TESTS_TABLE
+            //                .as_mut_ptr()
+            //                .write_volatile(MaybeUninit::zeroed().assume_init());
+            //            TL_BLE_LLD_TABLE
+            //                .as_mut_ptr()
+            //                .write_volatile(MaybeUninit::zeroed().assume_init());
+
+            EVT_POOL
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            SYS_SPARE_EVT_BUF
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            BLE_SPARE_EVT_BUF
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+
+            {
+                BLE_CMD_BUFFER
+                    .as_mut_ptr()
+                    .write_volatile(MaybeUninit::zeroed().assume_init());
+                HCI_ACL_DATA_BUFFER
+                    .as_mut_ptr()
+                    .write_volatile(MaybeUninit::zeroed().assume_init());
+                CS_BUFFER
+                    .as_mut_ptr()
+                    .write_volatile(MaybeUninit::zeroed().assume_init());
+            }
         }
 
+        compiler_fence(Ordering::SeqCst);
+
         Ipcc::enable(config);
 
         sys::Sys::enable();
diff --git a/embassy-stm32-wpan/src/mm.rs b/embassy-stm32-wpan/src/mm.rs
index ed13b0dbf..06063b89a 100644
--- a/embassy-stm32-wpan/src/mm.rs
+++ b/embassy-stm32-wpan/src/mm.rs
@@ -1,7 +1,5 @@
 //! Memory manager routines
 
-use core::mem::MaybeUninit;
-
 use embassy_stm32::ipcc::Ipcc;
 
 use crate::evt::EvtPacket;
@@ -20,7 +18,7 @@ impl MemoryManager {
             LinkedListNode::init_head(FREE_BUF_QUEUE.as_mut_ptr());
             LinkedListNode::init_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr());
 
-            TL_MEM_MANAGER_TABLE = MaybeUninit::new(MemManagerTable {
+            TL_MEM_MANAGER_TABLE.as_mut_ptr().write_volatile(MemManagerTable {
                 spare_ble_buffer: BLE_SPARE_EVT_BUF.as_ptr().cast(),
                 spare_sys_buffer: SYS_SPARE_EVT_BUF.as_ptr().cast(),
                 blepool: EVT_POOL.as_ptr().cast(),
@@ -53,11 +51,9 @@ impl MemoryManager {
     /// gives free event buffers back to CPU2 from local buffer queue
     pub fn send_free_buf() {
         unsafe {
-            let mut node_ptr = core::ptr::null_mut();
-            let node_ptr_ptr: *mut _ = &mut node_ptr;
-
             while !LinkedListNode::is_empty(LOCAL_FREE_BUF_QUEUE.as_mut_ptr()) {
-                LinkedListNode::remove_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), node_ptr_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,
diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs
index a19d12d27..0cff5c746 100644
--- a/embassy-stm32-wpan/src/sys.rs
+++ b/embassy-stm32-wpan/src/sys.rs
@@ -1,5 +1,3 @@
-use core::mem::MaybeUninit;
-
 use embassy_stm32::ipcc::Ipcc;
 
 use crate::cmd::{CmdPacket, CmdSerial};
@@ -15,7 +13,7 @@ impl Sys {
         unsafe {
             LinkedListNode::init_head(SYSTEM_EVT_QUEUE.as_mut_ptr());
 
-            TL_SYS_TABLE = MaybeUninit::new(SysTable {
+            TL_SYS_TABLE.as_mut_ptr().write_volatile(SysTable {
                 pcmd_buffer: SYS_CMD_BUF.as_mut_ptr(),
                 sys_queue: SYSTEM_EVT_QUEUE.as_ptr(),
             })
@@ -47,11 +45,8 @@ impl Sys {
 
     pub fn evt_handler() {
         unsafe {
-            let mut node_ptr = core::ptr::null_mut();
-            let node_ptr_ptr: *mut _ = &mut node_ptr;
-
             while !LinkedListNode::is_empty(SYSTEM_EVT_QUEUE.as_mut_ptr()) {
-                LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr(), node_ptr_ptr);
+                let node_ptr = LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr());
 
                 let event = node_ptr.cast();
                 let event = EvtBox::new(event);
diff --git a/embassy-stm32-wpan/src/unsafe_linked_list.rs b/embassy-stm32-wpan/src/unsafe_linked_list.rs
index 52c106fa2..a312178b3 100644
--- a/embassy-stm32-wpan/src/unsafe_linked_list.rs
+++ b/embassy-stm32-wpan/src/unsafe_linked_list.rs
@@ -11,6 +11,8 @@
     unused_mut
 )]
 
+use core::ptr;
+
 use cortex_m::interrupt;
 
 #[derive(Copy, Clone)]
@@ -30,55 +32,136 @@ impl Default for LinkedListNode {
 }
 
 impl LinkedListNode {
-    pub unsafe fn init_head(mut list_head: *mut LinkedListNode) {
-        (*list_head).next = list_head;
-        (*list_head).prev = list_head;
+    pub unsafe fn init_head(mut p_list_head: *mut LinkedListNode) {
+        ptr::write_volatile(
+            p_list_head,
+            LinkedListNode {
+                next: p_list_head,
+                prev: p_list_head,
+            },
+        );
     }
 
-    pub unsafe fn is_empty(mut list_head: *mut LinkedListNode) -> bool {
-        interrupt::free(|_| ((*list_head).next) == list_head)
+    pub unsafe fn is_empty(mut p_list_head: *mut LinkedListNode) -> bool {
+        interrupt::free(|_| ptr::read_volatile(p_list_head).next == p_list_head)
     }
 
-    pub unsafe fn insert_head(mut list_head: *mut LinkedListNode, mut node: *mut LinkedListNode) {
+    /// Insert `node` after `list_head` and before the next node
+    pub unsafe fn insert_head(mut p_list_head: *mut LinkedListNode, mut p_node: *mut LinkedListNode) {
         interrupt::free(|_| {
-            (*node).next = (*list_head).next;
-            (*node).prev = list_head;
-            (*list_head).next = node;
-            (*(*node).next).prev = node;
+            let mut list_head = ptr::read_volatile(p_list_head);
+            if p_list_head != list_head.next {
+                let mut node_next = ptr::read_volatile(list_head.next);
+                let node = LinkedListNode {
+                    next: list_head.next,
+                    prev: p_list_head,
+                };
+
+                list_head.next = p_node;
+                node_next.prev = p_node;
+
+                // All nodes must be written because they will all be seen by another core
+                ptr::write_volatile(p_node, node);
+                ptr::write_volatile(node.next, node_next);
+                ptr::write_volatile(p_list_head, list_head);
+            } else {
+                let node = LinkedListNode {
+                    next: list_head.next,
+                    prev: p_list_head,
+                };
+
+                list_head.next = p_node;
+                list_head.prev = p_node;
+
+                // All nodes must be written because they will all be seen by another core
+                ptr::write_volatile(p_node, node);
+                ptr::write_volatile(p_list_head, list_head);
+            }
         });
     }
 
-    pub unsafe fn insert_tail(mut list_head: *mut LinkedListNode, mut node: *mut LinkedListNode) {
+    /// Insert `node` before `list_tail` and after the second-to-last node
+    pub unsafe fn insert_tail(mut p_list_tail: *mut LinkedListNode, mut p_node: *mut LinkedListNode) {
         interrupt::free(|_| {
-            (*node).next = list_head;
-            (*node).prev = (*list_head).prev;
-            (*list_head).prev = node;
-            (*(*node).prev).next = node;
+            let mut list_tail = ptr::read_volatile(p_list_tail);
+            if p_list_tail != list_tail.prev {
+                let mut node_prev = ptr::read_volatile(list_tail.prev);
+                let node = LinkedListNode {
+                    next: p_list_tail,
+                    prev: list_tail.prev,
+                };
+
+                list_tail.prev = p_node;
+                node_prev.next = p_node;
+
+                // All nodes must be written because they will all be seen by another core
+                ptr::write_volatile(p_node, node);
+                ptr::write_volatile(node.prev, node_prev);
+                ptr::write_volatile(p_list_tail, list_tail);
+            } else {
+                let node = LinkedListNode {
+                    next: p_list_tail,
+                    prev: list_tail.prev,
+                };
+
+                list_tail.prev = p_node;
+                list_tail.next = p_node;
+
+                // All nodes must be written because they will all be seen by another core
+                ptr::write_volatile(p_node, node);
+                ptr::write_volatile(p_list_tail, list_tail);
+            }
         });
     }
 
     /// Remove `node` from the linked list
-    pub unsafe fn remove_node(mut node: *mut LinkedListNode) {
+    pub unsafe fn remove_node(mut p_node: *mut LinkedListNode) {
         interrupt::free(|_| {
-            (*(*node).prev).next = (*node).next;
-            (*(*node).next).prev = (*node).prev;
+            let node = ptr::read_volatile(p_node);
+            if node.next != node.prev {
+                let mut node_next = ptr::read_volatile(node.next);
+                let mut node_prev = ptr::read_volatile(node.prev);
+
+                node_prev.next = node.next;
+                node_next.prev = node.prev;
+
+                ptr::write_volatile(node.next, node_next);
+                ptr::write_volatile(node.prev, node_prev);
+            } else {
+                let mut node_next = ptr::read_volatile(node.next);
+
+                node_next.next = node.next;
+                node_next.prev = node.prev;
+
+                ptr::write_volatile(node.next, node_next);
+            }
         });
     }
 
-    /// Remove `list_head` into `node`
-    pub unsafe fn remove_head(mut list_head: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+    /// Remove `list_head` and return a pointer to the `node`.
+    pub unsafe fn remove_head(mut p_list_head: *mut LinkedListNode) -> *mut LinkedListNode {
         interrupt::free(|_| {
-            *node = (*list_head).next;
-            Self::remove_node((*list_head).next);
-        });
+            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);
+
+            p_node
+        })
     }
 
-    /// Remove `list_tail` into `node`
-    pub unsafe fn remove_tail(mut list_tail: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+    /// Remove `list_tail` and return a pointer to the `node`.
+    pub unsafe fn remove_tail(mut p_list_tail: *mut LinkedListNode) -> *mut LinkedListNode {
         interrupt::free(|_| {
-            *node = (*list_tail).prev;
-            Self::remove_node((*list_tail).prev);
-        });
+            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);
+
+            p_node
+        })
     }
 
     pub unsafe fn insert_node_after(mut node: *mut LinkedListNode, mut ref_node: *mut LinkedListNode) {
@@ -88,6 +171,8 @@ impl LinkedListNode {
             (*ref_node).next = node;
             (*(*node).next).prev = node;
         });
+
+        todo!("this function has not been converted to volatile semantics");
     }
 
     pub unsafe fn insert_node_before(mut node: *mut LinkedListNode, mut ref_node: *mut LinkedListNode) {
@@ -97,6 +182,8 @@ impl LinkedListNode {
             (*ref_node).prev = node;
             (*(*node).prev).next = node;
         });
+
+        todo!("this function has not been converted to volatile semantics");
     }
 
     pub unsafe fn get_size(mut list_head: *mut LinkedListNode) -> usize {
@@ -111,18 +198,47 @@ impl LinkedListNode {
             }
 
             size
-        })
+        });
+
+        todo!("this function has not been converted to volatile semantics");
     }
 
-    pub unsafe fn get_next_node(mut ref_node: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+    pub unsafe fn get_next_node(mut p_ref_node: *mut LinkedListNode) -> *mut LinkedListNode {
         interrupt::free(|_| {
-            *node = (*ref_node).next;
-        });
+            let ref_node = ptr::read_volatile(p_ref_node);
+
+            // Allowed because a removed node is not seen by another core
+            ref_node.next
+        })
     }
 
-    pub unsafe fn get_prev_node(mut ref_node: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+    pub unsafe fn get_prev_node(mut p_ref_node: *mut LinkedListNode) -> *mut LinkedListNode {
         interrupt::free(|_| {
-            *node = (*ref_node).prev;
-        });
+            let ref_node = ptr::read_volatile(p_ref_node);
+
+            // Allowed because a removed node is not seen by another core
+            ref_node.prev
+        })
+    }
+}
+
+#[allow(dead_code)]
+unsafe fn debug_linked_list(mut p_node: *mut LinkedListNode) {
+    info!("iterating list from node: {:x}", p_node);
+    let mut p_current_node = p_node;
+    let mut i = 0;
+    loop {
+        let current_node = ptr::read_volatile(p_current_node);
+        info!(
+            "node (prev, current, next): {:x}, {:x}, {:x}",
+            current_node.prev, p_current_node, current_node.next
+        );
+
+        i += 1;
+        if i > 10 || current_node.next == p_node {
+            break;
+        }
+
+        p_current_node = current_node.next;
     }
 }
diff --git a/tests/stm32/Cargo.toml b/tests/stm32/Cargo.toml
index dcc7d9b8a..487ef4626 100644
--- a/tests/stm32/Cargo.toml
+++ b/tests/stm32/Cargo.toml
@@ -12,7 +12,7 @@ stm32g071rb = ["embassy-stm32/stm32g071rb", "not-gpdma"]     # Nucleo
 stm32c031c6 = ["embassy-stm32/stm32c031c6", "not-gpdma"]     # Nucleo
 stm32g491re = ["embassy-stm32/stm32g491re", "not-gpdma"]     # Nucleo
 stm32h755zi = ["embassy-stm32/stm32h755zi-cm7", "not-gpdma"] # Nucleo
-stm32wb55rg = ["embassy-stm32/stm32wb55rg", "not-gpdma" ]     # Nucleo
+stm32wb55rg = ["embassy-stm32/stm32wb55rg", "not-gpdma", "ble" ]     # Nucleo
 stm32h563zi = ["embassy-stm32/stm32h563zi"]     # Nucleo
 stm32u585ai = ["embassy-stm32/stm32u585ai"]     # IoT board
 
diff --git a/tests/stm32/src/bin/tl_mbox.rs b/tests/stm32/src/bin/tl_mbox.rs
index b57519a54..4669cbc62 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::rc::RadioCoprocessor;
 use embassy_stm32_wpan::TlMbox;
 use embassy_time::{Duration, Timer};
 
@@ -50,14 +51,14 @@ async fn main(_spawner: Spawner) {
         Timer::after(Duration::from_millis(50)).await;
     }
 
-    //    let mut rc = RadioCoprocessor::new(mbox);
-    //
-    //    let response = rc.read().await;
-    //    info!("coprocessor ready {}", response);
-    //
-    //    rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]);
-    //    let response = rc.read().await;
-    //    info!("ble reset rsp {}", response);
+    let mut rc = RadioCoprocessor::new(mbox);
+
+    let response = rc.read().await;
+    info!("coprocessor ready {}", response);
+
+    rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]);
+    let response = rc.read().await;
+    info!("ble reset rsp {}", response);
 
     info!("Test OK");
     cortex_m::asm::bkpt();