diff --git a/Firmware/fsensor.cpp b/Firmware/fsensor.cpp
index 2753ede0..7908c5db 100755
--- a/Firmware/fsensor.cpp
+++ b/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)
diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index 5ad3f9bb..c16d39e7 100755
--- a/Firmware/temperature.cpp
+++ b/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()