parse out flow control config

src/error.rs

@@ -11,13 +11,13 @@ pub enum Error {
     #[error("Timeout")]
     Timeout,
     #[error(transparent)]
-    IsoTPError(crate::isotp::error::Error),
+    IsoTPError(#[from] crate::isotp::error::Error),
     #[error(transparent)]
     LibUsbError(#[from] rusb::Error),
     #[error(transparent)]
-    PandaError(crate::panda::error::Error),
+    PandaError(#[from] crate::panda::error::Error),
     #[error(transparent)]
-    UDSError(crate::uds::error::Error),
+    UDSError(#[from] crate::uds::error::Error),
 }
 
 impl From<tokio_stream::Elapsed> for Error {

src/isotp/constants.rs

@@ -1,23 +1,21 @@
-#[derive(Debug, PartialEq, Copy, Clone)]
+use strum_macros::FromRepr;
+
+pub static FRAME_TYPE_MASK: u8 = 0xf0;
+pub static FLOW_SATUS_MASK: u8 = 0x0f;
+
+#[derive(Debug, PartialEq, Copy, Clone, FromRepr)]
 #[repr(u8)]
 pub enum FrameType {
     Single = 0x00,
     First = 0x10,
     Consecutive = 0x20,
     FlowControl = 0x30,
-    Unknown = 0xff,
 }
 
-pub static FRAME_TYPE_MASK: u8 = 0xf0;
-
-impl From<u8> for FrameType {
-    fn from(val: u8) -> FrameType {
-        match val {
-            0x00 => FrameType::Single,
-            0x10 => FrameType::First,
-            0x20 => FrameType::Consecutive,
-            0x30 => FrameType::FlowControl,
-            _ => FrameType::Unknown,
-        }
-    }
+#[derive(Debug, PartialEq, Copy, Clone, FromRepr)]
+#[repr(u8)]
+pub enum FlowStatus {
+    ContinueToSend = 0x0,
+    Wait = 0x1,
+    Overflow = 0x2,
 }

src/isotp/error.rs

@@ -8,6 +8,8 @@ pub enum Error {
     DataTooLarge,
     #[error("Flow Control")]
     FlowControl,
+    #[error("Overflow")]
+    Overflow,
     #[error("Out Of Order")]
     OutOfOrder,
     #[error("Unknown Frame Type")]

src/isotp/mod.rs

@@ -14,11 +14,14 @@
 
 pub mod constants;
 pub mod error;
+pub mod types;
 
 use crate::async_can::AsyncCanAdapter;
 use crate::can::Frame;
 use crate::can::Identifier;
 use crate::error::Error;
+use crate::isotp::constants::FlowStatus;
+use crate::isotp::constants::FLOW_SATUS_MASK;
 use crate::isotp::constants::{FrameType, FRAME_TYPE_MASK};
 
 use async_stream::stream;
@@ -143,6 +146,28 @@ impl<'a> IsoTPAdapter<'a> {
         self.adapter.send(&frame).await;
     }
 
+    fn receive_flow_control(&self, frame: &Frame) -> Result<(), Error> {
+        // Check if Flow Control
+        if FrameType::from_repr(frame.data[0] & FRAME_TYPE_MASK) != Some(FrameType::FlowControl) {
+            return Err(crate::isotp::error::Error::FlowControl.into());
+        };
+
+        // Check Flow Status
+        match FlowStatus::from_repr(frame.data[0] & FLOW_SATUS_MASK) {
+            Some(FlowStatus::ContinueToSend) => {} // Ok
+            Some(FlowStatus::Wait) => unimplemented!("Wait flow control not implemented"),
+            Some(FlowStatus::Overflow) => return Err(crate::isotp::error::Error::Overflow.into()),
+            None => return Err(crate::isotp::error::Error::MalformedFrame.into()),
+        };
+
+        // Parse block size and separation time
+        let config = types::FlowControlConfig::try_from(frame)?;
+        println!("{:?}", config);
+
+        debug!("RX FC, data {}", hex::encode(&frame.data));
+        Ok(())
+    }
+
     async fn send_multiple(&self, data: &[u8]) -> Result<(), Error> {
         // Stream for receiving flow control
         let stream = self
@@ -153,10 +178,10 @@ impl<'a> IsoTPAdapter<'a> {
 
         self.send_first_frame(data).await;
         let frame = stream.next().await.unwrap()?;
-        if frame.data[0] & FRAME_TYPE_MASK != FrameType::FlowControl as u8 {
-            return Err(Error::IsoTPError(crate::isotp::error::Error::FlowControl));
-        };
+        self.receive_flow_control(&frame)?;
+
         debug!("RX FC, data {}", hex::encode(&frame.data));
+        println!("RX FC, data {}", hex::encode(&frame.data));
 
         let chunks = data[self.config.tx_dl - 2..].chunks(self.config.tx_dl - 1);
         for (idx, chunk) in chunks.enumerate() {
@@ -175,23 +200,20 @@ impl<'a> IsoTPAdapter<'a> {
         } else if data.len() <= 4095 {
             self.send_multiple(data).await?;
         } else {
-            return Err(Error::IsoTPError(crate::isotp::error::Error::DataTooLarge));
+            return Err(crate::isotp::error::Error::DataTooLarge.into());
         }
 
         Ok(())
     }
     async fn recv_single_frame(
         &self,
-        frame: Frame,
+        frame: &Frame,
         buf: &mut Vec<u8>,
         len: &mut usize,
     ) -> Result<(), Error> {
         *len = (frame.data[0] & 0xF) as usize;
         if *len == 0 {
-            // unimplemented!("CAN FD escape sequence for single frame not supported");
-            return Err(Error::IsoTPError(
-                crate::isotp::error::Error::MalformedFrame,
-            ));
+            unimplemented!("CAN FD escape sequence for single frame not supported");
         }
 
         debug!("RX SF, length: {} data {}", *len, hex::encode(&frame.data));
@@ -203,14 +225,18 @@ impl<'a> IsoTPAdapter<'a> {
 
     async fn recv_first_frame(
         &self,
-        frame: Frame,
+        frame: &Frame,
         buf: &mut Vec<u8>,
         len: &mut usize,
     ) -> Result<(), Error> {
         let b0 = frame.data[0] as u16;
         let b1 = frame.data[1] as u16;
         *len = ((b0 << 8 | b1) & 0xFFF) as usize;
 
+        if *len == 0 {
+            unimplemented!("CAN FD escape sequence for first frame not supported");
+        }
+
         debug!("RX FF, length: {}, data {}", *len, hex::encode(&frame.data));
 
         buf.extend(&frame.data[2..]);
@@ -229,7 +255,7 @@ impl<'a> IsoTPAdapter<'a> {
 
     async fn recv_consecutive_frame(
         &self,
-        frame: Frame,
+        frame: &Frame,
         buf: &mut Vec<u8>,
         len: &mut usize,
         idx: &mut u8,
@@ -247,7 +273,7 @@ impl<'a> IsoTPAdapter<'a> {
         );
 
         if msg_idx != *idx {
-            return Err(Error::IsoTPError(crate::isotp::error::Error::OutOfOrder));
+            return Err(crate::isotp::error::Error::OutOfOrder.into());
         }
 
         *idx = if *idx == 0xF { 0 } else { *idx + 1 };
@@ -266,17 +292,17 @@ impl<'a> IsoTPAdapter<'a> {
 
         while let Some(frame) = stream.next().await {
             let frame = frame?;
-            match (frame.data[0] & FRAME_TYPE_MASK).into() {
-                FrameType::Single => self.recv_single_frame(frame, &mut buf, &mut len).await?,
-                FrameType::First => self.recv_first_frame(frame, &mut buf, &mut len).await?,
-                FrameType::Consecutive => {
-                    self.recv_consecutive_frame(frame, &mut buf, &mut len, &mut idx)
+            match FrameType::from_repr(frame.data[0] & FRAME_TYPE_MASK) {
+                Some(FrameType::Single) => {
+                    self.recv_single_frame(&frame, &mut buf, &mut len).await?
+                }
+                Some(FrameType::First) => self.recv_first_frame(&frame, &mut buf, &mut len).await?,
+                Some(FrameType::Consecutive) => {
+                    self.recv_consecutive_frame(&frame, &mut buf, &mut len, &mut idx)
                         .await?
                 }
                 _ => {
-                    return Err(Error::IsoTPError(
-                        crate::isotp::error::Error::UnknownFrameType,
-                    ));
+                    return Err(crate::isotp::error::Error::UnknownFrameType.into());
                 }
             };
 

src/isotp/types.rs

@@ -0,0 +1,30 @@
+use crate::can::Frame;
+
+#[derive(Debug, Copy, Clone)]
+pub struct FlowControlConfig {
+    pub block_size: u8,
+    pub separation_time_min: std::time::Duration,
+}
+
+impl TryFrom<&Frame> for FlowControlConfig {
+    type Error = crate::error::Error;
+    fn try_from(frame: &Frame) -> Result<Self, Self::Error> {
+        if frame.data.len() < 3 {
+            return Err(crate::isotp::error::Error::MalformedFrame.into());
+        }
+
+        let block_size = frame.data[1];
+
+        let separation_time_min = frame.data[2] as u64;
+        let separation_time_min = match separation_time_min {
+            0x0..=0x7f => std::time::Duration::from_millis(separation_time_min),
+            0xf1..=0xf9 => std::time::Duration::from_micros((separation_time_min - 0xf0) * 100),
+            _ => return Err(crate::isotp::error::Error::MalformedFrame.into()),
+        };
+
+        Ok(Self {
+            block_size,
+            separation_time_min,
+        })
+    }
+}