From e497834d1d20dfacc60bfbf050facbcb895b25e3 Mon Sep 17 00:00:00 2001 From: morgan Date: Thu, 12 Sep 2024 12:20:21 +0800 Subject: [PATCH] cxp protocol: init testing: add packet printing helper function proto FW: use memory buffer for tx and rx proto FW: use byteoder crate to handle endianness proto FW: add event packet reader and writer proto FW: add error correction for 4x char proto FW: add pending packet --- src/libboard_artiq/src/cxp_proto.rs | 423 ++++++++++++++++++++++++++++ 1 file changed, 423 insertions(+) create mode 100644 src/libboard_artiq/src/cxp_proto.rs diff --git a/src/libboard_artiq/src/cxp_proto.rs b/src/libboard_artiq/src/cxp_proto.rs new file mode 100644 index 0000000..bc085f6 --- /dev/null +++ b/src/libboard_artiq/src/cxp_proto.rs @@ -0,0 +1,423 @@ +use core::fmt; + +use byteorder::{ByteOrder, NetworkEndian}; +use core_io::{Error as IoError, Read, Write}; +use crc::crc32::checksum_ieee; +use io::Cursor; +use libboard_zynq::println; + +const EV_MAXSIZE: usize = 253; +const CTRL_PACKET_MAXSIZE: usize = 128; // for compatibility with version1.x compliant Devices - Section 12.1.6 (CXP-001-2021) +pub const DATA_MAXSIZE: usize = + CTRL_PACKET_MAXSIZE - /*packet start KCodes, data packet types, CMD, Tag, Addr, CRC, packet end KCode*/4*7; + +#[derive(Debug)] +pub enum Error { + CorruptedPacket, + CtrlAckError(u8), + Io(IoError), + LengthOutOfRange, + TagMismatch, + TimedOut, + UnexpectedReply, + UnknownPacket(u8), +} + +impl fmt::Display for Error { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + match self { + &Error::CorruptedPacket => write!(f, "CorruptedPacket Received packet fail CRC test"), + &Error::CtrlAckError(ref ack_code) => match ack_code { + 0x40 => write!(f, "CtrlAckError Invalid Address"), + 0x41 => write!(f, "CtrlAckError Invalid data for the address"), + 0x42 => write!(f, "CtrlAckError Invalid operation code"), + 0x43 => write!(f, "CtrlAckError Write attempted to a read-only address"), + 0x44 => write!(f, "CtrlAckError Read attempted from a write-only address"), + 0x45 => write!(f, "CtrlAckError Size field too large, exceed packet size limit"), + 0x46 => write!(f, "CtrlAckError Message size is inconsistent with size field"), + 0x47 => write!(f, "CtrlAckError Malformed packet"), + 0x80 => write!(f, "CtrlAckError Failed CRC test in last received command"), + _ => write!(f, "CtrlAckError Unknown ack code {:#X}", ack_code), + }, + &Error::Io(ref err) => write!(f, "IoError {:?}", err), + &Error::LengthOutOfRange => write!(f, "LengthOutOfRange Message Length is too long"), + &Error::TagMismatch => write!(f, "TagMismatch Received tag is different from the transmitted tag"), + &Error::TimedOut => write!(f, "MessageTimedOut"), + &Error::UnexpectedReply => write!(f, "UnexpectedReply"), + &Error::UnknownPacket(packet_type) => write!(f, "UnknownPacket with type id {:#X} ", packet_type), + } + } +} + +impl From for Error { + fn from(value: IoError) -> Error { + Error::Io(value) + } +} + +fn get_cxp_crc(bytes: &[u8]) -> u32 { + // Section 9.2.2.2 (CXP-001-2021) + // Only Control packet need CRC32 appended in the end of the packet + // CoaXpress use the polynomial of IEEE-802.3 (Ethernet) CRC but the checksum calculation is different + (!checksum_ieee(bytes)).swap_bytes() +} + +trait CxpRead { + fn read_u8(&mut self) -> Result; + + fn read_u16(&mut self) -> Result; + + fn read_u32(&mut self) -> Result; + + fn read_u64(&mut self) -> Result; + + 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; +} +impl CxpRead for Cursor { + fn read_u8(&mut self) -> Result { + let mut bytes = [0; 1]; + self.read_exact(&mut bytes)?; + Ok(bytes[0]) + } + + 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)) + } + + fn read_exact_4x(&mut self, buf: &mut [u8]) -> Result<(), Error> { + for byte in buf { + // Section 9.2.2.1 (CXP-001-2021) + // decoder should immune to single bit errors when handling 4x duplicated characters + let a = self.read_u8()?; + let b = self.read_u8()?; + let c = self.read_u8()?; + let d = self.read_u8()?; + // vote and return majority + *byte = a & b & c | a & b & d | a & c & d | b & c & d; + } + Ok(()) + } + + fn read_4x_u8(&mut self) -> Result { + 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)) + } +} + +#[derive(Debug)] +pub enum NameSpace { + GenICam, + DeviceSpecific, +} + +#[derive(Debug)] +pub enum DownConnPacket { + CtrlReply { + tag: Option, + length: u32, + data: [u8; DATA_MAXSIZE], + }, + CtrlDelay { + tag: Option, + time: u32, + }, + 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 { + pub fn read_from(reader: &mut Cursor<&mut [u8]>, packet_type: u8) -> Result { + 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)), + } + } + + fn get_ctrl_packet(reader: &mut Cursor<&mut [u8]>, with_tag: bool) -> Result { + let mut tag: Option = None; + if with_tag { + tag = Some(reader.read_4x_u8()?); + } + + let ackcode = reader.read_4x_u8()?; + + match ackcode { + 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()]); + if reader.read_u32()? != checksum { + return Err(Error::CorruptedPacket); + } + + if ackcode == 0x00 { + return Ok(DownConnPacket::CtrlReply { tag, length, data }); + } else { + return Ok(DownConnPacket::CtrlDelay { + tag, + time: NetworkEndian::read_u32(&data[..4]), + }); + } + } + 0x01 => return Ok(DownConnPacket::CtrlAck { tag }), + _ => 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, + }) + } +} + +trait CxpWrite { + fn write_all_4x(&mut self, buf: &[u8]) -> Result<(), Error>; + + fn write_4x_u8(&mut self, value: u8) -> Result<(), Error>; + + fn write_4x_u16(&mut self, value: u16) -> Result<(), Error>; + + fn write_4x_u32(&mut self, value: u32) -> Result<(), Error>; + + fn write_u32(&mut self, value: u32) -> Result<(), Error>; +} +impl CxpWrite for Cursor { + fn write_all_4x(&mut self, buf: &[u8]) -> Result<(), Error> { + for byte in buf { + self.write_all(&[*byte; 4])?; + } + Ok(()) + } + + fn write_4x_u8(&mut self, value: u8) -> Result<(), Error> { + self.write_all_4x(&[value]) + } + + fn write_4x_u16(&mut self, value: u16) -> Result<(), Error> { + let mut bytes = [0; 2]; + NetworkEndian::write_u16(&mut bytes, value); + self.write_all_4x(&bytes) + } + + fn write_4x_u32(&mut self, value: u32) -> Result<(), Error> { + let mut bytes = [0; 4]; + NetworkEndian::write_u32(&mut bytes, value); + self.write_all_4x(&bytes) + } + + fn write_u32(&mut self, value: u32) -> Result<(), Error> { + let mut bytes = [0; 4]; + NetworkEndian::write_u32(&mut bytes, value); + self.write_all(&bytes)?; + Ok(()) + } +} + +#[derive(Debug)] +pub enum UpConnPacket { + CtrlRead { + tag: Option, + addr: u32, + length: u32, + }, + CtrlWrite { + tag: Option, + addr: u32, + length: u32, + data: [u8; DATA_MAXSIZE], + }, + EventAck { + packet_tag: u8, + }, +} + +impl UpConnPacket { + pub fn write_to(&self, writer: &mut Cursor<&mut [u8]>) -> Result<(), Error> { + match *self { + UpConnPacket::CtrlRead { tag, addr, length } => { + match tag { + Some(t) => { + writer.write_4x_u8(0x05)?; + writer.write_4x_u8(t)?; + } + None => { + writer.write_4x_u8(0x02)?; + } + } + + let mut bytes = [0; 3]; + NetworkEndian::write_u24(&mut bytes, length); + writer.write_all(&[0x00, bytes[0], bytes[1], bytes[2]])?; + + writer.write_u32(addr)?; + + // Section 9.6.2 (CXP-001-2021) + // only bytes after the first 4 are used in calculating the checksum + let checksum = get_cxp_crc(&writer.get_ref()[4..writer.position()]); + writer.write_u32(checksum)?; + } + UpConnPacket::CtrlWrite { + tag, + addr, + length, + data, + } => { + match tag { + Some(t) => { + writer.write_4x_u8(0x05)?; + writer.write_4x_u8(t)?; + } + None => { + writer.write_4x_u8(0x02)?; + } + } + + let mut bytes = [0; 3]; + NetworkEndian::write_u24(&mut bytes, length); + writer.write_all(&[0x01, bytes[0], bytes[1], bytes[2]])?; + + writer.write_u32(addr)?; + writer.write_all(&data[0..length as usize])?; + + // Section 9.6.2 (CXP-001-2021) + // only bytes after the first 4 are used in calculating the checksum + let checksum = get_cxp_crc(&writer.get_ref()[4..writer.position()]); + writer.write_u32(checksum)?; + } + UpConnPacket::EventAck { packet_tag } => { + writer.write_4x_u8(0x08)?; + writer.write_4x_u8(packet_tag)?; + } + } + Ok(()) + } +} + +// DEBUG: use only +// +// +// +pub fn print_packet(pak: &[u8]) { + println!("pak = ["); + for i in 0..(pak.len() / 4) { + println!( + "{:#03} {:#04X} {:#04X} {:#04X} {:#04X},", + i + 1, + pak[i * 4], + pak[i * 4 + 1], + pak[i * 4 + 2], + pak[i * 4 + 3] + ) + } + println!("]"); + println!("============================================"); +} + +pub fn print_packetu32(pak: &[u32], k: &[u8]) { + println!("pak = ["); + for i in 0..(pak.len()) { + let data: [u8; 4] = pak[i].to_le_bytes(); + println!( + "{:#03} {:#04X} {:#04X} {:#04X} {:#04X} | K {:04b},", + i + 1, + data[0], + data[1], + data[2], + data[3], + k[i], + ) + } + println!("]"); + println!("============================================"); +}