From da4d3e4babd57156914f6cd29b4cc7cbf5c3e2b9 Mon Sep 17 00:00:00 2001
From: Andrey Gusakov <dron0gus@gmail.com>
Date: Thu, 18 Apr 2024 23:30:47 +0300
Subject: [PATCH] msgpio: update

---
 firmware/hw_layer/drivers/gpio/can_gpio.cpp | 87 ++++++++++++++-------
 1 file changed, 58 insertions(+), 29 deletions(-)

diff --git a/firmware/hw_layer/drivers/gpio/can_gpio.cpp b/firmware/hw_layer/drivers/gpio/can_gpio.cpp
index f9965b47c2e..fe68b3e4a79 100644
--- a/firmware/hw_layer/drivers/gpio/can_gpio.cpp
+++ b/firmware/hw_layer/drivers/gpio/can_gpio.cpp
@@ -129,7 +129,6 @@ typedef enum {
 	MSIOBOX_DISABLED = 0,
 	MSIOBOX_WAIT_INIT,
 	MSIOBOX_WAIT_WHOAMI,
-	MSIOBOX_APPLY_SETTINGS,
 	MSIOBOX_READY,
 	MSIOBOX_FAILED
 } msiobox_state;
@@ -143,6 +142,7 @@ class MsIoBox final : public GpioChip, public CanListener {
 	bool acceptFrame(const CANRxFrame& frame) const override;
 
 	int init() override;
+	int config(uint32_t bus, uint32_t base, uint16_t period);
 #if 0
 	/* pin argument is pin number within gpio chip, not a global number */
 	int writePad(size_t pin, int value) override {
@@ -203,7 +203,7 @@ MsIoBox::MsIoBox()
 
 MsIoBox::MsIoBox(uint32_t bus, uint32_t base, uint16_t period)
 	: CanListener(0), bus(bus), base(base), period(period) {
-	/* Need init state */
+	/* init state */
 	state = MSIOBOX_WAIT_INIT;
 	stateTimer.reset();
 }
@@ -214,6 +214,23 @@ int MsIoBox::init()
 	return 0;
 }
 
+int MsIoBox::config(uint32_t _bus, uint32_t _base, uint16_t _period)
+{
+	/* TODO: sanity checks? */
+	bus = _bus;
+	base = _base;
+	period = _period;
+
+	/* Force init */
+	state = MSIOBOX_WAIT_INIT;
+	stateTimer.reset();
+
+	/* TODO: */
+	//registerCanListener(this);
+
+	return 0;
+}
+
 bool MsIoBox::acceptFrame(const CANRxFrame& frame) const {
 	/* 11 bit only */
 	if (CAN_ISX(frame)) {
@@ -235,9 +252,6 @@ bool MsIoBox::acceptFrame(const CANRxFrame& frame) const {
 int MsIoBox::ping() {
 	CanTxTyped<iobox_ping> frame(CanCategory::MEGASQUIRT, base + CAN_IOBOX_PING, false, 0);
 
-	state = MSIOBOX_WAIT_WHOAMI;
-	stateTimer.reset();
-
 	return 0;
 }
 
@@ -250,9 +264,6 @@ int MsIoBox::setup() {
 	cfg->adc_broadcast_interval = period;
 	cfg->tach_broadcast_interval = period;
 
-	state = MSIOBOX_READY;
-	stateTimer.reset();
-
 	return 0;
 }
 
@@ -309,6 +320,7 @@ void MsIoBox::decodeFrame(const CANRxFrame& frame, efitick_t) {
 			auto data = reinterpret_cast<const iobox_tach*>(&frame.data8[0]);
 
 			/* TODO: should be atomic, add lock here? */
+			/* TODO: accumulate totalTooth? */
 			Tach[i].period = data->period;
 			Tach[i].teeths = data->n_teeth;
 			Tach[i].totalTooth = data->total_tooth;
@@ -327,6 +339,10 @@ void MsIoBox::decodeFrame(const CANRxFrame& frame, efitick_t) {
 			/* apply settings and set sync output states */
 			setup();
 			update();
+
+			/* now we are ready */
+			state = MSIOBOX_READY;
+			stateTimer.reset();
 		} else {
 			handled = false;
 		}
@@ -343,33 +359,45 @@ void MsIoBox::decodeFrame(const CANRxFrame& frame, efitick_t) {
 
 void MsIoBox::checkState(void)
 {
-	if (state == MSIOBOX_READY) {
-		if (needUpdateConfig) {
-			setup();
-			needUpdateConfig = false;
-			/* Force update */
-			needUpdate = true;
-		}
-		if (needUpdate) {
-			update();
-		}
-	} else if (state == MSIOBOX_WAIT_INIT) {
+	switch (state) {
+	case MSIOBOX_DISABLED:
+		/* nop */
+		break;
+	case MSIOBOX_WAIT_INIT:
 		ping();
-	} else if (state == MSIOBOX_WAIT_WHOAMI) {
+
+		state = MSIOBOX_WAIT_WHOAMI;
+		stateTimer.reset();
+
+		break;
+	case MSIOBOX_WAIT_WHOAMI:
 		if (stateTimer.hasElapsedMs(MSIOBOX_PING_TIMEOUT)) {
 			state = MSIOBOX_FAILED;
 			stateTimer.reset();
 		}
-	} else if (state == MSIOBOX_FAILED) {
-		if (stateTimer.hasElapsedMs(MSIOBOX_RESTART_TIMEOUT)) {
-			state = MSIOBOX_WAIT_INIT;
-			stateTimer.reset();
-		}
-	} else if (state == MSIOBOX_READY) {
+		break;
+	case MSIOBOX_READY:
 		if (stateTimer.hasElapsedMs(period * 3)) {
 			state = MSIOBOX_FAILED;
 			stateTimer.reset();
+		} else {
+			if (needUpdateConfig) {
+				setup();
+				needUpdateConfig = false;
+				/* Force update */
+				needUpdate = true;
+			}
+			if (needUpdate) {
+				update();
+			}
 		}
+		break;
+	case MSIOBOX_FAILED:
+		if (stateTimer.hasElapsedMs(MSIOBOX_RESTART_TIMEOUT)) {
+			state = MSIOBOX_WAIT_INIT;
+			stateTimer.reset();
+		}
+		break;
 	}
 }
 
@@ -385,9 +413,10 @@ static MsIoBox instance[BOARD_CAN_GPIO_COUNT];
 void initCanGpio() {
 	// CAN_PIN_0
 	for (size_t i = 0; i < BOARD_CAN_GPIO_COUNT; i++) {
-		/* TODO: pick can bus and CAN ID base from settings */
-		//instance[i].MsIoBox(0, CAN_IOBOX_BASE0, 20);
-		registerCanListener(instance[i]);
+		/* TODO: pick can bus, base CAN ID (and refresh rate) from settings */
+		if (instance[i].config(0, CAN_IOBOX_BASE0, 20) == 0) {
+			registerCanListener(instance[i]);
+		}
 	}
 }
 #endif // EFI_CAN_GPIO