1use super::*;
4use byteorder::{BigEndian, ByteOrder};
5
6pub mod client;
7pub mod server;
8pub use crate::frame::rtu::*;
9
10const MAX_FRAME_LEN: usize = 256;
13
14#[derive(Debug, Clone, Copy, PartialEq, Eq)]
16pub struct DecodedFrame<'a> {
17 pub slave: SlaveId,
18 pub pdu: &'a [u8],
19}
20
21#[derive(Debug, Clone, Copy, PartialEq, Eq)]
23pub struct FrameLocation {
24 pub start: usize,
26 pub size: usize,
28}
29
30pub fn decode(
32 decoder_type: DecoderType,
33 buf: &[u8],
34) -> Result<Option<(DecodedFrame, FrameLocation)>> {
35 use DecoderType::{Request, Response};
36 let mut drop_cnt = 0;
37
38 if buf.is_empty() {
39 return Err(Error::BufferSize);
40 }
41
42 loop {
43 let mut retry = false;
44 if drop_cnt + 1 >= buf.len() {
45 return Ok(None);
46 }
47 let raw_frame = &buf[drop_cnt..];
48 let res = match decoder_type {
49 Request => request_pdu_len(raw_frame),
50 Response => response_pdu_len(raw_frame),
51 }
52 .and_then(|pdu_len| {
53 retry = false;
54 let Some(pdu_len) = pdu_len else {
55 return Ok(None);
57 };
58 extract_frame(raw_frame, pdu_len).map(|x| {
59 x.map(|res| {
60 let frame_location = FrameLocation {
61 start: drop_cnt,
62 size: pdu_len + 3, };
64 (res, frame_location)
65 })
66 })
67 })
68 .or_else(|err| {
69 if drop_cnt + 1 >= MAX_FRAME_LEN {
70 log::error!(
71 "Giving up to decode frame after dropping {drop_cnt} byte(s): {:X?}",
72 &buf[0..drop_cnt]
73 );
74 return Err(err);
75 }
76 log::warn!(
77 "Failed to decode {} frame: {err}",
78 match decoder_type {
79 Request => "request",
80 Response => "response",
81 }
82 );
83 drop_cnt += 1;
84 retry = true;
85 Ok(None)
86 });
87
88 if !retry {
89 return res;
90 }
91 }
92}
93
94#[allow(clippy::similar_names)]
96pub fn extract_frame(buf: &[u8], pdu_len: usize) -> Result<Option<DecodedFrame>> {
97 if buf.is_empty() {
98 return Err(Error::BufferSize);
99 }
100
101 let adu_len = 1 + pdu_len;
102 if buf.len() >= adu_len + 2 {
103 let (adu_buf, buf) = buf.split_at(adu_len);
104 let (crc_buf, _) = buf.split_at(2);
105 let expected_crc = BigEndian::read_u16(crc_buf);
107 let actual_crc = crc16(adu_buf);
108 if expected_crc != actual_crc {
109 return Err(Error::Crc(expected_crc, actual_crc));
110 }
111 let (slave_id, pdu_data) = adu_buf.split_at(1);
112 let slave_id = slave_id[0];
113 return Ok(Some(DecodedFrame {
114 slave: slave_id,
115 pdu: pdu_data,
116 }));
117 }
118 Ok(None)
120}
121
122#[must_use]
124pub fn crc16(data: &[u8]) -> u16 {
125 let mut crc = 0xFFFF;
126 for x in data {
127 crc ^= u16::from(*x);
128 for _ in 0..8 {
129 #[allow(clippy::branches_sharing_code)]
132 if (crc & 0x0001) != 0 {
133 crc >>= 1;
134 crc ^= 0xA001;
135 } else {
136 crc >>= 1;
137 }
138 }
139 }
140 crc << 8 | crc >> 8
141}
142
143pub const fn request_pdu_len(adu_buf: &[u8]) -> Result<Option<usize>> {
145 if adu_buf.len() < 2 {
146 return Ok(None);
147 }
148 let fn_code = adu_buf[1];
149 let len = match fn_code {
150 0x01..=0x06 => Some(5),
151 0x07 | 0x0B | 0x0C | 0x11 => Some(1),
152 0x0F | 0x10 => {
153 if adu_buf.len() > 4 {
154 Some(6 + adu_buf[4] as usize)
155 } else {
156 None
158 }
159 }
160 0x16 => Some(7),
161 0x18 => Some(3),
162 0x17 => {
163 if adu_buf.len() > 10 {
164 Some(10 + adu_buf[10] as usize)
165 } else {
166 None
168 }
169 }
170 _ => {
171 return Err(Error::FnCode(fn_code));
172 }
173 };
174 Ok(len)
175}
176
177pub fn response_pdu_len(adu_buf: &[u8]) -> Result<Option<usize>> {
179 if adu_buf.len() < 2 {
180 return Ok(None);
181 }
182 let fn_code = adu_buf[1];
183 let len = match fn_code {
184 0x01..=0x04 | 0x0C | 0x17 => {
185 if adu_buf.len() > 2 {
186 Some(2 + adu_buf[2] as usize)
187 } else {
188 None
190 }
191 }
192 0x05 | 0x06 | 0x0B | 0x0F | 0x10 => Some(5),
193 0x07 | 0x81..=0xAB => Some(2),
194 0x16 => Some(7),
195 0x18 => {
196 if adu_buf.len() > 3 {
197 Some(3 + BigEndian::read_u16(&adu_buf[2..=3]) as usize)
198 } else {
199 None
201 }
202 }
203 _ => return Err(Error::FnCode(fn_code)),
204 };
205 Ok(len)
206}
207
208#[cfg(test)]
209mod tests {
210 use super::*;
211
212 #[test]
213 fn test_calc_crc16() {
214 let msg = &[0x01, 0x03, 0x08, 0x2B, 0x00, 0x02];
215 assert_eq!(crc16(msg), 0xB663);
216
217 let msg = &[0x01, 0x03, 0x04, 0x00, 0x20, 0x00, 0x00];
218 assert_eq!(crc16(msg), 0xFBF9);
219 }
220
221 #[test]
222 fn test_request_pdu_len() {
223 let buf = &mut [0x66, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
224 assert!(request_pdu_len(buf).is_err());
225
226 buf[1] = 0x01;
227 assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
228
229 buf[1] = 0x02;
230 assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
231
232 buf[1] = 0x03;
233 assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
234
235 buf[1] = 0x04;
236 assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
237
238 buf[1] = 0x05;
239 assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
240
241 buf[1] = 0x06;
242 assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
243
244 buf[1] = 0x07;
245 assert_eq!(request_pdu_len(buf).unwrap(), Some(1));
246
247 buf[1] = 0x0B;
250 assert_eq!(request_pdu_len(buf).unwrap(), Some(1));
251
252 buf[1] = 0x0C;
253 assert_eq!(request_pdu_len(buf).unwrap(), Some(1));
254
255 buf[1] = 0x0F;
256 buf[4] = 99;
257 assert_eq!(request_pdu_len(buf).unwrap(), Some(105));
258
259 buf[1] = 0x10;
260 buf[4] = 99;
261 assert_eq!(request_pdu_len(buf).unwrap(), Some(105));
262
263 buf[1] = 0x11;
264 assert_eq!(request_pdu_len(buf).unwrap(), Some(1));
265
266 buf[1] = 0x16;
271 assert_eq!(request_pdu_len(buf).unwrap(), Some(7));
272
273 buf[1] = 0x17;
274 buf[10] = 99; assert_eq!(request_pdu_len(buf).unwrap(), Some(109));
276
277 buf[1] = 0x18;
278 assert_eq!(request_pdu_len(buf).unwrap(), Some(3));
279
280 }
282
283 #[test]
284 fn test_get_response_pdu_len() {
285 let buf = &mut [0x66, 0x01, 99];
286 assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
287
288 let buf = &mut [0x66, 0x00, 99, 0x00];
289 assert_eq!(response_pdu_len(buf).err().unwrap(), Error::FnCode(0));
290
291 let buf = &mut [0x66, 0xee, 99, 0x00];
292 assert_eq!(response_pdu_len(buf).err().unwrap(), Error::FnCode(0xee));
293
294 buf[1] = 0x01;
295 assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
296
297 buf[1] = 0x02;
298 assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
299
300 buf[1] = 0x03;
301 assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
302
303 buf[1] = 0x04;
304 assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
305
306 buf[1] = 0x05;
307 assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
308
309 buf[1] = 0x06;
310 assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
311
312 buf[1] = 0x07;
313 assert_eq!(response_pdu_len(buf).unwrap(), Some(2));
314
315 buf[1] = 0x0B;
318 assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
319
320 buf[1] = 0x0C;
321 assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
322
323 buf[1] = 0x0F;
324 assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
325
326 buf[1] = 0x10;
327 assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
328
329 buf[1] = 0x16;
336 assert_eq!(response_pdu_len(buf).unwrap(), Some(7));
337
338 buf[1] = 0x17;
339 assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
340
341 buf[1] = 0x18;
342 buf[2] = 0x01; buf[3] = 0x00; assert_eq!(response_pdu_len(buf).unwrap(), Some(259));
345
346 for i in 0x81..0xAB {
349 buf[1] = i;
350 assert_eq!(response_pdu_len(buf).unwrap(), Some(2));
351 }
352 }
353
354 mod frame_decoder {
355
356 use super::*;
357
358 #[test]
359 fn extract_partly_received_rtu_frame() {
360 let buf = &[
361 0x12, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, ];
370 let pdu_len = request_pdu_len(buf).unwrap().unwrap();
371 let res = extract_frame(buf, pdu_len).unwrap();
372 assert!(res.is_none());
373 }
374
375 #[test]
376 fn extract_usual_rtu_response_frame() {
377 let buf = &[
378 0x01, 0x03, 0x04, 0x89, 0x02, 0x42, 0xC7, 0x00, 0x9D, 0x03, ];
389 let pdu_len = response_pdu_len(buf).unwrap().unwrap();
390 let DecodedFrame { slave, pdu } = extract_frame(buf, pdu_len).unwrap().unwrap();
391 assert_eq!(slave, 0x01);
392 assert_eq!(pdu.len(), 6);
393 }
394
395 #[test]
396 fn decode_rtu_response_drop_invalid_bytes() {
397 let buf = &[
398 0x42, 0x43, 0x01, 0x03, 0x04, 0x89, 0x02, 0x42, 0xC7, 0x00, 0x9D, 0x00,
410 ];
411 let (frame, location) = decode(DecoderType::Response, buf).unwrap().unwrap();
412 assert_eq!(frame.slave, 0x01);
413 assert_eq!(frame.pdu.len(), 6);
414 assert_eq!(location.start, 2);
415 assert_eq!(location.size, 9);
416 }
417
418 #[test]
419 fn decode_rtu_response_with_max_drops() {
420 let buf = &[0x42; 10];
421 assert!(decode(DecoderType::Response, buf).unwrap().is_none());
422
423 let buf = &mut [0x42; MAX_FRAME_LEN * 2];
424 buf[256] = 0x01; buf[257] = 0x03; buf[258] = 0x04; buf[259] = 0x89; buf[260] = 0x02; buf[261] = 0x42; buf[262] = 0xC7; buf[263] = 0x00; buf[264] = 0x9D; assert!(decode(DecoderType::Response, buf).is_err());
434 }
435 }
436}