From 247bf24a207bf583a58402023e1861439d74fe1c Mon Sep 17 00:00:00 2001
From: Andrey Gusakov <dron0gus@gmail.com>
Date: Mon, 11 Mar 2024 22:48:11 +0300
Subject: [PATCH] max3185x: restart driver thread on settings change

---
 firmware/hw_layer/hardware.cpp         |  5 ----
 firmware/hw_layer/sensors/max3185x.cpp | 41 +++++++++++++++++++++-----
 firmware/hw_layer/sensors/max3185x.h   |  2 ++
 firmware/init/sensor/init_sensors.cpp  | 11 +++++++
 4 files changed, 47 insertions(+), 12 deletions(-)

diff --git a/firmware/hw_layer/hardware.cpp b/firmware/hw_layer/hardware.cpp
index 20340efa21..3a5d00d311 100644
--- a/firmware/hw_layer/hardware.cpp
+++ b/firmware/hw_layer/hardware.cpp
@@ -16,7 +16,6 @@
 #include "bench_test.h"
 #include "yaw_rate_sensor.h"
 #include "pin_repository.h"
-#include "max3185x.h"
 #include "logic_analyzer.h"
 #include "smart_gpio.h"
 #include "accelerometer.h"
@@ -620,10 +619,6 @@ void initHardware() {
 	initMc33816();
 #endif /* EFI_MC33816 */
 
-#if EFI_MAX_31855
-	initMax3185x(engineConfiguration->max31855spiDevice, engineConfiguration->max31855_cs);
-#endif /* EFI_MAX_31855 */
-
 #if EFI_CAN_SUPPORT
 #if EFI_SIMULATOR
 	// Set CAN device name
diff --git a/firmware/hw_layer/sensors/max3185x.cpp b/firmware/hw_layer/sensors/max3185x.cpp
index 8cdea9fb9d..e33c45e202 100644
--- a/firmware/hw_layer/sensors/max3185x.cpp
+++ b/firmware/hw_layer/sensors/max3185x.cpp
@@ -84,11 +84,28 @@ public:
 			ThreadController::start();
 			return 0;
 		}
+
+		efiPrintf("EGT not configured");
 		return -1;
 	}
 
+	void stop(void) {
+		ThreadController::stop();
+
+		for (size_t i = 0; i < EGT_CHANNEL_COUNT; i++) {
+			if (!isBrainPinValid(m_cs[i])) {
+				continue;
+			}
+
+			auto& sensor = egtSensors[i];
+
+			brain_pin_markUnused(m_cs[i]);
+			sensor.unregister();
+		}
+	}
+
 	void ThreadTask() override {
-		while (true) {
+		while (!chThdShouldTerminateX()) {
 			for (int i = 0; i < EGT_CHANNEL_COUNT; i++) {
 				float value;
 
@@ -104,6 +121,8 @@ public:
 
 			chThdSleepMilliseconds(MAX3185X_REFRESH_TIME);
 		}
+
+		chThdExit((msg_t)0x0);
 	}
 
 	/* Debug stuff */
@@ -435,12 +454,20 @@ static void egtRead() {
 }
 
 void initMax3185x(spi_device_e device, egt_cs_array_t max31855_cs) {
-	if (instance.start(device, max31855_cs) == 0) {
-		addConsoleAction("egtinfo", showEgtInfo);
-		addConsoleAction("egtread", egtRead);
-	} else {
-	  efiPrintf("EGT not configured");
-	}
+	addConsoleAction("egtinfo", (Void) showEgtInfo);
+	addConsoleAction("egtread", (Void) egtRead);
+
+	startMax3185x(device, max31855_cs);
+}
+
+void stopMax3185x(void)
+{
+	instance.stop();
+}
+
+void startMax3185x(spi_device_e device, egt_cs_array_t max31855_cs)
+{
+	instance.start(device, max31855_cs);
 }
 
 #endif /* EFI_MAX_31855 */
diff --git a/firmware/hw_layer/sensors/max3185x.h b/firmware/hw_layer/sensors/max3185x.h
index 9557216418..772a061648 100644
--- a/firmware/hw_layer/sensors/max3185x.h
+++ b/firmware/hw_layer/sensors/max3185x.h
@@ -12,4 +12,6 @@
 
 #if HAL_USE_SPI
 void initMax3185x(spi_device_e device, egt_cs_array_t max31855_cs);
+void stopMax3185x(void);
+void startMax3185x(spi_device_e device, egt_cs_array_t max31855_cs);
 #endif /* HAL_USE_SPI */
diff --git a/firmware/init/sensor/init_sensors.cpp b/firmware/init/sensor/init_sensors.cpp
index 32824fc319..de22570749 100644
--- a/firmware/init/sensor/init_sensors.cpp
+++ b/firmware/init/sensor/init_sensors.cpp
@@ -8,6 +8,7 @@
 #include "cli_registry.h"
 #include "io_pins.h"
 #include "lua_hooks.h"
+#include "max3185x.h"
 
 static void initSensorCli();
 
@@ -105,6 +106,10 @@ void initNewSensors() {
 	initOldAnalogInputs();
 	initAuxDigital();
 
+#if EFI_MAX_31855
+	initMax3185x(engineConfiguration->max31855spiDevice, engineConfiguration->max31855_cs);
+#endif /* EFI_MAX_31855 */
+
 	// Init CLI functionality for sensors (mocking)
 	initSensorCli();
 
@@ -137,6 +142,9 @@ void stopSensors() {
 	deinitAuxSpeedSensors();
 	deinitMap();
 	deinitInputShaftSpeedSensor();
+#if EFI_MAX_31855
+	stopMax3185x();
+#endif /* EFI_MAX_31855 */
 }
 
 void reconfigureSensors() {
@@ -152,6 +160,9 @@ void reconfigureSensors() {
 	initInputShaftSpeedSensor();
 
 	initOldAnalogInputs();
+#if EFI_MAX_31855
+	startMax3185x(engineConfiguration->max31855spiDevice, engineConfiguration->max31855_cs);
+#endif /* EFI_MAX_31855 */
 }
 
 // Mocking/testing helpers