From 26038cfbc60e9a7b64d70ade51e9e9f34c912e2e Mon Sep 17 00:00:00 2001 From: xoviat Date: Mon, 17 Nov 2025 11:16:55 -0600 Subject: wpan: parse frames correctly --- embassy-stm32-wpan/Cargo.toml | 3 +- embassy-stm32-wpan/src/mac/commands.rs | 26 +++++++++ embassy-stm32-wpan/src/mac/driver.rs | 12 ++--- embassy-stm32-wpan/src/mac/indications.rs | 18 +++++++ embassy-stm32-wpan/src/mac/runner.rs | 87 +++++++++++++++++++------------ embassy-stm32-wpan/src/mac/typedefs.rs | 42 +++++++++++++++ 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> for DataRequest { + type Error = (); + + fn try_from(frame: Frame<&'a T>) -> Result { + // 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 = blocking_mutex::Mutex>>>; @@ -68,55 +69,75 @@ impl<'a> Runner<'a> { } } + async fn send_request>(&self, frame: U) -> Result<(), ()> + where + (): From<>::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::(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 for AddressMode { + type Error = (); + + fn try_from(value: AddressingMode) -> Result { + 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
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 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 for PanId { + fn from(value: Pan) -> Self { + Self(value.0.to_be_bytes()) + } +} + +impl From for Pan { + fn from(value: PanId) -> Self { + Self(u16::from_be_bytes(value.0)) + } +} -- cgit