drtio: swap byte order, add work buffer, comm ok

This commit is contained in:
mwojcik 2021-09-30 13:32:19 +02:00
parent f23c6cdb18
commit 5f06db8787

View File

@ -1,4 +1,3 @@
use core::slice;
use crc; use crc;
use core_io::{ErrorKind as IoErrorKind, Error as IoError}; use core_io::{ErrorKind as IoErrorKind, Error as IoError};
@ -59,15 +58,35 @@ fn has_rx_error(linkno: u8) -> bool {
} }
} }
fn swap_byte_order(buf: &mut [u8]) -> () {
// for some reason, everything except checksum arrives
// with byte order swapped. and it must be sent as such too.
// call this for a slice without the checksum included.
for i in (0..buf.len()).step_by(4) {
let mut temp = buf[i];
buf[i] = buf[i + 3];
buf[i + 3] = temp;
temp = buf[i + 1];
buf[i + 1] = buf[i + 2];
buf[i + 2] = temp;
}
}
fn receive<F, T>(linkno: u8, f: F) -> Result<Option<T>, Error> fn receive<F, T>(linkno: u8, f: F) -> Result<Option<T>, Error>
where F: FnOnce(&[u8]) -> Result<T, Error> where F: FnOnce(&[u8]) -> Result<T, Error>
{ {
let linkidx = linkno as usize; let linkidx = linkno as usize;
unsafe { unsafe {
if (DRTIOAUX[linkidx].aux_rx_present_read)() == 1 { if (DRTIOAUX[linkidx].aux_rx_present_read)() == 1 {
let ptr = DRTIOAUX_MEM[linkidx].base + DRTIOAUX_MEM[linkidx].size / 2; let ptr = (DRTIOAUX_MEM[linkidx].base + DRTIOAUX_MEM[linkidx].size / 2) as *mut u8;
let len = (DRTIOAUX[linkidx].aux_rx_length_read)(); let len = (DRTIOAUX[linkidx].aux_rx_length_read)() as usize;
let result = f(slice::from_raw_parts(ptr as *mut u8, len as usize)); // work buffer, as byte order will need to be swapped, cannot be in place
let mut buf: [u8; 1024] = [0; 1024];
for i in 0..len {
buf[i] = *(ptr as *mut u8).offset(i as isize);
}
swap_byte_order(&mut buf[0..len-4]);
let result = f(&buf[0..len]);
(DRTIOAUX[linkidx].aux_rx_present_write)(1); (DRTIOAUX[linkidx].aux_rx_present_write)(1);
Ok(Some(result?)) Ok(Some(result?))
} else { } else {
@ -120,9 +139,14 @@ fn transmit<F>(linkno: u8, f: F) -> Result<(), Error>
let linkno = linkno as usize; let linkno = linkno as usize;
unsafe { unsafe {
while (DRTIOAUX[linkno].aux_tx_read)() != 0 {} while (DRTIOAUX[linkno].aux_tx_read)() != 0 {}
let ptr = DRTIOAUX_MEM[linkno].base; let pointer = DRTIOAUX_MEM[linkno].base as *mut u8;
let len = DRTIOAUX_MEM[linkno].size / 2; let len = DRTIOAUX_MEM[linkno].size / 2;
let len = f(slice::from_raw_parts_mut(ptr as *mut u8, len))?; // work buffer, works with unaligned mem access
let mut buf: [u8; 1024] = [0; 1024];
let len = f(&mut buf[0..len])?;
for i in 0..len {
*pointer.offset(i as isize) = buf[i];
}
(DRTIOAUX[linkno].aux_tx_length_write)(len as u16); (DRTIOAUX[linkno].aux_tx_length_write)(len as u16);
(DRTIOAUX[linkno].aux_tx_write)(1); (DRTIOAUX[linkno].aux_tx_write)(1);
Ok(()) Ok(())
@ -143,6 +167,8 @@ pub fn send(linkno: u8, packet: &Packet) -> Result<(), Error> {
} }
let checksum = crc::crc32::checksum_ieee(&writer.get_ref()[0..writer.position()]); let checksum = crc::crc32::checksum_ieee(&writer.get_ref()[0..writer.position()]);
let len = writer.position();
swap_byte_order(&mut writer.get_mut()[0..len]);
writer.write_u32(checksum)?; writer.write_u32(checksum)?;
Ok(writer.position()) Ok(writer.position())