Fix rtu request pdu length calculation

src/codec/rtu/mod.rs

@@ -147,9 +147,47 @@ pub const fn request_pdu_len(adu_buf: &[u8]) -> Result<Option<usize>> {
     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));
 

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
+            ),
         }
     }
 }