canadensis_linux/
lib.rs

1//!
2//! Utilities for running Cyphal nodes on Linux using the SocketCAN interface
3//!
4
5#![deny(missing_docs)]
6
7extern crate canadensis_can;
8extern crate canadensis_core;
9extern crate canadensis_filter_config;
10extern crate log;
11extern crate socketcan;
12
13use canadensis_can::driver::{optimize_filters, ReceiveDriver, TransmitDriver};
14use canadensis_can::{CanNodeId, Frame};
15use canadensis_core::subscription::Subscription;
16use canadensis_core::time::{Clock, Microseconds32};
17use canadensis_core::{nb, OutOfMemoryError};
18use socketcan::{CanSocket, EmbeddedFrame, Id, Socket, SocketOptions};
19use std::convert::TryInto;
20use std::io;
21use std::io::ErrorKind;
22
23/// An adapter between SocketCAN and the canadensis frame format
24pub struct LinuxCan {
25    socket: CanSocket,
26}
27
28impl LinuxCan {
29    /// Creates a Linux CAN adapter around a SocketCAN socket
30    pub fn new(socket: CanSocket) -> Self {
31        LinuxCan { socket }
32    }
33}
34
35impl TransmitDriver<SystemClock> for LinuxCan {
36    type Error = io::Error;
37
38    fn try_reserve(&mut self, _frames: usize) -> Result<(), OutOfMemoryError> {
39        // Assume there's enough space
40        Ok(())
41    }
42
43    fn transmit(
44        &mut self,
45        frame: Frame,
46        clock: &mut SystemClock,
47    ) -> nb::Result<Option<Frame>, Self::Error> {
48        // Drop this frame if its deadline has passed
49        let now = clock.now();
50        if frame.timestamp() < now {
51            log::warn!("Dropping frame that has missed its deadline");
52            return Ok(None);
53        }
54        let socketcan_frame = socketcan::CanFrame::new(
55            socketcan::Id::Extended(
56                socketcan::ExtendedId::new(frame.id().into()).expect("Invalid CAN ID"),
57            ),
58            frame.data(),
59        )
60        .expect("Invalid frame format");
61        self.socket
62            .write_frame_insist(&socketcan_frame)
63            .map(|()| None)
64            .map_err(|e| {
65                if e.kind() == ErrorKind::WouldBlock {
66                    nb::Error::WouldBlock
67                } else {
68                    nb::Error::Other(e)
69                }
70            })
71    }
72
73    fn flush(&mut self, _clock: &mut SystemClock) -> canadensis_core::nb::Result<(), Self::Error> {
74        // Presumably this happens automatically
75        Ok(())
76    }
77}
78
79impl ReceiveDriver<SystemClock> for LinuxCan {
80    type Error = io::Error;
81
82    fn receive(&mut self, clock: &mut SystemClock) -> nb::Result<Frame, Self::Error> {
83        loop {
84            let socketcan_frame = self.socket.read_frame()?;
85            if socketcan_frame.data().len() <= canadensis_can::FRAME_CAPACITY {
86                let raw_id = match socketcan_frame.id() {
87                    Id::Standard(_) => continue,
88                    Id::Extended(id) => id.as_raw(),
89                };
90                let cyphal_frame = canadensis_can::Frame::new(
91                    clock.now(),
92                    raw_id.try_into().expect("Invalid CAN ID"),
93                    socketcan_frame.data(),
94                );
95                return Ok(cyphal_frame);
96            } else {
97                log::warn!(
98                    "Ignoring a frame {} bytes long, which is too large",
99                    socketcan_frame.data().len()
100                );
101            }
102        }
103    }
104
105    fn apply_filters<S>(&mut self, local_node: Option<CanNodeId>, subscriptions: S)
106    where
107        S: IntoIterator<Item = Subscription>,
108    {
109        optimize_filters(local_node, subscriptions, usize::MAX, |optimized| {
110            let socketcan_filters = optimized
111                .iter()
112                .map(|filter| socketcan::CanFilter::new(filter.id(), filter.mask()))
113                .collect::<Vec<_>>();
114            self.socket.set_filters(&socketcan_filters).unwrap();
115        })
116        .unwrap()
117    }
118
119    fn apply_accept_all(&mut self) {
120        self.socket.set_filter_accept_all().unwrap();
121    }
122}
123
124/// A clock that uses the operating system's clock
125#[derive(Debug, Clone)]
126pub struct SystemClock {
127    start_time: std::time::Instant,
128}
129
130impl SystemClock {
131    /// Creates a new system clock
132    pub fn new() -> Self {
133        SystemClock {
134            start_time: std::time::Instant::now(),
135        }
136    }
137}
138
139impl Default for SystemClock {
140    fn default() -> Self {
141        Self::new()
142    }
143}
144
145impl Clock for SystemClock {
146    fn now(&mut self) -> Microseconds32 {
147        let since_start = std::time::Instant::now().duration_since(self.start_time);
148        let microseconds = since_start.as_micros();
149        Microseconds32::from_ticks(microseconds as u32)
150    }
151}
canadensis_linux/lib.rs

canadensis_linux/
lib.rs
