From 06c841224770fea01ca017672bd0f5b96c026a4b Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Sat, 11 May 2024 08:22:50 +0200 Subject: [PATCH] implement receive --- Cargo.lock | 1 + Cargo.toml | 1 + src/can/adapter.rs | 6 ++-- src/socketcan/frame.rs | 72 +++++++++++++++++++++++++++++++++++++++++ src/socketcan/mod.rs | 41 +++++++++++------------ src/socketcan/socket.rs | 55 +++++++++++++++++++++++++++++-- 6 files changed, 149 insertions(+), 27 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 93b3315..1a65704 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -50,6 +50,7 @@ name = "automotive" version = "0.1.4" dependencies = [ "async-stream", + "bitflags", "bstr", "futures", "hex", diff --git a/Cargo.toml b/Cargo.toml index 8299335..8b2adb2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -18,6 +18,7 @@ serde = ["dep:serde"] [dependencies] async-stream = "0.3.5" +bitflags = "2.5.0" bstr = "1.9.0" hex = "0.4" rusb = "0.9" diff --git a/src/can/adapter.rs b/src/can/adapter.rs index 282acdd..66f3668 100644 --- a/src/can/adapter.rs +++ b/src/can/adapter.rs @@ -9,8 +9,10 @@ pub fn get_adapter() -> Result #[cfg(target_os = "linux")] { // TODO: iterate over all available SocketCAN adapters to also find things like vcan0 - if let Ok(socket) = crate::socketcan::SocketCan::new_async("can0") { - return Ok(socket); + for iface in ["can0", "vcan0"] { + if let Ok(socket) = crate::socketcan::SocketCan::new_async(iface) { + return Ok(socket); + } } } diff --git a/src/socketcan/frame.rs b/src/socketcan/frame.rs index 8b13789..c8e8623 100644 --- a/src/socketcan/frame.rs +++ b/src/socketcan/frame.rs @@ -1 +1,73 @@ +use bitflags::bitflags; +use libc::{ + can_frame, canfd_frame, canid_t, CANFD_BRS, CANFD_ESI, CAN_EFF_FLAG, CAN_ERR_FLAG, CAN_RTR_FLAG, +}; +use crate::can::{Frame, Identifier}; + +#[inline(always)] +pub fn can_frame_default() -> can_frame { + unsafe { std::mem::zeroed() } +} + +#[inline(always)] +pub fn canfd_frame_default() -> canfd_frame { + unsafe { std::mem::zeroed() } +} + +bitflags! { + /// Bit flags in the composite SocketCAN ID word. + pub struct IdFlags: canid_t { + /// Indicates frame uses a 29-bit extended ID + const EFF = CAN_EFF_FLAG; + /// Indicates a remote request frame. + const RTR = CAN_RTR_FLAG; + /// Indicates an error frame. + const ERR = CAN_ERR_FLAG; + } + + /// Bit flags for the Flexible Data (FD) frames. + pub struct FdFlags: u8 { + /// Bit rate switch (second bit rate for payload data) + const BRS = CANFD_BRS as u8; + /// Error state indicator of the transmitting node + const ESI = CANFD_ESI as u8; + } +} + +fn id_to_canid_t(id: impl Into) -> canid_t { + let id = id.into(); + match id { + Identifier::Standard(id) => id as canid_t, + Identifier::Extended(id) => id | CAN_EFF_FLAG, + } +} + +fn canid_t_to_id(id: canid_t) -> Identifier { + match id & CAN_EFF_FLAG != 0 { + true => Identifier::Extended(id & 0x1fffffff), + false => Identifier::Standard(id & 0x7ff), + } +} + +impl From for Frame { + fn from(frame: can_frame) -> Self { + Self::new( + 0, + canid_t_to_id(frame.can_id), + &frame.data[..frame.can_dlc as usize], + ) + .unwrap() + } +} + +impl From for Frame { + fn from(frame: canfd_frame) -> Self { + Self::new( + 0, + canid_t_to_id(frame.can_id), + &frame.data[..frame.len as usize], + ) + .unwrap() + } +} diff --git a/src/socketcan/mod.rs b/src/socketcan/mod.rs index 06d7a38..b456198 100644 --- a/src/socketcan/mod.rs +++ b/src/socketcan/mod.rs @@ -99,29 +99,26 @@ impl CanAdapter for SocketCan { } fn recv(&mut self) -> Result> { - unimplemented!(); - // let mut frames = vec![]; - - // loop { - // match self.socket.read_frame_with_meta() { - // Ok((frame, meta)) => { - // let mut frame: crate::can::Frame = frame.into(); - // frame.loopback = meta.loopback; - // frames.push(frame); - // } - // Err(ref e) if e.kind() == std::io::ErrorKind::WouldBlock => { - // break; - // } - // Err(e) => { - // tracing::error!("Error reading frame: {}", e); - // return Err(crate::error::Error::Disconnected); - // } - // } - // } + let mut frames = vec![]; + + loop { + match self.socket.read_frame() { + Ok(frame) => { + frames.push(frame); + } + Err(ref e) if e.kind() == std::io::ErrorKind::WouldBlock => { + break; + } + Err(e) => { + tracing::error!("Error reading frame: {}", e); + return Err(crate::error::Error::Disconnected); + } + } + } - // // Add fake loopback frames to the receive queue - // frames.extend(self.loopback_queue.drain(..)); + // Add fake loopback frames to the receive queue + frames.extend(self.loopback_queue.drain(..)); - // Ok(frames) + Ok(frames) } } diff --git a/src/socketcan/socket.rs b/src/socketcan/socket.rs index 7667e96..4e0d61b 100644 --- a/src/socketcan/socket.rs +++ b/src/socketcan/socket.rs @@ -1,12 +1,14 @@ //! Low Level SocketCAN code //! Code based on https://github.com/socketcan-rs/socketcan-rs use libc::{ - c_int, c_void, sa_family_t, sockaddr_can, socklen_t, AF_CAN, CAN_RAW, CAN_RAW_LOOPBACK, PF_CAN, - SOL_CAN_RAW, + c_int, c_void, sa_family_t, sockaddr_can, socklen_t, AF_CAN, CANFD_MTU, CAN_MTU, CAN_RAW, + CAN_RAW_LOOPBACK, SOL_CAN_RAW, }; use nix::net::if_::if_nametoindex; use std::os::fd::AsRawFd; -use tokio::io; + +use crate::can::Frame; +use crate::socketcan::frame::{can_frame_default, canfd_frame_default}; pub struct CanFdSocket(socket2::Socket); @@ -41,6 +43,53 @@ impl CanFdSocket { Ok(Self(sock)) } + pub fn read_frame(&self) -> std::io::Result { + let mut frame = Vec::with_capacity(CANFD_MTU); + + let buf = socket2::MaybeUninitSlice::new(frame.spare_capacity_mut()); + let buf_slice = &mut [buf]; + + let mut header = socket2::MsgHdrMut::new().with_buffers(buf_slice); + + match self.as_raw_socket().recvmsg(&mut header, 0)? { + // If we only get 'can_frame' number of bytes, then the return is, + // by definition, a can_frame, so we just copy the bytes into the + // proper type. + CAN_MTU => { + let loopback = header.flags().is_confirm(); + + // SAFETY: just received CAN_MTU bytes + unsafe { + frame.set_len(CAN_MTU); + } + + let mut ret = can_frame_default(); + as_bytes_mut(&mut ret).copy_from_slice(&frame); + + let mut frame = Frame::from(ret); + frame.loopback = loopback; + Ok(frame) + } + CANFD_MTU => { + let loopback = header.flags().is_confirm(); + + // SAFETY: just received CANFD_MTU bytes + unsafe { + frame.set_len(CANFD_MTU); + } + + let mut ret = canfd_frame_default(); + as_bytes_mut(&mut ret).copy_from_slice(&frame); + + let mut frame = Frame::from(ret); + frame.fd = true; + frame.loopback = loopback; + Ok(frame) + } + _ => Err(std::io::Error::last_os_error()), + } + } + pub fn set_nonblocking(&self, nonblocking: bool) -> std::io::Result<()> { self.as_raw_socket().set_nonblocking(nonblocking) }