diff --git a/src/runner.rs b/src/runner.rs index deb45f9f..f0f6fcee 100644 --- a/src/runner.rs +++ b/src/runner.rs @@ -328,45 +328,23 @@ where let len = (status & STATUS_F2_PKT_LEN_MASK) >> STATUS_F2_PKT_LEN_SHIFT; self.bus.wlan_read(buf, len).await; trace!("rx {:02x}", Bytes(&slice8_mut(buf)[..(len as usize).min(48)])); - self.rx(&slice8_mut(buf)[..len as usize]); + self.rx(&mut slice8_mut(buf)[..len as usize]); } else { break; } } } - fn rx(&mut self, packet: &[u8]) { - if packet.len() < SdpcmHeader::SIZE { - warn!("packet too short, len={}", packet.len()); - return; - } - - let sdpcm_header = SdpcmHeader::from_bytes(packet[..SdpcmHeader::SIZE].try_into().unwrap()); - trace!("rx {:?}", sdpcm_header); - if sdpcm_header.len != !sdpcm_header.len_inv { - warn!("len inv mismatch"); - return; - } - if sdpcm_header.len as usize != packet.len() { - // TODO: is this guaranteed?? - warn!("len from header doesn't match len from spi"); - return; - } + fn rx(&mut self, packet: &mut [u8]) { + let Some((sdpcm_header, payload)) = SdpcmHeader::parse(packet) else { return }; self.update_credit(&sdpcm_header); let channel = sdpcm_header.channel_and_flags & 0x0f; - let payload = &packet[sdpcm_header.header_length as _..]; - match channel { CHANNEL_TYPE_CONTROL => { - if payload.len() < CdcHeader::SIZE { - warn!("payload too short, len={}", payload.len()); - return; - } - - let cdc_header = CdcHeader::from_bytes(payload[..CdcHeader::SIZE].try_into().unwrap()); + let Some((cdc_header, response)) = CdcHeader::parse(payload) else { return; }; trace!(" {:?}", cdc_header); if cdc_header.id == self.ioctl_id { @@ -375,28 +353,21 @@ where panic!("IOCTL error {}", cdc_header.status as i32); } - let resp_len = cdc_header.len as usize; - let response = &payload[CdcHeader::SIZE..][..resp_len]; info!("IOCTL Response: {:02x}", Bytes(response)); self.ioctl_state.ioctl_done(response); } } CHANNEL_TYPE_EVENT => { - let bcd_header = BcdHeader::from_bytes(&payload[..BcdHeader::SIZE].try_into().unwrap()); - trace!(" {:?}", bcd_header); - - let packet_start = BcdHeader::SIZE + 4 * bcd_header.data_offset as usize; - - if packet_start + EventPacket::SIZE > payload.len() { + let Some((_, bcd_packet)) = BcdHeader::parse(payload) else { warn!("BCD event, incomplete header"); return; - } - let bcd_packet = &payload[packet_start..]; - trace!(" {:02x}", Bytes(&bcd_packet[..(bcd_packet.len() as usize).min(36)])); + }; - let mut event_packet = EventPacket::from_bytes(&bcd_packet[..EventPacket::SIZE].try_into().unwrap()); - event_packet.byteswap(); + let Some((event_packet, evt_data)) = EventPacket::parse(bcd_packet) else { + warn!("BCD event, incomplete data"); + return; + }; const ETH_P_LINK_CTL: u16 = 0x886c; // HPNA, wlan link local tunnel, according to linux if_ether.h if event_packet.eth.ether_type != ETH_P_LINK_CTL { @@ -427,13 +398,7 @@ where return; } - if event_packet.msg.datalen as usize >= (bcd_packet.len() - EventMessage::SIZE) { - warn!("BCD event, incomplete data"); - return; - } - let evt_type = events::Event::from(event_packet.msg.event_type as u8); - let evt_data = &bcd_packet[EventMessage::SIZE..][..event_packet.msg.datalen as usize]; debug!( "=== EVENT {:?}: {:?} {:02x}", evt_type, @@ -449,16 +414,8 @@ where } } CHANNEL_TYPE_DATA => { - let bcd_header = BcdHeader::from_bytes(&payload[..BcdHeader::SIZE].try_into().unwrap()); - trace!(" {:?}", bcd_header); - - let packet_start = BcdHeader::SIZE + 4 * bcd_header.data_offset as usize; - if packet_start > payload.len() { - warn!("packet start out of range."); - return; - } - let packet = &payload[packet_start..]; - trace!("rx pkt {:02x}", Bytes(&packet[..(packet.len() as usize).min(48)])); + let Some((_, packet)) = BcdHeader::parse(payload) else { return }; + trace!("rx pkt {:02x}", Bytes(&packet[..packet.len().min(48)])); match self.ch.try_rx_buf() { Some(buf) => { diff --git a/src/structs.rs b/src/structs.rs index e16808f3..6d5d31b0 100644 --- a/src/structs.rs +++ b/src/structs.rs @@ -1,4 +1,5 @@ use crate::events::Event; +use crate::fmt::Bytes; macro_rules! impl_bytes { ($t:ident) => { @@ -11,8 +12,28 @@ macro_rules! impl_bytes { } #[allow(unused)] - pub fn from_bytes(bytes: &[u8; Self::SIZE]) -> Self { - unsafe { core::mem::transmute(*bytes) } + pub fn from_bytes(bytes: &[u8; Self::SIZE]) -> &Self { + let alignment = core::mem::align_of::(); + assert_eq!( + bytes.as_ptr().align_offset(alignment), + 0, + "{} is not aligned", + core::any::type_name::() + ); + unsafe { core::mem::transmute(bytes) } + } + + #[allow(unused)] + pub fn from_bytes_mut(bytes: &mut [u8; Self::SIZE]) -> &mut Self { + let alignment = core::mem::align_of::(); + assert_eq!( + bytes.as_ptr().align_offset(alignment), + 0, + "{} is not aligned", + core::any::type_name::() + ); + + unsafe { core::mem::transmute(bytes) } } } }; @@ -67,9 +88,35 @@ pub struct SdpcmHeader { } impl_bytes!(SdpcmHeader); +impl SdpcmHeader { + pub fn parse(packet: &mut [u8]) -> Option<(&mut Self, &mut [u8])> { + let packet_len = packet.len(); + if packet_len < Self::SIZE { + warn!("packet too short, len={}", packet.len()); + return None; + } + let (sdpcm_header, sdpcm_packet) = packet.split_at_mut(Self::SIZE); + let sdpcm_header = Self::from_bytes_mut(sdpcm_header.try_into().unwrap()); + trace!("rx {:?}", sdpcm_header); + + if sdpcm_header.len != !sdpcm_header.len_inv { + warn!("len inv mismatch"); + return None; + } + + if sdpcm_header.len as usize != packet_len { + warn!("len from header doesn't match len from spi"); + return None; + } + + let sdpcm_packet = &mut sdpcm_packet[(sdpcm_header.header_length as usize - Self::SIZE)..]; + Some((sdpcm_header, sdpcm_packet)) + } +} + #[derive(Debug, Clone, Copy)] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -#[repr(C)] +// #[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C, packed(2))] pub struct CdcHeader { pub cmd: u32, pub len: u32, @@ -79,6 +126,21 @@ pub struct CdcHeader { } impl_bytes!(CdcHeader); +impl CdcHeader { + pub fn parse(packet: &mut [u8]) -> Option<(&mut Self, &mut [u8])> { + if packet.len() < Self::SIZE { + warn!("payload too short, len={}", packet.len()); + return None; + } + + let (cdc_header, payload) = packet.split_at_mut(Self::SIZE); + let cdc_header = Self::from_bytes_mut(cdc_header.try_into().unwrap()); + + let payload = &mut payload[..cdc_header.len as usize]; + Some((cdc_header, payload)) + } +} + pub const BDC_VERSION: u8 = 2; pub const BDC_VERSION_SHIFT: u8 = 4; @@ -95,6 +157,25 @@ pub struct BcdHeader { } impl_bytes!(BcdHeader); +impl BcdHeader { + pub fn parse(packet: &mut [u8]) -> Option<(&mut Self, &mut [u8])> { + if packet.len() < Self::SIZE { + return None; + } + + let (bcd_header, bcd_packet) = packet.split_at_mut(Self::SIZE); + let bcd_header = Self::from_bytes_mut(bcd_header.try_into().unwrap()); + trace!(" {:?}", bcd_header); + + let packet_start = 4 * bcd_header.data_offset as usize; + + let bcd_packet = bcd_packet.get_mut(packet_start..)?; + trace!(" {:02x}", Bytes(&bcd_packet[..bcd_packet.len().min(36)])); + + Some((bcd_header, bcd_packet)) + } +} + #[derive(Clone, Copy)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] #[repr(C)] @@ -130,8 +211,8 @@ impl EventHeader { } #[derive(Debug, Clone, Copy)] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -#[repr(C)] +// #[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C, packed(2))] pub struct EventMessage { /// version pub version: u16, @@ -158,6 +239,45 @@ pub struct EventMessage { } impl_bytes!(EventMessage); +#[cfg(feature = "defmt")] +impl defmt::Format for EventMessage { + fn format(&self, fmt: defmt::Formatter) { + let event_type = self.event_type; + let status = self.status; + let reason = self.reason; + let auth_type = self.auth_type; + let datalen = self.datalen; + + defmt::write!( + fmt, + "EventMessage {{ \ + version: {=u16}, \ + flags: {=u16}, \ + event_type: {=u32}, \ + status: {=u32}, \ + reason: {=u32}, \ + auth_type: {=u32}, \ + datalen: {=u32}, \ + addr: {=[u8; 6]:x}, \ + ifname: {=[u8; 16]:x}, \ + ifidx: {=u8}, \ + bsscfgidx: {=u8}, \ + }} ", + self.version, + self.flags, + event_type, + status, + reason, + auth_type, + datalen, + self.addr, + self.ifname, + self.ifidx, + self.bsscfgidx + ); + } +} + impl EventMessage { pub fn byteswap(&mut self) { self.version = self.version.to_be(); @@ -172,7 +292,7 @@ impl EventMessage { #[derive(Clone, Copy)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -#[repr(C)] +#[repr(C, packed(2))] pub struct EventPacket { pub eth: EthernetHeader, pub hdr: EventHeader, @@ -181,6 +301,21 @@ pub struct EventPacket { impl_bytes!(EventPacket); impl EventPacket { + pub fn parse(packet: &mut [u8]) -> Option<(&mut Self, &mut [u8])> { + if packet.len() < Self::SIZE { + return None; + } + + let (event_header, event_packet) = packet.split_at_mut(Self::SIZE); + let event_header = Self::from_bytes_mut(event_header.try_into().unwrap()); + // warn!("event_header {:x}", event_header as *const _); + event_header.byteswap(); + + let event_packet = event_packet.get_mut(..event_header.msg.datalen as usize)?; + + Some((event_header, event_packet)) + } + pub fn byteswap(&mut self) { self.eth.byteswap(); self.hdr.byteswap();