stm32/ipcc: move tl_mbox into `embassy-stm32-wpan`

13 files changed, 1575 insertions, 0 deletions

diff --git a/embassy-stm32-wpan/src/ble.rs b/embassy-stm32-wpan/src/ble.rs
new file mode 100644
index 000000000..4546bde07
--- /dev/null
+++ b/embassy-stm32-wpan/src/ble.rs
@@ -0,0 +1,82 @@
+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 enable() {
+        unsafe {
+            LinkedListNode::init_head(EVT_QUEUE.as_mut_ptr());
+
+            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(),
+                phci_acl_data_buffer: HCI_ACL_DATA_BUFFER.as_mut_ptr().cast(),
+            });
+        }
+
+        Ipcc::c1_set_rx_channel(channels::cpu2::IPCC_BLE_EVENT_CHANNEL, true);
+    }
+
+    pub(super) fn evt_handler() {
+        unsafe {
+            let mut node_ptr = core::ptr::null_mut();
+            let node_ptr_ptr: *mut _ = &mut node_ptr;
+
+            while !LinkedListNode::is_empty(EVT_QUEUE.as_mut_ptr()) {
+                LinkedListNode::remove_head(EVT_QUEUE.as_mut_ptr(), node_ptr_ptr);
+
+                let event = node_ptr.cast();
+                let event = EvtBox::new(event);
+
+                EVT_CHANNEL.try_send(event).unwrap();
+            }
+        }
+
+        Ipcc::c1_clear_flag_channel(channels::cpu2::IPCC_BLE_EVENT_CHANNEL);
+    }
+
+    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 ble_send_cmd(buf: &[u8]) {
+        debug!("writing ble cmd");
+
+        unsafe {
+            let pcmd_buffer: *mut CmdPacket = (*TL_REF_TABLE.assume_init().ble_table).pcmd_buffer;
+            let pcmd_serial: *mut CmdSerial = &mut (*pcmd_buffer).cmdserial;
+            let pcmd_serial_buf: *mut u8 = pcmd_serial.cast();
+
+            core::ptr::copy(buf.as_ptr(), pcmd_serial_buf, buf.len());
+
+            let 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 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;
+
+        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-wpan/src/channels.rs b/embassy-stm32-wpan/src/channels.rs
new file mode 100644
index 000000000..9a2be1cfa
--- /dev/null
+++ b/embassy-stm32-wpan/src/channels.rs
@@ -0,0 +1,96 @@
+//! CPU1                                             CPU2
+//!  |             (SYSTEM)                            |
+//!  |----HW_IPCC_SYSTEM_CMD_RSP_CHANNEL-------------->|
+//!  |                                                 |
+//!  |<---HW_IPCC_SYSTEM_EVENT_CHANNEL-----------------|
+//!  |                                                 |
+//!  |            (ZIGBEE)                             |
+//!  |----HW_IPCC_ZIGBEE_CMD_APPLI_CHANNEL------------>|
+//!  |                                                 |
+//!  |----HW_IPCC_ZIGBEE_CMD_CLI_CHANNEL-------------->|
+//!  |                                                 |
+//!  |<---HW_IPCC_ZIGBEE_APPLI_NOTIF_ACK_CHANNEL-------|
+//!  |                                                 |
+//!  |<---HW_IPCC_ZIGBEE_CLI_NOTIF_ACK_CHANNEL---------|
+//!  |                                                 |
+//!  |             (THREAD)                            |
+//!  |----HW_IPCC_THREAD_OT_CMD_RSP_CHANNEL----------->|
+//!  |                                                 |
+//!  |----HW_IPCC_THREAD_CLI_CMD_CHANNEL-------------->|
+//!  |                                                 |
+//!  |<---HW_IPCC_THREAD_NOTIFICATION_ACK_CHANNEL------|
+//!  |                                                 |
+//!  |<---HW_IPCC_THREAD_CLI_NOTIFICATION_ACK_CHANNEL--|
+//!  |                                                 |
+//!  |             (BLE)                               |
+//!  |----HW_IPCC_BLE_CMD_CHANNEL--------------------->|
+//!  |                                                 |
+//!  |----HW_IPCC_HCI_ACL_DATA_CHANNEL---------------->|
+//!  |                                                 |
+//!  |<---HW_IPCC_BLE_EVENT_CHANNEL--------------------|
+//!  |                                                 |
+//!  |             (BLE LLD)                           |
+//!  |----HW_IPCC_BLE_LLD_CMD_CHANNEL----------------->|
+//!  |                                                 |
+//!  |<---HW_IPCC_BLE_LLD_RSP_CHANNEL------------------|
+//!  |                                                 |
+//!  |<---HW_IPCC_BLE_LLD_M0_CMD_CHANNEL---------------|
+//!  |                                                 |
+//!  |             (MAC)                               |
+//!  |----HW_IPCC_MAC_802_15_4_CMD_RSP_CHANNEL-------->|
+//!  |                                                 |
+//!  |<---HW_IPCC_MAC_802_15_4_NOTIFICATION_ACK_CHANNEL|
+//!  |                                                 |
+//!  |             (BUFFER)                            |
+//!  |----HW_IPCC_MM_RELEASE_BUFFER_CHANNE------------>|
+//!  |                                                 |
+//!  |             (TRACE)                             |
+//!  |<----HW_IPCC_TRACES_CHANNEL----------------------|
+//!  |                                                 |
+//!
+
+pub mod cpu1 {
+    use embassy_stm32::ipcc::IpccChannel;
+
+    pub const IPCC_BLE_CMD_CHANNEL: IpccChannel = IpccChannel::Channel1;
+    pub const IPCC_SYSTEM_CMD_RSP_CHANNEL: IpccChannel = IpccChannel::Channel2;
+    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;
+    #[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;
+    pub const IPCC_HCI_ACL_DATA_CHANNEL: IpccChannel = IpccChannel::Channel6;
+}
+
+pub mod cpu2 {
+    use embassy_stm32::ipcc::IpccChannel;
+
+    pub const IPCC_BLE_EVENT_CHANNEL: IpccChannel = IpccChannel::Channel1;
+    pub const IPCC_SYSTEM_EVENT_CHANNEL: IpccChannel = IpccChannel::Channel2;
+    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;
+    #[allow(dead_code)] // Not used currently but reserved
+    pub const IPCC_MAC_802_15_4_NOTIFICATION_ACK_CHANNEL: IpccChannel = IpccChannel::Channel3;
+    #[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;
+    pub const IPCC_TRACES_CHANNEL: IpccChannel = IpccChannel::Channel4;
+    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;
+    #[allow(dead_code)] // Not used currently but reserved
+    pub const IPCC_BLE_LLD_CLI_RSP_CHANNEL: IpccChannel = IpccChannel::Channel5;
+    #[allow(dead_code)] // Not used currently but reserved
+    pub const IPCC_BLE_LLD_RSP_CHANNEL: IpccChannel = IpccChannel::Channel5;
+    #[allow(dead_code)] // Not used currently but reserved
+    pub const IPCC_ZIGBEE_M0_REQUEST_CHANNEL: IpccChannel = IpccChannel::Channel5;
+}
diff --git a/embassy-stm32-wpan/src/cmd.rs b/embassy-stm32-wpan/src/cmd.rs
new file mode 100644
index 000000000..1f7dae7f7
--- /dev/null
+++ b/embassy-stm32-wpan/src/cmd.rs
@@ -0,0 +1,77 @@
+use crate::evt::{EvtPacket, EvtSerial};
+use crate::{PacketHeader, TL_EVT_HEADER_SIZE};
+
+#[derive(Copy, Clone)]
+#[repr(C, packed)]
+pub struct Cmd {
+    pub cmd_code: u16,
+    pub payload_len: u8,
+    pub payload: [u8; 255],
+}
+
+impl Default for Cmd {
+    fn default() -> Self {
+        Self {
+            cmd_code: 0,
+            payload_len: 0,
+            payload: [0u8; 255],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct CmdSerial {
+    pub ty: u8,
+    pub cmd: Cmd,
+}
+
+#[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct CmdPacket {
+    pub header: PacketHeader,
+    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)
+        }
+    }
+}
+
+#[derive(Copy, Clone)]
+#[repr(C, packed)]
+pub struct AclDataSerial {
+    pub ty: u8,
+    pub handle: u16,
+    pub length: u16,
+    pub acl_data: [u8; 1],
+}
+
+#[derive(Copy, Clone)]
+#[repr(C, packed)]
+pub struct AclDataPacket {
+    pub header: PacketHeader,
+    pub acl_data_serial: AclDataSerial,
+}
diff --git a/embassy-stm32-wpan/src/consts.rs b/embassy-stm32-wpan/src/consts.rs
new file mode 100644
index 000000000..caf26c06b
--- /dev/null
+++ b/embassy-stm32-wpan/src/consts.rs
@@ -0,0 +1,55 @@
+use core::convert::TryFrom;
+
+#[derive(Debug)]
+#[repr(C)]
+pub enum TlPacketType {
+    BleCmd = 0x01,
+    AclData = 0x02,
+    BleEvt = 0x04,
+
+    OtCmd = 0x08,
+    OtRsp = 0x09,
+    CliCmd = 0x0A,
+    OtNot = 0x0C,
+    OtAck = 0x0D,
+    CliNot = 0x0E,
+    CliAck = 0x0F,
+
+    SysCmd = 0x10,
+    SysRsp = 0x11,
+    SysEvt = 0x12,
+
+    LocCmd = 0x20,
+    LocRsp = 0x21,
+
+    TracesApp = 0x40,
+    TracesWl = 0x41,
+}
+
+impl TryFrom<u8> for TlPacketType {
+    type Error = ();
+
+    fn try_from(value: u8) -> Result<Self, Self::Error> {
+        match value {
+            0x01 => Ok(TlPacketType::BleCmd),
+            0x02 => Ok(TlPacketType::AclData),
+            0x04 => Ok(TlPacketType::BleEvt),
+            0x08 => Ok(TlPacketType::OtCmd),
+            0x09 => Ok(TlPacketType::OtRsp),
+            0x0A => Ok(TlPacketType::CliCmd),
+            0x0C => Ok(TlPacketType::OtNot),
+            0x0D => Ok(TlPacketType::OtAck),
+            0x0E => Ok(TlPacketType::CliNot),
+            0x0F => Ok(TlPacketType::CliAck),
+            0x10 => Ok(TlPacketType::SysCmd),
+            0x11 => Ok(TlPacketType::SysRsp),
+            0x12 => Ok(TlPacketType::SysEvt),
+            0x20 => Ok(TlPacketType::LocCmd),
+            0x21 => Ok(TlPacketType::LocRsp),
+            0x40 => Ok(TlPacketType::TracesApp),
+            0x41 => Ok(TlPacketType::TracesWl),
+
+            _ => Err(()),
+        }
+    }
+}
diff --git a/embassy-stm32-wpan/src/evt.rs b/embassy-stm32-wpan/src/evt.rs
new file mode 100644
index 000000000..b53fe506e
--- /dev/null
+++ b/embassy-stm32-wpan/src/evt.rs
@@ -0,0 +1,176 @@
+use core::mem::MaybeUninit;
+
+use super::cmd::{AclDataPacket, AclDataSerial};
+use super::consts::TlPacketType;
+use super::{PacketHeader, TL_EVT_HEADER_SIZE};
+use crate::mm;
+
+/**
+ * The payload of `Evt` for a command status event
+ */
+#[derive(Copy, Clone)]
+#[repr(C, packed)]
+pub struct CsEvt {
+    pub status: u8,
+    pub num_cmd: u8,
+    pub cmd_code: u16,
+}
+
+/**
+ * The payload of `Evt` for a command complete event
+ */
+#[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct CcEvt {
+    pub num_cmd: u8,
+    pub cmd_code: u16,
+    pub payload: [u8; 1],
+}
+
+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(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct AsynchEvt {
+    sub_evt_code: u16,
+    payload: [u8; 1],
+}
+
+#[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct Evt {
+    pub evt_code: u8,
+    pub payload_len: u8,
+    pub payload: [u8; 1],
+}
+
+#[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct EvtSerial {
+    pub kind: u8,
+    pub evt: Evt,
+}
+
+/// This format shall be used for all events (asynchronous and command response) reported
+/// by the CPU2 except for the command response of a system command where the header is not there
+/// and the format to be used shall be `EvtSerial`.
+///
+/// ### Note:
+/// 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(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct EvtPacket {
+    pub header: PacketHeader,
+    pub evt_serial: EvtSerial,
+}
+
+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,
+}
+
+unsafe impl Send for EvtBox {}
+impl EvtBox {
+    pub(super) fn new(ptr: *mut EvtPacket) -> Self {
+        Self { ptr }
+    }
+
+    /// copies event data from inner pointer and returns an event structure
+    pub fn evt(&self) -> EvtPacket {
+        let mut evt = MaybeUninit::uninit();
+        unsafe {
+            self.ptr.copy_to(evt.as_mut_ptr(), 1);
+            evt.assume_init()
+        }
+    }
+
+    /// 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)?;
+
+            let evt_data: *const EvtPacket = self.ptr.cast();
+            let evt_serial: *const EvtSerial = &(*evt_data).evt_serial;
+            let evt_serial_buf: *const u8 = evt_serial.cast();
+
+            let acl_data: *const AclDataPacket = self.ptr.cast();
+            let acl_serial: *const AclDataSerial = &(*acl_data).acl_data_serial;
+            let acl_serial_buf: *const u8 = acl_serial.cast();
+
+            if let TlPacketType::AclData = evt_kind {
+                let len = (*acl_serial).length as usize + 5;
+                if len > buf.len() {
+                    return Err(());
+                }
+
+                core::ptr::copy(evt_serial_buf, buf.as_mut_ptr(), len);
+
+                Ok(len)
+            } else {
+                let len = (*evt_serial).evt.payload_len as usize + TL_EVT_HEADER_SIZE;
+                if len > buf.len() {
+                    return Err(());
+                }
+
+                core::ptr::copy(acl_serial_buf, buf.as_mut_ptr(), len);
+
+                Ok(len)
+            }
+        }
+    }
+
+    /// returns the size of a buffer required to hold this event
+    #[allow(clippy::result_unit_err)]
+    pub fn size(&self) -> Result<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 {
+    fn drop(&mut self) {
+        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-wpan/src/lib.rs b/embassy-stm32-wpan/src/lib.rs
new file mode 100644
index 000000000..b3206428e
--- /dev/null
+++ b/embassy-stm32-wpan/src/lib.rs
@@ -0,0 +1,266 @@
+#![no_std]
+
+// This must go FIRST so that all the other modules see its macros.
+pub mod fmt;
+
+use core::mem::MaybeUninit;
+
+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 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 mm;
+pub mod rc;
+pub mod shci;
+pub mod sys;
+pub mod tables;
+pub mod unsafe_linked_list;
+
+/// Interrupt handler.
+pub struct ReceiveInterruptHandler {}
+
+impl interrupt::Handler<interrupt::IPCC_C1_RX> for ReceiveInterruptHandler {
+    unsafe fn on_interrupt() {
+        if Ipcc::is_rx_pending(channels::cpu2::IPCC_SYSTEM_EVENT_CHANNEL) {
+            debug!("RX SYS evt");
+            sys::Sys::evt_handler();
+        } else if Ipcc::is_rx_pending(channels::cpu2::IPCC_BLE_EVENT_CHANNEL) {
+            debug!("RX BLE evt");
+            ble::Ble::evt_handler();
+        }
+
+        STATE.signal(());
+    }
+}
+
+pub struct TransmitInterruptHandler {}
+
+impl interrupt::Handler<interrupt::IPCC_C1_TX> for TransmitInterruptHandler {
+    unsafe fn on_interrupt() {
+        if Ipcc::is_tx_pending(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL) {
+            debug!("TX SYS cmd rsp");
+            let cc = sys::Sys::cmd_evt_handler();
+
+            LAST_CC_EVT.signal(cc);
+        } else if Ipcc::is_tx_pending(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL) {
+            debug!("TX MM release");
+            mm::MemoryManager::free_buf_handler();
+        } else if Ipcc::is_tx_pending(channels::cpu1::IPCC_HCI_ACL_DATA_CHANNEL) {
+            debug!("TX HCI acl");
+            ble::Ble::acl_data_handler();
+        }
+
+        STATE.signal(());
+    }
+}
+
+#[link_section = "TL_REF_TABLE"]
+pub static mut TL_REF_TABLE: MaybeUninit<RefTable> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM1"]
+static mut TL_DEVICE_INFO_TABLE: MaybeUninit<DeviceInfoTable> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM1"]
+static mut TL_BLE_TABLE: MaybeUninit<BleTable> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM1"]
+static mut TL_THREAD_TABLE: MaybeUninit<ThreadTable> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM1"]
+static mut TL_SYS_TABLE: MaybeUninit<SysTable> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM1"]
+static mut TL_MEM_MANAGER_TABLE: MaybeUninit<MemManagerTable> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM1"]
+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_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_MEM2"]
+static mut TRACES_EVT_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]> =
+    MaybeUninit::uninit();
+
+#[link_section = "MB_MEM2"]
+static mut EVT_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM2"]
+static mut SYSTEM_EVT_QUEUE: MaybeUninit<LinkedListNode> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM2"]
+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();
+
+#[link_section = "MB_MEM2"]
+static mut SYS_SPARE_EVT_BUF: MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + TL_EVT_HEADER_SIZE + 255]> =
+    MaybeUninit::uninit();
+
+#[link_section = "MB_MEM2"]
+static mut BLE_SPARE_EVT_BUF: MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + TL_EVT_HEADER_SIZE + 255]> =
+    MaybeUninit::uninit();
+
+#[link_section = "MB_MEM2"]
+static mut BLE_CMD_BUFFER: MaybeUninit<CmdPacket> = MaybeUninit::uninit();
+
+#[link_section = "MB_MEM2"]
+//                                 fuck these "magic" numbers from ST ---v---v
+static mut HCI_ACL_DATA_BUFFER: MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + 5 + 251]> = MaybeUninit::uninit();
+
+/// 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();
+
+static STATE: Signal<CriticalSectionRawMutex, ()> = Signal::new();
+
+pub struct TlMbox<'d> {
+    _ipcc: PeripheralRef<'d, IPCC>,
+}
+
+impl<'d> TlMbox<'d> {
+    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>,
+        config: Config,
+    ) -> Self {
+        into_ref!(ipcc);
+
+        unsafe {
+            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(),
+                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(),
+            });
+
+            TL_SYS_TABLE = MaybeUninit::zeroed();
+            TL_DEVICE_INFO_TABLE = MaybeUninit::zeroed();
+            TL_BLE_TABLE = MaybeUninit::zeroed();
+            TL_THREAD_TABLE = MaybeUninit::zeroed();
+            TL_MEM_MANAGER_TABLE = MaybeUninit::zeroed();
+            TL_TRACES_TABLE = MaybeUninit::zeroed();
+            TL_MAC_802_15_4_TABLE = MaybeUninit::zeroed();
+
+            EVT_POOL = MaybeUninit::zeroed();
+            SYS_SPARE_EVT_BUF = MaybeUninit::zeroed();
+            BLE_SPARE_EVT_BUF = MaybeUninit::zeroed();
+
+            CS_BUFFER = MaybeUninit::zeroed();
+            BLE_CMD_BUFFER = MaybeUninit::zeroed();
+            HCI_ACL_DATA_BUFFER = MaybeUninit::zeroed();
+        }
+
+        Ipcc::enable(config);
+
+        sys::Sys::enable();
+        ble::Ble::enable();
+        mm::MemoryManager::enable();
+
+        // enable interrupts
+        interrupt::IPCC_C1_RX::unpend();
+        interrupt::IPCC_C1_TX::unpend();
+
+        unsafe { interrupt::IPCC_C1_RX::enable() };
+        unsafe { interrupt::IPCC_C1_TX::enable() };
+
+        STATE.reset();
+
+        Self { _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
+        if info.version != 0 {
+            Some(*info)
+        } else {
+            None
+        }
+    }
+
+    /// picks single [`EvtBox`] from internal event queue.
+    ///
+    /// Internal event queu is populated in IPCC_RX_IRQ handler
+    pub fn dequeue_event(&mut self) -> Option<EvtBox> {
+        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 = block_on(LAST_CC_EVT.wait());
+            LAST_CC_EVT.reset();
+            Some(cc)
+        } else {
+            None
+        }
+    }
+}
diff --git a/embassy-stm32-wpan/src/mm.rs b/embassy-stm32-wpan/src/mm.rs
new file mode 100644
index 000000000..ed13b0dbf
--- /dev/null
+++ b/embassy-stm32-wpan/src/mm.rs
@@ -0,0 +1,75 @@
+//! Memory manager routines
+
+use core::mem::MaybeUninit;
+
+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 enable() {
+        unsafe {
+            LinkedListNode::init_head(FREE_BUF_QUEUE.as_mut_ptr());
+            LinkedListNode::init_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr());
+
+            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(),
+                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(),
+                tracespoolsize: 0,
+            });
+        }
+    }
+
+    pub fn evt_drop(evt: *mut EvtPacket) {
+        unsafe {
+            let list_node = evt.cast();
+
+            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);
+
+            // 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);
+            }
+        }
+    }
+
+    /// 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;
+
+            while !LinkedListNode::is_empty(LOCAL_FREE_BUF_QUEUE.as_mut_ptr()) {
+                LinkedListNode::remove_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), node_ptr_ptr);
+                LinkedListNode::insert_tail(
+                    (*(*TL_REF_TABLE.as_ptr()).mem_manager_table).pevt_free_buffer_queue,
+                    node_ptr,
+                );
+            }
+        }
+    }
+
+    /// free buffer channel interrupt handler
+    pub fn free_buf_handler() {
+        Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL, false);
+        Self::send_free_buf();
+        Ipcc::c1_set_flag_channel(channels::cpu1::IPCC_MM_RELEASE_BUFFER_CHANNEL);
+    }
+}
diff --git a/embassy-stm32-wpan/src/rc.rs b/embassy-stm32-wpan/src/rc.rs
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-wpan/src/shci.rs b/embassy-stm32-wpan/src/shci.rs
new file mode 100644
index 000000000..4f4d08886
--- /dev/null
+++ b/embassy-stm32-wpan/src/shci.rs
@@ -0,0 +1,100 @@
+use super::cmd::CmdPacket;
+use super::consts::TlPacketType;
+use super::{sys, TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE, TL_SYS_TABLE};
+
+const SCHI_OPCODE_BLE_INIT: u16 = 0xfc66;
+
+#[derive(Debug, Clone, Copy)]
+#[repr(C, packed)]
+pub struct ShciBleInitCmdParam {
+    /// NOT USED CURRENTLY
+    pub p_ble_buffer_address: u32,
+
+    /// Size of the Buffer allocated in pBleBufferAddress
+    pub ble_buffer_size: u32,
+
+    pub num_attr_record: u16,
+    pub num_attr_serv: u16,
+    pub attr_value_arr_size: u16,
+    pub num_of_links: u8,
+    pub extended_packet_length_enable: u8,
+    pub pr_write_list_size: u8,
+    pub mb_lock_count: u8,
+
+    pub att_mtu: u16,
+    pub slave_sca: u16,
+    pub master_sca: u8,
+    pub ls_source: u8,
+    pub max_conn_event_length: u32,
+    pub hs_startup_time: u16,
+    pub viterbi_enable: u8,
+    pub ll_only: u8,
+    pub hw_version: u8,
+}
+
+impl Default for ShciBleInitCmdParam {
+    fn default() -> Self {
+        Self {
+            p_ble_buffer_address: 0,
+            ble_buffer_size: 0,
+            num_attr_record: 68,
+            num_attr_serv: 8,
+            attr_value_arr_size: 1344,
+            num_of_links: 2,
+            extended_packet_length_enable: 1,
+            pr_write_list_size: 0x3A,
+            mb_lock_count: 0x79,
+            att_mtu: 156,
+            slave_sca: 500,
+            master_sca: 0,
+            ls_source: 1,
+            max_conn_event_length: 0xFFFFFFFF,
+            hs_startup_time: 0x148,
+            viterbi_enable: 1,
+            ll_only: 0,
+            hw_version: 0,
+        }
+    }
+}
+
+#[derive(Debug, Clone, Copy, Default)]
+#[repr(C, packed)]
+pub struct ShciHeader {
+    metadata: [u32; 3],
+}
+
+#[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!("sending SHCI");
+
+    let mut packet = ShciBleInitCmdPacket {
+        header: ShciHeader::default(),
+        param,
+    };
+
+    let packet_ptr: *mut _ = &mut packet;
+
+    unsafe {
+        let cmd_ptr: *mut CmdPacket = packet_ptr.cast();
+
+        (*cmd_ptr).cmdserial.cmd.cmd_code = SCHI_OPCODE_BLE_INIT;
+        (*cmd_ptr).cmdserial.cmd.payload_len = core::mem::size_of::<ShciBleInitCmdParam>() as u8;
+
+        let mut p_cmd_buffer = &mut *(*TL_SYS_TABLE.as_mut_ptr()).pcmd_buffer;
+        core::ptr::write(p_cmd_buffer, *cmd_ptr);
+
+        p_cmd_buffer.cmdserial.ty = TlPacketType::SysCmd as u8;
+
+        sys::Sys::send_cmd();
+    }
+}
diff --git a/embassy-stm32-wpan/src/sys.rs b/embassy-stm32-wpan/src/sys.rs
new file mode 100644
index 000000000..a19d12d27
--- /dev/null
+++ b/embassy-stm32-wpan/src/sys.rs
@@ -0,0 +1,70 @@
+use core::mem::MaybeUninit;
+
+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 enable() {
+        unsafe {
+            LinkedListNode::init_head(SYSTEM_EVT_QUEUE.as_mut_ptr());
+
+            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);
+    }
+
+    pub fn cmd_evt_handler() -> CcEvt {
+        Ipcc::c1_set_tx_channel(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL, false);
+
+        // ST's command response data structure is really convoluted.
+        //
+        // for command response events on SYS channel, the header is missing
+        // and one should:
+        // 1. interpret the content of CMD_BUFFER as CmdPacket
+        // 2. Access CmdPacket's cmdserial field and interpret its content as EvtSerial
+        // 3. Access EvtSerial's evt field (as Evt) and interpret its payload as CcEvt
+        // 4. CcEvt type is the actual SHCI response
+        // 5. profit
+        unsafe {
+            let pcmd: *const CmdPacket = (*TL_SYS_TABLE.as_ptr()).pcmd_buffer;
+            let cmd_serial: *const CmdSerial = &(*pcmd).cmdserial;
+            let evt_serial: *const EvtSerial = cmd_serial.cast();
+            let cc: *const CcEvt = (*evt_serial).evt.payload.as_ptr().cast();
+            *cc
+        }
+    }
+
+    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);
+
+                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);
+    }
+}
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-wpan/src/unsafe_linked_list.rs b/embassy-stm32-wpan/src/unsafe_linked_list.rs
new file mode 100644
index 000000000..52c106fa2
--- /dev/null
+++ b/embassy-stm32-wpan/src/unsafe_linked_list.rs
@@ -0,0 +1,128 @@
+//! Unsafe linked list.
+//! Translated from ST's C by `c2rust` tool.
+
+#![allow(
+    dead_code,
+    mutable_transmutes,
+    non_camel_case_types,
+    non_snake_case,
+    non_upper_case_globals,
+    unused_assignments,
+    unused_mut
+)]
+
+use cortex_m::interrupt;
+
+#[derive(Copy, Clone)]
+#[repr(C, packed(4))]
+pub struct LinkedListNode {
+    pub next: *mut LinkedListNode,
+    pub prev: *mut LinkedListNode,
+}
+
+impl Default for LinkedListNode {
+    fn default() -> Self {
+        LinkedListNode {
+            next: core::ptr::null_mut(),
+            prev: core::ptr::null_mut(),
+        }
+    }
+}
+
+impl LinkedListNode {
+    pub unsafe fn init_head(mut list_head: *mut LinkedListNode) {
+        (*list_head).next = list_head;
+        (*list_head).prev = list_head;
+    }
+
+    pub unsafe fn is_empty(mut list_head: *mut LinkedListNode) -> bool {
+        interrupt::free(|_| ((*list_head).next) == list_head)
+    }
+
+    pub unsafe fn insert_head(mut list_head: *mut LinkedListNode, mut node: *mut LinkedListNode) {
+        interrupt::free(|_| {
+            (*node).next = (*list_head).next;
+            (*node).prev = list_head;
+            (*list_head).next = node;
+            (*(*node).next).prev = node;
+        });
+    }
+
+    pub unsafe fn insert_tail(mut list_head: *mut LinkedListNode, mut node: *mut LinkedListNode) {
+        interrupt::free(|_| {
+            (*node).next = list_head;
+            (*node).prev = (*list_head).prev;
+            (*list_head).prev = node;
+            (*(*node).prev).next = node;
+        });
+    }
+
+    /// Remove `node` from the linked list
+    pub unsafe fn remove_node(mut node: *mut LinkedListNode) {
+        interrupt::free(|_| {
+            (*(*node).prev).next = (*node).next;
+            (*(*node).next).prev = (*node).prev;
+        });
+    }
+
+    /// 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;
+            Self::remove_node((*list_head).next);
+        });
+    }
+
+    /// Remove `list_tail` into `node`
+    pub unsafe fn remove_tail(mut list_tail: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+        interrupt::free(|_| {
+            *node = (*list_tail).prev;
+            Self::remove_node((*list_tail).prev);
+        });
+    }
+
+    pub unsafe fn insert_node_after(mut node: *mut LinkedListNode, mut ref_node: *mut LinkedListNode) {
+        interrupt::free(|_| {
+            (*node).next = (*ref_node).next;
+            (*node).prev = ref_node;
+            (*ref_node).next = node;
+            (*(*node).next).prev = node;
+        });
+    }
+
+    pub unsafe fn insert_node_before(mut node: *mut LinkedListNode, mut ref_node: *mut LinkedListNode) {
+        interrupt::free(|_| {
+            (*node).next = ref_node;
+            (*node).prev = (*ref_node).prev;
+            (*ref_node).prev = node;
+            (*(*node).prev).next = node;
+        });
+    }
+
+    pub unsafe fn get_size(mut list_head: *mut LinkedListNode) -> usize {
+        interrupt::free(|_| {
+            let mut size = 0;
+            let mut temp: *mut LinkedListNode = core::ptr::null_mut::<LinkedListNode>();
+
+            temp = (*list_head).next;
+            while temp != list_head {
+                size += 1;
+                temp = (*temp).next
+            }
+
+            size
+        })
+    }
+
+    pub unsafe fn get_next_node(mut ref_node: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+        interrupt::free(|_| {
+            *node = (*ref_node).next;
+        });
+    }
+
+    pub unsafe fn get_prev_node(mut ref_node: *mut LinkedListNode, mut node: *mut *mut LinkedListNode) {
+        interrupt::free(|_| {
+            *node = (*ref_node).prev;
+        });
+    }
+}