diff --git a/embassy-futures/src/block_on.rs b/embassy-futures/src/block_on.rs index da90351e..77695216 100644 --- a/embassy-futures/src/block_on.rs +++ b/embassy-futures/src/block_on.rs @@ -31,3 +31,15 @@ pub fn block_on(mut fut: F) -> F::Output { } } } + +/// Poll a future once. +pub fn poll_once(mut fut: F) -> Poll { + // safety: we don't move the future after this line. + let mut fut = unsafe { Pin::new_unchecked(&mut fut) }; + + let raw_waker = RawWaker::new(ptr::null(), &VTABLE); + let waker = unsafe { Waker::from_raw(raw_waker) }; + let mut cx = Context::from_waker(&waker); + + fut.as_mut().poll(&mut cx) +} diff --git a/embassy-stm32-wpan/src/ble.rs b/embassy-stm32-wpan/src/ble.rs index 57348a92..f0bd6f48 100644 --- a/embassy-stm32-wpan/src/ble.rs +++ b/embassy-stm32-wpan/src/ble.rs @@ -1,18 +1,20 @@ +use core::marker::PhantomData; + 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; use crate::unsafe_linked_list::LinkedListNode; -use crate::{ - channels, BLE_CMD_BUFFER, CS_BUFFER, EVT_CHANNEL, EVT_QUEUE, HCI_ACL_DATA_BUFFER, TL_BLE_TABLE, TL_REF_TABLE, -}; +use crate::{channels, BLE_CMD_BUFFER, CS_BUFFER, EVT_QUEUE, HCI_ACL_DATA_BUFFER, TL_BLE_TABLE}; -pub struct Ble; +pub struct Ble { + phantom: PhantomData, +} impl Ble { - pub(super) fn enable() { + pub(crate) fn new() -> Self { unsafe { LinkedListNode::init_head(EVT_QUEUE.as_mut_ptr()); @@ -24,54 +26,38 @@ impl Ble { }); } - Ipcc::c1_set_rx_channel(channels::cpu2::IPCC_BLE_EVENT_CHANNEL, true); + Self { phantom: PhantomData } } - - 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); - - EVT_CHANNEL.try_send(event).unwrap(); + /// `HW_IPCC_BLE_EvtNot` + pub async fn read(&self) -> EvtBox { + Ipcc::receive(channels::cpu2::IPCC_BLE_EVENT_CHANNEL, || unsafe { + if let Some(node_ptr) = LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr()) { + Some(EvtBox::new(node_ptr.cast())) + } else { + None } - } - - Ipcc::c1_clear_flag_channel(channels::cpu2::IPCC_BLE_EVENT_CHANNEL); + }) + .await } - pub(super) fn acl_data_handler() { - Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_HCI_ACL_DATA_CHANNEL, false); - - // TODO: ACL data ack to the user + /// `TL_BLE_SendCmd` + pub async fn write(&self, opcode: u16, payload: &[u8]) { + Ipcc::send(channels::cpu1::IPCC_BLE_CMD_CHANNEL, || unsafe { + CmdPacket::write_into(BLE_CMD_BUFFER.as_mut_ptr(), TlPacketType::BleCmd, opcode, payload); + }) + .await; } - pub fn ble_send_cmd(buf: &[u8]) { - debug!("writing ble cmd"); - - unsafe { - let pcmd_buffer: *mut CmdPacket = (*TL_REF_TABLE.assume_init().ble_table).pcmd_buffer; - 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; - } - - Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_BLE_CMD_CHANNEL); - } - - #[allow(dead_code)] // Not used currently but reserved - pub(super) fn ble_send_acl_data() { - let cmd_packet = unsafe { &mut *(*TL_REF_TABLE.assume_init().ble_table).phci_acl_data_buffer }; - - cmd_packet.acl_data_serial.ty = TlPacketType::AclData as u8; - - Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_HCI_ACL_DATA_CHANNEL); - Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_HCI_ACL_DATA_CHANNEL, true); + /// `TL_BLE_SendAclData` + pub async fn acl_write(&self, handle: u16, payload: &[u8]) { + Ipcc::send(channels::cpu1::IPCC_HCI_ACL_DATA_CHANNEL, || unsafe { + CmdPacket::write_into( + HCI_ACL_DATA_BUFFER.as_mut_ptr() as *mut _, + TlPacketType::AclData, + handle, + payload, + ); + }) + .await; } } diff --git a/embassy-stm32-wpan/src/cmd.rs b/embassy-stm32-wpan/src/cmd.rs index 1f7dae7f..edca8239 100644 --- a/embassy-stm32-wpan/src/cmd.rs +++ b/embassy-stm32-wpan/src/cmd.rs @@ -1,5 +1,7 @@ -use crate::evt::{EvtPacket, EvtSerial}; -use crate::{PacketHeader, TL_EVT_HEADER_SIZE}; +use core::ptr; + +use crate::consts::TlPacketType; +use crate::PacketHeader; #[derive(Copy, Clone)] #[repr(C, packed)] @@ -26,6 +28,14 @@ pub struct CmdSerial { pub cmd: Cmd, } +#[derive(Copy, Clone, Default)] +#[repr(C, packed)] +pub struct CmdSerialStub { + pub ty: u8, + pub cmd_code: u16, + pub payload_len: u8, +} + #[derive(Copy, Clone, Default)] #[repr(C, packed)] pub struct CmdPacket { @@ -34,29 +44,20 @@ pub struct CmdPacket { } impl CmdPacket { - /// 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. - #[allow(clippy::result_unit_err)] - pub fn write(&self, buf: &mut [u8]) -> Result { - unsafe { - let cmd_ptr: *const CmdPacket = self; - let self_as_evt_ptr: *const EvtPacket = cmd_ptr.cast(); - let evt_serial: *const EvtSerial = &(*self_as_evt_ptr).evt_serial; + 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 _; - let acl_data: *const AclDataPacket = cmd_ptr.cast(); - let acl_serial: *const AclDataSerial = &(*acl_data).acl_data_serial; - let acl_serial_buf: *const u8 = acl_serial.cast(); + ptr::write_volatile( + p_cmd_serial, + CmdSerialStub { + ty: packet_type as u8, + cmd_code: cmd_code, + payload_len: payload.len() as u8, + }, + ); - let len = (*evt_serial).evt.payload_len as usize + TL_EVT_HEADER_SIZE; - if len > buf.len() { - return Err(()); - } - - core::ptr::copy(acl_serial_buf, buf.as_mut_ptr(), len); - - Ok(len) - } + ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len()); } } @@ -69,9 +70,35 @@ pub struct AclDataSerial { pub acl_data: [u8; 1], } +#[derive(Copy, Clone)] +#[repr(C, packed)] +pub struct AclDataSerialStub { + pub ty: u8, + pub handle: u16, + pub length: u16, +} + #[derive(Copy, Clone)] #[repr(C, packed)] pub struct AclDataPacket { pub header: PacketHeader, pub acl_data_serial: AclDataSerial, } + +impl AclDataPacket { + pub unsafe fn write_into(cmd_buf: *mut AclDataPacket, packet_type: TlPacketType, handle: u16, payload: &[u8]) { + let p_cmd_serial = &mut (*cmd_buf).acl_data_serial as *mut _ as *mut AclDataSerialStub; + let p_payload = &mut (*cmd_buf).acl_data_serial.acl_data as *mut _; + + ptr::write_volatile( + p_cmd_serial, + AclDataSerialStub { + ty: packet_type as u8, + handle: handle, + length: payload.len() as u16, + }, + ); + + ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len()); + } +} diff --git a/embassy-stm32-wpan/src/evt.rs b/embassy-stm32-wpan/src/evt.rs index b53fe506..3a9d0357 100644 --- a/embassy-stm32-wpan/src/evt.rs +++ b/embassy-stm32-wpan/src/evt.rs @@ -1,8 +1,6 @@ -use core::mem::MaybeUninit; +use core::{ptr, slice}; -use super::cmd::{AclDataPacket, AclDataSerial}; -use super::consts::TlPacketType; -use super::{PacketHeader, TL_EVT_HEADER_SIZE}; +use super::PacketHeader; use crate::mm; /** @@ -63,6 +61,12 @@ pub struct EvtSerial { pub evt: Evt, } +#[derive(Copy, Clone, Default)] +pub struct EvtStub { + pub kind: u8, + pub evt_code: u8, +} + /// This format shall be used for all events (asynchronous and command response) reported /// by the CPU2 except for the command response of a system command where the header is not there /// and the format to be used shall be `EvtSerial`. @@ -101,76 +105,91 @@ impl EvtBox { Self { ptr } } - /// copies event data from inner pointer and returns an event structure - pub fn evt(&self) -> EvtPacket { - let mut evt = MaybeUninit::uninit(); + /// Returns information about the event + pub fn stub(&self) -> EvtStub { unsafe { - self.ptr.copy_to(evt.as_mut_ptr(), 1); - evt.assume_init() + let p_evt_stub = &(*self.ptr).evt_serial as *const _ as *const EvtStub; + + ptr::read_volatile(p_evt_stub) } } - /// writes an underlying [`EvtPacket`] into the provided buffer. - /// Returns the number of bytes that were written. - /// Returns an error if event kind is unknown or if provided buffer size is not enough. - #[allow(clippy::result_unit_err)] - pub fn write(&self, buf: &mut [u8]) -> Result { + pub fn payload<'a>(&self) -> &'a [u8] { unsafe { - let evt_kind = TlPacketType::try_from((*self.ptr).evt_serial.kind)?; + let p_payload_len = &(*self.ptr).evt_serial.evt.payload_len as *const u8; + let p_payload = &(*self.ptr).evt_serial.evt.payload as *const u8; - let evt_data: *const EvtPacket = self.ptr.cast(); - let evt_serial: *const EvtSerial = &(*evt_data).evt_serial; - let evt_serial_buf: *const u8 = evt_serial.cast(); + let payload_len = ptr::read_volatile(p_payload_len); - let acl_data: *const AclDataPacket = self.ptr.cast(); - let acl_serial: *const AclDataSerial = &(*acl_data).acl_data_serial; - let acl_serial_buf: *const u8 = acl_serial.cast(); - - if let TlPacketType::AclData = evt_kind { - let len = (*acl_serial).length as usize + 5; - if len > buf.len() { - return Err(()); - } - - core::ptr::copy(evt_serial_buf, buf.as_mut_ptr(), len); - - Ok(len) - } else { - let len = (*evt_serial).evt.payload_len as usize + TL_EVT_HEADER_SIZE; - if len > buf.len() { - return Err(()); - } - - core::ptr::copy(acl_serial_buf, buf.as_mut_ptr(), len); - - Ok(len) - } + slice::from_raw_parts(p_payload, payload_len as usize) } } - /// returns the size of a buffer required to hold this event - #[allow(clippy::result_unit_err)] - pub fn size(&self) -> Result { - unsafe { - let evt_kind = TlPacketType::try_from((*self.ptr).evt_serial.kind)?; + // TODO: bring back acl - let evt_data: *const EvtPacket = self.ptr.cast(); - let evt_serial: *const EvtSerial = &(*evt_data).evt_serial; - - let acl_data: *const AclDataPacket = self.ptr.cast(); - let acl_serial: *const AclDataSerial = &(*acl_data).acl_data_serial; - - if let TlPacketType::AclData = evt_kind { - Ok((*acl_serial).length as usize + 5) - } else { - Ok((*evt_serial).evt.payload_len as usize + TL_EVT_HEADER_SIZE) - } - } - } + // /// writes an underlying [`EvtPacket`] into the provided buffer. + // /// Returns the number of bytes that were written. + // /// Returns an error if event kind is unknown or if provided buffer size is not enough. + // #[allow(clippy::result_unit_err)] + // pub fn write(&self, buf: &mut [u8]) -> Result { + // unsafe { + // let evt_kind = TlPacketType::try_from((*self.ptr).evt_serial.kind)?; + // + // let evt_data: *const EvtPacket = self.ptr.cast(); + // let evt_serial: *const EvtSerial = &(*evt_data).evt_serial; + // let evt_serial_buf: *const u8 = evt_serial.cast(); + // + // let acl_data: *const AclDataPacket = self.ptr.cast(); + // let acl_serial: *const AclDataSerial = &(*acl_data).acl_data_serial; + // let acl_serial_buf: *const u8 = acl_serial.cast(); + // + // if let TlPacketType::AclData = evt_kind { + // let len = (*acl_serial).length as usize + 5; + // if len > buf.len() { + // return Err(()); + // } + // + // core::ptr::copy(evt_serial_buf, buf.as_mut_ptr(), len); + // + // Ok(len) + // } else { + // let len = (*evt_serial).evt.payload_len as usize + TL_EVT_HEADER_SIZE; + // if len > buf.len() { + // return Err(()); + // } + // + // core::ptr::copy(acl_serial_buf, buf.as_mut_ptr(), len); + // + // Ok(len) + // } + // } + // } + // + // /// returns the size of a buffer required to hold this event + // #[allow(clippy::result_unit_err)] + // pub fn size(&self) -> Result { + // unsafe { + // let evt_kind = TlPacketType::try_from((*self.ptr).evt_serial.kind)?; + // + // let evt_data: *const EvtPacket = self.ptr.cast(); + // let evt_serial: *const EvtSerial = &(*evt_data).evt_serial; + // + // let acl_data: *const AclDataPacket = self.ptr.cast(); + // let acl_serial: *const AclDataSerial = &(*acl_data).acl_data_serial; + // + // if let TlPacketType::AclData = evt_kind { + // Ok((*acl_serial).length as usize + 5) + // } else { + // Ok((*evt_serial).evt.payload_len as usize + TL_EVT_HEADER_SIZE) + // } + // } + // } } impl Drop for EvtBox { fn drop(&mut self) { - mm::MemoryManager::evt_drop(self.ptr); + trace!("evt box drop packet"); + + unsafe { mm::MemoryManager::drop_event_packet(self.ptr) }; } } diff --git a/embassy-stm32-wpan/src/lib.rs b/embassy-stm32-wpan/src/lib.rs index 2852d627..833db0df 100644 --- a/embassy-stm32-wpan/src/lib.rs +++ b/embassy-stm32-wpan/src/lib.rs @@ -6,20 +6,21 @@ pub mod fmt; use core::mem::MaybeUninit; use core::sync::atomic::{compiler_fence, Ordering}; +use ble::Ble; use cmd::CmdPacket; -use embassy_futures::block_on; use embassy_hal_common::{into_ref, Peripheral, PeripheralRef}; use embassy_stm32::interrupt; use embassy_stm32::interrupt::typelevel::Interrupt; -use embassy_stm32::ipcc::{Config, Ipcc}; +use embassy_stm32::ipcc::{Config, Ipcc, ReceiveInterruptHandler, TransmitInterruptHandler}; use embassy_stm32::peripherals::IPCC; use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; use embassy_sync::channel::Channel; use embassy_sync::signal::Signal; use evt::{CcEvt, EvtBox}; +use mm::MemoryManager; +use sys::Sys; use tables::{ BleTable, DeviceInfoTable, Mac802_15_4Table, MemManagerTable, RefTable, SysTable, ThreadTable, TracesTable, - WirelessFwInfoTable, }; use unsafe_linked_list::LinkedListNode; @@ -29,50 +30,11 @@ pub mod cmd; pub mod consts; pub mod evt; pub mod mm; -pub mod rc; pub mod shci; pub mod sys; pub mod tables; pub mod unsafe_linked_list; -/// Interrupt handler. -pub struct ReceiveInterruptHandler {} - -impl interrupt::typelevel::Handler for ReceiveInterruptHandler { - unsafe fn on_interrupt() { - if Ipcc::is_rx_pending(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL) { - debug!("RX SYS evt"); - sys::Sys::evt_handler(); - } else if Ipcc::is_rx_pending(channels::cpu2::IPCC_BLE_EVENT_CHANNEL) { - debug!("RX BLE evt"); - ble::Ble::evt_handler(); - } - - STATE.signal(()); - } -} - -pub struct TransmitInterruptHandler {} - -impl interrupt::typelevel::Handler for TransmitInterruptHandler { - unsafe fn on_interrupt() { - if Ipcc::is_tx_pending(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL) { - debug!("TX SYS cmd rsp"); - let cc = sys::Sys::cmd_evt_handler(); - - LAST_CC_EVT.signal(cc); - } else if Ipcc::is_tx_pending(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL) { - debug!("TX MM release"); - mm::MemoryManager::free_buf_handler(); - } else if Ipcc::is_tx_pending(channels::cpu1::IPCC_HCI_ACL_DATA_CHANNEL) { - debug!("TX HCI acl"); - ble::Ble::acl_data_handler(); - } - - STATE.signal(()); - } -} - #[link_section = "TL_REF_TABLE"] pub static mut TL_REF_TABLE: MaybeUninit = MaybeUninit::uninit(); @@ -167,10 +129,14 @@ static mut BLE_CMD_BUFFER: MaybeUninit = MaybeUninit::uninit(); // fuck these "magic" numbers from ST ---v---v static mut HCI_ACL_DATA_BUFFER: MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + 5 + 251]> = MaybeUninit::uninit(); +// TODO: remove these items + +#[allow(dead_code)] /// current event that is produced during IPCC IRQ handler execution /// on SYS channel static EVT_CHANNEL: Channel = Channel::new(); +#[allow(dead_code)] /// last received Command Complete event static LAST_CC_EVT: Signal = Signal::new(); @@ -178,6 +144,10 @@ static STATE: Signal = Signal::new(); pub struct TlMbox<'d> { _ipcc: PeripheralRef<'d, IPCC>, + + pub sys_subsystem: Sys, + pub mm_subsystem: MemoryManager, + pub ble_subsystem: Ble, } impl<'d> TlMbox<'d> { @@ -262,9 +232,9 @@ impl<'d> TlMbox<'d> { Ipcc::enable(config); - sys::Sys::enable(); - ble::Ble::enable(); - mm::MemoryManager::enable(); + let sys = sys::Sys::new(); + let ble = ble::Ble::new(); + let mm = mm::MemoryManager::new(); // enable interrupts interrupt::typelevel::IPCC_C1_RX::unpend(); @@ -275,36 +245,11 @@ impl<'d> TlMbox<'d> { STATE.reset(); - Self { _ipcc: ipcc } - } - - /// Returns CPU2 wireless firmware information (if present). - pub fn wireless_fw_info(&self) -> Option { - let info = unsafe { &(*(*TL_REF_TABLE.as_ptr()).device_info_table).wireless_fw_info_table }; - - // Zero version indicates that CPU2 wasn't active and didn't fill the information table - if info.version != 0 { - Some(*info) - } else { - None - } - } - - /// picks single [`EvtBox`] from internal event queue. - /// - /// Internal event queu is populated in IPCC_RX_IRQ handler - pub fn dequeue_event(&mut self) -> Option { - EVT_CHANNEL.try_recv().ok() - } - - /// retrieves last Command Complete event and removes it from mailbox - pub fn pop_last_cc_evt(&mut self) -> Option { - if LAST_CC_EVT.signaled() { - let cc = block_on(LAST_CC_EVT.wait()); - LAST_CC_EVT.reset(); - Some(cc) - } else { - None + Self { + _ipcc: ipcc, + sys_subsystem: sys, + ble_subsystem: ble, + mm_subsystem: mm, } } } diff --git a/embassy-stm32-wpan/src/mm.rs b/embassy-stm32-wpan/src/mm.rs index 06063b89..21f42409 100644 --- a/embassy-stm32-wpan/src/mm.rs +++ b/embassy-stm32-wpan/src/mm.rs @@ -1,19 +1,29 @@ //! Memory manager routines +use core::future::poll_fn; +use core::marker::PhantomData; +use core::task::Poll; + +use cortex_m::interrupt; use embassy_stm32::ipcc::Ipcc; +use embassy_sync::waitqueue::AtomicWaker; use crate::evt::EvtPacket; 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; +static MM_WAKER: AtomicWaker = AtomicWaker::new(); + +pub struct MemoryManager { + phantom: PhantomData, +} impl MemoryManager { - pub fn enable() { + pub(crate) fn new() -> Self { unsafe { LinkedListNode::init_head(FREE_BUF_QUEUE.as_mut_ptr()); LinkedListNode::init_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr()); @@ -28,44 +38,40 @@ impl MemoryManager { tracespoolsize: 0, }); } + + Self { phantom: PhantomData } } - pub fn evt_drop(evt: *mut EvtPacket) { - unsafe { - let list_node = evt.cast(); + /// SAFETY: passing a pointer to something other than an event packet is UB + pub(crate) unsafe fn drop_event_packet(evt: *mut EvtPacket) { + interrupt::free(|_| unsafe { + LinkedListNode::insert_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), evt as *mut _); + }); - LinkedListNode::insert_tail(LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), list_node); + MM_WAKER.wake(); + } - let channel_is_busy = Ipcc::c1_is_active_flag(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL); + pub async fn run_queue(&self) { + loop { + poll_fn(|cx| unsafe { + MM_WAKER.register(cx.waker()); + if LinkedListNode::is_empty(LOCAL_FREE_BUF_QUEUE.as_mut_ptr()) { + Poll::Pending + } else { + Poll::Ready(()) + } + }) + .await; - // postpone event buffer freeing to IPCC interrupt handler - if channel_is_busy { - Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL, true); - } else { - Self::send_free_buf(); - Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL); - } + Ipcc::send(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL, || { + 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); + } + }) + }) + .await; } } - - /// 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, - ); - } - } - } - - /// free buffer channel interrupt handler - pub fn free_buf_handler() { - Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL, false); - Self::send_free_buf(); - Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL); - } } diff --git a/embassy-stm32-wpan/src/rc.rs b/embassy-stm32-wpan/src/rc.rs deleted file mode 100644 index aae2265e..00000000 --- a/embassy-stm32-wpan/src/rc.rs +++ /dev/null @@ -1,50 +0,0 @@ -use crate::ble::Ble; -use crate::consts::TlPacketType; -use crate::{shci, TlMbox, STATE}; - -pub struct RadioCoprocessor<'d> { - mbox: TlMbox<'d>, - rx_buf: [u8; 500], -} - -impl<'d> RadioCoprocessor<'d> { - pub fn new(mbox: TlMbox<'d>) -> Self { - Self { - mbox, - rx_buf: [0u8; 500], - } - } - - pub fn write(&self, buf: &[u8]) { - let cmd_code = buf[0]; - let cmd = TlPacketType::try_from(cmd_code).unwrap(); - - match &cmd { - TlPacketType::BleCmd => Ble::ble_send_cmd(buf), - _ => todo!(), - } - } - - pub async fn read(&mut self) -> &[u8] { - loop { - STATE.wait().await; - - while let Some(evt) = self.mbox.dequeue_event() { - let event = evt.evt(); - - evt.write(&mut self.rx_buf).unwrap(); - - if event.kind() == 18 { - shci::shci_ble_init(Default::default()); - self.rx_buf[0] = 0x04; - } - } - - if self.mbox.pop_last_cc_evt().is_some() { - continue; - } - - return &self.rx_buf; - } - } -} diff --git a/embassy-stm32-wpan/src/shci.rs b/embassy-stm32-wpan/src/shci.rs index 8537995f..cdf027d5 100644 --- a/embassy-stm32-wpan/src/shci.rs +++ b/embassy-stm32-wpan/src/shci.rs @@ -1,8 +1,8 @@ -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 core::{mem, slice}; -const SCHI_OPCODE_BLE_INIT: u16 = 0xfc66; +use super::{TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE}; + +pub const SCHI_OPCODE_BLE_INIT: u16 = 0xfc66; #[derive(Debug, Clone, Copy)] #[repr(C, packed)] @@ -32,6 +32,12 @@ pub struct ShciBleInitCmdParam { pub hw_version: u8, } +impl ShciBleInitCmdParam { + pub fn payload<'a>(&self) -> &'a [u8] { + unsafe { slice::from_raw_parts(self as *const _ as *const u8, mem::size_of::()) } + } +} + impl Default for ShciBleInitCmdParam { fn default() -> Self { Self { @@ -66,35 +72,10 @@ pub struct ShciHeader { #[derive(Debug, Clone, Copy)] #[repr(C, packed)] pub struct ShciBleInitCmdPacket { - header: ShciHeader, - param: ShciBleInitCmdParam, + pub header: ShciHeader, + pub param: ShciBleInitCmdParam, } pub const TL_BLE_EVT_CS_PACKET_SIZE: usize = TL_EVT_HEADER_SIZE + TL_CS_EVT_SIZE; #[allow(dead_code)] // Not used currently but reserved const TL_BLE_EVT_CS_BUFFER_SIZE: usize = TL_PACKET_HEADER_SIZE + TL_BLE_EVT_CS_PACKET_SIZE; - -pub fn shci_ble_init(param: ShciBleInitCmdParam) { - debug!("sending SHCI"); - - let mut packet = ShciBleInitCmdPacket { - header: ShciHeader::default(), - param, - }; - - let packet_ptr: *mut _ = &mut packet; - - unsafe { - let cmd_ptr: *mut CmdPacket = packet_ptr.cast(); - - (*cmd_ptr).cmdserial.cmd.cmd_code = SCHI_OPCODE_BLE_INIT; - (*cmd_ptr).cmdserial.cmd.payload_len = core::mem::size_of::() as u8; - - let p_cmd_buffer = &mut *(*TL_SYS_TABLE.as_mut_ptr()).pcmd_buffer; - core::ptr::write(p_cmd_buffer, *cmd_ptr); - - p_cmd_buffer.cmdserial.ty = TlPacketType::SysCmd as u8; - - sys::Sys::send_cmd(); - } -} diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs index 0cff5c74..a185cd4f 100644 --- a/embassy-stm32-wpan/src/sys.rs +++ b/embassy-stm32-wpan/src/sys.rs @@ -1,65 +1,70 @@ -use embassy_stm32::ipcc::Ipcc; +use core::marker::PhantomData; -use crate::cmd::{CmdPacket, CmdSerial}; -use crate::evt::{CcEvt, EvtBox, EvtSerial}; -use crate::tables::SysTable; +use crate::cmd::CmdPacket; +use crate::consts::TlPacketType; +use crate::evt::EvtBox; +use crate::shci::{ShciBleInitCmdParam, SCHI_OPCODE_BLE_INIT}; +use crate::tables::{SysTable, WirelessFwInfoTable}; use crate::unsafe_linked_list::LinkedListNode; -use crate::{channels, EVT_CHANNEL, SYSTEM_EVT_QUEUE, SYS_CMD_BUF, TL_SYS_TABLE}; +use crate::{channels, Ipcc, SYSTEM_EVT_QUEUE, SYS_CMD_BUF, TL_DEVICE_INFO_TABLE, TL_SYS_TABLE}; -pub struct Sys; +pub struct Sys { + phantom: PhantomData, +} impl Sys { - pub fn enable() { + /// TL_Sys_Init + pub(crate) fn new() -> Self { unsafe { LinkedListNode::init_head(SYSTEM_EVT_QUEUE.as_mut_ptr()); TL_SYS_TABLE.as_mut_ptr().write_volatile(SysTable { pcmd_buffer: SYS_CMD_BUF.as_mut_ptr(), sys_queue: SYSTEM_EVT_QUEUE.as_ptr(), - }) + }); } - Ipcc::c1_set_rx_channel(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL, true); + Self { phantom: PhantomData } } - pub fn cmd_evt_handler() -> CcEvt { - Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL, false); + /// Returns CPU2 wireless firmware information (if present). + pub fn wireless_fw_info(&self) -> Option { + let info = unsafe { TL_DEVICE_INFO_TABLE.as_mut_ptr().read_volatile().wireless_fw_info_table }; - // ST's command response data structure is really convoluted. - // - // for command response events on SYS channel, the header is missing - // and one should: - // 1. interpret the content of CMD_BUFFER as CmdPacket - // 2. Access CmdPacket's cmdserial field and interpret its content as EvtSerial - // 3. Access EvtSerial's evt field (as Evt) and interpret its payload as CcEvt - // 4. CcEvt type is the actual SHCI response - // 5. profit - unsafe { - let pcmd: *const CmdPacket = (*TL_SYS_TABLE.as_ptr()).pcmd_buffer; - let cmd_serial: *const CmdSerial = &(*pcmd).cmdserial; - let evt_serial: *const EvtSerial = cmd_serial.cast(); - let cc: *const CcEvt = (*evt_serial).evt.payload.as_ptr().cast(); - *cc + // Zero version indicates that CPU2 wasn't active and didn't fill the information table + if info.version != 0 { + Some(info) + } else { + None } } - pub fn evt_handler() { + pub fn write(&self, opcode: u16, payload: &[u8]) { unsafe { - while !LinkedListNode::is_empty(SYSTEM_EVT_QUEUE.as_mut_ptr()) { - let node_ptr = LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr()); + CmdPacket::write_into(SYS_CMD_BUF.as_mut_ptr(), TlPacketType::SysCmd, opcode, payload); + } + } - let event = node_ptr.cast(); - let event = EvtBox::new(event); + pub async fn shci_c2_ble_init(&self, param: ShciBleInitCmdParam) { + debug!("sending SHCI"); - EVT_CHANNEL.try_send(event).unwrap(); + Ipcc::send(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL, || { + self.write(SCHI_OPCODE_BLE_INIT, param.payload()); + }) + .await; + + Ipcc::flush(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL).await; + } + + /// `HW_IPCC_SYS_EvtNot` + pub async fn read(&self) -> EvtBox { + Ipcc::receive(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL, || unsafe { + if let Some(node_ptr) = LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr()) { + Some(EvtBox::new(node_ptr.cast())) + } else { + None } - } - - Ipcc::c1_clear_flag_channel(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL); - } - - pub fn send_cmd() { - Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL); - Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL, true); + }) + .await } } diff --git a/embassy-stm32-wpan/src/unsafe_linked_list.rs b/embassy-stm32-wpan/src/unsafe_linked_list.rs index a312178b..d8bc2976 100644 --- a/embassy-stm32-wpan/src/unsafe_linked_list.rs +++ b/embassy-stm32-wpan/src/unsafe_linked_list.rs @@ -117,7 +117,12 @@ impl LinkedListNode { /// Remove `node` from the linked list pub unsafe fn remove_node(mut p_node: *mut LinkedListNode) { interrupt::free(|_| { - let node = ptr::read_volatile(p_node); + // 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); @@ -139,28 +144,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/embassy-stm32/src/ipcc.rs b/embassy-stm32/src/ipcc.rs index 8bb0774b..609c4d2c 100644 --- a/embassy-stm32/src/ipcc.rs +++ b/embassy-stm32/src/ipcc.rs @@ -1,7 +1,77 @@ +use core::future::poll_fn; +use core::task::Poll; + +use atomic_polyfill::{compiler_fence, Ordering}; + use self::sealed::Instance; +use crate::interrupt; +use crate::interrupt::typelevel::Interrupt; use crate::peripherals::IPCC; use crate::rcc::sealed::RccPeripheral; +/// Interrupt handler. +pub struct ReceiveInterruptHandler {} + +impl interrupt::typelevel::Handler for ReceiveInterruptHandler { + unsafe fn on_interrupt() { + let regs = IPCC::regs(); + + let channels = [ + IpccChannel::Channel1, + IpccChannel::Channel2, + IpccChannel::Channel3, + IpccChannel::Channel4, + IpccChannel::Channel5, + IpccChannel::Channel6, + ]; + + // Status register gives channel occupied status. For rx, use cpu1. + let sr = unsafe { regs.cpu(1).sr().read() }; + regs.cpu(0).mr().modify(|w| { + for channel in channels { + if sr.chf(channel as usize) { + // If bit is set to 1 then interrupt is disabled; we want to disable the interrupt + w.set_chom(channel as usize, true); + + // There shouldn't be a race because the channel is masked only if the interrupt has fired + IPCC::state().rx_waker_for(channel).wake(); + } + } + }) + } +} + +pub struct TransmitInterruptHandler {} + +impl interrupt::typelevel::Handler for TransmitInterruptHandler { + unsafe fn on_interrupt() { + let regs = IPCC::regs(); + + let channels = [ + IpccChannel::Channel1, + IpccChannel::Channel2, + IpccChannel::Channel3, + IpccChannel::Channel4, + IpccChannel::Channel5, + IpccChannel::Channel6, + ]; + + // Status register gives channel occupied status. For tx, use cpu0. + let sr = unsafe { regs.cpu(0).sr().read() }; + regs.cpu(0).mr().modify(|w| { + for channel in channels { + if !sr.chf(channel as usize) { + // If bit is set to 1 then interrupt is disabled; we want to disable the interrupt + w.set_chfm(channel as usize, true); + + // There shouldn't be a race because the channel is masked only if the interrupt has fired + IPCC::state().tx_waker_for(channel).wake(); + } + } + }); + } +} + #[non_exhaustive] #[derive(Clone, Copy, Default)] pub struct Config { @@ -20,13 +90,6 @@ pub enum IpccChannel { Channel6 = 5, } -pub mod sealed { - pub trait Instance: crate::rcc::RccPeripheral { - fn regs() -> crate::pac::ipcc::Ipcc; - fn set_cpu2(enabled: bool); - } -} - pub struct Ipcc; impl Ipcc { @@ -45,114 +108,99 @@ impl Ipcc { w.set_txfie(true); }) } + + // enable interrupts + crate::interrupt::typelevel::IPCC_C1_RX::unpend(); + crate::interrupt::typelevel::IPCC_C1_TX::unpend(); + + unsafe { crate::interrupt::typelevel::IPCC_C1_RX::enable() }; + unsafe { crate::interrupt::typelevel::IPCC_C1_TX::enable() }; } - pub fn c1_set_rx_channel(channel: IpccChannel, enabled: bool) { + /// Send data to an IPCC channel. The closure is called to write the data when appropriate. + pub async fn send(channel: IpccChannel, f: impl FnOnce()) { let regs = IPCC::regs(); - // If bit is set to 1 then interrupt is disabled - unsafe { regs.cpu(0).mr().modify(|w| w.set_chom(channel as usize, !enabled)) } - } + Self::flush(channel).await; + compiler_fence(Ordering::SeqCst); - pub fn c1_get_rx_channel(channel: IpccChannel) -> bool { - let regs = IPCC::regs(); + f(); - // If bit is set to 1 then interrupt is disabled - unsafe { !regs.cpu(0).mr().read().chom(channel as usize) } - } - - #[allow(dead_code)] - pub fn c2_set_rx_channel(channel: IpccChannel, enabled: bool) { - let regs = IPCC::regs(); - - // If bit is set to 1 then interrupt is disabled - unsafe { regs.cpu(1).mr().modify(|w| w.set_chom(channel as usize, !enabled)) } - } - - #[allow(dead_code)] - pub fn c2_get_rx_channel(channel: IpccChannel) -> bool { - let regs = IPCC::regs(); - - // If bit is set to 1 then interrupt is disabled - unsafe { !regs.cpu(1).mr().read().chom(channel as usize) } - } - - pub fn c1_set_tx_channel(channel: IpccChannel, enabled: bool) { - let regs = IPCC::regs(); - - // If bit is set to 1 then interrupt is disabled - unsafe { regs.cpu(0).mr().modify(|w| w.set_chfm(channel as usize, !enabled)) } - } - - pub fn c1_get_tx_channel(channel: IpccChannel) -> bool { - let regs = IPCC::regs(); - - // If bit is set to 1 then interrupt is disabled - unsafe { !regs.cpu(0).mr().read().chfm(channel as usize) } - } - - #[allow(dead_code)] - pub fn c2_set_tx_channel(channel: IpccChannel, enabled: bool) { - let regs = IPCC::regs(); - - // If bit is set to 1 then interrupt is disabled - unsafe { regs.cpu(1).mr().modify(|w| w.set_chfm(channel as usize, !enabled)) } - } - - #[allow(dead_code)] - pub fn c2_get_tx_channel(channel: IpccChannel) -> bool { - let regs = IPCC::regs(); - - // If bit is set to 1 then interrupt is disabled - unsafe { !regs.cpu(1).mr().read().chfm(channel as usize) } - } - - /// clears IPCC receive channel status for CPU1 - pub fn c1_clear_flag_channel(channel: IpccChannel) { - let regs = IPCC::regs(); - - unsafe { regs.cpu(0).scr().write(|w| w.set_chc(channel as usize, true)) } - } - - #[allow(dead_code)] - /// clears IPCC receive channel status for CPU2 - pub fn c2_clear_flag_channel(channel: IpccChannel) { - let regs = IPCC::regs(); - - unsafe { regs.cpu(1).scr().write(|w| w.set_chc(channel as usize, true)) } - } - - pub fn c1_set_flag_channel(channel: IpccChannel) { - let regs = IPCC::regs(); + compiler_fence(Ordering::SeqCst); + trace!("ipcc: ch {}: send data", channel as u8); unsafe { regs.cpu(0).scr().write(|w| w.set_chs(channel as usize, true)) } } - #[allow(dead_code)] - pub fn c2_set_flag_channel(channel: IpccChannel) { + /// Wait for the tx channel to become clear + pub async fn flush(channel: IpccChannel) { let regs = IPCC::regs(); - unsafe { regs.cpu(1).scr().write(|w| w.set_chs(channel as usize, true)) } + // This is a race, but is nice for debugging + if unsafe { regs.cpu(0).sr().read() }.chf(channel as usize) { + trace!("ipcc: ch {}: wait for tx free", channel as u8); + } + + poll_fn(|cx| { + IPCC::state().tx_waker_for(channel).register(cx.waker()); + // If bit is set to 1 then interrupt is disabled; we want to enable the interrupt + unsafe { regs.cpu(0).mr().modify(|w| w.set_chfm(channel as usize, false)) } + + compiler_fence(Ordering::SeqCst); + + if !unsafe { regs.cpu(0).sr().read() }.chf(channel as usize) { + // If bit is set to 1 then interrupt is disabled; we want to disable the interrupt + unsafe { regs.cpu(0).mr().modify(|w| w.set_chfm(channel as usize, true)) } + + Poll::Ready(()) + } else { + Poll::Pending + } + }) + .await; } - pub fn c1_is_active_flag(channel: IpccChannel) -> bool { + /// Receive data from an IPCC channel. The closure is called to read the data when appropriate. + pub async fn receive(channel: IpccChannel, mut f: impl FnMut() -> Option) -> R { let regs = IPCC::regs(); - unsafe { regs.cpu(0).sr().read().chf(channel as usize) } - } + loop { + // This is a race, but is nice for debugging + if !unsafe { regs.cpu(1).sr().read() }.chf(channel as usize) { + trace!("ipcc: ch {}: wait for rx occupied", channel as u8); + } - pub fn c2_is_active_flag(channel: IpccChannel) -> bool { - let regs = IPCC::regs(); + poll_fn(|cx| { + IPCC::state().rx_waker_for(channel).register(cx.waker()); + // If bit is set to 1 then interrupt is disabled; we want to enable the interrupt + unsafe { regs.cpu(0).mr().modify(|w| w.set_chom(channel as usize, false)) } - unsafe { regs.cpu(1).sr().read().chf(channel as usize) } - } + compiler_fence(Ordering::SeqCst); - pub fn is_tx_pending(channel: IpccChannel) -> bool { - !Self::c1_is_active_flag(channel) && Self::c1_get_tx_channel(channel) - } + if unsafe { regs.cpu(1).sr().read() }.chf(channel as usize) { + // If bit is set to 1 then interrupt is disabled; we want to disable the interrupt + unsafe { regs.cpu(0).mr().modify(|w| w.set_chfm(channel as usize, true)) } - pub fn is_rx_pending(channel: IpccChannel) -> bool { - Self::c2_is_active_flag(channel) && Self::c1_get_rx_channel(channel) + Poll::Ready(()) + } else { + Poll::Pending + } + }) + .await; + + trace!("ipcc: ch {}: read data", channel as u8); + compiler_fence(Ordering::SeqCst); + + match f() { + Some(ret) => return ret, + None => {} + } + + trace!("ipcc: ch {}: clear rx", channel as u8); + compiler_fence(Ordering::SeqCst); + // If the channel is clear and the read function returns none, fetch more data + unsafe { regs.cpu(0).scr().write(|w| w.set_chc(channel as usize, true)) } + } } } @@ -164,9 +212,66 @@ impl sealed::Instance for crate::peripherals::IPCC { fn set_cpu2(enabled: bool) { unsafe { crate::pac::PWR.cr4().modify(|w| w.set_c2boot(enabled)) } } + + fn state() -> &'static self::sealed::State { + static STATE: self::sealed::State = self::sealed::State::new(); + &STATE + } +} + +pub(crate) mod sealed { + use embassy_sync::waitqueue::AtomicWaker; + + use super::*; + + pub struct State { + rx_wakers: [AtomicWaker; 6], + tx_wakers: [AtomicWaker; 6], + } + + impl State { + pub const fn new() -> Self { + const WAKER: AtomicWaker = AtomicWaker::new(); + + Self { + rx_wakers: [WAKER; 6], + tx_wakers: [WAKER; 6], + } + } + + pub fn rx_waker_for(&self, channel: IpccChannel) -> &AtomicWaker { + match channel { + IpccChannel::Channel1 => &self.rx_wakers[0], + IpccChannel::Channel2 => &self.rx_wakers[1], + IpccChannel::Channel3 => &self.rx_wakers[2], + IpccChannel::Channel4 => &self.rx_wakers[3], + IpccChannel::Channel5 => &self.rx_wakers[4], + IpccChannel::Channel6 => &self.rx_wakers[5], + } + } + + pub fn tx_waker_for(&self, channel: IpccChannel) -> &AtomicWaker { + match channel { + IpccChannel::Channel1 => &self.tx_wakers[0], + IpccChannel::Channel2 => &self.tx_wakers[1], + IpccChannel::Channel3 => &self.tx_wakers[2], + IpccChannel::Channel4 => &self.tx_wakers[3], + IpccChannel::Channel5 => &self.tx_wakers[4], + IpccChannel::Channel6 => &self.tx_wakers[5], + } + } + } + + pub trait Instance: crate::rcc::RccPeripheral { + fn regs() -> crate::pac::ipcc::Ipcc; + fn set_cpu2(enabled: bool); + fn state() -> &'static State; + } } unsafe fn _configure_pwr() { + // TODO: move this to RCC + let pwr = crate::pac::PWR; let rcc = crate::pac::RCC; diff --git a/examples/stm32wb/src/bin/tl_mbox.rs b/examples/stm32wb/src/bin/tl_mbox.rs index ae36a7e7..9fc4b8aa 100644 --- a/examples/stm32wb/src/bin/tl_mbox.rs +++ b/examples/stm32wb/src/bin/tl_mbox.rs @@ -5,14 +5,14 @@ use defmt::*; use embassy_executor::Spawner; use embassy_stm32::bind_interrupts; -use embassy_stm32::ipcc::Config; +use embassy_stm32::ipcc::{Config, ReceiveInterruptHandler, TransmitInterruptHandler}; use embassy_stm32_wpan::TlMbox; use embassy_time::{Duration, Timer}; use {defmt_rtt as _, panic_probe as _}; bind_interrupts!(struct Irqs{ - IPCC_C1_RX => embassy_stm32_wpan::ReceiveInterruptHandler; - IPCC_C1_TX => embassy_stm32_wpan::TransmitInterruptHandler; + IPCC_C1_RX => ReceiveInterruptHandler; + IPCC_C1_TX => TransmitInterruptHandler; }); #[embassy_executor::main] @@ -48,7 +48,7 @@ async fn main(_spawner: Spawner) { let mbox = TlMbox::init(p.IPCC, Irqs, config); loop { - let wireless_fw_info = mbox.wireless_fw_info(); + let wireless_fw_info = mbox.sys_subsystem.wireless_fw_info(); match wireless_fw_info { None => info!("not yet initialized"), Some(fw_info) => { diff --git a/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs b/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs index 3132ab3e..439bd01a 100644 --- a/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs +++ b/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs @@ -5,14 +5,13 @@ use defmt::*; use embassy_executor::Spawner; use embassy_stm32::bind_interrupts; -use embassy_stm32::ipcc::Config; -use embassy_stm32_wpan::rc::RadioCoprocessor; +use embassy_stm32::ipcc::{Config, ReceiveInterruptHandler, TransmitInterruptHandler}; use embassy_stm32_wpan::TlMbox; use {defmt_rtt as _, panic_probe as _}; bind_interrupts!(struct Irqs{ - IPCC_C1_RX => embassy_stm32_wpan::ReceiveInterruptHandler; - IPCC_C1_TX => embassy_stm32_wpan::TransmitInterruptHandler; + IPCC_C1_RX => ReceiveInterruptHandler; + IPCC_C1_TX => TransmitInterruptHandler; }); #[embassy_executor::main] @@ -47,14 +46,18 @@ async fn main(_spawner: Spawner) { let config = Config::default(); let mbox = TlMbox::init(p.IPCC, Irqs, config); - let mut rc = RadioCoprocessor::new(mbox); + let sys_event = mbox.sys_subsystem.read().await; + info!("sys event: {}", sys_event.payload()); - let response = rc.read().await; - info!("coprocessor ready {}", response); + mbox.sys_subsystem.shci_c2_ble_init(Default::default()).await; - rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]); - let response = rc.read().await; - info!("ble reset rsp {}", response); + info!("starting ble..."); + mbox.ble_subsystem.write(0x0c, &[]).await; + + info!("waiting for ble..."); + let ble_event = mbox.ble_subsystem.read().await; + + info!("ble event: {}", ble_event.payload()); info!("Test OK"); cortex_m::asm::bkpt(); diff --git a/tests/stm32/src/bin/tl_mbox.rs b/tests/stm32/src/bin/tl_mbox.rs index 4669cbc6..f6641ae3 100644 --- a/tests/stm32/src/bin/tl_mbox.rs +++ b/tests/stm32/src/bin/tl_mbox.rs @@ -6,60 +6,70 @@ #[path = "../common.rs"] mod common; +use core::mem; + use common::*; use embassy_executor::Spawner; +use embassy_futures::poll_once; use embassy_stm32::bind_interrupts; -use embassy_stm32::ipcc::Config; -use embassy_stm32_wpan::rc::RadioCoprocessor; -use embassy_stm32_wpan::TlMbox; +use embassy_stm32::ipcc::{Config, ReceiveInterruptHandler, TransmitInterruptHandler}; +use embassy_stm32_wpan::{mm, TlMbox}; use embassy_time::{Duration, Timer}; bind_interrupts!(struct Irqs{ - IPCC_C1_RX => embassy_stm32_wpan::ReceiveInterruptHandler; - IPCC_C1_TX => embassy_stm32_wpan::TransmitInterruptHandler; + IPCC_C1_RX => ReceiveInterruptHandler; + IPCC_C1_TX => TransmitInterruptHandler; }); +#[embassy_executor::task] +async fn run_mm_queue(memory_manager: mm::MemoryManager) { + memory_manager.run_queue().await; +} + #[embassy_executor::main] -async fn main(_spawner: Spawner) { +async fn main(spawner: Spawner) { let p = embassy_stm32::init(config()); info!("Hello World!"); let config = Config::default(); let mbox = TlMbox::init(p.IPCC, Irqs, config); - loop { - let wireless_fw_info = mbox.wireless_fw_info(); - match wireless_fw_info { - None => {} - Some(fw_info) => { - let version_major = fw_info.version_major(); - let version_minor = fw_info.version_minor(); - let subversion = fw_info.subversion(); + spawner.spawn(run_mm_queue(mbox.mm_subsystem)).unwrap(); - let sram2a_size = fw_info.sram2a_size(); - let sram2b_size = fw_info.sram2b_size(); + let ready_event = mbox.sys_subsystem.read().await; + let _ = poll_once(mbox.sys_subsystem.read()); // clear rx not - info!( - "version {}.{}.{} - SRAM2a {} - SRAM2b {}", - version_major, version_minor, subversion, sram2a_size, sram2b_size - ); + info!("coprocessor ready {}", ready_event.payload()); - break; - } - } + // test memory manager + mem::drop(ready_event); - Timer::after(Duration::from_millis(50)).await; - } + let fw_info = mbox.sys_subsystem.wireless_fw_info().unwrap(); + let version_major = fw_info.version_major(); + let version_minor = fw_info.version_minor(); + let subversion = fw_info.subversion(); - let mut rc = RadioCoprocessor::new(mbox); + let sram2a_size = fw_info.sram2a_size(); + let sram2b_size = fw_info.sram2b_size(); - let response = rc.read().await; - info!("coprocessor ready {}", response); + info!( + "version {}.{}.{} - SRAM2a {} - SRAM2b {}", + version_major, version_minor, subversion, sram2a_size, sram2b_size + ); - rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]); - let response = rc.read().await; - info!("ble reset rsp {}", response); + Timer::after(Duration::from_millis(50)).await; + mbox.sys_subsystem.shci_c2_ble_init(Default::default()).await; + + info!("starting ble..."); + mbox.ble_subsystem.write(0x0c, &[]).await; + + info!("waiting for ble..."); + let ble_event = mbox.ble_subsystem.read().await; + + info!("ble event: {}", ble_event.payload()); + + Timer::after(Duration::from_millis(150)).await; info!("Test OK"); cortex_m::asm::bkpt(); }