27 files changed, 4878 insertions, 0 deletions

diff --git a/embassy-stm32-wpan/src/wb55/channels.rs b/embassy-stm32-wpan/src/wb55/channels.rs
new file mode 100644
index 000000000..58f857136
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/channels.rs
@@ -0,0 +1,107 @@
+//! 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----------------------|
+//!  |                                                 |
+//!
+//!
+
+#[repr(u8)]
+pub enum IpccChannel {
+    Channel1 = 1,
+    Channel2 = 2,
+    Channel3 = 3,
+    Channel4 = 4,
+    Channel5 = 5,
+    Channel6 = 6,
+}
+
+pub mod cpu1 {
+    use super::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 super::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/wb55/cmd.rs b/embassy-stm32-wpan/src/wb55/cmd.rs
new file mode 100644
index 000000000..34f02d6e7
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/cmd.rs
@@ -0,0 +1,109 @@
+use core::ptr;
+use core::sync::atomic::{Ordering, compiler_fence};
+
+use crate::consts::TlPacketType;
+use crate::wb55::PacketHeader;
+
+#[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 CmdSerialStub {
+    pub ty: u8,
+    pub cmd_code: u16,
+    pub payload_len: u8,
+}
+
+#[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct CmdPacket {
+    pub header: PacketHeader,
+    pub cmdserial: CmdSerial,
+}
+
+impl CmdPacket {
+    pub unsafe fn write_into(cmd_buf: *mut CmdPacket, packet_type: TlPacketType, cmd_code: u16, payload: &[u8]) {
+        let p_cmd_serial = (cmd_buf as *mut u8).add(size_of::<PacketHeader>());
+        let p_payload = p_cmd_serial.add(size_of::<CmdSerialStub>());
+
+        ptr::write_unaligned(
+            p_cmd_serial as *mut _,
+            CmdSerialStub {
+                ty: packet_type as u8,
+                cmd_code,
+                payload_len: payload.len() as u8,
+            },
+        );
+
+        ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len());
+
+        compiler_fence(Ordering::Release);
+    }
+}
+
+#[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 AclDataSerialStub {
+    pub ty: u8,
+    pub handle: u16,
+    pub length: u16,
+}
+
+#[derive(Copy, Clone)]
+#[repr(C, packed)]
+pub struct AclDataPacket {
+    pub header: PacketHeader,
+    pub acl_data_serial: AclDataSerial,
+}
+
+impl AclDataPacket {
+    pub unsafe fn write_into(cmd_buf: *mut AclDataPacket, packet_type: TlPacketType, handle: u16, payload: &[u8]) {
+        let p_cmd_serial = (cmd_buf as *mut u8).add(size_of::<PacketHeader>());
+        let p_payload = p_cmd_serial.add(size_of::<AclDataSerialStub>());
+
+        ptr::write_unaligned(
+            p_cmd_serial as *mut _,
+            AclDataSerialStub {
+                ty: packet_type as u8,
+                handle: handle,
+                length: payload.len() as u16,
+            },
+        );
+
+        ptr::copy_nonoverlapping(payload as *const _ as *const u8, p_payload, payload.len());
+
+        compiler_fence(Ordering::Release);
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/consts.rs b/embassy-stm32-wpan/src/wb55/consts.rs
new file mode 100644
index 000000000..659e74e69
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/consts.rs
@@ -0,0 +1,94 @@
+use crate::evt::CsEvt;
+use crate::wb55::PacketHeader;
+
+#[derive(Debug)]
+#[repr(C)]
+pub enum TlPacketType {
+    MacCmd = 0x00,
+
+    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(()),
+        }
+    }
+}
+
+pub const TL_PACKET_HEADER_SIZE: usize = core::mem::size_of::<PacketHeader>();
+pub const TL_EVT_HEADER_SIZE: usize = 3;
+pub const TL_CS_EVT_SIZE: usize = core::mem::size_of::<CsEvt>();
+
+/**
+ * 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 too 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).
+ */
+pub const CFG_TL_BLE_EVT_QUEUE_LENGTH: usize = 5;
+pub const CFG_TL_BLE_MOST_EVENT_PAYLOAD_SIZE: usize = 255;
+pub const TL_BLE_EVENT_FRAME_SIZE: usize = TL_EVT_HEADER_SIZE + CFG_TL_BLE_MOST_EVENT_PAYLOAD_SIZE;
+
+pub const POOL_SIZE: usize = CFG_TL_BLE_EVT_QUEUE_LENGTH * 4 * divc(TL_PACKET_HEADER_SIZE + TL_BLE_EVENT_FRAME_SIZE, 4);
+pub const C_SIZE_CMD_STRING: usize = 256;
+
+pub const fn divc(x: usize, y: usize) -> usize {
+    (x + y - 1) / y
+}
+
+pub const TL_BLE_EVT_CS_PACKET_SIZE: usize = TL_EVT_HEADER_SIZE + TL_CS_EVT_SIZE;
+#[allow(dead_code)]
+pub const TL_BLE_EVT_CS_BUFFER_SIZE: usize = TL_PACKET_HEADER_SIZE + TL_BLE_EVT_CS_PACKET_SIZE;
+
+pub const TL_BLEEVT_CC_OPCODE: u8 = 0x0E;
+pub const TL_BLEEVT_CS_OPCODE: u8 = 0x0F;
+pub const TL_BLEEVT_VS_OPCODE: u8 = 0xFF;
diff --git a/embassy-stm32-wpan/src/wb55/evt.rs b/embassy-stm32-wpan/src/wb55/evt.rs
new file mode 100644
index 000000000..f32821269
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/evt.rs
@@ -0,0 +1,152 @@
+use core::marker::PhantomData;
+use core::{ptr, slice};
+
+use super::PacketHeader;
+use crate::consts::TL_EVT_HEADER_SIZE;
+
+/**
+ * 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)]
+#[repr(C, packed)]
+pub struct Evt {
+    pub evt_code: u8,
+    pub payload_len: u8,
+    pub payload: [u8; 255],
+}
+
+#[derive(Copy, Clone)]
+#[repr(C, packed)]
+pub struct EvtSerial {
+    pub kind: u8,
+    pub evt: Evt,
+}
+
+#[derive(Copy, Clone, Default)]
+#[repr(C, packed)]
+pub struct EvtStub {
+    pub kind: u8,
+    pub evt_code: u8,
+    pub payload_len: u8,
+}
+
+/// This format shall be used for all events (asynchronous and command response) reported
+/// by the CPU2 except for the command response of a system command where the header is not there
+/// and the format to be used shall be `EvtSerial`.
+///
+/// ### 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)]
+#[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
+    }
+}
+
+pub trait MemoryManager {
+    unsafe fn drop_event_packet(evt: *mut EvtPacket);
+}
+
+/// smart pointer to the [`EvtPacket`] that will dispose of [`EvtPacket`] buffer automatically
+/// on [`Drop`]
+#[derive(Debug)]
+pub struct EvtBox<T: MemoryManager> {
+    ptr: *mut EvtPacket,
+    mm: PhantomData<T>,
+}
+
+unsafe impl<T: MemoryManager> Send for EvtBox<T> {}
+impl<T: MemoryManager> EvtBox<T> {
+    pub(super) fn new(ptr: *mut EvtPacket) -> Self {
+        Self { ptr, mm: PhantomData }
+    }
+
+    /// Returns information about the event
+    pub fn stub(&self) -> EvtStub {
+        unsafe {
+            let p_evt_stub = &(*self.ptr).evt_serial as *const _ as *const EvtStub;
+
+            ptr::read_volatile(p_evt_stub)
+        }
+    }
+
+    pub fn payload<'a>(&'a self) -> &'a [u8] {
+        unsafe {
+            let p_payload_len = &(*self.ptr).evt_serial.evt.payload_len as *const u8;
+            let p_payload = &(*self.ptr).evt_serial.evt.payload as *const u8;
+
+            let payload_len = ptr::read_volatile(p_payload_len);
+
+            slice::from_raw_parts(p_payload, payload_len as usize)
+        }
+    }
+
+    pub fn serial<'a>(&'a self) -> &'a [u8] {
+        unsafe {
+            let evt_serial: *const EvtSerial = &(*self.ptr).evt_serial;
+            let evt_serial_buf: *const u8 = evt_serial.cast();
+
+            let len = (*evt_serial).evt.payload_len as usize + TL_EVT_HEADER_SIZE;
+
+            slice::from_raw_parts(evt_serial_buf, len)
+        }
+    }
+}
+
+impl<T: MemoryManager> Drop for EvtBox<T> {
+    fn drop(&mut self) {
+        unsafe { T::drop_event_packet(self.ptr) };
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/fmt.rs b/embassy-stm32-wpan/src/wb55/fmt.rs
new file mode 100644
index 000000000..8ca61bc39
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/fmt.rs
@@ -0,0 +1,270 @@
+#![macro_use]
+#![allow(unused)]
+
+use core::fmt::{Debug, Display, LowerHex};
+
+#[cfg(all(feature = "defmt", feature = "log"))]
+compile_error!("You may not enable both `defmt` and `log` features.");
+
+#[collapse_debuginfo(yes)]
+macro_rules! assert {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::assert!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::assert!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+macro_rules! assert_eq {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::assert_eq!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::assert_eq!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+macro_rules! assert_ne {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::assert_ne!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::assert_ne!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+macro_rules! debug_assert {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::debug_assert!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::debug_assert!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+macro_rules! debug_assert_eq {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::debug_assert_eq!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::debug_assert_eq!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+macro_rules! debug_assert_ne {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::debug_assert_ne!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::debug_assert_ne!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+macro_rules! todo {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::todo!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::todo!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+macro_rules! unreachable {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::unreachable!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::unreachable!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+macro_rules! panic {
+    ($($x:tt)*) => {
+        {
+            #[cfg(not(feature = "defmt"))]
+            ::core::panic!($($x)*);
+            #[cfg(feature = "defmt")]
+            ::defmt::panic!($($x)*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+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 ),*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+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 ),*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+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 ),*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+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 ),*);
+        }
+    };
+}
+
+#[collapse_debuginfo(yes)]
+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")]
+#[collapse_debuginfo(yes)]
+macro_rules! unwrap {
+    ($($x:tt)*) => {
+        ::defmt::unwrap!($($x)*)
+    };
+}
+
+#[cfg(not(feature = "defmt"))]
+#[collapse_debuginfo(yes)]
+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
+    }
+}
+
+pub(crate) struct Bytes<'a>(pub &'a [u8]);
+
+impl<'a> Debug for Bytes<'a> {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        write!(f, "{:#02x?}", self.0)
+    }
+}
+
+impl<'a> Display for Bytes<'a> {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        write!(f, "{:#02x?}", self.0)
+    }
+}
+
+impl<'a> LowerHex for Bytes<'a> {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        write!(f, "{:#02x?}", self.0)
+    }
+}
+
+#[cfg(feature = "defmt")]
+impl<'a> defmt::Format for Bytes<'a> {
+    fn format(&self, fmt: defmt::Formatter) {
+        defmt::write!(fmt, "{:02x}", self.0)
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/lhci.rs b/embassy-stm32-wpan/src/wb55/lhci.rs
new file mode 100644
index 000000000..59c8bfb5d
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/lhci.rs
@@ -0,0 +1,112 @@
+use core::ptr;
+
+use crate::cmd::CmdPacket;
+use crate::consts::{TL_EVT_HEADER_SIZE, TlPacketType};
+use crate::evt::{CcEvt, EvtPacket, EvtSerial};
+use crate::tables::{DeviceInfoTable, RssInfoTable, SafeBootInfoTable, TL_DEVICE_INFO_TABLE, WirelessFwInfoTable};
+
+const TL_BLEEVT_CC_OPCODE: u8 = 0x0e;
+const LHCI_OPCODE_C1_DEVICE_INF: u16 = 0xfd62;
+
+const PACKAGE_DATA_PTR: *const u8 = 0x1FFF_7500 as _;
+const UID64_PTR: *const u32 = 0x1FFF_7580 as _;
+
+#[derive(Debug, Copy, Clone)]
+#[repr(C, packed)]
+pub struct LhciC1DeviceInformationCcrp {
+    pub status: u8,
+    pub rev_id: u16,
+    pub dev_code_id: u16,
+    pub package_type: u8,
+    pub device_type_id: u8,
+    pub st_company_id: u32,
+    pub uid64: u32,
+
+    pub uid96_0: u32,
+    pub uid96_1: u32,
+    pub uid96_2: u32,
+
+    pub safe_boot_info_table: SafeBootInfoTable,
+    pub rss_info_table: RssInfoTable,
+    pub wireless_fw_info_table: WirelessFwInfoTable,
+
+    pub app_fw_inf: u32,
+}
+
+impl Default for LhciC1DeviceInformationCcrp {
+    fn default() -> Self {
+        let DeviceInfoTable {
+            safe_boot_info_table,
+            rss_info_table,
+            wireless_fw_info_table,
+        } = unsafe { ptr::read_volatile(TL_DEVICE_INFO_TABLE.as_ptr()) };
+
+        let device_id = stm32_device_signature::device_id();
+        let uid96_0 = (device_id[3] as u32) << 24
+            | (device_id[2] as u32) << 16
+            | (device_id[1] as u32) << 8
+            | device_id[0] as u32;
+        let uid96_1 = (device_id[7] as u32) << 24
+            | (device_id[6] as u32) << 16
+            | (device_id[5] as u32) << 8
+            | device_id[4] as u32;
+        let uid96_2 = (device_id[11] as u32) << 24
+            | (device_id[10] as u32) << 16
+            | (device_id[9] as u32) << 8
+            | device_id[8] as u32;
+
+        let package_type = unsafe { *PACKAGE_DATA_PTR };
+        let uid64 = unsafe { *UID64_PTR };
+        let st_company_id = unsafe { *UID64_PTR.offset(1) } >> 8 & 0x00FF_FFFF;
+        let device_type_id = (unsafe { *UID64_PTR.offset(1) } & 0x000000FF) as u8;
+
+        LhciC1DeviceInformationCcrp {
+            status: 0,
+            rev_id: 0,
+            dev_code_id: 0,
+            package_type,
+            device_type_id,
+            st_company_id,
+            uid64,
+            uid96_0,
+            uid96_1,
+            uid96_2,
+            safe_boot_info_table,
+            rss_info_table,
+            wireless_fw_info_table,
+            app_fw_inf: (1 << 8), // 0.0.1
+        }
+    }
+}
+
+impl LhciC1DeviceInformationCcrp {
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    pub fn write(&self, cmd_packet: &mut CmdPacket) {
+        let self_size = core::mem::size_of::<LhciC1DeviceInformationCcrp>();
+
+        unsafe {
+            let cmd_packet_ptr: *mut CmdPacket = cmd_packet;
+            let evet_packet_ptr: *mut EvtPacket = cmd_packet_ptr.cast();
+
+            let evt_serial: *mut EvtSerial = &mut (*evet_packet_ptr).evt_serial;
+            let evt_payload = (*evt_serial).evt.payload.as_mut_ptr();
+            let evt_cc: *mut CcEvt = evt_payload.cast();
+            let evt_cc_payload_buf = (*evt_cc).payload.as_mut_ptr();
+
+            (*evt_serial).kind = TlPacketType::LocRsp as u8;
+            (*evt_serial).evt.evt_code = TL_BLEEVT_CC_OPCODE;
+            (*evt_serial).evt.payload_len = TL_EVT_HEADER_SIZE as u8 + self_size as u8;
+
+            (*evt_cc).cmd_code = LHCI_OPCODE_C1_DEVICE_INF;
+            (*evt_cc).num_cmd = 1;
+
+            let self_ptr: *const LhciC1DeviceInformationCcrp = self;
+            let self_buf = self_ptr.cast();
+
+            ptr::copy(self_buf, evt_cc_payload_buf, self_size);
+        }
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/mac/commands.rs b/embassy-stm32-wpan/src/wb55/mac/commands.rs
new file mode 100644
index 000000000..d96f0094a
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/commands.rs
@@ -0,0 +1,509 @@
+#![allow(unused)]
+
+use core::{mem, slice};
+
+use smoltcp::wire::ieee802154::Frame;
+
+use super::opcodes::OpcodeM4ToM0;
+use super::typedefs::{
+    AddressMode, Capabilities, DisassociationReason, GtsCharacteristics, KeyIdMode, MacAddress, MacChannel, MacStatus,
+    PanId, PibId, ScanType, SecurityLevel,
+};
+
+pub trait MacCommand: Sized {
+    const OPCODE: OpcodeM4ToM0;
+
+    fn payload<'a>(&'a self) -> &'a [u8] {
+        unsafe { slice::from_raw_parts(self as *const _ as *const u8, mem::size_of::<Self>()) }
+    }
+}
+
+/// MLME ASSOCIATE Request used to request an association
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct AssociateRequest {
+    /// the logical channel on which to attempt association
+    pub channel_number: MacChannel,
+    /// the channel page on which to attempt association
+    pub channel_page: u8,
+    /// coordinator addressing mode
+    pub coord_addr_mode: AddressMode,
+    /// operational capabilities of the associating device
+    pub capability_information: Capabilities,
+    /// the identifier of the PAN with which to associate
+    pub coord_pan_id: PanId,
+    /// the security level to be used
+    pub security_level: SecurityLevel,
+    /// the mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// the originator of the key to be used
+    pub key_source: [u8; 8],
+    /// Coordinator address
+    pub coord_address: MacAddress,
+    /// the index of the key to be used
+    pub key_index: u8,
+}
+
+impl MacCommand for AssociateRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeAssociateReq;
+}
+
+/// MLME DISASSOCIATE Request sed to request a disassociation
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DisassociateRequest {
+    /// device addressing mode used
+    pub device_addr_mode: AddressMode,
+    /// the identifier of the PAN of the device
+    pub device_pan_id: PanId,
+    /// the reason for the disassociation
+    pub disassociation_reason: DisassociationReason,
+    /// device address
+    pub device_address: MacAddress,
+    /// `true` if the disassociation notification command is to be sent indirectly
+    pub tx_indirect: bool,
+    /// the security level to be used
+    pub security_level: SecurityLevel,
+    /// the mode to be used to indetify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// the index of the key to be used
+    pub key_index: u8,
+    /// the originator of the key to be used
+    pub key_source: [u8; 8],
+}
+
+impl MacCommand for DisassociateRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeDisassociateReq;
+}
+
+/// MLME GET Request used to request a PIB value
+#[repr(C)]
+#[derive(Default)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct GetRequest {
+    /// the name of the PIB attribute to read
+    pub pib_attribute: PibId,
+
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 3],
+}
+
+impl MacCommand for GetRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeGetReq;
+}
+
+/// MLME GTS Request used to request and maintain GTSs
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct GtsRequest {
+    /// the characteristics of the GTS
+    pub characteristics: GtsCharacteristics,
+    /// the security level to be used
+    pub security_level: SecurityLevel,
+    /// the mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// the index of the key to be used
+    pub key_index: u8,
+    /// the originator of the key to be used
+    pub key_source: [u8; 8],
+}
+
+impl MacCommand for GtsRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeGetReq;
+}
+
+#[repr(C)]
+#[derive(Default)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct ResetRequest {
+    /// MAC PIB attributes are set to their default values or not during reset
+    pub set_default_pib: bool,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 3],
+}
+
+impl MacCommand for ResetRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeResetReq;
+}
+
+/// MLME RX ENABLE Request used to request that the receiver is either enabled
+/// for a finite period of time or disabled
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct RxEnableRequest {
+    /// the request operation can be deferred or not
+    pub defer_permit: bool,
+    /// configure the transceiver to RX with ranging for a value of
+    /// RANGING_ON or to not enable ranging for RANGING_OFF
+    pub ranging_rx_control: u8,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 2],
+    /// number of symbols measured before the receiver is to be enabled or disabled
+    pub rx_on_time: [u8; 4],
+    /// number of symbols for which the receiver is to be enabled
+    pub rx_on_duration: [u8; 4],
+}
+
+impl MacCommand for RxEnableRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeRxEnableReq;
+}
+
+/// MLME SCAN Request used to initiate a channel scan over a given list of channels
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct ScanRequest {
+    /// the type of scan to be performed
+    pub scan_type: ScanType,
+    /// the time spent on scanning each channel
+    pub scan_duration: u8,
+    /// channel page on which to perform the scan
+    pub channel_page: u8,
+    /// security level to be used
+    pub security_level: SecurityLevel,
+    /// indicate which channels are to be scanned
+    pub scan_channels: [u8; 4],
+    /// originator the key to be used
+    pub key_source: [u8; 8],
+    /// mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// index of the key to be used
+    pub key_index: u8,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 2],
+}
+
+impl MacCommand for ScanRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeScanReq;
+}
+
+/// MLME SET Request used to attempt to write the given value to the indicated PIB attribute
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct SetRequest {
+    /// the pointer to the value of the PIB attribute to set
+    pub pib_attribute_ptr: *const u8,
+    /// the name of the PIB attribute to set
+    pub pib_attribute: PibId,
+}
+
+impl MacCommand for SetRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeSetReq;
+}
+
+/// MLME START Request used by the FFDs to intiate a new PAN or to begin using a new superframe
+/// configuration
+#[repr(C)]
+#[derive(Default)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct StartRequest {
+    /// PAN indentifier to used by the device
+    pub pan_id: PanId,
+    /// logical channel on which to begin
+    pub channel_number: MacChannel,
+    /// channel page on which to begin
+    pub channel_page: u8,
+    /// time at which to begin transmitting beacons
+    pub start_time: [u8; 4],
+    /// indicated how often the beacon is to be transmitted
+    pub beacon_order: u8,
+    /// length of the active portion of the superframe
+    pub superframe_order: u8,
+    /// indicated wheter the device is a PAN coordinator or not
+    pub pan_coordinator: bool,
+    /// indicates if the receiver of the beaconing device is disabled or not
+    pub battery_life_extension: bool,
+    /// indicated if the coordinator realignment command is to be trasmitted
+    pub coord_realignment: u8,
+    /// indicated if the coordinator realignment command is to be trasmitted
+    pub coord_realign_security_level: SecurityLevel,
+    /// index of the key to be used
+    pub coord_realign_key_id_index: u8,
+    /// originator of the key to be used
+    pub coord_realign_key_source: [u8; 8],
+    /// security level to be used for beacon frames
+    pub beacon_security_level: SecurityLevel,
+    /// mode used to identify the key to be used
+    pub beacon_key_id_mode: KeyIdMode,
+    /// index of the key to be used
+    pub beacon_key_index: u8,
+    /// originator of the key to be used
+    pub beacon_key_source: [u8; 8],
+}
+
+impl MacCommand for StartRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeStartReq;
+}
+
+/// MLME SYNC Request used to synchronize with the coordinator by acquiring and, if
+/// specified, tracking its beacons
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct SyncRequest {
+    /// the channel number on which to attempt coordinator synchronization
+    pub channel_number: MacChannel,
+    /// the channel page on which to attempt coordinator synchronization
+    pub channel_page: u8,
+    /// `true` if the MLME is to synchronize with the next beacon and attempts
+    /// to track all future beacons.
+    ///
+    /// `false` if the MLME is to synchronize with only the next beacon
+    pub track_beacon: bool,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 1],
+}
+
+impl MacCommand for SyncRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeSyncReq;
+}
+
+/// MLME POLL Request propmts the device to request data from the coordinator
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct PollRequest {
+    /// addressing mode of the coordinator
+    pub coord_addr_mode: AddressMode,
+    /// security level to be used
+    pub security_level: SecurityLevel,
+    /// mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// index of the key to be used
+    pub key_index: u8,
+    /// coordinator address
+    pub coord_address: MacAddress,
+    /// originator of the key to be used
+    pub key_source: [u8; 8],
+    /// PAN identifier of the coordinator
+    pub coord_pan_id: PanId,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 2],
+}
+
+impl MacCommand for PollRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmePollReq;
+}
+
+/// MLME DPS Request allows the next higher layer to request that the PHY utilize a
+/// given pair of preamble codes for a single use pending expiration of the DPSIndexDuration
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DpsRequest {
+    /// the index value for the transmitter
+    tx_dps_index: u8,
+    /// the index value of the receiver
+    rx_dps_index: u8,
+    /// the number of symbols for which the transmitter and receiver will utilize the
+    /// respective DPS indices
+    dps_index_duration: u8,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 1],
+}
+
+impl MacCommand for DpsRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeDpsReq;
+}
+
+/// MLME SOUNDING request primitive which is used by the next higher layer to request that
+/// the PHY respond with channel sounding information
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct SoundingRequest {
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 4],
+}
+
+impl MacCommand for SoundingRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeSoundingReq;
+}
+
+/// MLME CALIBRATE request primitive which used  to obtain the results of a ranging
+/// calibration request from an RDEV
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct CalibrateRequest {
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 4],
+}
+
+impl MacCommand for CalibrateRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeCalibrateReq;
+}
+
+/// MCPS DATA Request used for MAC data related requests from the application
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DataRequest {
+    /// the handle assocated with the MSDU to be transmitted
+    pub msdu_ptr: *const u8,
+    /// source addressing mode used
+    pub src_addr_mode: AddressMode,
+    /// destination addressing mode used
+    pub dst_addr_mode: AddressMode,
+    /// destination PAN Id
+    pub dst_pan_id: PanId,
+    /// destination address
+    pub dst_address: MacAddress,
+    /// the number of octets contained in the MSDU
+    pub msdu_length: u8,
+    /// the handle assocated with the MSDU to be transmitted
+    pub msdu_handle: u8,
+    /// the ACK transmittion options for the MSDU
+    pub ack_tx: u8,
+    /// `true` if a GTS is to be used for transmission
+    ///
+    /// `false` indicates that the CAP will be used
+    pub gts_tx: bool,
+    /// the pending bit transmission options for the MSDU
+    pub indirect_tx: u8,
+    /// the security level to be used
+    pub security_level: SecurityLevel,
+    /// the mode used to indentify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// the index of the key to be used
+    pub key_index: u8,
+    /// the originator of the key to be used
+    pub key_source: [u8; 8],
+    /// 2011 - the pulse repitition value
+    pub uwbprf: u8,
+    /// 2011 - the ranging configuration
+    pub ranging: u8,
+    /// 2011 - the preamble symbol repititions
+    pub uwb_preamble_symbol_repetitions: u8,
+    /// 2011 - indicates the data rate
+    pub datrate: u8,
+}
+
+impl DataRequest {
+    pub fn set_buffer<'a>(&'a mut self, buf: &'a [u8]) -> &'a mut Self {
+        self.msdu_ptr = buf as *const _ as *const u8;
+        self.msdu_length = buf.len() as u8;
+
+        self
+    }
+}
+
+impl<'a, T: AsRef<[u8]>> TryFrom<Frame<&'a T>> for DataRequest {
+    type Error = ();
+
+    fn try_from(frame: Frame<&'a T>) -> Result<Self, Self::Error> {
+        // TODO: map the rest of these
+
+        let mut request = DataRequest {
+            src_addr_mode: frame.src_addressing_mode().try_into()?,
+            dst_addr_mode: frame.dst_addressing_mode().try_into()?,
+            dst_pan_id: frame.dst_pan_id().ok_or(())?.into(),
+            dst_address: frame.dst_addr().ok_or(())?.into(),
+            msdu_handle: frame.sequence_number().ok_or(())?,
+            key_source: frame.key_source().unwrap_or_default().try_into().unwrap_or_default(),
+            ack_tx: frame.ack_request() as u8,
+            gts_tx: false,
+            security_level: if frame.security_enabled() {
+                SecurityLevel::Secured
+            } else {
+                SecurityLevel::Unsecure
+            },
+            ..Default::default()
+        };
+
+        request.set_buffer(frame.payload().ok_or(())?);
+
+        Ok(request)
+    }
+}
+
+impl Default for DataRequest {
+    fn default() -> Self {
+        Self {
+            msdu_ptr: 0 as *const u8,
+            src_addr_mode: AddressMode::NoAddress,
+            dst_addr_mode: AddressMode::NoAddress,
+            dst_pan_id: PanId([0, 0]),
+            dst_address: MacAddress { short: [0, 0] },
+            msdu_length: 0,
+            msdu_handle: 0,
+            ack_tx: 0,
+            gts_tx: false,
+            indirect_tx: 0,
+            security_level: SecurityLevel::Unsecure,
+            key_id_mode: KeyIdMode::Implicite,
+            key_index: 0,
+            key_source: [0u8; 8],
+            uwbprf: 0,
+            ranging: 0,
+            uwb_preamble_symbol_repetitions: 0,
+            datrate: 0,
+        }
+    }
+}
+
+impl MacCommand for DataRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::McpsDataReq;
+}
+
+/// for MCPS PURGE Request used to purge an MSDU from the transaction queue
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct PurgeRequest {
+    /// the handle associated with the MSDU to be purged from the transaction
+    /// queue
+    pub msdu_handle: u8,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 3],
+}
+
+impl MacCommand for PurgeRequest {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::McpsPurgeReq;
+}
+
+/// MLME ASSOCIATE Response used to initiate a response to an MLME-ASSOCIATE.indication
+#[repr(C)]
+#[derive(Default)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct AssociateResponse {
+    /// extended address of the device requesting association
+    pub device_address: [u8; 8],
+    /// 16-bitshort device address allocated by the coordinator on successful
+    /// association
+    pub assoc_short_address: [u8; 2],
+    /// status of the association attempt
+    pub status: MacStatus,
+    /// security level to be used
+    pub security_level: SecurityLevel,
+    /// the originator of the key to be used
+    pub key_source: [u8; 8],
+    /// the mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// the index of the key to be used
+    pub key_index: u8,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 2],
+}
+
+impl MacCommand for AssociateResponse {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeAssociateRes;
+}
+
+/// MLME ORPHAN Response used to respond to the MLME ORPHAN Indication
+#[repr(C)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct OrphanResponse {
+    /// extended address of the orphaned device
+    pub orphan_address: [u8; 8],
+    /// short address allocated to the orphaned device
+    pub short_address: [u8; 2],
+    /// if the orphaned device is associated with coordinator or not
+    pub associated_member: bool,
+    /// security level to be used
+    pub security_level: SecurityLevel,
+    /// the originator of the key to be used
+    pub key_source: [u8; 8],
+    /// the mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// the index of the key to be used
+    pub key_index: u8,
+    /// byte stuffing to keep 32 bit alignment
+    pub a_stuffing: [u8; 2],
+}
+
+impl MacCommand for OrphanResponse {
+    const OPCODE: OpcodeM4ToM0 = OpcodeM4ToM0::MlmeOrphanRes;
+}
diff --git a/embassy-stm32-wpan/src/wb55/mac/consts.rs b/embassy-stm32-wpan/src/wb55/mac/consts.rs
new file mode 100644
index 000000000..56903d980
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/consts.rs
@@ -0,0 +1,4 @@
+pub const MAX_PAN_DESC_SUPPORTED: usize = 6;
+pub const MAX_SOUNDING_LIST_SUPPORTED: usize = 6;
+pub const MAX_PENDING_ADDRESS: usize = 7;
+pub const MAX_ED_SCAN_RESULTS_SUPPORTED: usize = 16;
diff --git a/embassy-stm32-wpan/src/wb55/mac/control.rs b/embassy-stm32-wpan/src/wb55/mac/control.rs
new file mode 100644
index 000000000..14c6fdd2b
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/control.rs
@@ -0,0 +1,204 @@
+use core::cell::RefCell;
+use core::future::Future;
+use core::sync::atomic::{Ordering, compiler_fence};
+use core::task;
+use core::task::Poll;
+
+use embassy_net_driver::LinkState;
+use embassy_sync::blocking_mutex;
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::mutex::Mutex;
+use embassy_sync::signal::Signal;
+use futures_util::FutureExt;
+
+use crate::mac::commands::*;
+use crate::mac::driver::NetworkState;
+use crate::mac::event::MacEvent;
+use crate::mac::runner::ZeroCopyPubSub;
+use crate::mac::typedefs::*;
+use crate::sub::mac::MacTx;
+
+pub struct Control<'a> {
+    rx_event_channel: &'a ZeroCopyPubSub<CriticalSectionRawMutex, MacEvent<'a>>,
+    mac_tx: &'a Mutex<CriticalSectionRawMutex, MacTx<'a>>,
+    #[allow(unused)]
+    network_state: &'a blocking_mutex::Mutex<CriticalSectionRawMutex, RefCell<NetworkState>>,
+}
+
+impl<'a> Control<'a> {
+    pub(crate) fn new(
+        rx_event_channel: &'a ZeroCopyPubSub<CriticalSectionRawMutex, MacEvent<'a>>,
+        mac_tx: &'a Mutex<CriticalSectionRawMutex, MacTx<'a>>,
+        network_state: &'a blocking_mutex::Mutex<CriticalSectionRawMutex, RefCell<NetworkState>>,
+    ) -> Self {
+        Self {
+            rx_event_channel,
+            mac_tx,
+            network_state,
+        }
+    }
+
+    pub async fn init_link(&mut self, pan_id: [u8; 2]) {
+        debug!("resetting");
+
+        debug!(
+            "{:#x}",
+            self.send_command_and_get_response(&ResetRequest {
+                set_default_pib: true,
+                ..Default::default()
+            })
+            .await
+            .unwrap()
+            .await
+        );
+
+        let (short_address, mac_address) = critical_section::with(|cs| {
+            let mut network_state = self.network_state.borrow(cs).borrow_mut();
+
+            network_state.pan_id = pan_id;
+
+            (network_state.short_addr, network_state.mac_addr)
+        });
+
+        debug!("setting extended address");
+        debug!(
+            "{:#x}",
+            self.send_command_and_get_response(&SetRequest {
+                pib_attribute_ptr: &u64::from_be_bytes(mac_address) as *const _ as *const u8,
+                pib_attribute: PibId::ExtendedAddress,
+            })
+            .await
+            .unwrap()
+            .await
+        );
+
+        debug!("setting short address");
+        debug!(
+            "{:#x}",
+            self.send_command_and_get_response(&SetRequest {
+                pib_attribute_ptr: &u16::from_be_bytes(short_address) as *const _ as *const u8,
+                pib_attribute: PibId::ShortAddress,
+            })
+            .await
+            .unwrap()
+            .await
+        );
+
+        debug!("setting association permit");
+        let association_permit: bool = true;
+        debug!(
+            "{:#x}",
+            self.send_command_and_get_response(&SetRequest {
+                pib_attribute_ptr: &association_permit as *const _ as *const u8,
+                pib_attribute: PibId::AssociationPermit,
+            })
+            .await
+            .unwrap()
+            .await
+        );
+
+        debug!("setting TX power");
+        let transmit_power: i8 = 2;
+        debug!(
+            "{:#x}",
+            self.send_command_and_get_response(&SetRequest {
+                pib_attribute_ptr: &transmit_power as *const _ as *const u8,
+                pib_attribute: PibId::TransmitPower,
+            })
+            .await
+            .unwrap()
+            .await
+        );
+
+        debug!("starting FFD device");
+        debug!(
+            "{:#x}",
+            self.send_command_and_get_response(&StartRequest {
+                pan_id: PanId(pan_id),
+                channel_number: MacChannel::Channel16,
+                beacon_order: 0x0F,
+                superframe_order: 0x0F,
+                pan_coordinator: true,
+                battery_life_extension: false,
+                ..Default::default()
+            })
+            .await
+            .unwrap()
+            .await
+        );
+
+        debug!("setting RX on when idle");
+        let rx_on_while_idle: bool = true;
+        debug!(
+            "{:#x}",
+            self.send_command_and_get_response(&SetRequest {
+                pib_attribute_ptr: &rx_on_while_idle as *const _ as *const u8,
+                pib_attribute: PibId::RxOnWhenIdle,
+            })
+            .await
+            .unwrap()
+            .await
+        );
+
+        critical_section::with(|cs| {
+            let mut network_state = self.network_state.borrow(cs).borrow_mut();
+
+            network_state.link_state = LinkState::Up;
+            network_state.link_waker.wake();
+        });
+    }
+
+    pub async fn send_command<T>(&self, cmd: &T) -> Result<(), MacError>
+    where
+        T: MacCommand,
+    {
+        self.mac_tx.lock().await.send_command(cmd).await
+    }
+
+    pub async fn send_command_and_get_response<T>(&self, cmd: &T) -> Result<EventToken<'a>, MacError>
+    where
+        T: MacCommand,
+    {
+        let token = EventToken::new(self.rx_event_channel);
+
+        compiler_fence(Ordering::Release);
+
+        self.mac_tx.lock().await.send_command(cmd).await?;
+
+        Ok(token)
+    }
+}
+
+pub struct EventToken<'a> {
+    rx_event_channel: &'a ZeroCopyPubSub<CriticalSectionRawMutex, MacEvent<'a>>,
+}
+
+impl<'a> EventToken<'a> {
+    pub(crate) fn new(rx_event_channel: &'a ZeroCopyPubSub<CriticalSectionRawMutex, MacEvent<'a>>) -> Self {
+        // Enable event receiving
+        rx_event_channel.lock(|s| {
+            *s.borrow_mut() = Some(Signal::new());
+        });
+
+        Self { rx_event_channel }
+    }
+}
+
+impl<'a> Future for EventToken<'a> {
+    type Output = MacEvent<'a>;
+
+    fn poll(self: core::pin::Pin<&mut Self>, cx: &mut task::Context<'_>) -> Poll<Self::Output> {
+        self.rx_event_channel
+            .lock(|s| s.borrow_mut().as_mut().unwrap().wait().poll_unpin(cx))
+    }
+}
+
+impl<'a> Drop for EventToken<'a> {
+    fn drop(&mut self) {
+        // Disable event receiving
+        // This will also drop the contained event, if it exists, and will free up receiving the next event
+        self.rx_event_channel.lock(|s| {
+            *s.borrow_mut() = None;
+        });
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/mac/driver.rs b/embassy-stm32-wpan/src/wb55/mac/driver.rs
new file mode 100644
index 000000000..41171ce3d
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/driver.rs
@@ -0,0 +1,213 @@
+#![deny(unused_must_use)]
+
+use core::cell::RefCell;
+use core::task::Context;
+
+use embassy_net_driver::{Capabilities, HardwareAddress, LinkState};
+use embassy_sync::blocking_mutex;
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::channel::Channel;
+use embassy_sync::mutex::Mutex;
+use embassy_sync::waitqueue::AtomicWaker;
+
+use crate::mac::event::MacEvent;
+use crate::mac::indications::{write_frame_from_beacon_indication, write_frame_from_data_indication};
+use crate::mac::runner::{BUF_SIZE, ZeroCopyPubSub};
+use crate::mac::{Control, MTU, Runner};
+use crate::sub::mac::{Mac, MacRx, MacTx};
+
+pub struct NetworkState {
+    pub mac_addr: [u8; 8],
+    pub short_addr: [u8; 2],
+    pub pan_id: [u8; 2],
+    pub link_state: LinkState,
+    pub link_waker: AtomicWaker,
+}
+
+impl NetworkState {
+    pub const fn new() -> Self {
+        Self {
+            mac_addr: [0u8; 8],
+            short_addr: [0u8; 2],
+            pan_id: [0u8; 2],
+            link_state: LinkState::Down,
+            link_waker: AtomicWaker::new(),
+        }
+    }
+}
+
+pub struct DriverState<'d> {
+    pub mac_tx: Mutex<CriticalSectionRawMutex, MacTx<'d>>,
+    pub mac_rx: MacRx<'d>,
+    pub rx_event_channel: ZeroCopyPubSub<CriticalSectionRawMutex, MacEvent<'d>>,
+    pub rx_data_channel: Channel<CriticalSectionRawMutex, MacEvent<'d>, 1>,
+    pub tx_data_channel: Channel<CriticalSectionRawMutex, (&'d mut [u8; MTU], usize), BUF_SIZE>,
+    pub tx_buf_channel: Channel<CriticalSectionRawMutex, &'d mut [u8; MTU], BUF_SIZE>,
+    pub tx_buf_queue: [[u8; MTU]; BUF_SIZE],
+    pub network_state: blocking_mutex::Mutex<CriticalSectionRawMutex, RefCell<NetworkState>>,
+}
+
+impl<'d> DriverState<'d> {
+    pub const fn new(mac: Mac<'d>) -> Self {
+        let (mac_rx, mac_tx) = mac.split();
+        let mac_tx = Mutex::new(mac_tx);
+
+        Self {
+            mac_tx,
+            mac_rx,
+            rx_event_channel: ZeroCopyPubSub::new(RefCell::new(None)),
+            rx_data_channel: Channel::new(),
+            tx_data_channel: Channel::new(),
+            tx_buf_channel: Channel::new(),
+            tx_buf_queue: [[0u8; MTU]; BUF_SIZE],
+            network_state: blocking_mutex::Mutex::new(RefCell::new(NetworkState::new())),
+        }
+    }
+}
+
+pub struct Driver<'d> {
+    tx_data_channel: &'d Channel<CriticalSectionRawMutex, (&'d mut [u8; MTU], usize), BUF_SIZE>,
+    tx_buf_channel: &'d Channel<CriticalSectionRawMutex, &'d mut [u8; MTU], BUF_SIZE>,
+    rx_data_channel: &'d Channel<CriticalSectionRawMutex, MacEvent<'d>, 1>,
+    network_state: &'d blocking_mutex::Mutex<CriticalSectionRawMutex, RefCell<NetworkState>>,
+}
+
+impl<'d> Driver<'d> {
+    pub fn new(
+        driver_state: &'d mut DriverState<'d>,
+        short_address: [u8; 2],
+        mac_address: [u8; 8],
+    ) -> (Self, Runner<'d>, Control<'d>) {
+        (
+            Self {
+                tx_data_channel: &driver_state.tx_data_channel,
+                tx_buf_channel: &driver_state.tx_buf_channel,
+                rx_data_channel: &driver_state.rx_data_channel,
+                network_state: &driver_state.network_state,
+            },
+            Runner::new(
+                &driver_state.rx_event_channel,
+                &driver_state.rx_data_channel,
+                &mut driver_state.mac_rx,
+                &driver_state.tx_data_channel,
+                &driver_state.tx_buf_channel,
+                &driver_state.mac_tx,
+                &mut driver_state.tx_buf_queue,
+                &driver_state.network_state,
+                short_address,
+                mac_address,
+            ),
+            Control::new(
+                &driver_state.rx_event_channel,
+                &driver_state.mac_tx,
+                &driver_state.network_state,
+            ),
+        )
+    }
+}
+
+impl<'d> embassy_net_driver::Driver for Driver<'d> {
+    // type RxToken<'a> = RxToken<'a, 'd> where Self: 'a;
+    // type TxToken<'a> = TxToken<'a, 'd> where Self: 'a;
+    type RxToken<'a>
+        = RxToken<'d>
+    where
+        Self: 'a;
+    type TxToken<'a>
+        = TxToken<'d>
+    where
+        Self: 'a;
+
+    fn receive(&mut self, cx: &mut Context) -> Option<(Self::RxToken<'_>, Self::TxToken<'_>)> {
+        if self.rx_data_channel.poll_ready_to_receive(cx).is_ready()
+            && self.tx_buf_channel.poll_ready_to_receive(cx).is_ready()
+        {
+            Some((
+                RxToken {
+                    rx: self.rx_data_channel,
+                },
+                TxToken {
+                    tx: self.tx_data_channel,
+                    tx_buf: self.tx_buf_channel,
+                },
+            ))
+        } else {
+            None
+        }
+    }
+
+    fn transmit(&mut self, cx: &mut Context) -> Option<Self::TxToken<'_>> {
+        if self.tx_buf_channel.poll_ready_to_receive(cx).is_ready() {
+            Some(TxToken {
+                tx: self.tx_data_channel,
+                tx_buf: self.tx_buf_channel,
+            })
+        } else {
+            None
+        }
+    }
+
+    fn capabilities(&self) -> Capabilities {
+        let mut caps = Capabilities::default();
+        caps.max_transmission_unit = MTU;
+        // caps.max_burst_size = Some(self.tx.len());
+        caps
+    }
+
+    fn link_state(&mut self, cx: &mut Context) -> LinkState {
+        critical_section::with(|cs| {
+            let network_state = self.network_state.borrow(cs).borrow_mut();
+
+            // Unconditionally register the waker to avoid a race
+            network_state.link_waker.register(cx.waker());
+            network_state.link_state
+        })
+    }
+
+    fn hardware_address(&self) -> HardwareAddress {
+        HardwareAddress::Ieee802154(critical_section::with(|cs| {
+            self.network_state.borrow(cs).borrow().mac_addr
+        }))
+    }
+}
+
+pub struct RxToken<'d> {
+    rx: &'d Channel<CriticalSectionRawMutex, MacEvent<'d>, 1>,
+}
+
+impl<'d> embassy_net_driver::RxToken for RxToken<'d> {
+    fn consume<R, F>(self, f: F) -> R
+    where
+        F: FnOnce(&mut [u8]) -> R,
+    {
+        let mut buffer = [0u8; MTU];
+        match self.rx.try_receive().unwrap() {
+            MacEvent::McpsDataInd(data_event) => write_frame_from_data_indication(data_event, &mut buffer),
+            MacEvent::MlmeBeaconNotifyInd(data_event) => write_frame_from_beacon_indication(data_event, &mut buffer),
+            _ => {}
+        };
+
+        f(&mut buffer[..])
+    }
+}
+
+pub struct TxToken<'d> {
+    tx: &'d Channel<CriticalSectionRawMutex, (&'d mut [u8; MTU], usize), BUF_SIZE>,
+    tx_buf: &'d Channel<CriticalSectionRawMutex, &'d mut [u8; MTU], BUF_SIZE>,
+}
+
+impl<'d> embassy_net_driver::TxToken for TxToken<'d> {
+    fn consume<R, F>(self, len: usize, f: F) -> R
+    where
+        F: FnOnce(&mut [u8]) -> R,
+    {
+        // Only valid tx buffers should be put into the queue
+        let buf = self.tx_buf.try_receive().unwrap();
+        let r = f(&mut buf[..len]);
+
+        // The tx channel should always be of equal capacity to the tx_buf channel
+        self.tx.try_send((buf, len)).unwrap();
+
+        r
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/mac/event.rs b/embassy-stm32-wpan/src/wb55/mac/event.rs
new file mode 100644
index 000000000..39856e185
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/event.rs
@@ -0,0 +1,153 @@
+use core::{mem, ptr};
+
+use super::indications::{
+    AssociateIndication, BeaconNotifyIndication, CommStatusIndication, DataIndication, DisassociateIndication,
+    DpsIndication, GtsIndication, OrphanIndication, PollIndication, SyncLossIndication,
+};
+use super::responses::{
+    AssociateConfirm, CalibrateConfirm, DataConfirm, DisassociateConfirm, DpsConfirm, GetConfirm, GtsConfirm,
+    PollConfirm, PurgeConfirm, ResetConfirm, RxEnableConfirm, ScanConfirm, SetConfirm, SoundingConfirm, StartConfirm,
+};
+use crate::evt::{EvtBox, MemoryManager};
+use crate::mac::opcodes::OpcodeM0ToM4;
+use crate::sub::mac::{self, MacRx};
+
+pub(crate) trait ParseableMacEvent: Sized {
+    fn from_buffer<'a>(buf: &'a [u8]) -> Result<&'a Self, ()> {
+        if buf.len() < mem::size_of::<Self>() {
+            Err(())
+        } else {
+            Ok(unsafe { &*(buf as *const _ as *const Self) })
+        }
+    }
+}
+
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[derive(Debug)]
+pub enum MacEvent<'a> {
+    MlmeAssociateCnf(&'a AssociateConfirm),
+    MlmeDisassociateCnf(&'a DisassociateConfirm),
+    MlmeGetCnf(&'a GetConfirm),
+    MlmeGtsCnf(&'a GtsConfirm),
+    MlmeResetCnf(&'a ResetConfirm),
+    MlmeRxEnableCnf(&'a RxEnableConfirm),
+    MlmeScanCnf(&'a ScanConfirm),
+    MlmeSetCnf(&'a SetConfirm),
+    MlmeStartCnf(&'a StartConfirm),
+    MlmePollCnf(&'a PollConfirm),
+    MlmeDpsCnf(&'a DpsConfirm),
+    MlmeSoundingCnf(&'a SoundingConfirm),
+    MlmeCalibrateCnf(&'a CalibrateConfirm),
+    McpsDataCnf(&'a DataConfirm),
+    McpsPurgeCnf(&'a PurgeConfirm),
+    MlmeAssociateInd(&'a AssociateIndication),
+    MlmeDisassociateInd(&'a DisassociateIndication),
+    MlmeBeaconNotifyInd(&'a BeaconNotifyIndication),
+    MlmeCommStatusInd(&'a CommStatusIndication),
+    MlmeGtsInd(&'a GtsIndication),
+    MlmeOrphanInd(&'a OrphanIndication),
+    MlmeSyncLossInd(&'a SyncLossIndication),
+    MlmeDpsInd(&'a DpsIndication),
+    McpsDataInd(&'a DataIndication),
+    MlmePollInd(&'a PollIndication),
+}
+
+impl<'a> MacEvent<'a> {
+    pub(crate) fn new(event_box: EvtBox<MacRx>) -> Result<Self, ()> {
+        let payload = event_box.payload();
+        let opcode = u16::from_le_bytes(payload[0..2].try_into().unwrap());
+
+        let opcode = OpcodeM0ToM4::try_from(opcode)?;
+        let buf = &payload[2..];
+
+        // To avoid re-parsing the opcode, we store the result of the parse
+        // this requires use of unsafe because rust cannot assume that a reference will become
+        // invalid when the underlying result is moved. However, because we refer to a "heap"
+        // allocation, the underlying reference will not move until the struct is dropped.
+
+        let mac_event = match opcode {
+            OpcodeM0ToM4::MlmeAssociateCnf => {
+                MacEvent::MlmeAssociateCnf(unsafe { &*(AssociateConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeDisassociateCnf => {
+                MacEvent::MlmeDisassociateCnf(unsafe { &*(DisassociateConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeGetCnf => MacEvent::MlmeGetCnf(unsafe { &*(GetConfirm::from_buffer(buf)? as *const _) }),
+            OpcodeM0ToM4::MlmeGtsCnf => MacEvent::MlmeGtsCnf(unsafe { &*(GtsConfirm::from_buffer(buf)? as *const _) }),
+            OpcodeM0ToM4::MlmeResetCnf => {
+                MacEvent::MlmeResetCnf(unsafe { &*(ResetConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeRxEnableCnf => {
+                MacEvent::MlmeRxEnableCnf(unsafe { &*(RxEnableConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeScanCnf => {
+                MacEvent::MlmeScanCnf(unsafe { &*(ScanConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeSetCnf => MacEvent::MlmeSetCnf(unsafe { &*(SetConfirm::from_buffer(buf)? as *const _) }),
+            OpcodeM0ToM4::MlmeStartCnf => {
+                MacEvent::MlmeStartCnf(unsafe { &*(StartConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmePollCnf => {
+                MacEvent::MlmePollCnf(unsafe { &*(PollConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeDpsCnf => MacEvent::MlmeDpsCnf(unsafe { &*(DpsConfirm::from_buffer(buf)? as *const _) }),
+            OpcodeM0ToM4::MlmeSoundingCnf => {
+                MacEvent::MlmeSoundingCnf(unsafe { &*(SoundingConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeCalibrateCnf => {
+                MacEvent::MlmeCalibrateCnf(unsafe { &*(CalibrateConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::McpsDataCnf => {
+                MacEvent::McpsDataCnf(unsafe { &*(DataConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::McpsPurgeCnf => {
+                MacEvent::McpsPurgeCnf(unsafe { &*(PurgeConfirm::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeAssociateInd => {
+                MacEvent::MlmeAssociateInd(unsafe { &*(AssociateIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeDisassociateInd => {
+                MacEvent::MlmeDisassociateInd(unsafe { &*(DisassociateIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeBeaconNotifyInd => {
+                MacEvent::MlmeBeaconNotifyInd(unsafe { &*(BeaconNotifyIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeCommStatusInd => {
+                MacEvent::MlmeCommStatusInd(unsafe { &*(CommStatusIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeGtsInd => {
+                MacEvent::MlmeGtsInd(unsafe { &*(GtsIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeOrphanInd => {
+                MacEvent::MlmeOrphanInd(unsafe { &*(OrphanIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeSyncLossInd => {
+                MacEvent::MlmeSyncLossInd(unsafe { &*(SyncLossIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmeDpsInd => {
+                MacEvent::MlmeDpsInd(unsafe { &*(DpsIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::McpsDataInd => {
+                MacEvent::McpsDataInd(unsafe { &*(DataIndication::from_buffer(buf)? as *const _) })
+            }
+            OpcodeM0ToM4::MlmePollInd => {
+                MacEvent::MlmePollInd(unsafe { &*(PollIndication::from_buffer(buf)? as *const _) })
+            }
+        };
+
+        // Forget the event box so that drop isn't called
+        // We want to handle the lifetime ourselves
+
+        mem::forget(event_box);
+
+        Ok(mac_event)
+    }
+}
+
+unsafe impl<'a> Send for MacEvent<'a> {}
+
+impl<'a> Drop for MacEvent<'a> {
+    fn drop(&mut self) {
+        unsafe { mac::MacRx::drop_event_packet(ptr::null_mut()) };
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/mac/indications.rs b/embassy-stm32-wpan/src/wb55/mac/indications.rs
new file mode 100644
index 000000000..5673514c9
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/indications.rs
@@ -0,0 +1,300 @@
+use core::slice;
+
+use smoltcp::wire::Ieee802154FrameType;
+use smoltcp::wire::ieee802154::Frame;
+
+use super::consts::MAX_PENDING_ADDRESS;
+use super::event::ParseableMacEvent;
+use super::typedefs::{
+    AddressMode, Capabilities, DisassociationReason, KeyIdMode, MacAddress, MacChannel, MacStatus, PanDescriptor,
+    PanId, SecurityLevel,
+};
+use crate::mac::typedefs::MacAddressAndMode;
+
+/// MLME ASSOCIATE Indication which will be used by the MAC
+/// to indicate the reception of an association request command
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct AssociateIndication {
+    /// Extended address of the device requesting association
+    pub device_address: [u8; 8],
+    /// Operational capabilities of the device requesting association
+    pub capability_information: Capabilities,
+    /// Security level purportedly used by the received MAC command frame
+    pub security_level: SecurityLevel,
+    /// The mode used to identify the key used by the originator of frame
+    pub key_id_mode: KeyIdMode,
+    /// Index of the key used by the originator of the received frame
+    pub key_index: u8,
+    /// The originator of the key used by the originator of the received frame
+    pub key_source: [u8; 8],
+}
+
+impl ParseableMacEvent for AssociateIndication {}
+
+/// MLME DISASSOCIATE indication which will be used to send
+/// disassociation indication to the application.
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DisassociateIndication {
+    /// Extended address of the device requesting association
+    pub device_address: [u8; 8],
+    /// The reason for the disassociation
+    pub disassociation_reason: DisassociationReason,
+    /// The security level to be used
+    pub security_level: SecurityLevel,
+    /// The mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// The index of the key to be used
+    pub key_index: u8,
+    /// The originator of the key to be used
+    pub key_source: [u8; 8],
+}
+
+impl ParseableMacEvent for DisassociateIndication {}
+
+/// MLME BEACON NOTIIFY Indication which is used to send parameters contained
+/// within a beacon frame received by the MAC to the application
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct BeaconNotifyIndication {
+    /// he set of octets comprising the beacon payload to be transferred
+    /// from the MAC sublayer entity to the next higher layer
+    pub sdu_ptr: *const u8,
+    /// The PAN Descriptor for the received beacon
+    pub pan_descriptor: PanDescriptor,
+    /// The list of addresses of the devices
+    pub addr_list: [MacAddress; MAX_PENDING_ADDRESS],
+    /// Beacon Sequence Number
+    pub bsn: u8,
+    /// The beacon pending address specification
+    pub pend_addr_spec: u8,
+    /// Number of octets contained in the beacon payload of the beacon frame
+    pub sdu_length: u8,
+}
+
+impl ParseableMacEvent for BeaconNotifyIndication {}
+
+impl BeaconNotifyIndication {
+    pub fn payload<'a>(&'a self) -> &'a mut [u8] {
+        unsafe { slice::from_raw_parts_mut(self.sdu_ptr as *mut _, self.sdu_length as usize) }
+    }
+}
+
+pub fn write_frame_from_beacon_indication<'a, T: AsRef<[u8]> + AsMut<[u8]>>(
+    data: &'a BeaconNotifyIndication,
+    buffer: &'a mut T,
+) {
+    let mut frame = Frame::new_unchecked(buffer);
+
+    frame.set_frame_type(Ieee802154FrameType::Beacon);
+    frame.set_sequence_number(data.bsn);
+}
+
+/// MLME COMM STATUS Indication which is used by the MAC to indicate a communications status
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct CommStatusIndication {
+    /// The 16-bit PAN identifier of the device from which the frame
+    /// was received or to which the frame was being sent
+    pub pan_id: PanId,
+    /// Source addressing mode
+    pub src_addr_mode: AddressMode,
+    /// Destination addressing mode
+    pub dst_addr_mode: AddressMode,
+    /// Source address
+    pub src_address: MacAddress,
+    /// Destination address
+    pub dst_address: MacAddress,
+    /// The communications status
+    pub status: MacStatus,
+    /// Security level to be used
+    pub security_level: SecurityLevel,
+    /// Mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// Index of the key to be used
+    pub key_index: u8,
+    /// Originator of the key to be used
+    pub key_source: [u8; 8],
+}
+
+impl ParseableMacEvent for CommStatusIndication {}
+
+/// MLME GTS Indication indicates that a GTS has been allocated or that a
+/// previously allocated GTS has been deallocated
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct GtsIndication {
+    /// The short address of the device that has been allocated or deallocated a GTS
+    pub device_address: [u8; 2],
+    /// The characteristics of the GTS
+    pub gts_characteristics: u8,
+    /// Security level to be used
+    pub security_level: SecurityLevel,
+    /// Mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// Index of the key to be used
+    pub key_index: u8,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 2],
+    /// Originator of the key to be used
+    pub key_source: [u8; 8],
+}
+
+impl ParseableMacEvent for GtsIndication {}
+
+/// MLME ORPHAN Indication which is used by the coordinator to notify the
+/// application of the presence of an orphaned device
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct OrphanIndication {
+    /// Extended address of the orphaned device
+    pub orphan_address: [u8; 8],
+    /// Originator of the key used by the originator of the received frame
+    pub key_source: [u8; 8],
+    /// Security level purportedly used by the received MAC command frame
+    pub security_level: SecurityLevel,
+    /// Mode used to identify the key used by originator of received frame
+    pub key_id_mode: KeyIdMode,
+    /// Index of the key used by the originator of the received frame
+    pub key_index: u8,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 1],
+}
+
+impl ParseableMacEvent for OrphanIndication {}
+
+/// MLME SYNC LOSS Indication which is used by the MAC to indicate the loss
+/// of synchronization with the coordinator
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct SyncLossIndication {
+    /// The PAN identifier with which the device lost synchronization or to which it was realigned
+    pub pan_id: PanId,
+    /// The reason that synchronization was lost
+    pub loss_reason: u8,
+    /// The logical channel on which the device lost synchronization or to whi
+    pub channel_number: MacChannel,
+    /// The channel page on which the device lost synchronization or to which
+    pub channel_page: u8,
+    /// The security level used by the received MAC frame
+    pub security_level: SecurityLevel,
+    /// Mode used to identify the key used by originator of received frame
+    pub key_id_mode: KeyIdMode,
+    /// Index of the key used by the originator of the received frame
+    pub key_index: u8,
+    /// Originator of the key used by the originator of the received frame
+    pub key_source: [u8; 8],
+}
+
+impl ParseableMacEvent for SyncLossIndication {}
+
+/// MLME DPS Indication which indicates the expiration of the DPSIndexDuration
+///  and the resetting of the DPS values in the PHY
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DpsIndication {
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 4],
+}
+
+impl ParseableMacEvent for DpsIndication {}
+
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DataIndication {
+    /// Pointer to the set of octets forming the MSDU being indicated  
+    pub msdu_ptr: *const u8,
+    /// Source addressing mode used  
+    pub src_addr_mode: AddressMode,
+    /// Source PAN ID   
+    pub src_pan_id: PanId,
+    /// Source address  
+    pub src_address: MacAddress,
+    /// Destination addressing mode used  
+    pub dst_addr_mode: AddressMode,
+    /// Destination PAN ID   
+    pub dst_pan_id: PanId,
+    /// Destination address  
+    pub dst_address: MacAddress,
+    /// The number of octets contained in the MSDU being indicated  
+    pub msdu_length: u8,
+    /// QI value measured during reception of the MPDU
+    pub mpdu_link_quality: u8,
+    /// The data sequence number of the received data frame  
+    pub dsn: u8,
+    /// The time, in symbols, at which the data were received  
+    pub time_stamp: [u8; 4],
+    /// The security level purportedly used by the received data frame  
+    security_level: SecurityLevel,
+    /// Mode used to identify the key used by originator of received frame  
+    key_id_mode: KeyIdMode,
+    /// The originator of the key  
+    pub key_source: [u8; 8],
+    /// The index of the key  
+    pub key_index: u8,
+    /// he pulse repetition value of the received PPDU
+    pub uwbprf: u8,
+    /// The preamble symbol repetitions of the UWB PHY frame  
+    pub uwn_preamble_symbol_repetitions: u8,
+    /// Indicates the data rate
+    pub datrate: u8,
+    /// time units corresponding to an RMARKER at the antenna at the end of a ranging exchange,  
+    pub ranging_received: u8,
+    pub ranging_counter_start: u32,
+    pub ranging_counter_stop: u32,
+    /// ime units in a message exchange over which the tracking offset was measured
+    pub ranging_tracking_interval: u32,
+    /// time units slipped or advanced by the radio tracking system  
+    pub ranging_offset: u32,
+    /// The FoM characterizing the ranging measurement
+    pub ranging_fom: u8,
+    /// The Received Signal Strength Indicator measured
+    pub rssi: u8,
+}
+
+impl ParseableMacEvent for DataIndication {}
+
+impl DataIndication {
+    pub fn payload<'a>(&'a self) -> &'a mut [u8] {
+        unsafe { slice::from_raw_parts_mut(self.msdu_ptr as *mut _, self.msdu_length as usize) }
+    }
+}
+
+pub fn write_frame_from_data_indication<'a, T: AsRef<[u8]> + AsMut<[u8]>>(data: &'a DataIndication, buffer: &'a mut T) {
+    let mut frame = Frame::new_unchecked(buffer);
+
+    frame.set_frame_type(Ieee802154FrameType::Data);
+    frame.set_src_addr(MacAddressAndMode(data.src_address, data.src_addr_mode).into());
+    frame.set_dst_addr(MacAddressAndMode(data.dst_address, data.dst_addr_mode).into());
+    frame.set_dst_pan_id(data.dst_pan_id.into());
+    frame.set_src_pan_id(data.src_pan_id.into());
+    frame.set_sequence_number(data.dsn);
+    frame.set_security_enabled(data.security_level == SecurityLevel::Secured);
+
+    // No way around the copy with the current API
+    frame.payload_mut().unwrap().copy_from_slice(data.payload());
+}
+
+/// MLME POLL Indication which will be used for indicating the Data Request
+/// reception to upper layer as defined in Zigbee r22 - D.8.2
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct PollIndication {
+    /// addressing mode used
+    pub addr_mode: AddressMode,
+    /// Poll requester address
+    pub request_address: MacAddress,
+}
+
+impl ParseableMacEvent for PollIndication {}
diff --git a/embassy-stm32-wpan/src/wb55/mac/macros.rs b/embassy-stm32-wpan/src/wb55/mac/macros.rs
new file mode 100644
index 000000000..1a988a779
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/macros.rs
@@ -0,0 +1,32 @@
+#[macro_export]
+macro_rules! numeric_enum {
+    (#[repr($repr:ident)]
+     $(#$attrs:tt)* $vis:vis enum $name:ident {
+        $($(#$enum_attrs:tt)* $enum:ident = $constant:expr),* $(,)?
+    } ) => {
+        #[repr($repr)]
+        $(#$attrs)*
+        $vis enum $name {
+            $($(#$enum_attrs)* $enum = $constant),*
+        }
+
+        impl ::core::convert::TryFrom<$repr> for $name {
+            type Error = ();
+
+            fn try_from(value: $repr) -> ::core::result::Result<Self, ()> {
+                match value {
+                    $($constant => Ok( $name :: $enum ),)*
+                    _ => Err(())
+                }
+            }
+        }
+
+        impl ::core::convert::From<$name> for $repr {
+            fn from(value: $name) -> $repr {
+                match value {
+                    $($name :: $enum => $constant,)*
+                }
+            }
+        }
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/mac/mod.rs b/embassy-stm32-wpan/src/wb55/mac/mod.rs
new file mode 100644
index 000000000..ac50a6b29
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/mod.rs
@@ -0,0 +1,17 @@
+pub mod commands;
+mod consts;
+pub mod control;
+mod driver;
+pub mod event;
+pub mod indications;
+mod macros;
+mod opcodes;
+pub mod responses;
+pub mod runner;
+pub mod typedefs;
+
+pub use crate::mac::control::Control;
+pub use crate::mac::driver::{Driver, DriverState};
+pub use crate::mac::runner::Runner;
+
+const MTU: usize = 127;
diff --git a/embassy-stm32-wpan/src/wb55/mac/opcodes.rs b/embassy-stm32-wpan/src/wb55/mac/opcodes.rs
new file mode 100644
index 000000000..fd7011873
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/opcodes.rs
@@ -0,0 +1,92 @@
+const ST_VENDOR_OGF: u16 = 0x3F;
+const MAC_802_15_4_CMD_OPCODE_OFFSET: u16 = 0x280;
+
+const fn opcode(ocf: u16) -> isize {
+    ((ST_VENDOR_OGF << 9) | (MAC_802_15_4_CMD_OPCODE_OFFSET + ocf)) as isize
+}
+
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum OpcodeM4ToM0 {
+    MlmeAssociateReq = opcode(0x00),
+    MlmeAssociateRes = opcode(0x01),
+    MlmeDisassociateReq = opcode(0x02),
+    MlmeGetReq = opcode(0x03),
+    MlmeGtsReq = opcode(0x04),
+    MlmeOrphanRes = opcode(0x05),
+    MlmeResetReq = opcode(0x06),
+    MlmeRxEnableReq = opcode(0x07),
+    MlmeScanReq = opcode(0x08),
+    MlmeSetReq = opcode(0x09),
+    MlmeStartReq = opcode(0x0A),
+    MlmeSyncReq = opcode(0x0B),
+    MlmePollReq = opcode(0x0C),
+    MlmeDpsReq = opcode(0x0D),
+    MlmeSoundingReq = opcode(0x0E),
+    MlmeCalibrateReq = opcode(0x0F),
+    McpsDataReq = opcode(0x10),
+    McpsPurgeReq = opcode(0x11),
+}
+
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum OpcodeM0ToM4 {
+    MlmeAssociateCnf = 0x00,
+    MlmeDisassociateCnf,
+    MlmeGetCnf,
+    MlmeGtsCnf,
+    MlmeResetCnf,
+    MlmeRxEnableCnf,
+    MlmeScanCnf,
+    MlmeSetCnf,
+    MlmeStartCnf,
+    MlmePollCnf,
+    MlmeDpsCnf,
+    MlmeSoundingCnf,
+    MlmeCalibrateCnf,
+    McpsDataCnf,
+    McpsPurgeCnf,
+    MlmeAssociateInd,
+    MlmeDisassociateInd,
+    MlmeBeaconNotifyInd,
+    MlmeCommStatusInd,
+    MlmeGtsInd,
+    MlmeOrphanInd,
+    MlmeSyncLossInd,
+    MlmeDpsInd,
+    McpsDataInd,
+    MlmePollInd,
+}
+
+impl TryFrom<u16> for OpcodeM0ToM4 {
+    type Error = ();
+
+    fn try_from(value: u16) -> Result<Self, Self::Error> {
+        match value {
+            0 => Ok(Self::MlmeAssociateCnf),
+            1 => Ok(Self::MlmeDisassociateCnf),
+            2 => Ok(Self::MlmeGetCnf),
+            3 => Ok(Self::MlmeGtsCnf),
+            4 => Ok(Self::MlmeResetCnf),
+            5 => Ok(Self::MlmeRxEnableCnf),
+            6 => Ok(Self::MlmeScanCnf),
+            7 => Ok(Self::MlmeSetCnf),
+            8 => Ok(Self::MlmeStartCnf),
+            9 => Ok(Self::MlmePollCnf),
+            10 => Ok(Self::MlmeDpsCnf),
+            11 => Ok(Self::MlmeSoundingCnf),
+            12 => Ok(Self::MlmeCalibrateCnf),
+            13 => Ok(Self::McpsDataCnf),
+            14 => Ok(Self::McpsPurgeCnf),
+            15 => Ok(Self::MlmeAssociateInd),
+            16 => Ok(Self::MlmeDisassociateInd),
+            17 => Ok(Self::MlmeBeaconNotifyInd),
+            18 => Ok(Self::MlmeCommStatusInd),
+            19 => Ok(Self::MlmeGtsInd),
+            20 => Ok(Self::MlmeOrphanInd),
+            21 => Ok(Self::MlmeSyncLossInd),
+            22 => Ok(Self::MlmeDpsInd),
+            23 => Ok(Self::McpsDataInd),
+            24 => Ok(Self::MlmePollInd),
+            _ => Err(()),
+        }
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/mac/responses.rs b/embassy-stm32-wpan/src/wb55/mac/responses.rs
new file mode 100644
index 000000000..544fdaae8
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/responses.rs
@@ -0,0 +1,273 @@
+use super::consts::{MAX_ED_SCAN_RESULTS_SUPPORTED, MAX_PAN_DESC_SUPPORTED, MAX_SOUNDING_LIST_SUPPORTED};
+use super::event::ParseableMacEvent;
+use super::typedefs::{
+    AddressMode, AssociationStatus, KeyIdMode, MacAddress, MacStatus, PanDescriptor, PanId, PibId, ScanType,
+    SecurityLevel,
+};
+
+/// MLME ASSOCIATE Confirm used to inform of the initiating device whether
+/// its request to associate was successful or unsuccessful
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct AssociateConfirm {
+    /// short address allocated by the coordinator on successful association
+    pub assoc_short_address: [u8; 2],
+    /// status of the association request
+    pub status: AssociationStatus,
+    /// security level to be used
+    pub security_level: SecurityLevel,
+    /// the originator of the key to be used
+    pub key_source: [u8; 8],
+    /// the mode used to identify the key to be used
+    pub key_id_mode: KeyIdMode,
+    /// the index of the key to be used
+    pub key_index: u8,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 2],
+}
+
+impl ParseableMacEvent for AssociateConfirm {}
+
+/// MLME DISASSOCIATE Confirm used to send disassociation Confirmation to the application.
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DisassociateConfirm {
+    /// status of the disassociation attempt
+    pub status: MacStatus,
+    /// device addressing mode used
+    pub device_addr_mode: AddressMode,
+    /// the identifier of the PAN of the device
+    pub device_pan_id: PanId,
+    /// device address
+    pub device_address: MacAddress,
+}
+
+impl ParseableMacEvent for DisassociateConfirm {}
+
+///  MLME GET Confirm which requests information about a given PIB attribute
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct GetConfirm {
+    /// The pointer to the value of the PIB attribute attempted to read
+    pub pib_attribute_value_ptr: *const u8,
+    /// Status of the GET attempt
+    pub status: MacStatus,
+    /// The name of the PIB attribute attempted to read
+    pub pib_attribute: PibId,
+    /// The lenght of the PIB attribute Value return
+    pub pib_attribute_value_len: u8,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 1],
+}
+
+impl ParseableMacEvent for GetConfirm {}
+
+/// MLME GTS Confirm which eports the results of a request to allocate a new GTS
+/// or to deallocate an existing GTS
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct GtsConfirm {
+    /// The characteristics of the GTS
+    pub gts_characteristics: u8,
+    /// The status of the GTS reques
+    pub status: MacStatus,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 2],
+}
+
+impl ParseableMacEvent for GtsConfirm {}
+
+/// MLME RESET Confirm which is used to report the results of the reset operation
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct ResetConfirm {
+    /// The result of the reset operation
+    pub status: MacStatus,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 3],
+}
+
+impl ParseableMacEvent for ResetConfirm {}
+
+/// MLME RX ENABLE Confirm which is used to report the results of the attempt
+/// to enable or disable the receiver
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct RxEnableConfirm {
+    /// Result of the request to enable or disable the receiver
+    pub status: MacStatus,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 3],
+}
+
+impl ParseableMacEvent for RxEnableConfirm {}
+
+/// MLME SCAN Confirm which is used to report the result of the channel scan request
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct ScanConfirm {
+    /// Status of the scan request
+    pub status: MacStatus,
+    /// The type of scan performed
+    pub scan_type: ScanType,
+    /// Channel page on which the scan was performed
+    pub channel_page: u8,
+    /// Channels given in the request which were not scanned
+    pub unscanned_channels: [u8; 4],
+    /// Number of elements returned in the appropriate result lists
+    pub result_list_size: u8,
+    /// List of energy measurements
+    pub energy_detect_list: [u8; MAX_ED_SCAN_RESULTS_SUPPORTED],
+    /// List of PAN descriptors
+    pub pan_descriptor_list: [PanDescriptor; MAX_PAN_DESC_SUPPORTED],
+    /// Categorization of energy detected in channel
+    pub detected_category: u8,
+    ///  For UWB PHYs, the list of energy measurements taken
+    pub uwb_energy_detect_list: [u8; MAX_ED_SCAN_RESULTS_SUPPORTED],
+}
+
+impl ParseableMacEvent for ScanConfirm {}
+
+/// MLME SET Confirm which reports the result of an attempt to write a value to a PIB attribute
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct SetConfirm {
+    /// The result of the set operation
+    pub status: MacStatus,
+    /// The name of the PIB attribute that was written
+    pub pin_attribute: PibId,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 2],
+}
+
+impl ParseableMacEvent for SetConfirm {}
+
+/// MLME START Confirm which is used to report the results of the attempt to
+/// start using a new superframe configuration
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct StartConfirm {
+    /// Result of the attempt to start using an updated superframe configuration
+    pub status: MacStatus,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 3],
+}
+
+impl ParseableMacEvent for StartConfirm {}
+
+/// MLME POLL Confirm which is used to report the result of a request to poll the coordinator for data
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct PollConfirm {
+    /// The status of the data request
+    pub status: MacStatus,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 3],
+}
+
+impl ParseableMacEvent for PollConfirm {}
+
+/// MLME DPS Confirm which  reports the results of the attempt to enable or disable the DPS
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DpsConfirm {
+    /// The status of the DPS request
+    pub status: MacStatus,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 3],
+}
+
+impl ParseableMacEvent for DpsConfirm {}
+
+/// MLME SOUNDING Confirm which  reports the result of a request to the PHY to provide
+/// channel sounding information
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct SoundingConfirm {
+    /// Results of the sounding measurement
+    pub sounding_list: [u8; MAX_SOUNDING_LIST_SUPPORTED],
+
+    status: u8,
+}
+
+impl ParseableMacEvent for SoundingConfirm {}
+
+/// MLME CALIBRATE Confirm which reports the result of a request to the PHY
+/// to provide internal propagation path information
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct CalibrateConfirm {
+    /// The status of the attempt to return sounding data
+    pub status: MacStatus,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 3],
+    /// A count of the propagation time from the ranging counter
+    /// to the transmit antenna
+    pub cal_tx_rmaker_offset: u32,
+    /// A count of the propagation time from the receive antenna
+    /// to the ranging counter
+    pub cal_rx_rmaker_offset: u32,
+}
+
+impl ParseableMacEvent for CalibrateConfirm {}
+
+/// MCPS DATA Confirm which will be used for reporting the results of
+/// MAC data related requests from the application
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct DataConfirm {
+    /// The handle associated with the MSDU being confirmed
+    pub msdu_handle: u8,
+    /// The time, in symbols, at which the data were transmitted
+    pub time_stamp: [u8; 4],
+    /// ranging status
+    pub ranging_received: u8,
+    /// The status of the last MSDU transmission
+    pub status: MacStatus,
+    /// time units corresponding to an RMARKER at the antenna at
+    /// the beginning of a ranging exchange
+    pub ranging_counter_start: u32,
+    /// time units corresponding to an RMARKER at the antenna
+    /// at the end of a ranging exchange
+    pub ranging_counter_stop: u32,
+    /// time units in a message exchange over which the tracking offset was measured
+    pub ranging_tracking_interval: u32,
+    /// time units slipped or advanced by the radio tracking system
+    pub ranging_offset: u32,
+    /// The FoM characterizing the ranging measurement
+    pub ranging_fom: u8,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 3],
+}
+
+impl ParseableMacEvent for DataConfirm {}
+
+/// MCPS PURGE Confirm which will be used by the  MAC to notify the application of
+/// the status of its request to purge an MSDU from the transaction queue
+#[repr(C)]
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct PurgeConfirm {
+    /// Handle associated with the MSDU requested to be purged from the transaction queue
+    pub msdu_handle: u8,
+    /// The status of the request
+    pub status: MacStatus,
+    /// byte stuffing to keep 32 bit alignment
+    a_stuffing: [u8; 2],
+}
+
+impl ParseableMacEvent for PurgeConfirm {}
diff --git a/embassy-stm32-wpan/src/wb55/mac/runner.rs b/embassy-stm32-wpan/src/wb55/mac/runner.rs
new file mode 100644
index 000000000..3b7d895df
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/runner.rs
@@ -0,0 +1,151 @@
+use core::cell::RefCell;
+
+use embassy_futures::join;
+use embassy_sync::blocking_mutex;
+use embassy_sync::blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex};
+use embassy_sync::channel::Channel;
+use embassy_sync::mutex::Mutex;
+use embassy_sync::signal::Signal;
+use smoltcp::wire::Ieee802154FrameType;
+use smoltcp::wire::ieee802154::Frame;
+
+use crate::mac::MTU;
+use crate::mac::commands::*;
+use crate::mac::driver::NetworkState;
+use crate::mac::event::MacEvent;
+use crate::sub::mac::{MacRx, MacTx};
+
+pub type ZeroCopyPubSub<M, T> = blocking_mutex::Mutex<M, RefCell<Option<Signal<NoopRawMutex, T>>>>;
+
+pub const BUF_SIZE: usize = 3;
+
+pub struct Runner<'a> {
+    // rx event backpressure is already provided through the MacEvent drop mechanism
+    // therefore, we don't need to worry about overwriting events
+    rx_event_channel: &'a ZeroCopyPubSub<CriticalSectionRawMutex, MacEvent<'a>>,
+    rx_data_channel: &'a Channel<CriticalSectionRawMutex, MacEvent<'a>, 1>,
+    mac_rx: Mutex<NoopRawMutex, &'a mut MacRx<'a>>,
+
+    tx_data_channel: &'a Channel<CriticalSectionRawMutex, (&'a mut [u8; MTU], usize), BUF_SIZE>,
+    tx_buf_channel: &'a Channel<CriticalSectionRawMutex, &'a mut [u8; MTU], BUF_SIZE>,
+    mac_tx: &'a Mutex<CriticalSectionRawMutex, MacTx<'a>>,
+
+    #[allow(unused)]
+    network_state: &'a blocking_mutex::Mutex<CriticalSectionRawMutex, RefCell<NetworkState>>,
+}
+
+impl<'a> Runner<'a> {
+    pub(crate) fn new(
+        rx_event_channel: &'a ZeroCopyPubSub<CriticalSectionRawMutex, MacEvent<'a>>,
+        rx_data_channel: &'a Channel<CriticalSectionRawMutex, MacEvent<'a>, 1>,
+        mac_rx: &'a mut MacRx<'a>,
+        tx_data_channel: &'a Channel<CriticalSectionRawMutex, (&'a mut [u8; MTU], usize), BUF_SIZE>,
+        tx_buf_channel: &'a Channel<CriticalSectionRawMutex, &'a mut [u8; MTU], BUF_SIZE>,
+        mac_tx: &'a Mutex<CriticalSectionRawMutex, MacTx<'a>>,
+        tx_buf_queue: &'a mut [[u8; MTU]; BUF_SIZE],
+        network_state: &'a blocking_mutex::Mutex<CriticalSectionRawMutex, RefCell<NetworkState>>,
+        short_address: [u8; 2],
+        mac_address: [u8; 8],
+    ) -> Self {
+        for buf in tx_buf_queue {
+            tx_buf_channel.try_send(buf).unwrap();
+        }
+
+        critical_section::with(|cs| {
+            let mut network_state = network_state.borrow(cs).borrow_mut();
+
+            network_state.mac_addr = mac_address;
+            network_state.short_addr = short_address;
+        });
+
+        Self {
+            rx_event_channel,
+            rx_data_channel,
+            mac_rx: Mutex::new(mac_rx),
+            tx_data_channel,
+            tx_buf_channel,
+            mac_tx,
+            network_state,
+        }
+    }
+
+    async fn send_request<T: MacCommand, U: TryInto<T>>(&self, frame: U) -> Result<(), ()>
+    where
+        (): From<<U as TryInto<T>>::Error>,
+    {
+        let request: T = frame.try_into()?;
+        self.mac_tx.lock().await.send_command(&request).await.map_err(|_| ())?;
+
+        Ok(())
+    }
+
+    pub async fn run(&'a self) -> ! {
+        join::join(
+            async {
+                loop {
+                    if let Ok(mac_event) = self.mac_rx.try_lock().unwrap().read().await {
+                        match mac_event {
+                            MacEvent::MlmeAssociateCnf(_)
+                            | MacEvent::MlmeDisassociateCnf(_)
+                            | MacEvent::MlmeGetCnf(_)
+                            | MacEvent::MlmeGtsCnf(_)
+                            | MacEvent::MlmeResetCnf(_)
+                            | MacEvent::MlmeRxEnableCnf(_)
+                            | MacEvent::MlmeScanCnf(_)
+                            | MacEvent::MlmeSetCnf(_)
+                            | MacEvent::MlmeStartCnf(_)
+                            | MacEvent::MlmePollCnf(_)
+                            | MacEvent::MlmeDpsCnf(_)
+                            | MacEvent::MlmeSoundingCnf(_)
+                            | MacEvent::MlmeCalibrateCnf(_)
+                            | MacEvent::McpsDataCnf(_)
+                            | MacEvent::McpsPurgeCnf(_) => {
+                                self.rx_event_channel.lock(|s| {
+                                    s.borrow().as_ref().map(|signal| signal.signal(mac_event));
+                                });
+                            }
+                            MacEvent::McpsDataInd(_) => {
+                                // Pattern should match driver
+                                self.rx_data_channel.send(mac_event).await;
+                            }
+                            _ => {
+                                debug!("unhandled mac event: {:#x}", mac_event);
+                            }
+                        }
+                    }
+                }
+            },
+            async {
+                loop {
+                    let (buf, _) = self.tx_data_channel.receive().await;
+
+                    // Smoltcp has created this frame, so there's no need to reparse it.
+                    let frame = Frame::new_unchecked(&buf);
+
+                    let result: Result<(), ()> = match frame.frame_type() {
+                        Ieee802154FrameType::Beacon => Err(()),
+                        Ieee802154FrameType::Data => self.send_request::<DataRequest, _>(frame).await,
+                        Ieee802154FrameType::Acknowledgement => Err(()),
+                        Ieee802154FrameType::MacCommand => Err(()),
+                        Ieee802154FrameType::Multipurpose => Err(()),
+                        Ieee802154FrameType::FragmentOrFrak => Err(()),
+                        Ieee802154FrameType::Extended => Err(()),
+                        _ => Err(()),
+                    };
+
+                    if result.is_err() {
+                        debug!("failed to parse mac frame");
+                    } else {
+                        trace!("data frame sent!");
+                    }
+
+                    // The tx channel should always be of equal capacity to the tx_buf channel
+                    self.tx_buf_channel.try_send(buf).unwrap();
+                }
+            },
+        )
+        .await;
+
+        loop {}
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/mac/typedefs.rs b/embassy-stm32-wpan/src/wb55/mac/typedefs.rs
new file mode 100644
index 000000000..175d4a37d
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mac/typedefs.rs
@@ -0,0 +1,434 @@
+use core::fmt::Debug;
+
+use smoltcp::wire::ieee802154::{Address, AddressingMode, Pan};
+
+use crate::numeric_enum;
+
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum MacError {
+    Error = 0x01,
+    NotImplemented = 0x02,
+    NotSupported = 0x03,
+    HardwareNotSupported = 0x04,
+    Undefined = 0x05,
+}
+
+impl From<u8> for MacError {
+    fn from(value: u8) -> Self {
+        match value {
+            0x01 => Self::Error,
+            0x02 => Self::NotImplemented,
+            0x03 => Self::NotSupported,
+            0x04 => Self::HardwareNotSupported,
+            0x05 => Self::Undefined,
+            _ => Self::Undefined,
+        }
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    #[derive(Debug, Default)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub enum MacStatus {
+        #[default]
+        Success = 0x00,
+        Failure = 0xFF
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    /// this enum contains all the MAC PIB Ids
+    #[derive(Default, Debug)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub enum PibId {
+        // PHY
+        #[default]
+        CurrentChannel = 0x00,
+        ChannelsSupported = 0x01,
+        TransmitPower = 0x02,
+        CCAMode = 0x03,
+        CurrentPage = 0x04,
+        MaxFrameDuration = 0x05,
+        SHRDuration = 0x06,
+        SymbolsPerOctet = 0x07,
+
+        // MAC
+        AckWaitDuration = 0x40,
+        AssociationPermit = 0x41,
+        AutoRequest = 0x42,
+        BeaconPayload = 0x45,
+        BeaconPayloadLength = 0x46,
+        BeaconOrder = 0x47,
+        Bsn = 0x49,
+        CoordExtendedAdddress = 0x4A,
+        CoordShortAddress = 0x4B,
+        Dsn = 0x4C,
+        MaxFrameTotalWaitTime = 0x58,
+        MaxFrameRetries = 0x59,
+        PanId = 0x50,
+        ResponseWaitTime = 0x5A,
+        RxOnWhenIdle = 0x52,
+        SecurityEnabled = 0x5D,
+        ShortAddress = 0x53,
+        SuperframeOrder = 0x54,
+        TimestampSupported = 0x5C,
+        TransactionPersistenceTime = 0x55,
+        MaxBe = 0x57,
+        LifsPeriod = 0x5E,
+        SifsPeriod = 0x5F,
+        MaxCsmaBackoffs = 0x4E,
+        MinBe = 0x4F,
+        PanCoordinator = 0x10,
+        AssocPanCoordinator = 0x11,
+        ExtendedAddress = 0x6F,
+        AclEntryDescriptor = 0x70,
+        AclEntryDescriptorSize = 0x71,
+        DefaultSecurity = 0x72,
+        DefaultSecurityMaterialLength = 0x73,
+        DefaultSecurityMaterial = 0x74,
+        DefaultSecuritySuite = 0x75,
+        SecurityMode = 0x76,
+        CurrentAclEntries = 0x80,
+        DefaultSecurityExtendedAddress = 0x81,
+        AssociatedPanCoordinator = 0x56,
+        PromiscuousMode = 0x51,
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    #[derive(Default, Clone, Copy, Debug)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub enum AddressMode {
+        #[default]
+        NoAddress = 0x00,
+        Reserved = 0x01,
+        Short = 0x02,
+        Extended = 0x03,
+}
+}
+
+impl TryFrom<AddressingMode> for AddressMode {
+    type Error = ();
+
+    fn try_from(value: AddressingMode) -> Result<Self, Self::Error> {
+        match value {
+            AddressingMode::Absent => Ok(Self::NoAddress),
+            AddressingMode::Extended => Ok(Self::Extended),
+            AddressingMode::Short => Ok(Self::Short),
+            AddressingMode::Unknown(_) => Err(()),
+        }
+    }
+}
+
+#[derive(Clone, Copy)]
+pub union MacAddress {
+    pub short: [u8; 2],
+    pub extended: [u8; 8],
+}
+
+impl From<Address> for MacAddress {
+    fn from(value: Address) -> Self {
+        match value {
+            Address::Short(addr) => Self { short: addr },
+            Address::Extended(addr) => Self { extended: addr },
+            Address::Absent => Self { short: [0u8; 2] },
+        }
+    }
+}
+
+pub struct MacAddressAndMode(pub MacAddress, pub AddressMode);
+
+impl From<MacAddressAndMode> for Address {
+    fn from(mac_address_and_mode: MacAddressAndMode) -> Self {
+        let address = mac_address_and_mode.0;
+        let mode = mac_address_and_mode.1;
+
+        match mode {
+            AddressMode::Short => Address::Short(unsafe { address.short }),
+            AddressMode::Extended => Address::Extended(unsafe { address.extended }),
+            AddressMode::NoAddress => Address::Absent,
+            AddressMode::Reserved => Address::Absent,
+        }
+    }
+}
+
+impl Debug for MacAddress {
+    fn fmt(&self, fmt: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        unsafe {
+            write!(
+                fmt,
+                "MacAddress {{ short: {:?}, extended: {:?} }}",
+                self.short, self.extended
+            )
+        }
+    }
+}
+
+#[cfg(feature = "defmt")]
+impl defmt::Format for MacAddress {
+    fn format(&self, fmt: defmt::Formatter) {
+        unsafe {
+            defmt::write!(
+                fmt,
+                "MacAddress {{ short: {}, extended: {} }}",
+                self.short,
+                self.extended
+            )
+        }
+    }
+}
+
+impl Default for MacAddress {
+    fn default() -> Self {
+        Self { short: [0, 0] }
+    }
+}
+
+impl MacAddress {
+    pub const BROADCAST: Self = Self { short: [0xFF, 0xFF] };
+}
+
+impl TryFrom<&[u8]> for MacAddress {
+    type Error = ();
+
+    fn try_from(buf: &[u8]) -> Result<Self, Self::Error> {
+        const SIZE: usize = 8;
+        if buf.len() < SIZE {
+            return Err(());
+        }
+
+        Ok(Self {
+            extended: [buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]],
+        })
+    }
+}
+
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct GtsCharacteristics {
+    pub fields: u8,
+}
+
+/// MAC PAN Descriptor which contains the network details of the device from
+/// which the beacon is received
+#[derive(Default, Clone, Copy, Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct PanDescriptor {
+    /// PAN identifier of the coordinator
+    pub coord_pan_id: PanId,
+    /// Coordinator addressing mode
+    pub coord_addr_mode: AddressMode,
+    /// The current logical channel occupied by the network
+    pub logical_channel: MacChannel,
+    /// Coordinator address
+    pub coord_addr: MacAddress,
+    /// The current channel page occupied by the network
+    pub channel_page: u8,
+    /// PAN coordinator is accepting GTS requests or not
+    pub gts_permit: bool,
+    /// Superframe specification as specified in the received beacon frame
+    pub superframe_spec: [u8; 2],
+    /// The time at which the beacon frame was received, in symbols
+    pub time_stamp: [u8; 4],
+    /// The LQI at which the network beacon was received
+    pub link_quality: u8,
+    /// Security level purportedly used by the received beacon frame
+    pub security_level: u8,
+}
+
+impl TryFrom<&[u8]> for PanDescriptor {
+    type Error = ();
+
+    fn try_from(buf: &[u8]) -> Result<Self, Self::Error> {
+        const SIZE: usize = 22;
+        if buf.len() < SIZE {
+            return Err(());
+        }
+
+        let coord_addr_mode = AddressMode::try_from(buf[2])?;
+        let coord_addr = match coord_addr_mode {
+            AddressMode::NoAddress => MacAddress { short: [0, 0] },
+            AddressMode::Reserved => MacAddress { short: [0, 0] },
+            AddressMode::Short => MacAddress {
+                short: [buf[4], buf[5]],
+            },
+            AddressMode::Extended => MacAddress {
+                extended: [buf[4], buf[5], buf[6], buf[7], buf[8], buf[9], buf[10], buf[11]],
+            },
+        };
+
+        Ok(Self {
+            coord_pan_id: PanId([buf[0], buf[1]]),
+            coord_addr_mode,
+            logical_channel: MacChannel::try_from(buf[3])?,
+            coord_addr,
+            channel_page: buf[12],
+            gts_permit: buf[13] != 0,
+            superframe_spec: [buf[14], buf[15]],
+            time_stamp: [buf[16], buf[17], buf[18], buf[19]],
+            link_quality: buf[20],
+            security_level: buf[21],
+            // 2 byte stuffing
+        })
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    #[derive(Default, Clone, Copy, Debug)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    /// Building wireless applications with STM32WB series MCUs - Application note 13.10.3
+    pub enum MacChannel {
+        Channel11 = 0x0B,
+        Channel12 = 0x0C,
+        Channel13 = 0x0D,
+        Channel14 = 0x0E,
+        Channel15 = 0x0F,
+        #[default]
+        Channel16 = 0x10,
+        Channel17 = 0x11,
+        Channel18 = 0x12,
+        Channel19 = 0x13,
+        Channel20 = 0x14,
+        Channel21 = 0x15,
+        Channel22 = 0x16,
+        Channel23 = 0x17,
+        Channel24 = 0x18,
+        Channel25 = 0x19,
+        Channel26 = 0x1A,
+    }
+}
+
+#[cfg(not(feature = "defmt"))]
+bitflags::bitflags! {
+    pub struct Capabilities: u8 {
+        /// 1 if the device is capabaleof becoming a PAN coordinator
+        const IS_COORDINATOR_CAPABLE = 0b00000001;
+        /// 1 if the device is an FFD, 0 if it is an RFD
+        const IS_FFD = 0b00000010;
+        /// 1 if the device is receiving power from mains, 0 if it is battery-powered
+        const IS_MAINS_POWERED = 0b00000100;
+        /// 1 if the device does not disable its receiver to conserver power during idle periods
+        const RECEIVER_ON_WHEN_IDLE = 0b00001000;
+        // 0b00010000 reserved
+        // 0b00100000 reserved
+        /// 1 if the device is capable of sending and receiving secured MAC frames
+        const IS_SECURE = 0b01000000;
+        /// 1 if the device wishes the coordinator to allocate a short address as a result of the association
+        const ALLOCATE_ADDRESS = 0b10000000;
+    }
+}
+
+#[cfg(feature = "defmt")]
+defmt::bitflags! {
+    pub struct Capabilities: u8 {
+        /// 1 if the device is capabaleof becoming a PAN coordinator
+        const IS_COORDINATOR_CAPABLE = 0b00000001;
+        /// 1 if the device is an FFD, 0 if it is an RFD
+        const IS_FFD = 0b00000010;
+        /// 1 if the device is receiving power from mains, 0 if it is battery-powered
+        const IS_MAINS_POWERED = 0b00000100;
+        /// 1 if the device does not disable its receiver to conserver power during idle periods
+        const RECEIVER_ON_WHEN_IDLE = 0b00001000;
+        // 0b00010000 reserved
+        // 0b00100000 reserved
+        /// 1 if the device is capable of sending and receiving secured MAC frames
+        const IS_SECURE = 0b01000000;
+        /// 1 if the device wishes the coordinator to allocate a short address as a result of the association
+        const ALLOCATE_ADDRESS = 0b10000000;
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    #[derive(Default, Clone, Copy, Debug)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub enum KeyIdMode {
+        #[default]
+        /// the key is determined implicitly from the originator and recipient(s) of the frame
+        Implicite = 0x00,
+        /// the key is determined explicitly using a 1 bytes key source and a 1 byte key index
+        Explicite1Byte = 0x01,
+        /// the key is determined explicitly using a 4 bytes key source and a 1 byte key index
+        Explicite4Byte = 0x02,
+        /// the key is determined explicitly using a 8 bytes key source and a 1 byte key index
+        Explicite8Byte = 0x03,
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    #[derive(Debug)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub enum AssociationStatus {
+        /// Association successful
+        Success = 0x00,
+        /// PAN at capacity
+        PanAtCapacity = 0x01,
+        /// PAN access denied
+        PanAccessDenied = 0x02
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    #[derive(Clone, Copy, Debug)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub enum DisassociationReason {
+        /// The coordinator wishes the device to leave the PAN.
+        CoordRequested = 0x01,
+        /// The device wishes to leave the PAN.
+        DeviceRequested = 0x02,
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    #[derive(Default, Clone, Copy, Debug, PartialEq)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub enum SecurityLevel {
+        /// MAC Unsecured Mode Security
+        #[default]
+        Unsecure = 0x00,
+        /// MAC ACL Mode Security
+        AclMode = 0x01,
+        /// MAC Secured Mode Security
+        Secured = 0x02,
+    }
+}
+
+numeric_enum! {
+    #[repr(u8)]
+    #[derive(Debug)]
+    #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+    pub enum ScanType {
+        EdScan = 0x00,
+        Active = 0x01,
+        Passive = 0x02,
+        Orphan = 0x03
+    }
+}
+
+/// newtype for Pan Id
+#[derive(Default, Clone, Copy, Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct PanId(pub [u8; 2]);
+
+impl PanId {
+    pub const BROADCAST: Self = Self([0xFF, 0xFF]);
+}
+
+impl From<Pan> for PanId {
+    fn from(value: Pan) -> Self {
+        Self(value.0.to_be_bytes())
+    }
+}
+
+impl From<PanId> for Pan {
+    fn from(value: PanId) -> Self {
+        Self(u16::from_be_bytes(value.0))
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/mod.rs b/embassy-stm32-wpan/src/wb55/mod.rs
new file mode 100644
index 000000000..95cfe09f1
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/mod.rs
@@ -0,0 +1,198 @@
+// This must go FIRST so that all the other modules see its macros.
+mod fmt;
+
+use core::mem::MaybeUninit;
+use core::sync::atomic::{Ordering, compiler_fence};
+
+use embassy_hal_internal::Peri;
+use embassy_stm32::interrupt;
+use embassy_stm32::ipcc::{Config, Ipcc, IpccRxChannel, ReceiveInterruptHandler, TransmitInterruptHandler};
+use embassy_stm32::peripherals::IPCC;
+use sub::mm::MemoryManager;
+use sub::sys::Sys;
+use tables::*;
+use unsafe_linked_list::LinkedListNode;
+
+pub mod channels;
+pub mod cmd;
+pub mod consts;
+pub mod evt;
+pub mod lhci;
+pub mod shci;
+pub mod sub;
+pub mod tables;
+pub mod unsafe_linked_list;
+
+#[cfg(feature = "wb55_mac")]
+pub mod mac;
+
+#[cfg(feature = "wb55_ble")]
+pub use crate::sub::ble::hci;
+
+type PacketHeader = LinkedListNode;
+
+/// Transport Layer for the Mailbox interface
+pub struct TlMbox<'d> {
+    pub sys_subsystem: Sys<'d>,
+    pub mm_subsystem: MemoryManager<'d>,
+    #[cfg(feature = "wb55_ble")]
+    pub ble_subsystem: sub::ble::Ble<'d>,
+    #[cfg(feature = "wb55_mac")]
+    pub mac_subsystem: sub::mac::Mac<'d>,
+    pub traces: IpccRxChannel<'d>,
+}
+
+impl<'d> TlMbox<'d> {
+    /// Initialise the Transport Layer, and creates and returns a wrapper around it.
+    ///
+    /// This method performs the initialisation laid out in AN5289 annex 14.1. However, it differs
+    /// from the implementation documented in Figure 64, to avoid needing to reference any C
+    /// function pointers.
+    ///
+    /// Annex 14.1 lays out the following methods that should be called:
+    ///     1. tl_mbox.c/TL_Init, which initialises the reference table that is shared between CPU1
+    ///        and CPU2.
+    ///     2. shci_tl.c/shci_init(), which initialises the system transport layer, and in turn
+    ///        calls tl_mbox.c/TL_SYS_Init, which initialises SYSTEM_EVT_QUEUE channel.
+    ///     3. tl_mbox.c/TL_MM_Init(), which initialises the channel used for sending memory
+    ///        manager commands.
+    ///     4. tl_mbox.c/TL_Enable(), which enables the IPCC, and starts CPU2.
+    /// This implementation initialises all of the shared refernce tables and all IPCC channel that
+    /// would be initialised by this process. The developer should therefore treat this method as
+    /// completing all steps in Figure 64.
+    ///
+    /// Once this method has been called, no system commands may be sent until the CPU2 ready
+    /// signal is received, via [sys_subsystem.read]; this completes the procedure laid out in
+    /// Figure 65.
+    ///
+    /// If the `ble` feature is enabled, at this point, the user should call
+    /// [sys_subsystem.shci_c2_ble_init], before any commands are written to the
+    /// [TlMbox.ble_subsystem] ([sub::ble::Ble::new()] completes the process that would otherwise
+    /// be handled by `TL_BLE_Init`; see Figure 66). This completes the procedure laid out in
+    /// Figure 66.
+    // TODO: document what the user should do after calling init to use the mac_802_15_4 subsystem
+    pub async fn init(
+        ipcc: Peri<'d, IPCC>,
+        _irqs: impl interrupt::typelevel::Binding<interrupt::typelevel::IPCC_C1_RX, ReceiveInterruptHandler>
+        + interrupt::typelevel::Binding<interrupt::typelevel::IPCC_C1_TX, TransmitInterruptHandler>,
+        config: Config,
+    ) -> Self {
+        // this is an inlined version of TL_Init from the STM32WB firmware as requested by AN5289.
+        // HW_IPCC_Init is not called, and its purpose is (presumably?) covered by this
+        // implementation
+        unsafe {
+            TL_REF_TABLE.as_mut_ptr().write_volatile(RefTable {
+                device_info_table: TL_DEVICE_INFO_TABLE.as_ptr(),
+                ble_table: TL_BLE_TABLE.as_ptr(),
+                thread_table: TL_THREAD_TABLE.as_ptr(),
+                sys_table: TL_SYS_TABLE.as_ptr(),
+                mem_manager_table: TL_MEM_MANAGER_TABLE.as_ptr(),
+                traces_table: TL_TRACES_TABLE.as_ptr(),
+                mac_802_15_4_table: TL_MAC_802_15_4_TABLE.as_ptr(),
+                zigbee_table: TL_ZIGBEE_TABLE.as_ptr(),
+                lld_tests_table: TL_LLD_TESTS_TABLE.as_ptr(),
+                ble_lld_table: TL_BLE_LLD_TABLE.as_ptr(),
+            });
+
+            TL_SYS_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_DEVICE_INFO_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_BLE_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_THREAD_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_MEM_MANAGER_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+
+            TL_TRACES_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_MAC_802_15_4_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_ZIGBEE_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_LLD_TESTS_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            TL_BLE_LLD_TABLE
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+
+            EVT_POOL
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            SYS_SPARE_EVT_BUF
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+            CS_BUFFER
+                .as_mut_ptr()
+                .write_volatile(MaybeUninit::zeroed().assume_init());
+
+            #[cfg(feature = "wb55_ble")]
+            {
+                BLE_SPARE_EVT_BUF
+                    .as_mut_ptr()
+                    .write_volatile(MaybeUninit::zeroed().assume_init());
+
+                BLE_CMD_BUFFER
+                    .as_mut_ptr()
+                    .write_volatile(MaybeUninit::zeroed().assume_init());
+                HCI_ACL_DATA_BUFFER
+                    .as_mut_ptr()
+                    .write_volatile(MaybeUninit::zeroed().assume_init());
+            }
+
+            #[cfg(feature = "wb55_mac")]
+            {
+                MAC_802_15_4_CMD_BUFFER
+                    .as_mut_ptr()
+                    .write_volatile(MaybeUninit::zeroed().assume_init());
+                MAC_802_15_4_NOTIF_RSP_EVT_BUFFER
+                    .as_mut_ptr()
+                    .write_volatile(MaybeUninit::zeroed().assume_init());
+            }
+        }
+
+        compiler_fence(Ordering::SeqCst);
+
+        // this is equivalent to `HW_IPCC_Enable`, which is called by `TL_Enable`
+        let [
+            (_hw_ipcc_ble_cmd_channel, _ipcc_ble_event_channel),
+            (ipcc_system_cmd_rsp_channel, ipcc_system_event_channel),
+            (_ipcc_mac_802_15_4_cmd_rsp_channel, _ipcc_mac_802_15_4_notification_ack_channel),
+            (ipcc_mm_release_buffer_channel, _ipcc_traces_channel),
+            (_ipcc_ble_lld_cmd_channel, _ipcc_ble_lld_rsp_channel),
+            (_ipcc_hci_acl_data_channel, _),
+        ] = Ipcc::new(ipcc, _irqs, config).split();
+
+        let mm = sub::mm::MemoryManager::new(ipcc_mm_release_buffer_channel);
+        let mut sys = sub::sys::Sys::new(ipcc_system_cmd_rsp_channel, ipcc_system_event_channel);
+
+        debug!("sys event: {}", sys.read().await.payload());
+
+        Self {
+            sys_subsystem: sys,
+            #[cfg(feature = "wb55_ble")]
+            ble_subsystem: sub::ble::Ble::new(
+                _hw_ipcc_ble_cmd_channel,
+                _ipcc_ble_event_channel,
+                _ipcc_hci_acl_data_channel,
+            ),
+            #[cfg(feature = "wb55_mac")]
+            mac_subsystem: sub::mac::Mac::new(
+                _ipcc_mac_802_15_4_cmd_rsp_channel,
+                _ipcc_mac_802_15_4_notification_ack_channel,
+            ),
+            mm_subsystem: mm,
+            traces: _ipcc_traces_channel,
+        }
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/shci.rs b/embassy-stm32-wpan/src/wb55/shci.rs
new file mode 100644
index 000000000..3faa79209
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/shci.rs
@@ -0,0 +1,397 @@
+use core::sync::atomic::{Ordering, compiler_fence};
+use core::{mem, ptr, slice};
+
+use crate::cmd::CmdPacket;
+use crate::consts::{TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE};
+use crate::evt::{CcEvt, EvtStub};
+use crate::wb55::PacketHeader;
+
+const SHCI_OGF: u16 = 0x3F;
+
+const fn opcode(ogf: u16, ocf: u16) -> isize {
+    ((ogf << 10) + ocf) as isize
+}
+
+#[allow(dead_code)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum SchiCommandStatus {
+    ShciSuccess = 0x00,
+    ShciUnknownCmd = 0x01,
+    ShciMemoryCapacityExceededErrCode = 0x07,
+    ShciErrUnsupportedFeature = 0x11,
+    ShciErrInvalidHciCmdParams = 0x12,
+    ShciErrInvalidParams = 0x42,   /* only used for release < v1.13.0 */
+    ShciErrInvalidParamsV2 = 0x92, /* available for release >= v1.13.0 */
+    ShciFusCmdNotSupported = 0xFF,
+}
+
+impl SchiCommandStatus {
+    pub unsafe fn from_packet(cmd_buf: *const CmdPacket) -> Result<Self, ()> {
+        let p_cmd_serial = (cmd_buf as *mut u8).add(size_of::<PacketHeader>());
+        let p_evt_payload = p_cmd_serial.add(size_of::<EvtStub>());
+
+        compiler_fence(Ordering::Acquire);
+        let cc_evt = ptr::read_unaligned(p_evt_payload as *const CcEvt);
+
+        cc_evt.payload[0].try_into()
+    }
+}
+
+impl TryFrom<u8> for SchiCommandStatus {
+    type Error = ();
+
+    fn try_from(v: u8) -> Result<Self, Self::Error> {
+        match v {
+            x if x == SchiCommandStatus::ShciSuccess as u8 => Ok(SchiCommandStatus::ShciSuccess),
+            x if x == SchiCommandStatus::ShciUnknownCmd as u8 => Ok(SchiCommandStatus::ShciUnknownCmd),
+            x if x == SchiCommandStatus::ShciMemoryCapacityExceededErrCode as u8 => {
+                Ok(SchiCommandStatus::ShciMemoryCapacityExceededErrCode)
+            }
+            x if x == SchiCommandStatus::ShciErrUnsupportedFeature as u8 => {
+                Ok(SchiCommandStatus::ShciErrUnsupportedFeature)
+            }
+            x if x == SchiCommandStatus::ShciErrInvalidHciCmdParams as u8 => {
+                Ok(SchiCommandStatus::ShciErrInvalidHciCmdParams)
+            }
+            x if x == SchiCommandStatus::ShciErrInvalidParams as u8 => Ok(SchiCommandStatus::ShciErrInvalidParams), /* only used for release < v1.13.0 */
+            x if x == SchiCommandStatus::ShciErrInvalidParamsV2 as u8 => Ok(SchiCommandStatus::ShciErrInvalidParamsV2), /* available for release >= v1.13.0 */
+            x if x == SchiCommandStatus::ShciFusCmdNotSupported as u8 => Ok(SchiCommandStatus::ShciFusCmdNotSupported),
+            _ => Err(()),
+        }
+    }
+}
+
+#[allow(dead_code)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum ShciOpcode {
+    // 0x50 reserved
+    // 0x51 reserved
+    FusGetState = opcode(SHCI_OGF, 0x52),
+    // 0x53 reserved
+    FusFirmwareUpgrade = opcode(SHCI_OGF, 0x54),
+    FusFirmwareDelete = opcode(SHCI_OGF, 0x55),
+    FusUpdateAuthKey = opcode(SHCI_OGF, 0x56),
+    FusLockAuthKey = opcode(SHCI_OGF, 0x57),
+    FusStoreUserKey = opcode(SHCI_OGF, 0x58),
+    FusLoadUserKey = opcode(SHCI_OGF, 0x59),
+    FusStartWirelessStack = opcode(SHCI_OGF, 0x5a),
+    // 0x5b reserved
+    // 0x5c reserved
+    FusLockUserKey = opcode(SHCI_OGF, 0x5d),
+    FusUnloadUserKey = opcode(SHCI_OGF, 0x5e),
+    FusActivateAntirollback = opcode(SHCI_OGF, 0x5f),
+    // 0x60 reserved
+    // 0x61 reserved
+    // 0x62 reserved
+    // 0x63 reserved
+    // 0x64 reserved
+    // 0x65 reserved
+    BleInit = opcode(SHCI_OGF, 0x66),
+    ThreadInit = opcode(SHCI_OGF, 0x67),
+    DebugInit = opcode(SHCI_OGF, 0x68),
+    FlashEraseActivity = opcode(SHCI_OGF, 0x69),
+    ConcurrentSetMode = opcode(SHCI_OGF, 0x6a),
+    FlashStoreData = opcode(SHCI_OGF, 0x6b),
+    FlashEraseData = opcode(SHCI_OGF, 0x6c),
+    RadioAllowLowPower = opcode(SHCI_OGF, 0x6d),
+    Mac802_15_4Init = opcode(SHCI_OGF, 0x6e),
+    ReInit = opcode(SHCI_OGF, 0x6f),
+    ZigbeeInit = opcode(SHCI_OGF, 0x70),
+    LldTestsInit = opcode(SHCI_OGF, 0x71),
+    ExtraConfig = opcode(SHCI_OGF, 0x72),
+    SetFlashActivityControl = opcode(SHCI_OGF, 0x73),
+    BleLldInit = opcode(SHCI_OGF, 0x74),
+    Config = opcode(SHCI_OGF, 0x75),
+    ConcurrentGetNextBleEvtTime = opcode(SHCI_OGF, 0x76),
+    ConcurrentEnableNext802_15_4EvtNotification = opcode(SHCI_OGF, 0x77),
+    Mac802_15_4DeInit = opcode(SHCI_OGF, 0x78),
+}
+
+pub const SHCI_C2_CONFIG_EVTMASK1_BIT0_ERROR_NOTIF_ENABLE: u8 = 1 << 0;
+pub const SHCI_C2_CONFIG_EVTMASK1_BIT1_BLE_NVM_RAM_UPDATE_ENABLE: u8 = 1 << 1;
+pub const SHCI_C2_CONFIG_EVTMASK1_BIT2_THREAD_NVM_RAM_UPDATE_ENABLE: u8 = 1 << 2;
+pub const SHCI_C2_CONFIG_EVTMASK1_BIT3_NVM_START_WRITE_ENABLE: u8 = 1 << 3;
+pub const SHCI_C2_CONFIG_EVTMASK1_BIT4_NVM_END_WRITE_ENABLE: u8 = 1 << 4;
+pub const SHCI_C2_CONFIG_EVTMASK1_BIT5_NVM_START_ERASE_ENABLE: u8 = 1 << 5;
+pub const SHCI_C2_CONFIG_EVTMASK1_BIT6_NVM_END_ERASE_ENABLE: u8 = 1 << 6;
+
+#[derive(Clone, Copy)]
+#[repr(C, packed)]
+pub struct ShciConfigParam {
+    pub payload_cmd_size: u8,
+    pub config: u8,
+    pub event_mask: u8,
+    pub spare: u8,
+    pub ble_nvm_ram_address: u32,
+    pub thread_nvm_ram_address: u32,
+    pub revision_id: u16,
+    pub device_id: u16,
+}
+
+impl ShciConfigParam {
+    pub fn payload<'a>(&'a self) -> &'a [u8] {
+        unsafe { slice::from_raw_parts(self as *const _ as *const u8, mem::size_of::<Self>()) }
+    }
+}
+
+impl Default for ShciConfigParam {
+    fn default() -> Self {
+        Self {
+            payload_cmd_size: (mem::size_of::<Self>() - 1) as u8,
+            config: 0,
+            event_mask: SHCI_C2_CONFIG_EVTMASK1_BIT0_ERROR_NOTIF_ENABLE
+                + SHCI_C2_CONFIG_EVTMASK1_BIT1_BLE_NVM_RAM_UPDATE_ENABLE
+                + SHCI_C2_CONFIG_EVTMASK1_BIT2_THREAD_NVM_RAM_UPDATE_ENABLE
+                + SHCI_C2_CONFIG_EVTMASK1_BIT3_NVM_START_WRITE_ENABLE
+                + SHCI_C2_CONFIG_EVTMASK1_BIT4_NVM_END_WRITE_ENABLE
+                + SHCI_C2_CONFIG_EVTMASK1_BIT5_NVM_START_ERASE_ENABLE
+                + SHCI_C2_CONFIG_EVTMASK1_BIT6_NVM_END_ERASE_ENABLE,
+            spare: 0,
+            ble_nvm_ram_address: 0,
+            thread_nvm_ram_address: 0,
+            revision_id: 0,
+            device_id: 0,
+        }
+    }
+}
+
+#[derive(Clone, Copy)]
+#[repr(C, packed)]
+pub struct ShciBleInitCmdParam {
+    /// NOT USED - shall be set to 0
+    pub p_ble_buffer_address: u32,
+    /// NOT USED - shall be set to 0
+    pub ble_buffer_size: u32,
+    /// Maximum number of attribute records related to all the required characteristics (excluding the services)
+    /// that can be stored in the GATT database, for the specific BLE user application.
+    /// For each characteristic, the number of attribute records goes from two to five depending on the characteristic properties:
+    ///    - minimum of two (one for declaration and one for the value)
+    ///    - add one more record for each additional property: notify or indicate, broadcast, extended property.
+    /// The total calculated value must be increased by 9, due to the records related to the standard attribute profile and
+    /// GAP service characteristics, and automatically added when initializing GATT and GAP layers
+    ///  - Min value: <number of user attributes> + 9
+    ///  - Max value: depending on the GATT database defined by user application
+    pub num_attr_record: u16,
+    /// Defines the maximum number of services that can be stored in the GATT database. Note that the GAP and GATT services
+    /// are automatically added at initialization so this parameter must be the number of user services increased by two.
+    ///    - Min value: <number of user service> + 2
+    ///    - Max value: depending GATT database defined by user application
+    pub num_attr_serv: u16,
+    /// NOTE: This parameter is ignored by the CPU2 when the parameter "Options" is set to "LL_only" ( see Options description in that structure )
+    ///
+    /// Size of the storage area for the attribute values.
+    /// Each characteristic contributes to the attrValueArrSize value as follows:
+    ///    - Characteristic value length plus:
+    ///        + 5 bytes if characteristic UUID is 16 bits
+    ///        + 19 bytes if characteristic UUID is 128 bits
+    ///        + 2 bytes if characteristic has a server configuration descriptor
+    ///        + 2 bytes * NumOfLinks if the characteristic has a client configuration descriptor
+    ///        + 2 bytes if the characteristic has extended properties
+    /// Each descriptor contributes to the attrValueArrSize value as follows:
+    ///    - Descriptor length
+    pub attr_value_arr_size: u16,
+    /// Maximum number of BLE links supported
+    ///    - Min value: 1
+    ///    - Max value: 8
+    pub num_of_links: u8,
+    /// Disable/enable the extended packet length BLE 5.0 feature
+    ///    - Disable: 0
+    ///    - Enable: 1
+    pub extended_packet_length_enable: u8,
+    /// NOTE: This parameter is ignored by the CPU2 when the parameter "Options" is set to "LL_only" ( see Options description in that structure )
+    ///
+    /// Maximum number of supported "prepare write request"
+    ///    - Min value: given by the macro DEFAULT_PREP_WRITE_LIST_SIZE
+    ///    - Max value: a value higher than the minimum required can be specified, but it is not recommended
+    pub prepare_write_list_size: u8,
+    /// NOTE: This parameter is overwritten by the CPU2 with an hardcoded optimal value when the parameter "Options" is set to "LL_only"
+    /// ( see Options description in that structure )
+    ///
+    /// Number of allocated memory blocks for the BLE stack
+    ///     - Min value: given by the macro MBLOCKS_CALC
+    ///     - Max value: a higher value can improve data throughput performance, but uses more memory
+    pub block_count: u8,
+    /// NOTE: This parameter is ignored by the CPU2 when the parameter "Options" is set to "LL_only" ( see Options description in that structure )
+    ///
+    /// Maximum ATT MTU size supported
+    ///     - Min value: 23
+    ///     - Max value: 512
+    pub att_mtu: u16,
+    /// The sleep clock accuracy (ppm value) that used in BLE connected slave mode to calculate the window widening
+    /// (in combination with the sleep clock accuracy sent by master in CONNECT_REQ PDU),
+    /// refer to BLE 5.0 specifications - Vol 6 - Part B - chap 4.5.7 and 4.2.2
+    ///     - Min value: 0
+    ///     - Max value: 500 (worst possible admitted by specification)
+    pub slave_sca: u16,
+    /// The sleep clock accuracy handled in master mode. It is used to determine the connection and advertising events timing.
+    /// It is transmitted to the slave in CONNEC_REQ PDU used by the slave to calculate the window widening,
+    /// see SlaveSca and Bluetooth Core Specification v5.0 Vol 6 - Part B - chap 4.5.7 and 4.2.2
+    /// Possible values:
+    ///    - 251 ppm to 500 ppm: 0
+    ///    - 151 ppm to 250 ppm: 1
+    ///    - 101 ppm to 150 ppm: 2
+    ///    - 76 ppm to 100 ppm: 3
+    ///    - 51 ppm to 75 ppm: 4
+    ///    - 31 ppm to 50 ppm: 5
+    ///    - 21 ppm to 30 ppm: 6
+    ///    - 0 ppm to 20 ppm: 7
+    pub master_sca: u8,
+    /// Some information for Low speed clock mapped in bits field
+    /// - bit 0:
+    ///     - 1: Calibration for the RF system wakeup clock source
+    ///     - 0: No calibration for the RF system wakeup clock source
+    /// - bit 1:
+    ///     - 1: STM32W5M Module device
+    ///     - 0: Other devices as STM32WBxx SOC, STM32WB1M module
+    /// - bit 2:
+    ///     - 1: HSE/1024 Clock config
+    ///     - 0: LSE Clock config
+    pub ls_source: u8,
+    /// This parameter determines the maximum duration of a slave connection event. When this duration is reached the slave closes
+    /// the current connections event (whatever is the CE_length parameter specified by the master in HCI_CREATE_CONNECTION HCI command),
+    /// expressed in units of 625/256 µs (~2.44 µs)
+    ///    - Min value: 0 (if 0 is specified, the master and slave perform only a single TX-RX exchange per connection event)
+    ///    - Max value: 1638400 (4000 ms). A higher value can be specified (max 0xFFFFFFFF) but results in a maximum connection time
+    ///      of 4000 ms as specified. In this case the parameter is not applied, and the predicted CE length calculated on slave is not shortened
+    pub max_conn_event_length: u32,
+    /// Startup time of the high speed (16 or 32 MHz) crystal oscillator in units of 625/256 µs (~2.44 µs).
+    ///    - Min value: 0
+    ///    - Max value:  820 (~2 ms). A higher value can be specified, but the value that implemented in stack is forced to ~2 ms
+    pub hs_startup_time: u16,
+    /// Viterbi implementation in BLE LL reception.
+    ///    - 0: Enable
+    ///    - 1: Disable
+    pub viterbi_enable: u8,
+    /// - bit 0:
+    ///     - 1: LL only
+    ///     - 0: LL + host
+    /// - bit 1:
+    ///     - 1: no service change desc.
+    ///     - 0: with service change desc.
+    /// - bit 2:
+    ///     - 1: device name Read-Only
+    ///     - 0: device name R/W
+    /// - bit 3:
+    ///     - 1: extended advertizing supported
+    ///     - 0: extended advertizing not supported
+    /// - bit 4:
+    ///     - 1: CS Algo #2 supported
+    ///     - 0: CS Algo #2 not supported
+    /// - bit 5:
+    ///     - 1: Reduced GATT database in NVM
+    ///     - 0: Full GATT database in NVM
+    /// - bit 6:
+    ///     - 1: GATT caching is used
+    ///     - 0: GATT caching is not used
+    /// - bit 7:
+    ///     - 1: LE Power Class 1
+    ///     - 0: LE Power Classe 2-3
+    /// - other bits: complete with Options_extension flag
+    pub options: u8,
+    /// Reserved for future use - shall be set to 0
+    pub hw_version: u8,
+    ///
+    /// Maximum number of connection-oriented channels in initiator mode.
+    /// Range: 0 .. 64
+    pub max_coc_initiator_nbr: u8,
+
+    ///
+    /// Minimum transmit power in dBm supported by the Controller.
+    /// Range: -127 .. 20
+    pub min_tx_power: i8,
+
+    ///
+    /// Maximum transmit power in dBm supported by the Controller.
+    /// Range: -127 .. 20
+    pub max_tx_power: i8,
+
+    ///
+    /// RX model configuration
+    /// - bit 0:   1: agc_rssi model improved vs RF blockers    0: Legacy agc_rssi model
+    /// - other bits: reserved ( shall be set to 0)
+    pub rx_model_config: u8,
+
+    /// Maximum number of advertising sets.
+    /// Range: 1 .. 8 with limitation:
+    /// This parameter is linked to max_adv_data_len such as both compliant with allocated Total memory computed with BLE_EXT_ADV_BUFFER_SIZE based
+    /// on Max Extended advertising configuration supported.
+    /// This parameter is considered by the CPU2 when Options has SHCI_C2_BLE_INIT_OPTIONS_EXT_ADV flag set
+    pub max_adv_set_nbr: u8,
+
+    /// Maximum advertising data length (in bytes)
+    /// Range: 31 .. 1650 with limitation:
+    /// This parameter is linked to max_adv_set_nbr such as both compliant with allocated Total memory computed with BLE_EXT_ADV_BUFFER_SIZE based
+    /// on Max Extended advertising configuration supported.
+    /// This parameter is considered by the CPU2 when Options has SHCI_C2_BLE_INIT_OPTIONS_EXT_ADV flag set
+    pub max_adv_data_len: u16,
+
+    /// RF TX Path Compensation Value (16-bit signed integer). Units: 0.1 dB.
+    /// Range: -1280 .. 1280
+    pub tx_path_compens: i16,
+
+    //// RF RX Path Compensation Value (16-bit signed integer). Units: 0.1 dB.
+    /// Range: -1280 .. 1280
+    pub rx_path_compens: i16,
+
+    /// BLE core specification version (8-bit unsigned integer).
+    /// values as: 11(5.2), 12(5.3)
+    pub ble_core_version: u8,
+
+    /// Options flags extension
+    /// - bit 0:   1: appearance Writable              0: appearance Read-Only
+    /// - bit 1:   1: Enhanced ATT supported           0: Enhanced ATT not supported
+    /// - other bits: reserved ( shall be set to 0)
+    pub options_extension: u8,
+
+    /// MaxAddEattBearers
+    /// Maximum number of bearers that can be created for Enhanced ATT
+    /// in addition to the number of links
+    ///     - Range: 0 .. 4
+    pub max_add_eatt_bearers: u8,
+}
+
+impl ShciBleInitCmdParam {
+    pub fn payload<'a>(&'a self) -> &'a [u8] {
+        unsafe { slice::from_raw_parts(self as *const _ as *const u8, mem::size_of::<Self>()) }
+    }
+}
+
+impl Default for ShciBleInitCmdParam {
+    fn default() -> Self {
+        Self {
+            p_ble_buffer_address: 0,
+            ble_buffer_size: 0,
+            num_attr_record: 68,
+            num_attr_serv: 4,
+            attr_value_arr_size: 1344,
+            num_of_links: 2,
+            extended_packet_length_enable: 1,
+            prepare_write_list_size: 0x3A,
+            block_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,
+            options: 0,
+            hw_version: 0,
+            max_coc_initiator_nbr: 32,
+            min_tx_power: -40,
+            max_tx_power: 6,
+            rx_model_config: 0,
+            max_adv_set_nbr: 2,
+            max_adv_data_len: 1650,
+            tx_path_compens: 0,
+            rx_path_compens: 0,
+            ble_core_version: 11,
+            options_extension: 0,
+            max_add_eatt_bearers: 4,
+        }
+    }
+}
+
+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;
diff --git a/embassy-stm32-wpan/src/wb55/sub/ble.rs b/embassy-stm32-wpan/src/wb55/sub/ble.rs
new file mode 100644
index 000000000..a822d6530
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/sub/ble.rs
@@ -0,0 +1,139 @@
+use core::ptr;
+
+use embassy_stm32::ipcc::{IpccRxChannel, IpccTxChannel};
+use hci::Opcode;
+
+use crate::cmd::CmdPacket;
+use crate::consts::{TL_BLEEVT_CC_OPCODE, TL_BLEEVT_CS_OPCODE, TlPacketType};
+use crate::evt;
+use crate::evt::{EvtBox, EvtPacket, EvtStub};
+use crate::sub::mm;
+use crate::tables::{BLE_CMD_BUFFER, BleTable, CS_BUFFER, EVT_QUEUE, HCI_ACL_DATA_BUFFER, TL_BLE_TABLE};
+use crate::unsafe_linked_list::LinkedListNode;
+
+/// A guard that, once constructed, may be used to send BLE commands to CPU2.
+///
+/// It is the responsibility of the caller to ensure that they have awaited an event via
+/// [crate::sub::sys::Sys::read] before sending any of these commands, and to call
+/// [crate::sub::sys::Sys::shci_c2_ble_init] and await the HCI_COMMAND_COMPLETE_EVENT before
+/// sending any other commands.
+///
+/// # Example
+///
+/// ```
+/// # embassy_stm32::bind_interrupts!(struct Irqs{
+/// #     IPCC_C1_RX => ReceiveInterruptHandler;
+/// #     IPCC_C1_TX => TransmitInterruptHandler;
+/// # });
+/// #
+/// # let p = embassy_stm32::init(embassy_stm32::Config::default());
+/// # let mut mbox = embassy_stm32_wpan::TlMbox::init(p.IPCC, Irqs, embassy_stm32::ipcc::Config::default());
+/// #
+/// # let sys_event = mbox.sys_subsystem.read().await;
+/// # let _command_status = mbox.sys_subsystem.shci_c2_ble_init(Default::default());
+/// # // BLE commands may now be sent
+/// #
+/// # mbox.ble_subsystem.reset().await;
+/// # let _reset_response = mbox.ble_subsystem.read().await;
+/// ```
+pub struct Ble<'a> {
+    hw_ipcc_ble_cmd_channel: IpccTxChannel<'a>,
+    ipcc_ble_event_channel: IpccRxChannel<'a>,
+    ipcc_hci_acl_data_channel: IpccTxChannel<'a>,
+}
+
+impl<'a> Ble<'a> {
+    /// Constructs a guard that allows for BLE commands to be sent to CPU2.
+    ///
+    /// This takes the place of `TL_BLE_Init`, completing that step as laid out in AN5289, Fig 66.
+    pub(crate) fn new(
+        hw_ipcc_ble_cmd_channel: IpccTxChannel<'a>,
+        ipcc_ble_event_channel: IpccRxChannel<'a>,
+        ipcc_hci_acl_data_channel: IpccTxChannel<'a>,
+    ) -> Self {
+        unsafe {
+            LinkedListNode::init_head(EVT_QUEUE.as_mut_ptr());
+
+            TL_BLE_TABLE.as_mut_ptr().write_volatile(BleTable {
+                pcmd_buffer: BLE_CMD_BUFFER.as_mut_ptr().cast(),
+                pcs_buffer: CS_BUFFER.as_ptr().cast(),
+                pevt_queue: EVT_QUEUE.as_ptr().cast(),
+                phci_acl_data_buffer: HCI_ACL_DATA_BUFFER.as_mut_ptr().cast(),
+            });
+        }
+
+        Self {
+            hw_ipcc_ble_cmd_channel,
+            ipcc_ble_event_channel,
+            ipcc_hci_acl_data_channel,
+        }
+    }
+
+    /// `HW_IPCC_BLE_EvtNot`
+    pub async fn tl_read(&mut self) -> EvtBox<Self> {
+        self.ipcc_ble_event_channel
+            .receive(|| unsafe {
+                if let Some(node_ptr) =
+                    critical_section::with(|cs| LinkedListNode::remove_head(cs, EVT_QUEUE.as_mut_ptr()))
+                {
+                    Some(EvtBox::new(node_ptr.cast()))
+                } else {
+                    None
+                }
+            })
+            .await
+    }
+
+    /// `TL_BLE_SendCmd`
+    pub async fn tl_write(&mut self, opcode: u16, payload: &[u8]) {
+        self.hw_ipcc_ble_cmd_channel
+            .send(|| unsafe {
+                CmdPacket::write_into(BLE_CMD_BUFFER.as_mut_ptr(), TlPacketType::BleCmd, opcode, payload);
+            })
+            .await;
+    }
+
+    /// `TL_BLE_SendAclData`
+    pub async fn acl_write(&mut self, handle: u16, payload: &[u8]) {
+        self.ipcc_hci_acl_data_channel
+            .send(|| unsafe {
+                CmdPacket::write_into(
+                    HCI_ACL_DATA_BUFFER.as_mut_ptr() as *mut _,
+                    TlPacketType::AclData,
+                    handle,
+                    payload,
+                );
+            })
+            .await;
+    }
+}
+
+impl<'a> evt::MemoryManager for Ble<'a> {
+    /// SAFETY: passing a pointer to something other than a managed event packet is UB
+    unsafe fn drop_event_packet(evt: *mut EvtPacket) {
+        let stub = unsafe {
+            let p_evt_stub = &(*evt).evt_serial as *const _ as *const EvtStub;
+
+            ptr::read_volatile(p_evt_stub)
+        };
+
+        if !(stub.evt_code == TL_BLEEVT_CS_OPCODE || stub.evt_code == TL_BLEEVT_CC_OPCODE) {
+            mm::MemoryManager::drop_event_packet(evt);
+        }
+    }
+}
+
+pub extern crate stm32wb_hci as hci;
+
+impl<'a> hci::Controller for Ble<'a> {
+    async fn controller_write(&mut self, opcode: Opcode, payload: &[u8]) {
+        self.tl_write(opcode.0, payload).await;
+    }
+
+    async fn controller_read_into(&mut self, buf: &mut [u8]) {
+        let evt_box = self.tl_read().await;
+        let evt_serial = evt_box.serial();
+
+        buf[..evt_serial.len()].copy_from_slice(evt_serial);
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/sub/mac.rs b/embassy-stm32-wpan/src/wb55/sub/mac.rs
new file mode 100644
index 000000000..ce2903e61
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/sub/mac.rs
@@ -0,0 +1,173 @@
+use core::future::poll_fn;
+use core::ptr;
+use core::sync::atomic::{AtomicBool, Ordering};
+use core::task::Poll;
+
+use embassy_futures::poll_once;
+use embassy_stm32::ipcc::{Ipcc, IpccRxChannel, IpccTxChannel};
+use embassy_sync::waitqueue::AtomicWaker;
+
+use crate::cmd::CmdPacket;
+use crate::consts::TlPacketType;
+use crate::evt;
+use crate::evt::{EvtBox, EvtPacket};
+use crate::mac::commands::MacCommand;
+use crate::mac::event::MacEvent;
+use crate::mac::typedefs::MacError;
+use crate::tables::{MAC_802_15_4_CMD_BUFFER, MAC_802_15_4_NOTIF_RSP_EVT_BUFFER};
+use crate::unsafe_linked_list::LinkedListNode;
+
+static MAC_WAKER: AtomicWaker = AtomicWaker::new();
+static MAC_EVT_OUT: AtomicBool = AtomicBool::new(false);
+
+pub struct Mac<'a> {
+    ipcc_mac_802_15_4_cmd_rsp_channel: IpccTxChannel<'a>,
+    ipcc_mac_802_15_4_notification_ack_channel: IpccRxChannel<'a>,
+}
+
+impl<'a> Mac<'a> {
+    pub(crate) fn new(
+        ipcc_mac_802_15_4_cmd_rsp_channel: IpccTxChannel<'a>,
+        ipcc_mac_802_15_4_notification_ack_channel: IpccRxChannel<'a>,
+    ) -> Self {
+        use crate::tables::{
+            MAC_802_15_4_CMD_BUFFER, MAC_802_15_4_NOTIF_RSP_EVT_BUFFER, Mac802_15_4Table, TL_MAC_802_15_4_TABLE,
+            TL_TRACES_TABLE, TRACES_EVT_QUEUE, TracesTable,
+        };
+
+        unsafe {
+            LinkedListNode::init_head(TRACES_EVT_QUEUE.as_mut_ptr() as *mut _);
+
+            TL_TRACES_TABLE.as_mut_ptr().write_volatile(TracesTable {
+                traces_queue: TRACES_EVT_QUEUE.as_ptr() as *const _,
+            });
+
+            TL_MAC_802_15_4_TABLE.as_mut_ptr().write_volatile(Mac802_15_4Table {
+                p_cmdrsp_buffer: MAC_802_15_4_CMD_BUFFER.as_mut_ptr().cast(),
+                p_notack_buffer: MAC_802_15_4_NOTIF_RSP_EVT_BUFFER.as_mut_ptr().cast(),
+                evt_queue: core::ptr::null_mut(),
+            });
+        };
+
+        Self {
+            ipcc_mac_802_15_4_cmd_rsp_channel,
+            ipcc_mac_802_15_4_notification_ack_channel,
+        }
+    }
+
+    pub const fn split(self) -> (MacRx<'a>, MacTx<'a>) {
+        (
+            MacRx {
+                ipcc_mac_802_15_4_notification_ack_channel: self.ipcc_mac_802_15_4_notification_ack_channel,
+            },
+            MacTx {
+                ipcc_mac_802_15_4_cmd_rsp_channel: self.ipcc_mac_802_15_4_cmd_rsp_channel,
+            },
+        )
+    }
+}
+
+pub struct MacTx<'a> {
+    ipcc_mac_802_15_4_cmd_rsp_channel: IpccTxChannel<'a>,
+}
+
+impl<'a> MacTx<'a> {
+    /// `HW_IPCC_MAC_802_15_4_CmdEvtNot`
+    pub async fn tl_write_and_get_response(&mut self, opcode: u16, payload: &[u8]) -> u8 {
+        self.tl_write(opcode, payload).await;
+        self.ipcc_mac_802_15_4_cmd_rsp_channel.flush().await;
+
+        unsafe {
+            let p_event_packet = MAC_802_15_4_CMD_BUFFER.as_ptr() as *const EvtPacket;
+            let p_mac_rsp_evt = &((*p_event_packet).evt_serial.evt.payload) as *const u8;
+
+            ptr::read_volatile(p_mac_rsp_evt)
+        }
+    }
+
+    /// `TL_MAC_802_15_4_SendCmd`
+    pub async fn tl_write(&mut self, opcode: u16, payload: &[u8]) {
+        self.ipcc_mac_802_15_4_cmd_rsp_channel
+            .send(|| unsafe {
+                CmdPacket::write_into(
+                    MAC_802_15_4_CMD_BUFFER.as_mut_ptr(),
+                    TlPacketType::MacCmd,
+                    opcode,
+                    payload,
+                );
+            })
+            .await;
+    }
+
+    pub async fn send_command<T>(&mut self, cmd: &T) -> Result<(), MacError>
+    where
+        T: MacCommand,
+    {
+        let response = self.tl_write_and_get_response(T::OPCODE as u16, cmd.payload()).await;
+
+        if response == 0x00 {
+            Ok(())
+        } else {
+            Err(MacError::from(response))
+        }
+    }
+}
+
+pub struct MacRx<'a> {
+    ipcc_mac_802_15_4_notification_ack_channel: IpccRxChannel<'a>,
+}
+
+impl<'a> MacRx<'a> {
+    /// `HW_IPCC_MAC_802_15_4_EvtNot`
+    ///
+    /// This function will stall if the previous `EvtBox` has not been dropped
+    pub async fn tl_read(&mut self) -> EvtBox<MacRx<'a>> {
+        // Wait for the last event box to be dropped
+        poll_fn(|cx| {
+            MAC_WAKER.register(cx.waker());
+            if MAC_EVT_OUT.load(Ordering::Acquire) {
+                Poll::Pending
+            } else {
+                Poll::Ready(())
+            }
+        })
+        .await;
+
+        // Return a new event box
+        self.ipcc_mac_802_15_4_notification_ack_channel
+            .receive(|| unsafe {
+                // The closure is not async, therefore the closure must execute to completion (cannot be dropped)
+                // Therefore, the event box is guaranteed to be cleaned up if it's not leaked
+                MAC_EVT_OUT.store(true, Ordering::SeqCst);
+
+                Some(EvtBox::new(MAC_802_15_4_NOTIF_RSP_EVT_BUFFER.as_mut_ptr() as *mut _))
+            })
+            .await
+    }
+
+    pub async fn read<'b>(&mut self) -> Result<MacEvent<'b>, ()> {
+        MacEvent::new(self.tl_read().await)
+    }
+}
+
+impl<'a> evt::MemoryManager for MacRx<'a> {
+    /// SAFETY: passing a pointer to something other than a managed event packet is UB
+    unsafe fn drop_event_packet(_: *mut EvtPacket) {
+        trace!("mac drop event");
+
+        // Write the ack
+        CmdPacket::write_into(
+            MAC_802_15_4_NOTIF_RSP_EVT_BUFFER.as_mut_ptr() as *mut _,
+            TlPacketType::OtAck,
+            0,
+            &[],
+        );
+
+        // Clear the rx flag
+        let _ = poll_once(Ipcc::receive::<()>(3, || None));
+
+        // Allow a new read call
+        MAC_EVT_OUT.store(false, Ordering::Release);
+        MAC_WAKER.wake();
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/sub/mm.rs b/embassy-stm32-wpan/src/wb55/sub/mm.rs
new file mode 100644
index 000000000..0ca7d1835
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/sub/mm.rs
@@ -0,0 +1,84 @@
+//! Memory manager routines
+use core::future::poll_fn;
+use core::mem::MaybeUninit;
+use core::task::Poll;
+
+use aligned::{A4, Aligned};
+use embassy_stm32::ipcc::IpccTxChannel;
+use embassy_sync::waitqueue::AtomicWaker;
+
+use crate::consts::POOL_SIZE;
+use crate::evt;
+use crate::evt::EvtPacket;
+#[cfg(feature = "wb55_ble")]
+use crate::tables::BLE_SPARE_EVT_BUF;
+use crate::tables::{EVT_POOL, FREE_BUF_QUEUE, MemManagerTable, SYS_SPARE_EVT_BUF, TL_MEM_MANAGER_TABLE};
+use crate::unsafe_linked_list::LinkedListNode;
+
+static MM_WAKER: AtomicWaker = AtomicWaker::new();
+static mut LOCAL_FREE_BUF_QUEUE: Aligned<A4, MaybeUninit<LinkedListNode>> = Aligned(MaybeUninit::uninit());
+
+pub struct MemoryManager<'a> {
+    ipcc_mm_release_buffer_channel: IpccTxChannel<'a>,
+}
+
+impl<'a> MemoryManager<'a> {
+    pub(crate) fn new(ipcc_mm_release_buffer_channel: IpccTxChannel<'a>) -> Self {
+        unsafe {
+            LinkedListNode::init_head(FREE_BUF_QUEUE.as_mut_ptr());
+            LinkedListNode::init_head(LOCAL_FREE_BUF_QUEUE.as_mut_ptr());
+
+            TL_MEM_MANAGER_TABLE.as_mut_ptr().write_volatile(MemManagerTable {
+                #[cfg(feature = "wb55_ble")]
+                spare_ble_buffer: BLE_SPARE_EVT_BUF.as_ptr().cast(),
+                #[cfg(not(feature = "wb55_ble"))]
+                spare_ble_buffer: core::ptr::null(),
+                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,
+            });
+        }
+
+        Self {
+            ipcc_mm_release_buffer_channel,
+        }
+    }
+
+    pub async fn run_queue(&mut self) -> ! {
+        loop {
+            poll_fn(|cx| unsafe {
+                MM_WAKER.register(cx.waker());
+                if critical_section::with(|cs| LinkedListNode::is_empty(cs, LOCAL_FREE_BUF_QUEUE.as_mut_ptr())) {
+                    Poll::Pending
+                } else {
+                    Poll::Ready(())
+                }
+            })
+            .await;
+
+            self.ipcc_mm_release_buffer_channel
+                .send(|| {
+                    critical_section::with(|cs| unsafe {
+                        while let Some(node_ptr) = LinkedListNode::remove_head(cs, LOCAL_FREE_BUF_QUEUE.as_mut_ptr()) {
+                            LinkedListNode::insert_head(cs, FREE_BUF_QUEUE.as_mut_ptr(), node_ptr);
+                        }
+                    })
+                })
+                .await;
+        }
+    }
+}
+
+impl<'a> evt::MemoryManager for MemoryManager<'a> {
+    /// SAFETY: passing a pointer to something other than a managed event packet is UB
+    unsafe fn drop_event_packet(evt: *mut EvtPacket) {
+        critical_section::with(|cs| unsafe {
+            LinkedListNode::insert_head(cs, LOCAL_FREE_BUF_QUEUE.as_mut_ptr(), evt as *mut _);
+        });
+
+        MM_WAKER.wake();
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/sub/mod.rs b/embassy-stm32-wpan/src/wb55/sub/mod.rs
new file mode 100644
index 000000000..d3ebd822a
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/sub/mod.rs
@@ -0,0 +1,6 @@
+#[cfg(feature = "wb55_ble")]
+pub mod ble;
+#[cfg(feature = "wb55_mac")]
+pub mod mac;
+pub mod mm;
+pub mod sys;
diff --git a/embassy-stm32-wpan/src/wb55/sub/sys.rs b/embassy-stm32-wpan/src/wb55/sub/sys.rs
new file mode 100644
index 000000000..2e625a677
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/sub/sys.rs
@@ -0,0 +1,103 @@
+use embassy_stm32::ipcc::{IpccRxChannel, IpccTxChannel};
+
+use crate::cmd::CmdPacket;
+use crate::consts::TlPacketType;
+use crate::evt::EvtBox;
+#[cfg(feature = "wb55_ble")]
+use crate::shci::ShciBleInitCmdParam;
+use crate::shci::{SchiCommandStatus, ShciOpcode};
+use crate::sub::mm;
+use crate::tables::{SysTable, WirelessFwInfoTable};
+use crate::unsafe_linked_list::LinkedListNode;
+use crate::wb55::{SYS_CMD_BUF, SYSTEM_EVT_QUEUE, TL_DEVICE_INFO_TABLE, TL_SYS_TABLE};
+
+/// A guard that, once constructed, allows for sys commands to be sent to CPU2.
+pub struct Sys<'a> {
+    ipcc_system_cmd_rsp_channel: IpccTxChannel<'a>,
+    ipcc_system_event_channel: IpccRxChannel<'a>,
+}
+
+impl<'a> Sys<'a> {
+    /// TL_Sys_Init
+    pub(crate) fn new(
+        ipcc_system_cmd_rsp_channel: IpccTxChannel<'a>,
+        ipcc_system_event_channel: IpccRxChannel<'a>,
+    ) -> Self {
+        unsafe {
+            LinkedListNode::init_head(SYSTEM_EVT_QUEUE.as_mut_ptr());
+
+            TL_SYS_TABLE.as_mut_ptr().write_volatile(SysTable {
+                pcmd_buffer: SYS_CMD_BUF.as_mut_ptr(),
+                sys_queue: SYSTEM_EVT_QUEUE.as_ptr(),
+            });
+        }
+
+        Self {
+            ipcc_system_cmd_rsp_channel,
+            ipcc_system_event_channel,
+        }
+    }
+
+    /// Returns CPU2 wireless firmware information (if present).
+    pub fn wireless_fw_info(&self) -> Option<WirelessFwInfoTable> {
+        let info = unsafe { TL_DEVICE_INFO_TABLE.as_mut_ptr().read_volatile().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 }
+    }
+
+    pub async fn write(&mut self, opcode: ShciOpcode, payload: &[u8]) {
+        self.ipcc_system_cmd_rsp_channel
+            .send(|| unsafe {
+                CmdPacket::write_into(SYS_CMD_BUF.as_mut_ptr(), TlPacketType::SysCmd, opcode as u16, payload);
+            })
+            .await;
+    }
+
+    /// `HW_IPCC_SYS_CmdEvtNot`
+    pub async fn write_and_get_response(
+        &mut self,
+        opcode: ShciOpcode,
+        payload: &[u8],
+    ) -> Result<SchiCommandStatus, ()> {
+        self.write(opcode, payload).await;
+        self.ipcc_system_cmd_rsp_channel.flush().await;
+
+        unsafe { SchiCommandStatus::from_packet(SYS_CMD_BUF.as_ptr()) }
+    }
+
+    #[cfg(feature = "wb55_mac")]
+    pub async fn shci_c2_mac_802_15_4_init(&mut self) -> Result<SchiCommandStatus, ()> {
+        self.write_and_get_response(ShciOpcode::Mac802_15_4Init, &[]).await
+    }
+
+    /// Send a request to CPU2 to initialise the BLE stack.
+    ///
+    /// This must be called before any BLE commands are sent via the BLE channel (according to
+    /// AN5289, Figures 65 and 66). It should only be called after CPU2 sends a system event, via
+    /// `HW_IPCC_SYS_EvtNot`, aka `IoBusCallBackUserEvt` (as detailed in Figure 65), aka
+    /// [crate::sub::ble::hci::host::uart::UartHci::read].
+    #[cfg(feature = "wb55_ble")]
+    pub async fn shci_c2_ble_init(&mut self, param: ShciBleInitCmdParam) -> Result<SchiCommandStatus, ()> {
+        self.write_and_get_response(ShciOpcode::BleInit, param.payload()).await
+    }
+
+    /// `HW_IPCC_SYS_EvtNot`
+    ///
+    /// This method takes the place of the `HW_IPCC_SYS_EvtNot`/`SysUserEvtRx`/`APPE_SysUserEvtRx`,
+    /// as the embassy implementation avoids the need to call C public bindings, and instead
+    /// handles the event channels directly.
+    pub async fn read(&mut self) -> EvtBox<mm::MemoryManager<'_>> {
+        self.ipcc_system_event_channel
+            .receive(|| unsafe {
+                if let Some(node_ptr) =
+                    critical_section::with(|cs| LinkedListNode::remove_head(cs, SYSTEM_EVT_QUEUE.as_mut_ptr()))
+                {
+                    Some(EvtBox::new(node_ptr.cast()))
+                } else {
+                    None
+                }
+            })
+            .await
+    }
+}
diff --git a/embassy-stm32-wpan/src/wb55/tables.rs b/embassy-stm32-wpan/src/wb55/tables.rs
new file mode 100644
index 000000000..2e6a9199b
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/tables.rs
@@ -0,0 +1,283 @@
+use core::mem::MaybeUninit;
+
+use aligned::{A4, Aligned};
+use bit_field::BitField;
+
+use crate::cmd::{AclDataPacket, CmdPacket};
+#[cfg(feature = "wb55_mac")]
+use crate::consts::C_SIZE_CMD_STRING;
+use crate::consts::{POOL_SIZE, TL_CS_EVT_SIZE, TL_EVT_HEADER_SIZE, TL_PACKET_HEADER_SIZE};
+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)]
+pub struct DeviceInfoTable {
+    pub safe_boot_info_table: SafeBootInfoTable,
+    pub rss_info_table: RssInfoTable,
+    pub wireless_fw_info_table: WirelessFwInfoTable,
+}
+
+/// The bluetooth reference table, as defined in figure 67 of STM32WX AN5289.
+#[derive(Debug)]
+#[repr(C)]
+pub struct BleTable {
+    /// A pointer to the buffer that is used for sending BLE commands.
+    pub pcmd_buffer: *mut CmdPacket,
+    /// A pointer to the buffer used for storing Command statuses.
+    pub pcs_buffer: *const u8,
+    /// A pointer to the event queue, over which IPCC BLE events are sent. This may be accessed via
+    /// [crate::sub::ble::Ble::tl_read].
+    pub pevt_queue: *const u8,
+    /// A pointer to the buffer that is used for sending HCI (Host-Controller Interface) ACL
+    /// (Asynchronous Connection-oriented Logical transport) commands (unused).
+    pub phci_acl_data_buffer: *mut AclDataPacket,
+}
+
+#[derive(Debug)]
+#[repr(C)]
+pub struct ThreadTable {
+    pub nostack_buffer: *const u8,
+    pub clicmdrsp_buffer: *const u8,
+    pub otcmdrsp_buffer: *const u8,
+}
+
+#[derive(Debug)]
+#[repr(C)]
+pub struct LldTestsTable {
+    pub clicmdrsp_buffer: *const u8,
+    pub m0cmd_buffer: *const u8,
+}
+
+// TODO: use later
+#[derive(Debug)]
+#[repr(C)]
+pub struct BleLldTable {
+    pub cmdrsp_buffer: *const u8,
+    pub m0cmd_buffer: *const u8,
+}
+
+// TODO: use later
+#[derive(Debug)]
+#[repr(C)]
+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)]
+pub struct SysTable {
+    pub pcmd_buffer: *mut CmdPacket,
+    pub sys_queue: *const LinkedListNode,
+}
+
+#[derive(Debug)]
+#[repr(C)]
+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)]
+pub struct TracesTable {
+    pub traces_queue: *const u8,
+}
+
+#[derive(Debug)]
+#[repr(C)]
+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,
+    pub zigbee_table: *const ZigbeeTable,
+    pub lld_tests_table: *const LldTestsTable,
+    pub ble_lld_table: *const BleLldTable,
+}
+
+// --------------------- ref table ---------------------
+#[unsafe(link_section = "TL_REF_TABLE")]
+pub static mut TL_REF_TABLE: MaybeUninit<RefTable> = MaybeUninit::zeroed();
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_DEVICE_INFO_TABLE: Aligned<A4, MaybeUninit<DeviceInfoTable>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_BLE_TABLE: Aligned<A4, MaybeUninit<BleTable>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_THREAD_TABLE: Aligned<A4, MaybeUninit<ThreadTable>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_LLD_TESTS_TABLE: Aligned<A4, MaybeUninit<LldTestsTable>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_BLE_LLD_TABLE: Aligned<A4, MaybeUninit<BleLldTable>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_SYS_TABLE: Aligned<A4, MaybeUninit<SysTable>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_MEM_MANAGER_TABLE: Aligned<A4, MaybeUninit<MemManagerTable>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_TRACES_TABLE: Aligned<A4, MaybeUninit<TracesTable>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_MAC_802_15_4_TABLE: Aligned<A4, MaybeUninit<Mac802_15_4Table>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TL_ZIGBEE_TABLE: Aligned<A4, MaybeUninit<ZigbeeTable>> = Aligned(MaybeUninit::zeroed());
+
+// --------------------- tables ---------------------
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut FREE_BUF_QUEUE: Aligned<A4, MaybeUninit<LinkedListNode>> = Aligned(MaybeUninit::zeroed());
+
+#[allow(dead_code)]
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut TRACES_EVT_QUEUE: Aligned<A4, MaybeUninit<LinkedListNode>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut CS_BUFFER: Aligned<A4, MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + TL_EVT_HEADER_SIZE + TL_CS_EVT_SIZE]>> =
+    Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut EVT_QUEUE: Aligned<A4, MaybeUninit<LinkedListNode>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut SYSTEM_EVT_QUEUE: Aligned<A4, MaybeUninit<LinkedListNode>> = Aligned(MaybeUninit::zeroed());
+
+// --------------------- app tables ---------------------
+#[cfg(feature = "wb55_mac")]
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut MAC_802_15_4_CMD_BUFFER: Aligned<A4, MaybeUninit<CmdPacket>> = Aligned(MaybeUninit::zeroed());
+
+#[cfg(feature = "wb55_mac")]
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut MAC_802_15_4_NOTIF_RSP_EVT_BUFFER: MaybeUninit<
+    Aligned<A4, [u8; TL_PACKET_HEADER_SIZE + TL_EVT_HEADER_SIZE + 255]>,
+> = MaybeUninit::zeroed();
+
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut EVT_POOL: Aligned<A4, MaybeUninit<[u8; POOL_SIZE]>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut SYS_CMD_BUF: Aligned<A4, MaybeUninit<CmdPacket>> = Aligned(MaybeUninit::zeroed());
+
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut SYS_SPARE_EVT_BUF: Aligned<A4, MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + TL_EVT_HEADER_SIZE + 255]>> =
+    Aligned(MaybeUninit::zeroed());
+
+#[cfg(feature = "wb55_mac")]
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut MAC_802_15_4_CNFINDNOT: Aligned<A4, MaybeUninit<[u8; C_SIZE_CMD_STRING]>> =
+    Aligned(MaybeUninit::zeroed());
+
+#[cfg(feature = "wb55_ble")]
+#[unsafe(link_section = "MB_MEM1")]
+pub static mut BLE_CMD_BUFFER: Aligned<A4, MaybeUninit<CmdPacket>> = Aligned(MaybeUninit::zeroed());
+
+#[cfg(feature = "wb55_ble")]
+#[unsafe(link_section = "MB_MEM2")]
+pub static mut BLE_SPARE_EVT_BUF: Aligned<A4, MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + TL_EVT_HEADER_SIZE + 255]>> =
+    Aligned(MaybeUninit::zeroed());
+
+#[cfg(feature = "wb55_ble")]
+#[unsafe(link_section = "MB_MEM2")]
+//                                                 fuck these "magic" numbers from ST ---v---v
+pub static mut HCI_ACL_DATA_BUFFER: Aligned<A4, MaybeUninit<[u8; TL_PACKET_HEADER_SIZE + 5 + 251]>> =
+    Aligned(MaybeUninit::zeroed());
diff --git a/embassy-stm32-wpan/src/wb55/unsafe_linked_list.rs b/embassy-stm32-wpan/src/wb55/unsafe_linked_list.rs
new file mode 100644
index 000000000..c84ee1bb6
--- /dev/null
+++ b/embassy-stm32-wpan/src/wb55/unsafe_linked_list.rs
@@ -0,0 +1,269 @@
+//! 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 core::fmt::Debug;
+use core::ptr;
+
+use critical_section::CriticalSection;
+
+#[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 p_list_head: *mut LinkedListNode) {
+        ptr::write_volatile(
+            p_list_head,
+            LinkedListNode {
+                next: p_list_head,
+                prev: p_list_head,
+            },
+        );
+    }
+
+    pub unsafe fn is_empty(_cs: CriticalSection, mut p_list_head: *mut LinkedListNode) -> bool {
+        ptr::read_volatile(p_list_head).next == p_list_head
+    }
+
+    /// Insert `node` after `list_head` and before the next node
+    pub unsafe fn insert_head(
+        _cs: CriticalSection,
+        mut p_list_head: *mut LinkedListNode,
+        mut p_node: *mut LinkedListNode,
+    ) {
+        let mut list_head = ptr::read_volatile(p_list_head);
+        if p_list_head != list_head.next {
+            let mut node_next = ptr::read_volatile(list_head.next);
+            let node = LinkedListNode {
+                next: list_head.next,
+                prev: p_list_head,
+            };
+
+            list_head.next = p_node;
+            node_next.prev = p_node;
+
+            // All nodes must be written because they will all be seen by another core
+            ptr::write_volatile(p_node, node);
+            ptr::write_volatile(node.next, node_next);
+            ptr::write_volatile(p_list_head, list_head);
+        } else {
+            let node = LinkedListNode {
+                next: list_head.next,
+                prev: p_list_head,
+            };
+
+            list_head.next = p_node;
+            list_head.prev = p_node;
+
+            // All nodes must be written because they will all be seen by another core
+            ptr::write_volatile(p_node, node);
+            ptr::write_volatile(p_list_head, list_head);
+        }
+    }
+
+    /// Insert `node` before `list_tail` and after the second-to-last node
+    pub unsafe fn insert_tail(
+        _cs: CriticalSection,
+        mut p_list_tail: *mut LinkedListNode,
+        mut p_node: *mut LinkedListNode,
+    ) {
+        let mut list_tail = ptr::read_volatile(p_list_tail);
+        if p_list_tail != list_tail.prev {
+            let mut node_prev = ptr::read_volatile(list_tail.prev);
+            let node = LinkedListNode {
+                next: p_list_tail,
+                prev: list_tail.prev,
+            };
+
+            list_tail.prev = p_node;
+            node_prev.next = p_node;
+
+            // All nodes must be written because they will all be seen by another core
+            ptr::write_volatile(p_node, node);
+            ptr::write_volatile(node.prev, node_prev);
+            ptr::write_volatile(p_list_tail, list_tail);
+        } else {
+            let node = LinkedListNode {
+                next: p_list_tail,
+                prev: list_tail.prev,
+            };
+
+            list_tail.prev = p_node;
+            list_tail.next = p_node;
+
+            // All nodes must be written because they will all be seen by another core
+            ptr::write_volatile(p_node, node);
+            ptr::write_volatile(p_list_tail, list_tail);
+        }
+    }
+
+    /// Remove `node` from the linked list
+    pub unsafe fn remove_node(_cs: CriticalSection, mut p_node: *mut LinkedListNode) {
+        let node = ptr::read_unaligned(p_node);
+
+        if node.next != node.prev {
+            let mut node_next = ptr::read_volatile(node.next);
+            let mut node_prev = ptr::read_volatile(node.prev);
+
+            node_prev.next = node.next;
+            node_next.prev = node.prev;
+
+            ptr::write_volatile(node.next, node_next);
+            ptr::write_volatile(node.prev, node_prev);
+        } else {
+            let mut node_next = ptr::read_volatile(node.next);
+
+            node_next.next = node.next;
+            node_next.prev = node.prev;
+
+            ptr::write_volatile(node.next, node_next);
+        }
+    }
+
+    /// Remove `list_head` and return a pointer to the `node`.
+    pub unsafe fn remove_head(
+        _cs: CriticalSection,
+        mut p_list_head: *mut LinkedListNode,
+    ) -> Option<*mut LinkedListNode> {
+        let list_head = ptr::read_volatile(p_list_head);
+
+        if list_head.next == p_list_head {
+            None
+        } else {
+            // Allowed because a removed node is not seen by another core
+            let p_node = list_head.next;
+            Self::remove_node(_cs, p_node);
+
+            Some(p_node)
+        }
+    }
+
+    /// Remove `list_tail` and return a pointer to the `node`.
+    pub unsafe fn remove_tail(
+        _cs: CriticalSection,
+        mut p_list_tail: *mut LinkedListNode,
+    ) -> Option<*mut LinkedListNode> {
+        let list_tail = ptr::read_volatile(p_list_tail);
+
+        if list_tail.prev == p_list_tail {
+            None
+        } else {
+            // Allowed because a removed node is not seen by another core
+            let p_node = list_tail.prev;
+            Self::remove_node(_cs, p_node);
+
+            Some(p_node)
+        }
+    }
+
+    pub unsafe fn insert_node_after(
+        _cs: CriticalSection,
+        mut p_node: *mut LinkedListNode,
+        mut p_ref_node: *mut LinkedListNode,
+    ) {
+        let mut node = ptr::read_volatile(p_node);
+        let mut ref_node = ptr::read_volatile(p_ref_node);
+        let mut prev_node = ptr::read_volatile(ref_node.next);
+
+        node.next = ref_node.next;
+        node.prev = p_ref_node;
+        ref_node.next = p_node;
+        prev_node.prev = p_node;
+
+        ptr::write_volatile(p_node, node);
+        ptr::write_volatile(p_ref_node, ref_node);
+        ptr::write_volatile(node.next, prev_node);
+    }
+
+    pub unsafe fn insert_node_before(
+        _cs: CriticalSection,
+        mut node: *mut LinkedListNode,
+        mut ref_node: *mut LinkedListNode,
+    ) {
+        (*node).next = ref_node;
+        (*node).prev = (*ref_node).prev;
+        (*ref_node).prev = node;
+        (*(*node).prev).next = node;
+
+        todo!("this function has not been converted to volatile semantics");
+    }
+
+    pub unsafe fn get_size(_cs: CriticalSection, mut list_head: *mut LinkedListNode) -> usize {
+        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
+        }
+
+        let _ = size;
+
+        todo!("this function has not been converted to volatile semantics");
+    }
+
+    pub unsafe fn get_next_node(_cs: CriticalSection, mut p_ref_node: *mut LinkedListNode) -> *mut LinkedListNode {
+        let ref_node = ptr::read_volatile(p_ref_node);
+
+        // Allowed because a removed node is not seen by another core
+        ref_node.next
+    }
+
+    pub unsafe fn get_prev_node(_cs: CriticalSection, mut p_ref_node: *mut LinkedListNode) -> *mut LinkedListNode {
+        let ref_node = ptr::read_volatile(p_ref_node);
+
+        // Allowed because a removed node is not seen by another core
+        ref_node.prev
+    }
+}
+
+pub struct DebuggableLinkedListNode(*const LinkedListNode);
+
+impl Debug for DebuggableLinkedListNode {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        // Safe because this is just reading memory, and using unaligned to do it
+        let p_node = self.0;
+
+        f.write_fmt(format_args!("iterating list from node: {:x}", p_node as usize))?;
+
+        let mut p_current_node = p_node;
+        for _ in 0..30 {
+            let current_node = unsafe { ptr::read_unaligned(p_current_node) };
+            f.write_fmt(format_args!(
+                "node (prev, current, next): {:x}, {:x}, {:x}",
+                current_node.prev as usize, p_current_node as usize, current_node.next as usize
+            ))?;
+
+            if current_node.next == p_node as *mut _ {
+                break;
+            }
+
+            p_current_node = current_node.next;
+        }
+
+        Ok(())
+    }
+}