From 0d94fb7bccda7b2d646413405f15d4af5b4f9c40 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 15 Mar 2024 15:37:57 +0100 Subject: [PATCH] parse out flow control config --- src/error.rs | 6 ++-- src/isotp/constants.rs | 26 ++++++++--------- src/isotp/error.rs | 2 ++ src/isotp/mod.rs | 66 +++++++++++++++++++++++++++++------------- src/isotp/types.rs | 30 +++++++++++++++++++ 5 files changed, 93 insertions(+), 37 deletions(-) create mode 100644 src/isotp/types.rs diff --git a/src/error.rs b/src/error.rs index 7b828d8..32cc514 100644 --- a/src/error.rs +++ b/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 for Error { diff --git a/src/isotp/constants.rs b/src/isotp/constants.rs index cc87e19..3c94723 100644 --- a/src/isotp/constants.rs +++ b/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 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, } diff --git a/src/isotp/error.rs b/src/isotp/error.rs index 22b7570..486913e 100644 --- a/src/isotp/error.rs +++ b/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")] diff --git a/src/isotp/mod.rs b/src/isotp/mod.rs index b745269..53fee00 100644 --- a/src/isotp/mod.rs +++ b/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, 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,7 +225,7 @@ impl<'a> IsoTPAdapter<'a> { async fn recv_first_frame( &self, - frame: Frame, + frame: &Frame, buf: &mut Vec, len: &mut usize, ) -> Result<(), Error> { @@ -211,6 +233,10 @@ impl<'a> IsoTPAdapter<'a> { 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, 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()); } }; diff --git a/src/isotp/types.rs b/src/isotp/types.rs new file mode 100644 index 0000000..e8f68cc --- /dev/null +++ b/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 { + 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, + }) + } +}