diff --git a/embassy-stm32-wpan/Cargo.toml b/embassy-stm32-wpan/Cargo.toml
index 1325faed9..91540321f 100644
--- a/embassy-stm32-wpan/Cargo.toml
+++ b/embassy-stm32-wpan/Cargo.toml
@@ -29,7 +29,7 @@ stm32wb-hci = { version = "0.1.2", features = ["version-5-0"], optional = true }
 bitflags = { version = "2.3.3", optional = true }
 
 [features]
-default = ["stm32wb55rg", "mac", "ble", "defmt"]
+default = []
 defmt = ["dep:defmt", "embassy-sync/defmt", "embassy-embedded-hal/defmt", "embassy-hal-common/defmt"]
 
 ble = ["dep:stm32wb-hci"]
diff --git a/embassy-stm32-wpan/src/sub/mac/mod.rs b/embassy-stm32-wpan/src/sub/mac/mod.rs
index 26358bf81..ab39f89c2 100644
--- a/embassy-stm32-wpan/src/sub/mac/mod.rs
+++ b/embassy-stm32-wpan/src/sub/mac/mod.rs
@@ -98,8 +98,6 @@ impl Mac {
         let mut payload = [0u8; MAX_PACKET_SIZE];
         cmd.copy_into_slice(&mut payload);
 
-        debug!("sending {}", &payload[..T::SIZE]);
-
         let response = self
             .tl_write_and_get_response(T::OPCODE as u16, &payload[..T::SIZE])
             .await;
diff --git a/embassy-stm32-wpan/src/sub/mac/responses.rs b/embassy-stm32-wpan/src/sub/mac/responses.rs
index 0d3c09869..2f6f5bf58 100644
--- a/embassy-stm32-wpan/src/sub/mac/responses.rs
+++ b/embassy-stm32-wpan/src/sub/mac/responses.rs
@@ -28,8 +28,6 @@ impl ParseableMacEvent for AssociateConfirm {
     const SIZE: usize = 16;
 
     fn try_parse(buf: &[u8]) -> Result<Self, ()> {
-        debug!("{}", buf);
-
         Self::validate(buf)?;
 
         Ok(Self {
diff --git a/embassy-stm32-wpan/src/sub/sys.rs b/embassy-stm32-wpan/src/sub/sys.rs
index caa4845f2..c17fd531d 100644
--- a/embassy-stm32-wpan/src/sub/sys.rs
+++ b/embassy-stm32-wpan/src/sub/sys.rs
@@ -50,7 +50,7 @@ impl Sys {
     }
 
     /// `HW_IPCC_SYS_CmdEvtNot`
-    pub async fn write_and_get_response(&self, opcode: ShciOpcode, payload: &[u8]) -> SchiCommandStatus {
+    pub async fn write_and_get_response(&self, opcode: ShciOpcode, payload: &[u8]) -> Result<SchiCommandStatus, ()> {
         self.write(opcode, payload).await;
         Ipcc::flush(channels::cpu1::IPCC_SYSTEM_CMD_RSP_CHANNEL).await;
 
@@ -59,12 +59,12 @@ impl Sys {
             let p_command_event = &((*p_event_packet).evt_serial.evt.payload) as *const _ as *const CcEvt;
             let p_payload = &((*p_command_event).payload) as *const u8;
 
-            ptr::read_volatile(p_payload).try_into().unwrap()
+            ptr::read_volatile(p_payload).try_into()
         }
     }
 
     #[cfg(feature = "mac")]
-    pub async fn shci_c2_mac_802_15_4_init(&self) -> SchiCommandStatus {
+    pub async fn shci_c2_mac_802_15_4_init(&self) -> Result<SchiCommandStatus, ()> {
         use crate::tables::{
             Mac802_15_4Table, TracesTable, MAC_802_15_4_CMD_BUFFER, MAC_802_15_4_NOTIF_RSP_EVT_BUFFER,
             TL_MAC_802_15_4_TABLE, TL_TRACES_TABLE, TRACES_EVT_QUEUE,
@@ -88,7 +88,7 @@ impl Sys {
     }
 
     #[cfg(feature = "ble")]
-    pub async fn shci_c2_ble_init(&self, param: ShciBleInitCmdParam) -> SchiCommandStatus {
+    pub async fn shci_c2_ble_init(&self, param: ShciBleInitCmdParam) -> Result<SchiCommandStatus, ()> {
         self.write_and_get_response(ShciOpcode::BleInit, param.payload()).await
     }
 
diff --git a/examples/stm32wb/.cargo/config.toml b/examples/stm32wb/.cargo/config.toml
index cf62a10a0..51c499ee7 100644
--- a/examples/stm32wb/.cargo/config.toml
+++ b/examples/stm32wb/.cargo/config.toml
@@ -1,7 +1,7 @@
 [target.'cfg(all(target_arch = "arm", target_os = "none"))']
 # replace STM32WB55CCUx with your chip as listed in `probe-rs chip list`
-runner = "probe-run --chip STM32WB55RGVx --speed 1000 --connect-under-reset"
-# runner = "teleprobe local run --chip STM32WB55RG --elf"
+# runner = "probe-run --chip STM32WB55RGVx --speed 1000 --connect-under-reset"
+runner = "teleprobe local run --chip STM32WB55RG --elf"
 
 [build]
 target = "thumbv7em-none-eabihf"
diff --git a/examples/stm32wb/src/bin/mac_ffd.rs b/examples/stm32wb/src/bin/mac_ffd.rs
index 37d36fcdd..689a28353 100644
--- a/examples/stm32wb/src/bin/mac_ffd.rs
+++ b/examples/stm32wb/src/bin/mac_ffd.rs
@@ -168,9 +168,13 @@ async fn main(spawner: Spawner) {
                     .unwrap(),
                 MacEvent::McpsDataInd(data_ind) => {
                     let data_addr = data_ind.msdu_ptr;
-                    let mut a = [0u8; 256];
-                    unsafe { data_addr.copy_to(&mut a as *mut _, data_ind.msdu_length as usize) }
-                    info!("{}", a[..data_ind.msdu_length as usize])
+                    let mut data = [0u8; 256];
+                    unsafe { data_addr.copy_to(&mut data as *mut _, data_ind.msdu_length as usize) }
+                    info!("{}", data[..data_ind.msdu_length as usize]);
+
+                    if &data[..data_ind.msdu_length as usize] == b"Hello from embassy!" {
+                        info!("success");
+                    }
                 }
                 _ => {}
             }
diff --git a/examples/stm32wb/src/bin/mac_rfd.rs b/examples/stm32wb/src/bin/mac_rfd.rs
index 756709132..ea349f9a8 100644
--- a/examples/stm32wb/src/bin/mac_rfd.rs
+++ b/examples/stm32wb/src/bin/mac_rfd.rs
@@ -148,7 +148,7 @@ async fn main(spawner: Spawner) {
         .send_command(&DataRequest {
             src_addr_mode: AddressMode::Short,
             dst_addr_mode: AddressMode::Short,
-            dst_pan_id: PanId::BROADCAST,
+            dst_pan_id: PanId([0x1A, 0xAA]),
             dst_address: MacAddress::BROADCAST,
             msdu_handle: 0x02,
             ack_tx: 0x00,