Merge pull request #27 from rnd-ash/socket-can-read-fix

Optimize socketCAN Reading behaviour

Cargo.toml

@@ -47,4 +47,4 @@ winreg = { version = "0.10.1", optional = true }
 shellexpand = { version = "2.1.0", optional = true }
 #socketcan-isotp = { version = "1.0.0", optional = true }
 socketcan-isotp = { optional = true, version = "1.0.1" }
-socketcan = { version = "1.7.0", optional = true }
+socketcan = { version = "2.0.0", optional = true }

src/channel.rs

@@ -9,6 +9,9 @@ use std::{
     sync::{mpsc, Arc, Mutex, PoisonError},
 };
 
+#[cfg(feature="socketcan")]
+use socketcan::{EmbeddedFrame, Id, ExtendedId, StandardId, CanDataFrame};
+
 use crate::hardware::HardwareError;
 
 /// Communication channel result
@@ -445,6 +448,28 @@ impl Packet for CanFrame {
     }
 }
 
+#[cfg(feature="socketcan")]
+impl From<CanDataFrame> for CanFrame {
+    fn from(value: CanDataFrame) -> Self {
+        let (id, ext) = match value.id() {
+            Id::Standard(id) => (id.as_raw() as u32, true),
+            Id::Extended(id) => (id.as_raw(), false),
+        };
+        CanFrame::new(id, value.data(), ext)
+    }
+}
+
+#[cfg(feature="socketcan")]
+impl Into<CanDataFrame> for CanFrame {
+    fn into(self) -> CanDataFrame {
+        let id = match self.ext {
+            true => Id::Extended(ExtendedId::new(self.get_address()).unwrap()),
+            false => Id::Standard(StandardId::new(self.get_address() as u16).unwrap())
+        };
+        CanDataFrame::new(id, self.get_data()).unwrap()
+    }
+}
+
 /// ISO-TP configuration options (ISO15765-2)
 #[derive(Debug, Copy, Clone)]
 #[repr(C)]

src/hardware/socketcan.rs

@@ -3,15 +3,19 @@
 use std::{
     path::PathBuf,
     sync::{Arc, atomic::{AtomicBool, Ordering}},
+    io::ErrorKind, borrow::BorrowMut,
     time::{Instant, Duration},
 };
 
+use socketcan::Socket;
+
+use socketcan::CanFrame as SocketCanCanFrame;
 use socketcan_isotp::{
     ExtendedId, FlowControlOptions, Id, IsoTpBehaviour, IsoTpOptions, LinkLayerOptions, StandardId,
 };
 
 use crate::channel::{
-    CanChannel, CanFrame, ChannelError, ChannelResult, IsoTPChannel, IsoTPSettings, Packet,
+    CanChannel, CanFrame, ChannelError, ChannelResult, IsoTPChannel, IsoTPSettings,
     PacketChannel, PayloadChannel,
 };
 
@@ -100,16 +104,16 @@ impl Hardware for SocketCanDevice {
 /// SocketCAN CAN channel
 pub struct SocketCanCanChannel {
     device: SocketCanDevice,
-    channel: Option<socketcan::CANSocket>,
+    channel: Option<socketcan::CanSocket>,
 }
 
 impl SocketCanCanChannel {
-    fn safe_with_iface<X, T: FnOnce(&socketcan::CANSocket) -> ChannelResult<X>>(
+    fn safe_with_iface<X, T: FnOnce(&mut socketcan::CanSocket) -> ChannelResult<X>>(
         &mut self,
         function: T,
     ) -> ChannelResult<X> {
-        match self.channel {
-            Some(ref channel) => function(channel),
+        match self.channel.borrow_mut() {
+            Some(channel) => function(channel),
             None => Err(ChannelError::InterfaceNotOpen),
         }
     }
@@ -120,9 +124,11 @@ impl PacketChannel<CanFrame> for SocketCanCanChannel {
         if self.channel.is_some() {
             return Ok(()); // Already open!
         }
-        let channel = socketcan::CANSocket::open(&self.device.info.name)?;
-        channel.filter_accept_all()?;
-        channel.set_nonblocking(false)?;
+        let channel = socketcan::CanSocket::open(&self.device.info.name)?;
+
+        channel.set_error_filter_drop_all()?;
+        channel.set_filter_accept_all()?;
+
         self.channel = Some(channel);
         self.device.canbus_active.store(true, Ordering::Relaxed);
         Ok(())
@@ -139,38 +145,65 @@ impl PacketChannel<CanFrame> for SocketCanCanChannel {
 
     fn write_packets(&mut self, packets: Vec<CanFrame>, timeout_ms: u32) -> ChannelResult<()> {
         self.safe_with_iface(|iface| {
-            iface.set_write_timeout(std::time::Duration::from_millis(timeout_ms as u64))?;
-            let mut cf: socketcan::CANFrame;
-            for p in &packets {
-                cf = socketcan::CANFrame::new(p.get_address(), p.get_data(), false, false).unwrap();
+            if timeout_ms == 0 {
+                iface.set_nonblocking(true)?;
+            } else {
+                iface.set_nonblocking(false)?;
+                iface.set_write_timeout(std::time::Duration::from_millis(timeout_ms as u64))?;
+            }
+            let mut cf: SocketCanCanFrame;
+            for p in packets {
+                cf = SocketCanCanFrame::Data(p.into());
                 iface.write_frame(&cf)?;
             }
             Ok(())
         })
     }
 
     fn read_packets(&mut self, max: usize, timeout_ms: u32) -> ChannelResult<Vec<CanFrame>> {
-        let timeout = std::cmp::max(1, timeout_ms) as u128;
-        let mut result: Vec<CanFrame> = Vec::with_capacity(max);
         self.safe_with_iface(|iface| {
-            iface.set_read_timeout(std::time::Duration::from_millis(timeout_ms as u64))?;
-            let start = Instant::now();
-            let mut read: socketcan::CANFrame;
-            while start.elapsed().as_millis() <= timeout {
-                read = iface.read_frame()?;
-                result.push(CanFrame::new(read.id(), read.data(), read.is_extended()));
-                // Read complete
-                if result.len() == max {
-                    return Ok(());
+            let mut result: Vec<CanFrame> = Vec::new();
+            if timeout_ms == 0 {
+                iface.set_nonblocking(true)?;
+                while let Ok(f) = iface.read_frame() {
+                    if let SocketCanCanFrame::Data(d) = f {
+                        result.push(d.into())
+                    }
+                    if result.len() == max {
+                        break;
+                    }
+                }
+                if result.len() == 0 {
+                    Err(ChannelError::BufferEmpty)
+                } else {
+                    Ok(result)
+                }
+
+            } else {
+                iface.set_nonblocking(false)?;
+                iface.set_read_timeout(std::time::Duration::from_millis(timeout_ms as u64))?;
+                let start = Instant::now();
+                while start.elapsed().as_millis() <= timeout_ms as u128 {
+                    let f = iface.read_frame()?;
+                    if let SocketCanCanFrame::Data(d) = f {
+                        result.push(d.into())
+                    }
+                    if result.len() == max {
+                        break;
+                    }
+                }
+                if result.len() == 0 {
+                    Err(ChannelError::BufferEmpty)
+                } else {
+                    Ok(result)
                 }
             }
-            Ok(())
-        })?;
-        result.shrink_to_fit(); // Deallocate unneeded memory
-        Ok(result)
+
+        })
     }
 
     fn clear_rx_buffer(&mut self) -> ChannelResult<()> {
+        while self.read_packets(1, 0).is_ok(){}
         Ok(())
     }
 
@@ -496,15 +529,6 @@ impl HardwareScanner<SocketCanDevice> for SocketCanScanner {
     }
 }
 
-impl From<socketcan::CANSocketOpenError> for ChannelError {
-    fn from(e: socketcan::CANSocketOpenError) -> Self {
-        Self::HardwareError(HardwareError::APIError {
-            code: 99,
-            desc: e.to_string(),
-        })
-    }
-}
-
 impl From<socketcan_isotp::Error> for ChannelError {
     fn from(e: socketcan_isotp::Error) -> Self {
         Self::HardwareError(HardwareError::APIError {
@@ -516,6 +540,10 @@ impl From<socketcan_isotp::Error> for ChannelError {
 
 impl From<std::io::Error> for ChannelError {
     fn from(e: std::io::Error) -> Self {
-        Self::IOError(Arc::new(e))
+        if e.kind() == ErrorKind::WouldBlock {
+            Self::BufferEmpty
+        } else {
+            Self::IOError(Arc::new(e))
+        }
     }
 }

src/uds/mod.rs

@@ -152,7 +152,8 @@ impl DiagProtocol<ByteWrapper<UdsError>> for UDSProtocol {
     fn process_ecu_response(r: &[u8]) -> Result<Vec<u8>, (u8, UdsErrorByte)> {
         if r[0] == 0x7F {
             // [7F, SID, NRC]
-            Err((r[2], UdsErrorByte::from(r[2])))
+            // OR (Bootloader sometimes) [0x7F, NRC]
+            Err((*r.last().unwrap(), UdsErrorByte::from(*r.last().unwrap())))
         } else {
             Ok(r.to_vec())
         }