1 files changed, 185 insertions, 173 deletions

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(())
     }
 }