diff --git a/src/driver/can_id.rs b/src/driver/can_id.rs index 51affdd..89575fe 100644 --- a/src/driver/can_id.rs +++ b/src/driver/can_id.rs @@ -286,7 +286,7 @@ mod tests { Address(0x0F), Priority::Six, ); - assert!(matches!(encode_result, Err(_))); + assert!(encode_result.is_err()); let error_contents: EncodingError = encode_result.unwrap_err(); assert_eq!(error_contents.priority, Priority::Six); diff --git a/src/network_management/can_message.rs b/src/network_management/can_message.rs new file mode 100644 index 0000000..0355e7e --- /dev/null +++ b/src/network_management/can_message.rs @@ -0,0 +1,37 @@ +// Copyright 2023 Raven Industries inc. +use super::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::default(), + destination_name: NAME::default(), + } + } + + 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..34999a6 100644 --- a/src/network_management/control_function.rs +++ b/src/network_management/control_function.rs @@ -1,9 +1,12 @@ // Copyright 2023 Raven Industries inc. -#![allow(dead_code)] - +use crate::driver::Address; 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 +34,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 +50,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.into(), + ControlFunction::Internal { + address_claim_data: _, + } => claim_to_process.get_name().into(), + }; + + if (!claim_to_process.get_name().get_self_configurable_address() + && preferred_address_name > claim_to_process.get_name().into()) + || >::into(NAME::default()) == 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().into() { + // 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 +212,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 +228,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 != new_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 +256,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/network_manager.rs b/src/network_management/network_manager.rs new file mode 100644 index 0000000..7314bd1 --- /dev/null +++ b/src/network_management/network_manager.rs @@ -0,0 +1,363 @@ +// 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::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() == 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).into(), + ControlFunction::Internal { address_claim_data } => { + address_claim_data.get_name().into() + } + }; + + if >::into(NAME::default()) == preferred_address_name { + return Address(address); + } + } + } + Address::NULL + } + + pub(super) fn construct_address_claim(source_address: Address, name: NAME) -> CANMessage { + let address_claim = >::into(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(CanId::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 NAME::default() == current_message.get_destination_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::builder() + .device_class(0) + .device_class_instance(0) + .ecu_instance(0) + .function_code(130) + .function_instance(0) + .identity_number(123_u32) + .industry_group(2) + .function_instance(0) + .build(); + + let new_cf = ControlFunction::new_internal_control_function( + test_name, + Address(0x81), + true, + &mut network, + ); + + assert_eq!( + >::into(new_cf.borrow().get_name()), + test_name.into() + ); + } +}