diff --git a/src/codec/rtu/mod.rs b/src/codec/rtu/mod.rs index 06b350f..ea595ec 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 bytes_expected = if quantity % 8 == 0 { + quantity / 8 + } else { + quantity / 8 + 1 + }; + + if bytes_expected == bytes as u16 { + Some(6 + bytes as usize) + } else { + return Err(Error::QuantityBytesMismatch { + quantity, + bytes, + bytes_expected, + }); + } + } else { + // incomplete frame + None + } + } + + 0x10 => { + if adu_buf.len() > 6 { + let quantity = u16::from_be_bytes([adu_buf[4], adu_buf[5]]); + let bytes_expected = quantity.saturating_mul(2); + let bytes = adu_buf[6]; + + if bytes_expected == adu_buf[6] as u16 { + Some(6 + adu_buf[6] as usize) + } else { + return Err(Error::QuantityBytesMismatch { + quantity, + bytes, + bytes_expected, + }); + } } else { // incomplete frame None @@ -251,13 +289,53 @@ 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 { + quantity: 0, + bytes: 99, + bytes_expected: 0 + }) + ); buf[1] = 0x10; - buf[4] = 99; + buf[6] = 99; + assert_eq!( + request_pdu_len(buf), + Err(Error::QuantityBytesMismatch { + quantity: 0, + bytes: 99, + bytes_expected: 0 + }) + ); + + buf[1] = 0x0F; + buf[5] = 99; + buf[6] = 99; + assert_eq!( + request_pdu_len(buf), + Err(Error::QuantityBytesMismatch { + quantity: 99, + bytes: 99, + bytes_expected: 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..eb2ae1b 100644 --- a/src/error.rs +++ b/src/error.rs @@ -24,6 +24,12 @@ pub enum Error { LengthMismatch(usize, usize), /// Protocol not Modbus ProtocolNotModbus(u16), + /// Length Mismatch + QuantityBytesMismatch { + quantity: u16, + bytes_expected: u16, + bytes: u8, + }, } impl fmt::Display for Error { @@ -48,6 +54,14 @@ 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 +99,17 @@ 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 + ), } } }