From b7278af56e1854e6287023989278763a5427c8a1 Mon Sep 17 00:00:00 2001 From: morgan Date: Wed, 9 Oct 2024 10:08:11 +0800 Subject: [PATCH] proto fw: add event packet reader --- src/libboard_artiq/src/cxp_proto.rs | 157 +++++++++++++++++++++++----- 1 file changed, 133 insertions(+), 24 deletions(-) diff --git a/src/libboard_artiq/src/cxp_proto.rs b/src/libboard_artiq/src/cxp_proto.rs index e31d9d5..5ac366d 100644 --- a/src/libboard_artiq/src/cxp_proto.rs +++ b/src/libboard_artiq/src/cxp_proto.rs @@ -11,6 +11,7 @@ use crate::{mem::mem::{CXP_LOOPBACK_MEM, CXP_RX_MEM, CXP_TX_MEM}, const BUF_LEN: usize = 0x800; const DATA_MAXSIZE: usize = 48; +const EV_MAXSIZE: usize = 253; #[derive(Debug)] pub enum Error { @@ -35,23 +36,80 @@ fn get_cxp_crc(bytes: &[u8]) -> u32 { } trait CxpRead { + fn read_exact_4x(&mut self, buf: &mut [u8]) -> Result<(), Error>; + fn read_4x_u8(&mut self) -> Result; + fn read_4x_u16(&mut self) -> Result; + + fn read_4x_u32(&mut self) -> Result; + + fn read_4x_u64(&mut self) -> Result; + + fn read_u16(&mut self) -> Result; + fn read_u32(&mut self) -> Result; + + fn read_u64(&mut self) -> Result; } impl CxpRead for Cursor { + fn read_exact_4x(&mut self, buf: &mut [u8]) -> Result<(), Error> { + for b in buf { + // TODO: add error correction + let mut bytes = [0; 4]; + self.read_exact(&mut bytes)?; + *b = bytes[0]; + } + Ok(()) + } + fn read_4x_u8(&mut self) -> Result { - // TODO: add error correction - let mut bytes = [0; 4]; - self.read_exact(&mut bytes)?; + let mut bytes = [0; 1]; + self.read_exact_4x(&mut bytes)?; Ok(bytes[0]) } + fn read_4x_u16(&mut self) -> Result { + let mut bytes = [0; 2]; + self.read_exact_4x(&mut bytes)?; + Ok(NetworkEndian::read_u16(&bytes)) + } + + fn read_4x_u32(&mut self) -> Result { + let mut bytes = [0; 4]; + self.read_exact_4x(&mut bytes)?; + Ok(NetworkEndian::read_u32(&bytes)) + } + + fn read_4x_u64(&mut self) -> Result { + let mut bytes = [0; 6]; + self.read_exact_4x(&mut bytes)?; + Ok(NetworkEndian::read_u64(&bytes)) + } + + fn read_u16(&mut self) -> Result { + let mut bytes = [0; 2]; + self.read_exact(&mut bytes)?; + Ok(NetworkEndian::read_u16(&bytes)) + } + fn read_u32(&mut self) -> Result { let mut bytes = [0; 4]; self.read_exact(&mut bytes)?; Ok(NetworkEndian::read_u32(&bytes)) } + + fn read_u64(&mut self) -> Result { + let mut bytes = [0; 8]; + self.read_exact(&mut bytes)?; + Ok(NetworkEndian::read_u64(&bytes)) + } +} + +#[derive(Debug)] +pub enum NameSpace { + GenICam, + DeviceSpecific, } #[derive(Debug)] @@ -69,6 +127,16 @@ pub enum DownConnPacket { CtrlAck { tag: Option, }, + Event { + conn_id: u32, + packet_tag: u8, + length: u16, + ev_size: u16, + namespace: NameSpace, + event_id: u16, + timestamp: u64, + ev: [u8; EV_MAXSIZE], + }, } impl DownConnPacket { @@ -76,6 +144,7 @@ impl DownConnPacket { match packet_type { 0x03 => DownConnPacket::get_ctrl_packet(reader, false), 0x06 => DownConnPacket::get_ctrl_packet(reader, true), + 0x07 => DownConnPacket::get_event_packet(reader), _ => Err(Error::UnknownPacket(packet_type)), } } @@ -89,38 +158,78 @@ impl DownConnPacket { let ackcode = reader.read_4x_u8()?; match ackcode { - 0x00 => { + 0x00 | 0x04 => { let length = reader.read_u32()?; let mut data: [u8; DATA_MAXSIZE] = [0; DATA_MAXSIZE]; reader.read(&mut data[0..length as usize])?; let checksum = get_cxp_crc(&reader.get_ref()[0..reader.position()]); - let recv_checksum = reader.read_u32()?; - println!("calculated checksum = {:#010X}", checksum); - println!("read checksum = {:#010X}", recv_checksum); - if recv_checksum != checksum { + if reader.read_u32()? != checksum { return Err(Error::CorruptedPacket); } - return Ok(DownConnPacket::CtrlReply { tag, length, data }); + + if ackcode == 0x00 { + return Ok(DownConnPacket::CtrlReply { tag, length, data }); + } else { + return Ok(DownConnPacket::CtrlDelay { + tag, + length, + time: data, + }); + } } 0x01 => return Ok(DownConnPacket::CtrlAck { tag }), - 0x04 => { - let length = reader.read_u32()?; - let mut time: [u8; DATA_MAXSIZE] = [0; DATA_MAXSIZE]; - reader.read(&mut time[0..length as usize])?; - - let checksum = get_cxp_crc(&reader.get_ref()[0..reader.position()]); - let recv_checksum = reader.read_u32()?; - println!("calculated checksum = {:#010X}", checksum); - println!("read checksum = {:#010X}", recv_checksum); - if recv_checksum != checksum { - return Err(Error::CorruptedPacket); - } - return Ok(DownConnPacket::CtrlDelay { tag, length, time }); - } _ => return Err(Error::CtrlAckError(ackcode)), } } + + fn get_event_packet(reader: &mut Cursor<&mut [u8]>) -> Result { + let conn_id = reader.read_4x_u32()?; + let packet_tag = reader.read_4x_u8()?; + let length = reader.read_4x_u16()?; + + let ev_size = reader.read_u16()?; + if ev_size + 3 != length { + println!("length mismatch"); + return Err(Error::CorruptedPacket); + } + + let mut bytes = [0; 2]; + reader.read_exact(&mut bytes)?; + let namespace_bits = (bytes[0] & 0xC0) >> 6; + let namespace = match namespace_bits { + 0 => NameSpace::GenICam, + 2 => NameSpace::DeviceSpecific, + _ => { + println!("namespace = {} error", namespace_bits); + return Err(Error::CorruptedPacket); + } + }; + + let event_id = (bytes[0] & 0xF) as u16 | (bytes[1] as u16); + + let timestamp = reader.read_u64()?; + + let mut ev: [u8; EV_MAXSIZE] = [0; EV_MAXSIZE]; + reader.read(&mut ev[0..ev_size as usize])?; + + let checksum = get_cxp_crc(&reader.get_ref()[0..reader.position()]); + if reader.read_u32()? != checksum { + println!("crc error"); + return Err(Error::CorruptedPacket); + } + + Ok(DownConnPacket::Event { + conn_id, + packet_tag, + length, + ev_size, + namespace, + event_id, + timestamp, + ev, + }) + } } pub fn receive(channel: usize) -> Result, Error> { @@ -134,7 +243,7 @@ pub fn receive(channel: usize) -> Result, Error> { let packet_type = (CXP[channel].downconn_packet_type_read)(); let packet = DownConnPacket::read_from(&mut reader, packet_type); - println!("{:?}", packet); + println!("{:X?}", packet); (CXP[channel].downconn_pending_packet_write)(1); Ok(Some(packet?))