uf_crsf/packets/
device_information.rs

1use crate::packets::{CrsfPacket, PacketType};
2use crate::CrsfParsingError;
3use core::mem::size_of;
4use heapless::String;
5
6const MAX_DEVICE_NAME_LEN: usize = 43;
7const EXTENDED_HEADER_SIZE: usize = 2 * size_of::<u8>();
8const FIXED_FIELDS_SIZE: usize = 3 * size_of::<u32>() + 2 * size_of::<u8>();
9
10/// Represents a Device Information packet (0x29).
11#[derive(Clone, Debug, PartialEq, Eq)]
12pub struct DeviceInformation {
13    pub dst_addr: u8,
14    pub src_addr: u8,
15    device_name: String<MAX_DEVICE_NAME_LEN>,
16    pub serial_number: u32,
17    pub hardware_id: u32,
18    pub firmware_id: u32,
19    pub parameters_total: u8,
20    pub parameter_version_number: u8,
21}
22
23impl DeviceInformation {
24    /// Creates a new DeviceInformation packet.
25    #[allow(clippy::too_many_arguments)]
26    pub fn new(
27        dst_addr: u8,
28        src_addr: u8,
29        device_name: &str,
30        serial_number: u32,
31        hardware_id: u32,
32        firmware_id: u32,
33        parameters_total: u8,
34        parameter_version_number: u8,
35    ) -> Result<Self, CrsfParsingError> {
36        if device_name.len() > MAX_DEVICE_NAME_LEN {
37            return Err(CrsfParsingError::InvalidPayloadLength);
38        }
39        let mut s = String::new();
40        s.push_str(device_name)
41            .map_err(|_| CrsfParsingError::InvalidPayloadLength)?;
42        Ok(Self {
43            dst_addr,
44            src_addr,
45            device_name: s,
46            serial_number,
47            hardware_id,
48            firmware_id,
49            parameters_total,
50            parameter_version_number,
51        })
52    }
53
54    /// Returns the device name as a string slice.
55    pub fn device_name(&self) -> &str {
56        self.device_name.as_str()
57    }
58}
59
60#[cfg(feature = "defmt")]
61impl defmt::Format for DeviceInformation {
62    fn format(&self, fmt: defmt::Formatter) {
63        defmt::write!(
64            fmt,
65            "DeviceInformation {{ dst_addr: {=u8}, src_addrs: {=u8}, device_name: {}, serial_number: {=u32}, hardware_id: {=u32}, firmware_id: {=u32}, parameters_total: {=u8}, parameter_version_number: {=u8} }}",
66            self.dst_addr,
67            self.src_addr,
68            self.device_name(),
69            self.serial_number,
70            self.hardware_id,
71            self.firmware_id,
72            self.parameters_total,
73            self.parameter_version_number,
74        )
75    }
76}
77
78impl CrsfPacket for DeviceInformation {
79    const PACKET_TYPE: PacketType = PacketType::DeviceInfo;
80    // Minimum payload is dst, src, a null terminator for the string + 14 bytes of other data
81    const MIN_PAYLOAD_SIZE: usize = EXTENDED_HEADER_SIZE + 1 + FIXED_FIELDS_SIZE;
82
83    fn to_bytes(&self, buffer: &mut [u8]) -> Result<usize, CrsfParsingError> {
84        let name_bytes = self.device_name().as_bytes();
85        let name_len = name_bytes.len();
86        let payload_len = EXTENDED_HEADER_SIZE + name_len + 1 + FIXED_FIELDS_SIZE;
87
88        if buffer.len() < payload_len {
89            return Err(CrsfParsingError::BufferOverflow);
90        }
91
92        buffer[0] = self.dst_addr;
93        buffer[1] = self.src_addr;
94
95        let mut offset = EXTENDED_HEADER_SIZE;
96        buffer[offset..offset + name_len].copy_from_slice(name_bytes);
97        offset += name_len;
98        buffer[offset] = 0; // Null terminator
99        offset += 1;
100
101        buffer[offset..offset + 4].copy_from_slice(&self.serial_number.to_be_bytes());
102        offset += 4;
103        buffer[offset..offset + 4].copy_from_slice(&self.hardware_id.to_be_bytes());
104        offset += 4;
105        buffer[offset..offset + 4].copy_from_slice(&self.firmware_id.to_be_bytes());
106        offset += 4;
107        buffer[offset] = self.parameters_total;
108        offset += 1;
109        buffer[offset] = self.parameter_version_number;
110
111        Ok(payload_len)
112    }
113
114    fn from_bytes(data: &[u8]) -> Result<Self, CrsfParsingError> {
115        if data.len() < Self::MIN_PAYLOAD_SIZE {
116            return Err(CrsfParsingError::InvalidPayloadLength);
117        }
118
119        let dst_addr = data[0];
120        let src_addr = data[1];
121
122        let payload = &data[EXTENDED_HEADER_SIZE..];
123        let null_pos = payload
124            .iter()
125            .position(|&b| b == 0)
126            .ok_or(CrsfParsingError::InvalidPayload)?;
127        let s = core::str::from_utf8(&payload[..null_pos])
128            .map_err(|_| CrsfParsingError::InvalidPayload)?;
129        let mut device_name = String::new();
130        device_name
131            .push_str(s)
132            .map_err(|_e| CrsfParsingError::InvalidPayloadLength)?;
133
134        let mut offset = null_pos + 1;
135        if payload.len() < offset + FIXED_FIELDS_SIZE {
136            return Err(CrsfParsingError::InvalidPayloadLength);
137        }
138
139        let serial_number = u32::from_be_bytes(payload[offset..offset + 4].try_into().unwrap());
140        offset += 4;
141        let hardware_id = u32::from_be_bytes(payload[offset..offset + 4].try_into().unwrap());
142        offset += 4;
143        let firmware_id = u32::from_be_bytes(payload[offset..offset + 4].try_into().unwrap());
144        offset += 4;
145        let parameters_total = payload[offset];
146        offset += 1;
147        let parameter_version_number = payload[offset];
148
149        Ok(Self {
150            dst_addr,
151            src_addr,
152            device_name,
153            serial_number,
154            hardware_id,
155            firmware_id,
156            parameters_total,
157            parameter_version_number,
158        })
159    }
160}
161
162#[cfg(test)]
163mod tests {
164    use super::*;
165
166    #[test]
167    fn test_device_information_to_bytes() {
168        let info = DeviceInformation::new(
169            0xEA,
170            0xEE,
171            "TBS Tracer",
172            0x12345678,
173            0xABCDEF01,
174            0x98765432,
175            42,
176            5,
177        )
178        .unwrap();
179
180        let mut buffer = [0u8; 60];
181        let len = info.to_bytes(&mut buffer).unwrap();
182
183        let expected_name = b"TBS Tracer\0";
184        let expected_len = EXTENDED_HEADER_SIZE + expected_name.len() + FIXED_FIELDS_SIZE;
185        assert_eq!(len, expected_len);
186
187        assert_eq!(buffer[0], 0xEA);
188        assert_eq!(buffer[1], 0xEE);
189        assert_eq!(
190            &buffer[EXTENDED_HEADER_SIZE..EXTENDED_HEADER_SIZE + expected_name.len()],
191            expected_name
192        );
193        let mut offset = EXTENDED_HEADER_SIZE + expected_name.len();
194        assert_eq!(&buffer[offset..offset + 4], &0x12345678u32.to_be_bytes());
195        offset += 4;
196        assert_eq!(&buffer[offset..offset + 4], &0xABCDEF01u32.to_be_bytes());
197        offset += 4;
198        assert_eq!(&buffer[offset..offset + 4], &0x98765432u32.to_be_bytes());
199        offset += 4;
200        assert_eq!(buffer[offset], 42);
201        offset += 1;
202        assert_eq!(buffer[offset], 5);
203    }
204
205    #[test]
206    fn test_device_information_from_bytes() {
207        let data =
208            b"\xEA\xEE\nTBS Tracer\0\x12\x34\x56\x78\xAB\xCD\xEF\x01\x98\x76\x54\x32\x2A\x05";
209        let info = DeviceInformation::from_bytes(data).unwrap();
210
211        assert_eq!(info.dst_addr, 0xEA);
212        assert_eq!(info.src_addr, 0xEE);
213        assert_eq!(info.device_name(), "\nTBS Tracer");
214        assert_eq!(info.serial_number, 0x12345678);
215        assert_eq!(info.hardware_id, 0xABCDEF01);
216        assert_eq!(info.firmware_id, 0x98765432);
217        assert_eq!(info.parameters_total, 42);
218        assert_eq!(info.parameter_version_number, 5);
219    }
220
221    #[test]
222    fn test_device_information_round_trip() {
223        let info = DeviceInformation::new(0x12, 0x34, "MyDevice", 1, 2, 3, 4, 5).unwrap();
224
225        let mut buffer = [0u8; 60];
226        let len = info.to_bytes(&mut buffer).unwrap();
227        let round_trip_info = DeviceInformation::from_bytes(&buffer[..len]).unwrap();
228
229        assert_eq!(info, round_trip_info);
230    }
231
232    #[test]
233    fn test_device_information_from_bytes_invalid_len_too_short() {
234        let data = b"\xEA\xEEToo short\0";
235        let result = DeviceInformation::from_bytes(data);
236        assert!(matches!(
237            result,
238            Err(CrsfParsingError::InvalidPayloadLength)
239        ));
240    }
241
242    #[test]
243    fn test_device_information_from_bytes_invalid_len_no_room_for_fixed_fields() {
244        let data = b"\xEA\xEEThis string is long enough but no room for fixed fields\0";
245        let result = DeviceInformation::from_bytes(data);
246        assert!(matches!(
247            result,
248            Err(CrsfParsingError::InvalidPayloadLength)
249        ));
250    }
251
252    #[test]
253    fn test_device_buffer_too_small() {
254        let info = DeviceInformation::new(0x12, 0x34, "MyDevice", 1, 2, 3, 4, 5).unwrap();
255        let mut buffer = [0u8; DeviceInformation::MIN_PAYLOAD_SIZE - 1];
256        let result = info.to_bytes(&mut buffer);
257        assert_eq!(result, Err(CrsfParsingError::BufferOverflow));
258    }
259
260    #[test]
261    fn test_device_information_from_bytes_no_null() {
262        let data = b"\xEA\xEE\nNo null terminator here and lots of other data that should be enough for the rest of the fields 12345678901234";
263        let result = DeviceInformation::from_bytes(data);
264        assert!(matches!(result, Err(CrsfParsingError::InvalidPayload)));
265    }
266}
uf_crsf/packets/device_information.rs

uf_crsf/packets/
device_information.rs
