[ISO-TP] Implement extended addressing (#47)

scripts/vecu_isotp.py

@@ -22,6 +22,7 @@ def forwarding2(pkt):
     parser.add_argument("--bs", type=int, default=0)
     parser.add_argument("--padding", type=int, nargs="?", default=None, const=0xaa)
     parser.add_argument("--fd", action=argparse.BooleanOptionalAction)
+    parser.add_argument("--ext-address", type=int, default=None)
 
     args = parser.parse_args()
 
@@ -35,6 +36,8 @@ def forwarding2(pkt):
         'padding': args.padding,
         'bs': args.bs,
         'fd': args.fd,
+        'ext_address': args.ext_address,
+        'rx_ext_address': args.ext_address,
     }
 
     with ISOTPSocket(args.iface, tx_id=args.tx, rx_id=args.rx, **config) as sock1:

src/isotp/mod.rs

@@ -60,6 +60,8 @@ pub struct IsoTPConfig {
     pub separation_time_min: Option<std::time::Duration>,
     /// Enable CAN-FD Mode
     pub fd: bool,
+    /// Extended address
+    pub ext_address: Option<u8>,
 }
 
 impl IsoTPConfig {
@@ -96,6 +98,7 @@ impl IsoTPConfig {
             timeout: std::time::Duration::from_millis(DEFAULT_TIMEOUT_MS),
             separation_time_min: None,
             fd: false,
+            ext_address: None,
         }
     }
 }
@@ -119,26 +122,42 @@ impl<'a> IsoTPAdapter<'a> {
     }
 
     fn pad(&self, data: &mut Vec<u8>) {
+        // Ensure we leave space for the extended address
+        let offset = self.config.ext_address.is_some() as usize;
+        let len = data.len() + offset;
+
         // Pad to next valid DLC
-        if !DLC_TO_LEN.contains(&data.len()) {
+        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 len = DLC_TO_LEN[idx] - data.len();
-            data.extend(std::iter::repeat(padding).take(len));
+            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 len = self.max_can_data_length() - data.len();
-            data.extend(std::iter::repeat(padding).take(len));
+            let padding_len = self.max_can_data_length() - len;
+            data.extend(std::iter::repeat(padding).take(padding_len));
         }
     }
 
+    fn offset(&self) -> usize {
+        self.config.ext_address.is_some() as usize
+    }
+
+    fn can_max_dlen(&self) -> usize {
+        CAN_MAX_DLEN - self.offset()
+    }
+
+    fn can_fd_max_dlen(&self) -> usize {
+        CAN_FD_MAX_DLEN - self.offset()
+    }
+
     fn max_can_data_length(&self) -> usize {
         if self.config.fd {
-            CAN_FD_MAX_DLEN
+            self.can_fd_max_dlen()
         } else {
-            CAN_MAX_DLEN
+            self.can_max_dlen()
         }
     }
 
@@ -151,6 +170,12 @@ impl<'a> IsoTPAdapter<'a> {
     }
 
     fn frame(&self, data: &[u8]) -> Result<Frame, Error> {
+        let mut data = data.to_vec();
+
+        if let Some(ext_address) = self.config.ext_address {
+            data.insert(0, ext_address);
+        }
+
         // Check if the data length is valid
         if !DLC_TO_LEN.contains(&data.len()) {
             println!("len {}", data.len());
@@ -160,7 +185,7 @@ impl<'a> IsoTPAdapter<'a> {
         let frame = Frame {
             bus: self.config.bus,
             id: self.config.tx_id,
-            data: data.to_vec(),
+            data,
             loopback: false,
             fd: self.config.fd,
         };
@@ -170,7 +195,8 @@ impl<'a> IsoTPAdapter<'a> {
     pub async fn send_single_frame(&self, data: &[u8]) -> Result<(), Error> {
         let mut buf;
 
-        if data.len() < CAN_MAX_DLEN {
+        if data.len() < 0xf {
+            // Len fits in single nibble
             buf = vec![FrameType::Single as u8 | data.len() as u8];
         } else {
             // Use escape sequence for length, length is in the next byte
@@ -230,7 +256,11 @@ impl<'a> IsoTPAdapter<'a> {
         stream: &mut std::pin::Pin<&mut Timeout<impl Stream<Item = Frame>>>,
     ) -> Result<FlowControlConfig, Error> {
         for _ in 0..MAX_WAIT_FC {
-            let frame = stream.next().await.unwrap()?;
+            let mut frame = stream.next().await.unwrap()?;
+
+            // Remove extended address from frame
+            frame.data = frame.data.split_off(self.offset());
+
             debug!("RX FC, data {}", hex::encode(&frame.data));
 
             // Check if Flow Control
@@ -263,7 +293,17 @@ impl<'a> IsoTPAdapter<'a> {
         // Stream for receiving flow control
         let stream = self
             .adapter
-            .recv_filter(|frame| frame.id == self.config.rx_id && !frame.loopback)
+            .recv_filter(|frame| {
+                if frame.id != self.config.rx_id || frame.loopback {
+                    return false;
+                }
+
+                if self.config.ext_address.is_some() {
+                    return frame.data.first() == self.config.ext_address.as_ref();
+                }
+
+                true
+            })
             .timeout(self.config.timeout);
         tokio::pin!(stream);
 
@@ -304,7 +344,7 @@ impl<'a> IsoTPAdapter<'a> {
 
         // Single frame has 1 byte of overhead for CAN, and 2 bytes for CAN-FD with escape sequence
         let fits_in_single_frame =
-            data.len() < CAN_MAX_DLEN || data.len() < self.max_can_data_length() - 1;
+            data.len() < self.can_max_dlen() || data.len() < self.max_can_data_length() - 1;
 
         if fits_in_single_frame {
             self.send_single_frame(data).await?;
@@ -421,7 +461,10 @@ impl<'a> IsoTPAdapter<'a> {
         let mut idx: u8 = 1;
 
         while let Some(frame) = stream.next().await {
-            let frame = frame?;
+            // Remove extended address from frame
+            let mut frame = frame?;
+            frame.data = frame.data.split_off(self.offset());
+
             match FrameType::from_repr(frame.data[0] & FRAME_TYPE_MASK) {
                 Some(FrameType::Single) => {
                     return self.recv_single_frame(&frame).await;
@@ -458,7 +501,17 @@ impl<'a> IsoTPAdapter<'a> {
     pub fn recv(&self) -> impl Stream<Item = Result<Vec<u8>, Error>> + '_ {
         let stream = self
             .adapter
-            .recv_filter(|frame| frame.id == self.config.rx_id && !frame.loopback)
+            .recv_filter(|frame| {
+                if frame.id != self.config.rx_id || frame.loopback {
+                    return false;
+                }
+
+                if self.config.ext_address.is_some() {
+                    return frame.data.first() == self.config.ext_address.as_ref();
+                }
+
+                true
+            })
             .timeout(self.config.timeout);
 
         Box::pin(stream! {

tests/isotp_tests.rs

@@ -20,6 +20,7 @@ struct VECUConfig {
     pub bs: u32,
     pub padding: Option<u8>,
     pub fd: bool,
+    pub ext_address: Option<u8>,
 }
 
 impl VECUConfig {
@@ -37,6 +38,11 @@ impl VECUConfig {
             result.push(format!("{}", padding));
         }
 
+        if let Some(ext_address) = self.ext_address {
+            result.push("--ext-address".to_owned());
+            result.push(format!("{}", ext_address));
+        }
+
         if self.fd {
             result.push("--fd".to_owned());
         }
@@ -69,6 +75,7 @@ async fn isotp_test_echo(msg_len: usize, config: VECUConfig) {
     let mut isotp_config = IsoTPConfig::new(0, Identifier::Standard(0x7a1));
     isotp_config.padding = config.padding;
     isotp_config.fd = config.fd;
+    isotp_config.ext_address = config.ext_address;
 
     let isotp = IsoTPAdapter::new(&adapter, isotp_config);
 
@@ -164,3 +171,44 @@ async fn isotp_test_fd() {
     // First frame escape
     isotp_test_echo(5000, config).await;
 }
+
+#[cfg(feature = "test_vcan")]
+#[tokio::test]
+#[serial_test::serial]
+async fn isotp_test_extended() {
+    let config = VECUConfig {
+        ext_address: Some(0xff),
+        ..Default::default()
+    };
+    // Single frame
+    isotp_test_echo(1, config).await;
+    isotp_test_echo(7, config).await;
+    // Flow control
+    isotp_test_echo(62, config).await; // No padding on last CF
+    isotp_test_echo(64, config).await;
+    // Overflow IDX in flow control
+    isotp_test_echo(256, config).await;
+}
+
+#[cfg(feature = "test_vcan")]
+#[tokio::test]
+#[serial_test::serial]
+async fn isotp_test_fd_extended() {
+    let config = VECUConfig {
+        fd: true,
+        ext_address: Some(0xff),
+        ..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;
+}