From 1cbf91de95aea59c4d8eaa2cb2cce2873dc96352 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 testing: add rx loopback 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 --- src/libboard_artiq/src/cxp_proto.rs | 608 ++++++++++++++++++++++++++++ 1 file changed, 608 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..5ae4df6 --- /dev/null +++ b/src/libboard_artiq/src/cxp_proto.rs @@ -0,0 +1,608 @@ +use core::slice; + +use byteorder::{ByteOrder, NetworkEndian}; +use core_io::{Error as IoError, Read, Write}; +use crc::crc32::checksum_ieee; +use io::Cursor; +use libboard_zynq::println; + +use crate::{mem::mem::{CXP_LOOPBACK_MEM, CXP_RX_MEM, CXP_TX_MEM}, + pl::{csr, csr::CXP}}; + +const BUF_LEN: usize = 0x800; +const DATA_MAXSIZE: usize = 48; +const EV_MAXSIZE: usize = 253; + +#[derive(Debug)] +pub enum Error { + BufferError, + CorruptedPacket, + CtrlAckError(u8), + LinkDown, + UnknownPacket(u8), +} + +impl From for Error { + fn from(_: IoError) -> Error { + Error::BufferError + } +} + +// 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 +fn get_cxp_crc(bytes: &[u8]) -> u32 { + (!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, + length: u32, + time: [u8; DATA_MAXSIZE], + }, + 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, + length, + time: data, + }); + } + } + 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, + }) + } +} + +pub fn receive(channel: usize) -> Result, Error> { + unsafe { + if (CXP[channel].downconn_pending_packet_read)() == 1 { + let read_buffer_ptr = (CXP[channel].downconn_read_ptr_read)() as usize; + println!("buffer ptr = {}", read_buffer_ptr); + let ptr = (CXP_RX_MEM[channel].base + read_buffer_ptr * BUF_LEN) as *mut u32; + + let mut reader = Cursor::new(slice::from_raw_parts_mut(ptr as *mut u8, BUF_LEN)); + let packet_type = (CXP[channel].downconn_packet_type_read)(); + + let packet = DownConnPacket::read_from(&mut reader, packet_type); + println!("{:X?}", packet); + + (CXP[channel].downconn_pending_packet_write)(1); + Ok(Some(packet?)) + } else { + Ok(None) + } + } +} + +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, + }, + TestPacket, + + // DEBUG: Loopback message + CtrlAckLoopback { + ackcode: u8, + length: u8, + data: [u8; DATA_MAXSIZE], + }, + Event { + conn_id: u32, + packet_tag: u8, + length: u16, + event_size: u16, + namespace: u8, + event_id: u16, + timestamp: u64, + data: [u8; 253], + }, +} + +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)?; + } + // DEBUG: Loopback message + UpConnPacket::CtrlAckLoopback { ackcode, length, data } => { + writer.write_4x_u8(0x03)?; + writer.write_4x_u8(ackcode)?; + + if ackcode == 0x00 || ackcode == 0x04 { + writer.write_all(&[0x00, 0x00, 0x00, length])?; + writer.write_all(&data[0..length as usize])?; + } + + let checksum = get_cxp_crc(&writer.get_ref()[4..writer.position()]); + writer.write_u32(checksum)?; + } + UpConnPacket::Event { + conn_id, + packet_tag, + length, + event_size, + namespace, + event_id, + timestamp, + data, + } => { + // event packet header + writer.write_4x_u8(0x07)?; + writer.write_4x_u32(conn_id)?; + writer.write_4x_u8(packet_tag)?; + writer.write_4x_u16(length)?; + + // event message + let ev_size = event_size.to_be_bytes(); + let p2: u8 = ((namespace & 0b11) << 6) | ((event_id & 0xF00) >> 8) as u8; + let p3: u8 = (event_id & 0xFF) as u8; + writer.write_all(&[ev_size[0], ev_size[1], p2, p3])?; + writer.write_all(×tamp.to_be_bytes())?; + writer.write_all(&data[0..event_size as usize])?; + + let checksum = get_cxp_crc(&writer.get_ref()[4..writer.position()]); + writer.write_u32(checksum)?; + } + _ => {} + } + Ok(()) + } +} + +pub fn send(channel: usize, packet: &UpConnPacket) -> Result<(), Error> { + if unsafe { csr::cxp_phys::upconn_tx_enable_read() } == 0 { + Err(Error::LinkDown)? + } + + match *packet { + UpConnPacket::TestPacket => send_test_packet(channel), + _ => send_data_packet(channel, packet), + } +} + +fn send_data_packet(channel: usize, packet: &UpConnPacket) -> Result<(), Error> { + unsafe { + while (CXP[channel].upconn_bootstrap_tx_busy_read)() == 1 {} + let ptr = CXP_TX_MEM[0].base as *mut u32; + let mut writer = Cursor::new(slice::from_raw_parts_mut(ptr as *mut u8, BUF_LEN)); + + packet.write_to(&mut writer)?; + println!("TX MEM after writing"); + print_packet(&writer.get_ref()[0..40]); + + (CXP[channel].upconn_bootstrap_tx_word_len_write)(writer.position() as u16 / 4); + (CXP[channel].upconn_bootstrap_tx_write)(1); + } + + Ok(()) +} + +fn send_test_packet(channel: usize) -> Result<(), Error> { + unsafe { + while (CXP[channel].upconn_bootstrap_tx_busy_read)() == 1 {} + (CXP[channel].upconn_bootstrap_tx_testseq_write)(1); + } + Ok(()) +} + +pub fn write_u32(channel: u8, addr: u32, val: u32) -> Result<(), Error> { + // TODO: add tags after connection & verify it's CXPv2 + + let mut data: [u8; DATA_MAXSIZE] = [0; DATA_MAXSIZE]; + NetworkEndian::write_u32(&mut data[..4], val); + send( + channel as usize, + &UpConnPacket::CtrlWrite { + tag: None, + addr, + length: 4, + data, + }, + )?; + + Ok(()) +} + +pub fn read_u32(channel: u8, addr: u32) -> Result<(), Error> { + // TODO: add tags after connection & verify it's CXPv2 + + send( + channel as usize, + &UpConnPacket::CtrlRead { + tag: None, + addr, + length: 4, + }, + )?; + + Ok(()) +} + +// pub fn write_u64(channel: usize, addr: u32, data: u64) -> Result<(), Error> { +// let mut data_slice: [u8; DATA_MAXSIZE] = [0; DATA_MAXSIZE]; +// data_slice[..8].clone_from_slice(&data.to_be_bytes()); +// send( +// channel, +// &UpConnPacket::CtrlWrite { +// tag: None, +// addr, +// length: 8, +// data: data_slice, +// }, +// )?; + +// 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!("============================================"); +} + +pub fn downconn_debug_send(channel: usize, packet: &UpConnPacket) -> Result<(), Error> { + unsafe { + while (CXP[channel].downconn_bootstrap_loopback_tx_busy_read)() == 1 {} + let ptr = CXP_LOOPBACK_MEM[0].base as *mut u32; + let mut writer = Cursor::new(slice::from_raw_parts_mut(ptr as *mut u8, BUF_LEN)); + + packet.write_to(&mut writer)?; + + (CXP[channel].downconn_bootstrap_loopback_tx_word_len_write)(writer.position() as u16 / 4); + (CXP[channel].downconn_bootstrap_loopback_tx_write)(1); + } + + Ok(()) +} + +pub fn downconn_debug_mem_print(channel: usize) { + unsafe { + let ptr = CXP_RX_MEM[channel].base as *mut u32; + let arr = slice::from_raw_parts_mut(ptr as *mut u8, BUF_LEN * 4); + print_packet(arr); + } +} + +pub fn downconn_debug_send_trig_ack(channel: usize) { + unsafe { + (CXP[channel].downconn_ack_write)(1); + } +} + +pub fn downconn_send_test_packet(channel: usize) { + unsafe { + while (CXP[channel].downconn_bootstrap_loopback_tx_busy_read)() == 1 {} + (CXP[channel].downconn_bootstrap_loopback_tx_testseq_write)(1); + } +}