[Core/DP]: Add address violation handling

Added proper handling of address violations as outlined in ISO11783-5
which requires that we resend our address claims when another CF transmits
from our address, and requires that we activate a DTC as well.

isobus/include/isobus/isobus/can_address_claim_state_machine.hpp

@@ -56,6 +56,11 @@ namespace isobus
 		/// @returns The current state of the state machine
 		State get_current_state() const;
 
+		/// @brief Used to inform the address claim state machine that two CFs are using the same source address.
+		/// This function may cause the state machine to emit an address claim depending on its state, as is
+		/// required by ISO11783-5.
+		void on_address_violation();
+
 		/// @brief Attempts to process a commanded address.
 		/// @details If the state machine has claimed successfully before,
 		/// this will attempt to move a NAME from the claimed address to the new, specified address.

isobus/include/isobus/isobus/can_internal_control_function.hpp

@@ -51,6 +51,10 @@ namespace isobus
 		/// @param[in] CANPort The CAN channel index for this control function to use
 		InternalControlFunction(NAME desiredName, std::uint8_t preferredAddress, std::uint8_t CANPort, CANLibBadge<InternalControlFunction>);
 
+		/// @brief Used to inform the member address claim state machine that two CFs are using the same source address.
+		/// @note Address violation occurs when two CFs are using the same source address.
+		void on_address_violation(CANLibBadge<CANNetworkManager>);
+
 		/// @brief Used by the network manager to tell the ICF that the address claim state machine needs to process
 		/// a J1939 command to move address.
 		void process_commanded_address(std::uint8_t commandedAddress, CANLibBadge<CANNetworkManager>);

isobus/include/isobus/isobus/can_network_manager.hpp

@@ -24,6 +24,7 @@
 #include "isobus/isobus/can_network_configuration.hpp"
 #include "isobus/isobus/can_transport_protocol.hpp"
 #include "isobus/isobus/nmea2000_fast_packet_protocol.hpp"
+#include "isobus/utility/event_dispatcher.hpp"
 
 #include <array>
 #include <deque>
@@ -175,6 +176,11 @@ namespace isobus
 		/// @returns The configuration class for this network manager
 		CANNetworkConfiguration &get_configuration();
 
+		/// @brief Returns the network manager's event dispatcher for notifying consumers whenever an
+		/// address violation occurs involving an internal control function.
+		/// @returns An event dispatcher which can be used to get notified about address violations
+		EventDispatcher<std::shared_ptr<InternalControlFunction>> &get_address_violation_event_dispatcher();
+
 	protected:
 		// Using protected region to allow protocols use of special functions from the network manager
 		friend class AddressClaimStateMachine; ///< Allows the network manager to work closely with the address claiming process
@@ -289,6 +295,13 @@ namespace isobus
 		/// @param[in] currentMessage The message to process
 		void process_any_control_function_pgn_callbacks(const CANMessage &currentMessage);
 
+		/// @brief Validates that a CAN message has not caused an address violation.
+		/// If a violation is found, the network manager will notify the affected address claim state machine
+		/// to re-claim as is required by ISO 11783-5, and will attempt to activate a DTC that is defined in ISO 11783-5.
+		/// @note Address violation occurs when two CFs are using the same source address.
+		/// @param[in] currentMessage The message to process
+		void process_can_message_for_address_violations(const CANMessage &currentMessage);
+
 		/// @brief Checks the control function state callback list to see if we need to call
 		/// a control function state callback.
 		/// @param[in] controlFunction The controlFunction whose state has changed
@@ -361,6 +374,7 @@ namespace isobus
 		std::list<ControlFunctionStateCallback> controlFunctionStateCallbacks; ///< List of all control function state callbacks
 		std::vector<ParameterGroupNumberCallbackData> globalParameterGroupNumberCallbacks; ///< A list of all global PGN callbacks
 		std::vector<ParameterGroupNumberCallbackData> anyControlFunctionParameterGroupNumberCallbacks; ///< A list of all global PGN callbacks
+		EventDispatcher<std::shared_ptr<InternalControlFunction>> addressViolationEventDispatcher; // An event dispatcher for notifying consumers about address violations
 #if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO
 		std::mutex receiveMessageMutex; ///< A mutex for receive messages thread safety
 		std::mutex protocolPGNCallbacksMutex; ///< A mutex for PGN callback thread safety

isobus/include/isobus/isobus/isobus_diagnostic_protocol.hpp

@@ -391,9 +391,6 @@ namespace isobus
 		/// @returns The two bit lamp state for CAN
 		std::uint8_t convert_flash_state_to_byte(FlashState flash) const;
 
-		/// @brief A utility function that will clean up PGN registrations
-		void deregister_all_pgns();
-
 		/// @brief This is a way to find the overall lamp states to report
 		/// @details This searches the active DTC list to find if a lamp is on or off, and to find the overall flash state for that lamp.
 		/// Basically, since the lamp states are global to the CAN message, we need a way to resolve the "total" lamp state from the list.
@@ -410,6 +407,11 @@ namespace isobus
 		/// @param[out] lampOn If the lamp state is on for any DTC
 		void get_inactive_list_lamp_state_and_flash_state(Lamps targetLamp, FlashState &flash, bool &lampOn) const;
 
+		/// @brief A callback function used to consume address violation events and activate a DTC
+		/// as required in ISO11783-5.
+		/// @param[in] affectedControlFunction The control function affected by an address violation
+		void on_address_violation(std::shared_ptr<InternalControlFunction> affectedControlFunction);
+
 		/// @brief Sends a DM1 encoded CAN message
 		/// @returns true if the message was sent, otherwise false
 		bool send_diagnostic_message_1() const;
@@ -488,6 +490,7 @@ namespace isobus
 		static void process_flags(std::uint32_t flag, void *parentPointer);
 
 		std::shared_ptr<InternalControlFunction> myControlFunction; ///< The internal control function that this protocol will send from
+		std::shared_ptr<void> addressViolationEventHandle; ///< Stores the handle from registering for address violation events
 		NetworkType networkType; ///< The diagnostic network type that this protocol will use
 		std::vector<DiagnosticTroubleCode> activeDTCList; ///< Keeps track of all the active DTCs
 		std::vector<DiagnosticTroubleCode> inactiveDTCList; ///< Keeps track of all the previously active DTCs

isobus/src/can_address_claim_state_machine.cpp

@@ -44,6 +44,17 @@ namespace isobus
 		return m_currentState;
 	}
 
+	void AddressClaimStateMachine::on_address_violation()
+	{
+		if (State::AddressClaimingComplete == get_current_state())
+		{
+			CANStackLogger::warn("[AC]: Address violation for address %u",
+			                     get_claimed_address());
+
+			set_current_state(State::SendReclaimAddressOnRequest);
+		}
+	}
+
 	void AddressClaimStateMachine::process_commanded_address(std::uint8_t commandedAddress)
 	{
 		if (State::AddressClaimingComplete == get_current_state())

isobus/src/can_internal_control_function.cpp

@@ -42,6 +42,11 @@ namespace isobus
 		return ControlFunction::destroy(expectedRefCount);
 	}
 
+	void InternalControlFunction::on_address_violation(CANLibBadge<CANNetworkManager>)
+	{
+		stateMachine.on_address_violation();
+	}
+
 	void InternalControlFunction::process_commanded_address(std::uint8_t commandedAddress, CANLibBadge<CANNetworkManager>)
 	{
 		stateMachine.process_commanded_address(commandedAddress);

isobus/src/can_network_manager.cpp

@@ -392,6 +392,11 @@ namespace isobus
 		return configuration;
 	}
 
+	EventDispatcher<std::shared_ptr<InternalControlFunction>> &CANNetworkManager::get_address_violation_event_dispatcher()
+	{
+		return addressViolationEventDispatcher;
+	}
+
 	bool CANNetworkManager::add_protocol_parameter_group_number_callback(std::uint32_t parameterGroupNumber, CANLibCallback callback, void *parentPointer)
 	{
 		bool retVal = false;
@@ -840,6 +845,26 @@ namespace isobus
 		}
 	}
 
+	void CANNetworkManager::process_can_message_for_address_violations(const CANMessage &currentMessage)
+	{
+		auto sourceAddress = currentMessage.get_identifier().get_source_address();
+
+		if ((BROADCAST_CAN_ADDRESS != sourceAddress) &&
+		    (NULL_CAN_ADDRESS != sourceAddress))
+		{
+			for (auto &internalCF : internalControlFunctions)
+			{
+				if ((nullptr != internalCF) &&
+				    (internalCF->get_address() == sourceAddress) &&
+				    (currentMessage.get_can_port_index() == internalCF->get_can_port()))
+				{
+					internalCF->on_address_violation({});
+					addressViolationEventDispatcher.call(internalCF);
+				}
+			}
+		}
+	}
+
 	void CANNetworkManager::process_control_function_state_change_callback(std::shared_ptr<ControlFunction> controlFunction, ControlFunctionState state)
 	{
 #if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO
@@ -937,6 +962,7 @@ namespace isobus
 			CANMessage currentMessage = get_next_can_message_from_rx_queue();
 
 			update_address_table(currentMessage);
+			process_can_message_for_address_violations(currentMessage);
 
 			// Update Special Callbacks, like protocols and non-cf specific ones
 			process_protocol_pgn_callbacks(currentMessage);

isobus/src/isobus_diagnostic_protocol.cpp

@@ -102,6 +102,7 @@ namespace isobus
 			CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast<std::uint32_t>(CANLibParameterGroupNumber::DiagnosticMessage22), process_message, this);
 			CANNetworkManager::CANNetwork.add_protocol_parameter_group_number_callback(static_cast<std::uint32_t>(CANLibParameterGroupNumber::DiagnosticMessage13), process_message, this);
 			CANNetworkManager::CANNetwork.add_global_parameter_group_number_callback(static_cast<std::uint32_t>(CANLibParameterGroupNumber::DiagnosticMessage13), process_message, this);
+			addressViolationEventHandle = CANNetworkManager::CANNetwork.get_address_violation_event_dispatcher().add_listener([this](std::shared_ptr<InternalControlFunction> affectedCF) { this->on_address_violation(affectedCF); });
 
 			if (auto requestProtocol = myControlFunction->get_pgn_request_protocol().lock())
 			{
@@ -148,6 +149,7 @@ namespace isobus
 			CANNetworkManager::CANNetwork.remove_protocol_parameter_group_number_callback(static_cast<std::uint32_t>(CANLibParameterGroupNumber::DiagnosticMessage22), process_message, this);
 			CANNetworkManager::CANNetwork.remove_protocol_parameter_group_number_callback(static_cast<std::uint32_t>(CANLibParameterGroupNumber::DiagnosticMessage13), process_message, this);
 			CANNetworkManager::CANNetwork.remove_global_parameter_group_number_callback(static_cast<std::uint32_t>(CANLibParameterGroupNumber::DiagnosticMessage13), process_message, this);
+			addressViolationEventHandle.reset();
 		}
 	}
 
@@ -605,6 +607,22 @@ namespace isobus
 		}
 	}
 
+	void DiagnosticProtocol::on_address_violation(std::shared_ptr<InternalControlFunction> affectedControlFunction)
+	{
+		if ((nullptr != affectedControlFunction) &&
+		    (!get_j1939_mode()) &&
+		    (BROADCAST_CAN_ADDRESS != affectedControlFunction->get_address()) &&
+		    (NULL_CAN_ADDRESS != affectedControlFunction->get_address()))
+		{
+			constexpr std::uint32_t ADDRESS_VIOLATION_SPN_BASE = 2000; // Defined in ISO 11783-5 section 4.4.4.3
+
+			set_diagnostic_trouble_code_active(DiagnosticTroubleCode(ADDRESS_VIOLATION_SPN_BASE + affectedControlFunction->get_address(),
+			                                                         FailureModeIdentifier::ConditionExists,
+			                                                         LampStatus::None),
+			                                   true);
+		}
+	}
+
 	bool DiagnosticProtocol::send_diagnostic_message_1() const
 	{
 		bool retVal = false;

test/diagnostic_protocol_tests.cpp

@@ -977,6 +977,7 @@ TEST(DIAGNOSTIC_PROTOCOL_TESTS, MessageEncoding)
 
 		// Test a suspension by another ECU. Set only our network.
 		testFrame.dataLength = 8;
+		testFrame.identifier = 0x18DFFFAB;
 		testFrame.data[0] = 0xFC;
 		testFrame.data[1] = 0xFF;
 		testFrame.data[2] = 0xFF;
@@ -1358,6 +1359,23 @@ TEST(DIAGNOSTIC_PROTOCOL_TESTS, MessageEncoding)
 		protocolUnderTest.clear_active_diagnostic_trouble_codes();
 		protocolUnderTest.clear_inactive_diagnostic_trouble_codes();
 	}
+
+	{
+		// Test address violation
+		// Construct a random message from our address of 0xAA
+		testFrame.identifier = 0x18EFFFAA;
+		memset(testFrame.data, 0, sizeof(testFrame.data));
+		CANNetworkManager::process_receive_can_message_frame(testFrame);
+		CANNetworkManager::CANNetwork.update();
+		protocolUnderTest.update();
+
+		DiagnosticProtocol::DiagnosticTroubleCode addressViolationDTC(2000 + 0xAA, DiagnosticProtocol::FailureModeIdentifier::ConditionExists, DiagnosticProtocol::LampStatus::None);
+		EXPECT_TRUE(protocolUnderTest.get_diagnostic_trouble_code_active(addressViolationDTC));
+
+		// Reset back to a known state
+		protocolUnderTest.clear_active_diagnostic_trouble_codes();
+		protocolUnderTest.clear_inactive_diagnostic_trouble_codes();
+	}
 	protocolUnderTest.terminate();
 	EXPECT_FALSE(protocolUnderTest.get_initialized());
 	CANHardwareInterface::stop();