7 files changed, 158 insertions, 56 deletions

diff --git a/embassy-stm32-wpan/src/ble.rs b/embassy-stm32-wpan/src/ble.rs
index 19955a8a3..57195ef9d 100644
--- a/embassy-stm32-wpan/src/ble.rs
+++ b/embassy-stm32-wpan/src/ble.rs
@@ -18,7 +18,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(),
@@ -41,6 +41,8 @@ impl Ble {
                 let event = EvtBox::new(event);
 
                 EVT_CHANNEL.try_send(event).unwrap();
+
+                break;
             }
         }
 
diff --git a/embassy-stm32-wpan/src/lib.rs b/embassy-stm32-wpan/src/lib.rs
index c37b67dc4..000282f2b 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..e46c63558 100644
--- a/embassy-stm32-wpan/src/mm.rs
+++ b/embassy-stm32-wpan/src/mm.rs
@@ -20,7 +20,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(),
diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs
index a19d12d27..0a1db7c1f 100644
--- a/embassy-stm32-wpan/src/sys.rs
+++ b/embassy-stm32-wpan/src/sys.rs
@@ -15,7 +15,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(),
             })
@@ -51,12 +51,14 @@ impl Sys {
             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);
+                LinkedListNode::get_next_node(SYSTEM_EVT_QUEUE.as_mut_ptr(), node_ptr_ptr);
 
                 let event = node_ptr.cast();
                 let event = EvtBox::new(event);
 
                 EVT_CHANNEL.try_send(event).unwrap();
+
+                break;
             }
         }
 
diff --git a/embassy-stm32-wpan/src/unsafe_linked_list.rs b/embassy-stm32-wpan/src/unsafe_linked_list.rs
index 52c106fa2..95f4bef44 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,54 +32,102 @@ 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);
+            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);
         });
     }
 
-    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);
+            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);
         });
     }
 
     /// Remove `node` from the linked list
-    pub unsafe fn remove_node(mut node: *mut LinkedListNode) {
+    //    pub unsafe fn remove_node(mut node: *mut LinkedListNode) {
+    //        interrupt::free(|_| {
+    //            (*(*node).prev).next = (*node).next;
+    //            (*(*node).next).prev = (*node).prev;
+    //        });
+    //    }
+
+    /// Remove `node` from the linked list
+    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);
+            let mut node_prev = ptr::read_volatile(node.prev);
+            let mut node_next = ptr::read_volatile(node.next);
+
+            node_prev.next = node.next;
+            node_next.prev = node.prev;
+
+            ptr::write_volatile(node.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) {
+    pub unsafe fn remove_head(mut p_list_head: *mut LinkedListNode, mut p_node: *mut *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
+            *p_node = list_head.next;
+            Self::remove_node(list_head.next);
         });
     }
 
     /// Remove `list_tail` into `node`
-    pub unsafe fn remove_tail(mut list_tail: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+    pub unsafe fn remove_tail(mut p_list_tail: *mut LinkedListNode, mut p_node: *mut *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
+            *p_node = list_tail.prev;
+            Self::remove_node(list_tail.prev);
         });
     }
 
@@ -114,9 +164,12 @@ impl LinkedListNode {
         })
     }
 
-    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 p_node: *mut *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
+            *p_node = ref_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..26d9cc68e 100644
--- a/tests/stm32/src/bin/tl_mbox.rs
+++ b/tests/stm32/src/bin/tl_mbox.rs
@@ -11,6 +11,7 @@ use embassy_executor::Spawner;
 use embassy_stm32::bind_interrupts;
 use embassy_stm32::ipcc::Config;
 use embassy_stm32_wpan::TlMbox;
+use embassy_stm32_wpan::rc::RadioCoprocessor;
 use embassy_time::{Duration, Timer};
 
 bind_interrupts!(struct Irqs{
@@ -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();