diff --git a/src/libboard_artiq/src/cxp_proto.rs b/src/libboard_artiq/src/cxp_proto.rs new file mode 100644 index 0000000..40019ee --- /dev/null +++ b/src/libboard_artiq/src/cxp_proto.rs @@ -0,0 +1,398 @@ +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; +pub const DATA_MAXSIZE: usize = 48; + +#[derive(Debug)] +pub enum Error { + BufferError, + CorruptedPacket, + CtrlAckError(u8), + Io(IoError), + UnknownPacket(u8), +} + +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; + + fn read_4x_u64(&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)) + } + + fn read_4x_u64(&mut self) -> Result { + let mut bytes = [0; 6]; + self.read_exact_4x(&mut bytes)?; + Ok(NetworkEndian::read_u64(&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: u16, // max value of time is 10000 + }, + 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_u16(&data[..2]), + }); + } + } + 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_4x_u64(&mut self, value: u64) -> 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_4x_u64(&mut self, value: u64) -> Result<(), Error> { + let mut bytes = [0; 6]; + NetworkEndian::write_u64(&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: u8, + }, + CtrlWrite { + tag: Option, + addr: u32, + length: u8, + data: [u8; DATA_MAXSIZE], + }, // max register size is 8 bytes + 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)?; + } + } + writer.write_all(&[0x00, 0x00, 0x00, length])?; + 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)?; + } + } + writer.write_all(&[0x01, 0x00, 0x00, length])?; + 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!("============================================"); +}