forked from M-Labs/artiq-zynq
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 proto FW: add pending packet
This commit is contained in:
parent
cc0278086d
commit
73710dbdc8
|
|
@ -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<IoError> 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<u8, 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>;
|
||||
|
||||
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>;
|
||||
}
|
||||
impl<Cursor: Read> CxpRead for Cursor {
|
||||
fn read_u8(&mut self) -> Result<u8, Error> {
|
||||
let mut bytes = [0; 1];
|
||||
self.read_exact(&mut bytes)?;
|
||||
Ok(bytes[0])
|
||||
}
|
||||
|
||||
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))
|
||||
}
|
||||
|
||||
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<u8, Error> {
|
||||
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))
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum NameSpace {
|
||||
GenICam,
|
||||
DeviceSpecific,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum DownConnPacket {
|
||||
CtrlReply {
|
||||
tag: Option<u8>,
|
||||
length: u32,
|
||||
data: [u8; DATA_MAXSIZE],
|
||||
},
|
||||
CtrlDelay {
|
||||
tag: Option<u8>,
|
||||
time: u16, // max value of time is 10000
|
||||
},
|
||||
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 {
|
||||
pub fn read_from(reader: &mut Cursor<&mut [u8]>, packet_type: u8) -> Result<Self, Error> {
|
||||
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<Self, Error> {
|
||||
let mut tag: Option<u8> = 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<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,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
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<Cursor: Write> 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<u8>,
|
||||
addr: u32,
|
||||
length: u8,
|
||||
},
|
||||
CtrlWrite {
|
||||
tag: Option<u8>,
|
||||
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!("============================================");
|
||||
}
|
||||
Loading…
Reference in New Issue