morgan
/
artiq-zynq
forked from M-Labs/artiq-zynq
1
0
Fork 0
Code Pull Requests Activity

proto fw: add event packet reader

This commit is contained in:
morgan 2024-10-09 10:08:11 +08:00
parent c6413d44e5
commit b7278af56e
1 changed files with 133 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
src/libboard_artiq/src/cxp_proto.rs
View File

@ -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<u8, Error>;
fn read_4x_u16(&mut self) -> Result<u16, Error>;
fn read_4x_u32(&mut self) -> Result<u32, Error>;
fn read_4x_u64(&mut self) -> Result<u64, Error>;
fn read_u16(&mut self) -> Result<u16, Error>;
fn read_u32(&mut self) -> Result<u32, Error>;
fn read_u64(&mut self) -> Result<u64, Error>;
}
impl<Cursor: Read> 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<u8, Error> {
// 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<u16, Error> {
let mut bytes = [0; 2];
self.read_exact_4x(&mut bytes)?;
Ok(NetworkEndian::read_u16(&bytes))
}
fn read_4x_u32(&mut self) -> Result<u32, Error> {
let mut bytes = [0; 4];
self.read_exact_4x(&mut bytes)?;
Ok(NetworkEndian::read_u32(&bytes))
}
fn read_4x_u64(&mut self) -> Result<u64, Error> {
let mut bytes = [0; 6];
self.read_exact_4x(&mut bytes)?;
Ok(NetworkEndian::read_u64(&bytes))
}
fn read_u16(&mut self) -> Result<u16, Error> {
let mut bytes = [0; 2];
self.read_exact(&mut bytes)?;
Ok(NetworkEndian::read_u16(&bytes))
}
fn read_u32(&mut self) -> Result<u32, Error> {
let mut bytes = [0; 4];
self.read_exact(&mut bytes)?;
Ok(NetworkEndian::read_u32(&bytes))
}
fn read_u64(&mut self) -> Result<u64, Error> {
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<u8>,
},
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<Self, Error> {
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<Option<DownConnPacket>, Error> {
@ -134,7 +243,7 @@ pub fn receive(channel: usize) -> Result<Option<DownConnPacket>, 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?))