124 lines
3.5 KiB
Rust
124 lines
3.5 KiB
Rust
use anyhow::{
|
|
bail,
|
|
Context,
|
|
Result,
|
|
};
|
|
use log::{
|
|
debug,
|
|
log_enabled,
|
|
Level,
|
|
};
|
|
|
|
use crate::improv::{
|
|
IMPROV_HEADER,
|
|
RawPacket,
|
|
ImprovDataToPacket,
|
|
};
|
|
use crate::utils::{
|
|
to_ascii_debug,
|
|
};
|
|
|
|
pub fn find_begin_of_improv_packet(buffer: &Vec<u8>) -> Result<usize> {
|
|
let mut improv_header_char: usize = 0;
|
|
|
|
for (i, b) in buffer.iter().enumerate() {
|
|
if b == &IMPROV_HEADER[improv_header_char] {
|
|
improv_header_char += 1;
|
|
|
|
if improv_header_char >= IMPROV_HEADER.len() {
|
|
return Ok(i - (IMPROV_HEADER.len() - 1));
|
|
}
|
|
}
|
|
else {
|
|
improv_header_char = 0;
|
|
if b == &IMPROV_HEADER[improv_header_char] {
|
|
improv_header_char += 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
bail!("Improv header not found");
|
|
}
|
|
|
|
pub struct SerialInterface {
|
|
interface: Box<dyn tokio_serial::SerialPort>,
|
|
buffer: Vec<u8>,
|
|
}
|
|
|
|
impl SerialInterface {
|
|
|
|
pub fn new(path: &str, baud_rate: u32) -> Result<Self> {
|
|
let interface = tokio_serial::new(path, baud_rate).open().context("Failed to open serial device")?;
|
|
|
|
return Ok(Self {
|
|
interface: interface,
|
|
buffer: Vec::new(),
|
|
});
|
|
}
|
|
|
|
pub fn send_bytes(&mut self, packet_bytes: &[u8]) -> Result<()> {
|
|
if log_enabled!(Level::Debug) {
|
|
debug!("Sending packet: \n{}\n{}", hex::encode(packet_bytes), to_ascii_debug(packet_bytes));
|
|
}
|
|
self.interface.write(packet_bytes).context("Unable to write bytes to serial interface")?;
|
|
|
|
Ok(())
|
|
}
|
|
|
|
pub fn send_raw_packet(&mut self, raw_packet: RawPacket) -> Result<()> {
|
|
self.send_bytes(&raw_packet.to_bytes())?;
|
|
|
|
Ok(())
|
|
}
|
|
|
|
pub fn send(&mut self, packet: &impl ImprovDataToPacket) -> Result<()> {
|
|
self.send_raw_packet(packet.to_raw_packet())?;
|
|
|
|
Ok(())
|
|
}
|
|
|
|
pub fn recv_bytes(&mut self) -> Result<Vec<u8>> {
|
|
let available_bytes = self.interface.bytes_to_read()
|
|
.context("Failed to figure out how many bytes are available to read")?
|
|
.try_into()?;
|
|
|
|
debug!("Available bytes to read: {}", available_bytes);
|
|
let mut buffer: Vec<u8> = vec![0; available_bytes];
|
|
|
|
if buffer.len() > 0 {
|
|
self.interface.read(&mut buffer)
|
|
.context("Failed to read bytes from serial device")?;
|
|
}
|
|
|
|
if log_enabled!(Level::Debug) {
|
|
debug!("Received bytes: \n{}\n{}", hex::encode(&buffer), to_ascii_debug(&buffer));
|
|
}
|
|
|
|
self.buffer.append(&mut buffer);
|
|
|
|
buffer = self.buffer.clone();
|
|
|
|
if log_enabled!(Level::Debug) {
|
|
debug!("Buffer: \n{}\n{}", hex::encode(&buffer), to_ascii_debug(&buffer));
|
|
}
|
|
|
|
let improv_packet_offset = find_begin_of_improv_packet(&buffer).context("Failed to find improv header in received bytes from serial device")?;
|
|
|
|
let improv_packet_end = improv_packet_offset + 10 + <u8 as Into<usize>>::into(buffer[improv_packet_offset+8]);
|
|
|
|
if buffer.len() <= improv_packet_end {
|
|
bail!("Incomplete packet, packet longer than available in buffer");
|
|
}
|
|
|
|
let packet_bytes = buffer[improv_packet_offset..improv_packet_end].to_vec();
|
|
|
|
self.buffer = buffer[improv_packet_end..].to_vec();
|
|
|
|
if log_enabled!(Level::Debug) {
|
|
debug!("Received packet: \n{}\n{}", hex::encode(&packet_bytes), to_ascii_debug(&packet_bytes));
|
|
}
|
|
|
|
return Ok(packet_bytes);
|
|
}
|
|
}
|