From 50578af1f91122960096dcffb9c6c5866df9a9d0 Mon Sep 17 00:00:00 2001 From: AsgerWenneb Date: Mon, 11 Aug 2025 11:23:32 +0200 Subject: [PATCH 1/2] Fix rtu request pdu length calculation --- src/codec/rtu/mod.rs | 78 ++++++++++++++++++++++++++++++++++++++++---- src/error.rs | 13 ++++++++ 2 files changed, 85 insertions(+), 6 deletions(-) diff --git a/src/codec/rtu/mod.rs b/src/codec/rtu/mod.rs index 06b350f..1829de1 100644 --- a/src/codec/rtu/mod.rs +++ b/src/codec/rtu/mod.rs @@ -147,9 +147,47 @@ pub const fn request_pdu_len(adu_buf: &[u8]) -> Result> { let len = match fn_code { 0x01..=0x06 => Some(5), 0x07 | 0x0B | 0x0C | 0x11 => Some(1), - 0x0F | 0x10 => { - if adu_buf.len() > 4 { - Some(6 + adu_buf[4] as usize) + 0x0F => { + if adu_buf.len() > 6 { + let quantity = u16::from_be_bytes([adu_buf[4], adu_buf[5]]); + let bytes = adu_buf[6]; + + let expected_count = if quantity % 8 == 0 { + quantity / 8 + } else { + quantity / 8 + 1 + }; + + if expected_count == bytes as u16 { + Some(6 + bytes as usize) + } else { + return Err(Error::QuantityBytesMismatch( + quantity, + bytes, + expected_count, + )); + } + } else { + // incomplete frame + None + } + } + + 0x10 => { + if adu_buf.len() > 6 { + let quantity = u16::from_be_bytes([adu_buf[4], adu_buf[5]]); + let expected_count = quantity.saturating_mul(2); + let bytes = adu_buf[6]; + + if expected_count == adu_buf[6] as u16 { + Some(6 + adu_buf[6] as usize) + } else { + return Err(Error::QuantityBytesMismatch( + quantity, + bytes, + expected_count, + )); + } } else { // incomplete frame None @@ -251,13 +289,41 @@ mod tests { assert_eq!(request_pdu_len(buf).unwrap(), Some(1)); buf[1] = 0x0F; - buf[4] = 99; - assert_eq!(request_pdu_len(buf).unwrap(), Some(105)); + buf[6] = 99; + assert_eq!( + request_pdu_len(buf), + Err(Error::QuantityBytesMismatch(0, 99, 0)) + ); buf[1] = 0x10; - buf[4] = 99; + buf[6] = 99; + assert_eq!( + request_pdu_len(buf), + Err(Error::QuantityBytesMismatch(0, 99, 0)) + ); + + buf[1] = 0x0F; + buf[5] = 99; + buf[6] = 99; + assert_eq!( + request_pdu_len(buf), + Err(Error::QuantityBytesMismatch(99, 99, 13)) + ); + + buf[1] = 0x0F; + buf[4] = 0x03; + buf[5] = 0x14; + buf[6] = 99; assert_eq!(request_pdu_len(buf).unwrap(), Some(105)); + buf[1] = 0x10; + buf[4] = 0; + buf[5] = 49; + buf[6] = 98; + assert_eq!(request_pdu_len(buf).unwrap(), Some(104)); + buf[4] = 0x00; + buf[5] = 0x00; + buf[1] = 0x11; assert_eq!(request_pdu_len(buf).unwrap(), Some(1)); diff --git a/src/error.rs b/src/error.rs index 322ae7e..10e568c 100644 --- a/src/error.rs +++ b/src/error.rs @@ -24,6 +24,8 @@ pub enum Error { LengthMismatch(usize, usize), /// Protocol not Modbus ProtocolNotModbus(u16), + /// Length Mismatch + QuantityBytesMismatch(u16, u8, u16), } impl fmt::Display for Error { @@ -48,6 +50,10 @@ impl fmt::Display for Error { Self::ProtocolNotModbus(protocol_id) => { write!(f, "Protocol not Modbus(0), received {protocol_id} instead") } + Self::QuantityBytesMismatch(quantity, bytes, bytes_expected) => write!( + f, + "Quantity Byte Mismatch: quantity: {quantity}, bytes : {bytes}, bytes expected {bytes_expected}" + ), } } } @@ -85,6 +91,13 @@ impl defmt::Format for Error { protocol_id ) } + Self::QuantityBytesMismatch(quantity, bytes, bytes_expected) => defmt::write!( + f, + "Quantity Byte Mismatch: quantity: {}, bytes: {}, bytes expected: {}", + quantity, + bytes, + bytes_expected + ), } } } From 5435743bd86eb94ef77352d0d98dafce5813b57d Mon Sep 17 00:00:00 2001 From: Asger Wenneberg Date: Tue, 16 Dec 2025 13:40:52 +0100 Subject: [PATCH 2/2] named fields in QuantityBytesMismatch --- src/codec/rtu/mod.rs | 38 +++++++++++++++++++++++++------------- src/error.rs | 18 +++++++++++++++--- 2 files changed, 40 insertions(+), 16 deletions(-) diff --git a/src/codec/rtu/mod.rs b/src/codec/rtu/mod.rs index 1829de1..ea595ec 100644 --- a/src/codec/rtu/mod.rs +++ b/src/codec/rtu/mod.rs @@ -152,20 +152,20 @@ pub const fn request_pdu_len(adu_buf: &[u8]) -> Result> { let quantity = u16::from_be_bytes([adu_buf[4], adu_buf[5]]); let bytes = adu_buf[6]; - let expected_count = if quantity % 8 == 0 { + let bytes_expected = if quantity % 8 == 0 { quantity / 8 } else { quantity / 8 + 1 }; - if expected_count == bytes as u16 { + if bytes_expected == bytes as u16 { Some(6 + bytes as usize) } else { - return Err(Error::QuantityBytesMismatch( + return Err(Error::QuantityBytesMismatch { quantity, bytes, - expected_count, - )); + bytes_expected, + }); } } else { // incomplete frame @@ -176,17 +176,17 @@ pub const fn request_pdu_len(adu_buf: &[u8]) -> Result> { 0x10 => { if adu_buf.len() > 6 { let quantity = u16::from_be_bytes([adu_buf[4], adu_buf[5]]); - let expected_count = quantity.saturating_mul(2); + let bytes_expected = quantity.saturating_mul(2); let bytes = adu_buf[6]; - if expected_count == adu_buf[6] as u16 { + if bytes_expected == adu_buf[6] as u16 { Some(6 + adu_buf[6] as usize) } else { - return Err(Error::QuantityBytesMismatch( + return Err(Error::QuantityBytesMismatch { quantity, bytes, - expected_count, - )); + bytes_expected, + }); } } else { // incomplete frame @@ -292,14 +292,22 @@ mod tests { buf[6] = 99; assert_eq!( request_pdu_len(buf), - Err(Error::QuantityBytesMismatch(0, 99, 0)) + Err(Error::QuantityBytesMismatch { + quantity: 0, + bytes: 99, + bytes_expected: 0 + }) ); buf[1] = 0x10; buf[6] = 99; assert_eq!( request_pdu_len(buf), - Err(Error::QuantityBytesMismatch(0, 99, 0)) + Err(Error::QuantityBytesMismatch { + quantity: 0, + bytes: 99, + bytes_expected: 0 + }) ); buf[1] = 0x0F; @@ -307,7 +315,11 @@ mod tests { buf[6] = 99; assert_eq!( request_pdu_len(buf), - Err(Error::QuantityBytesMismatch(99, 99, 13)) + Err(Error::QuantityBytesMismatch { + quantity: 99, + bytes: 99, + bytes_expected: 13 + }) ); buf[1] = 0x0F; diff --git a/src/error.rs b/src/error.rs index 10e568c..eb2ae1b 100644 --- a/src/error.rs +++ b/src/error.rs @@ -25,7 +25,11 @@ pub enum Error { /// Protocol not Modbus ProtocolNotModbus(u16), /// Length Mismatch - QuantityBytesMismatch(u16, u8, u16), + QuantityBytesMismatch { + quantity: u16, + bytes_expected: u16, + bytes: u8, + }, } impl fmt::Display for Error { @@ -50,7 +54,11 @@ impl fmt::Display for Error { Self::ProtocolNotModbus(protocol_id) => { write!(f, "Protocol not Modbus(0), received {protocol_id} instead") } - Self::QuantityBytesMismatch(quantity, bytes, bytes_expected) => write!( + Self::QuantityBytesMismatch { + quantity, + bytes, + bytes_expected, + } => write!( f, "Quantity Byte Mismatch: quantity: {quantity}, bytes : {bytes}, bytes expected {bytes_expected}" ), @@ -91,7 +99,11 @@ impl defmt::Format for Error { protocol_id ) } - Self::QuantityBytesMismatch(quantity, bytes, bytes_expected) => defmt::write!( + Self::QuantityBytesMismatch { + quantity, + bytes, + bytes_expected, + } => defmt::write!( f, "Quantity Byte Mismatch: quantity: {}, bytes: {}, bytes expected: {}", quantity,