socketcan_rs/
driver.rs

1use std::{collections::HashMap, io, sync::Arc, os::{fd::{AsRawFd, BorrowedFd, FromRawFd, OwnedFd}, raw::{c_int, c_void}}, time::{Instant, Duration}};
2use libc::{can_filter, can_frame, canfd_frame, canxl_frame, fcntl, read, CAN_RAW_ERR_FILTER, CAN_RAW_FILTER, CAN_RAW_JOIN_FILTERS, CAN_RAW_LOOPBACK, CAN_RAW_RECV_OWN_MSGS, EINPROGRESS, F_GETFL, F_SETFL, O_NONBLOCK, SOL_CAN_RAW, SOL_SOCKET, SO_RCVTIMEO, SO_SNDTIMEO};
3use rs_can::{CanError, CanFilter, CanDirect, CanFrame, ERR_MASK};
4use crate::{c_timeval_new, raw_open_socket, raw_write_frame, set_fd_mode, set_socket_option, set_socket_option_mult, CanAddr, CanAnyFrame, CanMessage};
5
6pub(crate) const FRAME_SIZE: usize = std::mem::size_of::<can_frame>();
7pub(crate) const FD_FRAME_SIZE: usize = std::mem::size_of::<canfd_frame>();
8pub(crate) const XL_FRAME_SIZE: usize = std::mem::size_of::<canxl_frame>();
9
10#[derive(Debug, Clone)]
11pub struct SocketCan {
12    pub(crate) sockets: Arc<HashMap<String, OwnedFd>>,
13}
14
15impl SocketCan {
16    pub fn new() -> Self {
17        Self { sockets: Default::default() }
18    }
19
20    pub fn init_channel(&mut self, channel: &str, canfd: bool) -> Result<(), CanError> {
21        let addr = CanAddr::from_iface(channel)
22            .map_err(|e| CanError::InitializeError(e.to_string()))?;
23
24        let _ = raw_open_socket(&addr)
25            .and_then(|fd| {
26                set_fd_mode(fd, canfd)
27            })
28            .and_then(|fd| {
29                Arc::get_mut(&mut self.sockets)
30                    .ok_or(io::Error::last_os_error())?
31                    .insert(channel.to_owned(), unsafe { OwnedFd::from_raw_fd(fd) });
32                // Rc::get_mut(&mut self.sockets)
33                //     .ok_or(io::Error::last_os_error())?
34                //     .insert(channel.to_owned(), unsafe { OwnedFd::from_raw_fd(fd) });
35                Ok(())
36            })
37            .map_err(|_| CanError::InitializeError("device open failed".into()));
38
39        Ok(())
40    }
41
42    pub fn read(&self, channel: &str) -> Result<CanMessage, CanError> {
43        match self.sockets.get(channel) {
44            Some(s) => {
45                let mut buffer = [0; XL_FRAME_SIZE];
46
47                let rd = unsafe { read(
48                    s.as_raw_fd(),
49                    &mut buffer as *mut _ as *mut c_void,
50                    XL_FRAME_SIZE
51                ) };
52                match rd as usize {
53                    FRAME_SIZE => {
54                        let frame = unsafe { *(&buffer as *const _ as *const can_frame) };
55                        let mut frame = CanMessage::from(CanAnyFrame::from(frame));
56                        frame.set_direct(CanDirect::Receive);
57                        Ok(frame)
58                    },
59                    FD_FRAME_SIZE => {
60                        let frame = unsafe { *(&buffer as *const _ as *const canfd_frame) };
61                        let mut frame = CanMessage::from(CanAnyFrame::from(frame));
62                        frame.set_direct(CanDirect::Receive);
63                        Ok(frame)
64                    },
65                    XL_FRAME_SIZE => {
66                        let frame = unsafe { *(&buffer as *const _ as *const canxl_frame) };
67                        let mut frame = CanMessage::from(CanAnyFrame::from(frame));
68                        frame.set_direct(CanDirect::Receive);
69                        Ok(frame)
70                    },
71                    _ => Err(CanError::OperationError(io::Error::last_os_error().to_string()))
72                }
73            },
74            None => Err(CanError::channel_not_opened(channel))
75        }
76    }
77
78    /// Blocking read a single can frame with timeout.
79    pub fn read_timeout(&self, channel: &str, timeout: Duration) -> Result<CanMessage, CanError> {
80        match self.sockets.get(channel) {
81            Some(s) => {
82                use nix::poll::{poll, PollFd, PollFlags};
83                let borrowed_fd = unsafe { BorrowedFd::borrow_raw(s.as_raw_fd()) };
84                let pollfd = PollFd::new(borrowed_fd, PollFlags::POLLIN);
85
86                match poll::<u16>(&mut [pollfd], timeout.as_millis() as u16)
87                    .map_err(|e| CanError::OperationError(e.to_string()))?
88                {
89                    0 => Err(CanError::channel_timeout(channel)),
90                    _ => self.read(channel),
91                }
92            },
93            None => Err(CanError::channel_not_opened(channel)),
94        }
95    }
96
97    pub fn write(&self, msg: CanMessage) -> Result<(), CanError> {
98        let channel = msg.channel();
99        match self.sockets.get(&channel) {
100            Some(s) => {
101                let frame: CanAnyFrame = msg.into();
102                match frame {
103                    CanAnyFrame::Normal(f) |
104                    CanAnyFrame::Remote(f) |
105                    CanAnyFrame::Error(f) => {
106                        raw_write_frame(s.as_raw_fd(), &f, frame.size())
107                            .map_err(|e| CanError::OtherError(e.to_string()))
108                    }
109                    CanAnyFrame::Fd(f) => {
110                        raw_write_frame(s.as_raw_fd(), &f, frame.size())
111                            .map_err(|e| CanError::OtherError(e.to_string()))
112                    },
113                    CanAnyFrame::Xl(f) => {
114                        raw_write_frame(s.as_raw_fd(), &f, frame.size())
115                            .map_err(|e| CanError::OtherError(e.to_string()))
116                    },
117                }
118            },
119            None => Err(CanError::channel_not_opened(channel))
120        }
121    }
122
123    /// Blocking write a single can frame, retrying until it gets sent successfully.
124    pub fn write_timeout(&self, msg: CanMessage, timeout: Duration) -> Result<(), CanError> {
125        let channel = msg.channel();
126        let frame: CanAnyFrame = msg.into();
127        let start = Instant::now();
128        while start.elapsed() < timeout {
129            match self.sockets.get(&channel) {
130                Some(s) => {
131                    if let Err(e) = match frame {
132                        CanAnyFrame::Normal(f) |
133                        CanAnyFrame::Remote(f) |
134                        CanAnyFrame::Error(f) => {
135                            raw_write_frame(s.as_raw_fd(), &f, frame.size())
136                        }
137                        CanAnyFrame::Fd(f) => {
138                            raw_write_frame(s.as_raw_fd(), &f, frame.size())
139                        },
140                        CanAnyFrame::Xl(f) => {
141                            raw_write_frame(s.as_raw_fd(), &f, frame.size())
142                        }
143                    } {
144                        match e.kind() {
145                            io::ErrorKind::WouldBlock => {},
146                            io::ErrorKind::Other =>
147                                if !matches!(e.raw_os_error(), Some(errno) if errno == EINPROGRESS) {
148                                    return Err(CanError::OperationError(e.to_string()));
149                                }
150                            _ => return Err(CanError::OperationError(e.to_string())),
151                        }
152                    }
153                    else {
154                        return Ok(());
155                    }
156                },
157                None => return Err(CanError::channel_not_opened(channel))
158            }
159        }
160
161        Err(CanError::channel_timeout(channel))
162    }
163
164    /// Change socket to non-blocking mode or back to blocking mode.
165    pub fn set_nonblocking(&self, channel: &str, nonblocking: bool) -> Result<(), CanError> {
166        match self.sockets.get(channel) {
167            Some(s) => {
168                // retrieve current flags
169                let oldfl = unsafe { fcntl(s.as_raw_fd(), F_GETFL) };
170
171                if oldfl == -1 {
172                    return Err(CanError::OperationError(io::Error::last_os_error().to_string()));
173                }
174
175                let newfl = if nonblocking {
176                    oldfl | O_NONBLOCK
177                } else {
178                    oldfl & !O_NONBLOCK
179                };
180
181                let ret = unsafe { fcntl(s.as_raw_fd(), F_SETFL, newfl) };
182
183                if ret != 0 {
184                    Err(CanError::OperationError(io::Error::last_os_error().to_string()))
185                }
186                else {
187                    Ok(())
188                }
189            },
190            None => Err(CanError::channel_not_opened(channel))
191        }
192    }
193
194    /// Sets the read timeout on the socket
195    ///
196    /// For convenience, the result value can be checked using
197    /// `ShouldRetry::should_retry` when a timeout is set.
198    pub fn set_read_timeout(&self, channel: &str, duration: Duration) -> Result<(), CanError> {
199        match self.sockets.get(channel) {
200            Some(s) => {
201                set_socket_option(
202                    s.as_raw_fd(),
203                    SOL_SOCKET,
204                    SO_RCVTIMEO,
205                    &c_timeval_new(duration),
206                )
207                    .map_err(|e| CanError::OperationError(e.to_string()))
208            },
209            None => Err(CanError::channel_not_opened(channel))
210        }
211    }
212
213    /// Sets the write timeout on the socket
214    pub fn set_write_timeout(&self, channel: &str, duration: Duration) -> Result<(), CanError> {
215        match self.sockets.get(channel) {
216            Some(s) => {
217                set_socket_option(
218                    s.as_raw_fd(),
219                    SOL_SOCKET,
220                    SO_SNDTIMEO,
221                    &c_timeval_new(duration),
222                )
223                    .map_err(|e| CanError::OperationError(e.to_string()))
224            },
225            None => Err(CanError::channel_not_opened(channel))
226        }
227    }
228}
229
230impl SocketCan {
231
232    /// Sets CAN ID filters on the socket.
233    ///
234    /// CAN packages received by SocketCAN are matched against these filters,
235    /// only matching packets are returned by the interface.
236    ///
237    /// See `CanFilter` for details on how filtering works. By default, all
238    /// single filter matching all incoming frames is installed.
239    pub fn set_filters(&self, channel: &str, filters: &[CanFilter]) -> Result<(), CanError> {
240        match self.sockets.get(channel) {
241            Some(s) => {
242                let filters: Vec<can_filter> = filters.iter()
243                    .map(|&f| {
244                        can_filter {
245                            can_id: f.can_id,
246                            can_mask: f.can_mask,
247                        }
248                    })
249                    .collect();
250                set_socket_option_mult(s.as_raw_fd(), SOL_CAN_RAW, CAN_RAW_FILTER, &filters)
251                    .map_err(|e| CanError::OperationError(e.to_string()))
252            },
253            None => Err(CanError::channel_not_opened(channel)),
254        }
255    }
256
257    /// Disable reception of CAN frames.
258    ///
259    /// Sets a completely empty filter; disabling all CAN frame reception.
260    pub fn set_filter_drop_all(&self, channel: &str) -> Result<(), CanError> {
261        match self.sockets.get(channel) {
262            Some(s) => {
263                let filters: &[CanFilter] = &[];
264                set_socket_option_mult(s.as_raw_fd(), SOL_CAN_RAW, CAN_RAW_FILTER, filters)
265                    .map_err(|e| CanError::OperationError(e.to_string()))
266            }
267            None => Err(CanError::channel_not_opened(channel)),
268        }
269    }
270
271    /// Accept all frames, disabling any kind of filtering.
272    ///
273    /// Replace the current filter with one containing a single rule that
274    /// accepts all CAN frames.
275    #[inline(always)]
276    pub fn set_filter_accept_all(&self, channel: &str) -> Result<(), CanError> {
277        self.set_filters(channel, &[CanFilter::from((0, 0))])
278    }
279
280    /// Sets the error mask on the socket.
281    ///
282    /// By default (`ERR_MASK_NONE`) no error conditions are reported as
283    /// special error frames by the socket. Enabling error conditions by
284    /// setting `ERR_MASK_ALL` or another non-empty error mask causes the
285    /// socket to receive notification about the specified conditions.
286    pub fn set_error_filter(&self, channel: &str, mask: u32) -> Result<(), CanError> {
287        match self.sockets.get(channel) {
288            Some(s) => {
289                set_socket_option(s.as_raw_fd(), SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &mask)
290                    .map_err(|e| CanError::OperationError(e.to_string()))
291            }
292            None => Err(CanError::channel_not_opened(channel)),
293        }
294    }
295
296    /// Sets the error mask on the socket to reject all errors.
297    #[inline(always)]
298    pub fn set_error_filter_drop_all(&self, channel: &str) -> Result<(), CanError> {
299        self.set_error_filter(channel, 0)
300    }
301
302    /// Sets the error mask on the socket to accept all errors.
303    #[inline(always)]
304    pub fn set_error_filter_accept_all(&self, channel: &str) -> Result<(), CanError> {
305        self.set_error_filter(channel, ERR_MASK)
306    }
307
308    /// Enable or disable loopback.
309    ///
310    /// By default, loopback is enabled, causing other applications that open
311    /// the same CAN bus to see frames emitted by different applications on
312    /// the same system.
313    pub fn set_loopback(&self, channel: &str, enabled: bool) -> Result<(), CanError> {
314        match self.sockets.get(channel) {
315            Some(s) => {
316                let loopback = c_int::from(enabled);
317                set_socket_option(s.as_raw_fd(), SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback)
318                    .map_err(|e| CanError::OperationError(e.to_string()))
319            }
320            None => Err(CanError::channel_not_opened(channel)),
321        }
322    }
323
324    /// Enable or disable receiving of own frames.
325    ///
326    /// When loopback is enabled, this settings controls if CAN frames sent
327    /// are received back immediately by sender. Default is off.
328    pub fn set_recv_own_msgs(&self, channel: &str, enabled: bool) -> Result<(), CanError> {
329        match self.sockets.get(channel) {
330            Some(s) => {
331                let recv_own_msgs = c_int::from(enabled);
332                set_socket_option(
333                    s.as_raw_fd(),
334                    SOL_CAN_RAW,
335                    CAN_RAW_RECV_OWN_MSGS,
336                    &recv_own_msgs,
337                )
338                    .map_err(|e| CanError::OperationError(e.to_string()))
339            }
340            None => Err(CanError::channel_not_opened(channel)),
341        }
342    }
343
344    /// Enable or disable join filters.
345    ///
346    /// By default a frame is accepted if it matches any of the filters set
347    /// with `set_filters`. If join filters is enabled, a frame has to match
348    /// _all_ filters to be accepted.
349    pub fn set_join_filters(&self, channel: &str, enabled: bool) -> Result<(), CanError> {
350        match self.sockets.get(channel) {
351            Some(s) => {
352                let join_filters = c_int::from(enabled);
353                set_socket_option(
354                    s.as_raw_fd(),
355                    SOL_CAN_RAW,
356                    CAN_RAW_JOIN_FILTERS,
357                    &join_filters,
358                )
359                    .map_err(|e| CanError::OperationError(e.to_string()))
360            }
361            None => Err(CanError::channel_not_opened(channel)),
362        }
363    }
364}
socketcan_rs/driver.rs

socketcan_rs/
driver.rs
