7 files changed, 78 insertions, 62 deletions

diff --git a/embassy-stm32-wpan/src/consts.rs b/embassy-stm32-wpan/src/consts.rs
index 9a107306c..f234151d7 100644
--- a/embassy-stm32-wpan/src/consts.rs
+++ b/embassy-stm32-wpan/src/consts.rs
@@ -87,3 +87,7 @@ pub const fn divc(x: usize, y: usize) -> usize {
 pub const TL_BLE_EVT_CS_PACKET_SIZE: usize = TL_EVT_HEADER_SIZE + TL_CS_EVT_SIZE;
 #[allow(dead_code)]
 pub const TL_BLE_EVT_CS_BUFFER_SIZE: usize = TL_PACKET_HEADER_SIZE + TL_BLE_EVT_CS_PACKET_SIZE;
+
+pub const TL_BLEEVT_CC_OPCODE: u8 = 0x0E;
+pub const TL_BLEEVT_CS_OPCODE: u8 = 0x0F;
+pub const TL_BLEEVT_VS_OPCODE: u8 = 0xFF;
diff --git a/embassy-stm32-wpan/src/evt.rs b/embassy-stm32-wpan/src/evt.rs
index 22f089037..c6528413d 100644
--- a/embassy-stm32-wpan/src/evt.rs
+++ b/embassy-stm32-wpan/src/evt.rs
@@ -1,3 +1,4 @@
+use core::marker::PhantomData;
 use core::{ptr, slice};
 
 use super::PacketHeader;
@@ -93,17 +94,22 @@ impl EvtPacket {
     }
 }
 
+pub trait MemoryManager {
+    unsafe fn drop_event_packet(evt: *mut EvtPacket);
+}
+
 /// smart pointer to the [`EvtPacket`] that will dispose of [`EvtPacket`] buffer automatically
 /// on [`Drop`]
 #[derive(Debug)]
-pub struct EvtBox {
+pub struct EvtBox<T: MemoryManager> {
     ptr: *mut EvtPacket,
+    mm: PhantomData<T>,
 }
 
-unsafe impl Send for EvtBox {}
-impl EvtBox {
+unsafe impl<T: MemoryManager> Send for EvtBox<T> {}
+impl<T: MemoryManager> EvtBox<T> {
     pub(super) fn new(ptr: *mut EvtPacket) -> Self {
-        Self { ptr }
+        Self { ptr, mm: PhantomData }
     }
 
     /// Returns information about the event
@@ -126,9 +132,6 @@ impl EvtBox {
         }
     }
 
-    /// 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.
     pub fn serial<'a>(&'a self) -> &'a [u8] {
         unsafe {
             let evt_serial: *const EvtSerial = &(*self.ptr).evt_serial;
@@ -141,20 +144,8 @@ impl EvtBox {
     }
 }
 
-impl Drop for EvtBox {
+impl<T: MemoryManager> Drop for EvtBox<T> {
     fn drop(&mut self) {
-        #[cfg(feature = "ble")]
-        unsafe {
-            use crate::sub::mm;
-
-            mm::MemoryManager::drop_event_packet(self.ptr)
-        };
-
-        #[cfg(feature = "mac")]
-        unsafe {
-            use crate::sub::mac;
-
-            mac::Mac::drop_event_packet(self.ptr)
-        }
+        unsafe { T::drop_event_packet(self.ptr) };
     }
 }
diff --git a/embassy-stm32-wpan/src/sub/ble.rs b/embassy-stm32-wpan/src/sub/ble.rs
index 619cd66a0..cd32692e1 100644
--- a/embassy-stm32-wpan/src/sub/ble.rs
+++ b/embassy-stm32-wpan/src/sub/ble.rs
@@ -1,14 +1,16 @@
 use core::marker::PhantomData;
+use core::ptr;
 
 use embassy_stm32::ipcc::Ipcc;
 use hci::Opcode;
 
-use crate::channels;
 use crate::cmd::CmdPacket;
-use crate::consts::TlPacketType;
-use crate::evt::EvtBox;
+use crate::consts::{TlPacketType, TL_BLEEVT_CC_OPCODE, TL_BLEEVT_CS_OPCODE};
+use crate::evt::{EvtBox, EvtPacket, EvtStub};
+use crate::sub::mm;
 use crate::tables::{BleTable, BLE_CMD_BUFFER, CS_BUFFER, EVT_QUEUE, HCI_ACL_DATA_BUFFER, TL_BLE_TABLE};
 use crate::unsafe_linked_list::LinkedListNode;
+use crate::{channels, evt};
 
 pub struct Ble {
     phantom: PhantomData<Ble>,
@@ -30,7 +32,7 @@ impl Ble {
         Self { phantom: PhantomData }
     }
     /// `HW_IPCC_BLE_EvtNot`
-    pub async fn tl_read(&self) -> EvtBox {
+    pub async fn tl_read(&self) -> EvtBox<Self> {
         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()))
@@ -63,6 +65,21 @@ impl Ble {
     }
 }
 
+impl evt::MemoryManager for Ble {
+    /// SAFETY: passing a pointer to something other than a managed event packet is UB
+    unsafe fn drop_event_packet(evt: *mut EvtPacket) {
+        let stub = unsafe {
+            let p_evt_stub = &(*evt).evt_serial as *const _ as *const EvtStub;
+
+            ptr::read_volatile(p_evt_stub)
+        };
+
+        if !(stub.evt_code == TL_BLEEVT_CS_OPCODE || stub.evt_code == TL_BLEEVT_CC_OPCODE) {
+            mm::MemoryManager::drop_event_packet(evt);
+        }
+    }
+}
+
 pub extern crate stm32wb_hci as hci;
 
 impl hci::Controller for Ble {
diff --git a/embassy-stm32-wpan/src/sub/mac.rs b/embassy-stm32-wpan/src/sub/mac.rs
index d2be1b85c..fd8af8609 100644
--- a/embassy-stm32-wpan/src/sub/mac.rs
+++ b/embassy-stm32-wpan/src/sub/mac.rs
@@ -8,13 +8,13 @@ use embassy_futures::poll_once;
 use embassy_stm32::ipcc::Ipcc;
 use embassy_sync::waitqueue::AtomicWaker;
 
-use crate::channels;
 use crate::cmd::CmdPacket;
 use crate::consts::TlPacketType;
 use crate::evt::{EvtBox, EvtPacket};
 use crate::tables::{
     Mac802_15_4Table, MAC_802_15_4_CMD_BUFFER, MAC_802_15_4_NOTIF_RSP_EVT_BUFFER, TL_MAC_802_15_4_TABLE,
 };
+use crate::{channels, evt};
 
 static MAC_WAKER: AtomicWaker = AtomicWaker::new();
 static MAC_EVT_OUT: AtomicBool = AtomicBool::new(false);
@@ -36,31 +36,10 @@ impl Mac {
         Self { phantom: PhantomData }
     }
 
-    /// SAFETY: passing a pointer to something other than a managed event packet is UB
-    pub(crate) unsafe fn drop_event_packet(_: *mut EvtPacket) {
-        // Write the ack
-        CmdPacket::write_into(
-            MAC_802_15_4_NOTIF_RSP_EVT_BUFFER.as_mut_ptr() as *mut _,
-            TlPacketType::OtAck,
-            0,
-            &[],
-        );
-
-        // Clear the rx flag
-        let _ = poll_once(Ipcc::receive::<bool>(
-            channels::cpu2::IPCC_MAC_802_15_4_NOTIFICATION_ACK_CHANNEL,
-            || None,
-        ));
-
-        // Allow a new read call
-        MAC_EVT_OUT.store(false, Ordering::SeqCst);
-        MAC_WAKER.wake();
-    }
-
     /// `HW_IPCC_MAC_802_15_4_EvtNot`
     ///
     /// This function will stall if the previous `EvtBox` has not been dropped
-    pub async fn read(&self) -> EvtBox {
+    pub async fn read(&self) -> EvtBox<Self> {
         // Wait for the last event box to be dropped
         poll_fn(|cx| {
             MAC_WAKER.register(cx.waker());
@@ -109,3 +88,26 @@ impl Mac {
         .await;
     }
 }
+
+impl evt::MemoryManager for Mac {
+    /// SAFETY: passing a pointer to something other than a managed event packet is UB
+    unsafe fn drop_event_packet(_: *mut EvtPacket) {
+        // Write the ack
+        CmdPacket::write_into(
+            MAC_802_15_4_NOTIF_RSP_EVT_BUFFER.as_mut_ptr() as *mut _,
+            TlPacketType::OtAck,
+            0,
+            &[],
+        );
+
+        // Clear the rx flag
+        let _ = poll_once(Ipcc::receive::<bool>(
+            channels::cpu2::IPCC_MAC_802_15_4_NOTIFICATION_ACK_CHANNEL,
+            || None,
+        ));
+
+        // Allow a new read call
+        MAC_EVT_OUT.store(false, Ordering::SeqCst);
+        MAC_WAKER.wake();
+    }
+}
diff --git a/embassy-stm32-wpan/src/sub/mm.rs b/embassy-stm32-wpan/src/sub/mm.rs
index 047fddcd4..1f2ecac2e 100644
--- a/embassy-stm32-wpan/src/sub/mm.rs
+++ b/embassy-stm32-wpan/src/sub/mm.rs
@@ -8,13 +8,13 @@ use cortex_m::interrupt;
 use embassy_stm32::ipcc::Ipcc;
 use embassy_sync::waitqueue::AtomicWaker;
 
-use crate::channels;
 use crate::consts::POOL_SIZE;
 use crate::evt::EvtPacket;
 use crate::tables::{
     MemManagerTable, BLE_SPARE_EVT_BUF, EVT_POOL, FREE_BUF_QUEUE, SYS_SPARE_EVT_BUF, TL_MEM_MANAGER_TABLE,
 };
 use crate::unsafe_linked_list::LinkedListNode;
+use crate::{channels, evt};
 
 static MM_WAKER: AtomicWaker = AtomicWaker::new();
 static mut LOCAL_FREE_BUF_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
@@ -43,16 +43,6 @@ impl MemoryManager {
         Self { phantom: PhantomData }
     }
 
-    #[allow(dead_code)]
-    /// SAFETY: passing a pointer to something other than a managed 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 _);
-        });
-
-        MM_WAKER.wake();
-    }
-
     pub async fn run_queue(&self) {
         loop {
             poll_fn(|cx| unsafe {
@@ -77,3 +67,14 @@ impl MemoryManager {
         }
     }
 }
+
+impl evt::MemoryManager for MemoryManager {
+    /// 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 _);
+        });
+
+        MM_WAKER.wake();
+    }
+}
diff --git a/embassy-stm32-wpan/src/sub/sys.rs b/embassy-stm32-wpan/src/sub/sys.rs
index 2b699b725..af652860d 100644
--- a/embassy-stm32-wpan/src/sub/sys.rs
+++ b/embassy-stm32-wpan/src/sub/sys.rs
@@ -6,6 +6,7 @@ use crate::consts::TlPacketType;
 use crate::evt::{CcEvt, EvtBox, EvtPacket};
 #[allow(unused_imports)]
 use crate::shci::{SchiCommandStatus, ShciBleInitCmdParam, ShciOpcode};
+use crate::sub::mm;
 use crate::tables::{SysTable, WirelessFwInfoTable};
 use crate::unsafe_linked_list::LinkedListNode;
 use crate::{channels, Ipcc, SYSTEM_EVT_QUEUE, SYS_CMD_BUF, TL_DEVICE_INFO_TABLE, TL_SYS_TABLE};
@@ -73,7 +74,7 @@ impl Sys {
     }
 
     /// `HW_IPCC_SYS_EvtNot`
-    pub async fn read(&self) -> EvtBox {
+    pub async fn read(&self) -> EvtBox<mm::MemoryManager> {
         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()))
diff --git a/tests/stm32/src/bin/tl_mbox.rs b/tests/stm32/src/bin/tl_mbox.rs
index 8880554de..af3832709 100644
--- a/tests/stm32/src/bin/tl_mbox.rs
+++ b/tests/stm32/src/bin/tl_mbox.rs
@@ -39,14 +39,14 @@ async fn run_mm_queue(memory_manager: mm::MemoryManager) {
 }
 
 #[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 mut mbox = TlMbox::init(p.IPCC, Irqs, config);
 
-    // spawner.spawn(run_mm_queue(mbox.mm_subsystem)).unwrap();
+    spawner.spawn(run_mm_queue(mbox.mm_subsystem)).unwrap();
 
     let sys_event = mbox.sys_subsystem.read().await;
     info!("sys event: {}", sys_event.payload());