18 files changed, 653 insertions, 370 deletions

diff --git a/embassy-stm32-wpan/Cargo.toml b/embassy-stm32-wpan/Cargo.toml
new file mode 100644
index 000000000..a6673d472
--- /dev/null
+++ b/embassy-stm32-wpan/Cargo.toml
@@ -0,0 +1,23 @@
+[package]
+name = "embassy-stm32-wpan"
+version = "0.1.0"
+edition = "2021"
+license = "MIT OR Apache-2.0"
+
+[dependencies]
+embassy-stm32 = { version = "0.1.0", path = "../embassy-stm32", features = ["stm32wb55rg"] }
+embassy-sync = { version = "0.2.0", path = "../embassy-sync" }
+embassy-time = { version = "0.1.0", path = "../embassy-time", optional = true }
+embassy-futures = { version = "0.1.0", path = "../embassy-futures" }
+embassy-cortex-m = { version = "0.1.0", path = "../embassy-cortex-m", features = ["prio-bits-4"]}
+embassy-hal-common = {version = "0.1.0", path = "../embassy-hal-common" }
+embassy-embedded-hal = {version = "0.1.0", path = "../embassy-embedded-hal" }
+
+defmt = { version = "0.3", optional = true }
+cortex-m = "0.7.6"
+heapless = "0.7.16"
+
+bit_field = "0.10.2"
+
+[features]
+defmt = ["dep:defmt", "embassy-sync/defmt", "embassy-embedded-hal/defmt", "embassy-hal-common/defmt"]
diff --git a/embassy-stm32/src/tl_mbox/ble.rs b/embassy-stm32-wpan/src/ble.rs
index 45bf81ef2..4546bde07 100644
--- a/embassy-stm32/src/tl_mbox/ble.rs
+++ b/embassy-stm32-wpan/src/ble.rs
@@ -1,23 +1,24 @@
-use embassy_futures::block_on;
-
-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, HEAPLESS_EVT_QUEUE, TL_BLE_TABLE,
-    TL_REF_TABLE,
+use core::mem::MaybeUninit;
+
+use embassy_stm32::ipcc::Ipcc;
+
+use crate::cmd::{CmdPacket, CmdSerial};
+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,
 };
 
 pub struct Ble;
 
 impl Ble {
-    pub(super) fn new() -> Self {
+    pub(super) fn enable() {
         unsafe {
             LinkedListNode::init_head(EVT_QUEUE.as_mut_ptr());
 
-            TL_BLE_TABLE.as_mut_ptr().write_volatile(BleTable {
+            TL_BLE_TABLE = MaybeUninit::new(BleTable {
                 pcmd_buffer: BLE_CMD_BUFFER.as_mut_ptr().cast(),
                 pcs_buffer: CS_BUFFER.as_ptr().cast(),
                 pevt_queue: EVT_QUEUE.as_ptr().cast(),
@@ -26,8 +27,6 @@ impl Ble {
         }
 
         Ipcc::c1_set_rx_channel(channels::cpu2::IPCC_BLE_EVENT_CHANNEL, true);
-
-        Ble
     }
 
     pub(super) fn evt_handler() {
@@ -41,20 +40,22 @@ impl Ble {
                 let event = node_ptr.cast();
                 let event = EvtBox::new(event);
 
-                block_on(HEAPLESS_EVT_QUEUE.send(event));
+                EVT_CHANNEL.try_send(event).unwrap();
             }
         }
 
         Ipcc::c1_clear_flag_channel(channels::cpu2::IPCC_BLE_EVENT_CHANNEL);
     }
 
-    pub(super) fn acl_data_handler(&self) {
+    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
     }
 
-    pub fn send_cmd(buf: &[u8]) {
+    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;
@@ -70,8 +71,8 @@ impl Ble {
     }
 
     #[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 };
+    pub(super) fn ble_send_acl_data() {
+        let mut 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;
 
diff --git a/embassy-stm32/src/tl_mbox/channels.rs b/embassy-stm32-wpan/src/channels.rs
index 25a065ba4..9a2be1cfa 100644
--- a/embassy-stm32/src/tl_mbox/channels.rs
+++ b/embassy-stm32-wpan/src/channels.rs
@@ -50,36 +50,30 @@
 //!
 
 pub mod cpu1 {
-    use crate::tl_mbox::ipcc::IpccChannel;
+    use embassy_stm32::ipcc::IpccChannel;
 
-    // Not used currently but reserved
     pub const IPCC_BLE_CMD_CHANNEL: IpccChannel = IpccChannel::Channel1;
-    // Not used currently but reserved
     pub const IPCC_SYSTEM_CMD_RSP_CHANNEL: IpccChannel = IpccChannel::Channel2;
-    #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_THREAD_OT_CMD_RSP_CHANNEL: IpccChannel = IpccChannel::Channel3;
     #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_ZIGBEE_CMD_APPLI_CHANNEL: IpccChannel = IpccChannel::Channel3;
     #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_MAC_802_15_4_CMD_RSP_CHANNEL: IpccChannel = IpccChannel::Channel3;
-    // Not used currently but reserved
-    pub const IPCC_MM_RELEASE_BUFFER_CHANNEL: IpccChannel = IpccChannel::Channel4;
     #[allow(dead_code)] // Not used currently but reserved
+    pub const IPCC_MM_RELEASE_BUFFER_CHANNEL: IpccChannel = IpccChannel::Channel4;
     pub const IPCC_THREAD_CLI_CMD_CHANNEL: IpccChannel = IpccChannel::Channel5;
     #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_LLDTESTS_CLI_CMD_CHANNEL: IpccChannel = IpccChannel::Channel5;
     #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_BLE_LLD_CMD_CHANNEL: IpccChannel = IpccChannel::Channel5;
-    #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_HCI_ACL_DATA_CHANNEL: IpccChannel = IpccChannel::Channel6;
 }
 
 pub mod cpu2 {
-    use crate::tl_mbox::ipcc::IpccChannel;
+    use embassy_stm32::ipcc::IpccChannel;
 
     pub const IPCC_BLE_EVENT_CHANNEL: IpccChannel = IpccChannel::Channel1;
     pub const IPCC_SYSTEM_EVENT_CHANNEL: IpccChannel = IpccChannel::Channel2;
-    #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_THREAD_NOTIFICATION_ACK_CHANNEL: IpccChannel = IpccChannel::Channel3;
     #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_ZIGBEE_APPLI_NOTIF_ACK_CHANNEL: IpccChannel = IpccChannel::Channel3;
@@ -88,10 +82,8 @@ pub mod cpu2 {
     #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_LDDTESTS_M0_CMD_CHANNEL: IpccChannel = IpccChannel::Channel3;
     #[allow(dead_code)] // Not used currently but reserved
-    pub const IPCC_BLE_LLD_M0_CMD_CHANNEL: IpccChannel = IpccChannel::Channel3;
-    #[allow(dead_code)] // Not used currently but reserved
+    pub const IPCC_BLE_LLDÇM0_CMD_CHANNEL: IpccChannel = IpccChannel::Channel3;
     pub const IPCC_TRACES_CHANNEL: IpccChannel = IpccChannel::Channel4;
-    #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_THREAD_CLI_NOTIFICATION_ACK_CHANNEL: IpccChannel = IpccChannel::Channel5;
     #[allow(dead_code)] // Not used currently but reserved
     pub const IPCC_LLDTESTS_CLI_RSP_CHANNEL: IpccChannel = IpccChannel::Channel5;
diff --git a/embassy-stm32/src/tl_mbox/cmd.rs b/embassy-stm32-wpan/src/cmd.rs
index 781aa669d..1f7dae7f7 100644
--- a/embassy-stm32/src/tl_mbox/cmd.rs
+++ b/embassy-stm32-wpan/src/cmd.rs
@@ -1,5 +1,5 @@
-use crate::tl_mbox::evt::{EvtPacket, EvtSerial};
-use crate::tl_mbox::{PacketHeader, TL_EVT_HEADER_SIZE};
+use crate::evt::{EvtPacket, EvtSerial};
+use crate::{PacketHeader, TL_EVT_HEADER_SIZE};
 
 #[derive(Copy, Clone)]
 #[repr(C, packed)]
diff --git a/embassy-stm32/src/tl_mbox/consts.rs b/embassy-stm32-wpan/src/consts.rs
index caf26c06b..caf26c06b 100644
--- a/embassy-stm32/src/tl_mbox/consts.rs
+++ b/embassy-stm32-wpan/src/consts.rs
diff --git a/embassy-stm32/src/tl_mbox/evt.rs b/embassy-stm32-wpan/src/evt.rs
index 77ce7b4ca..b53fe506e 100644
--- a/embassy-stm32/src/tl_mbox/evt.rs
+++ b/embassy-stm32-wpan/src/evt.rs
@@ -2,13 +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::mm;
 
 /**
  * The payload of `Evt` for a command status event
  */
-#[derive(Debug, Copy, Clone)]
+#[derive(Copy, Clone)]
 #[repr(C, packed)]
 pub struct CsEvt {
     pub status: u8,
@@ -19,7 +19,7 @@ pub struct CsEvt {
 /**
  * The payload of `Evt` for a command complete event
  */
-#[derive(Debug, Copy, Clone, Default)]
+#[derive(Copy, Clone, Default)]
 #[repr(C, packed)]
 pub struct CcEvt {
     pub num_cmd: u8,
@@ -41,14 +41,14 @@ impl CcEvt {
     }
 }
 
-#[derive(Debug, Copy, Clone, Default)]
+#[derive(Copy, Clone, Default)]
 #[repr(C, packed)]
 pub struct AsynchEvt {
     sub_evt_code: u16,
     payload: [u8; 1],
 }
 
-#[derive(Debug, Copy, Clone, Default)]
+#[derive(Copy, Clone, Default)]
 #[repr(C, packed)]
 pub struct Evt {
     pub evt_code: u8,
@@ -56,7 +56,7 @@ pub struct Evt {
     pub payload: [u8; 1],
 }
 
-#[derive(Debug, Copy, Clone, Default)]
+#[derive(Copy, Clone, Default)]
 #[repr(C, packed)]
 pub struct EvtSerial {
     pub kind: u8,
@@ -171,6 +171,6 @@ impl EvtBox {
 
 impl Drop for EvtBox {
     fn drop(&mut self) {
-        MemoryManager::evt_drop(self.ptr);
+        mm::MemoryManager::evt_drop(self.ptr);
     }
 }
diff --git a/embassy-stm32-wpan/src/fmt.rs b/embassy-stm32-wpan/src/fmt.rs
new file mode 100644
index 000000000..066970813
--- /dev/null
+++ b/embassy-stm32-wpan/src/fmt.rs
@@ -0,0 +1,225 @@
+#![macro_use]
+#![allow(unused_macros)]
+
+#[cfg(all(feature = "defmt", feature = "log"))]
+compile_error!("You may not enable both `defmt` and `log` features.");
+
+macro_rules! assert {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::assert!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::assert!($($x)*);
+        }
+    };
+}
+
+macro_rules! assert_eq {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::assert_eq!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::assert_eq!($($x)*);
+        }
+    };
+}
+
+macro_rules! assert_ne {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::assert_ne!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::assert_ne!($($x)*);
+        }
+    };
+}
+
+macro_rules! debug_assert {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::debug_assert!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::debug_assert!($($x)*);
+        }
+    };
+}
+
+macro_rules! debug_assert_eq {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::debug_assert_eq!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::debug_assert_eq!($($x)*);
+        }
+    };
+}
+
+macro_rules! debug_assert_ne {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::debug_assert_ne!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::debug_assert_ne!($($x)*);
+        }
+    };
+}
+
+macro_rules! todo {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::todo!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::todo!($($x)*);
+        }
+    };
+}
+
+macro_rules! unreachable {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::unreachable!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::unreachable!($($x)*);
+        }
+    };
+}
+
+macro_rules! panic {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::panic!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::panic!($($x)*);
+        }
+    };
+}
+
+macro_rules! trace {
+    ($s:literal $(, $x:expr)* $(,)?) => {
+        {
+            #[cfg(feature = "log")]
+            ::log::trace!($s $(, $x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::trace!($s $(, $x)*);
+            #[cfg(not(any(feature = "log", feature="defmt")))]
+            let _ = ($( & $x ),*);
+        }
+    };
+}
+
+macro_rules! debug {
+    ($s:literal $(, $x:expr)* $(,)?) => {
+        {
+            #[cfg(feature = "log")]
+            ::log::debug!($s $(, $x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::debug!($s $(, $x)*);
+            #[cfg(not(any(feature = "log", feature="defmt")))]
+            let _ = ($( & $x ),*);
+        }
+    };
+}
+
+macro_rules! info {
+    ($s:literal $(, $x:expr)* $(,)?) => {
+        {
+            #[cfg(feature = "log")]
+            ::log::info!($s $(, $x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::info!($s $(, $x)*);
+            #[cfg(not(any(feature = "log", feature="defmt")))]
+            let _ = ($( & $x ),*);
+        }
+    };
+}
+
+macro_rules! warn {
+    ($s:literal $(, $x:expr)* $(,)?) => {
+        {
+            #[cfg(feature = "log")]
+            ::log::warn!($s $(, $x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::warn!($s $(, $x)*);
+            #[cfg(not(any(feature = "log", feature="defmt")))]
+            let _ = ($( & $x ),*);
+        }
+    };
+}
+
+macro_rules! error {
+    ($s:literal $(, $x:expr)* $(,)?) => {
+        {
+            #[cfg(feature = "log")]
+            ::log::error!($s $(, $x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::error!($s $(, $x)*);
+            #[cfg(not(any(feature = "log", feature="defmt")))]
+            let _ = ($( & $x ),*);
+        }
+    };
+}
+
+#[cfg(feature = "defmt")]
+macro_rules! unwrap {
+    ($($x:tt)*) => {
+        ::defmt::unwrap!($($x)*)
+    };
+}
+
+#[cfg(not(feature = "defmt"))]
+macro_rules! unwrap {
+    ($arg:expr) => {
+        match $crate::fmt::Try::into_result($arg) {
+            ::core::result::Result::Ok(t) => t,
+            ::core::result::Result::Err(e) => {
+                ::core::panic!("unwrap of `{}` failed: {:?}", ::core::stringify!($arg), e);
+            }
+        }
+    };
+    ($arg:expr, $($msg:expr),+ $(,)? ) => {
+        match $crate::fmt::Try::into_result($arg) {
+            ::core::result::Result::Ok(t) => t,
+            ::core::result::Result::Err(e) => {
+                ::core::panic!("unwrap of `{}` failed: {}: {:?}", ::core::stringify!($arg), ::core::format_args!($($msg,)*), e);
+            }
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+pub struct NoneError;
+
+pub trait Try {
+    type Ok;
+    type Error;
+    fn into_result(self) -> Result<Self::Ok, Self::Error>;
+}
+
+impl<T> Try for Option<T> {
+    type Ok = T;
+    type Error = NoneError;
+
+    #[inline]
+    fn into_result(self) -> Result<T, NoneError> {
+        self.ok_or(NoneError)
+    }
+}
+
+impl<T, E> Try for Result<T, E> {
+    type Ok = T;
+    type Error = E;
+
+    #[inline]
+    fn into_result(self) -> Self {
+        self
+    }
+}
diff --git a/embassy-stm32/src/tl_mbox/mod.rs b/embassy-stm32-wpan/src/lib.rs
index 21a954417..b3206428e 100644
--- a/embassy-stm32/src/tl_mbox/mod.rs
+++ b/embassy-stm32-wpan/src/lib.rs
@@ -1,30 +1,37 @@
+#![no_std]
+
+// This must go FIRST so that all the other modules see its macros.
+pub mod fmt;
+
 use core::mem::MaybeUninit;
 
-use bit_field::BitField;
+use cmd::CmdPacket;
 use embassy_cortex_m::interrupt::Interrupt;
 use embassy_futures::block_on;
 use embassy_hal_common::{into_ref, Peripheral, PeripheralRef};
+use embassy_stm32::interrupt;
+use embassy_stm32::ipcc::{Config, Ipcc};
+use embassy_stm32::peripherals::IPCC;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::channel::Channel;
 use embassy_sync::signal::Signal;
-
-use self::cmd::{AclDataPacket, CmdPacket};
-use self::evt::{CcEvt, EvtBox};
-use self::ipcc::{Config, Ipcc};
-use self::unsafe_linked_list::LinkedListNode;
-use crate::interrupt;
-use crate::peripherals::IPCC;
+use evt::{CcEvt, EvtBox};
+use tables::{
+    BleTable, DeviceInfoTable, Mac802_15_4Table, MemManagerTable, RefTable, SysTable, ThreadTable, TracesTable,
+    WirelessFwInfoTable,
+};
+use unsafe_linked_list::LinkedListNode;
 
 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 rc;
 pub mod shci;
 pub mod sys;
+pub mod tables;
 pub mod unsafe_linked_list;
 
 /// Interrupt handler.
@@ -32,16 +39,12 @@ pub struct ReceiveInterruptHandler {}
 
 impl interrupt::Handler<interrupt::IPCC_C1_RX> for ReceiveInterruptHandler {
     unsafe fn on_interrupt() {
-        debug!("ipcc rx interrupt");
-
         if Ipcc::is_rx_pending(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL) {
-            debug!("sys evt");
+            debug!("RX SYS evt");
             sys::Sys::evt_handler();
         } else if Ipcc::is_rx_pending(channels::cpu2::IPCC_BLE_EVENT_CHANNEL) {
-            debug!("ble evt");
+            debug!("RX BLE evt");
             ble::Ble::evt_handler();
-        } else {
-            todo!()
         }
 
         STATE.signal(());
@@ -52,198 +55,23 @@ pub struct TransmitInterruptHandler {}
 
 impl interrupt::Handler<interrupt::IPCC_C1_TX> for TransmitInterruptHandler {
     unsafe fn on_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
+            debug!("TX SYS cmd rsp");
             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) {
-            debug!("mm");
+            debug!("TX MM release");
             mm::MemoryManager::free_buf_handler();
-        } else {
-            todo!()
+        } else if Ipcc::is_tx_pending(channels::cpu1::IPCC_HCI_ACL_DATA_CHANNEL) {
+            debug!("TX HCI acl");
+            ble::Ble::acl_data_handler();
         }
 
         STATE.signal(());
     }
 }
 
-#[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)]
-pub struct WirelessFwInfoTable {
-    version: u32,
-    memory_size: u32,
-    thread_info: u32,
-    ble_info: u32,
-}
-
-impl WirelessFwInfoTable {
-    pub fn version_major(&self) -> u8 {
-        let version = self.version;
-        (version.get_bits(24..31) & 0xff) as u8
-    }
-
-    pub fn version_minor(&self) -> u8 {
-        let version = self.version;
-        (version.clone().get_bits(16..23) & 0xff) as u8
-    }
-
-    pub fn subversion(&self) -> u8 {
-        let version = self.version;
-        (version.clone().get_bits(8..15) & 0xff) as u8
-    }
-
-    /// Size of FLASH, expressed in number of 4K sectors.
-    pub fn flash_size(&self) -> u8 {
-        let memory_size = self.memory_size;
-        (memory_size.clone().get_bits(0..7) & 0xff) as u8
-    }
-
-    /// Size of SRAM2a, expressed in number of 1K sectors.
-    pub fn sram2a_size(&self) -> u8 {
-        let memory_size = self.memory_size;
-        (memory_size.clone().get_bits(24..31) & 0xff) as u8
-    }
-
-    /// Size of SRAM2b, expressed in number of 1K sectors.
-    pub fn sram2b_size(&self) -> u8 {
-        let memory_size = self.memory_size;
-        (memory_size.clone().get_bits(16..23) & 0xff) as u8
-    }
-}
-
-#[derive(Debug, Clone)]
-#[repr(C, align(4))]
-pub struct DeviceInfoTable {
-    pub safe_boot_info_table: SafeBootInfoTable,
-    pub rss_info_table: RssInfoTable,
-    pub wireless_fw_info_table: WirelessFwInfoTable,
-}
-
-#[derive(Debug)]
-#[repr(C, align(4))]
-struct BleTable {
-    pcmd_buffer: *mut CmdPacket,
-    pcs_buffer: *const u8,
-    pevt_queue: *const u8,
-    phci_acl_data_buffer: *mut AclDataPacket,
-}
-
-#[derive(Debug)]
-#[repr(C, align(4))]
-struct ThreadTable {
-    nostack_buffer: *const u8,
-    clicmdrsp_buffer: *const u8,
-    otcmdrsp_buffer: *const u8,
-}
-
-// TODO: use later
-#[derive(Debug)]
-#[repr(C, align(4))]
-pub struct LldTestsTable {
-    clicmdrsp_buffer: *const u8,
-    m0cmd_buffer: *const u8,
-}
-
-// TODO: use later
-#[derive(Debug)]
-#[repr(C, align(4))]
-pub struct BleLldTable {
-    cmdrsp_buffer: *const u8,
-    m0cmd_buffer: *const u8,
-}
-
-// TODO: use later
-#[derive(Debug)]
-#[repr(C, align(4))]
-pub struct ZigbeeTable {
-    notif_m0_to_m4_buffer: *const u8,
-    appli_cmd_m4_to_m0_bufer: *const u8,
-    request_m0_to_m4_buffer: *const u8,
-}
-
-#[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,
-
-    blepool: *const u8,
-    blepoolsize: u32,
-
-    pevt_free_buffer_queue: *mut LinkedListNode,
-
-    traces_evt_pool: *const u8,
-    tracespoolsize: u32,
-}
-
-#[derive(Debug)]
-#[repr(C, align(4))]
-struct TracesTable {
-    traces_queue: *const u8,
-}
-
-#[derive(Debug)]
-#[repr(C, align(4))]
-struct Mac802_15_4Table {
-    p_cmdrsp_buffer: *const u8,
-    p_notack_buffer: *const u8,
-    evt_queue: *const u8,
-}
-
-/// Reference table. Contains pointers to all other tables.
-#[derive(Debug, Copy, Clone)]
-#[repr(C)]
-pub struct RefTable {
-    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,
-}
-
 #[link_section = "TL_REF_TABLE"]
 pub static mut TL_REF_TABLE: MaybeUninit<RefTable> = MaybeUninit::uninit();
 
@@ -338,25 +166,21 @@ static mut BLE_CMD_BUFFER: MaybeUninit<CmdPacket> = 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();
 
-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
+static EVT_CHANNEL: Channel<CriticalSectionRawMutex, EvtBox, 32> = Channel::new();
+
 /// 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,
+static STATE: Signal<CriticalSectionRawMutex, ()> = Signal::new();
 
+pub struct TlMbox<'d> {
     _ipcc: PeripheralRef<'d, IPCC>,
 }
 
 impl<'d> TlMbox<'d> {
-    pub fn new(
+    pub fn init(
         ipcc: impl Peripheral<P = IPCC> + 'd,
         _irqs: impl interrupt::Binding<interrupt::IPCC_C1_RX, ReceiveInterruptHandler>
             + interrupt::Binding<interrupt::IPCC_C1_TX, TransmitInterruptHandler>,
@@ -365,7 +189,7 @@ impl<'d> TlMbox<'d> {
         into_ref!(ipcc);
 
         unsafe {
-            TL_REF_TABLE.as_mut_ptr().write_volatile(RefTable {
+            TL_REF_TABLE = MaybeUninit::new(RefTable {
                 device_info_table: TL_DEVICE_INFO_TABLE.as_mut_ptr(),
                 ble_table: TL_BLE_TABLE.as_ptr(),
                 thread_table: TL_THREAD_TABLE.as_ptr(),
@@ -394,23 +218,20 @@ impl<'d> TlMbox<'d> {
 
         Ipcc::enable(config);
 
-        let sys = sys::Sys::new();
-        let ble = ble::Ble::new();
-        let mm = mm::MemoryManager::new();
+        sys::Sys::enable();
+        ble::Ble::enable();
+        mm::MemoryManager::enable();
 
         // enable interrupts
-        crate::interrupt::IPCC_C1_RX::unpend();
-        crate::interrupt::IPCC_C1_TX::unpend();
+        interrupt::IPCC_C1_RX::unpend();
+        interrupt::IPCC_C1_TX::unpend();
 
-        unsafe { crate::interrupt::IPCC_C1_RX::enable() };
-        unsafe { crate::interrupt::IPCC_C1_TX::enable() };
+        unsafe { interrupt::IPCC_C1_RX::enable() };
+        unsafe { interrupt::IPCC_C1_TX::enable() };
 
-        Self {
-            sys,
-            ble,
-            _mm: mm,
-            _ipcc: ipcc,
-        }
+        STATE.reset();
+
+        Self { _ipcc: ipcc }
     }
 
     /// Returns CPU2 wireless firmware information (if present).
@@ -429,16 +250,15 @@ impl<'d> TlMbox<'d> {
     ///
     /// Internal event queu is populated in IPCC_RX_IRQ handler
     pub fn dequeue_event(&mut self) -> Option<EvtBox> {
-        HEAPLESS_EVT_QUEUE.try_recv().ok()
+        EVT_CHANNEL.try_recv().ok()
     }
 
     /// 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()));
+            let cc = block_on(LAST_CC_EVT.wait());
             LAST_CC_EVT.reset();
-
-            cc
+            Some(cc)
         } else {
             None
         }
diff --git a/embassy-stm32/src/tl_mbox/mm.rs b/embassy-stm32-wpan/src/mm.rs
index a40438499..ed13b0dbf 100644
--- a/embassy-stm32/src/tl_mbox/mm.rs
+++ b/embassy-stm32-wpan/src/mm.rs
@@ -2,18 +2,20 @@
 
 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_BUF_QUEUE, LOCAL_FREE_BUF_QUEUE, POOL_SIZE,
-    SYS_SPARE_EVT_BUF, TL_MEM_MANAGER_TABLE, TL_REF_TABLE,
+use embassy_stm32::ipcc::Ipcc;
+
+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,
 };
 
 pub(super) struct MemoryManager;
 
 impl MemoryManager {
-    pub fn new() -> Self {
+    pub fn enable() {
         unsafe {
             LinkedListNode::init_head(FREE_BUF_QUEUE.as_mut_ptr());
             LinkedListNode::init_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr());
@@ -28,8 +30,6 @@ impl MemoryManager {
                 tracespoolsize: 0,
             });
         }
-
-        MemoryManager
     }
 
     pub fn evt_drop(evt: *mut EvtPacket) {
diff --git a/embassy-stm32-wpan/src/rc.rs b/embassy-stm32-wpan/src/rc.rs
new file mode 100644
index 000000000..aae2265ed
--- /dev/null
+++ b/embassy-stm32-wpan/src/rc.rs
@@ -0,0 +1,50 @@
+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/src/tl_mbox/shci.rs b/embassy-stm32-wpan/src/shci.rs
index b19baa702..4f4d08886 100644
--- a/embassy-stm32/src/tl_mbox/shci.rs
+++ b/embassy-stm32-wpan/src/shci.rs
@@ -75,7 +75,7 @@ pub const TL_BLE_EVT_CS_PACKET_SIZE: usize = TL_EVT_HEADER_SIZE + TL_CS_EVT_SIZE
 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");
+    debug!("sending SHCI");
 
     let mut packet = ShciBleInitCmdPacket {
         header: ShciHeader::default(),
@@ -95,6 +95,6 @@ pub fn shci_ble_init(param: ShciBleInitCmdParam) {
 
         p_cmd_buffer.cmdserial.ty = TlPacketType::SysCmd as u8;
 
-        sys::send_cmd();
+        sys::Sys::send_cmd();
     }
 }
diff --git a/embassy-stm32/src/tl_mbox/sys.rs b/embassy-stm32-wpan/src/sys.rs
index c87aa440c..a19d12d27 100644
--- a/embassy-stm32/src/tl_mbox/sys.rs
+++ b/embassy-stm32-wpan/src/sys.rs
@@ -1,27 +1,27 @@
-use embassy_futures::block_on;
+use core::mem::MaybeUninit;
 
-use super::cmd::{CmdPacket, CmdSerial};
-use super::evt::{CcEvt, EvtBox, EvtSerial};
-use super::ipcc::Ipcc;
-use super::unsafe_linked_list::LinkedListNode;
-use super::{channels, SysTable, HEAPLESS_EVT_QUEUE, SYSTEM_EVT_QUEUE, SYS_CMD_BUF, TL_SYS_TABLE};
+use embassy_stm32::ipcc::Ipcc;
+
+use crate::cmd::{CmdPacket, CmdSerial};
+use crate::evt::{CcEvt, EvtBox, EvtSerial};
+use crate::tables::SysTable;
+use crate::unsafe_linked_list::LinkedListNode;
+use crate::{channels, EVT_CHANNEL, SYSTEM_EVT_QUEUE, SYS_CMD_BUF, TL_SYS_TABLE};
 
 pub struct Sys;
 
 impl Sys {
-    pub fn new() -> Self {
+    pub fn enable() {
         unsafe {
             LinkedListNode::init_head(SYSTEM_EVT_QUEUE.as_mut_ptr());
 
-            TL_SYS_TABLE.as_mut_ptr().write_volatile(SysTable {
+            TL_SYS_TABLE = MaybeUninit::new(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);
-
-        Sys
     }
 
     pub fn cmd_evt_handler() -> CcEvt {
@@ -38,12 +38,6 @@ impl Sys {
         // 5. profit
         unsafe {
             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: *const CcEvt = (*evt_serial).evt.payload.as_ptr().cast();
@@ -62,15 +56,15 @@ impl Sys {
                 let event = node_ptr.cast();
                 let event = EvtBox::new(event);
 
-                block_on(HEAPLESS_EVT_QUEUE.send(event));
+                EVT_CHANNEL.try_send(event).unwrap();
             }
         }
 
         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);
+    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/embassy-stm32-wpan/src/tables.rs b/embassy-stm32-wpan/src/tables.rs
new file mode 100644
index 000000000..151216958
--- /dev/null
+++ b/embassy-stm32-wpan/src/tables.rs
@@ -0,0 +1,175 @@
+use bit_field::BitField;
+
+use crate::cmd::{AclDataPacket, CmdPacket};
+use crate::unsafe_linked_list::LinkedListNode;
+
+#[derive(Debug, Copy, Clone)]
+#[repr(C, packed)]
+pub struct SafeBootInfoTable {
+    version: u32,
+}
+
+#[derive(Debug, Copy, Clone)]
+#[repr(C, packed)]
+pub struct RssInfoTable {
+    pub version: u32,
+    pub memory_size: u32,
+    pub 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)]
+pub struct WirelessFwInfoTable {
+    pub version: u32,
+    pub memory_size: u32,
+    pub thread_info: u32,
+    pub ble_info: u32,
+}
+
+impl WirelessFwInfoTable {
+    pub fn version_major(&self) -> u8 {
+        let version = self.version;
+        (version.get_bits(24..31) & 0xff) as u8
+    }
+
+    pub fn version_minor(&self) -> u8 {
+        let version = self.version;
+        (version.clone().get_bits(16..23) & 0xff) as u8
+    }
+
+    pub fn subversion(&self) -> u8 {
+        let version = self.version;
+        (version.clone().get_bits(8..15) & 0xff) as u8
+    }
+
+    /// Size of FLASH, expressed in number of 4K sectors.
+    pub fn flash_size(&self) -> u8 {
+        let memory_size = self.memory_size;
+        (memory_size.clone().get_bits(0..7) & 0xff) as u8
+    }
+
+    /// Size of SRAM2a, expressed in number of 1K sectors.
+    pub fn sram2a_size(&self) -> u8 {
+        let memory_size = self.memory_size;
+        (memory_size.clone().get_bits(24..31) & 0xff) as u8
+    }
+
+    /// Size of SRAM2b, expressed in number of 1K sectors.
+    pub fn sram2b_size(&self) -> u8 {
+        let memory_size = self.memory_size;
+        (memory_size.clone().get_bits(16..23) & 0xff) as u8
+    }
+}
+
+#[derive(Debug, Clone)]
+#[repr(C, align(4))]
+pub struct DeviceInfoTable {
+    pub safe_boot_info_table: SafeBootInfoTable,
+    pub rss_info_table: RssInfoTable,
+    pub wireless_fw_info_table: WirelessFwInfoTable,
+}
+
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct BleTable {
+    pub pcmd_buffer: *mut CmdPacket,
+    pub pcs_buffer: *const u8,
+    pub pevt_queue: *const u8,
+    pub phci_acl_data_buffer: *mut AclDataPacket,
+}
+
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct ThreadTable {
+    pub nostack_buffer: *const u8,
+    pub clicmdrsp_buffer: *const u8,
+    pub otcmdrsp_buffer: *const u8,
+}
+
+// TODO: use later
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct LldTestsTable {
+    pub clicmdrsp_buffer: *const u8,
+    pub m0cmd_buffer: *const u8,
+}
+
+// TODO: use later
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct BleLldTable {
+    pub cmdrsp_buffer: *const u8,
+    pub m0cmd_buffer: *const u8,
+}
+
+// TODO: use later
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct ZigbeeTable {
+    pub notif_m0_to_m4_buffer: *const u8,
+    pub appli_cmd_m4_to_m0_bufer: *const u8,
+    pub request_m0_to_m4_buffer: *const u8,
+}
+
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct SysTable {
+    pub pcmd_buffer: *mut CmdPacket,
+    pub sys_queue: *const LinkedListNode,
+}
+
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct MemManagerTable {
+    pub spare_ble_buffer: *const u8,
+    pub spare_sys_buffer: *const u8,
+
+    pub blepool: *const u8,
+    pub blepoolsize: u32,
+
+    pub pevt_free_buffer_queue: *mut LinkedListNode,
+
+    pub traces_evt_pool: *const u8,
+    pub tracespoolsize: u32,
+}
+
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct TracesTable {
+    pub traces_queue: *const u8,
+}
+
+#[derive(Debug)]
+#[repr(C, align(4))]
+pub struct Mac802_15_4Table {
+    pub p_cmdrsp_buffer: *const u8,
+    pub p_notack_buffer: *const u8,
+    pub evt_queue: *const u8,
+}
+
+/// Reference table. Contains pointers to all other tables.
+#[derive(Debug, Copy, Clone)]
+#[repr(C)]
+pub struct RefTable {
+    pub device_info_table: *const DeviceInfoTable,
+    pub ble_table: *const BleTable,
+    pub thread_table: *const ThreadTable,
+    pub sys_table: *const SysTable,
+    pub mem_manager_table: *const MemManagerTable,
+    pub traces_table: *const TracesTable,
+    pub mac_802_15_4_table: *const Mac802_15_4Table,
+}
diff --git a/embassy-stm32/src/tl_mbox/unsafe_linked_list.rs b/embassy-stm32-wpan/src/unsafe_linked_list.rs
index 482e2bf5a..52c106fa2 100644
--- a/embassy-stm32/src/tl_mbox/unsafe_linked_list.rs
+++ b/embassy-stm32-wpan/src/unsafe_linked_list.rs
@@ -57,6 +57,7 @@ impl LinkedListNode {
         });
     }
 
+    /// Remove `node` from the linked list
     pub unsafe fn remove_node(mut node: *mut LinkedListNode) {
         interrupt::free(|_| {
             (*(*node).prev).next = (*node).next;
@@ -64,6 +65,7 @@ impl LinkedListNode {
         });
     }
 
+    /// Remove `list_head` into `node`
     pub unsafe fn remove_head(mut list_head: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
         interrupt::free(|_| {
             *node = (*list_head).next;
@@ -71,10 +73,11 @@ impl LinkedListNode {
         });
     }
 
-    pub unsafe fn remove_tail(mut list_head: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+    /// Remove `list_tail` into `node`
+    pub unsafe fn remove_tail(mut list_tail: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
         interrupt::free(|_| {
-            *node = (*list_head).prev;
-            Self::remove_node((*list_head).prev);
+            *node = (*list_tail).prev;
+            Self::remove_node((*list_tail).prev);
         });
     }
 
diff --git a/embassy-stm32/src/tl_mbox/ipcc.rs b/embassy-stm32/src/ipcc.rs
index d1ac731ed..8bb0774b8 100644
--- a/embassy-stm32/src/tl_mbox/ipcc.rs
+++ b/embassy-stm32/src/ipcc.rs
@@ -167,8 +167,68 @@ impl sealed::Instance for crate::peripherals::IPCC {
 }
 
 unsafe fn _configure_pwr() {
+    let pwr = crate::pac::PWR;
     let rcc = crate::pac::RCC;
 
+    rcc.cfgr().modify(|w| w.set_stopwuck(true));
+
+    pwr.cr1().modify(|w| w.set_dbp(true));
+    pwr.cr1().modify(|w| w.set_dbp(true));
+
+    // configure LSE
+    rcc.bdcr().modify(|w| w.set_lseon(true));
+
+    // select system clock source = PLL
+    // set PLL coefficients
+    // m: 2,
+    // n: 12,
+    // r: 3,
+    // q: 4,
+    // p: 3,
+    let src_bits = 0b11;
+    let pllp = (3 - 1) & 0b11111;
+    let pllq = (4 - 1) & 0b111;
+    let pllr = (3 - 1) & 0b111;
+    let plln = 12 & 0b1111111;
+    let pllm = (2 - 1) & 0b111;
+    rcc.pllcfgr().modify(|w| {
+        w.set_pllsrc(src_bits);
+        w.set_pllm(pllm);
+        w.set_plln(plln);
+        w.set_pllr(pllr);
+        w.set_pllp(pllp);
+        w.set_pllpen(true);
+        w.set_pllq(pllq);
+        w.set_pllqen(true);
+    });
+    // enable PLL
+    rcc.cr().modify(|w| w.set_pllon(true));
+    rcc.cr().write(|w| w.set_hsion(false));
+    // while !rcc.cr().read().pllrdy() {}
+
+    // configure SYSCLK mux to use PLL clocl
+    rcc.cfgr().modify(|w| w.set_sw(0b11));
+
+    // configure CPU1 & CPU2 dividers
+    rcc.cfgr().modify(|w| w.set_hpre(0)); // not divided
+    rcc.extcfgr().modify(|w| {
+        w.set_c2hpre(0b1000); // div2
+        w.set_shdhpre(0); // not divided
+    });
+
+    // apply APB1 / APB2 values
+    rcc.cfgr().modify(|w| {
+        w.set_ppre1(0b000); // not divided
+        w.set_ppre2(0b000); // not divided
+    });
+
+    // TODO: required
     // set RF wake-up clock = LSE
     rcc.csr().modify(|w| w.set_rfwkpsel(0b01));
+
+    // set LPTIM1 & LPTIM2 clock source
+    rcc.ccipr().modify(|w| {
+        w.set_lptim1sel(0b00); // PCLK
+        w.set_lptim2sel(0b00); // PCLK
+    });
 }
diff --git a/embassy-stm32/src/lib.rs b/embassy-stm32/src/lib.rs
index 75d8af3dd..8c13774a0 100644
--- a/embassy-stm32/src/lib.rs
+++ b/embassy-stm32/src/lib.rs
@@ -41,6 +41,8 @@ pub mod crc;
 pub mod flash;
 #[cfg(all(spi_v1, rcc_f4))]
 pub mod i2s;
+#[cfg(stm32wb)]
+pub mod ipcc;
 pub mod pwm;
 #[cfg(quadspi)]
 pub mod qspi;
@@ -52,8 +54,6 @@ pub mod rtc;
 pub mod sdmmc;
 #[cfg(spi)]
 pub mod spi;
-#[cfg(stm32wb)]
-pub mod tl_mbox;
 #[cfg(usart)]
 pub mod usart;
 #[cfg(usb)]
diff --git a/embassy-stm32/src/tl_mbox/hci.rs b/embassy-stm32/src/tl_mbox/hci.rs
deleted file mode 100644
index 5bb4ba666..000000000
--- a/embassy-stm32/src/tl_mbox/hci.rs
+++ /dev/null
@@ -1,60 +0,0 @@
-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/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs b/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs
index 0525d3f37..3132ab3e4 100644
--- a/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs
+++ b/examples/stm32wb/src/bin/tl_mbox_tx_rx.rs
@@ -4,15 +4,15 @@
 
 use defmt::*;
 use embassy_executor::Spawner;
-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 embassy_stm32::bind_interrupts;
+use embassy_stm32::ipcc::Config;
+use embassy_stm32_wpan::rc::RadioCoprocessor;
+use embassy_stm32_wpan::TlMbox;
 use {defmt_rtt as _, panic_probe as _};
 
 bind_interrupts!(struct Irqs{
-    IPCC_C1_RX => tl_mbox::ReceiveInterruptHandler;
-    IPCC_C1_TX => tl_mbox::TransmitInterruptHandler;
+    IPCC_C1_RX => embassy_stm32_wpan::ReceiveInterruptHandler;
+    IPCC_C1_TX => embassy_stm32_wpan::TransmitInterruptHandler;
 });
 
 #[embassy_executor::main]
@@ -45,16 +45,16 @@ async fn main(_spawner: Spawner) {
     info!("Hello World!");
 
     let config = Config::default();
-    let mbox = TlMbox::new(p.IPCC, Irqs, config);
+    let mbox = TlMbox::init(p.IPCC, Irqs, config);
 
-    let mut rc = RadioCoprocessor::new(mbox, Default::default());
-    rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]).unwrap();
+    let mut rc = RadioCoprocessor::new(mbox);
 
     let response = rc.read().await;
     info!("coprocessor ready {}", response);
 
+    rc.write(&[0x01, 0x03, 0x0c, 0x00, 0x00]);
     let response = rc.read().await;
-    info!("coprocessor ready {}", response);
+    info!("ble reset rsp {}", response);
 
     info!("Test OK");
     cortex_m::asm::bkpt();