implement sending

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<Identifier>) -> 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<canfd_frame> for Frame {
         .unwrap()
     }
 }
+
+impl From<Frame> 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<Frame> 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
+    }
+}

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<Frame>) -> 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<Vec<Frame>> {

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<Frame> {
         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)
     }

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);
 }