stm32/wpan: fix linked list bug

2 files changed, 28 insertions, 17 deletions

diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs
index a7cbbae39..f41fac58a 100644
--- a/embassy-stm32-wpan/src/sys.rs
+++ b/embassy-stm32-wpan/src/sys.rs
@@ -49,14 +49,12 @@ impl Sys {
             let node_ptr_ptr: *mut _ = &mut node_ptr;
 
             while !LinkedListNode::is_empty(SYSTEM_EVT_QUEUE.as_mut_ptr()) {
-                LinkedListNode::get_next_node(SYSTEM_EVT_QUEUE.as_mut_ptr(), node_ptr_ptr);
+                LinkedListNode::remove_head(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 95f4bef44..c4a6c3a72 100644
--- a/embassy-stm32-wpan/src/unsafe_linked_list.rs
+++ b/embassy-stm32-wpan/src/unsafe_linked_list.rs
@@ -87,24 +87,16 @@ impl LinkedListNode {
     }
 
     /// Remove `node` from the linked list
-    //    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(|_| {
+            // 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);
-            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);
+
+            let mut node_next = ptr::read_volatile(node.next);
+            node_next.prev = node.prev;
             ptr::write_volatile(node.next, node_next);
         });
     }
@@ -116,7 +108,7 @@ impl LinkedListNode {
 
             // Allowed because a removed node is not seen by another core
             *p_node = list_head.next;
-            Self::remove_node(list_head.next);
+            Self::remove_node(*p_node);
         });
     }
 
@@ -127,7 +119,7 @@ impl LinkedListNode {
 
             // Allowed because a removed node is not seen by another core
             *p_node = list_tail.prev;
-            Self::remove_node(list_tail.prev);
+            Self::remove_node(*p_node);
         });
     }
 
@@ -179,3 +171,24 @@ impl LinkedListNode {
         });
     }
 }
+
+#[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
+        );
+
+        i += 1;
+        if i > 10 || current_node.next == p_node {
+            break;
+        }
+
+        p_current_node = current_node.next;
+    }
+}