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

canadensis_linux/
lib.rs
