From b055abe4ebb573f62b472adcd70fa190963e8b44 Mon Sep 17 00:00:00 2001 From: morgan Date: Fri, 4 Oct 2024 15:28:37 +0800 Subject: [PATCH] proto fw: add helper trait for cursor --- src/libboard_artiq/src/cxp_proto.rs | 153 +++++++++++++++------------- 1 file changed, 85 insertions(+), 68 deletions(-) diff --git a/src/libboard_artiq/src/cxp_proto.rs b/src/libboard_artiq/src/cxp_proto.rs index 10f9773..7560d1c 100644 --- a/src/libboard_artiq/src/cxp_proto.rs +++ b/src/libboard_artiq/src/cxp_proto.rs @@ -65,22 +65,44 @@ fn get_cxp_crc(bytes: &[u8]) -> u32 { (!checksum_ieee(bytes)).swap_bytes() } +trait CxpRead { + fn read_4x_u8(&mut self) -> Result; + + fn read_u32(&mut self) -> Result; +} +impl CxpRead for Cursor { + #[inline] + fn read_4x_u8(&mut self) -> Result { + // TODO: add error correction + let mut bytes = [0; 4]; + self.read_exact(&mut bytes)?; + Ok(bytes[0]) + } + + #[inline] + fn read_u32(&mut self) -> Result { + let mut bytes = [0; 4]; + self.read_exact(&mut bytes)?; + Ok(u32::from_be_bytes(bytes)) + } +} + fn capture_ctrl_packet(reader: &mut Cursor<&mut [u8]>, with_tag: bool) -> Result { let mut tag: Option = None; if with_tag { - tag = Some((read_word(reader)? & 0xFF) as u8); + tag = Some(reader.read_4x_u8()?); } - let ackcode: u8 = (read_word(reader)? & 0xFF) as u8; + let ackcode = reader.read_4x_u8()?; match ackcode { 0x00 => { - let length = read_word(reader)?; + 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 = read_word(reader)?; + let recv_checksum = reader.read_u32()?; println!("calculated checksum = {:#010X}", checksum); println!("read checksum = {:#010X}", recv_checksum); if recv_checksum != checksum { @@ -90,12 +112,12 @@ fn capture_ctrl_packet(reader: &mut Cursor<&mut [u8]>, with_tag: bool) -> Result } 0x01 => return Ok(DownConnPacket::CtrlAck { tag }), 0x04 => { - let length = read_word(reader)?; + 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 = read_word(reader)?; + let recv_checksum = reader.read_u32()?; println!("calculated checksum = {:#010X}", checksum); println!("read checksum = {:#010X}", recv_checksum); if recv_checksum != checksum { @@ -107,6 +129,22 @@ fn capture_ctrl_packet(reader: &mut Cursor<&mut [u8]>, with_tag: bool) -> Result } } +pub fn receive(channel: usize) -> Result<(), Error> { + unsafe { + let ptr = CXP_RX_MEM[channel].base as *mut u32; + let mut reader = Cursor::new(slice::from_raw_parts_mut(ptr as *mut u8, MEM_LEN)); + let packet_type = (CXP[channel].downconn_packet_type_read)(); + + let packet = match packet_type { + 0x03 => capture_ctrl_packet(&mut reader, false), + 0x06 => capture_ctrl_packet(&mut reader, true), + _ => return Err(Error::UnknownPacket(packet_type)), + }; + println!("{:?}", packet); + } + Ok(()) +} + #[derive(Debug)] pub enum UpConnPacket { CtrlRead { @@ -133,12 +171,6 @@ pub enum UpConnPacket { }, } -fn read_word(reader: &mut Cursor<&mut [u8]>) -> Result { - let mut bytes = [0; 4]; - reader.read(&mut bytes)?; - Ok(u32::from_be_bytes(bytes)) -} - impl UpConnPacket { pub fn write_to(&self, writer: &mut Cursor<&mut [u8]>) -> Result<(), Error> { // CoaXpress use big endian @@ -199,7 +231,8 @@ impl UpConnPacket { writer.write(&data[0..length as usize])?; } - writer.write(&get_cxp_crc(&writer.get_ref()[4..writer.position()]).to_be_bytes())?; + let checksum = get_cxp_crc(&writer.get_ref()[4..writer.position()]); + writer.write(&checksum.to_be_bytes())?; } _ => {} } @@ -207,22 +240,6 @@ impl UpConnPacket { } } -pub fn receive(channel: usize) -> Result<(), Error> { - unsafe { - let ptr = CXP_RX_MEM[channel].base as *mut u32; - let mut reader = Cursor::new(slice::from_raw_parts_mut(ptr as *mut u8, MEM_LEN)); - let packet_type = (CXP[channel].downconn_packet_type_read)(); - - let packet = match packet_type { - 0x03 => capture_ctrl_packet(&mut reader, false), - 0x06 => capture_ctrl_packet(&mut reader, true), - _ => return Err(Error::UnknownPacket(packet_type)), - }; - println!("{:?}", packet); - } - Ok(()) -} - pub fn send(channel: usize, packet: &UpConnPacket) -> Result<(), Error> { if unsafe { (CXP[channel].upconn_tx_enable_read)() } == 0 { Err(Error::LinkDown)? @@ -268,50 +285,50 @@ fn send_test_packet(channel: usize) -> Result<(), Error> { // // // -pub fn write_u32(channel: usize, addr: u32, data: u32) -> Result<(), Error> { - let mut data_slice: [u8; DATA_MAXSIZE] = [0; DATA_MAXSIZE]; - data_slice[..4].clone_from_slice(&data.to_be_bytes()); - send( - channel, - &UpConnPacket::CtrlWrite { - tag: None, - addr, - length: 4, - data: data_slice, - }, - )?; +// pub fn write_u32(channel: usize, addr: u32, data: u32) -> Result<(), Error> { +// let mut data_slice: [u8; DATA_MAXSIZE] = [0; DATA_MAXSIZE]; +// data_slice[..4].clone_from_slice(&data.to_be_bytes()); +// send( +// channel, +// &UpConnPacket::CtrlWrite { +// tag: None, +// addr, +// length: 4, +// data: data_slice, +// }, +// )?; - Ok(()) -} +// Ok(()) +// } -pub fn read_u32(channel: usize, addr: u32) -> Result<(), Error> { - send( - channel, - &UpConnPacket::CtrlRead { - tag: None, - addr, - length: 4, - }, - )?; +// pub fn read_u32(channel: usize, addr: u32) -> Result<(), Error> { +// send( +// channel, +// &UpConnPacket::CtrlRead { +// tag: None, +// addr, +// length: 4, +// }, +// )?; - Ok(()) -} +// 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, - }, - )?; +// 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(()) -} +// Ok(()) +// } pub fn print_packet(pak: &[u8]) { println!("pak = [");