parse data from device in-place

2 files changed, 154 insertions, 62 deletions

diff --git a/src/runner.rs b/src/runner.rs
index deb45f9f4..f0f6fceeb 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 e16808f30..6d5d31b06 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::<Self>();
+                assert_eq!(
+                    bytes.as_ptr().align_offset(alignment),
+                    0,
+                    "{} is not aligned",
+                    core::any::type_name::<Self>()
+                );
+                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::<Self>();
+                assert_eq!(
+                    bytes.as_ptr().align_offset(alignment),
+                    0,
+                    "{} is not aligned",
+                    core::any::type_name::<Self>()
+                );
+
+                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();