diff --git a/Cargo.toml b/Cargo.toml index d081944..a79b4c0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,9 +8,9 @@ keywords = ["agriculture", "can", "canbus", "isobus", "j1939", "agritech", "smar [dependencies] rand = "0.8.5" -pcan-basic = { version = "1.0.2", optional = true } -socketcan = { version = "2.0.0", optional = true } tracing = { version = "0.1.37", optional = true } +socketcan = { version = "2.0.0", optional = true } +pcan-basic = { version = "1.0.2", optional = true } [features] default = [] @@ -20,12 +20,12 @@ socketcan = ["dep:socketcan"] tracing = ["dep:tracing"] # Peak driver peak = ["dep:pcan-basic"] - [dev-dependencies] clap = { version = "4.3.19", features = ["derive"] } ctrlc = "3.4.0" +# TODO: Add optional tracing to the main library +tracing = "0.1.37" tracing-subscriber = "0.3.17" - [[example]] name = "forward" -required-features = ["tracing"] +required-features = ["tracing"] \ No newline at end of file diff --git a/examples/forward.rs b/examples/forward.rs index b4f41c2..a71ef8f 100644 --- a/examples/forward.rs +++ b/examples/forward.rs @@ -81,7 +81,7 @@ fn create_driver(iface: &str, driver: CanDriver) -> Box { CanDriver::Pcan => { let bus = parse_usb_bus(iface).unwrap(); let baud = ag_iso_stack::driver::Baudrate::Baud250K; - Box::new(PeakDriver::new(bus, baud)) + Box::new(PeakDriver::new_usb(bus, baud)) } #[allow(unreachable_patterns)] _ => unreachable!(), @@ -98,6 +98,8 @@ fn main() { .map_err(|_err| eprintln!("Unable to set global default subscriber")) .unwrap(); + tracing::info!("AgIsoStack-rs example starts..."); + tracing::info!( "Forwarding CAN traffic from {} to {}", opts.input_interface, diff --git a/src/driver/address.rs b/src/driver/address.rs index 5191171..ca65d05 100644 --- a/src/driver/address.rs +++ b/src/driver/address.rs @@ -5,7 +5,11 @@ pub struct Address(pub u8); impl Address { + /// Address representing broadcasts for destination specific PGNs pub const GLOBAL: Address = Address(0xFF); + /// Alias for the global address + pub const BROADCAST: Address = Address(0xFF); + /// The null address is used by ECUs without an address such as during address claiming pub const NULL: Address = Address(0xFE); } diff --git a/src/driver/can_id.rs b/src/driver/can_id.rs index 0a791ed..7ce6cb8 100644 --- a/src/driver/can_id.rs +++ b/src/driver/can_id.rs @@ -49,6 +49,14 @@ pub enum Type { Extended = 0x1, } +#[derive(Debug, Clone)] +pub struct EncodingError { + pub priority: Priority, + pub parameter_group_number: Pgn, + pub source_address: Address, + pub destination_address: Address, +} + #[derive(Debug, Default, Clone, Copy, PartialEq, Eq, Hash)] #[repr(transparent)] pub struct CanId(u32); @@ -71,6 +79,63 @@ impl CanId { Self(raw) } + /// Encodes a new extended ID using the discrete parts of an identifier + pub fn try_encode( + parameter_group_number: Pgn, + source_address: Address, + destination_address: Address, + priority: Priority, + ) -> Result { + if destination_address != Address::GLOBAL && parameter_group_number.is_broadcast() { + return Err(EncodingError { + priority, + parameter_group_number, + source_address, + destination_address, + }); + } + Ok(unsafe { + CanId::encode_unchecked( + parameter_group_number, + source_address, + destination_address, + priority, + ) + }) + } + + /// Encodes a new extended ID using the discrete parts of an identifier but won't validate + /// your combination of PGN and destination address. + /// + /// # Safety + /// Calling this without validating your PGN and destination address combination may result in your PGN field + /// getting trashed. Specifically, the risk is when you are using a broadcast PGN but supply a non-0xFF + /// destination address. + pub unsafe fn encode_unchecked( + parameter_group_number: Pgn, + source_address: Address, + destination_address: Address, + priority: Priority, + ) -> CanId { + let mut raw_id: u32 = 0; + + raw_id |= (priority as u32 & 0x07) << 26; + raw_id |= source_address.0 as u32; + + if Address::GLOBAL == destination_address { + if (parameter_group_number.raw() & 0xF000) >= 0xF000 { + raw_id |= (parameter_group_number.raw() & 0x3FFFF) << 8; + } else { + raw_id |= (destination_address.0 as u32) << 8; + raw_id |= (parameter_group_number.raw() & 0x3FF00) << 8; + } + } else if (parameter_group_number.raw() & 0xF000) < 0xF000 { + raw_id |= (destination_address.0 as u32) << 8; + raw_id |= (parameter_group_number.raw() & 0x3FF00) << 8; + } + CanId::new(raw_id & CAN_EFF_MASK, Type::Extended) + } + /// Get the raw value of the CAN ID #[inline] pub fn raw(&self) -> u32 { @@ -188,4 +253,45 @@ mod tests { let can_id = CanId::new(0x18EEFF1C, Type::Extended); assert_eq!(can_id.pgn(), Pgn::from_raw(0x0EE00)); } + + #[test] + fn test_encode() { + let encode_result = CanId::try_encode( + Pgn::from_raw(0x00EF00), + Address(0x81), + Address(0xF9), + Priority::Six, + ); + let can_id = encode_result.expect("EF00 Message was not encodable"); + assert_eq!(can_id.pgn(), Pgn::from_raw(0xEF00)); + assert_eq!(can_id.destination_address(), Address(0xF9)); + assert_eq!(can_id.source_address(), Address(0x81)); + assert_eq!(can_id.priority(), Priority::Six); + + let encode_result = CanId::try_encode( + Pgn::from_raw(0x00FF40), + Address(0x81), + Address(0xFF), + Priority::Six, + ); + let can_id = encode_result.expect("FF40 Message was not encodable"); + assert_eq!(can_id.pgn(), Pgn::from_raw(0xFF40)); + assert_eq!(can_id.destination_address(), Address(0xFF)); + assert_eq!(can_id.source_address(), Address(0x81)); + assert_eq!(can_id.priority(), Priority::Six); + + let encode_result = CanId::try_encode( + Pgn::from_raw(0x00FF40), + Address(0x81), + Address(0x0F), + Priority::Six, + ); + assert!(matches!(encode_result.is_err(), true)); + + let error_contents: EncodingError = encode_result.unwrap_err(); + assert_eq!(error_contents.priority, Priority::Six); + assert_eq!(error_contents.source_address, Address(0x81)); + assert_eq!(error_contents.destination_address, Address(0x0F)); + assert_eq!(error_contents.parameter_group_number, Pgn::from_raw(0xFF40)); + } } diff --git a/src/driver/driver.rs b/src/driver/driver.rs index c555db0..fa7d0dc 100644 --- a/src/driver/driver.rs +++ b/src/driver/driver.rs @@ -6,10 +6,6 @@ use crate::driver::Frame; pub enum DriverOpenError { /// The driver failed to open with filesystem semantics IoError(std::io::Error), - // TODO: Here and throughout. I don't love the pcan errors. They're not real std::error::Error - // types, and it's not obvious what they mean. Maybe we should re-think this error design? - #[cfg(feature = "peak")] - PeakError(pcan_basic::error::PcanError), } impl std::fmt::Display for DriverOpenError { @@ -21,10 +17,6 @@ impl std::error::Error for DriverOpenError { fn source(&self) -> Option<&(dyn std::error::Error + 'static)> { match &self { DriverOpenError::IoError(e) => Some(e), - // PcanError doesn't implement the Error trait - // DriverOpenError::PeakError(e) => Some(e), - #[allow(unreachable_patterns)] - _ => None, } } } @@ -57,8 +49,6 @@ pub enum DriverReadError { ErrorFrame(), /// The driver failed to read with filesystem semantics IoError(std::io::Error), - #[cfg(feature = "peak")] - PeakError(pcan_basic::error::PcanError), } impl std::fmt::Display for DriverReadError { @@ -89,8 +79,6 @@ pub enum DriverWriteError { BusError(), /// Some fault with filesystem semantics IoError(std::io::Error), - #[cfg(feature = "peak")] - PeakError(pcan_basic::error::PcanError), } impl std::fmt::Display for DriverWriteError { diff --git a/src/driver/mod.rs b/src/driver/mod.rs index 49861ba..4fd91c6 100644 --- a/src/driver/mod.rs +++ b/src/driver/mod.rs @@ -12,19 +12,22 @@ mod driver; mod frame; mod pgn; -#[cfg(feature = "peak")] -mod peak; - #[cfg(feature = "socketcan")] mod socketcan; +#[cfg(feature = "peak")] +mod peak; + pub use address::Address; pub use can_id::{CanId, Priority, Type}; pub use driver::{Driver, DriverCloseError, DriverOpenError, DriverReadError, DriverWriteError}; pub use frame::{Channel, Frame}; pub use pgn::Pgn; -#[cfg(feature = "peak")] -pub use self::peak::{Baudrate, PeakDriver}; #[cfg(feature = "socketcan")] pub use self::socketcan::SocketcanDriver; + +#[cfg(feature = "peak")] +pub use self::peak::Baudrate; +#[cfg(feature = "peak")] +pub use self::peak::PeakDriver; diff --git a/src/driver/peak.rs b/src/driver/peak.rs index 352af92..c284bd6 100644 --- a/src/driver/peak.rs +++ b/src/driver/peak.rs @@ -1,44 +1,152 @@ -pub use pcan_basic::bus::UsbBus; -// NOTE: PcanError is undocumented in the Rust source - you should go to -// https://github.com/tsabelmann/pcan-basic-sys/blob/master/header/PCANBasic.h for error -// documentation. -// -// PcanError::XmtFull - controller transmit buffer full -// PcanError::Overrun - controller was read too late -// PcanError::BusLight - error counter reached the light limit -// PcanError::BusHeavy - error counter reached the heavy limit -// PcanError::BusPassive -// PcanError::BusOff -// PcanError::AnyBusErr -// PcanError::QrcvEmpty - receive queue empty -// PcanError::QOverrun - receive queue overrun -// PcanError::QxmtFull - transmit queue full -// PcanError::RegTest - controller hardware registers test failed -// PcanError::NoDriver - driver not loaded -// PcanError::HwInUse - Hardware already in use by a network -// PcanError::NetInUse - A client is already connected to the Net -// PcanError::IllHw - Hardware handle is invalid -// PcanError::IllNet - Net handle is invalid -// PcanError::IllClient - Client handle is invalid -// PcanError::Resource - Resource (FIFO, Client, timeout) cannot be created -// PcanError::IllParamType - Invalid parameter -// PcanError::IllParamVal - Invalid parameter value -// PcanError::Unknown -// PcanError::IllData - Invalid data, function, or action -// PcanError::IllMode - Driver object state is wrong for the attempted operation -// PcanError::Caution - An opperation was successfully carried out, but irregularities were found -// PcanError::Initialize - Channel not initialized -// PcanError::IllOperation - Invalid operation -pub use pcan_basic::error::PcanError; +use std::io::ErrorKind; +// Copyright 2023 Raven Industries inc. +use std::time::Instant; + +use pcan_basic::bus::DngBus; +use pcan_basic::bus::IsaBus; +use pcan_basic::bus::LanBus; +use pcan_basic::bus::PccBus; +use pcan_basic::bus::PciBus; +use pcan_basic::bus::UsbBus; +use pcan_basic::error::PcanError; +use pcan_basic::socket::dng::DngCanSocket; +use pcan_basic::socket::isa::IsaCanSocket; +use pcan_basic::socket::lan::LanCanSocket; +use pcan_basic::socket::pcc::PccCanSocket; +use pcan_basic::socket::pci::PciCanSocket; use pcan_basic::socket::usb::UsbCanSocket; -use pcan_basic::socket::{Baudrate as PcanBaudrate, RecvCan, SendCan}; +use pcan_basic::socket::{Baudrate as PeakBaudrate, CanFrame, MessageType, RecvCan, SendCan}; use crate::driver::{ - Driver, DriverCloseError, DriverOpenError, DriverReadError, DriverWriteError, Frame, + CanId, Channel, Driver, DriverCloseError, DriverOpenError, DriverReadError, DriverWriteError, + Frame as InternalFrame, Type, }; -use crate::tracing; -#[derive(Debug, Clone, PartialEq, Eq)] +impl From for DriverOpenError { + fn from(e: PcanError) -> DriverOpenError { + match e { + PcanError::RegTest => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Test of the CAN controller hardware registers failed (no hardware found)", + )), + PcanError::NoDriver => { + DriverOpenError::IoError(std::io::Error::new(ErrorKind::Other, "Driver not loaded")) + } + PcanError::HwInUse => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::AddrInUse, + "Hardware already in use by a Net", + )), + PcanError::NetInUse => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::AddrInUse, + "A Client is already connected to the Net", + )), + PcanError::IllHw => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Hardware handle is invalid", + )), + PcanError::IllNet => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Net handle is invalid", + )), + PcanError::IllClient => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Client handle is invalid", + )), + PcanError::Resource => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Resource (FIFO, Client, timeout) cannot be created", + )), + PcanError::IllParamType => { + DriverOpenError::IoError(std::io::Error::new(ErrorKind::Other, "Invalid parameter")) + } + PcanError::IllParamVal => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Invalid parameter value", + )), + PcanError::Unknown => { + DriverOpenError::IoError(std::io::Error::new(ErrorKind::Other, "Unknown error")) + } + PcanError::IllData => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::InvalidData, + "Invalid data, function, or action", + )), + PcanError::IllMode => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Driver object state is wrong for the attempted operation", + )), + PcanError::Initialize => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Channel is not initialized [Value was changed from 0x40000 to 0x4000000]", + )), + PcanError::IllOperation => DriverOpenError::IoError(std::io::Error::new( + ErrorKind::Other, + "Invalid operation [Value was changed from 0x80000 to 0x8000000]", + )), + _ => DriverOpenError::IoError(std::io::Error::new(ErrorKind::Other, "Unknown")), + } + } +} + +impl From for DriverReadError { + fn from(e: PcanError) -> DriverReadError { + match e { + PcanError::QrcvEmpty | PcanError::QOverrun => DriverReadError::NoFrameReady, + PcanError::Resource => DriverReadError::IoError(std::io::Error::new( + ErrorKind::Other, + "Resource (FIFO, Client, timeout) cannot be created", + )), + PcanError::IllParamType => { + DriverReadError::IoError(std::io::Error::new(ErrorKind::Other, "Invalid parameter")) + } + PcanError::IllParamVal => DriverReadError::IoError(std::io::Error::new( + ErrorKind::Other, + "Invalid parameter value", + )), + PcanError::Unknown => { + DriverReadError::IoError(std::io::Error::new(ErrorKind::Other, "Unknown error")) + } + PcanError::IllData => DriverReadError::IoError(std::io::Error::new( + ErrorKind::InvalidData, + "Invalid data, function, or action", + )), + _ => DriverReadError::IoError(std::io::Error::new(ErrorKind::Other, "Unknown")), + } + } +} + +impl From for DriverWriteError { + fn from(e: PcanError) -> DriverWriteError { + match e { + PcanError::AnyBusErr => DriverWriteError::BusError(), + PcanError::XmtFull | PcanError::QxmtFull => DriverWriteError::NotReady, + PcanError::IllOperation => DriverWriteError::IoError(std::io::Error::new( + ErrorKind::Other, + "Invalid operation [Value was changed from 0x80000 to 0x8000000]", + )), + _ => DriverWriteError::IoError(std::io::Error::new(ErrorKind::Other, "Unknown")), + } + } +} + +impl From<&InternalFrame> for CanFrame { + fn from(f: &InternalFrame) -> CanFrame { + let msg_type = match f.id.type_() { + Type::Standard => MessageType::Standard, + Type::Extended => MessageType::Extended, + }; + CanFrame::new( + f.id.raw(), + msg_type, + &f.data[..f.data_length.min(8) as usize], + ) + // guaranteed to not crash, because `f.data` is an [u8; 8] + .expect("Can frame had too much data") + } +} + +/* Baudrate */ + +#[derive(Debug, PartialEq, Copy, Clone)] pub enum Baudrate { Baud1M, Baud800K, @@ -58,130 +166,217 @@ pub enum Baudrate { // It's unfortunate that we have to duplicate pcan's baudrate enum, but it doesn't implement Clone // or Debug, so we have to. -impl From for PcanBaudrate { - fn from(b: Baudrate) -> PcanBaudrate { +impl From for PeakBaudrate { + fn from(b: Baudrate) -> PeakBaudrate { match b { - Baudrate::Baud1M => PcanBaudrate::Baud1M, - Baudrate::Baud800K => PcanBaudrate::Baud800K, - Baudrate::Baud500K => PcanBaudrate::Baud500K, - Baudrate::Baud250K => PcanBaudrate::Baud250K, - Baudrate::Baud125K => PcanBaudrate::Baud125K, - Baudrate::Baud100K => PcanBaudrate::Baud100K, - Baudrate::Baud95K => PcanBaudrate::Baud95K, - Baudrate::Baud83K => PcanBaudrate::Baud83, - Baudrate::Baud50K => PcanBaudrate::Baud50K, - Baudrate::Baud47K => PcanBaudrate::Baud47K, - Baudrate::Baud33K => PcanBaudrate::Baud33K, - Baudrate::Baud20K => PcanBaudrate::Baud20K, - Baudrate::Baud10K => PcanBaudrate::Baud10K, - Baudrate::Baud5K => PcanBaudrate::Baud5K, + Baudrate::Baud1M => PeakBaudrate::Baud1M, + Baudrate::Baud800K => PeakBaudrate::Baud800K, + Baudrate::Baud500K => PeakBaudrate::Baud500K, + Baudrate::Baud250K => PeakBaudrate::Baud250K, + Baudrate::Baud125K => PeakBaudrate::Baud125K, + Baudrate::Baud100K => PeakBaudrate::Baud100K, + Baudrate::Baud95K => PeakBaudrate::Baud95K, + Baudrate::Baud83K => PeakBaudrate::Baud83, + Baudrate::Baud50K => PeakBaudrate::Baud50K, + Baudrate::Baud47K => PeakBaudrate::Baud47K, + Baudrate::Baud33K => PeakBaudrate::Baud33K, + Baudrate::Baud20K => PeakBaudrate::Baud20K, + Baudrate::Baud10K => PeakBaudrate::Baud10K, + Baudrate::Baud5K => PeakBaudrate::Baud5K, } } } -impl From for DriverOpenError { - fn from(e: PcanError) -> DriverOpenError { - DriverOpenError::PeakError(e) - } +enum PeakIface { + Dng(DngBus), + Isa(IsaBus), + Lan(LanBus), + Pcc(PccBus), + Pci(PciBus), + Usb(UsbBus), } -impl From for DriverReadError { - fn from(e: PcanError) -> DriverReadError { - match e { - PcanError::QrcvEmpty => DriverReadError::NoFrameReady, - _ => DriverReadError::PeakError(e), - } - } -} -impl From for DriverWriteError { - fn from(e: PcanError) -> DriverWriteError { - match e { - PcanError::QxmtFull | PcanError::XmtFull | PcanError::Overrun => { - DriverWriteError::NotReady - } - _ => DriverWriteError::PeakError(e), - } - } + +enum PeakSocket { + Dng(DngCanSocket), + Isa(IsaCanSocket), + Lan(LanCanSocket), + Pcc(PccCanSocket), + Pci(PciCanSocket), + Usb(UsbCanSocket), } +/// PCan Basic Driver [Driver] +/// +/// Enabled with the optional `pcan-basic` feature pub struct PeakDriver { - sock: Option, - bus: UsbBus, - baud: Baudrate, + iface: PeakIface, + baudrate: Baudrate, + socket: Option, + opened_timestamp: Instant, } impl PeakDriver { - pub fn new(bus: UsbBus, baud: Baudrate) -> Self { + pub fn new_dng(if_bus: DngBus, baudrate: Baudrate) -> Self { + Self { + iface: PeakIface::Dng(if_bus), + baudrate, + socket: None, + opened_timestamp: Instant::now(), + } + } + + pub fn new_isa(if_bus: IsaBus, baudrate: Baudrate) -> Self { + Self { + iface: PeakIface::Isa(if_bus), + baudrate, + socket: None, + opened_timestamp: Instant::now(), + } + } + + pub fn new_lan(if_bus: LanBus, baudrate: Baudrate) -> Self { Self { - sock: None, - bus, - baud, + iface: PeakIface::Lan(if_bus), + baudrate, + socket: None, + opened_timestamp: Instant::now(), + } + } + + pub fn new_pcc(if_bus: PccBus, baudrate: Baudrate) -> Self { + Self { + iface: PeakIface::Pcc(if_bus), + socket: None, + baudrate, + opened_timestamp: Instant::now(), + } + } + + pub fn new_pci(if_bus: PciBus, baudrate: Baudrate) -> Self { + Self { + iface: PeakIface::Pci(if_bus), + baudrate, + socket: None, + opened_timestamp: Instant::now(), + } + } + + pub fn new_usb(if_bus: UsbBus, baudrate: Baudrate) -> Self { + Self { + iface: PeakIface::Usb(if_bus), + baudrate, + socket: None, + opened_timestamp: Instant::now(), + } + } + + fn to_frame(&self, frame: CanFrame) -> InternalFrame { + let timestamp = self.opened_timestamp.elapsed(); + let raw_id = frame.can_id(); + let extended = frame.is_extended_frame(); + let frame_type = if extended { + Type::Extended + } else { + Type::Standard + }; + + let id = CanId::new(raw_id, frame_type); + // TODO: The Driver trait doesn't know anything about Channels yet. + // + // The channel exists so that we can tie Frames and CANMessages back to the network + // manager they originated from. This channel value should be passed to the Driver + // when it's created (or opened?) + let channel = Channel::default(); + let mut data = [0; 8]; + let data_length = usize::from(frame.dlc().min(8)); + data[..data_length].copy_from_slice(frame.data()); + let data_length = data_length as u8; + + InternalFrame { + timestamp, + id, + channel, + data, + data_length, + extended, } } } impl Driver for PeakDriver { fn is_valid(&self) -> bool { - self.sock.is_some() + self.socket.is_some() } fn open(&mut self) -> Result<(), DriverOpenError> { - // pcan's Baudrate type doesn't implement Clone, so you can't pass it to open() behind the - // &mut self mutable reference. - match UsbCanSocket::open(self.bus, self.baud.clone().into()) { - Ok(sock) => { - tracing::info!("Opened {:?} @ {:?}", self.bus, self.baud); - self.sock = Some(sock); - Ok(()) - } - Err(e) => { - let e = e.into(); - tracing::error!("Failed to open {:?} @ {:?}: {e:?}", self.bus, self.baud); - Err(e) - } - } + self.socket = match &self.iface { + PeakIface::Dng(dng) => Some(PeakSocket::Dng(DngCanSocket::open( + *dng, + PeakBaudrate::from(self.baudrate), + )?)), + PeakIface::Isa(isa) => Some(PeakSocket::Isa(IsaCanSocket::open( + *isa, + PeakBaudrate::from(self.baudrate), + )?)), + PeakIface::Lan(lan) => Some(PeakSocket::Lan(LanCanSocket::open( + *lan, + PeakBaudrate::from(self.baudrate), + )?)), + PeakIface::Pcc(pcc) => Some(PeakSocket::Pcc(PccCanSocket::open( + *pcc, + PeakBaudrate::from(self.baudrate), + )?)), + PeakIface::Pci(pci) => Some(PeakSocket::Pci(PciCanSocket::open( + *pci, + PeakBaudrate::from(self.baudrate), + )?)), + PeakIface::Usb(usb) => Some(PeakSocket::Usb(UsbCanSocket::open( + *usb, + PeakBaudrate::from(self.baudrate), + )?)), + }; + self.opened_timestamp = Instant::now(); + + Ok(()) } fn close(&mut self) -> Result<(), DriverCloseError> { - self.sock = None; + self.socket = None; Ok(()) } - fn read_nonblocking(&mut self, _frame: &mut Frame) -> Result<(), DriverReadError> { - let Some(sock) = self.sock.as_mut() else { - tracing::warn!("Tried to read from closed interface {:?}", self.bus); + /// Read a frame from the driver, if possible + /// + /// The timestamp on the frame is the duration since [`open`](Self::open) was last called. + fn read_nonblocking(&mut self, frame: &mut InternalFrame) -> Result<(), DriverReadError> { + let Some(sock) = self.socket.as_mut() else { return Err(DriverReadError::DriverClosed); }; + let peak_frame = match sock { + PeakSocket::Dng(dng) => dng.recv()?, + PeakSocket::Isa(isa) => isa.recv()?, + PeakSocket::Lan(lan) => lan.recv()?, + PeakSocket::Pcc(pcc) => pcc.recv()?, + PeakSocket::Pci(pci) => pci.recv()?, + PeakSocket::Usb(usb) => usb.recv()?, + }; - match sock.recv() { - Ok((_frame, _timestamp)) => { - tracing::trace!("Received {_frame:?} @ {_timestamp:?}"); - Ok(()) - } - Err(PcanError::QrcvEmpty) => Err(DriverReadError::NoFrameReady), - Err(e) => { - tracing::error!("Error receiving frame: {e:?}"); - Err(e.into()) - } - } + *frame = self.to_frame(peak_frame.0); + Ok(()) } - - fn write_nonblocking(&mut self, _frame: &Frame) -> Result<(), DriverWriteError> { - let Some(sock) = self.sock.as_mut() else { - tracing::warn!("Tried to write to closed interface {:?}", self.bus); + fn write_nonblocking(&mut self, frame: &InternalFrame) -> Result<(), DriverWriteError> { + let Some(sock) = self.socket.as_mut() else { return Err(DriverWriteError::DriverClosed); }; + let peak_frame: CanFrame = frame.into(); - let pcan_frame = pcan_basic::socket::CanFrame::default(); - match sock.send(pcan_frame) { - Ok(_) => { - tracing::trace!("Wrote frame: {_frame:?}"); - Ok(()) - } - Err(e @ (PcanError::QxmtFull | PcanError::XmtFull | PcanError::Overrun)) => { - Err(e.into()) - } - Err(e) => { - tracing::error!("Error: {e:?} writing frame: {_frame:?}"); - Err(e.into()) - } - } + match sock { + PeakSocket::Dng(dng) => dng.send(peak_frame)?, + PeakSocket::Isa(isa) => isa.send(peak_frame)?, + PeakSocket::Lan(lan) => lan.send(peak_frame)?, + PeakSocket::Pcc(pcc) => pcc.send(peak_frame)?, + PeakSocket::Pci(pci) => pci.send(peak_frame)?, + PeakSocket::Usb(usb) => usb.send(peak_frame)?, + }; + + Ok(()) } } diff --git a/src/network_management/can_message.rs b/src/network_management/can_message.rs new file mode 100644 index 0000000..9905ffd --- /dev/null +++ b/src/network_management/can_message.rs @@ -0,0 +1,41 @@ +// Copyright 2023 Raven Industries inc. +use super::name::{DEFAULT_NAME, NAME}; +use crate::driver::CanId; + +pub struct CANMessage { + data: Vec, + identifier: CanId, + source_name: NAME, + destination_name: NAME, +} + +impl CANMessage { + pub(super) fn new(data: Vec, identifier: CanId) -> CANMessage { + CANMessage { + data, + identifier, + source_name: NAME { + raw_name: DEFAULT_NAME, + }, + destination_name: NAME { + raw_name: DEFAULT_NAME, + }, + } + } + + pub fn get_data(&self) -> &[u8] { + self.data.as_slice() + } + + pub fn get_identifier(&self) -> CanId { + self.identifier + } + + pub fn get_source_name(&self) -> NAME { + self.source_name + } + + pub fn get_destination_name(&self) -> NAME { + self.destination_name + } +} diff --git a/src/network_management/control_function.rs b/src/network_management/control_function.rs index cc9c412..d4f55ce 100644 --- a/src/network_management/control_function.rs +++ b/src/network_management/control_function.rs @@ -1,9 +1,13 @@ // Copyright 2023 Raven Industries inc. -#![allow(dead_code)] - +use crate::driver::Address; +use crate::network_management::name::DEFAULT_NAME; use crate::network_management::name::NAME; use rand::Rng; -use std::time::Instant; +use std::cell::RefCell; +use std::rc::Rc; +use std::time::{Duration, Instant}; + +use super::network_manager::{MessageQueuePriority, NetworkManager}; #[derive(PartialEq, Eq, Clone, Copy)] pub enum AddressClaimingState { @@ -31,15 +35,15 @@ pub enum AddressClaimingState { pub struct AddressClaimingData { state: AddressClaimingState, + name: NAME, timestamp: Option, - preferred_address: u8, + preferred_address: Address, random_delay: u8, enabled: bool, } pub enum ControlFunction { Internal { - name: NAME, address_claim_data: AddressClaimingData, }, External { @@ -47,10 +51,148 @@ pub enum ControlFunction { }, } +impl ControlFunction { + pub fn new_internal_control_function( + name: NAME, + preferred_address: Address, + enabled: bool, + network: &mut NetworkManager, + ) -> Rc> { + let cf = Rc::new(RefCell::new(ControlFunction::Internal { + address_claim_data: AddressClaimingData::new(name, preferred_address, enabled), + })); + network.on_new_internal_control_function(cf.clone()); + cf + } + + pub fn get_name(&self) -> NAME { + match self { + ControlFunction::Internal { address_claim_data } => address_claim_data.get_name(), + ControlFunction::External { name } => *name, + } + } +} + +impl AddressClaimingState { + pub(super) fn new() -> Self { + Self::None + } + + pub(super) fn update_state_none(_claim_to_process: &AddressClaimingData) -> Self { + AddressClaimingState::WaitForClaim + } + + pub(super) fn update_state_wait_for_claim(claim_to_process: &AddressClaimingData) -> Self { + if Instant::now().duration_since(claim_to_process.get_timestamp().unwrap()) + > Duration::from_millis(claim_to_process.get_random_delay() as u64) + { + AddressClaimingState::SendRequestForClaim + } else { + AddressClaimingState::WaitForClaim + } + } + + pub(super) fn update_state_send_request_for_claim(network: &mut NetworkManager) -> Self { + network.enqueue_can_message( + NetworkManager::construct_request_for_address_claim(), + MessageQueuePriority::High, + ); + AddressClaimingState::WaitForRequestContentionPeriod + } + + pub(super) fn update_state_wait_for_request_contention( + claim_to_process: &AddressClaimingData, + network: &mut NetworkManager, + ) -> Self { + let contention_time_ms: u64 = 250; + + if Instant::now().duration_since(claim_to_process.get_timestamp().unwrap()) + > Duration::from_millis(claim_to_process.get_random_delay() as u64 + contention_time_ms) + { + let is_device_at_our_address = + network.get_control_function_by_address(claim_to_process.get_preferred_address()); + let is_valid_device: bool = is_device_at_our_address.is_some(); + + if is_valid_device { + let preferred_address_name: u64 = + match *is_device_at_our_address.as_ref().unwrap().clone().borrow() { + ControlFunction::External { name } => name.raw_name, + ControlFunction::Internal { + address_claim_data: _, + } => claim_to_process.get_name().raw_name, + }; + + if (!claim_to_process.get_name().get_self_configurable_address() + && preferred_address_name > claim_to_process.get_name().raw_name) + || DEFAULT_NAME == preferred_address_name + { + // Either our preferred address is free, this is the best case, or: + // Our address is not free, but we cannot be at an arbitrary address, and the address can be stolen by us + AddressClaimingState::SendPreferredAddressClaim + } else if !claim_to_process.get_name().get_self_configurable_address() { + // We cannot claim because we cannot tolerate an arbitrary address, and the CF at that spot wins due to its lower ISONAME + AddressClaimingState::UnableToClaim + } else { + // We will move to another address if whoever is in our spot has a lower NAME + if preferred_address_name < claim_to_process.get_name().raw_name { + // We must scan the address space and move to a free address + AddressClaimingState::SendArbitraryAddressClaim + } else { + // Our address claim wins because it's lower than the device that's in our preferred spot + AddressClaimingState::SendPreferredAddressClaim + } + } + } else { + AddressClaimingState::SendPreferredAddressClaim + } + } else { + AddressClaimingState::WaitForRequestContentionPeriod + } + } + + pub(super) fn update_state_send_preferred_address_claim( + claim_to_process: &AddressClaimingData, + network: &mut NetworkManager, + ) -> Self { + network.enqueue_can_message( + NetworkManager::construct_address_claim( + claim_to_process.get_preferred_address(), + claim_to_process.get_name(), + ), + MessageQueuePriority::High, + ); + AddressClaimingState::AddressClaimingComplete + } + + pub(super) fn update_state_send_arbitrary_address_claim( + claim_to_process: &AddressClaimingData, + network: &mut NetworkManager, + ) -> Self { + let next_address = network.get_next_free_arbitrary_address(); + + if Address::NULL != next_address { + // Found an address we can use + network.enqueue_can_message( + NetworkManager::construct_address_claim(next_address, claim_to_process.get_name()), + MessageQueuePriority::High, + ); + return AddressClaimingState::AddressClaimingComplete; + } + AddressClaimingState::UnableToClaim + } +} + +impl Default for AddressClaimingState { + fn default() -> Self { + Self::new() + } +} + impl AddressClaimingData { - pub fn new(preferred_address: u8, enabled: bool) -> AddressClaimingData { + pub fn new(name: NAME, preferred_address: Address, enabled: bool) -> AddressClaimingData { AddressClaimingData { state: AddressClaimingState::None, + name, timestamp: None, preferred_address, random_delay: AddressClaimingData::generate_random_delay(), @@ -71,10 +213,14 @@ impl AddressClaimingData { } } - pub fn get_preferred_address(&self) -> u8 { + pub fn get_preferred_address(&self) -> Address { self.preferred_address } + pub(super) fn set_preferred_address(&mut self, new_address: Address) { + self.preferred_address = new_address; + } + pub fn get_state(&self) -> AddressClaimingState { self.state } @@ -83,6 +229,17 @@ impl AddressClaimingData { self.state = new_state; } + pub fn get_name(&self) -> NAME { + self.name + } + + pub fn set_name(&mut self, new_name: NAME) { + if self.name.raw_name != new_name.raw_name { + self.state = AddressClaimingState::None; // Name changed, state no longer valid + } + self.name = new_name; + } + pub fn get_timestamp(&self) -> Option { self.timestamp } @@ -100,15 +257,3 @@ impl AddressClaimingData { (rng.gen_range(0..255) as f32 * 0.6_f32) as u8 } } - -impl Default for AddressClaimingData { - fn default() -> AddressClaimingData { - AddressClaimingData { - state: AddressClaimingState::None, - timestamp: None, - preferred_address: 0xFE_u8, - random_delay: AddressClaimingData::generate_random_delay(), - enabled: true, - } - } -} diff --git a/src/network_management/mod.rs b/src/network_management/mod.rs index 869de20..99d372c 100644 --- a/src/network_management/mod.rs +++ b/src/network_management/mod.rs @@ -1,4 +1,6 @@ // Copyright 2023 Raven Industries inc. +pub mod can_message; pub mod common_parameter_group_numbers; pub mod control_function; pub mod name; +pub mod network_manager; diff --git a/src/network_management/name.rs b/src/network_management/name.rs index 3f78ef6..02d7557 100644 --- a/src/network_management/name.rs +++ b/src/network_management/name.rs @@ -1,5 +1,5 @@ // Copyright 2023 Raven Industries inc. -const DEFAULT_NAME: u64 = 0xFFFFFFFFFFFFFFFF; +pub const DEFAULT_NAME: u64 = 0xFFFFFFFFFFFFFFFF; #[derive(PartialEq, Eq, Clone, Copy)] pub enum NameField { diff --git a/src/network_management/network_manager.rs b/src/network_management/network_manager.rs new file mode 100644 index 0000000..3bc00b9 --- /dev/null +++ b/src/network_management/network_manager.rs @@ -0,0 +1,351 @@ +// Copyright 2023 Raven Industries inc. +use std::time::Instant; + +use super::control_function::{AddressClaimingState, ControlFunction}; +use crate::driver::{Address, CanId, Pgn, Priority}; +use crate::network_management::can_message::CANMessage; +use crate::network_management::common_parameter_group_numbers::CommonParameterGroupNumbers; +use crate::network_management::name::{DEFAULT_NAME, NAME}; +use std::cell::RefCell; +use std::collections::VecDeque; +use std::rc::Rc; + +#[derive(Debug, Clone, Copy)] +pub(super) enum MessageQueuePriority { + /// High priority messages are always sent to the driver before normal ones + High, + /// Normal messages are sent to the driver when no high priority messages are in the queue (todo) + Normal, +} + +#[derive(Debug, Clone, Copy)] +pub enum CANTransmitState { + /// Used to describe that a CAN message was accepted by the CAN stack to be sent + Success, + /// Used to describe that a CAN message was not accepted by the stack and will not be sent + Fail, +} + +pub struct NetworkManager { + control_function_table: [Option>>; 253], + inactive_control_functions: Vec>>, + address_claim_state_machines: Vec>>, + high_priority_can_message_tx_queue: VecDeque, + normal_priority_can_message_tx_queue: VecDeque, + receive_message_queue: VecDeque, +} + +impl NetworkManager { + pub fn new() -> Self { + Self { + control_function_table: std::array::from_fn(|_| None), + inactive_control_functions: Vec::new(), + address_claim_state_machines: Vec::new(), + high_priority_can_message_tx_queue: VecDeque::new(), + normal_priority_can_message_tx_queue: VecDeque::new(), + receive_message_queue: VecDeque::new(), + } + } + + pub fn get_control_function_by_address( + &self, + address: Address, + ) -> &Option>> { + &self.control_function_table[address.0 as usize] + } + + pub fn get_control_function_address_by_name(&self, name: NAME) -> Address { + for (i, cf) in self.control_function_table.iter().enumerate() { + if let Some(extant_cf) = cf { + if extant_cf.borrow().get_name().raw_name == name.raw_name { + return Address(i as u8); + } + } + } + Address::NULL + } + + pub(super) fn on_new_internal_control_function( + &mut self, + new_cf: Rc>, + ) { + self.inactive_control_functions.push(new_cf.clone()); + self.address_claim_state_machines.push(new_cf); + } + + pub(super) fn get_next_free_arbitrary_address(&self) -> Address { + for address in 129..247 { + let is_device_at_address = self.get_control_function_by_address(Address(address)); + let is_valid_device: bool = is_device_at_address.is_some(); + + if !is_valid_device { + return Address(address); + } else { + let device_at_our_address = is_device_at_address.as_ref().unwrap().borrow(); + + let preferred_address_name: u64 = match &*device_at_our_address { + ControlFunction::External { name } => name.raw_name, + ControlFunction::Internal { address_claim_data } => { + address_claim_data.get_name().raw_name + } + }; + + if DEFAULT_NAME == preferred_address_name { + return Address(address); + } + } + } + Address::NULL + } + + pub(super) fn construct_address_claim(source_address: Address, name: NAME) -> CANMessage { + let address_claim = name.raw_name.to_le_bytes().to_vec(); + + let request_id = CanId::try_encode( + Pgn::from_raw(CommonParameterGroupNumbers::AddressClaim as u32), + source_address, + Address::BROADCAST, + Priority::Default, + ); + CANMessage::new(address_claim, request_id.unwrap()) + } + + pub(super) fn construct_request_for_address_claim() -> CANMessage { + let pgn_to_request: u32 = CommonParameterGroupNumbers::AddressClaim as u32; + let request = pgn_to_request.to_le_bytes().to_vec(); + let request_id = CanId::try_encode( + Pgn::from_raw(CommonParameterGroupNumbers::ParameterGroupNumberRequest as u32), + Address::NULL, + Address::BROADCAST, + Priority::Three, + ); + CANMessage::new(request, request_id.unwrap()) + } + + pub(super) fn enqueue_can_message( + &mut self, + message: CANMessage, + queue_priority: MessageQueuePriority, + ) { + // Todo, max queue depth? + match queue_priority { + MessageQueuePriority::High => { + self.high_priority_can_message_tx_queue.push_back(message) + } + MessageQueuePriority::Normal => { + self.normal_priority_can_message_tx_queue.push_back(message) + } + } + } + + pub fn send_can_message( + &mut self, + parameter_group_number: Pgn, + data: &[u8], + source: Rc>, + destination: Rc>, + priority: Priority, + ) -> CANTransmitState { + if !data.is_empty() { + // Todo, handle lengths greater than 8 + + if data.len() <= 8 { + let source = source.borrow(); + let destination = destination.borrow(); + let message_id = CanId::try_encode( + parameter_group_number, + self.get_control_function_address_by_name(source.get_name()), + self.get_control_function_address_by_name(destination.get_name()), + priority, + ) + .unwrap_or_default(); + + if message_id.raw() != CanId::default().raw() { + self.enqueue_can_message( + CANMessage::new(data.to_vec(), message_id), + MessageQueuePriority::Normal, + ); + return CANTransmitState::Success; + } + } + } + CANTransmitState::Fail + } + + fn update_address_claiming(&mut self) { + let mut state_machines = std::mem::take(&mut self.address_claim_state_machines); + for address_claimer in &mut state_machines { + let mut address_claimer = address_claimer.borrow_mut(); + match *address_claimer { + ControlFunction::Internal { + ref mut address_claim_data, + } => { + if address_claim_data.get_enabled() { + match address_claim_data.get_state() { + AddressClaimingState::None => { + address_claim_data.set_state( + AddressClaimingState::update_state_none(address_claim_data), + ); + } + AddressClaimingState::WaitForClaim => { + if address_claim_data.get_timestamp().is_none() { + address_claim_data.set_timestamp(Some(Instant::now())) + } + + address_claim_data.set_state( + AddressClaimingState::update_state_wait_for_claim( + address_claim_data, + ), + ); + } + AddressClaimingState::SendRequestForClaim => { + address_claim_data.set_state( + AddressClaimingState::update_state_send_request_for_claim(self), + ); + } + AddressClaimingState::WaitForRequestContentionPeriod => { + address_claim_data.set_state( + AddressClaimingState::update_state_wait_for_request_contention( + address_claim_data, + self, + ), + ); + } + AddressClaimingState::SendPreferredAddressClaim + | AddressClaimingState::SendReclaimAddressOnRequest + | AddressClaimingState::ContendForPreferredAddress => { + address_claim_data.set_state( + AddressClaimingState::update_state_send_preferred_address_claim( + address_claim_data, + self, + ), + ); + } + AddressClaimingState::SendArbitraryAddressClaim => { + address_claim_data.set_state( + AddressClaimingState::update_state_send_arbitrary_address_claim( + address_claim_data, + self, + ), + ); + address_claim_data + .set_preferred_address(self.get_next_free_arbitrary_address()); + } + AddressClaimingState::AddressClaimingComplete + | AddressClaimingState::UnableToClaim => { + // Nothing to do + } + } + } + } + _ => panic!("Only Internal CFs can perform address claiming"), + } + } + std::mem::swap(&mut state_machines, &mut self.address_claim_state_machines); + } + + fn update_receive_messages(&mut self) { + while !self.receive_message_queue.is_empty() { + // Todo receive messages, need to generalize message handling + let current_message = self.receive_message_queue.front().unwrap(); + + // Process address claims and requests to claim + if DEFAULT_NAME == current_message.get_destination_name().raw_name { + // Broadcast Message + if current_message.get_identifier().pgn() + == Pgn::from_raw(CommonParameterGroupNumbers::AddressClaim as u32) + { + // Todo + } else if current_message.get_identifier().pgn() + == Pgn::from_raw( + CommonParameterGroupNumbers::ParameterGroupNumberRequest as u32, + ) + && current_message.get_data().len() >= 3 + { + let message_data = current_message.get_data(); + let requested_pgn: u32 = (message_data[0] as u32) + | ((message_data[1] as u32) << 8) + | ((message_data[2] as u32) << 16); + + if requested_pgn + == CommonParameterGroupNumbers::ParameterGroupNumberRequest as u32 + { + for internal_cf in &mut self.address_claim_state_machines { + let mut address_claimer = internal_cf.borrow_mut(); + match *address_claimer { + ControlFunction::Internal { + ref mut address_claim_data, + } => { + if address_claim_data.get_state() + == AddressClaimingState::AddressClaimingComplete + { + address_claim_data.set_state( + AddressClaimingState::SendReclaimAddressOnRequest, + ); + } + } + ControlFunction::External { name: _ } => {} + } + } + } + } else { + // Destination specific + } + + self.receive_message_queue.pop_front(); + } + } + } + + fn update_transmit_messages(&mut self) { + let should_continue_sending: bool = true; // Todo, check driver return values. + + while !self.high_priority_can_message_tx_queue.is_empty() { + // todo hand off to driver + self.high_priority_can_message_tx_queue.pop_front(); + } + + while should_continue_sending && !self.normal_priority_can_message_tx_queue.is_empty() { + // todo hand off to driver + self.normal_priority_can_message_tx_queue.pop_front(); + } + } + + pub fn update(mut self) { + self.update_receive_messages(); + self.update_address_claiming(); + self.update_transmit_messages(); + } +} + +impl Default for NetworkManager { + fn default() -> Self { + Self::new() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_creating_network_manager() { + let network = NetworkManager::new(); + network.update(); + } + + #[test] + fn test_creating_internal_control_function() { + let mut network = NetworkManager::new(); + let test_name = NAME::build(4, 0, 8, 5, 6, 3, 2, 7, 1, true); + + let new_cf = ControlFunction::new_internal_control_function( + test_name, + Address(0x81), + true, + &mut network, + ); + + assert_eq!(new_cf.borrow().get_name().raw_name, test_name.raw_name); + } +}