4 files changed, 100 insertions, 58 deletions

diff --git a/embassy-stm32-wpan/src/ble.rs b/embassy-stm32-wpan/src/ble.rs
index ba369bb3a..57348a925 100644
--- a/embassy-stm32-wpan/src/ble.rs
+++ b/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);
diff --git a/embassy-stm32-wpan/src/mm.rs b/embassy-stm32-wpan/src/mm.rs
index 68db49b82..06063b89a 100644
--- a/embassy-stm32-wpan/src/mm.rs
+++ b/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,
diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs
index f41fac58a..0cff5c746 100644
--- a/embassy-stm32-wpan/src/sys.rs
+++ b/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);
diff --git a/embassy-stm32-wpan/src/unsafe_linked_list.rs b/embassy-stm32-wpan/src/unsafe_linked_list.rs
index c4a6c3a72..ddec1afbc 100644
--- a/embassy-stm32-wpan/src/unsafe_linked_list.rs
+++ b/embassy-stm32-wpan/src/unsafe_linked_list.rs
@@ -50,19 +50,33 @@ 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);
-            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);
+            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);
+            }
         });
     }
 
@@ -70,57 +84,84 @@ 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);
-            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);
+            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(|_| {
-            // 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);
-            node_prev.next = node.next;
-            ptr::write_volatile(node.prev, node_prev);
+            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;
 
-            let mut node_next = ptr::read_volatile(node.next);
-            node_next.prev = node.prev;
-            ptr::write_volatile(node.next, node_next);
+                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 p_list_head: *mut LinkedListNode, mut p_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(|_| {
             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(*p_node);
-        });
+            let p_node = list_head.next;
+            Self::remove_node(p_node);
+
+            p_node
+        })
     }
 
-    /// Remove `list_tail` into `node`
-    pub unsafe fn remove_tail(mut p_list_tail: *mut LinkedListNode, mut p_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(|_| {
             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(*p_node);
-        });
+            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) {
@@ -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;
         });
     }
 }