parse data from device in-place
This commit is contained in:
parent
e6e5685f7c
commit
76ebebd0c5
@ -328,45 +328,23 @@ where
|
|||||||
let len = (status & STATUS_F2_PKT_LEN_MASK) >> STATUS_F2_PKT_LEN_SHIFT;
|
let len = (status & STATUS_F2_PKT_LEN_MASK) >> STATUS_F2_PKT_LEN_SHIFT;
|
||||||
self.bus.wlan_read(buf, len).await;
|
self.bus.wlan_read(buf, len).await;
|
||||||
trace!("rx {:02x}", Bytes(&slice8_mut(buf)[..(len as usize).min(48)]));
|
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 {
|
} else {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
fn rx(&mut self, packet: &[u8]) {
|
fn rx(&mut self, packet: &mut [u8]) {
|
||||||
if packet.len() < SdpcmHeader::SIZE {
|
let Some((sdpcm_header, payload)) = SdpcmHeader::parse(packet) else { return };
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
self.update_credit(&sdpcm_header);
|
self.update_credit(&sdpcm_header);
|
||||||
|
|
||||||
let channel = sdpcm_header.channel_and_flags & 0x0f;
|
let channel = sdpcm_header.channel_and_flags & 0x0f;
|
||||||
|
|
||||||
let payload = &packet[sdpcm_header.header_length as _..];
|
|
||||||
|
|
||||||
match channel {
|
match channel {
|
||||||
CHANNEL_TYPE_CONTROL => {
|
CHANNEL_TYPE_CONTROL => {
|
||||||
if payload.len() < CdcHeader::SIZE {
|
let Some((cdc_header, response)) = CdcHeader::parse(payload) else { return; };
|
||||||
warn!("payload too short, len={}", payload.len());
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
let cdc_header = CdcHeader::from_bytes(payload[..CdcHeader::SIZE].try_into().unwrap());
|
|
||||||
trace!(" {:?}", cdc_header);
|
trace!(" {:?}", cdc_header);
|
||||||
|
|
||||||
if cdc_header.id == self.ioctl_id {
|
if cdc_header.id == self.ioctl_id {
|
||||||
@ -375,28 +353,21 @@ where
|
|||||||
panic!("IOCTL error {}", cdc_header.status as i32);
|
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));
|
info!("IOCTL Response: {:02x}", Bytes(response));
|
||||||
|
|
||||||
self.ioctl_state.ioctl_done(response);
|
self.ioctl_state.ioctl_done(response);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
CHANNEL_TYPE_EVENT => {
|
CHANNEL_TYPE_EVENT => {
|
||||||
let bcd_header = BcdHeader::from_bytes(&payload[..BcdHeader::SIZE].try_into().unwrap());
|
let Some((_, bcd_packet)) = BcdHeader::parse(payload) else {
|
||||||
trace!(" {:?}", bcd_header);
|
|
||||||
|
|
||||||
let packet_start = BcdHeader::SIZE + 4 * bcd_header.data_offset as usize;
|
|
||||||
|
|
||||||
if packet_start + EventPacket::SIZE > payload.len() {
|
|
||||||
warn!("BCD event, incomplete header");
|
warn!("BCD event, incomplete header");
|
||||||
return;
|
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());
|
let Some((event_packet, evt_data)) = EventPacket::parse(bcd_packet) else {
|
||||||
event_packet.byteswap();
|
warn!("BCD event, incomplete data");
|
||||||
|
return;
|
||||||
|
};
|
||||||
|
|
||||||
const ETH_P_LINK_CTL: u16 = 0x886c; // HPNA, wlan link local tunnel, according to linux if_ether.h
|
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 {
|
if event_packet.eth.ether_type != ETH_P_LINK_CTL {
|
||||||
@ -427,13 +398,7 @@ where
|
|||||||
return;
|
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_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!(
|
debug!(
|
||||||
"=== EVENT {:?}: {:?} {:02x}",
|
"=== EVENT {:?}: {:?} {:02x}",
|
||||||
evt_type,
|
evt_type,
|
||||||
@ -449,16 +414,8 @@ where
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
CHANNEL_TYPE_DATA => {
|
CHANNEL_TYPE_DATA => {
|
||||||
let bcd_header = BcdHeader::from_bytes(&payload[..BcdHeader::SIZE].try_into().unwrap());
|
let Some((_, packet)) = BcdHeader::parse(payload) else { return };
|
||||||
trace!(" {:?}", bcd_header);
|
trace!("rx pkt {:02x}", Bytes(&packet[..packet.len().min(48)]));
|
||||||
|
|
||||||
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)]));
|
|
||||||
|
|
||||||
match self.ch.try_rx_buf() {
|
match self.ch.try_rx_buf() {
|
||||||
Some(buf) => {
|
Some(buf) => {
|
||||||
|
149
src/structs.rs
149
src/structs.rs
@ -1,4 +1,5 @@
|
|||||||
use crate::events::Event;
|
use crate::events::Event;
|
||||||
|
use crate::fmt::Bytes;
|
||||||
|
|
||||||
macro_rules! impl_bytes {
|
macro_rules! impl_bytes {
|
||||||
($t:ident) => {
|
($t:ident) => {
|
||||||
@ -11,8 +12,28 @@ macro_rules! impl_bytes {
|
|||||||
}
|
}
|
||||||
|
|
||||||
#[allow(unused)]
|
#[allow(unused)]
|
||||||
pub fn from_bytes(bytes: &[u8; Self::SIZE]) -> Self {
|
pub fn from_bytes(bytes: &[u8; Self::SIZE]) -> &Self {
|
||||||
unsafe { core::mem::transmute(*bytes) }
|
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_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)]
|
#[derive(Debug, Clone, Copy)]
|
||||||
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
// #[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
||||||
#[repr(C)]
|
#[repr(C, packed(2))]
|
||||||
pub struct CdcHeader {
|
pub struct CdcHeader {
|
||||||
pub cmd: u32,
|
pub cmd: u32,
|
||||||
pub len: u32,
|
pub len: u32,
|
||||||
@ -79,6 +126,21 @@ pub struct CdcHeader {
|
|||||||
}
|
}
|
||||||
impl_bytes!(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: u8 = 2;
|
||||||
pub const BDC_VERSION_SHIFT: u8 = 4;
|
pub const BDC_VERSION_SHIFT: u8 = 4;
|
||||||
|
|
||||||
@ -95,6 +157,25 @@ pub struct BcdHeader {
|
|||||||
}
|
}
|
||||||
impl_bytes!(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)]
|
#[derive(Clone, Copy)]
|
||||||
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
||||||
#[repr(C)]
|
#[repr(C)]
|
||||||
@ -130,8 +211,8 @@ impl EventHeader {
|
|||||||
}
|
}
|
||||||
|
|
||||||
#[derive(Debug, Clone, Copy)]
|
#[derive(Debug, Clone, Copy)]
|
||||||
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
// #[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
||||||
#[repr(C)]
|
#[repr(C, packed(2))]
|
||||||
pub struct EventMessage {
|
pub struct EventMessage {
|
||||||
/// version
|
/// version
|
||||||
pub version: u16,
|
pub version: u16,
|
||||||
@ -158,6 +239,45 @@ pub struct EventMessage {
|
|||||||
}
|
}
|
||||||
impl_bytes!(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 {
|
impl EventMessage {
|
||||||
pub fn byteswap(&mut self) {
|
pub fn byteswap(&mut self) {
|
||||||
self.version = self.version.to_be();
|
self.version = self.version.to_be();
|
||||||
@ -172,7 +292,7 @@ impl EventMessage {
|
|||||||
|
|
||||||
#[derive(Clone, Copy)]
|
#[derive(Clone, Copy)]
|
||||||
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
|
||||||
#[repr(C)]
|
#[repr(C, packed(2))]
|
||||||
pub struct EventPacket {
|
pub struct EventPacket {
|
||||||
pub eth: EthernetHeader,
|
pub eth: EthernetHeader,
|
||||||
pub hdr: EventHeader,
|
pub hdr: EventHeader,
|
||||||
@ -181,6 +301,21 @@ pub struct EventPacket {
|
|||||||
impl_bytes!(EventPacket);
|
impl_bytes!(EventPacket);
|
||||||
|
|
||||||
impl 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) {
|
pub fn byteswap(&mut self) {
|
||||||
self.eth.byteswap();
|
self.eth.byteswap();
|
||||||
self.hdr.byteswap();
|
self.hdr.byteswap();
|
||||||
|
Loading…
Reference in New Issue
Block a user