stm32/wpan: cleanup linked list and fix edge cases

embassy-stm32-wpan/src/ble.rs

@@ -29,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);

embassy-stm32-wpan/src/mm.rs

@@ -51,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,

embassy-stm32-wpan/src/sys.rs

@@ -45,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);

embassy-stm32-wpan/src/unsafe_linked_list.rs

@@ -50,6 +50,7 @@ impl LinkedListNode {
     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,
@@ -63,6 +64,19 @@ impl LinkedListNode {
                 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);
+            }
         });
     }
 
@@ -70,6 +84,7 @@ impl LinkedListNode {
     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,
@@ -83,44 +98,70 @@ impl LinkedListNode {
                 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(|_| {
-            // Writes must occur sequentially, because if prev node, and next node are the same, both must be updated
             let node = ptr::read_volatile(p_node);
-            let mut node_prev = ptr::read_volatile(node.prev);
+            if node.next != node.prev {
-            node_prev.next = node.next;
-            ptr::write_volatile(node.prev, 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`
+    /// Remove `list_head` and return a pointer to the `node`.
-    pub unsafe fn remove_head(mut p_list_head: *mut LinkedListNode, mut p_node: *mut *mut LinkedListNode) {
+    pub unsafe fn remove_head(mut p_list_head: *mut LinkedListNode) -> *mut LinkedListNode {
         interrupt::free(|_| {
             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;
+            let p_node = list_head.next;
-            Self::remove_node(*p_node);
+            Self::remove_node(p_node);
-        });
+
+            p_node
+        })
     }
 
-    /// Remove `list_tail` into `node`
+    /// Remove `list_tail` and return a pointer to the `node`.
-    pub unsafe fn remove_tail(mut p_list_tail: *mut LinkedListNode, mut p_node: *mut *mut LinkedListNode) {
+    pub unsafe fn remove_tail(mut p_list_tail: *mut LinkedListNode) -> *mut LinkedListNode {
         interrupt::free(|_| {
             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;
+            let p_node = list_tail.prev;
-            Self::remove_node(*p_node);
+            Self::remove_node(p_node);
-        });
+
+            p_node
+        })
     }
 
     pub unsafe fn insert_node_after(mut node: *mut LinkedListNode, mut ref_node: *mut LinkedListNode) {
@@ -130,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) {
@@ -139,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 {
@@ -153,7 +198,9 @@ impl LinkedListNode {
             }
 
             size
-        })
+        });
+
+        todo!("this function has not been converted to volatile semantics");
     }
 
     pub unsafe fn get_next_node(mut p_ref_node: *mut LinkedListNode, mut p_node: *mut *mut LinkedListNode) {
@@ -165,9 +212,12 @@ impl LinkedListNode {
         });
     }
 
-    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 p_node: *mut *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
+            *p_node = ref_node.prev;
         });
     }
 }