From 4f39b22be65297698f288c96b44490b1dc1667e9 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Sat, 2 Mar 2024 15:53:57 -0800
Subject: [PATCH] registerCanListener should be idempotent

---
 firmware/controllers/can/can_listener.h |  4 ++++
 firmware/controllers/can/can_rx.cpp     | 25 ++++++++++++++++++++++---
 2 files changed, 26 insertions(+), 3 deletions(-)

diff --git a/firmware/controllers/can/can_listener.h b/firmware/controllers/can/can_listener.h
index 546c12ee3e..c842fcc04d 100644
--- a/firmware/controllers/can/can_listener.h
+++ b/firmware/controllers/can/can_listener.h
@@ -36,6 +36,10 @@ public:
 		return m_next;
 	}
 
+	bool hasNext() const {
+		return m_next;
+	}
+
 	// Return true if the provided frame should be accepted for processing by the listener.
 	// Override if you need more complex logic than comparing to a single ID.
 	virtual bool acceptFrame(const CANRxFrame& frame) const {
diff --git a/firmware/controllers/can/can_rx.cpp b/firmware/controllers/can/can_rx.cpp
index b605f11c50..a508c28114 100644
--- a/firmware/controllers/can/can_rx.cpp
+++ b/firmware/controllers/can/can_rx.cpp
@@ -55,7 +55,23 @@ static void printPacket(const size_t busIndex, const CANRxFrame &rx) {
 
 volatile float canMap = 0;
 
-CanListener *canListeners_head = nullptr;
+struct CanListenerTailSentinel : public CanListener {
+	CanListenerTailSentinel()
+		: CanListener(0)
+	{
+	}
+
+	bool acceptFrame(const CANRxFrame&) const override {
+		return false;
+	}
+
+	void decodeFrame(const CANRxFrame&, efitick_t) override {
+		// nothing to do
+	}
+};
+
+static CanListenerTailSentinel tailSentinel;
+CanListener *canListeners_head = &tailSentinel;
 
 void serviceCanSubscribers(const CANRxFrame &frame, efitick_t nowNt) {
 	CanListener *current = canListeners_head;
@@ -71,8 +87,11 @@ void serviceCanSubscribers(const CANRxFrame &frame, efitick_t nowNt) {
 }
 
 void registerCanListener(CanListener& listener) {
-	listener.setNext(canListeners_head);
-	canListeners_head = &listener;
+	// If the listener already has a next, it's already registered
+	if (!listener.hasNext()) {
+		listener.setNext(canListeners_head);
+		canListeners_head = &listener;
+	}
 }
 
 void registerCanSensor(CanSensorBase& sensor) {