1#![deny(clippy::all)]
2use libc::{
34 c_int, c_short, c_uint, c_void, close, connect, fcntl, read, sockaddr, socket, suseconds_t,
35 time_t, timeval, write, F_SETFL, O_NONBLOCK,
36};
37
38use bitflags::bitflags;
39use core::convert::TryFrom;
40use futures::prelude::*;
41use futures::ready;
42use futures::task::Context;
43use mio::event::Evented;
44use mio::unix::EventedFd;
45use mio::{unix::UnixReady, PollOpt, Ready, Token};
46use nix::net::if_::if_nametoindex;
47use socketcan::{EFF_FLAG, EFF_MASK, SFF_MASK};
48use std::fmt;
49use std::io::{Error, ErrorKind};
50use std::mem::size_of;
51use std::pin::Pin;
52use std::task::Poll;
53use std::{io, slice, time};
54use tokio::io::PollEvented;
55
56pub use socketcan::CANFrame;
58
59#[cfg(test)]
60mod tests {
61
62 use super::*;
63 use std::convert::TryFrom;
64
65 #[test]
66 fn eff_with_eff_bit_is_stripped_of_bit() {
67 let can_id = CANMessageId::try_from(0x98FE_F5EBu32);
68 assert_eq!(Ok(CANMessageId::EFF(0x18FE_F5EB)), can_id);
69 }
70}
71
72pub const AF_CAN: c_int = 29;
74pub const PF_CAN: c_int = AF_CAN;
76
77pub const CAN_BCM: c_int = 2;
78
79pub const SOCK_DGRAM: c_int = 2;
81
82const SFF_MASK_U16: u16 = 0x07ff;
83
84pub const MAX_NFRAMES: u32 = 256;
85
86pub const TX_SETUP: u32 = 1;
90pub const TX_DELETE: u32 = 2;
92pub const TX_READ: u32 = 3;
94pub const TX_SEND: u32 = 4;
96pub const RX_SETUP: u32 = 5;
98pub const RX_DELETE: u32 = 6;
100pub const RX_READ: u32 = 7;
102pub const TX_STATUS: u32 = 8;
104pub const TX_EXPIRED: u32 = 9;
106pub const RX_STATUS: u32 = 10;
108pub const RX_TIMEOUT: u32 = 11;
110pub const RX_CHANGED: u32 = 12;
112
113pub const SETTIMER: u32 = 0x0001;
117pub const STARTTIMER: u32 = 0x0002;
120pub const TX_COUNTEVT: u32 = 0x0004;
122pub const TX_ANNOUNCE: u32 = 0x0008;
125pub const TX_CP_CAN_ID: u32 = 0x0010;
128pub const RX_FILTER_ID: u32 = 0x0020;
130pub const RX_CHECK_DLC: u32 = 0x0040;
132pub const RX_NO_AUTOTIMER: u32 = 0x0080;
136pub const RX_ANNOUNCE_RESUM: u32 = 0x0100;
141pub const TX_RESET_MULTI_ID: u32 = 0x0200;
144pub const RX_RTR_FRAME: u32 = 0x0400;
146pub const CAN_FD_FRAME: u32 = 0x0800;
147
148#[repr(C)]
152pub struct BcmMsgHead {
153 _opcode: u32,
154 _flags: u32,
155 _count: u32,
157 _ival1: timeval,
159 _ival2: timeval,
161 _can_id: u32,
162 _nframes: u32,
164 #[cfg(all(target_pointer_width = "32"))]
166 _pad: u32,
167 _frames: [CANFrame; MAX_NFRAMES as usize],
170}
171
172impl BcmMsgHead {
173 pub fn can_id(&self) -> u32 {
174 self._can_id
175 }
176
177 #[inline]
178 pub fn frames(&self) -> &[CANFrame] {
179 unsafe { slice::from_raw_parts(self._frames.as_ptr(), self._nframes as usize) }
180 }
181}
182
183impl fmt::Debug for BcmMsgHead {
184 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
185 write!(f, "BcmMsgHead {{ _opcode: {}, _flags: {} , _count: {}, _ival1: {:?}, _ival2: {:?}, _can_id: {}, _nframes: {}}}", self._opcode, self._flags, self._count, self._ival1.tv_sec, self._ival2.tv_sec, self._can_id, self._nframes)
186 }
187}
188
189#[repr(C)]
194pub struct BcmMsgHeadFrameLess {
195 _opcode: u32,
196 _flags: u32,
197 _count: u32,
199 _ival1: timeval,
201 _ival2: timeval,
203 _can_id: u32,
204 _nframes: u32,
206 #[cfg(all(target_pointer_width = "32"))]
210 _pad: usize,
211}
212
213#[repr(C)]
214pub struct TxMsg {
215 _msg_head: BcmMsgHeadFrameLess,
216 _frames: [CANFrame; MAX_NFRAMES as usize],
217}
218
219#[derive(Debug)]
221pub struct BCMSocket {
222 pub fd: c_int,
223}
224
225pub struct BcmFrameStream {
226 io: PollEvented<BCMSocket>,
227}
228
229impl BcmFrameStream {
230 pub fn new(socket: BCMSocket) -> io::Result<BcmFrameStream> {
231 let io = PollEvented::new(socket)?;
232 Ok(BcmFrameStream { io })
233 }
234}
235
236impl Stream for BcmFrameStream {
237 type Item = io::Result<CANFrame>;
238
239 fn poll_next(self: Pin<&mut Self>, cx: &mut Context) -> Poll<Option<Self::Item>> {
240 let ready = Ready::readable();
241
242 ready!(self
243 .io
244 .poll_read_ready(cx, Ready::readable() | UnixReady::error()))?;
245
246 match self.io.get_ref().read_msg() {
247 Ok(n) => {
248 if let Some(frame) = n.frames().iter().next() {
249 Poll::Ready(Some(Ok(*frame)))
250 } else {
251 self.io.clear_read_ready(cx, ready)?;
253 Poll::Pending
254 }
255 }
256 Err(err) => {
257 if err.kind() == io::ErrorKind::WouldBlock {
258 self.io.clear_read_ready(cx, ready)?;
259 return Poll::Pending;
260 } else {
261 Poll::Ready(Some(Err(err)))
262 }
263 }
264 }
265 }
266}
267
268impl Evented for BcmFrameStream {
269 fn register(
270 &self,
271 poll: &mio::Poll,
272 token: Token,
273 interest: Ready,
274 opts: PollOpt,
275 ) -> io::Result<()> {
276 self.io.get_ref().register(poll, token, interest, opts)
277 }
278
279 fn reregister(
280 &self,
281 poll: &mio::Poll,
282 token: Token,
283 interest: Ready,
284 opts: PollOpt,
285 ) -> io::Result<()> {
286 self.io.get_ref().reregister(poll, token, interest, opts)
287 }
288
289 fn deregister(&self, poll: &mio::Poll) -> io::Result<()> {
290 self.io.get_ref().deregister(poll)
291 }
292}
293
294impl BCMSocket {
295 pub fn open_nb(ifname: &str) -> io::Result<BCMSocket> {
300 let if_index = if_nametoindex(ifname).map_err(|nix_error| {
301 if let nix::Error::Sys(err_no) = nix_error {
302 io::Error::from(err_no)
303 } else {
304 panic!("unexpected nix error type: {:?}", nix_error)
305 }
306 })?;
307 BCMSocket::open_if_nb(if_index)
308 }
309
310 pub fn open_if_nb(if_index: c_uint) -> io::Result<BCMSocket> {
314 let sock_fd;
316 unsafe {
317 sock_fd = socket(PF_CAN, SOCK_DGRAM, CAN_BCM);
318 }
319
320 if sock_fd == -1 {
321 return Err(io::Error::last_os_error());
322 }
323
324 let fcntl_resp = unsafe { fcntl(sock_fd, F_SETFL, O_NONBLOCK) };
325
326 if fcntl_resp == -1 {
327 return Err(io::Error::last_os_error());
328 }
329
330 let addr = CANAddr {
331 _af_can: AF_CAN as c_short,
332 if_index: if_index as c_int,
333 rx_id: 0, tx_id: 0, };
336
337 let sockaddr_ptr = &addr as *const CANAddr;
338
339 let connect_res;
340 unsafe {
341 connect_res = connect(
342 sock_fd,
343 sockaddr_ptr as *const sockaddr,
344 size_of::<CANAddr>() as u32,
345 );
346 }
347
348 if connect_res != 0 {
349 return Err(io::Error::last_os_error());
350 }
351
352 Ok(BCMSocket { fd: sock_fd })
353 }
354
355 fn close(&mut self) -> io::Result<()> {
356 unsafe {
357 let rv = close(self.fd);
358 if rv != -1 {
359 return Err(io::Error::last_os_error());
360 }
361 }
362 Ok(())
363 }
364
365 pub fn filter_id(
367 &self,
368 can_id: CANMessageId,
369 ival1: time::Duration,
370 ival2: time::Duration,
371 ) -> io::Result<()> {
372 let _ival1 = c_timeval_new(ival1);
373 let _ival2 = c_timeval_new(ival2);
374
375 let frames = [CANFrame::new(0x0, &[], false, false).unwrap(); MAX_NFRAMES as usize];
376 let msg = BcmMsgHeadFrameLess {
377 _opcode: RX_SETUP,
378 _flags: SETTIMER | RX_FILTER_ID,
379 _count: 0,
380 #[cfg(all(target_pointer_width = "32"))]
381 _pad: 0,
382 _ival1,
383 _ival2,
384 _can_id: can_id.with_eff_bit(),
385 _nframes: 0,
386 };
387
388 let tx_msg = &TxMsg {
389 _msg_head: msg,
390 _frames: frames,
391 };
392
393 let tx_msg_ptr = tx_msg as *const TxMsg;
394
395 let write_rv = unsafe { write(self.fd, tx_msg_ptr as *const c_void, size_of::<TxMsg>()) };
396
397 if write_rv < 0 {
398 return Err(Error::new(ErrorKind::WriteZero, io::Error::last_os_error()));
399 }
400
401 Ok(())
402 }
403
404 pub fn filter_id_incoming_frames(
429 self,
430 can_id: CANMessageId,
431 ival1: time::Duration,
432 ival2: time::Duration,
433 ) -> io::Result<BcmFrameStream> {
434 self.filter_id(can_id, ival1, ival2)?;
435 self.incoming_frames()
436 }
437
438 pub fn incoming_msg(self) -> io::Result<BcmStream> {
463 BcmStream::from(self)
464 }
465
466 pub fn incoming_frames(self) -> io::Result<BcmFrameStream> {
491 BcmFrameStream::new(self)
492 }
493
494 pub fn filter_delete(&self, can_id: CANMessageId) -> io::Result<()> {
496 let frames = [CANFrame::new(0x0, &[], false, false).unwrap(); MAX_NFRAMES as usize];
497
498 let msg = &BcmMsgHead {
499 _opcode: RX_DELETE,
500 _flags: 0,
501 _count: 0,
502 _ival1: c_timeval_new(time::Duration::new(0, 0)),
503 _ival2: c_timeval_new(time::Duration::new(0, 0)),
504 _can_id: can_id.with_eff_bit(),
505 _nframes: 0,
506 #[cfg(all(target_pointer_width = "32"))]
507 _pad: 0,
508 _frames: frames,
509 };
510
511 let msg_ptr = msg as *const BcmMsgHead;
512 let write_rv = unsafe { write(self.fd, msg_ptr as *const c_void, size_of::<BcmMsgHead>()) };
513
514 let expected_size = size_of::<BcmMsgHead>() - size_of::<[CANFrame; MAX_NFRAMES as usize]>();
515 if write_rv as usize != expected_size {
516 let msg = format!("Wrote {} but expected {}", write_rv, expected_size);
517 return Err(Error::new(ErrorKind::WriteZero, msg));
518 }
519
520 Ok(())
521 }
522
523 pub fn read_msg(&self) -> io::Result<BcmMsgHead> {
525 let ival1 = c_timeval_new(time::Duration::from_millis(0));
526 let ival2 = c_timeval_new(time::Duration::from_millis(0));
527 let frames = [CANFrame::new(0x0, &[], false, false).unwrap(); MAX_NFRAMES as usize];
528 let mut msg = BcmMsgHead {
529 _opcode: 0,
530 _flags: 0,
531 _count: 0,
532 _ival1: ival1,
533 _ival2: ival2,
534 _can_id: 0,
535 _nframes: 0,
536 #[cfg(all(target_pointer_width = "32"))]
537 _pad: 0,
538 _frames: frames,
539 };
540
541 let msg_ptr = &mut msg as *mut BcmMsgHead;
542 let count = unsafe { read(self.fd, msg_ptr as *mut c_void, size_of::<BcmMsgHead>()) };
543
544 let last_error = io::Error::last_os_error();
545 if count < 0 {
546 Err(last_error)
547 } else {
548 Ok(msg)
549 }
550 }
551}
552
553impl Evented for BCMSocket {
554 fn register(
555 &self,
556 poll: &mio::Poll,
557 token: Token,
558 interest: Ready,
559 opts: PollOpt,
560 ) -> io::Result<()> {
561 EventedFd(&self.fd).register(poll, token, interest, opts)
562 }
563
564 fn reregister(
565 &self,
566 poll: &mio::Poll,
567 token: Token,
568 interest: Ready,
569 opts: PollOpt,
570 ) -> io::Result<()> {
571 EventedFd(&self.fd).reregister(poll, token, interest, opts)
572 }
573
574 fn deregister(&self, poll: &mio::Poll) -> io::Result<()> {
575 EventedFd(&self.fd).deregister(poll)
576 }
577}
578
579impl Drop for BCMSocket {
580 fn drop(&mut self) {
581 self.close().ok(); }
583}
584
585pub struct BcmStream {
586 io: PollEvented<BCMSocket>,
587}
588
589pub trait IntoBcmStream {
590 type Stream: futures::stream::Stream;
591 type Error;
592
593 fn into_bcm(self) -> Result<Self::Stream, Self::Error>;
594}
595
596impl BcmStream {
597 pub fn from(bcm_socket: BCMSocket) -> io::Result<BcmStream> {
598 let io = PollEvented::new(bcm_socket)?;
599 Ok(BcmStream { io })
600 }
601}
602
603impl Stream for BcmStream {
604 type Item = io::Result<BcmMsgHead>;
605
606 fn poll_next(self: Pin<&mut Self>, cx: &mut Context) -> Poll<Option<Self::Item>> {
607 ready!(self
608 .io
609 .poll_read_ready(cx, Ready::readable() | UnixReady::error()))?;
610
611 match self.io.get_ref().read_msg() {
612 Ok(msg) => Poll::Ready(Some(Ok(msg))),
613 Err(err) => {
614 if err.kind() == io::ErrorKind::WouldBlock {
615 self.io.clear_read_ready(cx, Ready::readable())?;
616 Poll::Pending
617 } else {
618 Poll::Ready(Some(Err(err)))
619 }
620 }
621 }
622 }
623}
624
625bitflags! {
626 #[derive(Default)]
627 pub struct FrameFlags: u32 {
628 const EFF_FLAG = 0x8000_0000;
630
631 const RTR_FLAG = 0x4000_0000;
633
634 const ERR_FLAG = 0x2000_0000;
636 }
637}
638
639#[derive(Debug)]
640#[repr(C)]
641pub struct CANAddr {
642 pub _af_can: c_short,
643 pub if_index: c_int, pub rx_id: u32,
645 pub tx_id: u32,
646}
647
648#[derive(Copy, Clone, Debug, Eq, PartialEq, PartialOrd, Hash)]
650pub enum CANMessageId {
651 SFF(u16),
653 EFF(u32),
655}
656
657impl fmt::Display for CANMessageId {
658 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
659 match *self {
660 CANMessageId::SFF(id) => write!(f, "{}", id),
661 CANMessageId::EFF(id) => write!(f, "{}", id),
662 }
663 }
664}
665
666impl CANMessageId {
667 pub fn with_eff_bit(self) -> u32 {
668 match self {
669 CANMessageId::SFF(id) => u32::from(id),
670 CANMessageId::EFF(id) => id | FrameFlags::EFF_FLAG.bits(),
671 }
672 }
673}
674
675impl From<u16> for CANMessageId {
676 fn from(id: u16) -> CANMessageId {
677 match id {
678 0..=SFF_MASK_U16 => CANMessageId::SFF(id),
679 SFF_MASK_U16..=std::u16::MAX => CANMessageId::EFF(u32::from(id)),
680 }
681 }
682}
683
684impl TryFrom<u32> for CANMessageId {
685 type Error = ConstructionError;
686
687 fn try_from(id: u32) -> Result<CANMessageId, ConstructionError> {
688 match id {
689 0...SFF_MASK => Ok(CANMessageId::SFF(id as u16)),
690 SFF_MASK...EFF_MASK => Ok(CANMessageId::EFF(id)),
691 _ => {
692 if id & EFF_FLAG != 0 {
694 let without_flag = id & EFF_MASK;
695 Ok(CANMessageId::EFF(without_flag))
696 } else {
697 Err(ConstructionError::IDTooLarge)
698 }
699 }
700 }
701 }
702}
703
704impl From<CANMessageId> for u32 {
705 fn from(id: CANMessageId) -> u32 {
706 match id {
707 CANMessageId::SFF(id) => u32::from(id),
708 CANMessageId::EFF(id) => id,
709 }
710 }
711}
712
713#[derive(Debug, Copy, Clone, PartialEq)]
714pub enum ConstructionError {
716 IDTooLarge,
718}
719
720impl fmt::Display for ConstructionError {
721 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
722 match *self {
723 ConstructionError::IDTooLarge => write!(f, "CAN ID too large"),
724 }
725 }
726}
727
728impl std::error::Error for ConstructionError {
729 fn description(&self) -> &str {
730 match *self {
731 ConstructionError::IDTooLarge => "can id too large",
732 }
733 }
734}
735
736fn c_timeval_new(t: time::Duration) -> timeval {
737 timeval {
738 tv_sec: t.as_secs() as time_t,
739 tv_usec: i64::from(t.subsec_micros()) as suseconds_t,
740 }
741}