diff --git a/embassy-stm32-wpan/src/ble.rs b/embassy-stm32-wpan/src/ble.rs index 46a2f41c4..d8af861ae 100644 --- a/embassy-stm32-wpan/src/ble.rs +++ b/embassy-stm32-wpan/src/ble.rs @@ -1,6 +1,8 @@ +use core::mem::MaybeUninit; + use embassy_stm32::ipcc::Ipcc; -use crate::cmd::{CmdPacket, CmdSerial}; +use crate::cmd::CmdPacket; use crate::consts::TlPacketType; use crate::evt::EvtBox; use crate::tables::BleTable; @@ -14,6 +16,11 @@ pub struct Ble; impl Ble { pub(super) fn enable() { unsafe { + // Ensure reproducible behavior + BLE_CMD_BUFFER + .as_mut_ptr() + .write_volatile(MaybeUninit::zeroed().assume_init()); + LinkedListNode::init_head(EVT_QUEUE.as_mut_ptr()); TL_BLE_TABLE.as_mut_ptr().write_volatile(BleTable { @@ -29,11 +36,8 @@ impl Ble { pub(super) fn evt_handler() { unsafe { - while !LinkedListNode::is_empty(EVT_QUEUE.as_mut_ptr()) { - let node_ptr = LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr()); - - let event = node_ptr.cast(); - let event = EvtBox::new(event); + while let Some(node_ptr) = LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr()) { + let event = EvtBox::new(node_ptr.cast()); EVT_CHANNEL.try_send(event).unwrap(); } @@ -48,18 +52,11 @@ impl Ble { // TODO: ACL data ack to the user } - pub fn send_cmd(buf: &[u8]) { + pub fn send_cmd(opcode: u16, payload: &[u8]) { debug!("writing ble cmd"); unsafe { - let pcmd_buffer: *mut CmdPacket = BLE_CMD_BUFFER.as_mut_ptr(); - let pcmd_serial: *mut CmdSerial = &mut (*pcmd_buffer).cmdserial; - let pcmd_serial_buf: *mut u8 = pcmd_serial.cast(); - - core::ptr::copy(buf.as_ptr(), pcmd_serial_buf, buf.len()); - - let cmd_packet = &mut *(*TL_REF_TABLE.assume_init().ble_table).pcmd_buffer; - cmd_packet.cmdserial.ty = TlPacketType::BleCmd as u8; + CmdPacket::write_into(BLE_CMD_BUFFER.as_mut_ptr(), TlPacketType::BleCmd, opcode, payload); } Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_BLE_CMD_CHANNEL); diff --git a/embassy-stm32-wpan/src/cmd.rs b/embassy-stm32-wpan/src/cmd.rs index a023201ba..581e5019b 100644 --- a/embassy-stm32-wpan/src/cmd.rs +++ b/embassy-stm32-wpan/src/cmd.rs @@ -1,3 +1,6 @@ +use core::ptr; + +use crate::consts::TlPacketType; use crate::evt::{EvtPacket, EvtSerial}; use crate::{PacketHeader, TL_EVT_HEADER_SIZE}; @@ -42,6 +45,22 @@ pub struct CmdPacket { } impl CmdPacket { + pub unsafe fn write_into(cmd_buf: *mut CmdPacket, packet_type: TlPacketType, cmd_code: u16, payload: &[u8]) { + let p_cmd_serial = &mut (*cmd_buf).cmdserial as *mut _ as *mut CmdSerialStub; + let p_payload = &mut (*cmd_buf).cmdserial.cmd.payload as *mut _; + + ptr::write_volatile( + p_cmd_serial, + CmdSerialStub { + ty: packet_type as u8, + cmd_code: cmd_code, + payload_len: payload.len() as u8, + }, + ); + + ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len()); + } + /// Writes an underlying CmdPacket into the provided buffer. /// Returns a number of bytes that were written. /// Returns an error if event kind is unknown or if provided buffer size is not enough. diff --git a/embassy-stm32-wpan/src/mm.rs b/embassy-stm32-wpan/src/mm.rs index 06063b89a..1ea6edeff 100644 --- a/embassy-stm32-wpan/src/mm.rs +++ b/embassy-stm32-wpan/src/mm.rs @@ -7,7 +7,7 @@ use crate::tables::MemManagerTable; use crate::unsafe_linked_list::LinkedListNode; use crate::{ channels, BLE_SPARE_EVT_BUF, EVT_POOL, FREE_BUF_QUEUE, LOCAL_FREE_BUF_QUEUE, POOL_SIZE, SYS_SPARE_EVT_BUF, - TL_MEM_MANAGER_TABLE, TL_REF_TABLE, + TL_MEM_MANAGER_TABLE, }; pub(super) struct MemoryManager; @@ -51,13 +51,8 @@ impl MemoryManager { /// gives free event buffers back to CPU2 from local buffer queue pub fn send_free_buf() { unsafe { - while !LinkedListNode::is_empty(LOCAL_FREE_BUF_QUEUE.as_mut_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, - ); + 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); } } } diff --git a/embassy-stm32-wpan/src/rc.rs b/embassy-stm32-wpan/src/rc.rs index a217aa224..e67350f79 100644 --- a/embassy-stm32-wpan/src/rc.rs +++ b/embassy-stm32-wpan/src/rc.rs @@ -1,6 +1,6 @@ use crate::ble::Ble; use crate::consts::TlPacketType; -use crate::{shci, TlMbox, STATE}; +use crate::{TlMbox, STATE}; pub struct RadioCoprocessor<'d> { mbox: TlMbox<'d>, @@ -15,12 +15,12 @@ impl<'d> RadioCoprocessor<'d> { } } - pub fn write(&self, buf: &[u8]) { + pub fn write(&self, opcode: u16, buf: &[u8]) { let cmd_code = buf[0]; let cmd = TlPacketType::try_from(cmd_code).unwrap(); match &cmd { - TlPacketType::BleCmd => Ble::send_cmd(buf), + TlPacketType::BleCmd => Ble::send_cmd(opcode, buf), _ => todo!(), } } @@ -30,8 +30,6 @@ impl<'d> RadioCoprocessor<'d> { STATE.wait().await; while let Some(evt) = self.mbox.dequeue_event() { - let event = evt.evt(); - evt.write(&mut self.rx_buf).unwrap(); } diff --git a/embassy-stm32-wpan/src/shci.rs b/embassy-stm32-wpan/src/shci.rs index 6e58a7156..cdf027d5e 100644 --- a/embassy-stm32-wpan/src/shci.rs +++ b/embassy-stm32-wpan/src/shci.rs @@ -1,8 +1,6 @@ use core::{mem, slice}; -use super::cmd::CmdPacket; -use super::consts::TlPacketType; -use super::{sys, TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE, TL_SYS_TABLE}; +use super::{TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE}; pub const SCHI_OPCODE_BLE_INIT: u16 = 0xfc66; diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs index 76f65cbd8..fb247f276 100644 --- a/embassy-stm32-wpan/src/sys.rs +++ b/embassy-stm32-wpan/src/sys.rs @@ -1,9 +1,9 @@ -use core::ptr; +use core::mem::MaybeUninit; use core::sync::atomic::{compiler_fence, Ordering}; use embassy_stm32::ipcc::Ipcc; -use crate::cmd::{CmdPacket, CmdSerial, CmdSerialStub}; +use crate::cmd::{CmdPacket, CmdSerial}; use crate::consts::TlPacketType; use crate::evt::{CcEvt, EvtBox, EvtSerial}; use crate::shci::{ShciBleInitCmdParam, SCHI_OPCODE_BLE_INIT}; @@ -16,6 +16,11 @@ pub struct Sys; impl Sys { pub fn enable() { unsafe { + // Ensure reproducible behavior + SYS_CMD_BUF + .as_mut_ptr() + .write_volatile(MaybeUninit::zeroed().assume_init()); + LinkedListNode::init_head(SYSTEM_EVT_QUEUE.as_mut_ptr()); TL_SYS_TABLE.as_mut_ptr().write_volatile(SysTable { @@ -50,11 +55,8 @@ impl Sys { pub fn evt_handler() { unsafe { - while !LinkedListNode::is_empty(SYSTEM_EVT_QUEUE.as_mut_ptr()) { - let node_ptr = LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr()); - - let event = node_ptr.cast(); - let event = EvtBox::new(event); + while let Some(node_ptr) = LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr()) { + let event = EvtBox::new(node_ptr.cast()); EVT_CHANNEL.try_send(event).unwrap(); } @@ -71,19 +73,7 @@ impl Sys { pub fn send_cmd(opcode: u16, payload: &[u8]) { unsafe { - let p_cmd_serial = &mut (*SYS_CMD_BUF.as_mut_ptr()).cmdserial as *mut _ as *mut CmdSerialStub; - let p_payload = &mut (*SYS_CMD_BUF.as_mut_ptr()).cmdserial.cmd.payload as *mut _; - - ptr::write_volatile( - p_cmd_serial, - CmdSerialStub { - ty: TlPacketType::SysCmd as u8, - cmd_code: opcode, - payload_len: payload.len() as u8, - }, - ); - - ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len()); + CmdPacket::write_into(SYS_CMD_BUF.as_mut_ptr(), TlPacketType::SysCmd, opcode, payload); } compiler_fence(Ordering::SeqCst); diff --git a/embassy-stm32-wpan/src/unsafe_linked_list.rs b/embassy-stm32-wpan/src/unsafe_linked_list.rs index a312178b3..5b210aac4 100644 --- a/embassy-stm32-wpan/src/unsafe_linked_list.rs +++ b/embassy-stm32-wpan/src/unsafe_linked_list.rs @@ -139,28 +139,36 @@ impl LinkedListNode { } /// Remove `list_head` and return a pointer to the `node`. - pub unsafe fn remove_head(mut p_list_head: *mut LinkedListNode) -> *mut LinkedListNode { + 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); - // Allowed because a removed node is not seen by another core - let p_node = list_head.next; - Self::remove_node(p_node); + 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); - 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) -> *mut LinkedListNode { + 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); - // Allowed because a removed node is not seen by another core - let p_node = list_tail.prev; - Self::remove_node(p_node); + 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); - p_node + Some(p_node) + } }) } diff --git a/tests/stm32/src/bin/tl_mbox.rs b/tests/stm32/src/bin/tl_mbox.rs index 5e7d11ef5..5a2309263 100644 --- a/tests/stm32/src/bin/tl_mbox.rs +++ b/tests/stm32/src/bin/tl_mbox.rs @@ -10,6 +10,7 @@ use common::*; use embassy_executor::Spawner; use embassy_stm32::bind_interrupts; use embassy_stm32::ipcc::Config; +use embassy_stm32_wpan::ble::Ble; use embassy_stm32_wpan::rc::RadioCoprocessor; use embassy_stm32_wpan::sys::Sys; use embassy_stm32_wpan::TlMbox; @@ -59,7 +60,9 @@ async fn main(_spawner: Spawner) { Sys::shci_ble_init(Default::default()); - rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]); + // rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]); + Ble::send_cmd(0x0c, &[]); + let response = rc.read().await; info!("ble reset rsp {}", response);