#![deny(clippy::all)]
use bitflags::bitflags;
use libc::{
bind, c_int, c_short, c_void, close, fcntl, read, setsockopt, sockaddr, socket, write, F_GETFL,
F_SETFL, O_NONBLOCK, SOCK_DGRAM,
};
use nix::net::if_::if_nametoindex;
use std::convert::TryFrom;
use std::convert::TryInto;
use std::io;
use std::mem::size_of;
use std::num::TryFromIntError;
use std::os::unix::io::{AsRawFd, FromRawFd, IntoRawFd, RawFd};
use std::time::Duration;
use thiserror::Error;
pub const AF_CAN: c_short = 29;
pub const PF_CAN: c_int = 29;
pub const CAN_ISOTP: c_int = 6;
pub const SOL_CAN_BASE: c_int = 100;
pub const SOL_CAN_ISOTP: c_int = SOL_CAN_BASE + CAN_ISOTP;
pub const CAN_ISOTP_OPTS: c_int = 1;
pub const CAN_ISOTP_RECV_FC: c_int = 2;
pub const CAN_ISOTP_TX_STMIN: c_int = 3;
pub const CAN_ISOTP_RX_STMIN: c_int = 4;
pub const CAN_ISOTP_LL_OPTS: c_int = 5;
pub const CAN_MAX_DLEN: u8 = 8;
const RECV_BUFFER_SIZE: usize = 4096;
const SIZE_OF_CAN_FRAME: u8 = 16;
const FLOW_CONTROL_OPTIONS_SIZE: usize = size_of::<FlowControlOptions>();
const ISOTP_OPTIONS_SIZE: usize = size_of::<IsoTpOptions>();
const LINK_LAYER_OPTIONS_SIZE: usize = size_of::<LinkLayerOptions>();
bitflags! {
pub struct IsoTpBehaviour: u32 {
const CAN_ISOTP_LISTEN_MODE = 0x001;
const CAN_ISOTP_EXTEND_ADDR = 0x002;
const CAN_ISOTP_TX_PADDING = 0x004;
const CAN_ISOTP_RX_PADDING = 0x008;
const CAN_ISOTP_CHK_PAD_LEN = 0x010;
const CAN_ISOTP_CHK_PAD_DATA = 0x020;
const CAN_ISOTP_HALF_DUPLEX = 0x040;
const CAN_ISOTP_FORCE_TXSTMIN = 0x080;
const CAN_ISOTP_FORCE_RXSTMIN = 0x100;
const CAN_ISOTP_RX_EXT_ADDR = 0x200;
}
}
pub const EFF_FLAG: u32 = 0x8000_0000;
pub const RTR_FLAG: u32 = 0x4000_0000;
pub const ERR_FLAG: u32 = 0x2000_0000;
pub const SFF_MASK: u32 = 0x0000_07ff;
pub const EFF_MASK: u32 = 0x1fff_ffff;
pub const ERR_MASK: u32 = 0x1fff_ffff;
pub const ERR_MASK_ALL: u32 = ERR_MASK;
pub const ERR_MASK_NONE: u32 = 0;
#[derive(Debug)]
#[repr(C)]
struct CanAddr {
_af_can: c_short,
if_index: c_int,
rx_id: u32,
tx_id: u32,
_pgn: u32,
_addr: u8,
}
#[repr(C)]
pub struct IsoTpOptions {
flags: u32,
frame_txtime: u32,
ext_address: u8,
txpad_content: u8,
rxpad_content: u8,
rx_ext_address: u8,
}
impl IsoTpOptions {
pub fn new(
flags: IsoTpBehaviour,
frame_txtime: Duration,
ext_address: u8,
txpad_content: u8,
rxpad_content: u8,
rx_ext_address: u8,
) -> Result<Self, TryFromIntError> {
let flags = flags.bits();
let frame_txtime = u32::try_from(frame_txtime.as_nanos())?;
Ok(Self {
flags,
frame_txtime,
ext_address,
txpad_content,
rxpad_content,
rx_ext_address,
})
}
pub fn get_flags(&self) -> Option<IsoTpBehaviour> {
IsoTpBehaviour::from_bits(self.flags)
}
pub fn set_flags(&mut self, flags: IsoTpBehaviour) {
self.flags = flags.bits();
}
pub fn get_frame_txtime(&self) -> Duration {
Duration::from_nanos(self.frame_txtime.into())
}
pub fn set_frame_txtime(&mut self, frame_txtime: Duration) -> Result<(), TryFromIntError> {
self.frame_txtime = u32::try_from(frame_txtime.as_nanos())?;
Ok(())
}
pub fn get_ext_address(&self) -> u8 {
self.ext_address
}
pub fn set_ext_address(&mut self, ext_address: u8) {
self.ext_address = ext_address;
}
pub fn get_txpad_content(&self) -> u8 {
self.txpad_content
}
pub fn set_txpad_content(&mut self, txpad_content: u8) {
self.txpad_content = txpad_content;
}
pub fn get_rxpad_content(&self) -> u8 {
self.rxpad_content
}
pub fn set_rxpad_content(&mut self, rxpad_content: u8) {
self.rxpad_content = rxpad_content;
}
pub fn get_rx_ext_address(&self) -> u8 {
self.rx_ext_address
}
pub fn set_rx_ext_address(&mut self, rx_ext_address: u8) {
self.rx_ext_address = rx_ext_address;
}
}
impl Default for IsoTpOptions {
fn default() -> Self {
Self {
flags: 0x00,
frame_txtime: 0x00,
ext_address: 0x00,
txpad_content: 0xCC,
rxpad_content: 0xCC,
rx_ext_address: 0x00,
}
}
}
#[repr(C)]
pub struct FlowControlOptions {
bs: u8,
stmin: u8,
wftmax: u8,
}
impl Default for FlowControlOptions {
fn default() -> Self {
Self {
bs: 0,
stmin: 0x00,
wftmax: 0,
}
}
}
bitflags! {
pub struct TxFlags: u8 {
const CANFD_BRS = 0x01;
const CANFD_ESI = 0x02;
}
}
#[repr(C)]
pub struct LinkLayerOptions {
mtu: u8,
tx_dl: u8,
tx_flags: u8,
}
impl LinkLayerOptions {
pub fn new(mtu: u8, tx_dl: u8, tx_flags: TxFlags) -> Self {
let tx_flags = tx_flags.bits();
Self {
mtu,
tx_dl,
tx_flags,
}
}
}
impl Default for LinkLayerOptions {
fn default() -> Self {
Self {
mtu: SIZE_OF_CAN_FRAME,
tx_dl: CAN_MAX_DLEN,
tx_flags: 0x00,
}
}
}
#[derive(Error, Debug)]
pub enum Error {
#[error("Failed to find can device: {source:?}")]
LookupError {
#[from]
source: nix::Error,
},
#[error("IO error: {source:?}")]
IOError {
#[from]
source: io::Error,
},
}
pub struct IsoTpSocket {
fd: c_int,
recv_buffer: [u8; RECV_BUFFER_SIZE],
}
impl IsoTpSocket {
pub fn open(ifname: &str, src: u32, dst: u32) -> Result<Self, Error> {
Self::open_with_opts(
ifname,
src,
dst,
Some(IsoTpOptions::default()),
Some(FlowControlOptions::default()),
Some(LinkLayerOptions::default()),
)
}
pub fn open_with_opts(
ifname: &str,
src: u32,
dst: u32,
isotp_options: Option<IsoTpOptions>,
rx_flow_control_options: Option<FlowControlOptions>,
link_layer_options: Option<LinkLayerOptions>,
) -> Result<Self, Error> {
let if_index = if_nametoindex(ifname)?;
Self::open_if_with_opts(
if_index.try_into().unwrap(),
src,
dst,
isotp_options,
rx_flow_control_options,
link_layer_options,
)
}
pub fn open_if(if_index: c_int, src: u32, dst: u32) -> Result<Self, Error> {
Self::open_if_with_opts(
if_index.try_into().unwrap(),
src,
dst,
Some(IsoTpOptions::default()),
Some(FlowControlOptions::default()),
Some(LinkLayerOptions::default()),
)
}
pub fn open_if_with_opts(
if_index: c_int,
src: u32,
dst: u32,
isotp_options: Option<IsoTpOptions>,
rx_flow_control_options: Option<FlowControlOptions>,
link_layer_options: Option<LinkLayerOptions>,
) -> Result<Self, Error> {
let addr = CanAddr {
_af_can: AF_CAN,
if_index,
rx_id: src,
tx_id: dst,
_pgn: 0,
_addr: 0,
};
let sock_fd;
unsafe {
sock_fd = socket(PF_CAN, SOCK_DGRAM, CAN_ISOTP);
}
if sock_fd == -1 {
return Err(Error::from(io::Error::last_os_error()));
}
if let Some(isotp_options) = isotp_options {
let isotp_options_ptr: *const c_void = &isotp_options as *const _ as *const c_void;
let err = unsafe {
setsockopt(
sock_fd,
SOL_CAN_ISOTP,
CAN_ISOTP_OPTS,
isotp_options_ptr,
ISOTP_OPTIONS_SIZE.try_into().unwrap(),
)
};
if err == -1 {
return Err(Error::from(io::Error::last_os_error()));
}
}
if let Some(rx_flow_control_options) = rx_flow_control_options {
let rx_flow_control_options_ptr: *const c_void =
&rx_flow_control_options as *const _ as *const c_void;
let err = unsafe {
setsockopt(
sock_fd,
SOL_CAN_ISOTP,
CAN_ISOTP_RECV_FC,
rx_flow_control_options_ptr,
FLOW_CONTROL_OPTIONS_SIZE.try_into().unwrap(),
)
};
if err == -1 {
return Err(Error::from(io::Error::last_os_error()));
}
}
if let Some(link_layer_options) = link_layer_options {
let link_layer_options_ptr: *const c_void =
&link_layer_options as *const _ as *const c_void;
let err = unsafe {
setsockopt(
sock_fd,
SOL_CAN_ISOTP,
CAN_ISOTP_LL_OPTS,
link_layer_options_ptr,
LINK_LAYER_OPTIONS_SIZE.try_into().unwrap(),
)
};
if err == -1 {
return Err(Error::from(io::Error::last_os_error()));
}
}
let bind_rv;
unsafe {
let sockaddr_ptr = &addr as *const CanAddr;
bind_rv = bind(
sock_fd,
sockaddr_ptr as *const sockaddr,
size_of::<CanAddr>().try_into().unwrap(),
);
}
if bind_rv == -1 {
let e = io::Error::last_os_error();
unsafe {
close(sock_fd);
}
return Err(Error::from(e));
}
Ok(Self {
fd: sock_fd,
recv_buffer: [0x00; RECV_BUFFER_SIZE],
})
}
fn close(&mut self) -> io::Result<()> {
unsafe {
let rv = close(self.fd);
if rv != -1 {
return Err(io::Error::last_os_error());
}
}
Ok(())
}
pub fn set_nonblocking(&self, nonblocking: bool) -> io::Result<()> {
let oldfl = unsafe { fcntl(self.fd, F_GETFL) };
if oldfl == -1 {
return Err(io::Error::last_os_error());
}
let newfl = if nonblocking {
oldfl | O_NONBLOCK
} else {
oldfl & !O_NONBLOCK
};
let rv = unsafe { fcntl(self.fd, F_SETFL, newfl) };
if rv != 0 {
return Err(io::Error::last_os_error());
}
Ok(())
}
pub fn read(&mut self) -> io::Result<&[u8]> {
let buffer_ptr = &mut self.recv_buffer as *mut _ as *mut c_void;
let read_rv = unsafe { read(self.fd, buffer_ptr, RECV_BUFFER_SIZE) };
if read_rv < 0 {
return Err(io::Error::last_os_error());
}
Ok(&self.recv_buffer[0..read_rv.try_into().unwrap()])
}
pub fn write(&self, buffer: &[u8]) -> io::Result<()> {
let write_rv = unsafe {
let buffer_ptr = buffer as *const _ as *const c_void;
write(self.fd, buffer_ptr, buffer.len())
};
if write_rv != buffer.len().try_into().unwrap() {
return Err(io::Error::last_os_error());
}
Ok(())
}
}
impl AsRawFd for IsoTpSocket {
fn as_raw_fd(&self) -> RawFd {
self.fd
}
}
impl FromRawFd for IsoTpSocket {
unsafe fn from_raw_fd(fd: RawFd) -> Self {
Self {
fd,
recv_buffer: [0x00; RECV_BUFFER_SIZE],
}
}
}
impl IntoRawFd for IsoTpSocket {
fn into_raw_fd(self) -> RawFd {
self.fd
}
}
impl Drop for IsoTpSocket {
fn drop(&mut self) {
self.close().ok();
}
}