diff --git a/src/socketcan/frame.rs b/src/socketcan/frame.rs index c8e8623..d9d0369 100644 --- a/src/socketcan/frame.rs +++ b/src/socketcan/frame.rs @@ -1,6 +1,7 @@ use bitflags::bitflags; use libc::{ - can_frame, canfd_frame, canid_t, CANFD_BRS, CANFD_ESI, CAN_EFF_FLAG, CAN_ERR_FLAG, CAN_RTR_FLAG, + can_frame, canfd_frame, canid_t, CANFD_BRS, CANFD_ESI, CANFD_MAX_DLEN, CAN_EFF_FLAG, + CAN_ERR_FLAG, CAN_MAX_DLC, CAN_RTR_FLAG, }; use crate::can::{Frame, Identifier}; @@ -35,10 +36,9 @@ bitflags! { } } -fn id_to_canid_t(id: impl Into) -> canid_t { - let id = id.into(); +fn id_to_canid_t(id: Identifier) -> canid_t { match id { - Identifier::Standard(id) => id as canid_t, + Identifier::Standard(id) => id, Identifier::Extended(id) => id | CAN_EFF_FLAG, } } @@ -71,3 +71,32 @@ impl From for Frame { .unwrap() } } + +impl From for can_frame { + fn from(frame: Frame) -> can_frame { + assert!(!frame.fd); + assert!(frame.data.len() <= CAN_MAX_DLC as usize); + + let mut raw_frame = can_frame_default(); + raw_frame.can_id = id_to_canid_t(frame.id); + raw_frame.can_dlc = frame.data.len() as u8; + raw_frame.data[..frame.data.len()].copy_from_slice(&frame.data); + + raw_frame + } +} + +impl From for canfd_frame { + fn from(frame: Frame) -> canfd_frame { + assert!(frame.fd); + assert!(frame.data.len() <= CANFD_MAX_DLEN); + + let mut raw_frame = canfd_frame_default(); + raw_frame.can_id = id_to_canid_t(frame.id); + raw_frame.len = frame.data.len() as u8; + // TODO: Set flags like BRS + raw_frame.data[..frame.data.len()].copy_from_slice(&frame.data); + + raw_frame + } +} diff --git a/src/socketcan/mod.rs b/src/socketcan/mod.rs index b456198..c3f5426 100644 --- a/src/socketcan/mod.rs +++ b/src/socketcan/mod.rs @@ -44,6 +44,7 @@ impl SocketCan { Err(_) => return Err(crate::error::Error::NotFound), }; + socket.set_fd_mode(true).unwrap(); socket.set_nonblocking(true).unwrap(); socket.set_loopback(true).unwrap(); @@ -79,23 +80,20 @@ impl SocketCan { impl CanAdapter for SocketCan { fn send(&mut self, frames: &mut VecDeque) -> Result<()> { - unimplemented!(); - // while let Some(frame) = frames.pop_front() { - // let to_send: socketcan::frame::CanAnyFrame = frame.clone().into(); - - // if self.socket.write_frame(&to_send).is_err() { - // // Failed to send frame, push it back to the front of the queue for next send call - // frames.push_front(frame); - // break; - // } else if !self.iff_echo { - // // If IFF_ECHO is not set, we need to emulate the ACK logic. - // let mut frame = frame.clone(); - // frame.loopback = true; - // self.loopback_queue.push_back(frame); - // } - // } - - // Ok(()) + while let Some(frame) = frames.pop_front() { + if self.socket.write_frame(frame.clone()).is_err() { + // Failed to send frame, push it back to the front of the queue for next send call + frames.push_front(frame); + break; + } else if !self.iff_echo { + // If IFF_ECHO is not set, we need to emulate the ACK logic. + let mut frame = frame.clone(); + frame.loopback = true; + self.loopback_queue.push_back(frame); + } + } + + Ok(()) } fn recv(&mut self) -> Result> { diff --git a/src/socketcan/socket.rs b/src/socketcan/socket.rs index 4e0d61b..9922c0f 100644 --- a/src/socketcan/socket.rs +++ b/src/socketcan/socket.rs @@ -1,10 +1,11 @@ //! 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, CANFD_MTU, CAN_MTU, CAN_RAW, - CAN_RAW_LOOPBACK, SOL_CAN_RAW, + c_int, c_void, can_frame, canfd_frame, sa_family_t, sockaddr_can, socklen_t, AF_CAN, CANFD_MTU, + CAN_MTU, CAN_RAW, CAN_RAW_FD_FRAMES, CAN_RAW_LOOPBACK, SOL_CAN_RAW, }; use nix::net::if_::if_nametoindex; +use std::io::Write; use std::os::fd::AsRawFd; use crate::can::Frame; @@ -43,6 +44,21 @@ impl CanFdSocket { Ok(Self(sock)) } + pub fn write_frame(&self, frame: Frame) -> std::io::Result<()> { + match frame.fd { + true => { + let frame = canfd_frame::from(frame); + let bytes = as_bytes(&frame); + self.as_raw_socket().write_all(&bytes) + } + false => { + let frame = can_frame::from(frame); + let bytes = as_bytes(&frame); + self.as_raw_socket().write_all(&bytes) + } + } + } + pub fn read_frame(&self) -> std::io::Result { let mut frame = Vec::with_capacity(CANFD_MTU); @@ -90,6 +106,11 @@ impl CanFdSocket { } } + pub fn set_fd_mode(&self, enabled: bool) -> std::io::Result<()> { + let enable = c_int::from(enabled); + self.set_socket_option(SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable) + } + pub fn set_nonblocking(&self, nonblocking: bool) -> std::io::Result<()> { self.as_raw_socket().set_nonblocking(nonblocking) } diff --git a/tests/adapter_tests.rs b/tests/adapter_tests.rs index 49e905d..420eb52 100644 --- a/tests/adapter_tests.rs +++ b/tests/adapter_tests.rs @@ -109,7 +109,6 @@ async fn socketcan_bulk_send_async() { #[test] #[serial_test::serial] fn vcan_bulk_send_sync() { - use socketcan::Socket; let mut adapter = automotive::socketcan::SocketCan::new("vcan0").unwrap(); bulk_send_sync(&mut adapter); }