From e8d0b69e8c416cc2b9922acbf277b520d80f3134 Mon Sep 17 00:00:00 2001 From: xoviat Date: Fri, 12 Dec 2025 18:51:36 -0600 Subject: wpan: update aligned, critical_section --- embassy-stm32-wpan/Cargo.toml | 4 +- embassy-stm32-wpan/src/wb55/sub/ble.rs | 4 +- embassy-stm32-wpan/src/wb55/sub/mm.rs | 14 +- embassy-stm32-wpan/src/wb55/sub/sys.rs | 6 +- embassy-stm32-wpan/src/wb55/unsafe_linked_list.rs | 358 +++++++++++----------- 5 files changed, 200 insertions(+), 186 deletions(-) diff --git a/embassy-stm32-wpan/Cargo.toml b/embassy-stm32-wpan/Cargo.toml index 103dedead..9624c7932 100644 --- a/embassy-stm32-wpan/Cargo.toml +++ b/embassy-stm32-wpan/Cargo.toml @@ -41,8 +41,8 @@ log = { version = "0.4.17", optional = true } cortex-m = "0.7.6" heapless = "0.8" -aligned = "0.4.1" -critical-section = "1.1" +aligned = "0.4.2" +critical-section = "1.2" bit_field = "0.10.2" stm32-device-signature = { version = "0.3.3", features = ["stm32wb5x"] } 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.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> { + pub async fn read(&mut self) -> EvtBox> { 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::(); + 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::(); - 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(()) } } -- cgit