From 59463a515e2b73e2f7c929216d941b5a788e7760 Mon Sep 17 00:00:00 2001
From: rusefi <rusefillc@gmail.com>
Date: Sat, 10 Aug 2019 00:53:16 -0400
Subject: [PATCH] MRE hangs up on bench fix #899

---
 firmware/config/stm32f7ems/chconf.h      |  1 +
 firmware/hw_layer/drivers/gpio/tle8888.c | 24 +++++++++++++++++++-----
 2 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/firmware/config/stm32f7ems/chconf.h b/firmware/config/stm32f7ems/chconf.h
index 9b82ba0c7f..ce8d6caf5c 100644
--- a/firmware/config/stm32f7ems/chconf.h
+++ b/firmware/config/stm32f7ems/chconf.h
@@ -688,6 +688,7 @@
  */
 #define CH_CFG_SYSTEM_HALT_HOOK(reason) {                                   \
   /* System halt code here.*/                                               \
+  chDbgPanic3(reason, __FILE__, __LINE__);                                  \
 }
 
 /**
diff --git a/firmware/hw_layer/drivers/gpio/tle8888.c b/firmware/hw_layer/drivers/gpio/tle8888.c
index 9f29874738..30ffcb78d0 100644
--- a/firmware/hw_layer/drivers/gpio/tle8888.c
+++ b/firmware/hw_layer/drivers/gpio/tle8888.c
@@ -240,6 +240,20 @@ static int tle8888_update_direct_output(struct tle8888_priv *chip, int pin, int
 	return -1;
 }
 
+// ChibiOS does not offer this function so that's a copy-paste of 'chSemSignal' without locking
+void chSemSignalS(semaphore_t *sp) {
+
+  chDbgCheck(sp != NULL);
+
+  chDbgAssert(((sp->cnt >= (cnt_t)0) && queue_isempty(&sp->queue)) ||
+              ((sp->cnt < (cnt_t)0) && queue_notempty(&sp->queue)),
+              "inconsistent semaphore");
+  if (++sp->cnt <= (cnt_t)0) {
+    chSchWakeupS(queue_fifo_remove(&sp->queue), MSG_OK);
+  }
+}
+
+
 /**
  * @brief TLE8888 chip driver wakeup.
  * @details Wake up driver. Will cause output register update
@@ -249,16 +263,16 @@ static int tle8888_wake_driver(struct tle8888_priv *chip)
 {
 	(void)chip;
 
+	int wasLocked = lockAnyContext();
 	if (isIsrContext()) {
 		// this is for normal runtime
-		int wasLocked = lockAnyContext();
 		chSemSignalI(&tle8888_wake);
-		if (!wasLocked) {
-			unlockAnyContext();
-		}
 	} else {
 		// this is for start-up to not hang up
-		chSemSignal(&tle8888_wake);
+		chSemSignalS(&tle8888_wake);
+	}
+	if (!wasLocked) {
+		unlockAnyContext();
 	}
 
 	return 0;