Fix stack smashing in temperature/fsensor ISR

The temperature and fsensor ISR re-enable interrupts while executing.

However, we still need to protect the epilogue of the ISR so that
the saved return address is not altered while returning.

We hoist the body of the function out of the isr in both cases for
clarity (and to avoid a stray return bypassing the lock/cli), so that
the re-entrant portion is clearly indicated.

This should fix the "STATIC MEMORY OVERWRITTEN" error messages randomly
happening when stepping at high frequency (where either isr is
preempted more frequently).

Firmware/fsensor.cpp

@@ -478,22 +478,8 @@ bool fsensor_oq_result(void)
 }
 #endif //FSENSOR_QUALITY
 
-ISR(FSENSOR_INT_PIN_VECT)
+FORCE_INLINE static void fsensor_isr(int st_cnt)
 {
-	if (mmu_enabled || ir_sensor_detected) return;
-	if (!((fsensor_int_pin_old ^ FSENSOR_INT_PIN_PIN_REG) & FSENSOR_INT_PIN_MASK)) return;
-	fsensor_int_pin_old = FSENSOR_INT_PIN_PIN_REG;
-
-    // prevent isr re-entry
-	static bool _lock = false;
-	if (_lock) return;
-	_lock = true;
-
-    // fetch fsensor_st_cnt atomically
-	int st_cnt = fsensor_st_cnt;
-	fsensor_st_cnt = 0;
-	sei();
-
 	uint8_t old_err_cnt = fsensor_err_cnt;
 	uint8_t pat9125_res = fsensor_oq_meassure?pat9125_update():pat9125_update_y();
 	if (!pat9125_res)
@@ -578,8 +564,28 @@ ISR(FSENSOR_INT_PIN_VECT)
 #endif //DEBUG_FSENSOR_LOG
 
 	pat9125_y = 0;
-	_lock = false;
-	return;
+}
+
+ISR(FSENSOR_INT_PIN_VECT)
+{
+    if (mmu_enabled || ir_sensor_detected) return;
+    if (!((fsensor_int_pin_old ^ FSENSOR_INT_PIN_PIN_REG) & FSENSOR_INT_PIN_MASK)) return;
+    fsensor_int_pin_old = FSENSOR_INT_PIN_PIN_REG;
+
+    // prevent isr re-entry
+    static bool _lock = false;
+    if (!_lock)
+    {
+        // fetch fsensor_st_cnt atomically
+        int st_cnt = fsensor_st_cnt;
+        fsensor_st_cnt = 0;
+
+        _lock = true;
+        sei();
+        fsensor_isr(st_cnt);
+        cli();
+        _lock = false;
+    }
 }
 
 void fsensor_setup_interrupt(void)

Firmware/temperature.cpp

@@ -1696,18 +1696,8 @@ void adc_ready(void) //callback from adc when sampling finished
 
 } // extern "C"
 
-// Timer2 (originaly timer0) is shared with millies
-#ifdef SYSTEM_TIMER_2
-ISR(TIMER2_COMPB_vect)
-#else //SYSTEM_TIMER_2
-ISR(TIMER0_COMPB_vect)
-#endif //SYSTEM_TIMER_2
+FORCE_INLINE static void temperature_isr()
 {
-	static bool _lock = false;
-	if (_lock) return;
-	_lock = true;
-	asm("sei");
-
 	if (!temp_meas_ready) adc_cycle();
 	lcd_buttons_update();
 
@@ -2073,8 +2063,24 @@ ISR(TIMER0_COMPB_vect)
 #if (defined(FANCHECK) && defined(TACH_0) && (TACH_0 > -1))
   check_fans();
 #endif //(defined(TACH_0))
+}
 
-	_lock = false;
+// Timer2 (originaly timer0) is shared with millies
+#ifdef SYSTEM_TIMER_2
+ISR(TIMER2_COMPB_vect)
+#else //SYSTEM_TIMER_2
+ISR(TIMER0_COMPB_vect)
+#endif //SYSTEM_TIMER_2
+{
+    static bool _lock = false;
+    if (!_lock)
+    {
+        _lock = true;
+        sei();
+        temperature_isr();
+        cli();
+        _lock = false;
+    }
 }
 
 void check_max_temp()