8 files changed, 133 insertions, 41 deletions

diff --git a/embassy-stm32-wpan/src/sub/mac/commands.rs b/embassy-stm32-wpan/src/sub/mac/commands.rs
index 7ccf10026..8cfa0a054 100644
--- a/embassy-stm32-wpan/src/sub/mac/commands.rs
+++ b/embassy-stm32-wpan/src/sub/mac/commands.rs
@@ -1,7 +1,7 @@
 use super::opcodes::OpcodeM4ToM0;
 use super::typedefs::{
-    AddressMode, Capabilities, DisassociationReason, GtsCharacteristics, KeyIdMode, MacAddress, MacChannel, PibId,
-    ScanType, SecurityLevel,
+    AddressMode, Capabilities, DisassociationReason, GtsCharacteristics, KeyIdMode, MacAddress, MacChannel, MacStatus,
+    PanId, PibId, ScanType, SecurityLevel,
 };
 
 pub trait MacCommand {
@@ -26,7 +26,7 @@ pub struct AssociateRequest {
     /// operational capabilities of the associating device
     pub capability_information: Capabilities,
     /// the identifier of the PAN with which to associate
-    pub coord_pan_id: [u8; 2],
+    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
@@ -51,7 +51,7 @@ pub struct DisassociateRequest {
     /// device addressing mode used
     pub device_addr_mode: AddressMode,
     /// the identifier of the PAN of the device
-    pub device_pan_id: [u8; 2],
+    pub device_pan_id: PanId,
     /// the reason for the disassociation
     pub disassociation_reason: DisassociationReason,
     /// device address
@@ -201,7 +201,7 @@ impl MacCommand for SetRequest {
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct StartRequest {
     /// PAN indentifier to used by the device
-    pub pan_id: [u8; 2],
+    pub pan_id: PanId,
     /// logical channel on which to begin
     pub channel_number: MacChannel,
     /// channel page on which to begin
@@ -277,7 +277,7 @@ pub struct PollRequest {
     /// originator of the key to be used
     pub key_source: [u8; 8],
     /// PAN identifier of the coordinator
-    pub coord_pan_id: [u8; 2],
+    pub coord_pan_id: PanId,
 }
 
 impl MacCommand for PollRequest {
@@ -337,7 +337,7 @@ pub struct DataRequest {
     /// destination addressing mode used
     pub dst_addr_mode: AddressMode,
     /// destination PAN Id
-    pub dst_pan_id: [u8; 2],
+    pub dst_pan_id: PanId,
     /// destination address
     pub dst_address: MacAddress,
     /// the number of octets contained in the MSDU
@@ -370,6 +370,31 @@ pub struct DataRequest {
     pub datrate: u8,
 }
 
+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;
     const SIZE: usize = 40;
@@ -391,6 +416,7 @@ impl MacCommand for PurgeRequest {
 
 /// 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
@@ -399,7 +425,7 @@ pub struct AssociateResponse {
     /// association
     pub assoc_short_address: [u8; 2],
     /// status of the association attempt
-    pub status: u8,
+    pub status: MacStatus,
     /// security level to be used
     pub security_level: SecurityLevel,
     /// the originator of the key to be used
diff --git a/embassy-stm32-wpan/src/sub/mac/consts.rs b/embassy-stm32-wpan/src/sub/mac/consts.rs
index 892d533b4..56903d980 100644
--- a/embassy-stm32-wpan/src/sub/mac/consts.rs
+++ b/embassy-stm32-wpan/src/sub/mac/consts.rs
@@ -1,4 +1,4 @@
-pub const MAX_ED_SCAN_RESULTS_SUPPORTED: usize = 16;
 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/sub/mac/indications.rs b/embassy-stm32-wpan/src/sub/mac/indications.rs
index 4695f24ef..b67f0a686 100644
--- a/embassy-stm32-wpan/src/sub/mac/indications.rs
+++ b/embassy-stm32-wpan/src/sub/mac/indications.rs
@@ -3,7 +3,7 @@ use super::event::ParseableMacEvent;
 use super::helpers::to_u32;
 use super::typedefs::{
     AddressMode, Capabilities, DisassociationReason, KeyIdMode, MacAddress, MacChannel, MacStatus, PanDescriptor,
-    SecurityLevel,
+    PanId, SecurityLevel,
 };
 
 /// MLME ASSOCIATE Indication which will be used by the MAC
@@ -110,7 +110,7 @@ impl ParseableMacEvent for BeaconNotifyIndication {
 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: [u8; 2],
+    pub pan_id: PanId,
     /// Source addressing mode
     pub src_addr_mode: AddressMode,
     /// Destination addressing mode
@@ -163,7 +163,7 @@ impl ParseableMacEvent for CommStatusIndication {
         };
 
         Ok(Self {
-            pan_id: [buf[0], buf[1]],
+            pan_id: PanId([buf[0], buf[1]]),
             src_addr_mode,
             dst_addr_mode,
             src_address,
@@ -251,7 +251,7 @@ impl ParseableMacEvent for OrphanIndication {
 #[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: [u8; 2],
+    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
@@ -275,7 +275,7 @@ impl ParseableMacEvent for SyncLossIndication {
         Self::validate(buf)?;
 
         Ok(Self {
-            pan_id: [buf[0], buf[1]],
+            pan_id: PanId([buf[0], buf[1]]),
             loss_reason: buf[2],
             channel_number: MacChannel::try_from(buf[3])?,
             channel_page: buf[4],
@@ -303,19 +303,20 @@ impl ParseableMacEvent for DpsIndication {
 }
 
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
+#[repr(C, align(8))]
 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: [u8; 2],
+    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: [u8; 2],
+    pub dst_pan_id: PanId,
     /// Destination address  
     pub dst_address: MacAddress,
     /// The number of octets contained in the MSDU being indicated  
@@ -355,7 +356,7 @@ pub struct DataIndication {
 }
 
 impl ParseableMacEvent for DataIndication {
-    const SIZE: usize = 72;
+    const SIZE: usize = 68;
 
     fn try_parse(buf: &[u8]) -> Result<Self, ()> {
         Self::validate(buf)?;
@@ -387,24 +388,24 @@ impl ParseableMacEvent for DataIndication {
         Ok(Self {
             msdu_ptr: to_u32(&buf[0..4]) as *const u8,
             src_addr_mode,
-            src_pan_id: [buf[5], buf[6]],
+            src_pan_id: PanId([buf[5], buf[6]]),
             src_address,
             dst_addr_mode,
-            dst_pan_id: [buf[16], buf[17]],
+            dst_pan_id: PanId([buf[16], buf[17]]),
             dst_address,
             msdu_length: buf[26],
             mpdu_link_quality: buf[27],
             dsn: buf[28],
             time_stamp: [buf[29], buf[30], buf[31], buf[32]],
-            security_level: SecurityLevel::try_from(buf[33])?,
-            key_id_mode: KeyIdMode::try_from(buf[34])?,
+            security_level: SecurityLevel::try_from(buf[33]).unwrap_or(SecurityLevel::Unsecure), // TODO: this is totaly wrong, but I'm too smol brain to fix it
+            key_id_mode: KeyIdMode::try_from(buf[34]).unwrap_or(KeyIdMode::Implicite), // TODO: this is totaly wrong, but I'm too smol brain to fix it
             key_source: [buf[35], buf[36], buf[37], buf[38], buf[39], buf[40], buf[41], buf[42]],
             key_index: buf[43],
             uwbprf: buf[44],
             uwn_preamble_symbol_repetitions: buf[45],
             datrate: buf[46],
             ranging_received: buf[47],
-            ranging_counter_start: to_u32(&buf[58..52]),
+            ranging_counter_start: to_u32(&buf[48..52]),
             ranging_counter_stop: to_u32(&buf[52..56]),
             ranging_tracking_interval: to_u32(&buf[56..60]),
             ranging_offset: to_u32(&buf[60..64]),
diff --git a/embassy-stm32-wpan/src/sub/mac/opcodes.rs b/embassy-stm32-wpan/src/sub/mac/opcodes.rs
index c9a07d6af..fd7011873 100644
--- a/embassy-stm32-wpan/src/sub/mac/opcodes.rs
+++ b/embassy-stm32-wpan/src/sub/mac/opcodes.rs
@@ -5,6 +5,7 @@ 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),
@@ -26,6 +27,7 @@ pub enum OpcodeM4ToM0 {
     McpsPurgeReq = opcode(0x11),
 }
 
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum OpcodeM0ToM4 {
     MlmeAssociateCnf = 0x00,
     MlmeDisassociateCnf,
diff --git a/embassy-stm32-wpan/src/sub/mac/responses.rs b/embassy-stm32-wpan/src/sub/mac/responses.rs
index 37271ec28..2b90ccdc6 100644
--- a/embassy-stm32-wpan/src/sub/mac/responses.rs
+++ b/embassy-stm32-wpan/src/sub/mac/responses.rs
@@ -2,7 +2,8 @@ use super::consts::{MAX_ED_SCAN_RESULTS_SUPPORTED, MAX_PAN_DESC_SUPPORTED, MAX_S
 use super::event::ParseableMacEvent;
 use super::helpers::to_u32;
 use super::typedefs::{
-    AddressMode, AssociationStatus, KeyIdMode, MacAddress, MacStatus, PanDescriptor, PibId, ScanType, SecurityLevel,
+    AddressMode, AssociationStatus, KeyIdMode, MacAddress, MacStatus, PanDescriptor, PanId, PibId, ScanType,
+    SecurityLevel,
 };
 
 /// MLME ASSOCIATE Confirm used to inform of the initiating device whether
@@ -50,7 +51,7 @@ pub struct DisassociateConfirm {
     /// device addressing mode used
     pub device_addr_mode: AddressMode,
     /// the identifier of the PAN of the device
-    pub device_pan_id: [u8; 2],
+    pub device_pan_id: PanId,
     /// device address
     pub device_address: MacAddress,
 }
@@ -76,7 +77,7 @@ impl ParseableMacEvent for DisassociateConfirm {
         Ok(Self {
             status: MacStatus::try_from(buf[0])?,
             device_addr_mode,
-            device_pan_id: [buf[2], buf[3]],
+            device_pan_id: PanId([buf[2], buf[3]]),
             device_address,
         })
     }
diff --git a/embassy-stm32-wpan/src/sub/mac/typedefs.rs b/embassy-stm32-wpan/src/sub/mac/typedefs.rs
index 1a4c30cbc..5ff051c97 100644
--- a/embassy-stm32-wpan/src/sub/mac/typedefs.rs
+++ b/embassy-stm32-wpan/src/sub/mac/typedefs.rs
@@ -25,15 +25,12 @@ impl From<u8> for MacError {
 
 numeric_enum! {
     #[repr(u8)]
-    #[derive(Debug)]
+    #[derive(Debug, Default)]
     #[cfg_attr(feature = "defmt", derive(defmt::Format))]
     pub enum MacStatus {
+        #[default]
         Success = 0x00,
-        Error = 0x01,
-        NotImplemented = 0x02,
-        NotSupported = 0x03,
-        HardwareNotSupported = 0x04,
-        Undefined = 0x05,
+        Failure = 0xFF
     }
 }
 
@@ -134,6 +131,10 @@ impl Default for MacAddress {
     }
 }
 
+impl MacAddress {
+    pub const BROADCAST: Self = Self { short: [0xFF, 0xFF] };
+}
+
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct GtsCharacteristics {
     pub fields: u8,
@@ -145,7 +146,7 @@ pub struct GtsCharacteristics {
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct PanDescriptor {
     /// PAN identifier of the coordinator
-    pub coord_pan_id: [u8; 2],
+    pub coord_pan_id: PanId,
     /// Coordinator addressing mode
     pub coord_addr_mode: AddressMode,
     /// The current logical channel occupied by the network
@@ -188,7 +189,7 @@ impl TryFrom<&[u8]> for PanDescriptor {
         };
 
         Ok(Self {
-            coord_pan_id: [buf[0], buf[1]],
+            coord_pan_id: PanId([buf[0], buf[1]]),
             coord_addr_mode,
             logical_channel: MacChannel::try_from(buf[3])?,
             coord_addr,
@@ -336,3 +337,12 @@ numeric_enum! {
         Orphan = 0x03
     }
 }
+
+/// newtype for Pan Id
+#[derive(Default, Clone, Copy)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct PanId(pub [u8; 2]);
+
+impl PanId {
+    pub const BROADCAST: Self = Self([0xFF, 0xFF]);
+}
diff --git a/examples/stm32wb/src/bin/mac_ffd.rs b/examples/stm32wb/src/bin/mac_ffd.rs
index 4e2578a21..37d36fcdd 100644
--- a/examples/stm32wb/src/bin/mac_ffd.rs
+++ b/examples/stm32wb/src/bin/mac_ffd.rs
@@ -6,8 +6,9 @@ use defmt::*;
 use embassy_executor::Spawner;
 use embassy_stm32::bind_interrupts;
 use embassy_stm32::ipcc::{Config, ReceiveInterruptHandler, TransmitInterruptHandler};
-use embassy_stm32_wpan::sub::mac::commands::{ResetRequest, SetRequest, StartRequest};
-use embassy_stm32_wpan::sub::mac::typedefs::{MacChannel, PibId};
+use embassy_stm32_wpan::sub::mac::commands::{AssociateResponse, ResetRequest, SetRequest, StartRequest};
+use embassy_stm32_wpan::sub::mac::event::MacEvent;
+use embassy_stm32_wpan::sub::mac::typedefs::{MacChannel, MacStatus, PanId, PibId, SecurityLevel};
 use embassy_stm32_wpan::sub::mm;
 use embassy_stm32_wpan::TlMbox;
 use {defmt_rtt as _, panic_probe as _};
@@ -123,7 +124,7 @@ async fn main(spawner: Spawner) {
     info!("starting FFD device");
     mbox.mac_subsystem
         .send_command(&StartRequest {
-            pan_id: [0xAA, 0x1A],
+            pan_id: PanId([0x1A, 0xAA]),
             channel_number: MacChannel::Channel16,
             beacon_order: 0x0F,
             superframe_order: 0x0F,
@@ -151,5 +152,28 @@ async fn main(spawner: Spawner) {
     loop {
         let evt = mbox.mac_subsystem.read().await;
         defmt::info!("{:#x}", evt);
+
+        if let Ok(evt) = evt {
+            match evt {
+                MacEvent::MlmeAssociateInd(association) => mbox
+                    .mac_subsystem
+                    .send_command(&AssociateResponse {
+                        device_address: association.device_address,
+                        assoc_short_address: [0x33, 0x44],
+                        status: MacStatus::Success,
+                        security_level: SecurityLevel::Unsecure,
+                        ..Default::default()
+                    })
+                    .await
+                    .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])
+                }
+                _ => {}
+            }
+        }
     }
 }
diff --git a/examples/stm32wb/src/bin/mac_rfd.rs b/examples/stm32wb/src/bin/mac_rfd.rs
index e5f8d54c9..756709132 100644
--- a/examples/stm32wb/src/bin/mac_rfd.rs
+++ b/examples/stm32wb/src/bin/mac_rfd.rs
@@ -6,10 +6,10 @@ use defmt::*;
 use embassy_executor::Spawner;
 use embassy_stm32::bind_interrupts;
 use embassy_stm32::ipcc::{Config, ReceiveInterruptHandler, TransmitInterruptHandler};
-use embassy_stm32_wpan::sub::mac::commands::{AssociateRequest, GetRequest, ResetRequest, SetRequest};
+use embassy_stm32_wpan::sub::mac::commands::{AssociateRequest, DataRequest, GetRequest, ResetRequest, SetRequest};
 use embassy_stm32_wpan::sub::mac::event::MacEvent;
 use embassy_stm32_wpan::sub::mac::typedefs::{
-    AddressMode, Capabilities, KeyIdMode, MacAddress, MacChannel, PibId, SecurityLevel,
+    AddressMode, Capabilities, KeyIdMode, MacAddress, MacChannel, PanId, PibId, SecurityLevel,
 };
 use embassy_stm32_wpan::sub::mm;
 use embassy_stm32_wpan::TlMbox;
@@ -112,7 +112,7 @@ async fn main(spawner: Spawner) {
         coord_addr_mode: AddressMode::Short,
         coord_address: MacAddress { short: [34, 17] },
         capability_information: Capabilities::ALLOCATE_ADDRESS,
-        coord_pan_id: [0xAA, 0x1A],
+        coord_pan_id: PanId([0x1A, 0xAA]),
         security_level: SecurityLevel::Unsecure,
         key_id_mode: KeyIdMode::Implicite,
         key_source: [0; 8],
@@ -123,11 +123,16 @@ async fn main(spawner: Spawner) {
     let evt = mbox.mac_subsystem.read().await;
     info!("{:#x}", evt);
 
+    let short_addr = if let Ok(MacEvent::MlmeAssociateCnf(conf)) = evt {
+        conf.assoc_short_address
+    } else {
+        defmt::panic!()
+    };
+
     info!("setting short address");
-    let short: u64 = 0xACDE480000000002;
     mbox.mac_subsystem
         .send_command(&SetRequest {
-            pib_attribute_ptr: &short as *const _ as *const u8,
+            pib_attribute_ptr: &short_addr as *const _ as *const u8,
             pib_attribute: PibId::ShortAddress,
         })
         .await
@@ -135,6 +140,29 @@ async fn main(spawner: Spawner) {
     let evt = mbox.mac_subsystem.read().await;
     info!("{:#x}", evt);
 
+    info!("sending data");
+    let mut data_buffer = [0u8; 256];
+    let data = b"Hello from embassy!";
+    data_buffer[..data.len()].copy_from_slice(data);
+    mbox.mac_subsystem
+        .send_command(&DataRequest {
+            src_addr_mode: AddressMode::Short,
+            dst_addr_mode: AddressMode::Short,
+            dst_pan_id: PanId::BROADCAST,
+            dst_address: MacAddress::BROADCAST,
+            msdu_handle: 0x02,
+            ack_tx: 0x00,
+            gts_tx: false,
+            msdu_ptr: &data_buffer as *const _ as *const u8,
+            msdu_length: data.len() as u8,
+            security_level: SecurityLevel::Unsecure,
+            ..Default::default()
+        })
+        .await
+        .unwrap();
+    let evt = mbox.mac_subsystem.read().await;
+    info!("{:#x}", evt);
+
     loop {
         let evt = mbox.mac_subsystem.read().await;
         info!("{:#x}", evt);