cxp ctrl: Control packet handler

ctrl: add buildin delay_ms
ctrl: add proper RX Downconn ACK & REPLAY packet handling
ctrl: add send, receive & receive with timeout
ctrl: add write/read u32 u64
ctrl: add read bytes

src/libboard_artiq/src/cxp_ctrl.rs

@@ -0,0 +1,261 @@
+use core::slice;
+
+use byteorder::{ByteOrder, NetworkEndian};
+use io::Cursor;
+use libboard_zynq::{println, time::Milliseconds, timer::GlobalTimer};
+
+use crate::{cxp_proto::{print_packet, DownConnPacket, Error, UpConnPacket, DATA_MAXSIZE},
+            mem::mem::CXP_MEM,
+            pl::csr::CXP};
+
+const BUF_LEN: usize = 0x800;
+const TRANSMISSION_TIMEOUT: u64 = 200;
+
+fn packet_pending(channel: u8) -> bool {
+    unsafe { (CXP[channel as usize].downconn_pending_packet_read)() == 1 }
+}
+
+fn receive(channel: u8) -> Result<Option<DownConnPacket>, Error> {
+    if packet_pending(channel) {
+        let channel = channel as usize;
+        unsafe {
+            let read_buffer_ptr = (CXP[channel].downconn_read_ptr_read)() as usize;
+            println!("buffer ptr = {}", read_buffer_ptr);
+            let ptr = (CXP_MEM[channel].base + CXP_MEM[channel].size / 2 + 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)();
+            println!("packet_type = {}", packet_type);
+            // DEBUG:
+            // println!("RX MEM before reading");
+            // print_packet(&reader.get_ref()[0..40]);
+
+            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)
+    }
+}
+
+fn receive_timeout(channel: u8, timeout_ms: u64) -> Result<DownConnPacket, Error> {
+    // assume timer was initialized successfully
+    let timer = unsafe { GlobalTimer::get() };
+    let limit = timer.get_time() + Milliseconds(timeout_ms);
+    while timer.get_time() < limit {
+        match receive(channel)? {
+            None => (),
+            Some(packet) => return Ok(packet),
+        }
+    }
+    Err(Error::TimedOut)
+}
+
+fn send_data_packet(channel: u8, packet: &UpConnPacket) -> Result<(), Error> {
+    // assume upconn tx is enabled
+    let channel = channel as usize;
+    unsafe {
+        while (CXP[channel].upconn_bootstrap_tx_busy_read)() == 1 {}
+        let ptr = CXP_MEM[channel].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)?;
+        // DEBUG:
+        // 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(())
+}
+
+pub fn send_test_packet(channel: u8) -> Result<(), Error> {
+    // assume upconn tx is enabled
+    let channel = channel as usize;
+    unsafe {
+        while (CXP[channel].upconn_bootstrap_tx_busy_read)() == 1 {}
+        (CXP[channel].upconn_bootstrap_tx_testseq_write)(1);
+    }
+    Ok(())
+}
+
+//
+//
+// DEBUG:
+//
+//
+pub fn downconn_debug_mem_print(channel: u8) {
+    unsafe {
+        let ptr = CXP_MEM[channel as usize].base as *mut u32;
+        let arr = slice::from_raw_parts_mut(ptr as *mut u8, BUF_LEN * 4);
+        print_packet(arr);
+    }
+}
+
+pub fn print_decode_error(channel: u8) {
+    unsafe {
+        println!(
+            "CH#{} Decode error = {}",
+            channel,
+            (CXP[channel as usize].downconn_bootstrap_decoder_err_read)()
+        );
+    }
+}
+
+//
+//
+// CTRL packet
+//
+//
+
+// Section 9.6.1.2 (CXP-001-2021)
+// CTRL packet need to be tagged for CXP 2.0 or greater
+static mut TAG: u8 = 0;
+
+pub fn reset_tag() {
+    unsafe { TAG = 0 }
+}
+
+fn increment_tag() {
+    unsafe { TAG = TAG.wrapping_add(1) };
+}
+
+fn check_tag(tag: Option<u8>) -> Result<(), Error> {
+    unsafe {
+        if tag.is_some() && tag != Some(TAG) {
+            Err(Error::TagMismatch)
+        } else {
+            Ok(())
+        }
+    }
+}
+
+fn check_length(length: u32) -> Result<(), Error> {
+    if length > DATA_MAXSIZE as u32 || length == 0 {
+        Err(Error::LengthOutOfRange)
+    } else {
+        Ok(())
+    }
+}
+
+fn process_ack_packet(channel: u8, timeout: u64) -> Result<(), Error> {
+    match receive_timeout(channel, timeout) {
+        Ok(DownConnPacket::CtrlAck { tag }) => {
+            check_tag(tag)?;
+            Ok(())
+        }
+        Ok(DownConnPacket::CtrlDelay { tag, time }) => {
+            check_tag(tag)?;
+
+            // info!("delaying by {} ms ....", time);
+            process_ack_packet(channel, time as u64)
+        }
+        Ok(_) => Err(Error::UnexpectedReply),
+        Err(e) => Err(e),
+    }
+}
+
+fn process_reply_packet(channel: u8, timeout: u64, expected_length: u32) -> Result<[u8; DATA_MAXSIZE], Error> {
+    match receive_timeout(channel, timeout) {
+        Ok(DownConnPacket::CtrlReply { tag, length, data }) => {
+            check_tag(tag)?;
+
+            if length != expected_length {
+                return Err(Error::UnexpectedReply);
+            };
+
+            Ok(data)
+        }
+        Ok(DownConnPacket::CtrlDelay { tag, time }) => {
+            check_tag(tag)?;
+
+            // info!("delaying by {} ms ....", time);
+            process_reply_packet(channel, time as u64, expected_length)
+        }
+        Ok(_) => Err(Error::UnexpectedReply),
+        Err(e) => Err(e),
+    }
+}
+
+pub fn write_bytes_no_ack(channel: u8, addr: u32, val: &[u8], with_tag: bool) -> Result<(), Error> {
+    let length = val.len() as u32;
+    check_length(length)?;
+
+    let mut data: [u8; DATA_MAXSIZE] = [0; DATA_MAXSIZE];
+    data[..length as usize].clone_from_slice(val);
+
+    let tag: Option<u8> = if with_tag { Some(unsafe { TAG }) } else { None };
+    send_data_packet(
+        channel,
+        &UpConnPacket::CtrlWrite {
+            tag,
+            addr,
+            length,
+            data,
+        },
+    )
+}
+
+pub fn write_bytes(channel: u8, addr: u32, val: &[u8], with_tag: bool) -> Result<(), Error> {
+    write_bytes_no_ack(channel, addr, val, with_tag)?;
+    process_ack_packet(channel, TRANSMISSION_TIMEOUT)?;
+
+    // DEBUG
+    print_decode_error(channel);
+
+    if with_tag {
+        increment_tag();
+    };
+    Ok(())
+}
+
+pub fn write_u32(channel: u8, addr: u32, val: u32, with_tag: bool) -> Result<(), Error> {
+    write_bytes(channel, addr, &val.to_be_bytes(), with_tag)
+}
+
+pub fn write_u64(channel: u8, addr: u32, val: u64, with_tag: bool) -> Result<(), Error> {
+    write_bytes(channel, addr, &val.to_be_bytes(), with_tag)
+}
+
+fn read(channel: u8, addr: u32, length: u32, with_tag: bool) -> Result<(), Error> {
+    check_length(length)?;
+    let tag: Option<u8> = if with_tag { Some(unsafe { TAG }) } else { None };
+    send_data_packet(channel, &UpConnPacket::CtrlRead { tag, addr, length })
+}
+
+pub fn read_bytes(channel: u8, addr: u32, bytes: &mut [u8], with_tag: bool) -> Result<(), Error> {
+    let length = bytes.len() as u32;
+    read(channel, addr, length, with_tag)?;
+
+    let data = process_reply_packet(channel, TRANSMISSION_TIMEOUT, length)?;
+    bytes.clone_from_slice(&data[..length as usize]);
+
+    // DEBUG
+    print_decode_error(channel);
+
+    if with_tag {
+        increment_tag();
+    };
+    Ok(())
+}
+
+pub fn read_u32(channel: u8, addr: u32, with_tag: bool) -> Result<u32, Error> {
+    let mut bytes: [u8; 4] = [0; 4];
+    read_bytes(channel, addr, &mut bytes, with_tag)?;
+    let val = NetworkEndian::read_u32(&bytes);
+
+    Ok(val)
+}
+
+pub fn read_u64(channel: u8, addr: u32, with_tag: bool) -> Result<u64, Error> {
+    let mut bytes: [u8; 8] = [0; 8];
+    read_bytes(channel, addr, &mut bytes, with_tag)?;
+    let val = NetworkEndian::read_u64(&bytes);
+
+    Ok(val)
+}