trigger/scheduler perf improvements (#1885)

* avoid 64b on hot path

* no tooth log when fast

* use crit section locker

* final

* only flip pins if not also self stim

* only trace if doing work

* slightly drop lateDelay

* trace if not bailing out

firmware/console/binary/tooth_logger.cpp

@@ -98,12 +98,18 @@ static void SetNextCompositeEntry(efitick_t timestamp, bool trigger1, bool trigg
 }
 
 void LogTriggerTooth(trigger_event_e tooth, efitick_t timestamp DECLARE_ENGINE_PARAMETER_SUFFIX) {
-	ScopePerf perf(PE::LogTriggerTooth);
 	// bail if we aren't enabled
 	if (!ToothLoggerEnabled) {
 		return;
 	}
 
+	// Don't log at significant engine speed
+	if (engine->rpmCalculator.getRpm() > 4000) {
+		return;
+	}
+
+	ScopePerf perf(PE::LogTriggerTooth);
+
 /*
 		// We currently only support the primary trigger falling edge
     	// (this is the edge that VR sensors are accurate on)

firmware/controllers/engine_cycle/map_averaging.cpp

@@ -269,9 +269,6 @@ void refreshMapAveragingPreCalc(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
  */
 void mapAveragingTriggerCallback(
 		uint32_t index, efitick_t edgeTimestamp DECLARE_ENGINE_PARAMETER_SUFFIX) {
-
-	ScopePerf perf(PE::MapAveragingTriggerCallback);
-	
 #if EFI_ENGINE_CONTROL
 	// this callback is invoked on interrupt thread
 	if (index != (uint32_t)CONFIG(mapAveragingSchedulingAtIndex))
@@ -282,6 +279,8 @@ void mapAveragingTriggerCallback(
 		return;
 	}
 
+	ScopePerf perf(PE::MapAveragingTriggerCallback);
+
 	if (CONFIG(mapMinBufferLength) != mapMinBufferLength) {
 		applyMapMinBufferLength(PASS_ENGINE_PARAMETER_SIGNATURE);
 	}

firmware/controllers/system/timer/single_timer_executor.cpp

@@ -44,8 +44,8 @@ void globalTimerCallback() {
 }
 
 SingleTimerExecutor::SingleTimerExecutor()
-	// 10us is roughly double the cost of the interrupt + overhead of a single timer event
-	: queue(US2NT(10))
+	// 8us is roughly the cost of the interrupt + overhead of a single timer event
+	: queue(US2NT(8))
 {
 }
 
@@ -71,39 +71,36 @@ void SingleTimerExecutor::scheduleByTimestampNt(scheduling_s* scheduling, efitim
 	ScopePerf perf(PE::SingleTimerExecutorScheduleByTimestamp);
 
 #if EFI_ENABLE_ASSERTS
-	int deltaTimeUs = NT2US(nt - getTimeNowNt());
+	int32_t deltaTimeNt = (int32_t)nt - getTimeNowLowerNt();
 
-	if (deltaTimeUs >= TOO_FAR_INTO_FUTURE_US) {
+	if (deltaTimeNt >= TOO_FAR_INTO_FUTURE_NT) {
 		// we are trying to set callback for too far into the future. This does not look right at all
-		firmwareError(CUSTOM_ERR_TASK_TIMER_OVERFLOW, "scheduleByTimestampNt() too far: %d", deltaTimeUs);
+		firmwareError(CUSTOM_ERR_TASK_TIMER_OVERFLOW, "scheduleByTimestampNt() too far: %d", deltaTimeNt);
 		return;
 	}
 #endif
 
 	scheduleCounter++;
-	bool alreadyLocked = true;
-	if (!reentrantFlag) {
-		// this would guard the queue and disable interrupts
-		alreadyLocked = lockAnyContext();
-	}
+
+	// Lock for queue insertion - we may already be locked, but that's ok
+	chibios_rt::CriticalSectionLocker csl;
+
 	bool needToResetTimer = queue.insertTask(scheduling, nt, action);
 	if (!reentrantFlag) {
 		executeAllPendingActions();
 		if (needToResetTimer) {
 			scheduleTimerCallback();
 		}
-		if (!alreadyLocked)
-			unlockAnyContext();
 	}
 }
 
 void SingleTimerExecutor::onTimerCallback() {
 	timerCallbackCounter++;
-	bool alreadyLocked = lockAnyContext();
+
+	chibios_rt::CriticalSectionLocker csl;
+
 	executeAllPendingActions();
 	scheduleTimerCallback();
-	if (!alreadyLocked)
-		unlockAnyContext();
 }
 
 /*
@@ -119,7 +116,7 @@ void SingleTimerExecutor::executeAllPendingActions() {
 	 * further invocations
 	 */
 	reentrantFlag = true;
-	int shouldExecute = 1;
+
 	/**
 	 * in real life it could be that while we executing listeners time passes and it's already time to execute
 	 * next listeners.

firmware/controllers/system/timer/single_timer_executor.h

@@ -10,7 +10,7 @@
 #include "scheduler.h"
 #include "event_queue.h"
 
-class SingleTimerExecutor : public ExecutorInterface {
+class SingleTimerExecutor final : public ExecutorInterface {
 public:
 	SingleTimerExecutor();
 	void scheduleByTimestamp(scheduling_s *scheduling, efitimeus_t timeUs, action_s action) override;

firmware/controllers/trigger/trigger_emulator_algo.cpp

@@ -150,7 +150,7 @@ static void emulatorApplyPinState(int stateIndex, PwmConfig *state) /* pwm_gen_c
 
 #if EFI_PROD_CODE
 	// Only set pins if they're configured - no need to waste the cycles otherwise
-	if (hasStimPins) {
+	else if (hasStimPins) {
 		applyPinState(stateIndex, state);
 	}
 #endif /* EFI_PROD_CODE */

firmware/hw_layer/microsecond_timer.h

@@ -15,7 +15,8 @@ extern "C"
 void initMicrosecondTimer(void);
 void setHardwareUsTimer(int32_t deltaTimeUs);
 
-#define TOO_FAR_INTO_FUTURE_US 10 * US_PER_SECOND
+#define TOO_FAR_INTO_FUTURE_US (10 * US_PER_SECOND)
+#define TOO_FAR_INTO_FUTURE_NT US2NT(TOO_FAR_INTO_FUTURE_US)
 
 #ifdef __cplusplus
 }