diff --git a/stm-firmware/main.c b/stm-firmware/main.c
index 4d8a0b2..661d1a2 100644
--- a/stm-firmware/main.c
+++ b/stm-firmware/main.c
@@ -152,6 +152,7 @@ int main()
 	static struct pid_controller pid;
 	uint64_t pid_timestamp = 0ULL;
 	uint64_t display_timestamp = 0ULL;
+	enum lcd_fsm_ret lcd_ret;
 
 	setup_nvic_priorities();
 	systick_setup();
@@ -194,13 +195,15 @@ int main()
 
 		pt1000_value_status = adc_pt1000_get_current_resistance(&pt1000_value);
 
+		//snprintf(&disp[2][0], 21, "Foobar");
+
 		if (pt1000_value_status >= 0) {
 			(void)temp_converter_convert_resistance_to_temp(pt1000_value, (float *)&current_temperature);
 			if (systick_ticks_have_passed(pid_timestamp, 250)) {
 
 				pid_out = pid_sample(&pid, 100.0 - current_temperature);
 				pid_timestamp = systick_get_global_tick();
-				snprintf(&disp[2][0], 21, "Temp: %.1f C", current_temperature);
+				//snprintf(&disp[2][0], 21, "Temp: %.1f C", current_temperature);
 				led_set(1, !led_get(1));
 			}
 		}
@@ -221,11 +224,16 @@ int main()
 		strcpy(&disp[1][0], "Line 2");
 		strcpy(&disp[3][0], "Shimatta Reflow");
 
-		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: