wpan: parse frames correctly

6 files changed, 148 insertions, 40 deletions

diff --git a/embassy-stm32-wpan/Cargo.toml b/embassy-stm32-wpan/Cargo.toml
index 75d978d1a..05d76f4a6 100644
--- a/embassy-stm32-wpan/Cargo.toml
+++ b/embassy-stm32-wpan/Cargo.toml
@@ -33,6 +33,7 @@ embassy-futures = { version = "0.1.2", path = "../embassy-futures" }
 embassy-hal-internal = { version = "0.3.0", path = "../embassy-hal-internal" }
 embassy-embedded-hal = { version = "0.5.0", path = "../embassy-embedded-hal" }
 embassy-net-driver = { version = "0.2.0", path = "../embassy-net-driver", optional=true }
+smoltcp = { version = "0.12.0", optional=true, default-features = false }
 
 defmt = { version = "1.0.1", optional = true }
 log = { version = "0.4.17", optional = true }
@@ -52,7 +53,7 @@ bitflags = { version = "2.3.3", optional = true }
 defmt = ["dep:defmt", "embassy-sync/defmt", "embassy-embedded-hal/defmt", "embassy-hal-internal/defmt", "stm32wb-hci?/defmt"]
 
 ble = ["dep:stm32wb-hci"]
-mac = ["dep:bitflags", "dep:embassy-net-driver" ]
+mac = ["dep:bitflags", "dep:embassy-net-driver", "dep:smoltcp", "smoltcp/medium-ieee802154"]
 
 extended = []
 
diff --git a/embassy-stm32-wpan/src/mac/commands.rs b/embassy-stm32-wpan/src/mac/commands.rs
index 82b9d2772..e0bc50e2a 100644
--- a/embassy-stm32-wpan/src/mac/commands.rs
+++ b/embassy-stm32-wpan/src/mac/commands.rs
@@ -8,6 +8,8 @@ use super::typedefs::{
     PanId, PibId, ScanType, SecurityLevel,
 };
 
+use smoltcp::wire::ieee802154::Frame;
+
 pub trait MacCommand: Sized {
     const OPCODE: OpcodeM4ToM0;
 
@@ -379,6 +381,30 @@ impl DataRequest {
     }
 }
 
+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(())?,
+            ack_tx: 0x00,
+            gts_tx: false,
+            security_level: SecurityLevel::Unsecure,
+            ..Default::default()
+        };
+
+        request.set_buffer(frame.payload().ok_or(())?);
+
+        Ok(request)
+    }
+}
+
 impl Default for DataRequest {
     fn default() -> Self {
         Self {
diff --git a/embassy-stm32-wpan/src/mac/driver.rs b/embassy-stm32-wpan/src/mac/driver.rs
index 5592723a2..c71fabe09 100644
--- a/embassy-stm32-wpan/src/mac/driver.rs
+++ b/embassy-stm32-wpan/src/mac/driver.rs
@@ -11,6 +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::runner::{BUF_SIZE, ZeroCopyPubSub};
 use crate::mac::{Control, MTU, Runner};
 use crate::sub::mac::{Mac, MacRx, MacTx};
@@ -179,14 +180,13 @@ impl<'d> embassy_net_driver::RxToken for RxToken<'d> {
     where
         F: FnOnce(&mut [u8]) -> R,
     {
-        // Only valid data events should be put into the queue
-
-        let data_event = match self.rx.try_receive().unwrap() {
-            MacEvent::McpsDataInd(data_event) => data_event,
-            _ => unreachable!(),
+        let mut buffer = [0u8; MTU];
+        match self.rx.try_receive().unwrap() {
+            MacEvent::McpsDataInd(data_event) => write_frame_from_data_indication(data_event, &mut buffer),
+            _ => {}
         };
 
-        f(&mut data_event.payload())
+        f(&mut buffer[..])
     }
 }
 
diff --git a/embassy-stm32-wpan/src/mac/indications.rs b/embassy-stm32-wpan/src/mac/indications.rs
index c0b86d745..45f79ac5b 100644
--- a/embassy-stm32-wpan/src/mac/indications.rs
+++ b/embassy-stm32-wpan/src/mac/indications.rs
@@ -7,6 +7,9 @@ use super::typedefs::{
     PanId, SecurityLevel,
 };
 
+use smoltcp::wire::Ieee802154FrameType;
+use smoltcp::wire::ieee802154::Frame;
+
 /// MLME ASSOCIATE Indication which will be used by the MAC
 /// to indicate the reception of an association request command
 #[repr(C)]
@@ -250,6 +253,21 @@ 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_dst_pan_id(data.dst_pan_id.into());
+    frame.set_src_pan_id(data.src_pan_id.into());
+    frame.set_sequence_number(data.dsn);
+
+    // 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)]
diff --git a/embassy-stm32-wpan/src/mac/runner.rs b/embassy-stm32-wpan/src/mac/runner.rs
index 92c74c2ee..acca70019 100644
--- a/embassy-stm32-wpan/src/mac/runner.rs
+++ b/embassy-stm32-wpan/src/mac/runner.rs
@@ -6,12 +6,13 @@ 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::mac::typedefs::*;
 use crate::sub::mac::{MacRx, MacTx};
 
 pub type ZeroCopyPubSub<M, T> = blocking_mutex::Mutex<M, RefCell<Option<Signal<NoopRawMutex, T>>>>;
@@ -68,55 +69,75 @@ impl<'a> Runner<'a> {
         }
     }
 
+    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.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;
                             }
                             _ => {
-                                self.rx_event_channel.lock(|s| {
-                                    s.borrow().as_ref().map(|signal| signal.signal(mac_event));
-                                });
+                                debug!("unhandled mac event: {:#x}", mac_event);
                             }
                         }
                     }
                 }
             },
             async {
-                let mut msdu_handle = 0x02;
-
                 loop {
-                    let (buf, len) = self.tx_data_channel.receive().await;
-                    let mac_tx = self.mac_tx.lock().await;
-
-                    let pan_id = critical_section::with(|cs| self.network_state.borrow(cs).borrow().pan_id);
-
-                    // TODO: get the destination address from the packet instead of using the broadcast address
-
-                    // The mutex should be dropped on the next loop iteration
-                    mac_tx
-                        .send_command(
-                            DataRequest {
-                                src_addr_mode: AddressMode::Short,
-                                dst_addr_mode: AddressMode::Short,
-                                dst_pan_id: PanId(pan_id),
-                                dst_address: MacAddress::BROADCAST,
-                                msdu_handle: msdu_handle,
-                                ack_tx: 0x00,
-                                gts_tx: false,
-                                security_level: SecurityLevel::Unsecure,
-                                ..Default::default()
-                            }
-                            .set_buffer(&buf[..len]),
-                        )
-                        .await
-                        .unwrap();
-
-                    msdu_handle = msdu_handle.wrapping_add(1);
+                    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();
diff --git a/embassy-stm32-wpan/src/mac/typedefs.rs b/embassy-stm32-wpan/src/mac/typedefs.rs
index 0552b8ea1..44028bf47 100644
--- a/embassy-stm32-wpan/src/mac/typedefs.rs
+++ b/embassy-stm32-wpan/src/mac/typedefs.rs
@@ -1,4 +1,5 @@
 use core::fmt::Debug;
+use smoltcp::wire::ieee802154::{Address, AddressingMode, Pan};
 
 use crate::numeric_enum;
 
@@ -109,12 +110,41 @@ numeric_enum! {
 }
 }
 
+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] },
+        }
+    }
+}
+
+impl From<MacAddress> for Address {
+    fn from(_value: MacAddress) -> Self {
+        todo!()
+    }
+}
+
 impl Debug for MacAddress {
     fn fmt(&self, fmt: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
         unsafe {
@@ -379,3 +409,15 @@ 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))
+    }
+}