From 23fdcce47e9bfab26e293c8b8c6b22b8e6ff2f27 Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Mon, 2 Sep 2024 11:45:14 +0200
Subject: [PATCH] can-fd do not pad to 64 bytes

---
 src/isotp/mod.rs     | 16 +++++++-------
 tests/isotp_tests.rs | 51 +++++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 58 insertions(+), 9 deletions(-)

diff --git a/src/isotp/mod.rs b/src/isotp/mod.rs
index 0c710ac..88f1701 100644
--- a/src/isotp/mod.rs
+++ b/src/isotp/mod.rs
@@ -127,19 +127,19 @@ impl<'a> IsoTPAdapter<'a> {
         let offset = self.config.ext_address.is_some() as usize;
         let len = data.len() + offset;
 
-        // Pad to next valid DLC
+        // Pad to at least 8 bytes if padding is enabled
+        if let Some(padding) = self.config.padding {
+            let padding_len = CAN_MAX_DLEN - len; // Offset for extended address is already accounted for
+            data.extend(std::iter::repeat(padding).take(padding_len));
+        }
+
+        // Pad to next valid DLC for CAN-FD
         if !DLC_TO_LEN.contains(&len) {
             let idx = DLC_TO_LEN.iter().position(|&x| x > data.len()).unwrap();
             let padding = self.config.padding.unwrap_or(DEFAULT_PADDING_BYTE);
             let padding_len = DLC_TO_LEN[idx] - len;
             data.extend(std::iter::repeat(padding).take(padding_len));
         }
-
-        // Pad to full length if padding is enabled
-        if let Some(padding) = self.config.padding {
-            let padding_len = self.max_can_data_length() - len;
-            data.extend(std::iter::repeat(padding).take(padding_len));
-        }
     }
 
     /// Ofset from the start of the frame. 1 in case of extended address, 0 otherwise.
@@ -160,7 +160,7 @@ impl<'a> IsoTPAdapter<'a> {
     /// Maximum data length for a CAN frame based on the current config
     fn max_can_data_length(&self) -> usize {
         match self.config.max_dlen {
-            Some(dlen) => dlen,
+            Some(dlen) => dlen - self.offset(),
             None => {
                 if self.config.fd {
                     self.can_fd_max_dlen()
diff --git a/tests/isotp_tests.rs b/tests/isotp_tests.rs
index 7652a82..35eefdb 100644
--- a/tests/isotp_tests.rs
+++ b/tests/isotp_tests.rs
@@ -21,6 +21,7 @@ struct VECUConfig {
     pub padding: Option<u8>,
     pub fd: bool,
     pub ext_address: Option<u8>,
+    pub max_dlen: Option<usize>,
 }
 
 impl VECUConfig {
@@ -78,11 +79,12 @@ async fn isotp_test_echo(msg_len: usize, config: VECUConfig) {
     isotp_config.fd = config.fd;
     isotp_config.ext_address = config.ext_address;
     isotp_config.timeout = std::time::Duration::from_millis(1000);
+    isotp_config.max_dlen = config.max_dlen;
 
     let isotp = IsoTPAdapter::new(&adapter, isotp_config);
 
     let mut stream = isotp.recv();
-    let request = vec![0xaa; msg_len];
+    let request = vec![0xcc; msg_len];
     isotp.send(&request).await.unwrap();
     let response = stream.next().await.unwrap().unwrap();
 
@@ -174,6 +176,29 @@ async fn isotp_test_fd() {
     isotp_test_echo(5000, config).await;
 }
 
+#[cfg(feature = "test_vcan")]
+#[tokio::test]
+#[serial_test::serial]
+async fn isotp_test_fd_max_dlen() {
+    let config = VECUConfig {
+        fd: true,
+        max_dlen: Some(8),
+        ..Default::default()
+    };
+
+    // Single frame escape
+    isotp_test_echo(62, config).await;
+
+    // Single frame with some padding to reach next DLC
+    isotp_test_echo(50, config).await;
+
+    // Multiple frames
+    isotp_test_echo(256, config).await;
+
+    // First frame escape
+    isotp_test_echo(5000, config).await;
+}
+
 #[cfg(feature = "test_vcan")]
 #[tokio::test]
 #[serial_test::serial]
@@ -214,3 +239,27 @@ async fn isotp_test_fd_extended() {
     // First frame escape
     isotp_test_echo(5000, config).await;
 }
+
+#[cfg(feature = "test_vcan")]
+#[tokio::test]
+#[serial_test::serial]
+async fn isotp_test_fd_extended_max_dlen() {
+    let config = VECUConfig {
+        fd: true,
+        ext_address: Some(0xff),
+        max_dlen: Some(8),
+        ..Default::default()
+    };
+
+    // Single frame escape
+    isotp_test_echo(62, config).await;
+
+    // Single frame with some padding to reach next DLC
+    isotp_test_echo(50, config).await;
+
+    // Multiple frames
+    isotp_test_echo(256, config).await;
+
+    // First frame escape
+    isotp_test_echo(5000, config).await;
+}