modbus_core/codec/tcp/
mod.rs

1// SPDX-FileCopyrightText: Copyright (c) 2018-2025 slowtec GmbH <post@slowtec.de>
2// SPDX-License-Identifier: MIT OR Apache-2.0
3
4//! Modbus RTU
5
6use super::*;
7use byteorder::{BigEndian, ByteOrder};
8
9pub mod client;
10pub mod server;
11pub use crate::frame::tcp::*;
12
13// [MODBUS MESSAGING ON TCP/IP IMPLEMENTATION GUIDE V1.0b](http://modbus.org/docs/Modbus_Messaging_Implementation_Guide_V1_0b.pdf), page 18
14// "a MODBUS request needs a maximum of 256 bytes + the MBAP header size"
15pub const MAX_FRAME_LEN: usize = 256;
16
17/// An extracted TCP PDU frame.
18#[cfg_attr(all(feature = "defmt", target_os = "none"), derive(defmt::Format))]
19#[derive(Debug, Clone, Copy, PartialEq, Eq)]
20pub struct DecodedFrame<'a> {
21    pub transaction_id: TransactionId,
22    pub unit_id: UnitId,
23    pub pdu: &'a [u8],
24}
25
26/// The location of all bytes that belong to the frame.
27#[cfg_attr(all(feature = "defmt", target_os = "none"), derive(defmt::Format))]
28#[derive(Debug, Clone, Copy, PartialEq, Eq)]
29pub struct FrameLocation {
30    /// The index where the frame starts
31    pub start: usize,
32    /// Number of bytes that belong to the frame
33    pub size: usize,
34}
35
36/// Decode TCP PDU frames from a buffer.
37pub fn decode(
38    decoder_type: DecoderType,
39    buf: &[u8],
40) -> Result<Option<(DecodedFrame<'_>, FrameLocation)>> {
41    use DecoderType::{Request, Response};
42    let mut drop_cnt = 0;
43
44    if buf.is_empty() {
45        return Err(Error::BufferSize);
46    }
47
48    loop {
49        let mut retry = false;
50        if drop_cnt + 1 >= buf.len() {
51            return Ok(None);
52        }
53        let raw_frame = &buf[drop_cnt..];
54        let res = match decoder_type {
55            Request => request_pdu_len(raw_frame),
56            Response => response_pdu_len(raw_frame),
57        }
58        .and_then(|pdu_len| {
59            retry = false;
60            if let Some(pdu_len) = pdu_len {
61                extract_frame(raw_frame, pdu_len).map(|x| {
62                    x.map(|res| {
63                        (
64                            res,
65                            FrameLocation {
66                                start: drop_cnt,
67                                size: pdu_len + 7,
68                            },
69                        )
70                    })
71                })
72            } else {
73                // Incomplete frame
74                Ok(None)
75            }
76        })
77        .or_else(|err| {
78            if drop_cnt + 1 >= MAX_FRAME_LEN {
79                #[cfg(feature = "log")]
80                log::error!(
81                    "Giving up to decode frame after dropping {drop_cnt} byte(s): {:X?}",
82                    &buf[0..drop_cnt]
83                );
84                return Err(err);
85            }
86            #[cfg(feature = "log")]
87            log::warn!(
88                "Failed to decode {pdu_type} frame: {err}",
89                pdu_type = match decoder_type {
90                    Request => "request",
91                    Response => "response",
92                }
93            );
94            drop_cnt += 1;
95            retry = true;
96            Ok(None)
97        });
98
99        if !retry {
100            return res;
101        }
102    }
103}
104
105/// Extract a PDU frame out of a buffer.
106pub fn extract_frame(buf: &[u8], pdu_len: usize) -> Result<Option<DecodedFrame<'_>>> {
107    if buf.is_empty() {
108        return Err(Error::BufferSize);
109    }
110    let adu_len = 7 + pdu_len;
111    if buf.len() >= adu_len {
112        let (adu_buf, _next_frame) = buf.split_at(adu_len);
113        let (adu_buf, pdu_data) = adu_buf.split_at(7);
114        let (transaction_buf, adu_buf) = adu_buf.split_at(2);
115        let (protocol_buf, adu_buf) = adu_buf.split_at(2);
116        let (length_buf, adu_buf) = adu_buf.split_at(2);
117        let protocol_id = BigEndian::read_u16(protocol_buf);
118        if protocol_id != 0 {
119            return Err(Error::ProtocolNotModbus(protocol_id));
120        }
121        let transaction = BigEndian::read_u16(transaction_buf);
122        let m_length = BigEndian::read_u16(length_buf) as usize;
123        let unit = adu_buf[0];
124        if m_length != pdu_len + 1 {
125            return Err(Error::LengthMismatch(m_length, pdu_len + 1));
126        }
127        return Ok(Some(DecodedFrame {
128            transaction_id: transaction,
129            unit_id: unit,
130            pdu: pdu_data,
131        }));
132    }
133    // Incomplete frame
134    Ok(None)
135}
136
137/// Extract the PDU length out of the ADU request buffer.
138pub const fn request_pdu_len(adu_buf: &[u8]) -> Result<Option<usize>> {
139    if adu_buf.len() < 8 {
140        return Ok(None);
141    }
142    let fn_code = adu_buf[7];
143    let len = match fn_code {
144        0x01..=0x06 => Some(5),
145        0x07 | 0x0B | 0x0C | 0x11 => Some(1),
146        0x0F | 0x10 => {
147            if adu_buf.len() > 10 {
148                Some(6 + adu_buf[12] as usize)
149            } else {
150                // incomplete frame
151                None
152            }
153        }
154        0x16 => Some(7),
155        0x18 => Some(3),
156        0x17 => {
157            if adu_buf.len() > 16 {
158                Some(10 + adu_buf[16] as usize)
159            } else {
160                // incomplete frame
161                None
162            }
163        }
164        _ => {
165            return Err(Error::FnCode(fn_code));
166        }
167    };
168    Ok(len)
169}
170
171/// Extract the PDU length out of the ADU response buffer.
172pub fn response_pdu_len(adu_buf: &[u8]) -> Result<Option<usize>> {
173    if adu_buf.len() < 8 {
174        return Ok(None);
175    }
176    let fn_code = adu_buf[7];
177    let len = match fn_code {
178        0x01..=0x04 | 0x0C | 0x17 => {
179            if adu_buf.len() > 8 {
180                Some(2 + adu_buf[8] as usize)
181            } else {
182                // incomplete frame
183                None
184            }
185        }
186        0x05 | 0x06 | 0x0B | 0x0F | 0x10 => Some(5),
187        0x07 | 0x81..=0xAB => Some(2),
188        0x16 => Some(7),
189        0x18 => {
190            if adu_buf.len() > 9 {
191                Some(3 + BigEndian::read_u16(&adu_buf[8..=9]) as usize)
192            } else {
193                // incomplete frame
194                None
195            }
196        }
197        _ => return Err(Error::FnCode(fn_code)),
198    };
199    Ok(len)
200}
201
202#[cfg(test)]
203mod tests {
204    use super::*;
205
206    #[test]
207    fn test_request_pdu_len() {
208        let buf = &mut [0x66, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
209        assert!(request_pdu_len(buf).is_err());
210
211        buf[7] = 0x01;
212        assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
213
214        buf[7] = 0x02;
215        assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
216
217        buf[7] = 0x03;
218        assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
219
220        buf[7] = 0x04;
221        assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
222
223        buf[7] = 0x05;
224        assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
225
226        buf[7] = 0x06;
227        assert_eq!(request_pdu_len(buf).unwrap(), Some(5));
228
229        buf[7] = 0x07;
230        assert_eq!(request_pdu_len(buf).unwrap(), Some(1));
231
232        // TODO: 0x08
233
234        buf[7] = 0x0B;
235        assert_eq!(request_pdu_len(buf).unwrap(), Some(1));
236
237        buf[7] = 0x0C;
238        assert_eq!(request_pdu_len(buf).unwrap(), Some(1));
239
240        buf[7] = 0x0F;
241        buf[12] = 99;
242        assert_eq!(request_pdu_len(buf).unwrap(), Some(105));
243
244        buf[7] = 0x10;
245        buf[12] = 99;
246        assert_eq!(request_pdu_len(buf).unwrap(), Some(105));
247
248        buf[7] = 0x11;
249        assert_eq!(request_pdu_len(buf).unwrap(), Some(1));
250
251        // TODO: 0x14
252
253        // TODO: 0x15
254
255        buf[7] = 0x16;
256        assert_eq!(request_pdu_len(buf).unwrap(), Some(7));
257
258        buf[7] = 0x17;
259        buf[16] = 99; // write byte count
260        assert_eq!(request_pdu_len(buf).unwrap(), Some(109));
261
262        buf[7] = 0x18;
263        assert_eq!(request_pdu_len(buf).unwrap(), Some(3));
264
265        // TODO: 0x2B
266    }
267
268    #[test]
269    fn test_get_response_pdu_len() {
270        let buf = &mut [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x66, 0x01, 99];
271        assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
272
273        let buf = &mut [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x66, 0x00, 99, 0x00];
274        assert_eq!(response_pdu_len(buf).err().unwrap(), Error::FnCode(0));
275
276        let buf = &mut [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x66, 0xee, 99, 0x00];
277        assert_eq!(response_pdu_len(buf).err().unwrap(), Error::FnCode(0xee));
278
279        buf[7] = 0x01;
280        assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
281
282        buf[7] = 0x02;
283        assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
284
285        buf[7] = 0x03;
286        assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
287
288        buf[7] = 0x04;
289        assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
290
291        buf[7] = 0x05;
292        assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
293
294        buf[7] = 0x06;
295        assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
296
297        buf[7] = 0x07;
298        assert_eq!(response_pdu_len(buf).unwrap(), Some(2));
299
300        // TODO: 0x08
301
302        buf[7] = 0x0B;
303        assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
304
305        buf[7] = 0x0C;
306        assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
307
308        buf[7] = 0x0F;
309        assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
310
311        buf[7] = 0x10;
312        assert_eq!(response_pdu_len(buf).unwrap(), Some(5));
313
314        // TODO: 0x11
315
316        // TODO: 0x14
317
318        // TODO: 0x15
319
320        buf[7] = 0x16;
321        assert_eq!(response_pdu_len(buf).unwrap(), Some(7));
322
323        buf[7] = 0x17;
324        assert_eq!(response_pdu_len(buf).unwrap(), Some(101));
325
326        buf[7] = 0x18;
327        buf[8] = 0x01; // byte count Hi
328        buf[9] = 0x00; // byte count Lo
329        assert_eq!(response_pdu_len(buf).unwrap(), Some(259));
330
331        // TODO: 0x2B
332
333        for i in 0x81..0xAB {
334            buf[7] = i;
335            assert_eq!(response_pdu_len(buf).unwrap(), Some(2));
336        }
337    }
338
339    mod frame_decoder {
340
341        use super::*;
342
343        #[test]
344        fn extract_partly_received_tcp_frame() {
345            let buf = &[
346                0x01, // transaction id
347                0x02, // transaction id
348                0x00, // protocol id
349                0x00, // protocol id
350                0x00, // length
351                0x06, // length
352                0x01, // unit id
353                0x02, // function code
354                0x03, // byte count
355                0x00, // data
356                0x00, // data
357                      // missing final data byte
358            ];
359            let pdu_len = request_pdu_len(buf).unwrap().unwrap();
360            let res = extract_frame(buf, pdu_len).unwrap();
361            assert!(res.is_none());
362        }
363
364        #[test]
365        fn extract_usual_tcp_response_frame() {
366            let buf = &[
367                0x01, // transaction id
368                0x02, // transaction id
369                0x00, // protocol id
370                0x00, // protocol id
371                0x00, // length
372                0x07, // length
373                0x01, // unit id
374                0x03, // function code
375                0x04, // byte count
376                0x89, //
377                0x02, //
378                0x42, //
379                0xC7, //
380                0x03, // -- start of next frame
381            ];
382            let pdu_len = response_pdu_len(buf).unwrap().unwrap();
383            let DecodedFrame {
384                transaction_id,
385                unit_id,
386                pdu,
387            } = extract_frame(buf, pdu_len).unwrap().unwrap();
388            assert_eq!(transaction_id, 258);
389            assert_eq!(unit_id, 0x01);
390            assert_eq!(pdu.len(), 6);
391        }
392
393        #[test]
394        fn decode_tcp_response_drop_invalid_bytes() {
395            let buf = &[
396                0x42, // dropped byte
397                0x43, // dropped byte
398                0x01, // transaction id
399                0x02, // transaction id
400                0x00, // protocol id
401                0x00, // protocol id
402                0x00, // length
403                0x07, // length
404                0x01, // unit id
405                0x03, // function code
406                0x04, // byte count
407                0x89, //
408                0x02, //
409                0x42, //
410                0xC7, //
411                0x00, //next frame
412            ];
413            let (frame, location) = decode(DecoderType::Response, buf).unwrap().unwrap();
414            assert_eq!(frame.transaction_id, 258);
415            assert_eq!(frame.unit_id, 0x01);
416            assert_eq!(frame.pdu.len(), 6);
417            assert_eq!(location.start, 2);
418            assert_eq!(location.size, 13);
419        }
420
421        #[test]
422        fn decode_tcp_response_with_max_drops() {
423            let buf = &[0x42; 10];
424            assert!(decode(DecoderType::Response, buf).unwrap().is_none());
425
426            let buf = &mut [0x42; MAX_FRAME_LEN * 2];
427            buf[256] = 0x01; // slave address
428            buf[257] = 0x03; // function code
429            buf[258] = 0x04; // byte count
430            buf[259] = 0x89; //
431            buf[260] = 0x02; //
432            buf[261] = 0x42; //
433            buf[262] = 0xC7; //
434            assert!(decode(DecoderType::Response, buf).is_err());
435        }
436    }
437}
modbus_core/codec/tcp/mod.rs

modbus_core/codec/tcp/
mod.rs
