diff --git a/stm-firmware/main.c b/stm-firmware/main.c
index fffdb84..334308f 100644
--- a/stm-firmware/main.c
+++ b/stm-firmware/main.c
@@ -155,7 +155,6 @@ const char *oven_controller_hello_world = "Hello world :)\n";
 
 static inline void setup_system()
 {
-
 	setup_nvic_priorities();
 	systick_setup();
 
@@ -186,6 +185,7 @@ int main()
 	int32_t pid_controller_output;
 	uint64_t display_timestamp = 0ULL;
 	char disp[4][21] = {0};
+	enum lcd_fsm_ret lcd_ret = LCD_FSM_NOP;
 
 	target_temperature = 25.0f;
 
@@ -209,7 +209,6 @@ int main()
 
 		pt1000_value_status = adc_pt1000_get_current_resistance(&pt1000_value);
 
-
 		if (systick_ticks_have_passed(pid_timestamp, 250)) {
 			pid_timestamp = systick_get_global_tick();
 			if (pt1000_value_status >= 0 && pid_controller_active)
@@ -242,11 +241,16 @@ int main()
 		if (uart_receive_status >= 1)
 			shell_handle_input(shell_handle, uart_input, uart_input_len);
 
-		if (systick_ticks_have_passed(display_timestamp, 1)) {
+		if (systick_ticks_have_passed(display_timestamp, 1) || lcd_ret == LCD_FSM_CALL_AGAIN) {
+			lcd_ret = lcd_fsm_write_buffer(disp);
 			display_timestamp = systick_get_global_tick();
-			lcd_fsm_write_buffer(disp);
 		}
-		__WFI();
+
+		if (lcd_ret == LCD_FSM_CALL_AGAIN) {
+			/* Nothing */
+		} else {
+			__WFI();
+		}
 	}
 
 }
diff --git a/stm-firmware/ui/lcd.c b/stm-firmware/ui/lcd.c
index bbacbd1..edd82b4 100644
--- a/stm-firmware/ui/lcd.c
+++ b/stm-firmware/ui/lcd.c
@@ -405,18 +405,18 @@ enum lcd_fsm_ret lcd_fsm_write_buffer(const char (*display_buffer)[21])
 		case 5:
 			lcd_fsm_write_data(true, (char_cnt >= line_len) ? ' ' : shadow_display[row_cnt][char_cnt]);
 			lcd_fsm_enable(true);
-			ret = LCD_FSM_WAIT_CALL;
+			ret = LCD_FSM_CALL_AGAIN;
 			state_cnt++;
 			break;
 		case 6:
 			lcd_fsm_enable(false);
-			ret = LCD_FSM_WAIT_CALL;
+			ret = LCD_FSM_CALL_AGAIN;
 			state_cnt++;
 			break;
 		case 7:
 			lcd_fsm_write_data(false, (char_cnt >= line_len) ? ' ' : shadow_display[row_cnt][char_cnt]);
 			lcd_fsm_enable(true);
-			ret = LCD_FSM_WAIT_CALL;
+			ret = LCD_FSM_CALL_AGAIN;
 			state_cnt++;
 			break;
 		case 8: