can_motor_control/transport/
socketcan.rs

1//! Linux SocketCAN transport.
2//!
3//! Built directly on libc syscalls: `socket(PF_CAN, SOCK_RAW, CAN_RAW)`,
4//! `if_nametoindex`, `bind`, non-blocking via `fcntl`. Avoids the
5//! `socketcan` crate so we own the FD-vs-classical decode path without
6//! upstream coupling.
7
8use std::ffi::CString;
9use std::io;
10use std::mem::size_of;
11use std::os::fd::{AsRawFd, FromRawFd, OwnedFd, RawFd};
12
13use motor_codec::{BusCapabilities, CanFrame, FrameFlags};
14
15use super::{CanBus, TransportError};
16
17// SocketCAN constants. The libc crate doesn't expose these uniformly across
18// versions, so we define them ourselves matching <linux/can.h>.
19const AF_CAN: libc::c_int = 29;
20const PF_CAN: libc::c_int = AF_CAN;
21const CAN_RAW: libc::c_int = 1;
22
23// setsockopt level/option for enabling CAN-FD reception on a raw CAN socket.
24// `SOL_CAN_RAW` = `SOL_CAN_BASE (100)` + `CAN_RAW (1)`; `CAN_RAW_FD_FRAMES` = 5.
25// Matches <linux/can/raw.h>. With this set, the socket may both read and write
26// `canfd_frame`s in addition to classical `can_frame`s.
27const SOL_CAN_BASE: libc::c_int = 100;
28const SOL_CAN_RAW: libc::c_int = SOL_CAN_BASE + CAN_RAW;
29const CAN_RAW_FD_FRAMES: libc::c_int = 5;
30
31const CAN_EFF_FLAG: u32 = 0x8000_0000;
32const CAN_RTR_FLAG: u32 = 0x4000_0000;
33const CAN_EFF_MASK: u32 = 0x1FFF_FFFF;
34const CAN_SFF_MASK: u32 = 0x0000_07FF;
35
36// Kernel struct layouts. Sized for x86_64 Linux; the alignment of u32 + u8s
37// matches the kernel's `struct can_frame` / `struct canfd_frame`.
38
39#[repr(C)]
40#[derive(Copy, Clone, Default)]
41struct KernelCanFrame {
42    can_id: u32,
43    can_dlc: u8,
44    __pad: u8,
45    __res0: u8,
46    len8_dlc: u8,
47    data: [u8; 8],
48}
49
50#[repr(C)]
51#[derive(Copy, Clone)]
52struct KernelCanFdFrame {
53    can_id: u32,
54    len: u8,
55    flags: u8,
56    __res0: u8,
57    __res1: u8,
58    data: [u8; 64],
59}
60
61const KERNEL_CLASSICAL_LEN: usize = size_of::<KernelCanFrame>(); // 16
62const KERNEL_FD_LEN: usize = size_of::<KernelCanFdFrame>(); // 72
63
64#[repr(C)]
65struct SockaddrCan {
66    can_family: u16,
67    _pad: u16,
68    can_ifindex: i32,
69    rx_id: u32,
70    tx_id: u32,
71}
72
73/// Linux SocketCAN bus.
74pub struct SocketCanBus {
75    name: String,
76    fd: OwnedFd,
77    caps: BusCapabilities,
78}
79
80impl SocketCanBus {
81    /// Open a SocketCAN interface.
82    ///
83    /// With `fd_enabled = true` the socket is configured for CAN-FD
84    /// (`CAN_RAW_FD_FRAMES`) and the bus advertises `BusCapabilities::fd()`; the
85    /// interface itself must be FD-capable. With `fd_enabled = false` the bus is
86    /// classical-only (no FD socket option, 8-byte payload cap).
87    pub fn open(interface: &str, fd_enabled: bool) -> Result<Self, TransportError> {
88        if interface.len() >= libc::IFNAMSIZ {
89            return Err(TransportError::InterfaceNotFound(interface.to_string()));
90        }
91
92        // Resolve the interface index before creating the CAN socket so a
93        // missing interface cannot leak an open descriptor.
94        let ifindex = resolve_ifindex(interface)?;
95
96        // SAFETY: socket() with valid family/type/protocol; returns -1 on error.
97        let raw_fd = unsafe { libc::socket(PF_CAN, libc::SOCK_RAW, CAN_RAW) };
98        if raw_fd < 0 {
99            return Err(map_open_error(io::Error::last_os_error()));
100        }
101        // SAFETY: raw_fd is a kernel-owned fd; OwnedFd takes exclusive ownership.
102        let owned = unsafe { OwnedFd::from_raw_fd(raw_fd) };
103
104        // Enable CAN-FD frames on the socket before bind when requested. Without
105        // this option the kernel delivers and accepts only classical frames.
106        if fd_enabled {
107            let enable: libc::c_int = 1;
108            // SAFETY: setsockopt with a valid level/option and an int-sized value.
109            let rc = unsafe {
110                libc::setsockopt(
111                    raw_fd,
112                    SOL_CAN_RAW,
113                    CAN_RAW_FD_FRAMES,
114                    &enable as *const libc::c_int as *const libc::c_void,
115                    size_of::<libc::c_int>() as libc::socklen_t,
116                )
117            };
118            if rc < 0 {
119                return Err(map_open_error(io::Error::last_os_error()));
120            }
121        }
122
123        // bind(sockaddr_can)
124        let addr = SockaddrCan {
125            can_family: AF_CAN as u16,
126            _pad: 0,
127            can_ifindex: ifindex,
128            rx_id: 0,
129            tx_id: 0,
130        };
131        // SAFETY: addr lives until bind returns; cast to *const sockaddr is the
132        // canonical SocketCAN bind pattern.
133        let rc = unsafe {
134            libc::bind(
135                raw_fd,
136                &addr as *const SockaddrCan as *const libc::sockaddr,
137                size_of::<SockaddrCan>() as libc::socklen_t,
138            )
139        };
140        if rc < 0 {
141            return Err(map_open_error(io::Error::last_os_error()));
142        }
143
144        // Set non-blocking.
145        // SAFETY: F_GETFL returns the current flag value; F_SETFL applies the OR.
146        unsafe {
147            let flags = libc::fcntl(raw_fd, libc::F_GETFL);
148            if flags < 0 {
149                return Err(TransportError::Io(io::Error::last_os_error()));
150            }
151            if libc::fcntl(raw_fd, libc::F_SETFL, flags | libc::O_NONBLOCK) < 0 {
152                return Err(TransportError::Io(io::Error::last_os_error()));
153            }
154        }
155
156        Ok(Self {
157            name: interface.to_string(),
158            fd: owned,
159            caps: if fd_enabled {
160                BusCapabilities::fd()
161            } else {
162                BusCapabilities::classical()
163            },
164        })
165    }
166
167    /// Write one already-serialized kernel frame (`expected` bytes), retrying
168    /// on `EAGAIN`/`EINTR` within the send budget. Shared by the classical and
169    /// FD send paths.
170    fn write_frame_bytes(&self, bytes: &[u8], expected: usize) -> Result<(), TransportError> {
171        let raw = self.fd.as_raw_fd();
172        let mut retries = 0u32;
173        loop {
174            // SAFETY: write() to an owned fd with a valid byte slice.
175            let n = unsafe { libc::write(raw, bytes.as_ptr() as *const libc::c_void, expected) };
176            if n == expected as isize {
177                return Ok(());
178            }
179            if n < 0 {
180                let err = io::Error::last_os_error();
181                match err.raw_os_error() {
182                    Some(libc::EAGAIN) => {
183                        retries += 1;
184                        if retries >= SEND_RETRY_BUDGET {
185                            return Err(TransportError::SendBufferFull);
186                        }
187                        continue;
188                    }
189                    Some(libc::EINTR) => continue,
190                    _ => return Err(TransportError::Io(err)),
191                }
192            }
193            // Short write — shouldn't happen for CAN, treat as IO error.
194            return Err(TransportError::Io(io::Error::other(format!(
195                "short write: {n} bytes"
196            ))));
197        }
198    }
199}
200
201fn resolve_ifindex(interface: &str) -> Result<i32, TransportError> {
202    let cname = CString::new(interface)
203        .map_err(|_| TransportError::InterfaceNotFound(interface.to_string()))?;
204    if cname.as_bytes_with_nul().len() > libc::IFNAMSIZ {
205        return Err(TransportError::InterfaceNotFound(interface.to_string()));
206    }
207
208    // SAFETY: cname is a valid, NUL-terminated interface name.
209    let index = unsafe { libc::if_nametoindex(cname.as_ptr()) };
210    if index == 0 {
211        let err = io::Error::last_os_error();
212        if err.raw_os_error() == Some(libc::ENODEV) {
213            return Err(TransportError::InterfaceNotFound(interface.to_string()));
214        }
215        return Err(map_open_error(err));
216    }
217    i32::try_from(index)
218        .map_err(|_| TransportError::Io(io::Error::other("CAN interface index exceeds i32")))
219}
220
221fn map_open_error(e: io::Error) -> TransportError {
222    match e.raw_os_error() {
223        Some(libc::EACCES) | Some(libc::EPERM) => TransportError::PermissionDenied,
224        Some(libc::ENODEV) | Some(libc::ENXIO) => {
225            TransportError::InterfaceNotFound(format!("os errno {}", libc::ENODEV))
226        }
227        _ => TransportError::Io(e),
228    }
229}
230
231fn validate_send(caps: &BusCapabilities, frame: &CanFrame) -> Result<(), TransportError> {
232    if frame.is_fd() && !caps.supports_fd {
233        return Err(TransportError::FdFrameOnNonFdBus);
234    }
235    if frame.len > caps.max_payload_len {
236        return Err(TransportError::PayloadExceedsBusCapacity {
237            len: frame.len,
238            max: caps.max_payload_len,
239        });
240    }
241    Ok(())
242}
243
244const SEND_RETRY_BUDGET: u32 = 8;
245
246impl CanBus for SocketCanBus {
247    fn name(&self) -> &str {
248        &self.name
249    }
250
251    fn capabilities(&self) -> BusCapabilities {
252        self.caps
253    }
254
255    fn send(&mut self, frame: &CanFrame) -> Result<(), TransportError> {
256        validate_send(&self.caps, frame)?;
257        // Format is chosen per-frame: an FD frame goes out as a `canfd_frame`,
258        // a classical frame as a `can_frame` — even on an FD-capable socket,
259        // which can carry both. `validate_send` has already rejected an FD frame
260        // on a classical bus, so reaching the FD arm implies `caps.supports_fd`.
261        if frame.is_fd() {
262            let mut kf = KernelCanFdFrame {
263                can_id: encode_can_id(frame),
264                len: frame.len,
265                flags: fd_flags_byte(frame),
266                __res0: 0,
267                __res1: 0,
268                data: [0u8; 64],
269            };
270            kf.data[..frame.len as usize].copy_from_slice(frame.payload());
271            // SAFETY: viewing a #[repr(C)] struct as its byte representation.
272            let bytes = unsafe {
273                std::slice::from_raw_parts(
274                    &kf as *const KernelCanFdFrame as *const u8,
275                    KERNEL_FD_LEN,
276                )
277            };
278            self.write_frame_bytes(bytes, KERNEL_FD_LEN)
279        } else {
280            let mut kf = KernelCanFrame {
281                can_id: encode_can_id(frame),
282                can_dlc: frame.len,
283                ..Default::default()
284            };
285            kf.data[..frame.len as usize].copy_from_slice(frame.payload());
286            // SAFETY: viewing a #[repr(C)] struct as its byte representation.
287            let bytes = unsafe {
288                std::slice::from_raw_parts(
289                    &kf as *const KernelCanFrame as *const u8,
290                    KERNEL_CLASSICAL_LEN,
291                )
292            };
293            self.write_frame_bytes(bytes, KERNEL_CLASSICAL_LEN)
294        }
295    }
296
297    fn drain_inbound_nonblocking(&mut self) -> Result<Vec<CanFrame>, TransportError> {
298        let mut out = Vec::new();
299        let raw = self.fd.as_raw_fd();
300        // Buffer sized for FD frames. A classical-only socket reads only
301        // classical frames; an FD socket (opened with fd=true) may read either,
302        // discriminated below by the kernel read length.
303        let mut buf = [0u8; KERNEL_FD_LEN];
304        loop {
305            // SAFETY: read() into an owned buffer; the returned count is the
306            // exact bytes filled.
307            let n =
308                unsafe { libc::read(raw, buf.as_mut_ptr() as *mut libc::c_void, KERNEL_FD_LEN) };
309            if n < 0 {
310                let err = io::Error::last_os_error();
311                match err.raw_os_error() {
312                    Some(libc::EAGAIN) => return Ok(out),
313                    Some(libc::EINTR) => continue,
314                    _ => return Err(TransportError::Io(err)),
315                }
316            }
317            match n as usize {
318                KERNEL_CLASSICAL_LEN => out.push(decode_kernel_classical(&buf)),
319                KERNEL_FD_LEN => out.push(decode_kernel_fd(&buf)),
320                0 => return Ok(out),
321                other => {
322                    return Err(TransportError::Io(io::Error::other(format!(
323                        "unexpected SocketCAN read size: {other}"
324                    ))));
325                }
326            }
327        }
328    }
329
330    fn raw_fd(&self) -> Option<RawFd> {
331        Some(self.fd.as_raw_fd())
332    }
333}
334
335/// Pack a `CanFrame`'s FD-only flags into the kernel `canfd_frame.flags` byte
336/// (bit 0 = BRS, bit 1 = ESI), matching `decode_kernel_fd`'s reader.
337fn fd_flags_byte(frame: &CanFrame) -> u8 {
338    let mut f = 0u8;
339    if frame.flags.contains(FrameFlags::BIT_RATE_SWITCH) {
340        f |= 0x01;
341    }
342    if frame.flags.contains(FrameFlags::ERROR_STATE) {
343        f |= 0x02;
344    }
345    f
346}
347
348fn encode_can_id(frame: &CanFrame) -> u32 {
349    let mut id = frame.id
350        & if frame.is_extended() {
351            CAN_EFF_MASK
352        } else {
353            CAN_SFF_MASK
354        };
355    if frame.is_extended() {
356        id |= CAN_EFF_FLAG;
357    }
358    if frame.flags.contains(FrameFlags::REMOTE_REQUEST) {
359        id |= CAN_RTR_FLAG;
360    }
361    id
362}
363
364pub(crate) fn decode_kernel_classical(buf: &[u8; KERNEL_FD_LEN]) -> CanFrame {
365    let kf = unsafe { &*(buf.as_ptr() as *const KernelCanFrame) };
366    let extended = kf.can_id & CAN_EFF_FLAG != 0;
367    let rtr = kf.can_id & CAN_RTR_FLAG != 0;
368    let id = kf.can_id & if extended { CAN_EFF_MASK } else { CAN_SFF_MASK };
369    let len = kf.can_dlc.min(8);
370    let mut flags = FrameFlags::empty();
371    if extended {
372        flags |= FrameFlags::EXTENDED_ID;
373    }
374    if rtr {
375        flags |= FrameFlags::REMOTE_REQUEST;
376    }
377    let payload = &kf.data[..len as usize];
378    let mut frame = if extended {
379        CanFrame::classical_extended(id, payload)
380    } else {
381        CanFrame::classical(id, payload)
382    }
383    .expect("kernel-bounded payload");
384    frame.flags = flags;
385    frame
386}
387
388pub(crate) fn decode_kernel_fd(buf: &[u8; KERNEL_FD_LEN]) -> CanFrame {
389    let kf = unsafe { &*(buf.as_ptr() as *const KernelCanFdFrame) };
390    let extended = kf.can_id & CAN_EFF_FLAG != 0;
391    let id = kf.can_id & if extended { CAN_EFF_MASK } else { CAN_SFF_MASK };
392    let len = kf.len.min(64);
393    let mut flags = FrameFlags::FD_FORMAT;
394    if extended {
395        flags |= FrameFlags::EXTENDED_ID;
396    }
397    if kf.flags & 0x01 != 0 {
398        flags |= FrameFlags::BIT_RATE_SWITCH;
399    }
400    if kf.flags & 0x02 != 0 {
401        flags |= FrameFlags::ERROR_STATE;
402    }
403    let payload = &kf.data[..len as usize];
404    let mut frame = if extended {
405        CanFrame::fd_extended(id, payload)
406    } else {
407        CanFrame::fd(id, payload)
408    }
409    .expect("kernel-bounded FD payload");
410    frame.flags = flags;
411    frame
412}
413
414#[cfg(test)]
415mod tests {
416    use super::*;
417
418    fn vcan_available(name: &str) -> bool {
419        std::path::Path::new(&format!("/sys/class/net/{name}")).exists()
420    }
421
422    /// The kernel `canfd_frame` layout we mirror is 72 bytes on Linux; the
423    /// receive path discriminates classical vs FD purely by read length, so a
424    /// layout drift would silently mis-parse frames. Assert it loudly instead.
425    #[test]
426    fn kernel_fd_frame_layout_is_72_bytes() {
427        assert_eq!(size_of::<KernelCanFdFrame>(), 72);
428        assert_eq!(size_of::<KernelCanFrame>(), 16);
429        assert_eq!(KERNEL_FD_LEN, 72);
430        assert_eq!(KERNEL_CLASSICAL_LEN, 16);
431    }
432
433    #[test]
434    fn nonexistent_interface_returns_interface_not_found() {
435        let r = SocketCanBus::open("ghost-iface-x", false);
436        assert!(matches!(r, Err(TransportError::InterfaceNotFound(_))));
437    }
438
439    #[test]
440    fn vcan0_open_and_capabilities() {
441        if !vcan_available("vcan0") {
442            eprintln!("skipping: vcan0 not present");
443            return;
444        }
445        let bus = SocketCanBus::open("vcan0", false).expect("open vcan0");
446        assert_eq!(bus.name(), "vcan0");
447        let caps = bus.capabilities();
448        assert!(!caps.supports_fd);
449        assert_eq!(caps.max_payload_len, 8);
450        assert!(bus.raw_fd().is_some());
451    }
452
453    #[test]
454    fn vcan0_send_round_trip() {
455        if !vcan_available("vcan0") {
456            eprintln!("skipping: vcan0 not present");
457            return;
458        }
459        let mut tx = SocketCanBus::open("vcan0", false).expect("open vcan0 tx");
460        let mut rx = SocketCanBus::open("vcan0", false).expect("open vcan0 rx");
461        let f = CanFrame::classical(0x101, &[1, 2, 3, 4]).unwrap();
462        tx.send(&f).unwrap();
463        // Best-effort: a small busy-wait so the kernel can route the frame.
464        let mut got = Vec::new();
465        for _ in 0..100 {
466            got = rx.drain_inbound_nonblocking().unwrap();
467            if !got.is_empty() {
468                break;
469            }
470            std::thread::sleep(std::time::Duration::from_millis(1));
471        }
472        assert_eq!(got.len(), 1);
473        assert_eq!(got[0].id, 0x101);
474        assert_eq!(got[0].payload(), &[1, 2, 3, 4]);
475    }
476
477    #[test]
478    fn fd_frame_rejected_on_classical_socketcanbus() {
479        if !vcan_available("vcan0") {
480            eprintln!("skipping: vcan0 not present");
481            return;
482        }
483        let mut bus = SocketCanBus::open("vcan0", false).unwrap();
484        let f = CanFrame::fd(0x100, &[0; 16]).unwrap();
485        assert!(matches!(
486            bus.send(&f),
487            Err(TransportError::FdFrameOnNonFdBus)
488        ));
489    }
490
491    #[test]
492    fn synthetic_fd_frame_decodes_with_fd_format_flag() {
493        // Direct unit test of the FD parsing path (v1 SocketCanBus never
494        // receives real FD frames because we don't enable CAN_RAW_FD_FRAMES,
495        // but the decode path must be correct so v2 unlocks without re-touch).
496        let mut buf = [0u8; KERNEL_FD_LEN];
497        // Build a canfd_frame: id=0x101, len=16, BRS flag set, data[0..16] = 0xAA
498        let kf = KernelCanFdFrame {
499            can_id: 0x101,
500            len: 16,
501            flags: 0x01, // BRS
502            __res0: 0,
503            __res1: 0,
504            data: {
505                let mut d = [0u8; 64];
506                d[..16].fill(0xAA);
507                d
508            },
509        };
510        // SAFETY: writing a #[repr(C)] struct into a byte buffer of equal size.
511        unsafe {
512            std::ptr::copy_nonoverlapping(
513                &kf as *const KernelCanFdFrame as *const u8,
514                buf.as_mut_ptr(),
515                KERNEL_FD_LEN,
516            );
517        }
518        let frame = decode_kernel_fd(&buf);
519        assert!(frame.is_fd());
520        assert!(frame.flags.contains(FrameFlags::BIT_RATE_SWITCH));
521        assert_eq!(frame.id, 0x101);
522        assert_eq!(frame.len, 16);
523        assert_eq!(frame.payload(), &[0xAA; 16]);
524    }
525
526    #[test]
527    fn fd_open_advertises_fd_capabilities() {
528        // Requires an FD-capable interface (e.g. `ip link add vcanfd0 type
529        // vcan; ip link set vcanfd0 mtu 72`). Skipped when absent.
530        if !vcan_available("vcanfd0") {
531            eprintln!("skipping: vcanfd0 not present");
532            return;
533        }
534        let bus = SocketCanBus::open("vcanfd0", true).expect("open vcanfd0 fd");
535        let caps = bus.capabilities();
536        assert!(caps.supports_fd);
537        assert_eq!(caps.max_payload_len, 64);
538    }
539
540    #[test]
541    fn fd_round_trip_over_fd_socketcan() {
542        if !vcan_available("vcanfd0") {
543            eprintln!("skipping: vcanfd0 not present");
544            return;
545        }
546        let mut tx = SocketCanBus::open("vcanfd0", true).expect("open tx");
547        let mut rx = SocketCanBus::open("vcanfd0", true).expect("open rx");
548        let f = CanFrame::fd(0x123, &[0xAB; 16]).unwrap();
549        tx.send(&f).unwrap();
550        let mut got = Vec::new();
551        for _ in 0..100 {
552            got = rx.drain_inbound_nonblocking().unwrap();
553            if !got.is_empty() {
554                break;
555            }
556            std::thread::sleep(std::time::Duration::from_millis(1));
557        }
558        assert_eq!(got.len(), 1);
559        assert!(got[0].is_fd());
560        assert_eq!(got[0].id, 0x123);
561        assert_eq!(got[0].payload(), &[0xAB; 16]);
562    }
563}
can_motor_control/transport/socketcan.rs

can_motor_control/transport/
socketcan.rs
