improv-setup/src/serial.rs

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>,
}

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,
        });
    }

    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()?.try_into()?;
        let mut buffer: Vec<u8> = vec![0; available_bytes];
        self.interface.read(&mut buffer)?;

        if log_enabled!(Level::Debug) {
            debug!("Received bytes: \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]);

        let packet_bytes = buffer[improv_packet_offset..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);
    }
}