linux_socketcan_iso_tp/
linux.rs

1//! Linux kernel ISO-TP socket backend.
2//!
3//! This crate provides a minimal wrapper around Linux `CAN_ISOTP` sockets and
4//! implements the shared `can-isotp-interface` traits.
5
6use can_isotp_interface::{
7    IsoTpRxFlowControlConfig, RecvControl, RecvError, RecvMeta, RecvMetaIntoStatus, RecvStatus,
8    RxFlowControl, SendError,
9};
10use can_uds::uds29;
11use core::time::Duration;
12use embedded_can::{ExtendedId, Id};
13use socket2::{Domain, Protocol, Socket, Type};
14use socketcan::CanAddr;
15use std::io;
16use std::mem::size_of;
17use std::os::fd::{AsRawFd, FromRawFd, IntoRawFd, OwnedFd, RawFd};
18use std::thread;
19use std::time::Instant;
20
21#[cfg(feature = "tokio")]
22use tokio::io::unix::AsyncFd;
23
24#[cfg(feature = "tokio")]
25use can_isotp_interface::IsoTpAsyncEndpoint;
26#[cfg(feature = "tokio")]
27use can_isotp_interface::IsoTpAsyncEndpointRecvInto;
28
29/// ISO-TP socket error type.
30#[derive(Debug)]
31pub enum Error {
32    /// I/O error from the OS.
33    Io(io::Error),
34    /// Invalid configuration passed to the constructor.
35    InvalidConfig(&'static str),
36}
37
38impl From<io::Error> for Error {
39    fn from(err: io::Error) -> Self {
40        Self::Io(err)
41    }
42}
43
44impl core::fmt::Display for Error {
45    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
46        match self {
47            Error::Io(err) => write!(f, "io error: {err}"),
48            Error::InvalidConfig(msg) => write!(f, "invalid config: {msg}"),
49        }
50    }
51}
52
53impl std::error::Error for Error {}
54
55/// Socket-level ISO-TP options.
56#[derive(Debug, Clone, Default)]
57pub struct IsoTpSocketOptions {
58    /// Raw kernel flag bits (see `flags` module).
59    pub flags: u32,
60    /// Frame transmit time (N_As/N_Ar) for the kernel in nanoseconds.
61    pub frame_txtime: Option<Duration>,
62    /// Extended addressing byte for TX and RX (normal extended addressing).
63    pub ext_address: Option<u8>,
64    /// Padding byte for transmit frames (enables TX padding flag).
65    pub tx_padding: Option<u8>,
66    /// Padding byte for receive frames (enables RX padding flag).
67    pub rx_padding: Option<u8>,
68    /// Separate RX extended address (enables RX_EXT_ADDR flag).
69    pub rx_ext_address: Option<u8>,
70}
71
72/// Flow control options advertised by the kernel.
73#[derive(Debug, Clone, Copy)]
74pub struct IsoTpFlowControlOptions {
75    /// Block size (0 = unlimited).
76    pub block_size: u8,
77    /// STmin raw encoding as defined by ISO-TP.
78    pub st_min: u8,
79    /// Maximum number of wait frames.
80    pub wft_max: u8,
81}
82
83impl IsoTpFlowControlOptions {
84    /// Build a new flow control configuration.
85    pub fn new(block_size: u8, st_min: u8, wft_max: u8) -> Self {
86        Self {
87            block_size,
88            st_min,
89            wft_max,
90        }
91    }
92}
93
94/// Link-layer options for kernel ISO-TP sockets.
95#[derive(Debug, Clone, Copy)]
96pub struct IsoTpLinkLayerOptions {
97    /// CAN MTU to use (e.g. CAN_MTU or CANFD_MTU).
98    pub mtu: u8,
99    /// TX data length in bytes for CAN FD (8/12/16/20/24/32/48/64).
100    pub tx_dl: u8,
101    /// CAN FD flags (e.g. BRS).
102    pub tx_flags: u8,
103}
104
105impl IsoTpLinkLayerOptions {
106    /// Build a new link-layer configuration.
107    pub fn new(mtu: u8, tx_dl: u8, tx_flags: u8) -> Self {
108        Self {
109            mtu,
110            tx_dl,
111            tx_flags,
112        }
113    }
114}
115
116/// Kernel ISO-TP socket configuration.
117#[derive(Debug, Clone)]
118pub struct IsoTpKernelOptions {
119    /// Maximum payload size to receive into the internal buffer.
120    pub max_rx_payload: usize,
121    /// Base ISO-TP socket options.
122    pub socket: IsoTpSocketOptions,
123    /// Flow control options (optional).
124    pub flow_control: Option<IsoTpFlowControlOptions>,
125    /// Link-layer options (optional).
126    pub link_layer: Option<IsoTpLinkLayerOptions>,
127    /// Force TX STmin (nanoseconds) regardless of FC frames.
128    pub force_tx_stmin: Option<Duration>,
129    /// Force RX STmin (nanoseconds) regardless of received CF timestamps.
130    pub force_rx_stmin: Option<Duration>,
131}
132
133impl Default for IsoTpKernelOptions {
134    fn default() -> Self {
135        Self {
136            max_rx_payload: 4095,
137            socket: IsoTpSocketOptions::default(),
138            flow_control: None,
139            link_layer: None,
140            force_tx_stmin: None,
141            force_rx_stmin: None,
142        }
143    }
144}
145
146/// Kernel ISO-TP endpoint using a single socket.
147#[derive(Debug)]
148pub struct SocketCanIsoTp {
149    fd: OwnedFd,
150    rx_buf: Vec<u8>,
151    wft_max: u8,
152}
153
154impl SocketCanIsoTp {
155    /// Open a kernel ISO-TP socket on `iface` with fixed RX/TX CAN IDs.
156    pub fn open(
157        iface: &str,
158        rx_id: Id,
159        tx_id: Id,
160        options: &IsoTpKernelOptions,
161    ) -> Result<Self, Error> {
162        if options.max_rx_payload == 0 {
163            return Err(Error::InvalidConfig("max_rx_payload must be > 0"));
164        }
165
166        let socket = Socket::new(
167            Domain::from(libc::AF_CAN),
168            Type::DGRAM,
169            Some(Protocol::from(libc::CAN_ISOTP)),
170        )?;
171
172        socket.set_nonblocking(true)?;
173
174        // Most ISO-TP socket options must be applied before binding.
175        apply_kernel_options(socket.as_raw_fd(), options)?;
176
177        let addr = CanAddr::from_iface_isotp(iface, rx_id, tx_id).map_err(Error::Io)?;
178        socket.bind(&addr.into_sock_addr())?;
179
180        let fd = unsafe { OwnedFd::from_raw_fd(socket.into_raw_fd()) };
181        let wft_max = options.flow_control.map(|fc| fc.wft_max).unwrap_or(0);
182        Ok(Self {
183            fd,
184            rx_buf: vec![0u8; options.max_rx_payload],
185            wft_max,
186        })
187    }
188
189    /// Update receive-side FlowControl parameters (BS/STmin) used by the kernel.
190    ///
191    /// This applies `CAN_ISOTP_RECV_FC` at runtime. For best dynamic behavior, enable
192    /// [`flags::CAN_ISOTP_DYN_FC_PARMS`] when opening the socket (kernel-dependent).
193    pub fn set_rx_flow_control(&mut self, fc: RxFlowControl) -> Result<(), Error> {
194        let c = can_isotp_fc_options {
195            bs: fc.block_size,
196            stmin: duration_to_isotp_stmin(fc.st_min),
197            wftmax: self.wft_max,
198        };
199        let res = unsafe {
200            libc::setsockopt(
201                self.fd.as_raw_fd(),
202                SOL_CAN_ISOTP,
203                CAN_ISOTP_RECV_FC,
204                &c as *const can_isotp_fc_options as *const libc::c_void,
205                size_of::<can_isotp_fc_options>() as libc::socklen_t,
206            )
207        };
208        if res < 0 {
209            return Err(Error::Io(io::Error::last_os_error()));
210        }
211        Ok(())
212    }
213
214    fn recv_one_ready<Cb>(&mut self, mut on_payload: Cb) -> Result<RecvStatus, RecvError<Error>>
215    where
216        Cb: FnMut(&[u8]) -> Result<RecvControl, Error>,
217    {
218        let read = unsafe {
219            libc::recv(
220                self.fd.as_raw_fd(),
221                self.rx_buf.as_mut_ptr().cast(),
222                self.rx_buf.len(),
223                libc::MSG_DONTWAIT,
224            )
225        };
226        if read < 0 {
227            let err = io::Error::last_os_error();
228            if err.kind() == io::ErrorKind::WouldBlock {
229                return Ok(RecvStatus::TimedOut);
230            }
231            return Err(RecvError::Backend(Error::Io(err)));
232        }
233        if read == 0 {
234            return Err(RecvError::Backend(Error::Io(io::Error::new(
235                io::ErrorKind::UnexpectedEof,
236                "iso-tp socket returned 0 bytes",
237            ))));
238        }
239        let payload = &self.rx_buf[..read as usize];
240        let _ = on_payload(payload).map_err(RecvError::Backend)?;
241        Ok(RecvStatus::DeliveredOne)
242    }
243}
244
245impl AsRawFd for SocketCanIsoTp {
246    fn as_raw_fd(&self) -> RawFd {
247        self.fd.as_raw_fd()
248    }
249}
250
251impl can_isotp_interface::IsoTpEndpoint for SocketCanIsoTp {
252    type Error = Error;
253
254    fn send_to(
255        &mut self,
256        _to: u8,
257        payload: &[u8],
258        timeout: Duration,
259    ) -> Result<(), SendError<Self::Error>> {
260        let deadline = Instant::now() + timeout;
261        loop {
262            let sent = unsafe {
263                libc::send(
264                    self.fd.as_raw_fd(),
265                    payload.as_ptr().cast(),
266                    payload.len(),
267                    libc::MSG_DONTWAIT,
268                )
269            };
270            if sent >= 0 {
271                return Ok(());
272            }
273
274            let err = io::Error::last_os_error();
275            if err.kind() == io::ErrorKind::Interrupted {
276                continue;
277            }
278            if err.kind() != io::ErrorKind::WouldBlock {
279                return Err(SendError::Backend(Error::Io(err)));
280            }
281
282            let now = Instant::now();
283            if now >= deadline {
284                return Err(SendError::Timeout);
285            }
286            let remaining = deadline - now;
287            let ready = poll_fd(self.fd.as_raw_fd(), libc::POLLOUT, remaining)
288                .map_err(Error::Io)
289                .map_err(SendError::Backend)?;
290            if !ready {
291                return Err(SendError::Timeout);
292            }
293        }
294    }
295
296    fn send_functional_to(
297        &mut self,
298        _functional_to: u8,
299        payload: &[u8],
300        timeout: Duration,
301    ) -> Result<(), SendError<Self::Error>> {
302        self.send_to(0, payload, timeout)
303    }
304
305    fn recv_one<Cb>(
306        &mut self,
307        timeout: Duration,
308        mut on_payload: Cb,
309    ) -> Result<RecvStatus, RecvError<Self::Error>>
310    where
311        Cb: FnMut(RecvMeta, &[u8]) -> Result<RecvControl, Self::Error>,
312    {
313        let deadline = Instant::now() + timeout;
314        loop {
315            let read = unsafe {
316                libc::recv(
317                    self.fd.as_raw_fd(),
318                    self.rx_buf.as_mut_ptr().cast(),
319                    self.rx_buf.len(),
320                    libc::MSG_DONTWAIT,
321                )
322            };
323            if read >= 0 {
324                if read == 0 {
325                    return Err(RecvError::Backend(Error::Io(io::Error::new(
326                        io::ErrorKind::UnexpectedEof,
327                        "iso-tp socket returned 0 bytes",
328                    ))));
329                }
330
331                let payload = &self.rx_buf[..read as usize];
332                let _ =
333                    on_payload(RecvMeta { reply_to: 0 }, payload).map_err(RecvError::Backend)?;
334                return Ok(RecvStatus::DeliveredOne);
335            }
336
337            let err = io::Error::last_os_error();
338            if err.kind() == io::ErrorKind::Interrupted {
339                continue;
340            }
341            if err.kind() != io::ErrorKind::WouldBlock {
342                return Err(RecvError::Backend(Error::Io(err)));
343            }
344
345            let now = Instant::now();
346            if now >= deadline {
347                return Ok(RecvStatus::TimedOut);
348            }
349            let remaining = deadline - now;
350            let ready = poll_fd(self.fd.as_raw_fd(), libc::POLLIN, remaining)
351                .map_err(Error::Io)
352                .map_err(RecvError::Backend)?;
353            if !ready {
354                return Ok(RecvStatus::TimedOut);
355            }
356        }
357    }
358}
359
360impl IsoTpRxFlowControlConfig for SocketCanIsoTp {
361    type Error = Error;
362
363    fn set_rx_flow_control(&mut self, fc: RxFlowControl) -> Result<(), Self::Error> {
364        SocketCanIsoTp::set_rx_flow_control(self, fc)
365    }
366}
367
368/// Tokio-native async wrapper around [`SocketCanIsoTp`].
369///
370/// This uses `tokio::io::unix::AsyncFd` and non-blocking `send`/`recv` syscalls to integrate
371/// kernel ISO-TP sockets into an async runtime without blocking threads.
372#[cfg(feature = "tokio")]
373#[derive(Debug)]
374pub struct TokioSocketCanIsoTp {
375    io: AsyncFd<SocketCanIsoTp>,
376}
377
378#[cfg(feature = "tokio")]
379impl TokioSocketCanIsoTp {
380    /// Open a kernel ISO-TP socket and wrap it for tokio async I/O.
381    pub fn open(
382        iface: &str,
383        rx_id: Id,
384        tx_id: Id,
385        options: &IsoTpKernelOptions,
386    ) -> Result<Self, Error> {
387        let inner = SocketCanIsoTp::open(iface, rx_id, tx_id, options)?;
388        let io = AsyncFd::new(inner).map_err(Error::Io)?;
389        Ok(Self { io })
390    }
391
392    pub fn inner(&self) -> &SocketCanIsoTp {
393        self.io.get_ref()
394    }
395
396    pub fn inner_mut(&mut self) -> &mut SocketCanIsoTp {
397        self.io.get_mut()
398    }
399
400    pub fn into_inner(self) -> SocketCanIsoTp {
401        self.io.into_inner()
402    }
403}
404
405#[cfg(feature = "tokio")]
406impl IsoTpRxFlowControlConfig for TokioSocketCanIsoTp {
407    type Error = Error;
408
409    fn set_rx_flow_control(&mut self, fc: RxFlowControl) -> Result<(), Self::Error> {
410        self.io.get_mut().set_rx_flow_control(fc)
411    }
412}
413
414#[cfg(feature = "tokio")]
415impl IsoTpAsyncEndpoint for TokioSocketCanIsoTp {
416    type Error = Error;
417
418    async fn send_to(
419        &mut self,
420        _to: u8,
421        payload: &[u8],
422        timeout: Duration,
423    ) -> Result<(), SendError<Self::Error>> {
424        let res = tokio::time::timeout(timeout, async {
425            loop {
426                let fd = self.io.get_ref().as_raw_fd();
427                let sent = unsafe {
428                    libc::send(
429                        fd,
430                        payload.as_ptr().cast(),
431                        payload.len(),
432                        libc::MSG_DONTWAIT,
433                    )
434                };
435                if sent >= 0 {
436                    return Ok(());
437                }
438
439                let err = io::Error::last_os_error();
440                if err.kind() == io::ErrorKind::Interrupted {
441                    continue;
442                }
443                if err.kind() != io::ErrorKind::WouldBlock {
444                    return Err(SendError::Backend(Error::Io(err)));
445                }
446
447                let mut guard = self
448                    .io
449                    .writable()
450                    .await
451                    .map_err(|e| SendError::Backend(Error::Io(e)))?;
452                guard.clear_ready();
453            }
454        })
455        .await;
456
457        match res {
458            Ok(v) => v,
459            Err(_) => Err(SendError::Timeout),
460        }
461    }
462
463    async fn send_functional_to(
464        &mut self,
465        _functional_to: u8,
466        payload: &[u8],
467        timeout: Duration,
468    ) -> Result<(), SendError<Self::Error>> {
469        self.send_to(0, payload, timeout).await
470    }
471
472    async fn recv_one<Cb>(
473        &mut self,
474        timeout: Duration,
475        mut on_payload: Cb,
476    ) -> Result<RecvStatus, RecvError<Self::Error>>
477    where
478        Cb: FnMut(RecvMeta, &[u8]) -> Result<RecvControl, Self::Error>,
479    {
480        let res = tokio::time::timeout(timeout, async {
481            loop {
482                match self
483                    .io
484                    .get_mut()
485                    .recv_one_ready(|payload| on_payload(RecvMeta { reply_to: 0 }, payload))
486                {
487                    Ok(RecvStatus::DeliveredOne) => return Ok(RecvStatus::DeliveredOne),
488                    Ok(RecvStatus::TimedOut) => {
489                        let mut guard = self
490                            .io
491                            .readable()
492                            .await
493                            .map_err(|e| RecvError::Backend(Error::Io(e)))?;
494                        guard.clear_ready();
495                        continue;
496                    }
497                    Err(e) => return Err(e),
498                }
499            }
500        })
501        .await;
502
503        match res {
504            Ok(v) => v,
505            Err(_) => Ok(RecvStatus::TimedOut),
506        }
507    }
508}
509
510#[cfg(feature = "tokio")]
511impl IsoTpAsyncEndpointRecvInto for TokioSocketCanIsoTp {
512    type Error = Error;
513
514    async fn recv_one_into(
515        &mut self,
516        timeout: Duration,
517        out: &mut [u8],
518    ) -> Result<RecvMetaIntoStatus, RecvError<Self::Error>> {
519        let res = tokio::time::timeout(timeout, async {
520            loop {
521                let mut guard = self
522                    .io
523                    .readable()
524                    .await
525                    .map_err(|e| RecvError::Backend(Error::Io(e)))?;
526
527                let recv = guard.try_io(|inner| {
528                    let fd = inner.get_ref().as_raw_fd();
529                    let read = unsafe {
530                        libc::recv(
531                            fd,
532                            out.as_mut_ptr().cast(),
533                            out.len(),
534                            libc::MSG_DONTWAIT | libc::MSG_TRUNC,
535                        )
536                    };
537                    if read < 0 {
538                        Err(io::Error::last_os_error())
539                    } else {
540                        Ok(read as usize)
541                    }
542                });
543
544                match recv {
545                    Ok(Ok(read)) => {
546                        if read == 0 {
547                            return Err(RecvError::Backend(Error::Io(io::Error::new(
548                                io::ErrorKind::UnexpectedEof,
549                                "iso-tp socket returned 0 bytes",
550                            ))));
551                        }
552                        if read > out.len() {
553                            return Err(RecvError::BufferTooSmall {
554                                needed: read,
555                                got: out.len(),
556                            });
557                        }
558                        return Ok(RecvMetaIntoStatus::DeliveredOne {
559                            meta: RecvMeta { reply_to: 0 },
560                            len: read,
561                        });
562                    }
563                    Ok(Err(err)) => {
564                        if err.kind() == io::ErrorKind::Interrupted {
565                            continue;
566                        }
567                        if err.kind() == io::ErrorKind::WouldBlock {
568                            guard.clear_ready();
569                            continue;
570                        }
571                        return Err(RecvError::Backend(Error::Io(err)));
572                    }
573                    Err(_would_block) => {
574                        continue;
575                    }
576                }
577            }
578        })
579        .await;
580
581        match res {
582            Ok(v) => v,
583            Err(_) => Ok(RecvMetaIntoStatus::TimedOut),
584        }
585    }
586}
587
588/// Kernel-backed ISO-TP demux for UDS normal-fixed addressing (`0x18DA_TA_SA`).
589#[derive(Debug)]
590pub struct KernelUdsDemux {
591    iface: String,
592    local_addr: u8,
593    options: IsoTpKernelOptions,
594    rx_flow_control: RxFlowControl,
595    peers: Vec<PeerSocket>,
596}
597
598#[derive(Debug)]
599struct PeerSocket {
600    addr: u8,
601    socket: SocketCanIsoTp,
602}
603
604impl KernelUdsDemux {
605    /// Create a demux for `local_addr` on `iface` using the provided kernel options.
606    pub fn new(iface: impl Into<String>, local_addr: u8, options: IsoTpKernelOptions) -> Self {
607        let rx_flow_control = match options.flow_control {
608            Some(fc) => RxFlowControl {
609                block_size: fc.block_size,
610                st_min: isotp_stmin_to_duration(fc.st_min).unwrap_or(Duration::from_millis(0)),
611            },
612            None => RxFlowControl {
613                block_size: 0,
614                st_min: Duration::from_millis(0),
615            },
616        };
617        Self {
618            iface: iface.into(),
619            local_addr,
620            options,
621            rx_flow_control,
622            peers: Vec::new(),
623        }
624    }
625
626    /// UDS local address (target address).
627    pub fn local_addr(&self) -> u8 {
628        self.local_addr
629    }
630
631    /// Register a peer address and open a dedicated kernel ISO-TP socket for it.
632    pub fn register_peer(&mut self, peer: u8) -> Result<(), Error> {
633        let _ = self.ensure_peer(peer)?;
634        Ok(())
635    }
636
637    fn ensure_peer(&mut self, peer: u8) -> Result<&mut SocketCanIsoTp, Error> {
638        if peer == self.local_addr {
639            return Err(Error::InvalidConfig(
640                "peer address must differ from local_addr",
641            ));
642        }
643
644        if let Some(idx) = self.peers.iter().position(|p| p.addr == peer) {
645            return Ok(&mut self.peers[idx].socket);
646        }
647
648        let rx_id = uds_phys_id(self.local_addr, peer);
649        let tx_id = uds_phys_id(peer, self.local_addr);
650        let mut socket = SocketCanIsoTp::open(&self.iface, rx_id, tx_id, &self.options)?;
651        let _ = socket.set_rx_flow_control(self.rx_flow_control);
652        self.peers.push(PeerSocket { addr: peer, socket });
653        let idx = self.peers.len() - 1;
654        Ok(&mut self.peers[idx].socket)
655    }
656}
657
658impl IsoTpRxFlowControlConfig for KernelUdsDemux {
659    type Error = Error;
660
661    fn set_rx_flow_control(&mut self, fc: RxFlowControl) -> Result<(), Self::Error> {
662        self.rx_flow_control = fc;
663        for peer in self.peers.iter_mut() {
664            peer.socket.set_rx_flow_control(fc)?;
665        }
666        Ok(())
667    }
668}
669
670impl can_isotp_interface::IsoTpEndpoint for KernelUdsDemux {
671    type Error = Error;
672
673    fn send_to(
674        &mut self,
675        to: u8,
676        payload: &[u8],
677        timeout: Duration,
678    ) -> Result<(), SendError<Self::Error>> {
679        let socket = self.ensure_peer(to).map_err(SendError::Backend)?;
680        socket.send_to(to, payload, timeout)
681    }
682
683    fn send_functional_to(
684        &mut self,
685        _functional_to: u8,
686        _payload: &[u8],
687        _timeout: Duration,
688    ) -> Result<(), SendError<Self::Error>> {
689        Err(SendError::Backend(Error::InvalidConfig(
690            "functional send is not supported by KernelUdsDemux",
691        )))
692    }
693
694    fn recv_one<Cb>(
695        &mut self,
696        timeout: Duration,
697        mut on_payload: Cb,
698    ) -> Result<RecvStatus, RecvError<Self::Error>>
699    where
700        Cb: FnMut(RecvMeta, &[u8]) -> Result<RecvControl, Self::Error>,
701    {
702        if self.peers.is_empty() {
703            thread::sleep(timeout);
704            return Ok(RecvStatus::TimedOut);
705        }
706
707        let mut fds: Vec<libc::pollfd> = self
708            .peers
709            .iter()
710            .map(|peer| libc::pollfd {
711                fd: peer.socket.as_raw_fd(),
712                events: libc::POLLIN,
713                revents: 0,
714            })
715            .collect();
716
717        let ready = poll_fds(&mut fds, timeout)
718            .map_err(Error::Io)
719            .map_err(RecvError::Backend)?;
720        if ready == 0 {
721            return Ok(RecvStatus::TimedOut);
722        }
723
724        for (idx, fd) in fds.iter().enumerate() {
725            if fd.revents & libc::POLLIN != 0 {
726                let reply_to = self.peers[idx].addr;
727                return self.peers[idx]
728                    .socket
729                    .recv_one_ready(|payload| on_payload(RecvMeta { reply_to }, payload));
730            }
731            if fd.revents & (libc::POLLERR | libc::POLLHUP | libc::POLLNVAL) != 0 {
732                return Err(RecvError::Backend(Error::Io(io::Error::other(
733                    "poll error on iso-tp socket",
734                ))));
735            }
736        }
737
738        Ok(RecvStatus::TimedOut)
739    }
740}
741
742fn poll_fd(fd: RawFd, events: i16, timeout: Duration) -> io::Result<bool> {
743    let mut fds = libc::pollfd {
744        fd,
745        events,
746        revents: 0,
747    };
748    let timeout_ms = duration_to_poll_timeout(timeout);
749    loop {
750        let res = unsafe { libc::poll(&mut fds, 1, timeout_ms) };
751        if res >= 0 {
752            return Ok(res > 0);
753        }
754        let err = io::Error::last_os_error();
755        if err.kind() == io::ErrorKind::Interrupted {
756            continue;
757        }
758        return Err(err);
759    }
760}
761
762fn poll_fds(fds: &mut [libc::pollfd], timeout: Duration) -> io::Result<i32> {
763    let timeout_ms = duration_to_poll_timeout(timeout);
764    loop {
765        let res = unsafe { libc::poll(fds.as_mut_ptr(), fds.len() as u64, timeout_ms) };
766        if res >= 0 {
767            return Ok(res);
768        }
769        let err = io::Error::last_os_error();
770        if err.kind() == io::ErrorKind::Interrupted {
771            continue;
772        }
773        return Err(err);
774    }
775}
776
777fn duration_to_poll_timeout(timeout: Duration) -> i32 {
778    let ms = timeout.as_millis();
779    i32::try_from(ms).unwrap_or(i32::MAX)
780}
781
782fn duration_to_nanos_u32(d: Duration) -> u32 {
783    d.as_nanos().min(u32::MAX as u128) as u32
784}
785
786fn isotp_stmin_to_duration(raw: u8) -> Option<Duration> {
787    match raw {
788        0x00..=0x7F => Some(Duration::from_millis(raw as u64)),
789        0xF1..=0xF9 => Some(Duration::from_micros((raw as u64 - 0xF0) * 100)),
790        _ => None,
791    }
792}
793
794fn duration_to_isotp_stmin(duration: Duration) -> u8 {
795    let micros = duration.as_micros();
796    if micros == 0 {
797        return 0;
798    }
799    if (100..=900).contains(&micros) && micros.is_multiple_of(100) {
800        return 0xF0 + (micros / 100) as u8;
801    }
802    let millis = duration.as_millis();
803    if millis <= 0x7F { millis as u8 } else { 0x7F }
804}
805
806fn uds_phys_id(target: u8, source: u8) -> Id {
807    let raw = uds29::encode_phys_id_raw(target, source);
808    let ext = ExtendedId::new(raw).expect("UDS 29-bit ID must fit in 29 bits");
809    Id::Extended(ext)
810}
811
812fn apply_kernel_options(fd: RawFd, options: &IsoTpKernelOptions) -> Result<(), Error> {
813    if let Some(txtime) = options.socket.frame_txtime {
814        let nanos = duration_to_nanos_u32(txtime);
815        let res = unsafe {
816            libc::setsockopt(
817                fd,
818                libc::SOL_SOCKET,
819                libc::SO_TXTIME,
820                &nanos as *const u32 as *const libc::c_void,
821                size_of::<u32>() as libc::socklen_t,
822            )
823        };
824        if res < 0 {
825            return Err(Error::Io(io::Error::last_os_error()));
826        }
827    }
828
829    let base = build_can_isotp_options(&options.socket);
830    let res = unsafe {
831        libc::setsockopt(
832            fd,
833            SOL_CAN_ISOTP,
834            CAN_ISOTP_OPTS,
835            &base as *const can_isotp_options as *const libc::c_void,
836            size_of::<can_isotp_options>() as libc::socklen_t,
837        )
838    };
839    if res < 0 {
840        return Err(Error::Io(io::Error::last_os_error()));
841    }
842
843    if let Some(fc) = options.flow_control {
844        let c = can_isotp_fc_options {
845            bs: fc.block_size,
846            stmin: fc.st_min,
847            wftmax: fc.wft_max,
848        };
849        let res = unsafe {
850            libc::setsockopt(
851                fd,
852                SOL_CAN_ISOTP,
853                CAN_ISOTP_RECV_FC,
854                &c as *const can_isotp_fc_options as *const libc::c_void,
855                size_of::<can_isotp_fc_options>() as libc::socklen_t,
856            )
857        };
858        if res < 0 {
859            return Err(Error::Io(io::Error::last_os_error()));
860        }
861    }
862
863    if let Some(stmin) = options.force_tx_stmin {
864        let nanos = duration_to_nanos_u32(stmin);
865        let res = unsafe {
866            libc::setsockopt(
867                fd,
868                SOL_CAN_ISOTP,
869                CAN_ISOTP_TX_STMIN,
870                &nanos as *const u32 as *const libc::c_void,
871                size_of::<u32>() as libc::socklen_t,
872            )
873        };
874        if res < 0 {
875            return Err(Error::Io(io::Error::last_os_error()));
876        }
877    }
878
879    if let Some(stmin) = options.force_rx_stmin {
880        let nanos = duration_to_nanos_u32(stmin);
881        let res = unsafe {
882            libc::setsockopt(
883                fd,
884                SOL_CAN_ISOTP,
885                CAN_ISOTP_RX_STMIN,
886                &nanos as *const u32 as *const libc::c_void,
887                size_of::<u32>() as libc::socklen_t,
888            )
889        };
890        if res < 0 {
891            return Err(Error::Io(io::Error::last_os_error()));
892        }
893    }
894
895    if let Some(ll) = options.link_layer {
896        let c = can_isotp_ll_options {
897            mtu: ll.mtu,
898            tx_dl: ll.tx_dl,
899            tx_flags: ll.tx_flags,
900        };
901        let res = unsafe {
902            libc::setsockopt(
903                fd,
904                SOL_CAN_ISOTP,
905                CAN_ISOTP_LL_OPTS,
906                &c as *const can_isotp_ll_options as *const libc::c_void,
907                size_of::<can_isotp_ll_options>() as libc::socklen_t,
908            )
909        };
910        if res < 0 {
911            return Err(Error::Io(io::Error::last_os_error()));
912        }
913    }
914
915    Ok(())
916}
917
918fn build_can_isotp_options(opts: &IsoTpSocketOptions) -> can_isotp_options {
919    let mut flags = opts.flags;
920    if opts.ext_address.is_some() {
921        flags |= CAN_ISOTP_EXTEND_ADDR;
922    }
923    if opts.tx_padding.is_some() {
924        flags |= CAN_ISOTP_TX_PADDING;
925    }
926    if opts.rx_padding.is_some() {
927        flags |= CAN_ISOTP_RX_PADDING;
928    }
929    if opts.rx_ext_address.is_some() {
930        flags |= CAN_ISOTP_RX_EXT_ADDR;
931    }
932
933    can_isotp_options {
934        flags,
935        frame_txtime: opts.frame_txtime.map(duration_to_nanos_u32).unwrap_or(0),
936        ext_address: opts.ext_address.unwrap_or(0),
937        txpad_content: opts.tx_padding.unwrap_or(0),
938        rxpad_content: opts.rx_padding.unwrap_or(0),
939        rx_ext_address: opts.rx_ext_address.unwrap_or(0),
940    }
941}
942
943#[repr(C)]
944#[derive(Debug, Clone, Copy)]
945struct can_isotp_options {
946    flags: u32,
947    frame_txtime: u32,
948    ext_address: u8,
949    txpad_content: u8,
950    rxpad_content: u8,
951    rx_ext_address: u8,
952}
953
954#[repr(C)]
955#[derive(Debug, Clone, Copy)]
956struct can_isotp_fc_options {
957    bs: u8,
958    stmin: u8,
959    wftmax: u8,
960}
961
962#[repr(C)]
963#[derive(Debug, Clone, Copy)]
964struct can_isotp_ll_options {
965    mtu: u8,
966    tx_dl: u8,
967    tx_flags: u8,
968}
969
970const SOL_CAN_ISOTP: i32 = libc::SOL_CAN_BASE + libc::CAN_ISOTP;
971const CAN_ISOTP_OPTS: i32 = 1;
972const CAN_ISOTP_RECV_FC: i32 = 2;
973const CAN_ISOTP_TX_STMIN: i32 = 3;
974const CAN_ISOTP_RX_STMIN: i32 = 4;
975const CAN_ISOTP_LL_OPTS: i32 = 5;
976
977/// Kernel ISO-TP flag constants.
978pub mod flags {
979    /// Listen-only mode (do not send FC frames).
980    pub const CAN_ISOTP_LISTEN_MODE: u32 = 0x0001;
981    /// Enable extended addressing.
982    pub const CAN_ISOTP_EXTEND_ADDR: u32 = 0x0002;
983    /// Enable CAN frame padding on the TX path.
984    pub const CAN_ISOTP_TX_PADDING: u32 = 0x0004;
985    /// Enable CAN frame padding on the RX path.
986    pub const CAN_ISOTP_RX_PADDING: u32 = 0x0008;
987    /// Check received CAN frame padding length.
988    pub const CAN_ISOTP_CHK_PAD_LEN: u32 = 0x0010;
989    /// Check received CAN frame padding content.
990    pub const CAN_ISOTP_CHK_PAD_DATA: u32 = 0x0020;
991    /// Half-duplex error state handling.
992    pub const CAN_ISOTP_HALF_DUPLEX: u32 = 0x0040;
993    /// Ignore STmin from received FC frames.
994    pub const CAN_ISOTP_FORCE_TXSTMIN: u32 = 0x0080;
995    /// Ignore CFs depending on RX STmin.
996    pub const CAN_ISOTP_FORCE_RXSTMIN: u32 = 0x0100;
997    /// Different RX extended addressing.
998    pub const CAN_ISOTP_RX_EXT_ADDR: u32 = 0x0200;
999    /// Wait for TX completion.
1000    pub const CAN_ISOTP_WAIT_TX_DONE: u32 = 0x0400;
1001    /// 1-to-N functional addressing (single frame).
1002    pub const CAN_ISOTP_SF_BROADCAST: u32 = 0x0800;
1003    /// 1-to-N transmission without FC.
1004    pub const CAN_ISOTP_CF_BROADCAST: u32 = 0x1000;
1005    /// Dynamic FC parameters.
1006    pub const CAN_ISOTP_DYN_FC_PARMS: u32 = 0x2000;
1007}
1008
1009const CAN_ISOTP_EXTEND_ADDR: u32 = flags::CAN_ISOTP_EXTEND_ADDR;
1010const CAN_ISOTP_TX_PADDING: u32 = flags::CAN_ISOTP_TX_PADDING;
1011const CAN_ISOTP_RX_PADDING: u32 = flags::CAN_ISOTP_RX_PADDING;
1012const CAN_ISOTP_RX_EXT_ADDR: u32 = flags::CAN_ISOTP_RX_EXT_ADDR;
1013
1014#[cfg(test)]
1015mod tests {
1016    use super::*;
1017
1018    #[test]
1019    fn can_isotp_options_layout() {
1020        assert_eq!(size_of::<can_isotp_options>(), 12);
1021        assert_eq!(size_of::<can_isotp_fc_options>(), 3);
1022        assert_eq!(size_of::<can_isotp_ll_options>(), 3);
1023    }
1024
1025    #[test]
1026    fn duration_to_nanos_clamps() {
1027        let huge = Duration::from_secs(u32::MAX as u64 + 42);
1028        assert_eq!(super::duration_to_nanos_u32(huge), u32::MAX);
1029    }
1030
1031    #[test]
1032    fn stmin_duration_round_trips_common_values() {
1033        assert_eq!(
1034            super::duration_to_isotp_stmin(Duration::from_millis(0)),
1035            0x00
1036        );
1037        assert_eq!(
1038            super::duration_to_isotp_stmin(Duration::from_millis(10)),
1039            0x0A
1040        );
1041        assert_eq!(
1042            super::duration_to_isotp_stmin(Duration::from_micros(100)),
1043            0xF1
1044        );
1045        assert_eq!(
1046            super::duration_to_isotp_stmin(Duration::from_micros(900)),
1047            0xF9
1048        );
1049        assert_eq!(
1050            super::isotp_stmin_to_duration(0xF4),
1051            Some(Duration::from_micros(400))
1052        );
1053    }
1054
1055    #[test]
1056    fn options_flags_from_padding_and_ext_addr() {
1057        let opts = IsoTpSocketOptions {
1058            ext_address: Some(0x12),
1059            rx_ext_address: Some(0x34),
1060            tx_padding: Some(0xAA),
1061            rx_padding: Some(0xBB),
1062            ..IsoTpSocketOptions::default()
1063        };
1064        let c = build_can_isotp_options(&opts);
1065        assert_eq!(c.ext_address, 0x12);
1066        assert_eq!(c.rx_ext_address, 0x34);
1067        assert_eq!(c.txpad_content, 0xAA);
1068        assert_eq!(c.rxpad_content, 0xBB);
1069        assert!(c.flags & CAN_ISOTP_EXTEND_ADDR != 0);
1070        assert!(c.flags & CAN_ISOTP_RX_EXT_ADDR != 0);
1071        assert!(c.flags & CAN_ISOTP_TX_PADDING != 0);
1072        assert!(c.flags & CAN_ISOTP_RX_PADDING != 0);
1073    }
1074}
linux_socketcan_iso_tp/linux.rs

linux_socketcan_iso_tp/
linux.rs
