From 045db15dac987037b9c446166adaffec145913b9 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Sat, 14 Aug 2021 05:41:27 -0700
Subject: [PATCH] implement adc unsubscription (#3149)

* allow re-register of the same sensor

* adc unsub
---
 firmware/controllers/sensors/sensor.cpp    |  2 +-
 firmware/hw_layer/adc/adc_subscription.cpp | 71 +++++++++++++++++-----
 firmware/hw_layer/adc/adc_subscription.h   |  1 +
 3 files changed, 57 insertions(+), 17 deletions(-)

diff --git a/firmware/controllers/sensors/sensor.cpp b/firmware/controllers/sensors/sensor.cpp
index 476dd4c4c0..a923ddbc38 100644
--- a/firmware/controllers/sensors/sensor.cpp
+++ b/firmware/controllers/sensors/sensor.cpp
@@ -81,7 +81,7 @@ public:
 
 	bool Register(Sensor* sensor) {
 		// If there's somebody already here - a consumer tried to double-register a sensor
-		if (m_sensor) {
+		if (m_sensor && m_sensor != sensor) {
 			// This sensor has already been registered. Don't re-register it.
 			firmwareError(CUSTOM_OBD_26, "Duplicate registration for sensor \"%s\"", sensor->getSensorName());
 			return false;
diff --git a/firmware/hw_layer/adc/adc_subscription.cpp b/firmware/hw_layer/adc/adc_subscription.cpp
index 25ce9bc234..14113a3fa6 100644
--- a/firmware/hw_layer/adc/adc_subscription.cpp
+++ b/firmware/hw_layer/adc/adc_subscription.cpp
@@ -6,11 +6,10 @@
 
 #if EFI_UNIT_TEST
 
-void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
-									  adc_channel_e channel,
-									  float lowpassCutoff,
-									  float voltsPerAdcVolt /*= 0.0f*/)
-{
+/*static*/ void AdcSubscription::SubscribeSensor(FunctionalSensor&, adc_channel_e, float, float) {
+}
+
+/*static*/ void AdcSubscription::UnsubscribeSensor(FunctionalSensor&) {
 }
 
 #else
@@ -23,10 +22,24 @@ struct AdcSubscriptionEntry {
 	bool HasUpdated = false;
 };
 
-static size_t s_nextEntry = 0;
 static AdcSubscriptionEntry s_entries[16];
 
-void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
+static AdcSubscriptionEntry* findEntry(FunctionalSensor* sensor) {
+	for (size_t i = 0; i < efi::size(s_entries); i++) {
+		if (s_entries[i].Sensor == sensor) {
+			return &s_entries[i];
+		}
+	}
+
+	return nullptr;
+}
+
+static AdcSubscriptionEntry* findEntry() {
+	// Find an entry with no sensor set
+	return findEntry(nullptr);
+}
+
+/*static*/ void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
 									  adc_channel_e channel,
 									  float lowpassCutoff,
 									  float voltsPerAdcVolt /*= 0.0f*/) {
@@ -41,8 +54,10 @@ void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
 		return;
 	}
 
-	// Ensure that enough entries are available
-	if (s_nextEntry >= efi::size(s_entries)) {
+	auto entry = findEntry();
+
+	// Ensure that a free entry was found
+	if (!entry) {
 		firmwareError(CUSTOM_INVALID_ADC, "too many ADC subscriptions");
 		return;
 	}
@@ -58,21 +73,45 @@ void AdcSubscription::SubscribeSensor(FunctionalSensor &sensor,
 	}
 
 	// Populate the entry
-	auto &entry = s_entries[s_nextEntry];
-	entry.Sensor = &sensor;
-	entry.VoltsPerAdcVolt = voltsPerAdcVolt;
-	entry.Channel = channel;
-	entry.Filter.configureLowpass(SLOW_ADC_RATE, lowpassCutoff);
+	entry->VoltsPerAdcVolt = voltsPerAdcVolt;
+	entry->Channel = channel;
+	entry->Filter.configureLowpass(SLOW_ADC_RATE, lowpassCutoff);
 
-	s_nextEntry++;
+	// Set the sensor last - it's the field we use to determine whether this entry is in use
+	entry->Sensor = &sensor;
+}
+
+/*static*/ void AdcSubscription::UnsubscribeSensor(FunctionalSensor& sensor) {
+	auto entry = findEntry(&sensor);
+
+	if (!entry) {
+		// This sensor wasn't configured, skip it
+		return;
+	}
+
+#if EFI_PROD_CODE
+	// Release the pin
+	efiSetPadUnused(getAdcChannelBrainPin("adc unsubscribe", entry->Channel));
+#endif // EFI_PROD_CODE
+
+	// clear the sensor first to mark this entry not in use
+	entry->Sensor = nullptr;
+
+	entry->VoltsPerAdcVolt = 0;
+	entry->Channel = EFI_ADC_NONE;
 }
 
 void AdcSubscription::UpdateSubscribers(efitick_t nowNt) {
 	ScopePerf perf(PE::AdcSubscriptionUpdateSubscribers);
 
-	for (size_t i = 0; i < s_nextEntry; i++) {
+	for (size_t i = 0; i < efi::size(s_entries); i++) {
 		auto &entry = s_entries[i];
 
+		if (!entry.Sensor) {
+			// Skip unconfigured entries
+			continue;
+		}
+
 		float mcuVolts = getVoltage("sensor", entry.Channel);
 		float sensorVolts = mcuVolts * entry.VoltsPerAdcVolt;
 
diff --git a/firmware/hw_layer/adc/adc_subscription.h b/firmware/hw_layer/adc/adc_subscription.h
index f8f85728f8..661e891359 100644
--- a/firmware/hw_layer/adc/adc_subscription.h
+++ b/firmware/hw_layer/adc/adc_subscription.h
@@ -10,5 +10,6 @@
 class AdcSubscription {
 public:
 	static void SubscribeSensor(FunctionalSensor &sensor, adc_channel_e channel, float lowpassCutoff, float voltsPerAdcVolt = 0.0f);
+	static void UnsubscribeSensor(FunctionalSensor& sensor);
 	static void UpdateSubscribers(efitick_t nowNt);
 };