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
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163

use crate::err::{ConstructionError, CanError, CanErrorDecodingFailure};
use embedded_hal::can::{Frame, Id, StandardId, ExtendedId};
use crate::constants::*;
use crate::util::hal_id_to_raw;

use std::fmt;

use itertools::Itertools;


/// CanFrame
///
/// Uses the same memory layout as the underlying kernel struct for performance
/// reasons.
#[derive(Default, Debug, Copy, Clone)]
#[repr(C)]
pub struct CanFrame {
    /// 32 bit CAN_ID + EFF/RTR/ERR flags
    _id: u32,

    /// data length. Bytes beyond are not valid
    _data_len: u8,

    /// padding
    _pad: u8,

    /// reserved
    _res0: u8,

    /// reserved
    _res1: u8,

    /// buffer for data
    _data: [u8; 8],
}

impl CanFrame {
    pub fn init(id: u32, data: &[u8], rtr: bool, err: bool) -> Result<CanFrame, ConstructionError> {
        let mut _id = id;

        if data.len() > 8 {
            return Err(ConstructionError::TooMuchData);
        }

        if id > EFF_MASK {
            return Err(ConstructionError::IDTooLarge);
        }

        // set EFF_FLAG on large message
        if id > SFF_MASK {
            _id |= EFF_FLAG;
        }


        if rtr {
            _id |= RTR_FLAG;
        }

        if err {
            _id |= ERR_FLAG;
        }

        let mut full_data = [0; 8];

        // not cool =/
        for (n, c) in data.iter().enumerate() {
            full_data[n] = *c;
        }

        Ok(CanFrame {
               _id,
               _data_len: data.len() as u8,
               _pad: 0,
               _res0: 0,
               _res1: 0,
               _data: full_data,
           })
    }

    /// Return the error message
    pub fn err(&self) -> u32 {
        self._id & ERR_MASK
    }

    /// Check if frame is an error message
    pub fn is_error(&self) -> bool {
        self._id & ERR_FLAG != 0
    }

    // Read error from message and transform it into a `CanError`.
    //
    // SocketCAN errors are indicated using the error bit and coded inside
    // id and data payload. Call `error()` converts these into usable
    // `CanError` instances.
    //
    // If the frame is malformed, this may fail with a
    // `CanErrorDecodingFailure`.
    #[inline]
    pub fn error(&self) -> Result<CanError, CanErrorDecodingFailure> {
        CanError::from_frame(self)
    }

}

impl Frame for CanFrame {
    /// Create a new frame
    fn new(id: impl Into<Id>, data: &[u8]) -> Option<Self> {
        let raw_id = hal_id_to_raw(id.into());
        CanFrame::init(raw_id, data, false, false).ok()
    }

    fn new_remote(id: impl Into<Id>, dlc: usize) -> Option<Self> {
        let raw_id = hal_id_to_raw(id.into());
        let data: [u8; 8] = Default::default();

        CanFrame::init(raw_id, &data[0..dlc], true, false).ok()
    }

    /// Return the actual CAN ID (without EFF/RTR/ERR flags)
    fn id(&self) -> Id {
        if self.is_extended() {
            Id::Extended(
                ExtendedId::new(self._id & EFF_MASK).unwrap()
            )
        } else {
            Id::Standard(
                StandardId::new((self._id & SFF_MASK) as u16).unwrap()
            )
        }
    }

    /// Check if frame uses 29 bit extended frame format
    fn is_extended(&self) -> bool {
        self._id & EFF_FLAG != 0
    }

    /// Check if frame is a remote transmission request
    fn is_remote_frame(&self) -> bool {
        self._id & RTR_FLAG != 0
    }
    
    /// Data length
    fn dlc(&self) -> usize {
        self._data_len as usize
    }

    /// A slice into the actual data. Slice will always be <= 8 bytes in length
    fn data(&self) -> &[u8] {
        &self._data[..(self._data_len as usize)]
    }
}

impl fmt::UpperHex for CanFrame {
    fn fmt(&self, f: &mut fmt::Formatter) -> Result<(), fmt::Error> {
        write!(f, "{:X}#", hal_id_to_raw(self.id()))?;

        let mut parts = self.data().iter().map(|v| format!("{:02X}", v));

        let sep = if f.alternate() { " " } else { "" };
        write!(f, "{}", parts.join(sep))
    }
}