diff --git a/scripts/vecu_isotp.py b/scripts/vecu_isotp.py index 98788a8..989a0ae 100755 --- a/scripts/vecu_isotp.py +++ b/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: diff --git a/src/isotp/mod.rs b/src/isotp/mod.rs index e09d3b7..beaf1d8 100644 --- a/src/isotp/mod.rs +++ b/src/isotp/mod.rs @@ -60,6 +60,8 @@ pub struct IsoTPConfig { pub separation_time_min: Option, /// Enable CAN-FD Mode pub fd: bool, + /// Extended address + pub ext_address: Option, } 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) { + // 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 { + 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>>, ) -> Result { 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, 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! { diff --git a/tests/isotp_tests.rs b/tests/isotp_tests.rs index 3df731b..fb7976d 100644 --- a/tests/isotp_tests.rs +++ b/tests/isotp_tests.rs @@ -20,6 +20,7 @@ struct VECUConfig { pub bs: u32, pub padding: Option, pub fd: bool, + pub ext_address: Option, } 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; +}