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

@@ -0,0 +1,606 @@
+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::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<IoError> 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<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>,
+        length: u32,
+        time: [u8; DATA_MAXSIZE],
+    },
+    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,
+                        length,
+                        time: data,
+                    });
+                }
+            }
+            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,
+        })
+    }
+}
+
+pub fn receive(channel: usize) -> Result<Option<DownConnPacket>, 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<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,
+    },
+    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(&timestamp.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 { (CXP[channel].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)?;
+
+        (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);
+    }
+}