diff --git a/clock.c b/clock.c
index 3d47f16..b01b58f 100644
--- a/clock.c
+++ b/clock.c
@@ -87,12 +87,25 @@ static void clock_250ms(void) {
   temp_heater_tick();
 
 	ifclock(clock_flag_1s) {
+    static uint8_t wait_for_temp = 0;
+
     #ifdef DISPLAY
       display_clock();
     #endif
 
     temp_residency_tick();
 
+    if (temp_waiting()) {
+      serial_writestr_P(PSTR("Waiting for target temp\n"));
+      wait_for_temp = 1;
+    }
+    else {
+      if (wait_for_temp) {
+        serial_writestr_P(PSTR("Temp achieved\n"));
+        wait_for_temp = 0;
+      }
+    }
+
 		if (DEBUG_POSITION && (debug_flags & DEBUG_POSITION)) {
 			// current position
 			update_current_position();
diff --git a/gcode_process.c b/gcode_process.c
index aaa495b..5c185ca 100644
--- a/gcode_process.c
+++ b/gcode_process.c
@@ -108,6 +108,7 @@ void process_gcode_command() {
 				//?
 				//? In this case move rapidly to X = 12 mm.  In fact, the RepRap firmware uses exactly the same code for rapid as it uses for controlled moves (see G1 below), as - for the RepRap machine - this is just as efficient as not doing so.  (The distinction comes from some old machine tools that used to move faster if the axes were not driven in a straight line.  For them G0 allowed any movement in space to get to the destination as fast as possible.)
 				//?
+        temp_wait();
 				backup_f = next_target.target.F;
 				next_target.target.F = MAXIMUM_FEEDRATE_X * 2L;
 				enqueue(&next_target.target);
@@ -121,6 +122,7 @@ void process_gcode_command() {
 				//?
 				//? Go in a straight line from the current (X, Y) point to the point (90.6, 13.8), extruding material as the move happens from the current extruded length to a length of 22.4 mm.
 				//?
+        temp_wait();
 				enqueue(&next_target.target);
 				break;
 
@@ -445,9 +447,7 @@ void process_gcode_command() {
 				//? --- M101: extruder on ---
 				//?
 				//? Undocumented.
-				if (temp_achieved() == 0) {
-					enqueue(NULL);
-				}
+        temp_wait();
 				#ifdef DC_EXTRUDER
 					heater_set(DC_EXTRUDER, DC_EXTRUDER_PWM);
 				#endif
@@ -648,8 +648,7 @@ void process_gcode_command() {
 				//? Example: M116
 				//?
 				//? Wait for temperatures and other slowly-changing variables to arrive at their set values.
-
-				enqueue(NULL);
+        temp_set_wait();
 				break;
 
       case 119:
diff --git a/temp.c b/temp.c
index f957a70..1784c91 100644
--- a/temp.c
+++ b/temp.c
@@ -12,6 +12,7 @@
 #include	<stdlib.h>
 #include	"arduino.h"
 #include "serial.h"
+#include "clock.h"
 #include	"debug.h"
 #ifndef	EXTRUDER
 	#include	"sersendf.h"
@@ -101,6 +102,12 @@ static struct {
 #define TEMP_READ_CONTINUOUS (EWMA_ALPHA < EWMA_SCALE)
 #define TEMP_NOT_READY       0xffff
 
+/**
+  Flag for wether we currently wait for achieving temperatures.
+*/
+static uint8_t wait_for_temp = 0;
+
+
 /// Set up temp sensors.
 void temp_init() {
 	temp_sensor_t i;
@@ -550,6 +557,33 @@ uint8_t	temp_achieved() {
 	return all_ok;
 }
 
+/**
+  Note that we're waiting for temperatures to achieve their target. Typically
+  set by M116.
+*/
+void temp_set_wait() {
+  wait_for_temp = 1;
+}
+
+/**
+  Wether we're currently waiting for achieving temperatures.
+*/
+uint8_t temp_waiting(void) {
+  if (temp_achieved()) {
+    wait_for_temp = 0;
+  }
+  return wait_for_temp;
+}
+
+/**
+  Wait until all temperatures are achieved.
+*/
+void temp_wait(void) {
+  while (wait_for_temp && ! temp_achieved()) {
+    clock();
+  }
+}
+
 /// specify a target temperature
 /// \param index sensor to set a target for
 /// \param temperature target temperature to aim for
diff --git a/temp.h b/temp.h
index 5e252bd..f5fec46 100644
--- a/temp.h
+++ b/temp.h
@@ -42,6 +42,10 @@ void temp_residency_tick(void);
 
 uint8_t	temp_achieved(void);
 
+void temp_set_wait(void);
+uint8_t temp_waiting(void);
+void temp_wait(void);
+
 void temp_set(temp_sensor_t index, uint16_t temperature);
 uint16_t temp_get(temp_sensor_t index);