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

use bxcan::{Can, Instance};
use nb::block;

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

#[derive(Debug, PartialEq)]
pub enum RossCanError {
    BufferOverrun,
    MailboxFull,
}

pub struct RossCan<I: Instance> {
    can: Can<I>,
    packet_builder: Option<RossPacketBuilder>,
}

impl<I: Instance> RossCan<I> {
    pub fn new(can: Can<I>) -> Self {
        RossCan {
            can,
            packet_builder: None,
        }
    }
}

impl<I: Instance> RossInterface for RossCan<I> {
    fn try_get_packet(&mut self) -> Result<RossPacket, RossInterfaceError> {
        loop {
            match self.can.receive() {
                Ok(frame) => {
                    let ross_frame = match RossFrame::from_bxcan_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 {
            if let Ok(Some(_)) = block!(self.can.transmit(&frame.to_bxcan_frame())) {
                return Err(RossInterfaceError::CanError(RossCanError::MailboxFull));
            }
        }

        Ok(())
    }
}