4 files changed, 198 insertions, 184 deletions

diff --git a/embassy-stm32-wpan/src/wb55/sub/ble.rs b/embassy-stm32-wpan/src/wb55/sub/ble.rs
index afc4a510a..a2558d735 100644
--- a/embassy-stm32-wpan/src/wb55/sub/ble.rs
+++ b/embassy-stm32-wpan/src/wb55/sub/ble.rs
@@ -73,7 +73,9 @@ impl<'a> Ble<'a> {
     pub async fn tl_read(&mut self) -> EvtBox<Self> {
         self.ipcc_ble_event_channel
             .receive(|| unsafe {
-                if let Some(node_ptr) = LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr()) {
+                if let Some(node_ptr) =
+                    critical_section::with(|cs| LinkedListNode::remove_head(cs, EVT_QUEUE.as_mut_ptr()))
+                {
                     Some(EvtBox::new(node_ptr.cast()))
                 } else {
                     None
diff --git a/embassy-stm32-wpan/src/wb55/sub/mm.rs b/embassy-stm32-wpan/src/wb55/sub/mm.rs
index cbb5f130b..0ca7d1835 100644
--- a/embassy-stm32-wpan/src/wb55/sub/mm.rs
+++ b/embassy-stm32-wpan/src/wb55/sub/mm.rs
@@ -4,7 +4,6 @@ use core::mem::MaybeUninit;
 use core::task::Poll;
 
 use aligned::{A4, Aligned};
-use cortex_m::interrupt;
 use embassy_stm32::ipcc::IpccTxChannel;
 use embassy_sync::waitqueue::AtomicWaker;
 
@@ -52,7 +51,7 @@ impl<'a> MemoryManager<'a> {
         loop {
             poll_fn(|cx| unsafe {
                 MM_WAKER.register(cx.waker());
-                if LinkedListNode::is_empty(LOCAL_FREE_BUF_QUEUE.as_mut_ptr()) {
+                if critical_section::with(|cs| LinkedListNode::is_empty(cs, LOCAL_FREE_BUF_QUEUE.as_mut_ptr())) {
                     Poll::Pending
                 } else {
                     Poll::Ready(())
@@ -62,10 +61,9 @@ impl<'a> MemoryManager<'a> {
 
             self.ipcc_mm_release_buffer_channel
                 .send(|| {
-                    interrupt::free(|_| unsafe {
-                        // CS required while moving nodes
-                        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);
+                    critical_section::with(|cs| unsafe {
+                        while let Some(node_ptr) = LinkedListNode::remove_head(cs, LOCAL_FREE_BUF_QUEUE.as_mut_ptr()) {
+                            LinkedListNode::insert_head(cs, FREE_BUF_QUEUE.as_mut_ptr(), node_ptr);
                         }
                     })
                 })
@@ -77,8 +75,8 @@ impl<'a> MemoryManager<'a> {
 impl<'a> evt::MemoryManager for MemoryManager<'a> {
     /// SAFETY: passing a pointer to something other than a managed event packet is UB
     unsafe fn drop_event_packet(evt: *mut EvtPacket) {
-        interrupt::free(|_| unsafe {
-            LinkedListNode::insert_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), evt as *mut _);
+        critical_section::with(|cs| unsafe {
+            LinkedListNode::insert_head(cs, LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), evt as *mut _);
         });
 
         MM_WAKER.wake();
diff --git a/embassy-stm32-wpan/src/wb55/sub/sys.rs b/embassy-stm32-wpan/src/wb55/sub/sys.rs
index 4376314c7..2e625a677 100644
--- a/embassy-stm32-wpan/src/wb55/sub/sys.rs
+++ b/embassy-stm32-wpan/src/wb55/sub/sys.rs
@@ -87,10 +87,12 @@ impl<'a> Sys<'a> {
     /// This method takes the place of the `HW_IPCC_SYS_EvtNot`/`SysUserEvtRx`/`APPE_SysUserEvtRx`,
     /// as the embassy implementation avoids the need to call C public bindings, and instead
     /// handles the event channels directly.
-    pub async fn read<'b>(&mut self) -> EvtBox<mm::MemoryManager<'b>> {
+    pub async fn read(&mut self) -> EvtBox<mm::MemoryManager<'_>> {
         self.ipcc_system_event_channel
             .receive(|| unsafe {
-                if let Some(node_ptr) = LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr()) {
+                if let Some(node_ptr) =
+                    critical_section::with(|cs| LinkedListNode::remove_head(cs, SYSTEM_EVT_QUEUE.as_mut_ptr()))
+                {
                     Some(EvtBox::new(node_ptr.cast()))
                 } else {
                     None
diff --git a/embassy-stm32-wpan/src/wb55/unsafe_linked_list.rs b/embassy-stm32-wpan/src/wb55/unsafe_linked_list.rs
index d8bc29763..c84ee1bb6 100644
--- a/embassy-stm32-wpan/src/wb55/unsafe_linked_list.rs
+++ b/embassy-stm32-wpan/src/wb55/unsafe_linked_list.rs
@@ -11,9 +11,10 @@
     unused_mut
 )]
 
+use core::fmt::Debug;
 use core::ptr;
 
-use cortex_m::interrupt;
+use critical_section::CriticalSection;
 
 #[derive(Copy, Clone)]
 #[repr(C, packed(4))]
@@ -42,216 +43,227 @@ impl LinkedListNode {
         );
     }
 
-    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 is_empty(_cs: CriticalSection, mut p_list_head: *mut LinkedListNode) -> bool {
+        ptr::read_volatile(p_list_head).next == p_list_head
     }
 
     /// 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(|_| {
-            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_head(
+        _cs: CriticalSection,
+        mut p_list_head: *mut LinkedListNode,
+        mut p_node: *mut LinkedListNode,
+    ) {
+        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);
+        }
     }
 
     /// 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(|_| {
-            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);
-            }
-        });
+    pub unsafe fn insert_tail(
+        _cs: CriticalSection,
+        mut p_list_tail: *mut LinkedListNode,
+        mut p_node: *mut LinkedListNode,
+    ) {
+        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 p_node: *mut LinkedListNode) {
-        interrupt::free(|_| {
-            // trace!("remove node: {:x}", p_node);
-            // apparently linked list nodes are not always aligned.
-            // if more hardfaults occur, more of these may need to be converted to unaligned.
-            let node = ptr::read_unaligned(p_node);
-            // trace!("remove node: prev/next {:x}/{:x}", node.prev, node.next);
-
-            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);
-            }
-        });
+    pub unsafe fn remove_node(_cs: CriticalSection, mut p_node: *mut LinkedListNode) {
+        let node = ptr::read_unaligned(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` and return a pointer to the `node`.
-    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);
-
-            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);
-
-                Some(p_node)
-            }
-        })
+    pub unsafe fn remove_head(
+        _cs: CriticalSection,
+        mut p_list_head: *mut LinkedListNode,
+    ) -> Option<*mut LinkedListNode> {
+        let list_head = ptr::read_volatile(p_list_head);
+
+        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(_cs, 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) -> Option<*mut LinkedListNode> {
-        interrupt::free(|_| {
-            let list_tail = ptr::read_volatile(p_list_tail);
-
-            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);
-
-                Some(p_node)
-            }
-        })
-    }
+    pub unsafe fn remove_tail(
+        _cs: CriticalSection,
+        mut p_list_tail: *mut LinkedListNode,
+    ) -> Option<*mut LinkedListNode> {
+        let list_tail = ptr::read_volatile(p_list_tail);
+
+        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(_cs, p_node);
 
-    pub unsafe fn insert_node_after(mut node: *mut LinkedListNode, mut ref_node: *mut LinkedListNode) {
-        interrupt::free(|_| {
-            (*node).next = (*ref_node).next;
-            (*node).prev = ref_node;
-            (*ref_node).next = node;
-            (*(*node).next).prev = node;
-        });
+            Some(p_node)
+        }
+    }
 
-        todo!("this function has not been converted to volatile semantics");
+    pub unsafe fn insert_node_after(
+        _cs: CriticalSection,
+        mut p_node: *mut LinkedListNode,
+        mut p_ref_node: *mut LinkedListNode,
+    ) {
+        let mut node = ptr::read_volatile(p_node);
+        let mut ref_node = ptr::read_volatile(p_ref_node);
+        let mut prev_node = ptr::read_volatile(ref_node.next);
+
+        node.next = ref_node.next;
+        node.prev = p_ref_node;
+        ref_node.next = p_node;
+        prev_node.prev = p_node;
+
+        ptr::write_volatile(p_node, node);
+        ptr::write_volatile(p_ref_node, ref_node);
+        ptr::write_volatile(node.next, prev_node);
     }
 
-    pub unsafe fn insert_node_before(mut node: *mut LinkedListNode, mut ref_node: *mut LinkedListNode) {
-        interrupt::free(|_| {
-            (*node).next = ref_node;
-            (*node).prev = (*ref_node).prev;
-            (*ref_node).prev = node;
-            (*(*node).prev).next = node;
-        });
+    pub unsafe fn insert_node_before(
+        _cs: CriticalSection,
+        mut node: *mut LinkedListNode,
+        mut ref_node: *mut LinkedListNode,
+    ) {
+        (*node).next = ref_node;
+        (*node).prev = (*ref_node).prev;
+        (*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 {
-        interrupt::free(|_| {
-            let mut size = 0;
-            let mut temp: *mut LinkedListNode = core::ptr::null_mut::<LinkedListNode>();
+    pub unsafe fn get_size(_cs: CriticalSection, mut list_head: *mut LinkedListNode) -> usize {
+        let mut size = 0;
+        let mut temp: *mut LinkedListNode = core::ptr::null_mut::<LinkedListNode>();
 
-            temp = (*list_head).next;
-            while temp != list_head {
-                size += 1;
-                temp = (*temp).next
-            }
+        temp = (*list_head).next;
+        while temp != list_head {
+            size += 1;
+            temp = (*temp).next
+        }
 
-            size
-        });
+        let _ = size;
 
         todo!("this function has not been converted to volatile semantics");
     }
 
-    pub unsafe fn get_next_node(mut p_ref_node: *mut LinkedListNode) -> *mut LinkedListNode {
-        interrupt::free(|_| {
-            let ref_node = ptr::read_volatile(p_ref_node);
+    pub unsafe fn get_next_node(_cs: CriticalSection, mut p_ref_node: *mut LinkedListNode) -> *mut LinkedListNode {
+        let ref_node = ptr::read_volatile(p_ref_node);
 
-            // Allowed because a removed node is not seen by another core
-            ref_node.next
-        })
+        // Allowed because a removed node is not seen by another core
+        ref_node.next
     }
 
-    pub unsafe fn get_prev_node(mut p_ref_node: *mut LinkedListNode) -> *mut LinkedListNode {
-        interrupt::free(|_| {
-            let ref_node = ptr::read_volatile(p_ref_node);
+    pub unsafe fn get_prev_node(_cs: CriticalSection, mut p_ref_node: *mut LinkedListNode) -> *mut LinkedListNode {
+        let ref_node = ptr::read_volatile(p_ref_node);
 
-            // Allowed because a removed node is not seen by another core
-            ref_node.prev
-        })
+        // 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
-        );
+pub struct DebuggableLinkedListNode(*const LinkedListNode);
+
+impl Debug for DebuggableLinkedListNode {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        // Safe because this is just reading memory, and using unaligned to do it
+        let p_node = self.0;
+
+        f.write_fmt(format_args!("iterating list from node: {:x}", p_node as usize))?;
+
+        let mut p_current_node = p_node;
+        for _ in 0..30 {
+            let current_node = unsafe { ptr::read_unaligned(p_current_node) };
+            f.write_fmt(format_args!(
+                "node (prev, current, next): {:x}, {:x}, {:x}",
+                current_node.prev as usize, p_current_node as usize, current_node.next as usize
+            ))?;
+
+            if current_node.next == p_node as *mut _ {
+                break;
+            }
 
-        i += 1;
-        if i > 10 || current_node.next == p_node {
-            break;
+            p_current_node = current_node.next;
         }
 
-        p_current_node = current_node.next;
+        Ok(())
     }
 }