ports: stm32: h7: CAN recovery

Author: dron0gus

firmware/controllers/can/can.h

@@ -19,7 +19,8 @@
 
 #include "periodic_thread_controller.h"
 
-#define CAN_TIMEOUT MS2NT(100)
+// Try to recover CAN after following timeout
+#define CAN_RX_TIMEOUT	TIME_MS2I(100)
 
 //can tx periodic task cycle time in frequency, 200hz -> 5ms period
 #define CAN_CYCLE_FREQ		(200.0f)

firmware/hw_layer/drivers/can/can_hw.cpp

@@ -59,9 +59,10 @@ class CanRead final : protected ThreadController<UTILITY_THREAD_STACK_SIZE> {
 	void ThreadTask() override {
 		while (true) {
 			// Block until we get a message
-			msg_t result = canReceiveTimeout(m_device, CAN_ANY_MAILBOX, &m_buffer, TIME_INFINITE);
+			msg_t result = canReceiveTimeout(m_device, CAN_ANY_MAILBOX, &m_buffer, CAN_RX_TIMEOUT);
 
 			if (result != MSG_OK) {
+				canHwRecover(m_index, m_device);
 				continue;
 			}
 

firmware/hw_layer/ports/at32/at32_can.cpp

@@ -132,4 +132,9 @@ void canHwInfo(CANDriver* cand)
    (void)cand;
 }
 
+void canHwRecover(const size_t, CANDriver *)
+{
+	/* TODO: */
+}
+
 #endif /* EFI_CAN_SUPPORT */

firmware/hw_layer/ports/cypress/mpu_util.cpp

@@ -228,6 +228,11 @@ void canHwInfo(CANDriver* cand)
 	(void)cand;
 }
 
+void canHwRecover(const size_t, CANDriver *)
+{
+	/* TODO: */
+}
+
 #endif /* EFI_CAN_SUPPORT */
 
 bool mcuCanFlashWhileRunning() {

firmware/hw_layer/ports/kinetis/mpu_util.cpp

@@ -221,6 +221,11 @@ void canHwInfo(CANDriver* cand)
 	(void)cand;
 }
 
+void canHwRecover(const size_t, CANDriver *)
+{
+	/* TODO: */
+}
+
 #endif /* EFI_CAN_SUPPORT */
 
 bool mcuCanFlashWhileRunning() {

firmware/hw_layer/ports/mpu_util.h

@@ -50,6 +50,7 @@ bool isValidCanTxPin(brain_pin_e pin);
 bool isValidCanRxPin(brain_pin_e pin);
 CANDriver* detectCanDevice(brain_pin_e pinRx, brain_pin_e pinTx);
 void canHwInfo(CANDriver* cand);
+void canHwRecover(const size_t busIndex, CANDriver *cand);
 #endif // HAL_USE_CAN
 
 // Serial

firmware/hw_layer/ports/stm32/stm32_can.cpp

@@ -417,4 +417,28 @@ void canHwInfo(CANDriver* cand) {
 #endif
 }
 
+void canHwRecover(const size_t busIndex, CANDriver *cand)
+{
+#if STM32_CAN_USE_FDCAN1 || STM32_CAN_USE_FDCAN2 || STM32_CAN_USE_FDCAN3
+	// equal to TIMEOUT_INIT_MS
+	#define TIMEOUT_RECOVER_MS	250U
+
+	// mostly copy-paste of static bool fdcan_active_mode(CANDriver *canp) from ChibiOS hal_can_lld.c
+	if (cand->fdcan->CCCR & FDCAN_CCCR_INIT) {
+		systime_t start, end;
+
+		/* Going in active mode then waiting for it to happen.*/
+		cand->fdcan->CCCR &= ~FDCAN_CCCR_INIT;
+		start = osalOsGetSystemTimeX();
+		end = osalTimeAddX(start, TIME_MS2I(TIMEOUT_RECOVER_MS));
+		while ((cand->fdcan->CCCR & FDCAN_CCCR_INIT) != 0U) {
+			if (!osalTimeIsInRangeX(osalOsGetSystemTimeX(), start, end)) {
+				return;
+			}
+			osalThreadSleep(1);
+		}
+	}
+#endif
+}
+
 #endif /* EFI_CAN_SUPPORT */