10 files changed, 482 insertions, 343 deletions

diff --git a/embassy-stm32/src/tl_mbox/ble.rs b/embassy-stm32/src/tl_mbox/ble.rs
index 062377999..45bf81ef2 100644
--- a/embassy-stm32/src/tl_mbox/ble.rs
+++ b/embassy-stm32/src/tl_mbox/ble.rs
@@ -1,35 +1,36 @@
 use embassy_futures::block_on;
 
-use super::cmd::CmdSerial;
+use super::cmd::{CmdPacket, CmdSerial};
 use super::consts::TlPacketType;
 use super::evt::EvtBox;
+use super::ipcc::Ipcc;
 use super::unsafe_linked_list::LinkedListNode;
 use super::{
-    channels, BleTable, BLE_CMD_BUFFER, CS_BUFFER, EVT_QUEUE, HCI_ACL_DATA_BUFFER, TL_BLE_TABLE, TL_CHANNEL,
+    channels, BleTable, BLE_CMD_BUFFER, CS_BUFFER, EVT_QUEUE, HCI_ACL_DATA_BUFFER, HEAPLESS_EVT_QUEUE, TL_BLE_TABLE,
     TL_REF_TABLE,
 };
-use crate::tl_mbox::cmd::CmdPacket;
-use crate::tl_mbox::ipcc::Ipcc;
 
 pub struct Ble;
 
 impl Ble {
-    pub fn enable() {
+    pub(super) fn new() -> Self {
         unsafe {
             LinkedListNode::init_head(EVT_QUEUE.as_mut_ptr());
 
             TL_BLE_TABLE.as_mut_ptr().write_volatile(BleTable {
                 pcmd_buffer: BLE_CMD_BUFFER.as_mut_ptr().cast(),
-                pcs_buffer: CS_BUFFER.as_mut_ptr().cast(),
+                pcs_buffer: CS_BUFFER.as_ptr().cast(),
                 pevt_queue: EVT_QUEUE.as_ptr().cast(),
                 phci_acl_data_buffer: HCI_ACL_DATA_BUFFER.as_mut_ptr().cast(),
             });
         }
 
         Ipcc::c1_set_rx_channel(channels::cpu2::IPCC_BLE_EVENT_CHANNEL, true);
+
+        Ble
     }
 
-    pub fn evt_handler() {
+    pub(super) fn evt_handler() {
         unsafe {
             let mut node_ptr = core::ptr::null_mut();
             let node_ptr_ptr: *mut _ = &mut node_ptr;
@@ -40,25 +41,41 @@ impl Ble {
                 let event = node_ptr.cast();
                 let event = EvtBox::new(event);
 
-                block_on(TL_CHANNEL.send(event));
+                block_on(HEAPLESS_EVT_QUEUE.send(event));
             }
         }
 
         Ipcc::c1_clear_flag_channel(channels::cpu2::IPCC_BLE_EVENT_CHANNEL);
     }
 
+    pub(super) fn acl_data_handler(&self) {
+        Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_HCI_ACL_DATA_CHANNEL, false);
+
+        // TODO: ACL data ack to the user
+    }
+
     pub fn send_cmd(buf: &[u8]) {
         unsafe {
             let pcmd_buffer: *mut CmdPacket = (*TL_REF_TABLE.assume_init().ble_table).pcmd_buffer;
-            let pcmd_serial: *mut CmdSerial = &mut (*pcmd_buffer).cmd_serial;
+            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.cmd_serial.ty = TlPacketType::BleCmd as u8;
+            let mut 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 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);
+    }
 }
diff --git a/embassy-stm32/src/tl_mbox/cmd.rs b/embassy-stm32/src/tl_mbox/cmd.rs
index 3507c3231..781aa669d 100644
--- a/embassy-stm32/src/tl_mbox/cmd.rs
+++ b/embassy-stm32/src/tl_mbox/cmd.rs
@@ -1,7 +1,8 @@
-use super::PacketHeader;
+use crate::tl_mbox::evt::{EvtPacket, EvtSerial};
+use crate::tl_mbox::{PacketHeader, TL_EVT_HEADER_SIZE};
 
-#[repr(C, packed)]
 #[derive(Copy, Clone)]
+#[repr(C, packed)]
 pub struct Cmd {
     pub cmd_code: u16,
     pub payload_len: u8,
@@ -18,22 +19,49 @@ impl Default for Cmd {
     }
 }
 
-#[repr(C, packed)]
 #[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
 pub struct CmdSerial {
     pub ty: u8,
     pub cmd: Cmd,
 }
 
-#[repr(C, packed)]
 #[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
 pub struct CmdPacket {
     pub header: PacketHeader,
-    pub cmd_serial: CmdSerial,
+    pub cmdserial: CmdSerial,
+}
+
+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<usize, ()> {
+        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;
+
+            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();
+
+            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)
+        }
+    }
 }
 
-#[repr(C, packed)]
 #[derive(Copy, Clone)]
+#[repr(C, packed)]
 pub struct AclDataSerial {
     pub ty: u8,
     pub handle: u16,
@@ -41,8 +69,8 @@ pub struct AclDataSerial {
     pub acl_data: [u8; 1],
 }
 
-#[repr(C, packed)]
 #[derive(Copy, Clone)]
+#[repr(C, packed)]
 pub struct AclDataPacket {
     pub header: PacketHeader,
     pub acl_data_serial: AclDataSerial,
diff --git a/embassy-stm32/src/tl_mbox/consts.rs b/embassy-stm32/src/tl_mbox/consts.rs
index e16a26cd0..caf26c06b 100644
--- a/embassy-stm32/src/tl_mbox/consts.rs
+++ b/embassy-stm32/src/tl_mbox/consts.rs
@@ -1,4 +1,6 @@
-#[derive(PartialEq)]
+use core::convert::TryFrom;
+
+#[derive(Debug)]
 #[repr(C)]
 pub enum TlPacketType {
     BleCmd = 0x01,
diff --git a/embassy-stm32/src/tl_mbox/evt.rs b/embassy-stm32/src/tl_mbox/evt.rs
index 47a8b72fd..77ce7b4ca 100644
--- a/embassy-stm32/src/tl_mbox/evt.rs
+++ b/embassy-stm32/src/tl_mbox/evt.rs
@@ -2,11 +2,13 @@ use core::mem::MaybeUninit;
 
 use super::cmd::{AclDataPacket, AclDataSerial};
 use super::consts::TlPacketType;
+use super::mm::MemoryManager;
 use super::{PacketHeader, TL_EVT_HEADER_SIZE};
-use crate::tl_mbox::mm::MemoryManager;
 
-/// the payload of [`Evt`] for a command status event
-#[derive(Copy, Clone)]
+/**
+ * The payload of `Evt` for a command status event
+ */
+#[derive(Debug, Copy, Clone)]
 #[repr(C, packed)]
 pub struct CsEvt {
     pub status: u8,
@@ -14,16 +16,39 @@ pub struct CsEvt {
     pub cmd_code: u16,
 }
 
-/// the payload of [`Evt`] for a command complete event
-#[derive(Clone, Copy, Default)]
+/**
+ * The payload of `Evt` for a command complete event
+ */
+#[derive(Debug, Copy, Clone, Default)]
 #[repr(C, packed)]
 pub struct CcEvt {
     pub num_cmd: u8,
-    pub cmd_code: u8,
+    pub cmd_code: u16,
     pub payload: [u8; 1],
 }
 
-#[derive(Clone, Copy, Default)]
+impl CcEvt {
+    pub fn write(&self, buf: &mut [u8]) {
+        unsafe {
+            let len = core::mem::size_of::<CcEvt>();
+            assert!(buf.len() >= len);
+
+            let self_ptr: *const CcEvt = self;
+            let self_buf_ptr: *const u8 = self_ptr.cast();
+
+            core::ptr::copy(self_buf_ptr, buf.as_mut_ptr(), len);
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct AsynchEvt {
+    sub_evt_code: u16,
+    payload: [u8; 1],
+}
+
+#[derive(Debug, Copy, Clone, Default)]
 #[repr(C, packed)]
 pub struct Evt {
     pub evt_code: u8,
@@ -31,7 +56,7 @@ pub struct Evt {
     pub payload: [u8; 1],
 }
 
-#[derive(Clone, Copy, Default)]
+#[derive(Debug, Copy, Clone, Default)]
 #[repr(C, packed)]
 pub struct EvtSerial {
     pub kind: u8,
@@ -46,14 +71,26 @@ pub struct EvtSerial {
 /// Be careful that the asynchronous events reported by the CPU2 on the system channel do
 /// include the header and shall use `EvtPacket` format. Only the command response format on the
 /// system channel is different.
-#[derive(Clone, Copy, Default)]
+#[derive(Copy, Clone, Default)]
 #[repr(C, packed)]
 pub struct EvtPacket {
     pub header: PacketHeader,
     pub evt_serial: EvtSerial,
 }
 
-/// Smart pointer to the [`EvtPacket`] that will dispose of it automatically on drop
+impl EvtPacket {
+    pub fn kind(&self) -> u8 {
+        self.evt_serial.kind
+    }
+
+    pub fn evt(&self) -> &Evt {
+        &self.evt_serial.evt
+    }
+}
+
+/// smart pointer to the [`EvtPacket`] that will dispose of [`EvtPacket`] buffer automatically
+/// on [`Drop`]
+#[derive(Debug)]
 pub struct EvtBox {
     ptr: *mut EvtPacket,
 }
@@ -64,7 +101,7 @@ impl EvtBox {
         Self { ptr }
     }
 
-    /// Copies the event data from inner pointer and returns and event structure
+    /// copies event data from inner pointer and returns an event structure
     pub fn evt(&self) -> EvtPacket {
         let mut evt = MaybeUninit::uninit();
         unsafe {
@@ -73,28 +110,11 @@ impl EvtBox {
         }
     }
 
-    /// Returns the size of a buffer required to hold this event
-    pub fn size(&self) -> Result<usize, ()> {
-        unsafe {
-            let evt_kind = TlPacketType::try_from((*self.ptr).evt_serial.kind)?;
-
-            if evt_kind == TlPacketType::AclData {
-                let acl_data: *const AclDataPacket = self.ptr.cast();
-                let acl_serial: *const AclDataSerial = &(*acl_data).acl_data_serial;
-
-                Ok((*acl_serial).length as usize + 5)
-            } else {
-                let evt_data: *const EvtPacket = self.ptr.cast();
-                let evt_serial: *const EvtSerial = &(*evt_data).evt_serial;
-
-                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 unkown or if provided buffer size is not enough
-    pub fn copy_into_slice(&self, buf: &mut [u8]) -> Result<usize, ()> {
+    /// 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<usize, ()> {
         unsafe {
             let evt_kind = TlPacketType::try_from((*self.ptr).evt_serial.kind)?;
 
@@ -127,6 +147,26 @@ impl EvtBox {
             }
         }
     }
+
+    /// returns the size of a buffer required to hold this event
+    #[allow(clippy::result_unit_err)]
+    pub fn size(&self) -> Result<usize, ()> {
+        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 {
diff --git a/embassy-stm32/src/tl_mbox/hci.rs b/embassy-stm32/src/tl_mbox/hci.rs
new file mode 100644
index 000000000..5bb4ba666
--- /dev/null
+++ b/embassy-stm32/src/tl_mbox/hci.rs
@@ -0,0 +1,60 @@
+use super::ble::Ble;
+use super::consts::TlPacketType;
+use super::evt::CcEvt;
+use super::shci::{shci_ble_init, ShciBleInitCmdParam};
+use super::{TlMbox, STATE};
+
+pub struct RadioCoprocessor<'d> {
+    mbox: TlMbox<'d>,
+    config: ShciBleInitCmdParam,
+    rx_buffer: [u8; 500],
+}
+
+impl<'d> RadioCoprocessor<'d> {
+    pub fn new(mbox: TlMbox<'d>, config: ShciBleInitCmdParam) -> Self {
+        Self {
+            mbox,
+            config,
+            rx_buffer: [0u8; 500],
+        }
+    }
+
+    pub fn write(&mut self, params: &[u8]) -> Result<(), ()> {
+        let cmd_code = params[0];
+        let cmd = TlPacketType::try_from(cmd_code)?;
+
+        match cmd {
+            TlPacketType::BleCmd => Ble::send_cmd(params),
+            _ => todo!(),
+        }
+
+        Ok(())
+    }
+
+    pub async fn read(&mut self) -> &[u8] {
+        self.rx_buffer = [0u8; 500];
+
+        loop {
+            STATE.wait().await;
+
+            if let Some(evt) = self.mbox.dequeue_event() {
+                let event = evt.evt();
+                evt.write(&mut self.rx_buffer).unwrap();
+
+                if event.kind() == 18 {
+                    shci_ble_init(self.config);
+                    self.rx_buffer[0] = 0x04; // replace event code with one that is supported by HCI
+                }
+
+                if let Some(cc) = self.mbox.pop_last_cc_evt() {
+
+
+                    continue;
+                }
+
+                let payload_len = self.rx_buffer[2];
+                return &self.rx_buffer[..3 + payload_len as usize];
+            }
+        }
+    }
+}
diff --git a/embassy-stm32/src/tl_mbox/mm.rs b/embassy-stm32/src/tl_mbox/mm.rs
index e28a6aa0c..a40438499 100644
--- a/embassy-stm32/src/tl_mbox/mm.rs
+++ b/embassy-stm32/src/tl_mbox/mm.rs
@@ -1,56 +1,57 @@
+//! Memory manager routines
+
+use core::mem::MaybeUninit;
+
 use super::evt::EvtPacket;
+use super::ipcc::Ipcc;
 use super::unsafe_linked_list::LinkedListNode;
 use super::{
-    channels, MemManagerTable, BLE_SPARE_EVT_BUF, EVT_POOL, FREE_BUFF_QUEUE, LOCAL_FREE_BUF_QUEUE, POOL_SIZE,
+    channels, MemManagerTable, 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,
 };
-use crate::tl_mbox::ipcc::Ipcc;
 
-pub struct MemoryManager;
+pub(super) struct MemoryManager;
 
 impl MemoryManager {
-    pub fn enable() {
+    pub fn new() -> Self {
         unsafe {
-            LinkedListNode::init_head(FREE_BUFF_QUEUE.as_mut_ptr());
+            LinkedListNode::init_head(FREE_BUF_QUEUE.as_mut_ptr());
             LinkedListNode::init_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr());
 
-            TL_MEM_MANAGER_TABLE.as_mut_ptr().write_volatile(MemManagerTable {
+            TL_MEM_MANAGER_TABLE = MaybeUninit::new(MemManagerTable {
                 spare_ble_buffer: BLE_SPARE_EVT_BUF.as_ptr().cast(),
                 spare_sys_buffer: SYS_SPARE_EVT_BUF.as_ptr().cast(),
-                ble_pool: EVT_POOL.as_ptr().cast(),
-                ble_pool_size: POOL_SIZE as u32,
-                pevt_free_buffer_queue: FREE_BUFF_QUEUE.as_mut_ptr(),
+                blepool: EVT_POOL.as_ptr().cast(),
+                blepoolsize: POOL_SIZE as u32,
+                pevt_free_buffer_queue: FREE_BUF_QUEUE.as_mut_ptr(),
                 traces_evt_pool: core::ptr::null(),
-                traces_pool_size: 0,
+                tracespoolsize: 0,
             });
         }
-    }
 
-    pub fn evt_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);
+        MemoryManager
     }
 
     pub fn evt_drop(evt: *mut EvtPacket) {
         unsafe {
             let list_node = evt.cast();
 
-            LinkedListNode::remove_tail(LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), list_node);
-        }
+            LinkedListNode::insert_tail(LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), list_node);
 
-        let channel_is_busy = Ipcc::c1_is_active_flag(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL);
+            let channel_is_busy = Ipcc::c1_is_active_flag(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL);
 
-        // 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);
+            // 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);
+            }
         }
     }
 
-    fn send_free_buf() {
+    /// gives free event buffers back to CPU2 from local buffer queue
+    pub fn send_free_buf() {
         unsafe {
             let mut node_ptr = core::ptr::null_mut();
             let node_ptr_ptr: *mut _ = &mut node_ptr;
@@ -64,4 +65,11 @@ impl MemoryManager {
             }
         }
     }
+
+    /// 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/src/tl_mbox/mod.rs b/embassy-stm32/src/tl_mbox/mod.rs
index efbbf2d1d..21a954417 100644
--- a/embassy-stm32/src/tl_mbox/mod.rs
+++ b/embassy-stm32/src/tl_mbox/mod.rs
@@ -1,79 +1,50 @@
 use core::mem::MaybeUninit;
 
-use atomic_polyfill::{compiler_fence, Ordering};
 use bit_field::BitField;
 use embassy_cortex_m::interrupt::Interrupt;
+use embassy_futures::block_on;
 use embassy_hal_common::{into_ref, Peripheral, PeripheralRef};
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::channel::Channel;
+use embassy_sync::signal::Signal;
 
-use self::ble::Ble;
 use self::cmd::{AclDataPacket, CmdPacket};
-use self::evt::{CsEvt, EvtBox};
-use self::mm::MemoryManager;
-use self::shci::{shci_ble_init, ShciBleInitCmdParam};
-use self::sys::Sys;
+use self::evt::{CcEvt, EvtBox};
+use self::ipcc::{Config, Ipcc};
 use self::unsafe_linked_list::LinkedListNode;
 use crate::interrupt;
 use crate::peripherals::IPCC;
-pub use crate::tl_mbox::ipcc::Config;
-use crate::tl_mbox::ipcc::Ipcc;
-
-mod ble;
-mod channels;
-mod cmd;
-mod consts;
-mod evt;
-mod ipcc;
-mod mm;
-mod shci;
-mod sys;
-mod unsafe_linked_list;
-
-pub type PacketHeader = LinkedListNode;
 
-const TL_PACKET_HEADER_SIZE: usize = core::mem::size_of::<PacketHeader>();
-const TL_EVT_HEADER_SIZE: usize = 3;
-const TL_CS_EVT_SIZE: usize = core::mem::size_of::<CsEvt>();
-
-const CFG_TL_BLE_EVT_QUEUE_LENGTH: usize = 5;
-const CFG_TL_BLE_MOST_EVENT_PAYLOAD_SIZE: usize = 255;
-const TL_BLE_EVENT_FRAME_SIZE: usize = TL_EVT_HEADER_SIZE + CFG_TL_BLE_MOST_EVENT_PAYLOAD_SIZE;
-
-const POOL_SIZE: usize = CFG_TL_BLE_EVT_QUEUE_LENGTH * 4 * divc(TL_PACKET_HEADER_SIZE + TL_BLE_EVENT_FRAME_SIZE, 4);
-
-const fn divc(x: usize, y: usize) -> usize {
-    (x + y - 1) / y
-}
-
-#[repr(C, packed)]
-#[derive(Copy, Clone)]
-pub struct SafeBootInfoTable {
-    version: u32,
-}
-
-#[repr(C, packed)]
-#[derive(Copy, Clone)]
-pub struct FusInfoTable {
-    version: u32,
-    memory_size: u32,
-    fus_info: u32,
-}
+pub mod ble;
+pub mod channels;
+pub mod cmd;
+pub mod consts;
+pub mod evt;
+pub mod hci;
+pub mod ipcc;
+pub mod mm;
+pub mod shci;
+pub mod sys;
+pub mod unsafe_linked_list;
 
 /// Interrupt handler.
 pub struct ReceiveInterruptHandler {}
 
 impl interrupt::Handler<interrupt::IPCC_C1_RX> for ReceiveInterruptHandler {
     unsafe fn on_interrupt() {
-        // info!("ipcc rx interrupt");
+        debug!("ipcc rx interrupt");
 
         if Ipcc::is_rx_pending(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL) {
+            debug!("sys evt");
             sys::Sys::evt_handler();
         } else if Ipcc::is_rx_pending(channels::cpu2::IPCC_BLE_EVENT_CHANNEL) {
+            debug!("ble evt");
             ble::Ble::evt_handler();
         } else {
             todo!()
         }
+
+        STATE.signal(());
     }
 }
 
@@ -81,37 +52,62 @@ pub struct TransmitInterruptHandler {}
 
 impl interrupt::Handler<interrupt::IPCC_C1_TX> for TransmitInterruptHandler {
     unsafe fn on_interrupt() {
-        // info!("ipcc tx interrupt");
+        debug!("ipcc tx interrupt");
 
         if Ipcc::is_tx_pending(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL) {
+            debug!("sys cmd rsp");
             // TODO: handle this case
-            let _ = sys::Sys::cmd_evt_handler();
+            let cc = sys::Sys::cmd_evt_handler();
+            let a = unsafe { core::slice::from_raw_parts(&cc as *const _ as *const u8, core::mem::size_of::<CcEvt>()) };
+            debug!("{:#04x}", a);
+
+            LAST_CC_EVT.signal(cc);
         } else if Ipcc::is_tx_pending(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL) {
-            mm::MemoryManager::evt_handler();
+            debug!("mm");
+            mm::MemoryManager::free_buf_handler();
         } else {
             todo!()
         }
+
+        STATE.signal(());
     }
 }
 
-/// # Version
-/// - 0 -> 3   = Build - 0: Untracked - 15:Released - x: Tracked version
-/// - 4 -> 7   = branch - 0: Mass Market - x: ...
-/// - 8 -> 15  = Subversion
-/// - 16 -> 23 = Version minor
-/// - 24 -> 31 = Version major
-/// # Memory Size
-/// - 0 -> 7   = Flash ( Number of 4k sector)
-/// - 8 -> 15  = Reserved ( Shall be set to 0 - may be used as flash extension )
-/// - 16 -> 23 = SRAM2b ( Number of 1k sector)
-/// - 24 -> 31 = SRAM2a ( Number of 1k sector)
+#[derive(Debug, Copy, Clone)]
+#[repr(C, packed)]
+pub struct SafeBootInfoTable {
+    version: u32,
+}
+
+#[derive(Debug, Copy, Clone)]
+#[repr(C, packed)]
+pub struct RssInfoTable {
+    version: u32,
+    memory_size: u32,
+    rss_info: u32,
+}
+
+/**
+ * Version
+ * [0:3]   = Build - 0: Untracked - 15:Released - x: Tracked version
+ * [4:7]   = branch - 0: Mass Market - x: ...
+ * [8:15]  = Subversion
+ * [16:23] = Version minor
+ * [24:31] = Version major
+ *
+ * Memory Size
+ * [0:7]   = Flash ( Number of 4k sector)
+ * [8:15]  = Reserved ( Shall be set to 0 - may be used as flash extension )
+ * [16:23] = SRAM2b ( Number of 1k sector)
+ * [24:31] = SRAM2a ( Number of 1k sector)
+ */
+#[derive(Debug, Copy, Clone)]
 #[repr(C, packed)]
-#[derive(Copy, Clone)]
 pub struct WirelessFwInfoTable {
     version: u32,
     memory_size: u32,
-    info_stack: u32,
-    reserved: u32,
+    thread_info: u32,
+    ble_info: u32,
 }
 
 impl WirelessFwInfoTable {
@@ -122,42 +118,43 @@ impl WirelessFwInfoTable {
 
     pub fn version_minor(&self) -> u8 {
         let version = self.version;
-        (version.get_bits(16..23) & 0xff) as u8
+        (version.clone().get_bits(16..23) & 0xff) as u8
     }
 
     pub fn subversion(&self) -> u8 {
         let version = self.version;
-        (version.get_bits(8..15) & 0xff) as u8
+        (version.clone().get_bits(8..15) & 0xff) as u8
     }
 
-    /// size of FLASH, expressed in number of 4K sectors
+    /// Size of FLASH, expressed in number of 4K sectors.
     pub fn flash_size(&self) -> u8 {
         let memory_size = self.memory_size;
-        (memory_size.get_bits(0..7) & 0xff) as u8
+        (memory_size.clone().get_bits(0..7) & 0xff) as u8
     }
 
-    /// size for SRAM2a, expressed in number of 1K sectors
+    /// Size of SRAM2a, expressed in number of 1K sectors.
     pub fn sram2a_size(&self) -> u8 {
         let memory_size = self.memory_size;
-        (memory_size.get_bits(24..31) & 0xff) as u8
+        (memory_size.clone().get_bits(24..31) & 0xff) as u8
     }
 
-    /// size of SRAM2b, expressed in number of 1K sectors
+    /// Size of SRAM2b, expressed in number of 1K sectors.
     pub fn sram2b_size(&self) -> u8 {
         let memory_size = self.memory_size;
-        (memory_size.get_bits(16..23) & 0xff) as u8
+        (memory_size.clone().get_bits(16..23) & 0xff) as u8
     }
 }
 
-#[repr(C, packed)]
-#[derive(Copy, Clone)]
+#[derive(Debug, Clone)]
+#[repr(C, align(4))]
 pub struct DeviceInfoTable {
     pub safe_boot_info_table: SafeBootInfoTable,
-    pub fus_info_table: FusInfoTable,
+    pub rss_info_table: RssInfoTable,
     pub wireless_fw_info_table: WirelessFwInfoTable,
 }
 
-#[repr(C, packed)]
+#[derive(Debug)]
+#[repr(C, align(4))]
 struct BleTable {
     pcmd_buffer: *mut CmdPacket,
     pcs_buffer: *const u8,
@@ -165,81 +162,86 @@ struct BleTable {
     phci_acl_data_buffer: *mut AclDataPacket,
 }
 
-#[repr(C, packed)]
+#[derive(Debug)]
+#[repr(C, align(4))]
 struct ThreadTable {
-    no_stack_buffer: *const u8,
-    cli_cmd_rsp_buffer: *const u8,
-    ot_cmd_rsp_buffer: *const u8,
+    nostack_buffer: *const u8,
+    clicmdrsp_buffer: *const u8,
+    otcmdrsp_buffer: *const u8,
 }
 
-#[repr(C, packed)]
-struct SysTable {
-    pcmd_buffer: *mut CmdPacket,
-    sys_queue: *const LinkedListNode,
-}
-
-#[allow(dead_code)] // Not used currently but reserved
-#[repr(C, packed)]
-struct LldTestTable {
-    cli_cmd_rsp_buffer: *const u8,
-    m0_cmd_buffer: *const u8,
+// TODO: use later
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct LldTestsTable {
+    clicmdrsp_buffer: *const u8,
+    m0cmd_buffer: *const u8,
 }
 
-#[allow(dead_code)] // Not used currently but reserved
-#[repr(C, packed)]
-struct BleLldTable {
-    cmd_rsp_buffer: *const u8,
-    m0_cmd_buffer: *const u8,
+// TODO: use later
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct BleLldTable {
+    cmdrsp_buffer: *const u8,
+    m0cmd_buffer: *const u8,
 }
 
-#[allow(dead_code)] // Not used currently but reserved
-#[repr(C, packed)]
-struct ZigbeeTable {
+// TODO: use later
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct ZigbeeTable {
     notif_m0_to_m4_buffer: *const u8,
-    appli_cmd_m4_to_m0_buffer: *const u8,
+    appli_cmd_m4_to_m0_bufer: *const u8,
     request_m0_to_m4_buffer: *const u8,
 }
 
-#[repr(C, packed)]
+#[derive(Debug)]
+#[repr(C, align(4))]
+struct SysTable {
+    pcmd_buffer: *mut CmdPacket,
+    sys_queue: *const LinkedListNode,
+}
+
+#[derive(Debug)]
+#[repr(C, align(4))]
 struct MemManagerTable {
     spare_ble_buffer: *const u8,
     spare_sys_buffer: *const u8,
 
-    ble_pool: *const u8,
-    ble_pool_size: u32,
+    blepool: *const u8,
+    blepoolsize: u32,
 
     pevt_free_buffer_queue: *mut LinkedListNode,
 
     traces_evt_pool: *const u8,
-    traces_pool_size: u32,
+    tracespoolsize: u32,
 }
 
-#[repr(C, packed)]
+#[derive(Debug)]
+#[repr(C, align(4))]
 struct TracesTable {
     traces_queue: *const u8,
 }
 
-#[repr(C, packed)]
+#[derive(Debug)]
+#[repr(C, align(4))]
 struct Mac802_15_4Table {
-    pcmd_rsp_buffer: *const u8,
-    pnotack_buffer: *const u8,
+    p_cmdrsp_buffer: *const u8,
+    p_notack_buffer: *const u8,
     evt_queue: *const u8,
 }
 
-/// reference table. Contains pointers to all other tables
-#[repr(C, packed)]
-#[derive(Copy, Clone)]
+/// Reference table. Contains pointers to all other tables.
+#[derive(Debug, Copy, Clone)]
+#[repr(C)]
 pub struct RefTable {
-    pub device_info_table: *const DeviceInfoTable,
+    device_info_table: *const DeviceInfoTable,
     ble_table: *const BleTable,
     thread_table: *const ThreadTable,
     sys_table: *const SysTable,
     mem_manager_table: *const MemManagerTable,
     traces_table: *const TracesTable,
     mac_802_15_4_table: *const Mac802_15_4Table,
-    zigbee_table: *const ZigbeeTable,
-    lld_tests_table: *const LldTestTable,
-    ble_lld_table: *const BleLldTable,
 }
 
 #[link_section = "TL_REF_TABLE"]
@@ -255,12 +257,6 @@ static mut TL_BLE_TABLE: MaybeUninit<BleTable> = MaybeUninit::uninit();
 static mut TL_THREAD_TABLE: MaybeUninit<ThreadTable> = MaybeUninit::uninit();
 
 #[link_section = "MB_MEM1"]
-static mut TL_LLD_TESTS_TABLE: MaybeUninit<LldTestTable> = MaybeUninit::uninit();
-
-#[link_section = "MB_MEM1"]
-static mut TL_BLE_LLD_TABLE: MaybeUninit<BleLldTable> = MaybeUninit::uninit();
-
-#[link_section = "MB_MEM1"]
 static mut TL_SYS_TABLE: MaybeUninit<SysTable> = MaybeUninit::uninit();
 
 #[link_section = "MB_MEM1"]
@@ -272,15 +268,21 @@ static mut TL_TRACES_TABLE: MaybeUninit<TracesTable> = MaybeUninit::uninit();
 #[link_section = "MB_MEM1"]
 static mut TL_MAC_802_15_4_TABLE: MaybeUninit<Mac802_15_4Table> = MaybeUninit::uninit();
 
-#[link_section = "MB_MEM1"]
-static mut TL_ZIGBEE_TABLE: MaybeUninit<ZigbeeTable> = MaybeUninit::uninit();
+#[link_section = "MB_MEM2"]
+static mut FREE_BUF_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
+
+// Not in shared RAM
+static mut LOCAL_FREE_BUF_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
 
 #[allow(dead_code)] // Not used currently but reserved
-#[link_section = "MB_MEM1"]
-static mut FREE_BUFF_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
+#[link_section = "MB_MEM2"]
+static mut TRACES_EVT_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
 
-// not in shared RAM
-static mut LOCAL_FREE_BUF_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
+type PacketHeader = LinkedListNode;
+
+const TL_PACKET_HEADER_SIZE: usize = core::mem::size_of::<PacketHeader>();
+const TL_EVT_HEADER_SIZE: usize = 3;
+const TL_CS_EVT_SIZE: usize = core::mem::size_of::<evt::CsEvt>();
 
 #[link_section = "MB_MEM2"]
 static mut CS_BUFFER: MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + TL_EVT_HEADER_SIZE + TL_CS_EVT_SIZE]> =
@@ -293,7 +295,30 @@ static mut EVT_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
 static mut SYSTEM_EVT_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
 
 #[link_section = "MB_MEM2"]
-static mut SYS_CMD_BUF: MaybeUninit<CmdPacket> = MaybeUninit::uninit();
+pub static mut SYS_CMD_BUF: MaybeUninit<CmdPacket> = MaybeUninit::uninit();
+
+/**
+ * Queue length of BLE Event
+ * This parameter defines the number of asynchronous events that can be stored in the HCI layer before
+ * being reported to the application. When a command is sent to the BLE core coprocessor, the HCI layer
+ * is waiting for the event with the Num_HCI_Command_Packets set to 1. The receive queue shall be large
+ * enough to store all asynchronous events received in between.
+ * When CFG_TLBLE_MOST_EVENT_PAYLOAD_SIZE is set to 27, this allow to store three 255 bytes long asynchronous events
+ * between the HCI command and its event.
+ * This parameter depends on the value given to CFG_TLBLE_MOST_EVENT_PAYLOAD_SIZE. When the queue size is to small,
+ * the system may hang if the queue is full with asynchronous events and the HCI layer is still waiting
+ * for a CC/CS event, In that case, the notification TL_BLE_HCI_ToNot() is called to indicate
+ * to the application a HCI command did not receive its command event within 30s (Default HCI Timeout).
+ */
+const CFG_TLBLE_EVT_QUEUE_LENGTH: usize = 5;
+const CFG_TLBLE_MOST_EVENT_PAYLOAD_SIZE: usize = 255;
+const TL_BLE_EVENT_FRAME_SIZE: usize = TL_EVT_HEADER_SIZE + CFG_TLBLE_MOST_EVENT_PAYLOAD_SIZE;
+
+const fn divc(x: usize, y: usize) -> usize {
+    ((x) + (y) - 1) / (y)
+}
+
+const POOL_SIZE: usize = CFG_TLBLE_EVT_QUEUE_LENGTH * 4 * divc(TL_PACKET_HEADER_SIZE + TL_BLE_EVENT_FRAME_SIZE, 4);
 
 #[link_section = "MB_MEM2"]
 static mut EVT_POOL: MaybeUninit<[u8; POOL_SIZE]> = MaybeUninit::uninit();
@@ -310,18 +335,27 @@ static mut BLE_SPARE_EVT_BUF: MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + TL_EVT_HE
 static mut BLE_CMD_BUFFER: MaybeUninit<CmdPacket> = MaybeUninit::uninit();
 
 #[link_section = "MB_MEM2"]
-//                                            "magic" numbers from ST ---v---v
+//                                 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: get a better size, this is a placeholder
-pub(crate) static TL_CHANNEL: Channel<CriticalSectionRawMutex, EvtBox, 5> = Channel::new();
+static HEAPLESS_EVT_QUEUE: Channel<CriticalSectionRawMutex, EvtBox, 32> = Channel::new();
+
+static STATE: Signal<CriticalSectionRawMutex, ()> = Signal::new();
+
+/// current event that is produced during IPCC IRQ handler execution
+/// on SYS channel
+/// last received Command Complete event
+static LAST_CC_EVT: Signal<CriticalSectionRawMutex, CcEvt> = Signal::new();
 
 pub struct TlMbox<'d> {
+    sys: sys::Sys,
+    ble: ble::Ble,
+    _mm: mm::MemoryManager,
+
     _ipcc: PeripheralRef<'d, IPCC>,
 }
 
 impl<'d> TlMbox<'d> {
-    /// initializes low-level transport between CPU1 and BLE stack on CPU2
     pub fn new(
         ipcc: impl Peripheral<P = IPCC> + 'd,
         _irqs: impl interrupt::Binding<interrupt::IPCC_C1_RX, ReceiveInterruptHandler>
@@ -331,25 +365,16 @@ impl<'d> TlMbox<'d> {
         into_ref!(ipcc);
 
         unsafe {
-            compiler_fence(Ordering::AcqRel);
-
             TL_REF_TABLE.as_mut_ptr().write_volatile(RefTable {
-                device_info_table: TL_DEVICE_INFO_TABLE.as_ptr(),
+                device_info_table: TL_DEVICE_INFO_TABLE.as_mut_ptr(),
                 ble_table: TL_BLE_TABLE.as_ptr(),
                 thread_table: TL_THREAD_TABLE.as_ptr(),
                 sys_table: TL_SYS_TABLE.as_ptr(),
                 mem_manager_table: TL_MEM_MANAGER_TABLE.as_ptr(),
                 traces_table: TL_TRACES_TABLE.as_ptr(),
                 mac_802_15_4_table: TL_MAC_802_15_4_TABLE.as_ptr(),
-                zigbee_table: TL_ZIGBEE_TABLE.as_ptr(),
-                lld_tests_table: TL_LLD_TESTS_TABLE.as_ptr(),
-                ble_lld_table: TL_BLE_LLD_TABLE.as_ptr(),
             });
 
-            // info!("TL_REF_TABLE addr: {:x}", TL_REF_TABLE.as_ptr() as usize);
-
-            compiler_fence(Ordering::AcqRel);
-
             TL_SYS_TABLE = MaybeUninit::zeroed();
             TL_DEVICE_INFO_TABLE = MaybeUninit::zeroed();
             TL_BLE_TABLE = MaybeUninit::zeroed();
@@ -357,9 +382,6 @@ impl<'d> TlMbox<'d> {
             TL_MEM_MANAGER_TABLE = MaybeUninit::zeroed();
             TL_TRACES_TABLE = MaybeUninit::zeroed();
             TL_MAC_802_15_4_TABLE = MaybeUninit::zeroed();
-            TL_ZIGBEE_TABLE = MaybeUninit::zeroed();
-            TL_LLD_TESTS_TABLE = MaybeUninit::zeroed();
-            TL_BLE_LLD_TABLE = MaybeUninit::zeroed();
 
             EVT_POOL = MaybeUninit::zeroed();
             SYS_SPARE_EVT_BUF = MaybeUninit::zeroed();
@@ -368,15 +390,13 @@ impl<'d> TlMbox<'d> {
             CS_BUFFER = MaybeUninit::zeroed();
             BLE_CMD_BUFFER = MaybeUninit::zeroed();
             HCI_ACL_DATA_BUFFER = MaybeUninit::zeroed();
-
-            compiler_fence(Ordering::AcqRel);
         }
 
         Ipcc::enable(config);
 
-        Sys::enable();
-        Ble::enable();
-        MemoryManager::enable();
+        let sys = sys::Sys::new();
+        let ble = ble::Ble::new();
+        let mm = mm::MemoryManager::new();
 
         // enable interrupts
         crate::interrupt::IPCC_C1_RX::unpend();
@@ -385,13 +405,19 @@ impl<'d> TlMbox<'d> {
         unsafe { crate::interrupt::IPCC_C1_RX::enable() };
         unsafe { crate::interrupt::IPCC_C1_TX::enable() };
 
-        Self { _ipcc: ipcc }
+        Self {
+            sys,
+            ble,
+            _mm: mm,
+            _ipcc: ipcc,
+        }
     }
 
+    /// Returns CPU2 wireless firmware information (if present).
     pub fn wireless_fw_info(&self) -> Option<WirelessFwInfoTable> {
         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
+        // Zero version indicates that CPU2 wasn't active and didn't fill the information table
         if info.version != 0 {
             Some(*info)
         } else {
@@ -399,19 +425,22 @@ impl<'d> TlMbox<'d> {
         }
     }
 
-    pub fn shci_ble_init(&self, param: ShciBleInitCmdParam) {
-        shci_ble_init(param);
+    /// picks single [`EvtBox`] from internal event queue.
+    ///
+    /// Internal event queu is populated in IPCC_RX_IRQ handler
+    pub fn dequeue_event(&mut self) -> Option<EvtBox> {
+        HEAPLESS_EVT_QUEUE.try_recv().ok()
     }
 
-    pub fn send_ble_cmd(&self, buf: &[u8]) {
-        ble::Ble::send_cmd(buf);
-    }
-
-    // pub fn send_sys_cmd(&self, buf: &[u8]) {
-    //     sys::Sys::send_cmd(buf);
-    // }
+    /// retrieves last Command Complete event and removes it from mailbox
+    pub fn pop_last_cc_evt(&mut self) -> Option<CcEvt> {
+        if LAST_CC_EVT.signaled() {
+            let cc = Some(block_on(LAST_CC_EVT.wait()));
+            LAST_CC_EVT.reset();
 
-    pub async fn read(&self) -> EvtBox {
-        TL_CHANNEL.recv().await
+            cc
+        } else {
+            None
+        }
     }
 }
diff --git a/embassy-stm32/src/tl_mbox/shci.rs b/embassy-stm32/src/tl_mbox/shci.rs
index 6b5b2dd19..b19baa702 100644
--- a/embassy-stm32/src/tl_mbox/shci.rs
+++ b/embassy-stm32/src/tl_mbox/shci.rs
@@ -1,16 +1,10 @@
-//! HCI commands for system channel
-
 use super::cmd::CmdPacket;
 use super::consts::TlPacketType;
-use super::{channels, TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE, TL_SYS_TABLE};
-use crate::tl_mbox::ipcc::Ipcc;
+use super::{sys, TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE, TL_SYS_TABLE};
 
 const SCHI_OPCODE_BLE_INIT: u16 = 0xfc66;
-pub const TL_BLE_EVT_CS_PACKET_SIZE: usize = TL_EVT_HEADER_SIZE + TL_CS_EVT_SIZE;
-#[allow(dead_code)]
-const TL_BLE_EVT_CS_BUFFER_SIZE: usize = TL_PACKET_HEADER_SIZE + TL_BLE_EVT_CS_PACKET_SIZE;
 
-#[derive(Clone, Copy)]
+#[derive(Debug, Clone, Copy)]
 #[repr(C, packed)]
 pub struct ShciBleInitCmdParam {
     /// NOT USED CURRENTLY
@@ -63,39 +57,44 @@ impl Default for ShciBleInitCmdParam {
     }
 }
 
-#[derive(Clone, Copy, Default)]
+#[derive(Debug, Clone, Copy, Default)]
 #[repr(C, packed)]
 pub struct ShciHeader {
     metadata: [u32; 3],
 }
 
-#[derive(Clone, Copy)]
+#[derive(Debug, Clone, Copy)]
 #[repr(C, packed)]
 pub struct ShciBleInitCmdPacket {
     header: ShciHeader,
     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!("shci init");
+
     let mut packet = ShciBleInitCmdPacket {
         header: ShciHeader::default(),
         param,
     };
 
-    let packet_ptr: *mut ShciBleInitCmdPacket = &mut packet;
+    let packet_ptr: *mut _ = &mut packet;
 
     unsafe {
         let cmd_ptr: *mut CmdPacket = packet_ptr.cast();
 
-        (*cmd_ptr).cmd_serial.cmd.cmd_code = SCHI_OPCODE_BLE_INIT;
-        (*cmd_ptr).cmd_serial.cmd.payload_len = core::mem::size_of::<ShciBleInitCmdParam>() as u8;
+        (*cmd_ptr).cmdserial.cmd.cmd_code = SCHI_OPCODE_BLE_INIT;
+        (*cmd_ptr).cmdserial.cmd.payload_len = core::mem::size_of::<ShciBleInitCmdParam>() as u8;
 
-        let cmd_buf = &mut *(*TL_SYS_TABLE.as_mut_ptr()).pcmd_buffer;
-        core::ptr::write(cmd_buf, *cmd_ptr);
+        let mut p_cmd_buffer = &mut *(*TL_SYS_TABLE.as_mut_ptr()).pcmd_buffer;
+        core::ptr::write(p_cmd_buffer, *cmd_ptr);
 
-        cmd_buf.cmd_serial.ty = TlPacketType::SysCmd as u8;
+        p_cmd_buffer.cmdserial.ty = TlPacketType::SysCmd as u8;
 
-        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);
+        sys::send_cmd();
     }
 }
diff --git a/embassy-stm32/src/tl_mbox/sys.rs b/embassy-stm32/src/tl_mbox/sys.rs
index 9685fb920..c87aa440c 100644
--- a/embassy-stm32/src/tl_mbox/sys.rs
+++ b/embassy-stm32/src/tl_mbox/sys.rs
@@ -1,45 +1,27 @@
 use embassy_futures::block_on;
 
 use super::cmd::{CmdPacket, CmdSerial};
-use super::consts::TlPacketType;
 use super::evt::{CcEvt, EvtBox, EvtSerial};
+use super::ipcc::Ipcc;
 use super::unsafe_linked_list::LinkedListNode;
-use super::{channels, SysTable, SYSTEM_EVT_QUEUE, SYS_CMD_BUF, TL_CHANNEL, TL_REF_TABLE, TL_SYS_TABLE};
-use crate::tl_mbox::ipcc::Ipcc;
+use super::{channels, SysTable, HEAPLESS_EVT_QUEUE, SYSTEM_EVT_QUEUE, SYS_CMD_BUF, TL_SYS_TABLE};
 
 pub struct Sys;
 
 impl Sys {
-    pub fn enable() {
+    pub 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);
-    }
-
-    pub fn evt_handler() {
-        unsafe {
-            let mut node_ptr = core::ptr::null_mut();
-            let node_ptr_ptr: *mut _ = &mut node_ptr;
-
-            while !LinkedListNode::is_empty(SYSTEM_EVT_QUEUE.as_mut_ptr()) {
-                LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr(), node_ptr_ptr);
-
-                let event = node_ptr.cast();
-                let event = EvtBox::new(event);
-
-                // TODO: not really happy about this
-                block_on(TL_CHANNEL.send(event));
-            }
-        }
 
-        Ipcc::c1_clear_flag_channel(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL);
+        Sys
     }
 
     pub fn cmd_evt_handler() -> CcEvt {
@@ -55,29 +37,40 @@ impl Sys {
         // 4. CcEvt type is the actual SHCI response
         // 5. profit
         unsafe {
-            let cmd: *const CmdPacket = (*TL_SYS_TABLE.as_ptr()).pcmd_buffer;
-            let cmd_serial: *const CmdSerial = &(*cmd).cmd_serial;
+            let pcmd: *const CmdPacket = (*TL_SYS_TABLE.as_ptr()).pcmd_buffer;
+
+            let a = unsafe {
+                core::slice::from_raw_parts(&pcmd as *const _ as *const u8, core::mem::size_of::<CmdPacket>())
+            };
+            debug!("shci response {:#04x}", a);
+
+            let cmd_serial: *const CmdSerial = &(*pcmd).cmdserial;
             let evt_serial: *const EvtSerial = cmd_serial.cast();
-            let cc = (*evt_serial).evt.payload.as_ptr().cast();
+            let cc: *const CcEvt = (*evt_serial).evt.payload.as_ptr().cast();
             *cc
         }
     }
 
-    #[allow(dead_code)]
-    pub fn send_cmd(buf: &[u8]) {
+    pub fn evt_handler() {
         unsafe {
-            // TODO: check this
-            let cmd_buffer = &mut *(*TL_REF_TABLE.assume_init().sys_table).pcmd_buffer;
-            let cmd_serial: *mut CmdSerial = &mut cmd_buffer.cmd_serial;
-            let cmd_serial_buf = cmd_serial.cast();
+            let mut node_ptr = core::ptr::null_mut();
+            let node_ptr_ptr: *mut _ = &mut node_ptr;
 
-            core::ptr::copy(buf.as_ptr(), cmd_serial_buf, buf.len());
+            while !LinkedListNode::is_empty(SYSTEM_EVT_QUEUE.as_mut_ptr()) {
+                LinkedListNode::remove_head(SYSTEM_EVT_QUEUE.as_mut_ptr(), node_ptr_ptr);
 
-            let cmd_packet = &mut *(*TL_REF_TABLE.assume_init().sys_table).pcmd_buffer;
-            cmd_packet.cmd_serial.ty = TlPacketType::SysCmd as u8;
+                let event = node_ptr.cast();
+                let event = EvtBox::new(event);
 
-            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);
+                block_on(HEAPLESS_EVT_QUEUE.send(event));
+            }
         }
+
+        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);
+}
diff --git a/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs b/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs
index 1724d946f..0525d3f37 100644
--- a/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs
+++ b/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs
@@ -4,7 +4,9 @@
 
 use defmt::*;
 use embassy_executor::Spawner;
-use embassy_stm32::tl_mbox::{Config, TlMbox};
+use embassy_stm32::tl_mbox::hci::RadioCoprocessor;
+use embassy_stm32::tl_mbox::ipcc::Config;
+use embassy_stm32::tl_mbox::TlMbox;
 use embassy_stm32::{bind_interrupts, tl_mbox};
 use {defmt_rtt as _, panic_probe as _};
 
@@ -45,53 +47,14 @@ async fn main(_spawner: Spawner) {
     let config = Config::default();
     let mbox = TlMbox::new(p.IPCC, Irqs, config);
 
-    info!("waiting for coprocessor to boot");
-    let event_box = mbox.read().await;
+    let mut rc = RadioCoprocessor::new(mbox, Default::default());
+    rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]).unwrap();
 
-    let mut payload = [0u8; 6];
-    event_box.copy_into_slice(&mut payload).unwrap();
+    let response = rc.read().await;
+    info!("coprocessor ready {}", response);
 
-    let event_packet = event_box.evt();
-    let kind = event_packet.evt_serial.kind;
-
-    // means recieved SYS event, which indicates in this case that the coprocessor is ready
-    if kind == 0x12 {
-        let code = event_packet.evt_serial.evt.evt_code;
-        let payload_len = event_packet.evt_serial.evt.payload_len;
-
-        info!(
-            "==> kind: {:#04x}, code: {:#04x}, payload_length: {}, payload: {:#04x}",
-            kind,
-            code,
-            payload_len,
-            payload[3..]
-        );
-    }
-
-    // initialize ble stack, does not return a response
-    mbox.shci_ble_init(Default::default());
-
-    info!("resetting BLE");
-    mbox.send_ble_cmd(&[0x01, 0x03, 0x0c, 0x00, 0x00]);
-
-    let event_box = mbox.read().await;
-
-    let mut payload = [0u8; 7];
-    event_box.copy_into_slice(&mut payload).unwrap();
-
-    let event_packet = event_box.evt();
-    let kind = event_packet.evt_serial.kind;
-
-    let code = event_packet.evt_serial.evt.evt_code;
-    let payload_len = event_packet.evt_serial.evt.payload_len;
-
-    info!(
-        "==> kind: {:#04x}, code: {:#04x}, payload_length: {}, payload: {:#04x}",
-        kind,
-        code,
-        payload_len,
-        payload[3..]
-    );
+    let response = rc.read().await;
+    info!("coprocessor ready {}", response);
 
     info!("Test OK");
     cortex_m::asm::bkpt();