From f1096c91f322934484e1d95eeb7a87bcc4a40ce9 Mon Sep 17 00:00:00 2001
From: Andrei <andreika-git@users.noreply.github.com>
Date: Thu, 25 May 2017 21:51:21 +0300
Subject: [PATCH] Fix lockAnyContext

---
 firmware/controllers/map_averaging.cpp                |  9 +++++----
 firmware/controllers/system/SingleTimerExecutor.cpp   | 11 +++++++----
 firmware/controllers/system/signal_executor_sleep.cpp |  5 +++--
 3 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/firmware/controllers/map_averaging.cpp b/firmware/controllers/map_averaging.cpp
index eb3c9a2991..9100683a13 100644
--- a/firmware/controllers/map_averaging.cpp
+++ b/firmware/controllers/map_averaging.cpp
@@ -99,7 +99,7 @@ static void startAveraging(void *arg) {
 	mapMeasurementsCounter = 0;
 	isAveraging = true;
 	if (!wasLocked)
-		chSysUnlockFromISR()
+		unlockAnyContext();
 	;
 	mapAveragingPin.setHigh();
 }
@@ -144,13 +144,14 @@ void mapAveragingCallback(adcsample_t adcValue) {
 	readIndex = writeIndex;
 
 	// todo: migrate to the lock-free implementation
-	chSysLockFromISR()
+	bool alreadyLocked = lockAnyContext();
 	;
 	// with locking we would have a consistent state
 
 	mapAccumulator += adcValue;
 	mapMeasurementsCounter++;
-	chSysUnlockFromISR()
+	if (!alreadyLocked)
+		unlockAnyContext();
 	;
 }
 #endif
@@ -165,7 +166,7 @@ static void endAveraging(void *arg) {
 			mapAccumulator / mapMeasurementsCounter);
 #endif
 	if (!wasLocked)
-		chSysUnlockFromISR()
+		unlockAnyContext();
 	;
 	mapAveragingPin.setLow();
 }
diff --git a/firmware/controllers/system/SingleTimerExecutor.cpp b/firmware/controllers/system/SingleTimerExecutor.cpp
index def902afdd..683e91c473 100644
--- a/firmware/controllers/system/SingleTimerExecutor.cpp
+++ b/firmware/controllers/system/SingleTimerExecutor.cpp
@@ -68,9 +68,10 @@ void Executor::scheduleByTime(scheduling_s *scheduling, efitimeus_t timeUs, schf
 //		callback(param);
 //		return;
 //	}
+	bool alreadyLocked = true;
 	if (!reentrantFlag) {
 		// this would guard the queue and disable interrupts
-		lockAnyContext();
+		alreadyLocked = lockAnyContext();
 	}
 	bool needToResetTimer = queue.insertTask(scheduling, US2NT(timeUs), callback, param);
 	if (!reentrantFlag) {
@@ -78,15 +79,17 @@ void Executor::scheduleByTime(scheduling_s *scheduling, efitimeus_t timeUs, schf
 		if (needToResetTimer) {
 			scheduleTimerCallback();
 		}
-		unlockAnyContext();
+		if (!alreadyLocked)
+			unlockAnyContext();
 	}
 }
 
 void Executor::onTimerCallback() {
-	lockAnyContext();
+	bool alreadyLocked = lockAnyContext();
 	doExecute();
 	scheduleTimerCallback();
-	unlockAnyContext();
+	if (!alreadyLocked)
+		unlockAnyContext();
 }
 
 /*
diff --git a/firmware/controllers/system/signal_executor_sleep.cpp b/firmware/controllers/system/signal_executor_sleep.cpp
index 9f8291b55e..19ce84ac9f 100644
--- a/firmware/controllers/system/signal_executor_sleep.cpp
+++ b/firmware/controllers/system/signal_executor_sleep.cpp
@@ -62,7 +62,7 @@ void scheduleTask(scheduling_s *scheduling, int delayUs, schfunc_t callback, voi
 		return;
 	}
 
-	lockAnyContext();
+	bool alreadyLocked = lockAnyContext();
 	scheduling->callback = callback;
 	scheduling->param = param;
 	int isArmed = chVTIsArmedI(&scheduling->timer);
@@ -82,7 +82,8 @@ void scheduleTask(scheduling_s *scheduling, int delayUs, schfunc_t callback, voi
 #endif /* EFI_SIMULATOR */
 
 	chVTSetI(&scheduling->timer, delaySt, (vtfunc_t)timerCallback, scheduling);
-	unlockAnyContext();
+	if (!alreadyLocked)
+		unlockAnyContext();
 }
 
 void initSignalExecutorImpl(void) {