implement receive

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"

src/can/adapter.rs

@@ -9,8 +9,10 @@ pub fn get_adapter() -> Result<crate::can::AsyncCanAdapter, crate::error::Error>
     #[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);
+            }
         }
     }
 

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<Identifier>) -> 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<can_frame> 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<canfd_frame> 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()
+    }
+}

src/socketcan/mod.rs

@@ -99,29 +99,26 @@ impl CanAdapter for SocketCan {
     }
 
     fn recv(&mut self) -> Result<Vec<Frame>> {
-        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)
     }
 }

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