1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108

use alloc::vec;
use embedded_hal::serial::{Read, Write};
use nb::block;

use crate::ross_frame::*;
use crate::ross_packet::*;
use crate::ross_interface::*;

#[derive(Debug, PartialEq)]
pub enum RossUsartError {
    ReadError,
}

pub struct RossUsart<S: Read<u8> + Write<u8>> {
    serial: S,
    packet_builder: Option<RossPacketBuilder>,
}

impl<S: Read<u8> + Write<u8>> RossUsart<S> {
    pub fn new(serial: S) -> Self {
        RossUsart {
            serial,
            packet_builder: None,
        }
    }
}

impl<S: Read<u8> + Write<u8>> RossInterface for RossUsart<S> {
    fn try_get_packet(&mut self) -> Result<RossPacket, RossInterfaceError> {
        loop {
            match self.serial.read() {
                Ok(frame_start) => {
                    if frame_start == 0x00 {
                        let mut frame = vec![];

                        let expected_length = match block!(self.serial.read()) {
                            Ok(length) => length,
                            Err(_) => return Err(RossInterfaceError::UsartError(RossUsartError::ReadError)),
                        };

                        loop {
                            match block!(self.serial.read()) {
                                Ok(byte) => frame.push(byte),
                                Err(_) => return Err(RossInterfaceError::UsartError(RossUsartError::ReadError)),
                            }

                            if frame.len() == expected_length as usize {
                                break;
                            }
                        }

                        let ross_frame = match RossFrame::from_usart_frame(frame) {
                            Ok(frame) => frame,
                            Err(err) => return Err(RossInterfaceError::FrameError(err)),
                        };

                        if let Some(ref mut packet_builder) = self.packet_builder {
                            if let Err(err) = packet_builder.add_frame(ross_frame) {
                                self.packet_builder = None;

                                return Err(RossInterfaceError::BuilderError(err));
                            }
                        } else {
                            self.packet_builder = match RossPacketBuilder::new(ross_frame) {
                                Ok(builder) => Some(builder),
                                Err(err) => return Err(RossInterfaceError::BuilderError(err)),
                            };
                        }

                        if let Some(ref mut packet_builder) = self.packet_builder {
                            if packet_builder.frames_left() == 0 {
                                let packet = match packet_builder.build() {
                                    Ok(packet) => packet,
                                    Err(err) => return Err(RossInterfaceError::BuilderError(err)),
                                };

                                self.packet_builder = None;

                                return Ok(packet);
                            }
                        }
                    }
                }
                Err(_) => break,
            }
        }

        Err(RossInterfaceError::NoPacketReceived)
    }

    fn try_send_packet(&mut self, packet: &RossPacket) -> Result<(), RossInterfaceError> {
        let frames = packet.to_frames();

        for frame in frames {
            let _ = block!(self.serial.write(0x00));

            let usart_frame = frame.to_usart_frame();

            let _ = block!(self.serial.write(usart_frame.len() as u8));

            for byte in frame.to_usart_frame().iter() {
                let _ = block!(self.serial.write(*byte));
            }
        }

        Ok(())
    }
}