diff --git a/src/can.rs b/src/can.rs index b7b7b1c..3a3def9 100644 --- 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))] @@ -69,15 +71,30 @@ pub struct Frame { impl Unpin for Frame {} impl Frame { - pub fn new(bus: u8, id: Identifier, data: &[u8]) -> Frame { - // TODO: Check if data has a valid length (check DLC table) - Frame { + pub fn new(bus: u8, id: Identifier, data: &[u8]) -> Result { + // 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, - } + }) } } @@ -88,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/isotp/mod.rs b/src/isotp/mod.rs index b0b4c19..c79bb0e 100644 --- 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 { @@ -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/tests/adapter_tests.rs b/tests/adapter_tests.rs index d311039..3c01349 100644 --- a/tests/adapter_tests.rs +++ b/tests/adapter_tests.rs @@ -15,7 +15,7 @@ fn bulk_send_sync(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)); @@ -122,5 +122,7 @@ async fn vcan_bulk_send_async() { #[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])).await; + adapter + .send(&Frame::new(0, 0x123.into(), &[0u8; 64]).unwrap()) + .await; }