wpan: fix src/dst address

3 files changed, 37 insertions, 8 deletions

diff --git a/embassy-stm32-wpan/src/mac/driver.rs b/embassy-stm32-wpan/src/mac/driver.rs
index c71fabe09..c43d595b7 100644
--- a/embassy-stm32-wpan/src/mac/driver.rs
+++ b/embassy-stm32-wpan/src/mac/driver.rs
@@ -11,7 +11,7 @@ use embassy_sync::mutex::Mutex;
 use embassy_sync::waitqueue::AtomicWaker;
 
 use crate::mac::event::MacEvent;
-use crate::mac::indications::write_frame_from_data_indication;
+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};
@@ -183,6 +183,7 @@ impl<'d> embassy_net_driver::RxToken for RxToken<'d> {
         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),
             _ => {}
         };
 
diff --git a/embassy-stm32-wpan/src/mac/indications.rs b/embassy-stm32-wpan/src/mac/indications.rs
index 05869ba2a..a43777e75 100644
--- a/embassy-stm32-wpan/src/mac/indications.rs
+++ b/embassy-stm32-wpan/src/mac/indications.rs
@@ -3,6 +3,8 @@ use core::slice;
 use smoltcp::wire::Ieee802154FrameType;
 use smoltcp::wire::ieee802154::Frame;
 
+use crate::mac::typedefs::MacAddressAndMode;
+
 use super::consts::MAX_PENDING_ADDRESS;
 use super::event::ParseableMacEvent;
 use super::typedefs::{
@@ -77,6 +79,22 @@ pub struct BeaconNotifyIndication {
 
 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)]
@@ -256,13 +274,13 @@ impl DataIndication {
 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);
 
-    // TODO: complete frame creation
     frame.set_frame_type(Ieee802154FrameType::Data);
-    frame.set_dst_addr(data.dst_address.into());
-    frame.set_src_addr(data.src_address.into());
+    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());
diff --git a/embassy-stm32-wpan/src/mac/typedefs.rs b/embassy-stm32-wpan/src/mac/typedefs.rs
index 7e3ef4962..175d4a37d 100644
--- a/embassy-stm32-wpan/src/mac/typedefs.rs
+++ b/embassy-stm32-wpan/src/mac/typedefs.rs
@@ -140,9 +140,19 @@ impl From<Address> for MacAddress {
     }
 }
 
-impl From<MacAddress> for Address {
-    fn from(_value: MacAddress) -> Self {
-        todo!()
+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,
+        }
     }
 }
 
@@ -377,7 +387,7 @@ numeric_enum! {
 
 numeric_enum! {
     #[repr(u8)]
-    #[derive(Default, Clone, Copy, Debug)]
+    #[derive(Default, Clone, Copy, Debug, PartialEq)]
     #[cfg_attr(feature = "defmt", derive(defmt::Format))]
     pub enum SecurityLevel {
         /// MAC Unsecured Mode Security