Support sending CAN FD frames (#44)

* Add support for FD frames * check if frame data has correct length
I-CAN-hack · Mar 19, 2024 · 378acc3 · 378acc3
1 parent 9ef3eec
commit 378acc3
Show file tree

Hide file tree

Showing 6 changed files with 104 additions and 21 deletions.
diff --git a/src/can.rs b/src/can.rs
@@ -2,6 +2,8 @@
 
 use std::fmt;
 
+static DLC_TO_LEN: &[usize] = &[0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64];
+
 /// Identifier for a CAN frame
 #[derive(Copy, Clone, PartialOrd, Eq, PartialEq, Hash)]
 #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
@@ -62,18 +64,37 @@ pub struct Frame {
     pub data: Vec<u8>,
     /// Wheter the frame was sent out by the adapter
     pub loopback: bool,
-    // TODO: Add timestamp, can-fd, rtr, dlc
+    /// CAN-FD Frame
+    pub fd: bool,
+    // TODO: Add timestamp, rtr, dlc
 }
 impl Unpin for Frame {}
 
 impl Frame {
-    pub fn new(bus: u8, id: Identifier, data: &[u8]) -> Frame {
-        Frame {
+    pub fn new(bus: u8, id: Identifier, data: &[u8]) -> Result<Frame, crate::error::Error> {
+        // Check if the data length is valid
+        if !DLC_TO_LEN.contains(&data.len()) {
+            return Err(crate::error::Error::MalformedFrame);
+        }
+
+        // Check if the ID makes sense
+        match id {
+            Identifier::Standard(id) if id > 0x7ff => {
+                return Err(crate::error::Error::MalformedFrame)
+            }
+            Identifier::Extended(id) if id > 0x1fffffff => {
+                return Err(crate::error::Error::MalformedFrame)
+            }
+            _ => {}
+        };
+
+        Ok(Frame {
             bus,
             id,
             data: data.to_vec(),
             loopback: false,
-        }
+            fd: data.len() > 8,
+        })
     }
 }
 
@@ -84,6 +105,7 @@ impl fmt::Debug for Frame {
             .field("id", &self.id)
             .field("data", &hex::encode(&self.data))
             .field("loopback", &self.loopback)
+            .field("fd", &self.fd)
             .finish()
     }
 }

diff --git a/src/error.rs b/src/error.rs
@@ -6,6 +6,8 @@ use thiserror::Error;
 pub enum Error {
     #[error("Not Found")]
     NotFound,
+    #[error("Not Supported")]
+    NotSupported,
     #[error("Malformed Frame")]
     MalformedFrame,
     #[error("Timeout")]

diff --git a/src/isotp/mod.rs b/src/isotp/mod.rs
@@ -115,19 +115,19 @@ impl<'a> IsoTPAdapter<'a> {
             data.extend(std::iter::repeat(padding).take(len));
         }
     }
-
-    pub async fn send_single_frame(&self, data: &[u8]) {
+    pub async fn send_single_frame(&self, data: &[u8]) -> Result<(), Error> {
         let mut buf = vec![FrameType::Single as u8 | data.len() as u8];
         buf.extend(data);
         self.pad(&mut buf);
 
         debug!("TX SF, length: {} data {}", data.len(), hex::encode(&buf));
 
-        let frame = Frame::new(self.config.bus, self.config.tx_id, &buf);
+        let frame = Frame::new(self.config.bus, self.config.tx_id, &buf)?;
         self.adapter.send(&frame).await;
+        Ok(())
     }
 
-    pub async fn send_first_frame(&self, data: &[u8]) {
+    pub async fn send_first_frame(&self, data: &[u8]) -> Result<(), Error> {
         let b0: u8 = FrameType::First as u8 | ((data.len() >> 8) & 0xF) as u8;
         let b1: u8 = (data.len() & 0xFF) as u8;
 
@@ -136,11 +136,12 @@ impl<'a> IsoTPAdapter<'a> {
 
         debug!("TX FF, length: {} data {}", data.len(), hex::encode(&buf));
 
-        let frame = Frame::new(self.config.bus, self.config.tx_id, &buf);
+        let frame = Frame::new(self.config.bus, self.config.tx_id, &buf)?;
         self.adapter.send(&frame).await;
+        Ok(())
     }
 
-    pub async fn send_consecutive_frame(&self, data: &[u8], idx: usize) {
+    pub async fn send_consecutive_frame(&self, data: &[u8], idx: usize) -> Result<(), Error> {
         let idx = ((idx + 1) & 0xF) as u8;
 
         let mut buf = vec![FrameType::Consecutive as u8 | idx];
@@ -149,8 +150,10 @@ impl<'a> IsoTPAdapter<'a> {
 
         debug!("TX CF, idx: {} data {}", idx, hex::encode(&buf));
 
-        let frame = Frame::new(self.config.bus, self.config.tx_id, &buf);
+        let frame = Frame::new(self.config.bus, self.config.tx_id, &buf)?;
         self.adapter.send(&frame).await;
+
+        Ok(())
     }
 
     fn receive_flow_control(&self, frame: &Frame) -> Result<FlowControlConfig, Error> {
@@ -182,7 +185,7 @@ impl<'a> IsoTPAdapter<'a> {
             .timeout(self.config.timeout);
         tokio::pin!(stream);
 
-        self.send_first_frame(data).await;
+        self.send_first_frame(data).await?;
         let frame = stream.next().await.unwrap()?;
         let mut fc_config = self.receive_flow_control(&frame)?;
 
@@ -197,7 +200,7 @@ impl<'a> IsoTPAdapter<'a> {
         let chunks = data[self.config.tx_dl - 2..].chunks(self.config.tx_dl - 1);
         let mut it = chunks.enumerate().peekable();
         while let Some((idx, chunk)) = it.next() {
-            self.send_consecutive_frame(chunk, idx).await;
+            self.send_consecutive_frame(chunk, idx).await?;
 
             // Wait for flow control every `block_size` frames, except for the first frame
             if fc_config.block_size != 0 && idx > 0 && idx % fc_config.block_size as usize == 0 {
@@ -221,7 +224,7 @@ impl<'a> IsoTPAdapter<'a> {
         debug!("TX {}", hex::encode(data));
 
         if data.len() < self.config.tx_dl {
-            self.send_single_frame(data).await;
+            self.send_single_frame(data).await?;
         } else if data.len() <= 4095 {
             self.send_multiple(data).await?;
         } else {
@@ -269,7 +272,7 @@ impl<'a> IsoTPAdapter<'a> {
 
         debug!("TX FC, data {}", hex::encode(&flow_control));
 
-        let frame = Frame::new(self.config.bus, self.config.tx_id, &flow_control);
+        let frame = Frame::new(self.config.bus, self.config.tx_id, &flow_control)?;
         self.adapter.send(&frame).await;
 
         Ok(len)

diff --git a/src/panda/usb_protocol.rs b/src/panda/usb_protocol.rs
@@ -47,6 +47,10 @@ pub fn pack_can_buffer(frames: &[Frame]) -> Result<Vec<Vec<u8>>, Error> {
             return Err(Error::MalformedFrame);
         }
 
+        if frame.fd {
+            return Err(Error::NotSupported);
+        }
+
         let dlc = DLC_TO_LEN.iter().position(|&x| x == frame.data.len());
         let dlc = dlc.ok_or(Error::MalformedFrame)? as u8;
 
@@ -111,11 +115,16 @@ pub fn unpack_can_buffer(dat: &mut Vec<u8>) -> Result<Vec<Frame>, Error> {
             ));
         }
 
+        // Panda doesn't properly communicate if a frame was FD or not.
+        // We'll assume it was FD when the data length is > 8.
+        let fd = data_len > 8;
+
         ret.push(Frame {
             id,
             bus,
             data: dat[CANPACKET_HEAD_SIZE..(CANPACKET_HEAD_SIZE + data_len)].to_vec(),
             loopback: returned,
+            fd,
         });
 
         dat.drain(0..(CANPACKET_HEAD_SIZE + data_len));
@@ -171,12 +180,14 @@ mod tests {
                 id: Identifier::Standard(0x123),
                 data: vec![1, 2, 3, 4, 5, 6, 7, 8],
                 loopback: false,
+                fd: false,
             },
             Frame {
                 bus: 1,
                 id: Identifier::Extended(0x123),
                 data: vec![1, 2, 3, 4],
                 loopback: false,
+                fd: false,
             },
         ];
 
@@ -194,6 +205,7 @@ mod tests {
             id: Identifier::Standard(0x123),
             data: vec![1, 2, 3, 4, 5, 6, 7, 8, 9],
             loopback: false,
+            fd: false,
         }];
         let r = pack_can_buffer(&frames);
         assert_eq!(r, Err(Error::MalformedFrame));
@@ -206,6 +218,7 @@ mod tests {
             id: Identifier::Standard(0xfff),
             data: vec![1, 2, 3, 4, 5, 6, 7, 8],
             loopback: false,
+            fd: false,
         }];
         let r = pack_can_buffer(&frames);
         assert_eq!(r, Err(Error::MalformedFrame));

diff --git a/src/socketcan/frame.rs b/src/socketcan/frame.rs
@@ -17,6 +17,7 @@ impl From<socketcan::frame::CanDataFrame> for crate::can::Frame {
             id: frame.id().into(),
             data: frame.data().to_vec(),
             loopback: false,
+            fd: false,
         }
     }
 }
@@ -28,16 +29,22 @@ impl From<socketcan::frame::CanFdFrame> for crate::can::Frame {
             id: frame.id().into(),
             data: frame.data().to_vec(),
             loopback: false,
+            fd: true,
         }
     }
 }
 
 impl From<crate::can::Frame> for socketcan::frame::CanAnyFrame {
     fn from(frame: crate::can::Frame) -> Self {
-        // TODO: Check if source is FD frame
-        socketcan::frame::CanAnyFrame::Normal(
-            socketcan::frame::CanDataFrame::new(frame.id, &frame.data).unwrap(),
-        )
+        let id: socketcan::Id = frame.id.into();
+        match frame.fd {
+            true => socketcan::frame::CanAnyFrame::Fd(
+                socketcan::frame::CanFdFrame::new(id, &frame.data).unwrap(),
+            ),
+            false => socketcan::frame::CanAnyFrame::Normal(
+                socketcan::frame::CanDataFrame::new(id, &frame.data).unwrap(),
+            ),
+        }
     }
 }
 

diff --git a/tests/adapter_tests.rs b/tests/adapter_tests.rs
@@ -15,7 +15,7 @@ fn bulk_send_sync<T: CanAdapter>(adapter: &mut T) {
     let mut frames = vec![];
 
     for i in 0..BULK_NUM_FRAMES {
-        frames.push(Frame::new(0, 0x123.into(), &i.to_be_bytes()));
+        frames.push(Frame::new(0, 0x123.into(), &i.to_be_bytes()).unwrap());
     }
 
     adapter.send(&frames).unwrap();
@@ -45,7 +45,7 @@ async fn bulk_send(adapter: &AsyncCanAdapter) {
     let mut frames = vec![];
 
     for i in 0..BULK_NUM_FRAMES {
-        frames.push(Frame::new(0, 0x123.into(), &i.to_be_bytes()));
+        frames.push(Frame::new(0, 0x123.into(), &i.to_be_bytes()).unwrap());
     }
 
     let r = frames.iter().map(|frame| adapter.send(frame));
@@ -90,3 +90,39 @@ async fn socketcan_bulk_send_async() {
     let adapter = automotive::socketcan::SocketCan::new_async_from_name("can0").unwrap();
     bulk_send(&adapter).await;
 }
+
+// #[cfg(feature = "test_socketcan")]
+// #[tokio::test]
+// #[serial_test::serial]
+// async fn vcan_bulk_send_fd() {
+//     let adapter = automotive::socketcan::SocketCan::new_async_from_name("can0").unwrap();
+//     adapter.send(&Frame::new(0, 0x123.into(), &[0u8; 64])).await;
+// }
+
+#[cfg(feature = "test_vcan")]
+#[test]
+#[serial_test::serial]
+fn vcan_bulk_send_sync() {
+    use socketcan::Socket;
+    let socket = socketcan::CanFdSocket::open("vcan0").unwrap();
+    let mut adapter = automotive::socketcan::SocketCan::new(socket);
+    bulk_send_sync(&mut adapter);
+}
+
+#[cfg(feature = "test_vcan")]
+#[tokio::test]
+#[serial_test::serial]
+async fn vcan_bulk_send_async() {
+    let adapter = automotive::socketcan::SocketCan::new_async_from_name("vcan0").unwrap();
+    bulk_send(&adapter).await;
+}
+
+#[cfg(feature = "test_vcan")]
+#[tokio::test]
+#[serial_test::serial]
+async fn vcan_bulk_send_fd() {
+    let adapter = automotive::socketcan::SocketCan::new_async_from_name("vcan0").unwrap();
+    adapter
+        .send(&Frame::new(0, 0x123.into(), &[0u8; 64]).unwrap())
+        .await;
+}