Version 1.17 dev 4

Support concurrenrt execution of commands form different channels
M143 now supports individual temperature limits per heater
Fixed issue with dely of up to 2 seconds when switching Duet WiFi heater
channel 3 to servo mode
When enable polarity is set using the M569 R parameter, disable the
corresponding drive
Add exdperimental S99 option on G1 command to log probe changes (Duet
WiFi only)

25 files changed, 1801 insertions, 1409 deletions

diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index e47fc4b9..4530d927 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.internal.build.crossgcc.CrossGCCBuiltinSpecsDetector" console="false" env-hash="337623625225680352" id="org.eclipse.cdt.build.crossgcc.CrossGCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.internal.build.crossgcc.CrossGCCBuiltinSpecsDetector" console="false" env-hash="-303428918971435778" id="org.eclipse.cdt.build.crossgcc.CrossGCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.internal.build.crossgcc.CrossGCCBuiltinSpecsDetector" console="false" env-hash="337623625225680352" id="org.eclipse.cdt.build.crossgcc.CrossGCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.internal.build.crossgcc.CrossGCCBuiltinSpecsDetector" console="false" env-hash="-303428918971435778" id="org.eclipse.cdt.build.crossgcc.CrossGCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -27,7 +27,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.internal.build.crossgcc.CrossGCCBuiltinSpecsDetector" console="false" env-hash="337623625225680352" id="org.eclipse.cdt.build.crossgcc.CrossGCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.internal.build.crossgcc.CrossGCCBuiltinSpecsDetector" console="false" env-hash="-303428918971435778" id="org.eclipse.cdt.build.crossgcc.CrossGCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/src/Configuration.h b/src/Configuration.h
index 4392bdd9..f3947989 100644
--- a/src/Configuration.h
+++ b/src/Configuration.h
@@ -23,14 +23,16 @@ Licence: GPL
 #ifndef CONFIGURATION_H
 #define CONFIGURATION_H
 
+#include <cstddef>			// for size_t
+
 // Firmware name is now defined in the Pins file
 
 #ifndef VERSION
-# define VERSION "1.17dev1"
+# define VERSION "1.17dev4"
 #endif
 
 #ifndef DATE
-# define DATE "2016-11-12"
+# define DATE "2016-11-17"
 #endif
 
 #define AUTHORS "reprappro, dc42, zpl, t3p3, dnewman"
@@ -82,7 +84,8 @@ const float HOT_ENOUGH_TO_RETRACT = 90.0;			// Celsius
 
 const uint8_t MAX_BAD_TEMPERATURE_COUNT = 4;		// Number of bad temperature samples permitted before a heater fault is reported
 const float BAD_LOW_TEMPERATURE = -10.0;			// Celsius
-const float DEFAULT_TEMPERATURE_LIMIT = 262.0;		// Celsius
+const float DefaultExtruderTemperatureLimit = 262.0; // Celsius
+const float DefaultBedTemperatureLimit = 125.0;		// Celsius
 const float HOT_END_FAN_TEMPERATURE = 45.0;			// Temperature at which a thermostatic hot end fan comes on
 const float BAD_ERROR_TEMPERATURE = 2000.0;			// Must exceed any reasonable 5temperature limit including DEFAULT_TEMPERATURE_LIMIT
 
@@ -179,6 +182,8 @@ const unsigned int MaxTriggers = 10;				// Must be <= 32 because we store a bitm
 const float NOZZLE_DIAMETER = 0.5;					// Millimetres
 const float FILAMENT_WIDTH = 1.75;					// Millimetres
 
+const unsigned int MaxStackDepth = 5;				// Maximum depth of stack
+
 // Webserver stuff
 
 #define DEFAULT_PASSWORD "reprap"					// Default machine password
diff --git a/src/Duet/Pins_Duet.h b/src/Duet/Pins_Duet.h
index c097a01a..72d292d8 100644
--- a/src/Duet/Pins_Duet.h
+++ b/src/Duet/Pins_Duet.h
@@ -71,7 +71,9 @@ const float EXT_BETA = 4388.0;
 const float EXT_SHC = 0.0;
 
 // Thermistor series resistor value in Ohms
-const float THERMISTOR_SERIES_RS = 4700.0;
+// On later Duet 0.6 and all Duet 0.8.5 boards it is 4700 ohms. However, if we change the default then machines that have 1K series resistors
+// and don't have R1000 in the M305 commands in config.g will overheat. So for safety we leave the default as 1000.
+const float THERMISTOR_SERIES_RS = 1000.0;
 
 // Number of SPI temperature sensors to support
 
diff --git a/src/DuetNG/DueXn.cpp b/src/DuetNG/DueXn.cpp
index c89d50b0..f8678409 100644
--- a/src/DuetNG/DueXn.cpp
+++ b/src/DuetNG/DueXn.cpp
@@ -70,7 +70,7 @@ namespace DuetExpansion
 			pinMode(DueX_INT, INPUT_PULLUP);
 			pinMode(DueX_SG, INPUT_PULLUP);
 
-			expander.pinModeMultiple(AllFanBits, OUTPUT_PWM);			// Initialise the PWM pins
+			expander.pinModeMultiple(AllFanBits, OUTPUT_PWM_LOW);		// Initialise the PWM pins
 			const uint16_t stopBits = (boardType == ExpansionBoardType::DueX5) ? AllStopBitsX5 : AllStopBitsX2;	// I am assuming that the X0 has 2 endstop inputs
 			expander.pinModeMultiple(stopBits | AllGpioBits, INPUT);	// Initialise the endstop inputs and GPIO pins (no pullups because 5V-tolerant)
 
@@ -118,7 +118,8 @@ namespace DuetExpansion
 				case OUTPUT_HIGH:
 					mode = OUTPUT_HIGH_OPEN_DRAIN;
 					break;
-				case OUTPUT_PWM:
+				case OUTPUT_PWM_LOW:
+				case OUTPUT_PWM_HIGH:
 					mode = OUTPUT_PWM_OPEN_DRAIN;
 					break;
 				case INPUT_PULLUP:
diff --git a/src/DuetNG/SX1509.cpp b/src/DuetNG/SX1509.cpp
index 98035cca..d2e628bf 100644
--- a/src/DuetNG/SX1509.cpp
+++ b/src/DuetNG/SX1509.cpp
@@ -146,7 +146,8 @@ void SX1509::pinModeMultiple(uint16_t pins, PinMode inOut)
 		clearBitsInWord(REG_DIR_B, pins);
 		break;
 
-	case OUTPUT_PWM:
+	case OUTPUT_PWM_LOW:
+	case OUTPUT_PWM_HIGH:
 		ledDriverInitMultiple(pins, false, false);
 		break;
 
diff --git a/src/DuetNG/WifiFirmwareUploader.cpp b/src/DuetNG/WifiFirmwareUploader.cpp
index ad74a523..070c86f2 100644
--- a/src/DuetNG/WifiFirmwareUploader.cpp
+++ b/src/DuetNG/WifiFirmwareUploader.cpp
@@ -713,7 +713,7 @@ void WifiFirmwareUploader::Spin()
 // Try to upload the given file at the given address
 void WifiFirmwareUploader::SendUpdateFile(const char *file, const char *dir, uint32_t address)
 {
-	Platform *platform = reprap.GetPlatform();
+	Platform * const platform = reprap.GetPlatform();
 	uploadFile = platform->GetFileStore(dir, file, false);
 	if (uploadFile == nullptr)
 	{
diff --git a/src/GCodes/GCodeBuffer.cpp b/src/GCodes/GCodeBuffer.cpp
index 5938ba41..8fa81bac 100644
--- a/src/GCodes/GCodeBuffer.cpp
+++ b/src/GCodes/GCodeBuffer.cpp
@@ -9,10 +9,10 @@
 
 #include "RepRapFirmware.h"
 
-// This class stores a single G Code and provides functions to allow it to be parsed
-
-GCodeBuffer::GCodeBuffer(Platform* p, const char* id, MessageType mt)
-	: platform(p), identity(id), checksumRequired(false), writingFileDirectory(nullptr), toolNumberAdjust(0), responseMessageType(mt)
+// Create a default GCodeBuffer
+GCodeBuffer::GCodeBuffer(const char* id, MessageType mt)
+	: machineState(new GCodeMachineState()), identity(id), checksumRequired(false), writingFileDirectory(nullptr),
+	  toolNumberAdjust(0), responseMessageType(mt)
 {
 	Init();
 }
@@ -22,23 +22,23 @@ void GCodeBuffer::Init()
 	gcodePointer = 0;
 	readPointer = -1;
 	inComment = false;
-	state = GCodeState::idle;
+	bufferState = GCodeBufferState::idle;
 }
 
 void GCodeBuffer::Diagnostics(MessageType mtype)
 {
-	switch (state)
+	switch (bufferState)
 	{
-		case GCodeState::idle:
-			platform->MessageF(mtype, "%s is idle\n", identity);
+		case GCodeBufferState::idle:
+			reprap.GetPlatform()->MessageF(mtype, "%s is idle\n", identity);
 			break;
 
-		case GCodeState::ready:
-			platform->MessageF(mtype, "%s is ready with \"%s\"\n", identity, Buffer());
+		case GCodeBufferState::ready:
+			reprap.GetPlatform()->MessageF(mtype, "%s is ready with \"%s\"\n", identity, Buffer());
 			break;
 
-		case GCodeState::executing:
-			platform->MessageF(mtype, "%s is doing \"%s\"\n", identity, Buffer());
+		case GCodeBufferState::executing:
+			reprap.GetPlatform()->MessageF(mtype, "%s is doing \"%s\"\n", identity, Buffer());
 	}
 }
 
@@ -72,7 +72,7 @@ bool GCodeBuffer::Put(char c)
 		gcodeBuffer[gcodePointer] = 0;
 		if (reprap.Debug(moduleGcodes) && gcodeBuffer[0] != 0 && !writingFileDirectory) // Don't bother with blank/comment lines
 		{
-			platform->MessageF(DEBUG_MESSAGE, "%s: %s\n", identity, gcodeBuffer);
+			reprap.GetPlatform()->MessageF(DEBUG_MESSAGE, "%s: %s\n", identity, gcodeBuffer);
 		}
 
 		// Deal with line numbers and checksums
@@ -116,7 +116,7 @@ bool GCodeBuffer::Put(char c)
 			}
 			gcodeBuffer[gp2] = 0;
 		}
-		else if (checksumRequired || IsEmpty())
+		else if ((checksumRequired && machineState->previous == nullptr) || IsEmpty())
 		{
 			// Checksum not found or buffer empty - cannot do anything
 			gcodeBuffer[0] = 0;
@@ -124,7 +124,7 @@ bool GCodeBuffer::Put(char c)
 			return false;
 		}
 		Init();
-		state = GCodeState::ready;
+		bufferState = GCodeBufferState::ready;
 		return true;
 	}
 	else if (!inComment || writingFileDirectory)
@@ -132,7 +132,7 @@ bool GCodeBuffer::Put(char c)
 		gcodeBuffer[gcodePointer++] = c;
 		if (gcodePointer >= (int)GCODE_LENGTH)
 		{
-			platform->Message(GENERIC_MESSAGE, "Error: G-Code buffer length overflow.\n");
+			reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Error: G-Code buffer length overflow.\n");
 			gcodePointer = 0;
 			gcodeBuffer[0] = 0;
 		}
@@ -174,7 +174,7 @@ bool GCodeBuffer::Seen(char c)
 	readPointer = 0;
 	for (;;)
 	{
-		char b = gcodeBuffer[readPointer];
+		const char b = gcodeBuffer[readPointer];
 		if (b == 0 || b == ';') break;
 		if (b == c) return true;
 		++readPointer;
@@ -188,7 +188,7 @@ float GCodeBuffer::GetFValue()
 {
 	if (readPointer < 0)
 	{
-		platform->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode float before a search.\n");
+		reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode float before a search.\n");
 		readPointer = -1;
 		return 0.0;
 	}
@@ -202,7 +202,7 @@ const void GCodeBuffer::GetFloatArray(float a[], size_t& returnedLength, bool do
 {
 	if(readPointer < 0)
 	{
-		platform->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode float array before a search.\n");
+		reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode float array before a search.\n");
 		readPointer = -1;
 		returnedLength = 0;
 		return;
@@ -214,7 +214,7 @@ const void GCodeBuffer::GetFloatArray(float a[], size_t& returnedLength, bool do
 	{
 		if (length >= returnedLength)		// array limit has been set in here
 		{
-			platform->MessageF(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode float array that is too long: %s\n", gcodeBuffer);
+			reprap.GetPlatform()->MessageF(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode float array that is too long: %s\n", gcodeBuffer);
 			readPointer = -1;
 			returnedLength = 0;
 			return;
@@ -253,7 +253,7 @@ const void GCodeBuffer::GetLongArray(long l[], size_t& returnedLength)
 {
 	if (readPointer < 0)
 	{
-		platform->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode long array before a search.\n");
+		reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode long array before a search.\n");
 		readPointer = -1;
 		return;
 	}
@@ -264,7 +264,7 @@ const void GCodeBuffer::GetLongArray(long l[], size_t& returnedLength)
 	{
 		if (length >= returnedLength) // Array limit has been set in here
 		{
-			platform->MessageF(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode long array that is too long: %s\n", gcodeBuffer);
+			reprap.GetPlatform()->MessageF(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode long array that is too long: %s\n", gcodeBuffer);
 			readPointer = -1;
 			returnedLength = 0;
 			return;
@@ -290,11 +290,11 @@ const char* GCodeBuffer::GetString()
 {
 	if (readPointer < 0)
 	{
-		platform->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode string before a search.\n");
+		reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode string before a search.\n");
 		readPointer = -1;
 		return "";
 	}
-	const char* result = &gcodeBuffer[readPointer + 1];
+	const char* const result = &gcodeBuffer[readPointer + 1];
 	readPointer = -1;
 	return result;
 }
@@ -324,12 +324,12 @@ const char* GCodeBuffer::GetUnprecedentedString(bool optional)
 		readPointer = -1;
 		if (!optional)
 		{
-			platform->Message(GENERIC_MESSAGE, "Error: GCodes: String expected but not seen.\n");
+			reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Error: GCodes: String expected but not seen.\n");
 		}
 		return nullptr;
 	}
 
-	const char* result = &gcodeBuffer[readPointer + 1];
+	const char* const result = &gcodeBuffer[readPointer + 1];
 	readPointer = -1;
 	return result;
 }
@@ -339,11 +339,11 @@ int32_t GCodeBuffer::GetIValue()
 {
 	if (readPointer < 0)
 	{
-		platform->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode int before a search.\n");
+		reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Error: GCodes: Attempt to read a GCode int before a search.\n");
 		readPointer = -1;
 		return 0;
 	}
-	int32_t result = strtol(&gcodeBuffer[readPointer + 1], 0, 0);
+	const int32_t result = strtol(&gcodeBuffer[readPointer + 1], 0, 0);
 	readPointer = -1;
 	return result;
 }
@@ -368,27 +368,60 @@ void GCodeBuffer::TryGetIValue(char c, int32_t& val, bool& seen)
 	}
 }
 
-// Return true if this buffer contains a poll request or empty request that can be executed while macros etc. from another source are being completed
-bool GCodeBuffer::IsPollRequest()
+// Get the original machine state before we pushed anything
+GCodeMachineState& GCodeBuffer::OriginalMachineState() const
 {
-	if (state == GCodeState::ready)
-	{	if (IsEmpty())
-		{
-			return true;
-		}
-		if (Seen('M'))
-		{
-			return IsPollCode(GetIValue());
-		}
+	GCodeMachineState *ms = machineState;
+	while (ms->previous != nullptr)
+	{
+		ms = ms->previous;
 	}
-	return false;
+	return *ms;
+}
+
+// Push state returning true if successful (i.e. stack not overflowed)
+bool GCodeBuffer::PushState()
+{
+	// Check the current stack depth
+	unsigned int depth = 0;
+	for (const GCodeMachineState *m1 = machineState; m1 != nullptr; m1 = m1->previous)
+	{
+		++depth;
+	}
+	if (depth >= MaxStackDepth + 1)				// the +1 is to allow for the initial state record
+	{
+		return false;
+	}
+
+	GCodeMachineState * const ms = GCodeMachineState::Allocate();
+	ms->previous = machineState;
+	ms->feedrate = machineState->feedrate;
+	ms->drivesRelative = machineState->drivesRelative;
+	ms->axesRelative = machineState->axesRelative;
+	ms->doingFileMacro = machineState->doingFileMacro;
+	ms->lockedResources = machineState->lockedResources;
+	machineState = ms;
+	return true;
+}
+
+// Pop state returning true if successful (i.e. no stack underrun)
+bool GCodeBuffer::PopState()
+{
+	GCodeMachineState * const ms = machineState;
+	if (ms->previous == nullptr)
+	{
+		return false;
+	}
+
+	machineState = ms->previous;
+	GCodeMachineState::Release(ms);
+	return true;
 }
 
-// Return true if the specified M code can be executed concurrently with other requests.
-// These should be codes that do not modify the state (we make an exception for M111) and are always executed in a single call to function HandleMCode.
-/*static*/ bool GCodeBuffer::IsPollCode(int code)
+// Return true if this source is executing a file macro
+bool GCodeBuffer::IsDoingFileMacro() const
 {
-	return code == 27 || code == 105 || code == 111 || code == 114 || code == 119 || code == 122 || code == 408 || code == 573;
+	return machineState->doingFileMacro;
 }
 
 // End
diff --git a/src/GCodes/GCodeBuffer.h b/src/GCodes/GCodeBuffer.h
index ee357882..0032c081 100644
--- a/src/GCodes/GCodeBuffer.h
+++ b/src/GCodes/GCodeBuffer.h
@@ -8,61 +8,67 @@
 #ifndef GCODEBUFFER_H_
 #define GCODEBUFFER_H_
 
-// Small class to hold an individual GCode and provide functions to allow it to be parsed
+#include "GCodeMachineState.h"
+
+// Class to hold an individual GCode and provide functions to allow it to be parsed
 class GCodeBuffer
 {
-  public:
-    GCodeBuffer(Platform* p, const char* id, MessageType mt);
-    void Init(); 										// Set it up
+public:
+	GCodeBuffer(const char* id, MessageType mt);
+	void Init(); 										// Set it up
 	void Diagnostics(MessageType mtype);				// Write some debug info
-    bool Put(char c);									// Add a character to the end
-    bool Put(const char *str, size_t len);				// Add an entire string
-    bool IsEmpty() const;								// Does this buffer contain any code?
-    bool Seen(char c);									// Is a character present?
-    float GetFValue();									// Get a float after a key letter
-    int32_t GetIValue();								// Get an integer after a key letter
-    void TryGetFValue(char c, float& val, bool& seen);
-    void TryGetIValue(char c, int32_t& val, bool& seen);
-    const char* GetUnprecedentedString(bool optional = false);	// Get a string with no preceding key letter
-    const char* GetString();							// Get a string after a key letter
-    const void GetFloatArray(float a[], size_t& length, bool doPad); // Get a :-separated list of floats after a key letter
-    const void GetLongArray(long l[], size_t& length);	// Get a :-separated list of longs after a key letter
-    const char* Buffer() const;
-    bool IsIdle() const;
-    bool IsReady() const;								// Return true if a gcode is ready but hasn't been started yet
-    bool IsExecuting() const;							// Return true if a gcode has been started and is not paused
-    void SetFinished(bool f);							// Set the G Code executed (or not)
-    const char* WritingFileDirectory() const;			// If we are writing the G Code to a file, where that file is
-    void SetWritingFileDirectory(const char* wfd);		// Set the directory for the file to write the GCode in
-    int GetToolNumberAdjust() const { return toolNumberAdjust; }
-    void SetToolNumberAdjust(int arg) { toolNumberAdjust = arg; }
-    void SetCommsProperties(uint32_t arg) { checksumRequired = (arg & 1); }
-    bool StartingNewCode() const { return gcodePointer == 0; }
-    bool IsPollRequest();
-    MessageType GetResponseMessageType() const { return responseMessageType; }
-
-    static bool IsPollCode(int code);
+	bool Put(char c);									// Add a character to the end
+	bool Put(const char *str, size_t len);				// Add an entire string
+	bool IsEmpty() const;								// Does this buffer contain any code?
+	bool Seen(char c);									// Is a character present?
+	float GetFValue();									// Get a float after a key letter
+	int32_t GetIValue();								// Get an integer after a key letter
+	void TryGetFValue(char c, float& val, bool& seen);
+	void TryGetIValue(char c, int32_t& val, bool& seen);
+	const char* GetUnprecedentedString(bool optional = false);	// Get a string with no preceding key letter
+	const char* GetString();							// Get a string after a key letter
+	const void GetFloatArray(float a[], size_t& length, bool doPad); // Get a :-separated list of floats after a key letter
+	const void GetLongArray(long l[], size_t& length);	// Get a :-separated list of longs after a key letter
+	const char* Buffer() const;
+	bool IsIdle() const;
+	bool IsReady() const;								// Return true if a gcode is ready but hasn't been started yet
+	bool IsExecuting() const;							// Return true if a gcode has been started and is not paused
+	void SetFinished(bool f);							// Set the G Code executed (or not)
+	const char* WritingFileDirectory() const;			// If we are writing the G Code to a file, where that file is
+	void SetWritingFileDirectory(const char* wfd);		// Set the directory for the file to write the GCode in
+	int GetToolNumberAdjust() const { return toolNumberAdjust; }
+	void SetToolNumberAdjust(int arg) { toolNumberAdjust = arg; }
+	void SetCommsProperties(uint32_t arg) { checksumRequired = (arg & 1); }
+	bool StartingNewCode() const { return gcodePointer == 0; }
+	MessageType GetResponseMessageType() const { return responseMessageType; }
+	GCodeMachineState& MachineState() const { return *machineState; }
+	GCodeMachineState& OriginalMachineState() const;
+	bool PushState();									// Push state returning true if successful (i.e. stack not overflowed)
+	bool PopState();									// Pop state returning true if successful (i.e. no stack underrun)
+	bool IsDoingFileMacro() const;						// Return true if this source is executing a file macro
 
-  private:
+private:
 
-    enum class GCodeState
+	enum class GCodeBufferState
 	{
-    	idle,			// we don't have a complete gcode ready
+		idle,			// we don't have a complete gcode ready
 		ready,			// we have a complete gcode but haven't started executing it
 		executing		// we have a complete gcode and have started executing it
 	};
-    int CheckSum() const;								// Compute the checksum (if any) at the end of the G Code
-    Platform* platform;									// Pointer to the RepRap's controlling class
-    char gcodeBuffer[GCODE_LENGTH];						// The G Code
-    const char* identity;								// Where we are from (web, file, serial line etc)
-    int gcodePointer;									// Index in the buffer
-    int readPointer;									// Where in the buffer to read next
-    bool inComment;										// Are we after a ';' character?
-    bool checksumRequired;								// True if we only accept commands with a valid checksum
-    GCodeState state;									// Idle, executing or paused
-    const char* writingFileDirectory;					// If the G Code is going into a file, where that is
-    int toolNumberAdjust;								// The adjustment to tool numbers in commands we receive
-    const MessageType responseMessageType;				// The message type we use for responses to commands coming from this channel
+
+	int CheckSum() const;								// Compute the checksum (if any) at the end of the G Code
+
+	GCodeMachineState *machineState;					// Machine state for this gcode source
+	char gcodeBuffer[GCODE_LENGTH];						// The G Code
+	const char* identity;								// Where we are from (web, file, serial line etc)
+	int gcodePointer;									// Index in the buffer
+	int readPointer;									// Where in the buffer to read next
+	bool inComment;										// Are we after a ';' character?
+	bool checksumRequired;								// True if we only accept commands with a valid checksum
+	GCodeBufferState bufferState;									// Idle, executing or paused
+	const char* writingFileDirectory;					// If the G Code is going into a file, where that is
+	int toolNumberAdjust;								// The adjustment to tool numbers in commands we receive
+	const MessageType responseMessageType;				// The message type we use for responses to commands coming from this channel
 };
 
 inline const char* GCodeBuffer::Buffer() const
@@ -72,22 +78,22 @@ inline const char* GCodeBuffer::Buffer() const
 
 inline bool GCodeBuffer::IsIdle() const
 {
-	return state == GCodeState::idle;
+	return bufferState == GCodeBufferState::idle;
 }
 
 inline bool GCodeBuffer::IsReady() const
 {
-	return state == GCodeState::ready;
+	return bufferState == GCodeBufferState::ready;
 }
 
 inline bool GCodeBuffer::IsExecuting() const
 {
-	return state == GCodeState::executing;
+	return bufferState == GCodeBufferState::executing;
 }
 
 inline void GCodeBuffer::SetFinished(bool f)
 {
-	state = (f) ? GCodeState::idle : GCodeState::executing;
+	bufferState = (f) ? GCodeBufferState::idle : GCodeBufferState::executing;
 }
 
 inline const char* GCodeBuffer::WritingFileDirectory() const
diff --git a/src/GCodes/GCodeMachineState.cpp b/src/GCodes/GCodeMachineState.cpp
new file mode 100644
index 00000000..3cfa1204
--- /dev/null
+++ b/src/GCodes/GCodeMachineState.cpp
@@ -0,0 +1,55 @@
+/*
+ * GCodeMachineState.cpp
+ *
+ *  Created on: 15 Nov 2016
+ *      Author: David
+ */
+
+#include "GCodeMachineState.h"
+
+GCodeMachineState *GCodeMachineState::freeList = nullptr;
+unsigned int GCodeMachineState::numAllocated = 0;
+
+// Create a default initialised GCodeMachineState
+GCodeMachineState::GCodeMachineState()
+	: previous(nullptr), feedrate(DEFAULT_FEEDRATE), fileState(), lockedResources(0), state(GCodeState::normal),
+	  drivesRelative(false), axesRelative(false), doingFileMacro(false)
+{
+}
+
+// Allocate a new GCodeMachineState
+/*static*/ GCodeMachineState *GCodeMachineState::Allocate()
+{
+	GCodeMachineState *ms = freeList;
+	if (ms != nullptr)
+	{
+		freeList = ms->previous;
+		ms->lockedResources = 0;
+		ms->state = GCodeState::normal;
+	}
+	else
+	{
+		ms = new GCodeMachineState();
+		++numAllocated;
+	}
+	return ms;
+}
+
+/*static*/ void GCodeMachineState::Release(GCodeMachineState *ms)
+{
+	ms->fileState.Close();
+	ms->previous = freeList;
+	freeList = ms;
+}
+
+/*static*/ unsigned int GCodeMachineState::GetNumInUse()
+{
+	unsigned int inUse = numAllocated;
+	for (GCodeMachineState *ms = freeList; ms != nullptr; ms = ms->previous)
+	{
+		--inUse;
+	}
+	return inUse;
+}
+
+// End
diff --git a/src/GCodes/GCodeMachineState.h b/src/GCodes/GCodeMachineState.h
new file mode 100644
index 00000000..3e60f661
--- /dev/null
+++ b/src/GCodes/GCodeMachineState.h
@@ -0,0 +1,65 @@
+/*
+ * GCodeMachineState.h
+ *
+ *  Created on: 15 Nov 2016
+ *      Author: David
+ */
+
+#ifndef SRC_GCODES_GCODEMACHINESTATE_H_
+#define SRC_GCODES_GCODEMACHINESTATE_H_
+
+#include <cstdint>
+#include "Configuration.h"
+#include "Storage/FileData.h"
+
+// Enumeration to list all the possible states that the Gcode processing machine may be in
+enum class GCodeState : uint8_t
+{
+	normal,												// not doing anything and ready to process a new GCode
+	waitingForMoveToComplete,							// doing a homing move, so we must wait for it to finish before processing another GCode
+	homing,
+	setBed1,
+	setBed2,
+	setBed3,
+	toolChange1,
+	toolChange2,
+	toolChange3,
+	pausing1,
+	pausing2,
+	resuming1,
+	resuming2,
+	resuming3,
+	flashing1,
+	flashing2,
+	stopping,
+	sleeping
+};
+
+// Class to hold the state of gcode execution for some input source
+class GCodeMachineState
+{
+public:
+	GCodeMachineState();
+
+	GCodeMachineState *previous;
+	float feedrate;
+	FileData fileState;
+	uint32_t lockedResources;
+	GCodeState state;
+	bool drivesRelative;
+	bool axesRelative;
+	bool doingFileMacro;
+
+	static GCodeMachineState *Allocate()
+	post(!result.IsLive(); result.state == GCodeState::normal);
+
+	static void Release(GCodeMachineState *ms);
+	static unsigned int GetNumAllocated() { return numAllocated; }
+	static unsigned int GetNumInUse();
+
+private:
+	static GCodeMachineState *freeList;
+	static unsigned int numAllocated;
+};
+
+#endif /* SRC_GCODES_GCODEMACHINESTATE_H_ */
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index 65e67dd8..79ca3dda 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -58,15 +58,15 @@ void GCodes::RestorePoint::Init()
 }
 
 GCodes::GCodes(Platform* p, Webserver* w) :
-	platform(p), webserver(w), active(false), stackPointer(0), isFlashing(false),
+	platform(p), webserver(w), active(false), isFlashing(false),
 	fileBeingHashed(nullptr)
 {
-	httpGCode = new GCodeBuffer(platform, "http", HTTP_MESSAGE);
-	telnetGCode = new GCodeBuffer(platform, "telnet", TELNET_MESSAGE);
-	fileGCode = new GCodeBuffer(platform, "file", GENERIC_MESSAGE);
-	serialGCode = new GCodeBuffer(platform, "serial", HOST_MESSAGE);
-	auxGCode = new GCodeBuffer(platform, "aux", AUX_MESSAGE);
-	fileMacroGCode = new GCodeBuffer(platform, "macro", GENERIC_MESSAGE);
+	httpGCode = new GCodeBuffer("http", HTTP_MESSAGE);
+	telnetGCode = new GCodeBuffer("telnet", TELNET_MESSAGE);
+	fileGCode = new GCodeBuffer("file", GENERIC_MESSAGE);
+	serialGCode = new GCodeBuffer("serial", HOST_MESSAGE);
+	auxGCode = new GCodeBuffer("aux", AUX_MESSAGE);
+	daemonGCode = new GCodeBuffer("daemon", GENERIC_MESSAGE);
 }
 
 void GCodes::Exit()
@@ -120,17 +120,13 @@ void GCodes::Reset()
 	serialGCode->Init();
 	auxGCode->Init();
 	auxGCode->SetCommsProperties(1);					// by default, we require a checksum on the aux port
-	fileMacroGCode->Init();
-	moveAvailable = false;
-	fileBeingPrinted.Close();
+	daemonGCode->Init();
+
+	nextGcodeSource = 0;
+
 	fileToPrint.Close();
 	fileBeingWritten = NULL;
-	doingFileMacro = false;
 	dwellWaiting = false;
-	stackPointer = 0;
-	state = GCodeState::normal;
-	drivesRelative = true;
-	axesRelative = false;
 	probeCount = 0;
 	cannedCycleMoveCount = 0;
 	cannedCycleMoveQueued = false;
@@ -161,120 +157,73 @@ void GCodes::Reset()
 	filePos = moveBuffer.filePos = noFilePosition;
 	lastEndstopStates = platform->GetAllEndstopStates();
 	firmwareUpdateModuleMap = 0;
+
+	cancelWait = isWaiting = false;
+
+	for (size_t i = 0; i < NumResources; ++i)
+	{
+		resourceOwners[i] = nullptr;
+	}
 }
 
 float GCodes::FractionOfFilePrinted() const
 {
-	if (isPaused)
-	{
-		return (fileToPrint.IsLive()) ? fileToPrint.FractionRead() : -1.0;
-	}
-	if (stackPointer == 0)
-	{
-		return (fileBeingPrinted.IsLive() && !doingFileMacro) ? fileBeingPrinted.FractionRead() : -1.0;
-	}
-	return (stack[0].fileState.IsLive() && !stack[0].doingFileMacro) ? stack[0].fileState.FractionRead() : -1.0;
+	const FileData& fileBeingPrinted = fileGCode->OriginalMachineState().fileState;
+	return (fileBeingPrinted.IsLive()) ? fileBeingPrinted.FractionRead() : -1.0;
 }
 
-void GCodes::DoFilePrint(GCodeBuffer* gb, StringRef& reply)
+// Start running the config file
+// We use triggerCGode as the source to prevent any triggers being executed until we have finished
+bool GCodes::RunConfigFile(const char* fileName)
 {
-	if (gb != fileGCode || !isPaused)
-	{
-		for (int i = 0; i < 50 && fileBeingPrinted.IsLive(); ++i)
-		{
-			char b;
-			if (fileBeingPrinted.Read(b))
-			{
-				if (gb->StartingNewCode() && gb == fileGCode)
-				{
-					filePos = fileBeingPrinted.GetPosition() - 1;
-					//debugPrintf("Set file pos %u\n", filePos);
-				}
-				if (gb->Put(b))
-				{
-					gb->SetFinished(ActOnCode(gb, reply));
-					break;
-				}
-			}
-			else
-			{
-				// We have reached the end of the file.
-				// Don't close the file until all moves have been completed, in case the print gets paused.
-				// Also, this keeps the state as 'Printing' until the print really has finished.
-				if (!gb->StartingNewCode())		// if there is something in the buffer
-				{
-					if (gb->Put('\n')) 			// in case there wasn't one ending the file
-					{
-						gb->SetFinished(ActOnCode(gb, reply));
-					}
-					else
-					{
-						gb->Init();
-					}
-				}
-				else if (AllMovesAreFinishedAndMoveBufferIsLoaded())
-				{
-					fileBeingPrinted.Close();
-					if (gb == fileGCode)
-					{
-						reprap.GetPrintMonitor()->StoppedPrint();
-						if (platform->Emulating() == marlin)
-						{
-							// Pronterface expects a "Done printing" message
-							HandleReply(gb, false, "Done printing file");
-						}
-					}
-				}
-				break;
-			}
-		}
-	}
+	return DoFileMacro(*daemonGCode, fileName, false);
+}
+
+// Are we still running the config file?
+bool GCodes::IsRunningConfigFile() const
+{
+	return daemonGCode->MachineState().fileState.IsLive();
 }
 
 void GCodes::Spin()
 {
 	if (!active)
+	{
 		return;
+	}
+
+	CheckTriggers();
 
-	// First check for new gcodes from all sources except file
-	FillGCodeBuffers();
+	// Get the GCodeBuffer that we want to work from
+	GCodeBuffer& gb = *(gcodeSources[nextGcodeSource]);
 
+	// Set up a buffer for the reply
 	char replyBuffer[gcodeReplyLength];
 	StringRef reply(replyBuffer, ARRAY_SIZE(replyBuffer));
 	reply.Clear();
 
-	// Check for poll requests from Pronterface and PanelDue so that the status is kept up to date during execution of file macros etc.
-	if (serialGCode->IsReady() && serialGCode->IsPollRequest())
+	if (gb.MachineState().state == GCodeState::normal)
 	{
-		serialGCode->SetFinished(ActOnCode(serialGCode, reply));
-	}
-	else if (auxGCode->IsReady() && auxGCode->IsPollRequest())
-	{
-		auxGCode->SetFinished(ActOnCode(auxGCode, reply));
+		StartNextGCode(gb, reply);
 	}
 	else
 	{
-		// Perform the next operation of the state machine
-		// Note: if we change the state to 'normal' from another state, we must call HandleReply to tell the host about the command we have just completed.
-		switch (state)
-		{
-		case GCodeState::normal:
-			StartNextGCode(reply);
-			break;
+		// Perform the next operation of the state machine for this gcode source
+		bool error = false;
 
+		switch (gb.MachineState().state)
+		{
 		case GCodeState::waitingForMoveToComplete:
 			if (AllMovesAreFinishedAndMoveBufferIsLoaded())
 			{
-				HandleReply(gbCurrent, false, "");
-				state = GCodeState::normal;
+				gb.MachineState().state = GCodeState::normal;
 			}
 			break;
 
 		case GCodeState::homing:
 			if (toBeHomed == 0)
 			{
-				HandleReply(gbCurrent, false, "");
-				state = GCodeState::normal;
+				gb.MachineState().state = GCodeState::normal;
 			}
 			else
 			{
@@ -284,7 +233,7 @@ void GCodes::Spin()
 					if ((toBeHomed & (1u << axis)) != 0 && (axis != Z_AXIS || toBeHomed == (1u << Z_AXIS)))
 					{
 						toBeHomed &= ~(1u << axis);
-						DoFileMacro(homingFileNames[axis]);
+						DoFileMacro(gb, homingFileNames[axis]);
 						break;
 					}
 				}
@@ -294,21 +243,20 @@ void GCodes::Spin()
 		case GCodeState::setBed1:
 			reprap.GetMove()->SetIdentityTransform();
 			probeCount = 0;
-			state = GCodeState::setBed2;
+			gb.MachineState().state = GCodeState::setBed2;
 			// no break
 
 		case GCodeState::setBed2:
 			{
 				int numProbePoints = reprap.GetMove()->NumberOfXYProbePoints();
-				if (DoSingleZProbeAtPoint(probeCount, 0.0))
+				if (DoSingleZProbeAtPoint(gb, probeCount, 0.0))
 				{
 					probeCount++;
 					if (probeCount >= numProbePoints)
 					{
 						zProbesSet = true;
 						reprap.GetMove()->FinishedBedProbing(0, reply);
-						HandleReply(gbCurrent, false, reply.Pointer());
-						state = GCodeState::normal;
+						gb.MachineState().state = GCodeState::normal;
 					}
 				}
 			}
@@ -322,40 +270,38 @@ void GCodes::Spin()
 					reprap.StandbyTool(oldTool->Number());
 				}
 			}
-			state = GCodeState::toolChange2;
+			gb.MachineState().state = GCodeState::toolChange2;
 			if (reprap.GetTool(newToolNumber) != nullptr && AllAxesAreHomed())
 			{
 				scratchString.printf("tpre%d.g", newToolNumber);
-				DoFileMacro(scratchString.Pointer(), false);
+				DoFileMacro(gb, scratchString.Pointer(), false);
 			}
 			break;
 
 		case GCodeState::toolChange2: // Select the new tool (even if it doesn't exist - that just deselects all tools)
 			reprap.SelectTool(newToolNumber);
-			state = GCodeState::toolChange3;
+			gb.MachineState().state = GCodeState::toolChange3;
 			if (reprap.GetTool(newToolNumber) != nullptr && AllAxesAreHomed())
 			{
 				scratchString.printf("tpost%d.g", newToolNumber);
-				DoFileMacro(scratchString.Pointer(), false);
+				DoFileMacro(gb, scratchString.Pointer(), false);
 			}
 			break;
 
 		case GCodeState::toolChange3:
-			HandleReply(gbCurrent, false, "");
-			state = GCodeState::normal;
+			gb.MachineState().state = GCodeState::normal;
 			break;
 
 		case GCodeState::pausing1:
 			if (AllMovesAreFinishedAndMoveBufferIsLoaded())
 			{
-				state = GCodeState::pausing2;
-				DoFileMacro(PAUSE_G);
+				gb.MachineState().state = GCodeState::pausing2;
+				DoFileMacro(gb, PAUSE_G);
 			}
 			break;
 
 		case GCodeState::pausing2:
-			HandleReply(gbCurrent, false, "Printing paused");
-			state = GCodeState::normal;
+			reply.copy("Printing paused");
 			break;
 
 		case GCodeState::resuming1:
@@ -378,16 +324,16 @@ void GCodes::Spin()
 				moveBuffer.endStopsToCheck = 0;
 				moveBuffer.usePressureAdvance = false;
 				moveBuffer.filePos = noFilePosition;
-				if (state == GCodeState::resuming1 && currentZ > pauseRestorePoint.moveCoords[Z_AXIS])
+				if (gb.MachineState().state == GCodeState::resuming1 && currentZ > pauseRestorePoint.moveCoords[Z_AXIS])
 				{
 					// First move the head to the correct XY point, then move it down in a separate move
 					moveBuffer.coords[Z_AXIS] = currentZ;
-					state = GCodeState::resuming2;
+					gb.MachineState().state = GCodeState::resuming2;
 				}
 				else
 				{
 					// Just move to the saved position in one go
-					state = GCodeState::resuming3;
+					gb.MachineState().state = GCodeState::resuming3;
 				}
 				moveAvailable = true;
 			}
@@ -406,13 +352,13 @@ void GCodes::Spin()
 				}
 				feedRate = pauseRestorePoint.feedRate;
 				isPaused = false;
-				HandleReply(gbCurrent, false, "Printing resumed");
-				state = GCodeState::normal;
+				reply.copy("Printing resumed");
+				gb.MachineState().state = GCodeState::normal;
 			}
 			break;
 
 		case GCodeState::flashing1:
-	#ifdef DUET_NG
+#ifdef DUET_NG
 			// Update additional modules before the main firmware
 			if (FirmwareUpdater::IsReady())
 			{
@@ -429,12 +375,12 @@ void GCodes::Spin()
 				}
 				if (!updating)
 				{
-					state = GCodeState::flashing2;
+					gb.MachineState().state = GCodeState::flashing2;
 				}
 			}
-	#else
-			state = GCodeState::flashing2;
-	#endif
+#else
+			gb.MachineState().state = GCodeState::flashing2;
+#endif
 			break;
 
 		case GCodeState::flashing2:
@@ -446,13 +392,13 @@ void GCodes::Spin()
 				// The above call does not return unless an error occurred
 			}
 			isFlashing = false;
-			state = GCodeState::normal;
+			gb.MachineState().state = GCodeState::normal;
 			break;
 
 		case GCodeState::stopping:		// MO after executing stop.g if present
 		case GCodeState::sleeping:		// M1 after executing sleep.g if present
 			// Deselect the active tool and turn off all heaters, unless parameter Hn was used with n > 0
-			if (!gbCurrent->Seen('H') || gbCurrent->GetIValue() <= 0)
+			if (!gb.Seen('H') || gb.GetIValue() <= 0)
 			{
 				Tool* tool = reprap.GetCurrentTool();
 				if (tool != nullptr)
@@ -464,7 +410,7 @@ void GCodes::Spin()
 
 			// chrishamm 2014-18-10: Although RRP says M0 is supposed to turn off all drives and heaters,
 			// I think M1 is sufficient for this purpose. Leave M0 for a normal reset.
-			if (state == GCodeState::sleeping)
+			if (gb.MachineState().state == GCodeState::sleeping)
 			{
 				DisableDrives();
 			}
@@ -472,171 +418,190 @@ void GCodes::Spin()
 			{
 				platform->SetDriversIdle();
 			}
-			HandleReply(gbCurrent, false, "");
-			state = GCodeState::normal;
+			gb.MachineState().state = GCodeState::normal;
 			break;
 
 		default:				// should not happen
 			break;
 		}
+
+		if (gb.MachineState().state == GCodeState::normal)
+		{
+			// We completed a command, so unlock resources and tell the host about it
+			UnlockAll(gb);
+			HandleReply(gb, error, reply.Pointer());
+		}
+	}
+
+	// Move on to the next gcode source ready for next time
+	++nextGcodeSource;
+	if (nextGcodeSource == ARRAY_SIZE(gcodeSources))
+	{
+		nextGcodeSource = 0;
 	}
 
 	platform->ClassReport(longWait);
 }
 
-// Get new data into the gcode buffers except the file and macro gcode buffers, and deal with any file uploading
-void GCodes::FillGCodeBuffers()
+// Start a new gcode, or continue to execute one that has already been started:
+void GCodes::StartNextGCode(GCodeBuffer& gb, StringRef& reply)
 {
-	// Webserver
-	if (httpGCode->IsIdle())
+	if (gb.IsReady() || gb.IsExecuting())
 	{
+		gb.SetFinished(ActOnCode(gb, reply));
+	}
+	else if (gb.MachineState().fileState.IsLive())
+	{
+		if (&gb != fileGCode || !isPaused)
+		{
+			DoFilePrint(gb, reply);
+		}
+	}
+	else if (&gb == httpGCode)
+	{
+		// Webserver
 		for (unsigned int i = 0; i < 16 && webserver->GCodeAvailable(WebSource::HTTP); ++i)
 		{
-			char b = webserver->ReadGCode(WebSource::HTTP);
-			if (httpGCode->Put(b))
+			const char b = webserver->ReadGCode(WebSource::HTTP);
+			if (gb.Put(b))
 			{
 				// We have a complete gcode
-				if (httpGCode->WritingFileDirectory() != nullptr)
+				if (gb.WritingFileDirectory() != nullptr)
+				{
+					WriteGCodeToFile(gb);
+					gb.SetFinished(true);
+				}
+				else
 				{
-					WriteGCodeToFile(httpGCode);
-					httpGCode->SetFinished(true);
+					gb.SetFinished(ActOnCode(gb, reply));
 				}
 				break;
 			}
 		}
 	}
-
-	// Telnet
-	if (telnetGCode->IsIdle())
+	else if (&gb == telnetGCode)
 	{
+		// Telnet
 		for (unsigned int i = 0; i < GCODE_LENGTH && webserver->GCodeAvailable(WebSource::Telnet); ++i)
 		{
 			char b = webserver->ReadGCode(WebSource::Telnet);
-			if (telnetGCode->Put(b))
+			if (gb.Put(b))
 			{
+				gb.SetFinished(ActOnCode(gb, reply));
 				break;
 			}
 		}
 	}
-
-	// USB interface
-	if (serialGCode->IsIdle())
+	else if (&gb == serialGCode)
 	{
+		// USB interface
 		for (unsigned int i = 0; i < 16 && platform->GCodeAvailable(SerialSource::USB); ++i)
 		{
-			char b = platform->ReadFromSource(SerialSource::USB);
+			const char b = platform->ReadFromSource(SerialSource::USB);
 			// Check the special case of uploading the reprap.htm file
-			if (serialGCode->WritingFileDirectory() == platform->GetWebDir())
+			if (gb.WritingFileDirectory() == platform->GetWebDir())
 			{
-				WriteHTMLToFile(b, serialGCode);
+				WriteHTMLToFile(gb, b);
 			}
-			else if (serialGCode->Put(b))	// add char to buffer and test whether the gcode is complete
+			else if (gb.Put(b))	// add char to buffer and test whether the gcode is complete
 			{
 				// We have a complete gcode
-				if (serialGCode->WritingFileDirectory() != nullptr)
+				if (gb.WritingFileDirectory() != nullptr)
+				{
+					WriteGCodeToFile(gb);
+					gb.SetFinished(true);
+				}
+				else
 				{
-					WriteGCodeToFile(serialGCode);
-					serialGCode->SetFinished(true);
+					gb.SetFinished(ActOnCode(gb, reply));
 				}
 				break;
 			}
 		}
 	}
-
-	// Aux serial port (typically PanelDue)
-	if (auxGCode->IsIdle())
+	else if (&gb == auxGCode)
 	{
+		// Aux serial port (typically PanelDue)
 		for (unsigned int i = 0; i < 16 && platform->GCodeAvailable(SerialSource::AUX); ++i)
 		{
 			char b = platform->ReadFromSource(SerialSource::AUX);
-			if (auxGCode->Put(b))	// add char to buffer and test whether the gcode is complete
+			if (gb.Put(b))	// add char to buffer and test whether the gcode is complete
 			{
 				platform->SetAuxDetected();
+				gb.SetFinished(ActOnCode(gb, reply));
 				break;
 			}
 		}
 	}
 }
 
-// Start a new gcode, or continue to execute one that has already been started:
-// 1. If we're doing a file macro, don't allow anything else to interrupt it
-// 2. Continue executing any gcode that we have already started
-// 3. Check for external triggers
-// 4. If we have a gcode ready from any non-file sources, start executing it
-// 5. Else continue a print from file, if one is running
-void GCodes::StartNextGCode(StringRef& reply)
+void GCodes::DoFilePrint(GCodeBuffer& gb, StringRef& reply)
 {
-	// If a file macro is running, we don't allow anything to interrupt it
-	if (doingFileMacro)
+	FileData& fd = gb.MachineState().fileState;
+	for (int i = 0; i < 50 && fd.IsLive(); ++i)
 	{
-		// Complete the current move (must do this before checking whether we have finished the file in case it didn't end in newline)
-		if (fileMacroGCode->IsReady() || fileMacroGCode->IsExecuting())
+		char b;
+		if (fd.Read(b))
 		{
-			fileMacroGCode->SetFinished(ActOnCode(fileMacroGCode, reply));
-		}
-		else if (fileBeingPrinted.IsLive())				// Have we finished the file?
-		{
-			DoFilePrint(fileMacroGCode, reply);			// No - Do more of the file
+			if (gb.StartingNewCode() && &gb == fileGCode && gb.MachineState().previous == nullptr)
+			{
+				filePos = fd.GetPosition() - 1;
+				//debugPrintf("Set file pos %u\n", filePos);
+			}
+			if (gb.Put(b))
+			{
+				gb.SetFinished(ActOnCode(gb, reply));
+				return;
+			}
 		}
-		else if (AllMovesAreFinishedAndMoveBufferIsLoaded())
+		else
 		{
-			Pop();
-			fileMacroGCode->Init();
+			// We have reached the end of the file. Check for the last line of gcode not ending in newline.
+			if (!gb.StartingNewCode())				// if there is something in the buffer
+			{
+				if (gb.Put('\n')) 					// in case there wasn't a newline ending the file
+				{
+					gb.SetFinished(ActOnCode(gb, reply));
+					return;
+				}
+			}
+
+			gb.Init();								// mark buffer as empty
+
+			// Don't close the file until all moves have been completed, in case the print gets paused.
+			// Also, this keeps the state as 'Printing' until the print really has finished.
+			if (AllMovesAreFinishedAndMoveBufferIsLoaded())
+			{
+				fd.Close();
+				if (gb.MachineState().previous == nullptr)
+				{
+					// Finished printing SD card file
+					reprap.GetPrintMonitor()->StoppedPrint();
+					if (platform->Emulating() == marlin)
+					{
+						// Pronterface expects a "Done printing" message
+						HandleReply(gb, false, "Done printing file");
+					}
+				}
+				else
+				{
+					// Finished a macro
+					Pop(gb);
+					gb.Init();
+					if (gb.MachineState().state == GCodeState::normal)
+					{
+						UnlockAll(gb);
+						HandleReply(gb, false, "");
+					}
+				}
+			}
+			return;
 		}
 	}
-	// Check for gcodes that we have already started
-	else if (httpGCode->IsExecuting())
-	{
-		httpGCode->SetFinished(ActOnCode(httpGCode, reply));
-	}
-	else if (telnetGCode->IsExecuting())
-	{
-		telnetGCode->SetFinished(ActOnCode(telnetGCode, reply));
-	}
-	else if (serialGCode->IsExecuting())
-	{
-		serialGCode->SetFinished(ActOnCode(serialGCode, reply));
-	}
-	else if (auxGCode->IsExecuting())
-	{
-		auxGCode->SetFinished(ActOnCode(auxGCode, reply));
-	}
-	else if (fileGCode->IsExecuting())
-	{
-		fileGCode->SetFinished(ActOnCode(fileGCode, reply));
-	}
-	// Check triggers
-	else if (CheckTriggers())
-	{
-		// We've handled a trigger, so nothing else to do
-	}
-	// Check for gcodes we can start
-	else if (httpGCode->IsReady())
-	{
-		httpGCode->SetFinished(ActOnCode(httpGCode, reply));
-	}
-	else if (telnetGCode->IsReady())
-	{
-		telnetGCode->SetFinished(ActOnCode(telnetGCode, reply));
-	}
-	else if (serialGCode->IsReady())
-	{
-		serialGCode->SetFinished(ActOnCode(serialGCode, reply));
-	}
-	else if (auxGCode->IsReady())
-	{
-		auxGCode->SetFinished(ActOnCode(auxGCode, reply));
-	}
-	// Print some more of the current file
-	else
-	{
-		DoFilePrint(fileGCode, reply);
-	}
 }
 
-// Check for and execute triggers, returning true if started executing one.
-// We already checked that no file macro is being executed before calling this.
-bool GCodes::CheckTriggers()
+// Check for and execute triggers
+void GCodes::CheckTriggers()
 {
 	// Check for endstop state changes that activate new triggers
 	const TriggerMask oldEndstopStates = lastEndstopStates;
@@ -662,19 +627,19 @@ bool GCodes::CheckTriggers()
 	// If any triggers are pending, activate the one with the lowest number
 	if (lowestTriggerPending < MaxTriggers)
 	{
-		gbCurrent = nullptr;
-		triggersPending &= ~(1u << lowestTriggerPending);		// clear the trigger
 
 		// Execute the trigger
 		switch(lowestTriggerPending)
 		{
 		case 0:
 			// Trigger 0 does an emergency stop
+			triggersPending &= ~(1u << lowestTriggerPending);			// clear the trigger
 			DoEmergencyStop();
 			break;
 
 		case 1:
 			// Trigger 1 pauses the print, if printing from file
+			triggersPending &= ~(1u << lowestTriggerPending);			// clear the trigger
 			if (!isPaused && reprap.GetPrintMonitor()->IsPrinting())
 			{
 				DoPause(true);
@@ -683,16 +648,16 @@ bool GCodes::CheckTriggers()
 
 		default:
 			// All other trigger numbers execute the corresponding macro file
+			if (!daemonGCode->MachineState().fileState.IsLive())		// if not already executing a trigger or config.g
 			{
+				triggersPending &= ~(1u << lowestTriggerPending);		// clear the trigger
 				char buffer[25];
 				StringRef filename(buffer, ARRAY_SIZE(buffer));
 				filename.printf(SYS_DIR "trigger%u.g", lowestTriggerPending);
-				DoFileMacro(filename.Pointer(), true);
+				DoFileMacro(*daemonGCode, filename.Pointer(), true);
 			}
 		}
-		return true;		// we processed a trigger
 	}
-	return false;			// no triggers were pending
 }
 
 // Execute an emergency stop
@@ -711,10 +676,10 @@ void GCodes::DoPause(bool externalToFile)
 		// Pausing a file print via another input source
 		pauseRestorePoint.feedRate = feedRate;										// the call to PausePrint may or may not change this
 		FilePosition fPos = reprap.GetMove()->PausePrint(pauseRestorePoint.moveCoords, pauseRestorePoint.feedRate);	// tell Move we wish to pause the current print
-		FileData& fdata = (stackPointer == 0) ? fileBeingPrinted : stack[0].fileState;
+		FileData& fdata = fileGCode->MachineState().fileState;
 		if (fPos != noFilePosition && fdata.IsLive())
 		{
-			fdata.Seek(fPos);											// replay the abandoned instructions if/when we resume
+			fdata.Seek(fPos);														// replay the abandoned instructions if/when we resume
 		}
 		fileGCode->Init();
 		if (moveAvailable)
@@ -754,7 +719,7 @@ void GCodes::DoPause(bool externalToFile)
 	{
 		pausedFanValues[i] = platform->GetFanValue(i);
 	}
-	state = GCodeState::pausing1;
+	fileGCode->MachineState().state = GCodeState::pausing1;
 	isPaused = true;
 }
 
@@ -762,29 +727,30 @@ void GCodes::Diagnostics(MessageType mtype)
 {
 	platform->Message(mtype, "=== GCodes ===\n");
 	platform->MessageF(mtype, "Move available? %s\n", moveAvailable ? "yes" : "no");
-	platform->MessageF(mtype, "Stack pointer: %u of %u\n", stackPointer, StackSize);
-	fileMacroGCode->Diagnostics(mtype);
-	httpGCode->Diagnostics(mtype);
-	telnetGCode->Diagnostics(mtype);
-	serialGCode->Diagnostics(mtype);
-	auxGCode->Diagnostics(mtype);
-	fileGCode->Diagnostics(mtype);
+	platform->MessageF(mtype, "Stack records: %u allocated, %u in use\n", GCodeMachineState::GetNumAllocated(), GCodeMachineState::GetNumInUse());
+
+	for (size_t i = 0; i < ARRAY_SIZE(gcodeSources); ++i)
+	{
+		gcodeSources[i]->Diagnostics(mtype);
+	}
 }
 
 // The wait till everything's done function.  If you need the machine to
 // be idle before you do something (for example homing an axis, or shutting down) call this
-// until it returns true.  As a side-effect it loads moveBuffer with the last
-// position and feedrate for you.
-
+// until it returns true.  As a side-effect it loads moveBuffer with the last position and feedrate for you.
 bool GCodes::AllMovesAreFinishedAndMoveBufferIsLoaded()
 {
 	// Last one gone?
 	if (moveAvailable)
+	{
 		return false;
+	}
 
 	// Wait for all the queued moves to stop so we get the actual last position
 	if (!reprap.GetMove()->AllMovesAreFinished())
+	{
 		return false;
+	}
 
 	reprap.GetMove()->ResumeMoving();
 	reprap.GetMove()->GetCurrentUserPosition(moveBuffer.coords, 0);
@@ -792,48 +758,30 @@ bool GCodes::AllMovesAreFinishedAndMoveBufferIsLoaded()
 }
 
 // Save (some of) the state of the machine for recovery in the future.
-void GCodes::Push()
+bool GCodes::Push(GCodeBuffer& gb)
 {
-	if (stackPointer >= StackSize)
+	bool ok = gb.PushState();
+	if (!ok)
 	{
 		platform->Message(GENERIC_MESSAGE, "Push(): stack overflow!\n");
-		return;
 	}
-
-	stack[stackPointer].state = state;
-	stack[stackPointer].gb = gbCurrent;
-	stack[stackPointer].feedrate = feedRate;
-	stack[stackPointer].fileState.CopyFrom(fileBeingPrinted);
-	stack[stackPointer].drivesRelative = drivesRelative;
-	stack[stackPointer].axesRelative = axesRelative;
-	stack[stackPointer].doingFileMacro = doingFileMacro;
-	stackPointer++;
+	return ok;
 }
 
 // Recover a saved state
-void GCodes::Pop()
+void GCodes::Pop(GCodeBuffer& gb)
 {
-	if (stackPointer < 1)
+	if (!gb.PopState())
 	{
 		platform->Message(GENERIC_MESSAGE, "Pop(): stack underflow!\n");
-		return;
 	}
-
-	stackPointer--;
-	state = stack[stackPointer].state;
-	gbCurrent = stack[stackPointer].gb;
-	feedRate = stack[stackPointer].feedrate;
-	fileBeingPrinted.MoveFrom(stack[stackPointer].fileState);
-	drivesRelative = stack[stackPointer].drivesRelative;
-	axesRelative = stack[stackPointer].axesRelative;
-	doingFileMacro = stack[stackPointer].doingFileMacro;
 }
 
 // Move expects all axis movements to be absolute, and all extruder drive moves to be relative.  This function serves that.
 // 'moveType' is the S parameter in the G0 or G1 command, or -1 if we are doing G92.
 // For regular (type 0) moves, we apply limits and do X axis mapping.
 // Returns true if we have a legal move (or G92 argument), false if this gcode should be discarded
-bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
+bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer& gb, int moveType)
 {
 	// Zero every extruder drive as some drives may not be changed
 	for (size_t drive = numAxes; drive < DRIVES; drive++)
@@ -842,15 +790,15 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
 	}
 
 	// Deal with feed rate
-	if (gb->Seen(feedrateLetter))
+	if (gb.Seen(feedrateLetter))
 	{
-		feedRate = gb->GetFValue() * distanceScale * speedFactor;
+		feedRate = gb.GetFValue() * distanceScale * speedFactor;
 	}
 	moveBuffer.feedRate = feedRate;
 
 	// First do extrusion, and check, if we are extruding, that we have a tool to extrude with
 	Tool* tool = reprap.GetCurrentTool();
-	if (gb->Seen(extrudeLetter))
+	if (gb.Seen(extrudeLetter))
 	{
 		if (tool == nullptr)
 		{
@@ -863,7 +811,7 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
 			float eMovement[MaxExtruders];
 			if (tool->GetMixing())
 			{
-				float length = gb->GetFValue();
+				float length = gb.GetFValue();
 				for (size_t drive = 0; drive < tool->DriveCount(); drive++)
 				{
 					eMovement[drive] = length * tool->GetMix()[drive];
@@ -872,10 +820,10 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
 			else
 			{
 				size_t mc = eMoveCount;
-				gb->GetFloatArray(eMovement, mc, false);
+				gb.GetFloatArray(eMovement, mc, false);
 				if (eMoveCount != mc)
 				{
-					platform->MessageF(GENERIC_MESSAGE, "Wrong number of extruder drives for the selected tool: %s\n", gb->Buffer());
+					platform->MessageF(GENERIC_MESSAGE, "Wrong number of extruder drives for the selected tool: %s\n", gb.Buffer());
 					return false;
 				}
 			}
@@ -893,7 +841,7 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
 				}
 				else
 				{
-					float extrusionAmount = (drivesRelative)
+					float extrusionAmount = (gb.MachineState().drivesRelative)
 												? moveArg
 												: moveArg - lastRawExtruderPosition[drive];
 					lastRawExtruderPosition[drive] += extrusionAmount;
@@ -909,9 +857,9 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
 	const Tool *currentTool = reprap.GetCurrentTool();
 	for (size_t axis = 0; axis < numAxes; axis++)
 	{
-		if (gb->Seen(axisLetters[axis]))
+		if (gb.Seen(axisLetters[axis]))
 		{
-			float moveArg = gb->GetFValue() * distanceScale * axisScaleFactors[axis];
+			float moveArg = gb.GetFValue() * distanceScale * axisScaleFactors[axis];
 			if (moveType == -1)						// if doing G92
 			{
 				SetAxisIsHomed(axis);				// doing a G92 defines the absolute axis position
@@ -924,7 +872,7 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
 				{
 					const size_t mappedAxis = currentTool->GetAxisMap()[i];
 					float mappedMoveArg = moveArg;
-					if (axesRelative)
+					if (gb.MachineState().axesRelative)
 					{
 						mappedMoveArg += moveBuffer.coords[mappedAxis];
 					}
@@ -937,7 +885,7 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
 			}
 			else
 			{
-				if (axesRelative)
+				if (gb.MachineState().axesRelative)
 				{
 					moveArg += moveBuffer.coords[axis];
 				}
@@ -1003,7 +951,7 @@ bool GCodes::LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType)
 // If we have queued the move and the caller doesn't need to wait for it to complete, return 1.
 // If we need to wait for the move to complete before doing another one (e.g. because endstops are checked in this move), return 2.
 
-int GCodes::SetUpMove(GCodeBuffer *gb, StringRef& reply)
+int GCodes::SetUpMove(GCodeBuffer& gb, StringRef& reply)
 {
 	// Last one gone yet?
 	if (moveAvailable)
@@ -1014,9 +962,9 @@ int GCodes::SetUpMove(GCodeBuffer *gb, StringRef& reply)
 	// Check to see if the move is a 'homing' move that endstops are checked on.
 	moveBuffer.endStopsToCheck = 0;
 	moveBuffer.moveType = 0;
-	if (gb->Seen('S'))
+	if (gb.Seen('S'))
 	{
-		int ival = gb->GetIValue();
+		int ival = gb.GetIValue();
 		if (ival == 1 || ival == 2)
 		{
 			moveBuffer.moveType = ival;
@@ -1026,18 +974,22 @@ int GCodes::SetUpMove(GCodeBuffer *gb, StringRef& reply)
 		{
 			for (size_t i = 0; i < numAxes; ++i)
 			{
-				if (gb->Seen(axisLetters[i]))
+				if (gb.Seen(axisLetters[i]))
 				{
 					moveBuffer.endStopsToCheck |= (1u << i);
 				}
 			}
 		}
+		else if (ival == 99)		// temporary code to log Z probe change positions
+		{
+			moveBuffer.endStopsToCheck |= LogProbeChanges;
+		}
 	}
 
 	if (reprap.GetMove()->IsDeltaMode())
 	{
 		// Extra checks to avoid damaging delta printers
-		if (moveBuffer.moveType != 0 && !axesRelative)
+		if (moveBuffer.moveType != 0 && !gb.MachineState().axesRelative)
 		{
 			// We have been asked to do a move without delta mapping on a delta machine, but the move is not relative.
 			// This may be damaging and is almost certainly a user mistake, so ignore the move.
@@ -1049,7 +1001,7 @@ int GCodes::SetUpMove(GCodeBuffer *gb, StringRef& reply)
 		{
 			// The user may be attempting to move a delta printer to an XYZ position before homing the axes
 			// This may be damaging and is almost certainly a user mistake, so ignore the move. But allow extruder-only moves.
-			if (gb->Seen(axisLetters[X_AXIS]) || gb->Seen(axisLetters[Y_AXIS]) || gb->Seen(axisLetters[Z_AXIS]))
+			if (gb.Seen(axisLetters[X_AXIS]) || gb.Seen(axisLetters[Y_AXIS]) || gb.Seen(axisLetters[Z_AXIS]))
 			{
 				reply.copy("Attempt to move the head of a delta printer before homing the towers");
 				return 1;
@@ -1087,10 +1039,10 @@ int GCodes::SetUpMove(GCodeBuffer *gb, StringRef& reply)
 				break;
 			}
 		}
-		moveBuffer.filePos = (gb == fileGCode) ? filePos : noFilePosition;
+		moveBuffer.filePos = (&gb == fileGCode) ? filePos : noFilePosition;
 		//debugPrintf("Queue move pos %u\n", moveFilePos);
 	}
-	return (moveBuffer.moveType != 0) ? 2 : 1;
+	return (moveBuffer.moveType != 0 || moveBuffer.endStopsToCheck != 0) ? 2 : 1;
 }
 
 // The Move class calls this function to find what to do next.
@@ -1117,7 +1069,7 @@ void GCodes::ClearMove()
 
 // Run a file macro. Prior to calling this, 'state' must be set to the state we want to enter when the macro has been completed.
 // Return true if the file was found or it wasn't and we were asked to report that fact.
-bool GCodes::DoFileMacro(const char* fileName, bool reportMissing)
+bool GCodes::DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing)
 {
 	FileStore *f = platform->GetFileStore(platform->GetSysDir(), fileName, false);
 	if (f == nullptr)
@@ -1131,40 +1083,44 @@ bool GCodes::DoFileMacro(const char* fileName, bool reportMissing)
 		return false;
 	}
 
-	Push();
-	fileBeingPrinted.Set(f);
-	doingFileMacro = true;
-	fileMacroGCode->Init();
-	state = GCodeState::normal;
+	if (!Push(gb))
+	{
+		return true;
+	}
+	gb.MachineState().fileState.Set(f);
+	gb.MachineState().doingFileMacro = true;
+	gb.MachineState().state = GCodeState::normal;
+	gb.Init();
 	return true;
 }
 
-void GCodes::FileMacroCyclesReturn()
+void GCodes::FileMacroCyclesReturn(GCodeBuffer& gb)
 {
-	if (doingFileMacro)
+	if (gb.MachineState().doingFileMacro)
 	{
-		Pop();
-		fileMacroGCode->Init();
+		gb.PopState();
+		gb.Init();
 	}
 }
 
 // To execute any move, call this until it returns true.
-// moveToDo[] entries corresponding with false entries in action[] will
-// be ignored.  Recall that moveToDo[DRIVES] should contain the feedrate
-// you want (if action[DRIVES] is true).
-bool GCodes::DoCannedCycleMove(EndstopChecks ce)
+// There is only one copy of the canned cycle variable so you must acquire the move lock before calling this.
+bool GCodes::DoCannedCycleMove(GCodeBuffer& gb, EndstopChecks ce)
 {
-	if (AllMovesAreFinishedAndMoveBufferIsLoaded())
+	if (LockMovementAndWaitForStandstill(gb))
 	{
 		if (cannedCycleMoveQueued)		// if the move has already been queued, it must have finished
 		{
-			Pop();
+			Pop(gb);
 			cannedCycleMoveQueued = false;
 			return true;
 		}
 
 		// Otherwise, the move has not been queued yet
-		Push();
+		if (!Push(gb))
+		{
+			return true;				// stack overflow
+		}
 
 		for (size_t drive = 0; drive < DRIVES; drive++)
 		{
@@ -1191,14 +1147,14 @@ bool GCodes::DoCannedCycleMove(EndstopChecks ce)
 }
 
 // This sets positions.  I.e. it handles G92.
-bool GCodes::SetPositions(GCodeBuffer *gb)
+bool GCodes::SetPositions(GCodeBuffer& gb)
 {
 	// Don't pause the machine if only extruder drives are being reset (DC, 2015-09-06).
 	// This avoids blobs and seams when the gcode uses absolute E coordinates and periodically includes G92 E0.
 	bool includingAxes = false;
 	for (size_t drive = 0; drive < numAxes; ++drive)
 	{
-		if (gb->Seen(axisLetters[drive]))
+		if (gb.Seen(axisLetters[drive]))
 		{
 			includingAxes = true;
 			break;
@@ -1207,7 +1163,7 @@ bool GCodes::SetPositions(GCodeBuffer *gb)
 
 	if (includingAxes)
 	{
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -1242,11 +1198,11 @@ bool GCodes::SetPositions(GCodeBuffer *gb)
 // Offset the axes by the X, Y, and Z amounts in the M code in gb.  Say the machine is at [10, 20, 30] and
 // the offsets specified are [8, 2, -5].  The machine will move to [18, 22, 25] and henceforth consider that point
 // to be [10, 20, 30].
-bool GCodes::OffsetAxes(GCodeBuffer* gb)
+bool GCodes::OffsetAxes(GCodeBuffer& gb)
 {
 	if (!offSetSet)
 	{
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -1255,9 +1211,9 @@ bool GCodes::OffsetAxes(GCodeBuffer* gb)
 			if (drive < numAxes)
 			{
 				record[drive] = moveBuffer.coords[drive];
-				if (gb->Seen(axisLetters[drive]))
+				if (gb.Seen(axisLetters[drive]))
 				{
-					cannedMoveCoords[drive] = gb->GetFValue();
+					cannedMoveCoords[drive] = gb.GetFValue();
 					cannedMoveType[drive] = CannedMoveType::relative;
 				}
 			}
@@ -1268,9 +1224,9 @@ bool GCodes::OffsetAxes(GCodeBuffer* gb)
 			cannedMoveType[drive] = CannedMoveType::none;
 		}
 
-		if (gb->Seen(feedrateLetter)) // Has the user specified a feedrate?
+		if (gb.Seen(feedrateLetter)) // Has the user specified a feedrate?
 		{
-			cannedFeedRate = gb->GetFValue() * distanceScale * SECONDS_TO_MINUTES;
+			cannedFeedRate = gb.GetFValue() * distanceScale * SECONDS_TO_MINUTES;
 		}
 		else
 		{
@@ -1280,7 +1236,7 @@ bool GCodes::OffsetAxes(GCodeBuffer* gb)
 		offSetSet = true;
 	}
 
-	if (DoCannedCycleMove(0))
+	if (DoCannedCycleMove(gb, 0))
 	{
 		// Restore positions
 		for (size_t drive = 0; drive < DRIVES; drive++)
@@ -1301,9 +1257,9 @@ bool GCodes::OffsetAxes(GCodeBuffer* gb)
 // Returns true if completed, false if needs to be called again.
 // 'reply' is only written if there is an error.
 // 'error' is false on entry, gets changed to true if there is an error.
-bool GCodes::DoHome(GCodeBuffer *gb, StringRef& reply, bool& error)
+bool GCodes::DoHome(GCodeBuffer& gb, StringRef& reply, bool& error)
 {
-	if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+	if (!LockMovementAndWaitForStandstill(gb))
 	{
 		return false;
 	}
@@ -1327,14 +1283,14 @@ bool GCodes::DoHome(GCodeBuffer *gb, StringRef& reply, bool& error)
 	if (reprap.GetMove()->IsDeltaMode())
 	{
 		SetAllAxesNotHomed();
-		DoFileMacro(HOME_DELTA_G);
+		DoFileMacro(gb, HOME_DELTA_G);
 	}
 	else
 	{
 		toBeHomed = 0;
 		for (size_t axis = 0; axis < numAxes; ++axis)
 		{
-			if (gb->Seen(axisLetters[axis]))
+			if (gb.Seen(axisLetters[axis]))
 			{
 				toBeHomed |= (1u << axis);
 				SetAxisNotHomed(axis);
@@ -1345,7 +1301,7 @@ bool GCodes::DoHome(GCodeBuffer *gb, StringRef& reply, bool& error)
 		{
 			// Homing everything
 			SetAllAxesNotHomed();
-			DoFileMacro(HOME_ALL_G);
+			DoFileMacro(gb, HOME_ALL_G);
 		}
 		else if (   platform->MustHomeXYBeforeZ()
 				 && ((toBeHomed & (1u << Z_AXIS)) != 0)
@@ -1358,7 +1314,7 @@ bool GCodes::DoHome(GCodeBuffer *gb, StringRef& reply, bool& error)
 		}
 		else
 		{
-			state = GCodeState::homing;
+			gb.MachineState().state = GCodeState::homing;
 		}
 	}
 	return true;
@@ -1368,7 +1324,7 @@ bool GCodes::DoHome(GCodeBuffer *gb, StringRef& reply, bool& error)
 // probes the bed height, and records the Z coordinate probed.  If you want to program any general
 // internal canned cycle, this shows how to do it.
 // On entry, probePointIndex specifies which of the points this is.
-bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
+bool GCodes::DoSingleZProbeAtPoint(GCodeBuffer& gb, int probePointIndex, float heightAdjust)
 {
 	reprap.GetMove()->SetIdentityTransform(); 		// It doesn't matter if these are called repeatedly
 
@@ -1383,7 +1339,7 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 		cannedMoveCoords[Z_AXIS] = platform->GetZProbeDiveHeight() + max<float>(platform->ZProbeStopHeight(), 0.0);
 		cannedMoveType[Z_AXIS] = CannedMoveType::absolute;
 		cannedFeedRate = platform->GetZProbeTravelSpeed();
-		if (DoCannedCycleMove(0))
+		if (DoCannedCycleMove(gb, 0))
 		{
 			cannedCycleMoveCount++;
 		}
@@ -1395,7 +1351,7 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 		cannedMoveType[Y_AXIS] = CannedMoveType::absolute;
 		// NB - we don't use the Z value
 		cannedFeedRate = platform->GetZProbeTravelSpeed();
-		if (DoCannedCycleMove(0))
+		if (DoCannedCycleMove(gb, 0))
 		{
 			cannedCycleMoveCount++;
 		}
@@ -1406,7 +1362,7 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 			const float height = (GetAxisIsHomed(Z_AXIS))
 									? 2 * platform->GetZProbeDiveHeight()			// Z axis has been homed, so no point in going very far
 									: 1.1 * platform->AxisTotalLength(Z_AXIS);		// Z axis not homed yet, so treat this as a homing move
-			switch(DoZProbe(height))
+			switch(DoZProbe(gb, height))
 			{
 			case 0:
 				// Z probe is already triggered at the start of the move, so abandon the probe and record an error
@@ -1450,7 +1406,7 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 		cannedMoveCoords[Z_AXIS] = platform->GetZProbeDiveHeight() + max<float>(platform->ZProbeStopHeight(), 0.0);
 		cannedMoveType[Z_AXIS] = CannedMoveType::absolute;
 		cannedFeedRate = platform->GetZProbeTravelSpeed();
-		if (DoCannedCycleMove(0))
+		if (DoCannedCycleMove(gb, 0))
 		{
 			cannedCycleMoveCount = 0;
 			return true;
@@ -1465,9 +1421,9 @@ bool GCodes::DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust)
 
 // This simply moves down till the Z probe/switch is triggered. Call it repeatedly until it returns true.
 // Called when we do a G30 with no P parameter.
-bool GCodes::DoSingleZProbe(bool reportOnly, float heightAdjust)
+bool GCodes::DoSingleZProbe(GCodeBuffer& gb, bool reportOnly, float heightAdjust)
 {
-	switch (DoZProbe(1.1 * platform->AxisTotalLength(Z_AXIS)))
+	switch (DoZProbe(gb, 1.1 * platform->AxisTotalLength(Z_AXIS)))
 	{
 	case 0:		// failed
 		platform->Message(GENERIC_MESSAGE, "Error: Z probe already triggered at start of probing move\n");
@@ -1497,7 +1453,7 @@ bool GCodes::DoSingleZProbe(bool reportOnly, float heightAdjust)
 // Returns 0 if Z probe already triggered at start of probing
 // Returns 1 if Z probe didn't trigger
 // Returns 2 if success, with the current position in moveBuffer
-int GCodes::DoZProbe(float distance)
+int GCodes::DoZProbe(GCodeBuffer& gb, float distance)
 {
 	if (platform->GetZProbeType() == ZProbeTypeDelta)
 	{
@@ -1526,7 +1482,7 @@ int GCodes::DoZProbe(float distance)
 		cannedMoveType[Z_AXIS] = CannedMoveType::relative;
 		cannedFeedRate = platform->GetZProbeParameters().probeSpeed;
 
-		if (DoCannedCycleMove(ZProbeActive))
+		if (DoCannedCycleMove(gb, ZProbeActive))
 		{
 			return (zProbeTriggered) ? 2 : 1;
 		}
@@ -1543,7 +1499,7 @@ int GCodes::DoZProbe(float distance)
 // then that value is used.  If it's less than SILLY_Z_VALUE the bed is
 // probed and that value is used.
 // Call this repeatedly until it returns true.
-bool GCodes::SetSingleZProbeAtAPosition(GCodeBuffer *gb, StringRef& reply)
+bool GCodes::SetSingleZProbeAtAPosition(GCodeBuffer& gb, StringRef& reply)
 {
 	if (reprap.GetMove()->IsDeltaMode() && !AllAxesAreHomed())
 	{
@@ -1553,28 +1509,28 @@ bool GCodes::SetSingleZProbeAtAPosition(GCodeBuffer *gb, StringRef& reply)
 
 	float heightAdjust = 0.0;
 	bool dummy;
-	gb->TryGetFValue('H', heightAdjust, dummy);
+	gb.TryGetFValue('H', heightAdjust, dummy);
 
-	if (!gb->Seen('P'))
+	if (!gb.Seen('P'))
 	{
 		bool reportOnly = false;
-		if (gb->Seen('S') && gb->GetIValue() < 0)
+		if (gb.Seen('S') && gb.GetIValue() < 0)
 		{
 			reportOnly = true;
 		}
-		return DoSingleZProbe(reportOnly, heightAdjust);
+		return DoSingleZProbe(gb, reportOnly, heightAdjust);
 	}
 
-	int probePointIndex = gb->GetIValue();
+	int probePointIndex = gb.GetIValue();
 	if (probePointIndex < 0 || (unsigned int)probePointIndex >= MAX_PROBE_POINTS)
 	{
 		reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Z probe point index out of range.\n");
 		return true;
 	}
 
-	float x = (gb->Seen(axisLetters[X_AXIS])) ? gb->GetFValue() : moveBuffer.coords[X_AXIS];
-	float y = (gb->Seen(axisLetters[Y_AXIS])) ? gb->GetFValue() : moveBuffer.coords[Y_AXIS];
-	float z = (gb->Seen(axisLetters[Z_AXIS])) ? gb->GetFValue() : moveBuffer.coords[Z_AXIS];
+	float x = (gb.Seen(axisLetters[X_AXIS])) ? gb.GetFValue() : moveBuffer.coords[X_AXIS];
+	float y = (gb.Seen(axisLetters[Y_AXIS])) ? gb.GetFValue() : moveBuffer.coords[Y_AXIS];
+	float z = (gb.Seen(axisLetters[Z_AXIS])) ? gb.GetFValue() : moveBuffer.coords[Z_AXIS];
 
 	reprap.GetMove()->SetXBedProbePoint(probePointIndex, x);
 	reprap.GetMove()->SetYBedProbePoint(probePointIndex, y);
@@ -1582,21 +1538,21 @@ bool GCodes::SetSingleZProbeAtAPosition(GCodeBuffer *gb, StringRef& reply)
 	if (z > SILLY_Z_VALUE)
 	{
 		reprap.GetMove()->SetZBedProbePoint(probePointIndex, z, false, false);
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
 			zProbesSet = true;
-			reprap.GetMove()->FinishedBedProbing(gb->GetIValue(), reply);
+			reprap.GetMove()->FinishedBedProbing(gb.GetIValue(), reply);
 		}
 		return true;
 	}
 	else
 	{
-		if (DoSingleZProbeAtPoint(probePointIndex, heightAdjust))
+		if (DoSingleZProbeAtPoint(gb, probePointIndex, heightAdjust))
 		{
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
 				zProbesSet = true;
-				int sParam = gb->GetIValue();
+				int sParam = gb.GetIValue();
 				if (sParam == 1)
 				{
 					// G30 with a silly Z value and S=1 is equivalent to G30 with no parameters in that it sets the current Z height
@@ -1628,22 +1584,22 @@ bool GCodes::GetProbeCoordinates(int count, float& x, float& y, float& z) const
 	return zProbesSet;
 }
 
-bool GCodes::SetPrintZProbe(GCodeBuffer* gb, StringRef& reply)
+bool GCodes::SetPrintZProbe(GCodeBuffer& gb, StringRef& reply)
 {
 	ZProbeParameters params = platform->GetZProbeParameters();
 	bool seen = false;
-	gb->TryGetFValue(axisLetters[X_AXIS], params.xOffset, seen);
-	gb->TryGetFValue(axisLetters[Y_AXIS], params.yOffset, seen);
-	gb->TryGetFValue(axisLetters[Z_AXIS], params.height, seen);
-	gb->TryGetIValue('P', params.adcValue, seen);
+	gb.TryGetFValue(axisLetters[X_AXIS], params.xOffset, seen);
+	gb.TryGetFValue(axisLetters[Y_AXIS], params.yOffset, seen);
+	gb.TryGetFValue(axisLetters[Z_AXIS], params.height, seen);
+	gb.TryGetIValue('P', params.adcValue, seen);
 
-	if (gb->Seen('C'))
+	if (gb.Seen('C'))
 	{
-		params.temperatureCoefficient = gb->GetFValue();
+		params.temperatureCoefficient = gb.GetFValue();
 		seen = true;
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			params.calibTemperature = gb->GetFValue();
+			params.calibTemperature = gb.GetFValue();
 		}
 		else
 		{
@@ -1711,7 +1667,7 @@ void GCodes::GetCurrentCoordinates(StringRef& s) const
 	}
 }
 
-bool GCodes::OpenFileToWrite(const char* directory, const char* fileName, GCodeBuffer *gb)
+bool GCodes::OpenFileToWrite(GCodeBuffer& gb, const char* directory, const char* fileName)
 {
 	fileBeingWritten = platform->GetFileStore(directory, fileName, true);
 	eofStringCounter = 0;
@@ -1722,12 +1678,12 @@ bool GCodes::OpenFileToWrite(const char* directory, const char* fileName, GCodeB
 	}
 	else
 	{
-		gb->SetWritingFileDirectory(directory);
+		gb.SetWritingFileDirectory(directory);
 		return true;
 	}
 }
 
-void GCodes::WriteHTMLToFile(char b, GCodeBuffer *gb)
+void GCodes::WriteHTMLToFile(GCodeBuffer& gb, char b)
 {
 	if (fileBeingWritten == NULL)
 	{
@@ -1748,7 +1704,7 @@ void GCodes::WriteHTMLToFile(char b, GCodeBuffer *gb)
 		{
 			fileBeingWritten->Close();
 			fileBeingWritten = NULL;
-			gb->SetWritingFileDirectory(NULL);
+			gb.SetWritingFileDirectory(NULL);
 			const char* r = (platform->Emulating() == marlin) ? "Done saving file." : "";
 			HandleReply(gb, false, r);
 			return;
@@ -1760,7 +1716,7 @@ void GCodes::WriteHTMLToFile(char b, GCodeBuffer *gb)
 	}
 }
 
-void GCodes::WriteGCodeToFile(GCodeBuffer *gb)
+void GCodes::WriteGCodeToFile(GCodeBuffer& gb)
 {
 	if (fileBeingWritten == NULL)
 	{
@@ -1769,13 +1725,13 @@ void GCodes::WriteGCodeToFile(GCodeBuffer *gb)
 	}
 
 	// End of file?
-	if (gb->Seen('M'))
+	if (gb.Seen('M'))
 	{
-		if (gb->GetIValue() == 29)
+		if (gb.GetIValue() == 29)
 		{
 			fileBeingWritten->Close();
 			fileBeingWritten = NULL;
-			gb->SetWritingFileDirectory(NULL);
+			gb.SetWritingFileDirectory(NULL);
 			const char* r = (platform->Emulating() == marlin) ? "Done saving file." : "";
 			HandleReply(gb, false, r);
 			return;
@@ -1783,20 +1739,20 @@ void GCodes::WriteGCodeToFile(GCodeBuffer *gb)
 	}
 
 	// Resend request?
-	if (gb->Seen('G'))
+	if (gb.Seen('G'))
 	{
-		if (gb->GetIValue() == 998)
+		if (gb.GetIValue() == 998)
 		{
-			if (gb->Seen('P'))
+			if (gb.Seen('P'))
 			{
-				scratchString.printf("%d\n", gb->GetIValue());
+				scratchString.printf("%d\n", gb.GetIValue());
 				HandleReply(gb, false, scratchString.Pointer());
 				return;
 			}
 		}
 	}
 
-	fileBeingWritten->Write(gb->Buffer());
+	fileBeingWritten->Write(gb.Buffer());
 	fileBeingWritten->Write('\n');
 	HandleReply(gb, false, "");
 }
@@ -1841,16 +1797,16 @@ void GCodes::DeleteFile(const char* fileName)
 }
 
 // Function to handle dwell delays.  Return true for dwell finished, false otherwise.
-bool GCodes::DoDwell(GCodeBuffer *gb)
+bool GCodes::DoDwell(GCodeBuffer& gb)
 {
 	float dwell;
-	if (gb->Seen('S'))
+	if (gb.Seen('S'))
 	{
-		dwell = gb->GetFValue();
+		dwell = gb.GetFValue();
 	}
-	else if (gb->Seen('P'))
+	else if (gb.Seen('P'))
 	{
-		dwell = 0.001 * (float) gb->GetIValue(); // P values are in milliseconds; we need seconds
+		dwell = 0.001 * (float) gb.GetIValue(); // P values are in milliseconds; we need seconds
 	}
 	else
 	{
@@ -1861,12 +1817,12 @@ bool GCodes::DoDwell(GCodeBuffer *gb)
 	// Deal with a Roland configuration
 	if (reprap.GetRoland()->Active())
 	{
-		return reprap.GetRoland()->ProcessDwell(gb->GetLValue());
+		return reprap.GetRoland()->ProcessDwell(gb.GetLValue());
 	}
 #endif
 
 	// Wait for all the queued moves to stop
-	if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+	if (!LockMovementAndWaitForStandstill(gb))
 	{
 		return false;
 	}
@@ -1903,17 +1859,17 @@ bool GCodes::DoDwellTime(float dwell)
 }
 
 // Set offset, working and standby temperatures for a tool. I.e. handle a G10.
-void GCodes::SetOrReportOffsets(StringRef& reply, GCodeBuffer *gb)
+bool GCodes::SetOrReportOffsets(GCodeBuffer &gb, StringRef& reply)
 {
-	if (gb->Seen('P'))
+	if (gb.Seen('P'))
 	{
-		int8_t toolNumber = gb->GetIValue();
-		toolNumber += gb->GetToolNumberAdjust();
+		int8_t toolNumber = gb.GetIValue();
+		toolNumber += gb.GetToolNumberAdjust();
 		Tool* tool = reprap.GetTool(toolNumber);
 		if (tool == NULL)
 		{
 			reply.printf("Attempt to set/report offsets and temperatures for non-existent tool: %d", toolNumber);
-			return;
+			return true;
 		}
 
 		// Deal with setting offsets
@@ -1926,10 +1882,14 @@ void GCodes::SetOrReportOffsets(StringRef& reply, GCodeBuffer *gb)
 		bool settingOffset = false;
 		for (size_t axis = 0; axis < numAxes; ++axis)
 		{
-			gb->TryGetFValue(axisLetters[axis], offset[axis], settingOffset);
+			gb.TryGetFValue(axisLetters[axis], offset[axis], settingOffset);
 		}
 		if (settingOffset)
 		{
+			if (!LockMovement(gb))
+			{
+				return false;
+			}
 			tool->SetOffset(offset);
 		}
 
@@ -1941,14 +1901,14 @@ void GCodes::SetOrReportOffsets(StringRef& reply, GCodeBuffer *gb)
 		if (hCount > 0)
 		{
 			tool->GetVariables(standby, active);
-			if (gb->Seen('R'))
+			if (gb.Seen('R'))
 			{
-				gb->GetFloatArray(standby, hCount, true);
+				gb.GetFloatArray(standby, hCount, true);
 				settingTemps = true;
 			}
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
-				gb->GetFloatArray(active, hCount, true);
+				gb.GetFloatArray(active, hCount, true);
 				settingTemps = true;
 			}
 
@@ -1976,24 +1936,25 @@ void GCodes::SetOrReportOffsets(StringRef& reply, GCodeBuffer *gb)
 			}
 		}
 	}
+	return true;
 }
 
-void GCodes::ManageTool(GCodeBuffer *gb, StringRef& reply)
+void GCodes::ManageTool(GCodeBuffer& gb, StringRef& reply)
 {
-	if (!gb->Seen('P'))
+	if (!gb.Seen('P'))
 	{
 		// DC temporary code to allow tool numbers to be adjusted so that we don't need to edit multi-media files generated by slic3r
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			int adjust = gb->GetIValue();
-			gb->SetToolNumberAdjust(adjust);
+			int adjust = gb.GetIValue();
+			gb.SetToolNumberAdjust(adjust);
 		}
 		return;
 	}
 
 	// Check tool number
 	bool seen = false;
-	const int toolNumber = gb->GetIValue();
+	const int toolNumber = gb.GetIValue();
 	if (toolNumber < 0)
 	{
 		platform->Message(GENERIC_MESSAGE, "Tool number must be positive!\n");
@@ -2003,9 +1964,9 @@ void GCodes::ManageTool(GCodeBuffer *gb, StringRef& reply)
 	// Check drives
 	long drives[MaxExtruders];  		// There can never be more than we have...
 	size_t dCount = numExtruders;	// Sets the limit and returns the count
-	if (gb->Seen('D'))
+	if (gb.Seen('D'))
 	{
-		gb->GetLongArray(drives, dCount);
+		gb.GetLongArray(drives, dCount);
 		seen = true;
 	}
 	else
@@ -2016,9 +1977,9 @@ void GCodes::ManageTool(GCodeBuffer *gb, StringRef& reply)
 	// Check heaters
 	long heaters[HEATERS];
 	size_t hCount = HEATERS;
-	if (gb->Seen('H'))
+	if (gb.Seen('H'))
 	{
-		gb->GetLongArray(heaters, hCount);
+		gb.GetLongArray(heaters, hCount);
 		seen = true;
 	}
 	else
@@ -2029,9 +1990,9 @@ void GCodes::ManageTool(GCodeBuffer *gb, StringRef& reply)
 	// Check X axis mapping
 	long xMapping[MAX_AXES];
 	size_t xCount = numAxes;
-	if (gb->Seen('X'))
+	if (gb.Seen('X'))
 	{
-		gb->GetLongArray(xMapping, xCount);
+		gb.GetLongArray(xMapping, xCount);
 		seen = true;
 	}
 	else
@@ -2042,11 +2003,11 @@ void GCodes::ManageTool(GCodeBuffer *gb, StringRef& reply)
 
 	// Check for fan mapping
 	uint32_t fanMap;
-	if (gb->Seen('F'))
+	if (gb.Seen('F'))
 	{
 		long fanMapping[NUM_FANS];
 		size_t fanCount = NUM_FANS;
-		gb->GetLongArray(fanMapping, fanCount);
+		gb.GetLongArray(fanMapping, fanCount);
 		fanMap = 0;
 		for (size_t i = 0; i < fanCount; ++i)
 		{
@@ -2099,10 +2060,10 @@ void GCodes::DisableDrives()
 }
 
 // Does what it says.
-void GCodes::SetEthernetAddress(GCodeBuffer *gb, int mCode)
+void GCodes::SetEthernetAddress(GCodeBuffer& gb, int mCode)
 {
 	byte eth[4];
-	const char* ipString = gb->GetString();
+	const char* ipString = gb.GetString();
 	uint8_t sp = 0;
 	uint8_t spp = 0;
 	uint8_t ipp = 0;
@@ -2114,7 +2075,7 @@ void GCodes::SetEthernetAddress(GCodeBuffer *gb, int mCode)
 			ipp++;
 			if (ipp > 3)
 			{
-				platform->MessageF(GENERIC_MESSAGE, "Dud IP address: %s\n", gb->Buffer());
+				platform->MessageF(GENERIC_MESSAGE, "Dud IP address: %s\n", gb.Buffer());
 				return;
 			}
 			sp++;
@@ -2146,14 +2107,14 @@ void GCodes::SetEthernetAddress(GCodeBuffer *gb, int mCode)
 	}
 	else
 	{
-		platform->MessageF(GENERIC_MESSAGE, "Dud IP address: %s\n", gb->Buffer());
+		platform->MessageF(GENERIC_MESSAGE, "Dud IP address: %s\n", gb.Buffer());
 	}
 }
 
-void GCodes::SetMACAddress(GCodeBuffer *gb)
+void GCodes::SetMACAddress(GCodeBuffer& gb)
 {
 	uint8_t mac[6];
-	const char* ipString = gb->GetString();
+	const char* ipString = gb.GetString();
 	uint8_t sp = 0;
 	uint8_t spp = 0;
 	uint8_t ipp = 0;
@@ -2165,7 +2126,7 @@ void GCodes::SetMACAddress(GCodeBuffer *gb)
 			ipp++;
 			if (ipp > 5)
 			{
-				platform->MessageF(GENERIC_MESSAGE, "Dud MAC address: %s\n", gb->Buffer());
+				platform->MessageF(GENERIC_MESSAGE, "Dud MAC address: %s\n", gb.Buffer());
 				return;
 			}
 			sp++;
@@ -2183,7 +2144,7 @@ void GCodes::SetMACAddress(GCodeBuffer *gb)
 	}
 	else
 	{
-		platform->MessageF(GENERIC_MESSAGE, "Dud MAC address: %s\n", gb->Buffer());
+		platform->MessageF(GENERIC_MESSAGE, "Dud MAC address: %s\n", gb.Buffer());
 	}
 }
 
@@ -2227,38 +2188,38 @@ void GCodes::SetMappedFanSpeed()
 // Handle sending a reply back to the appropriate interface(s).
 // Note that 'reply' may be empty. If it isn't, then we need to append newline when sending it.
 // Also, gb may be null if we were executing a trigger macro.
-void GCodes::HandleReply(GCodeBuffer *gb, bool error, const char* reply)
+void GCodes::HandleReply(GCodeBuffer& gb, bool error, const char* reply)
 {
 	// Don't report "ok" responses if a (macro) file is being processed
 	// Also check that this response was triggered by a gcode
-	if (gb == nullptr || ((gb == fileMacroGCode || gb == fileGCode) && reply[0] == 0))
+	if ((gb.MachineState().doingFileMacro || &gb == fileGCode) && reply[0] == 0)
 	{
 		return;
 	}
 
 	// Second UART device, e.g. dc42's PanelDue. Do NOT use emulation for this one!
-	if (gb == auxGCode || (stackPointer != 0 && stack[0].gb == auxGCode))
+	if (&gb == auxGCode)
 	{
 		platform->AppendAuxReply(reply);
 		return;
 	}
 
-	const Compatibility c = (gb == serialGCode || gb == telnetGCode) ? platform->Emulating() : me;
+	const Compatibility c = (&gb == serialGCode || &gb == telnetGCode) ? platform->Emulating() : me;
 	MessageType type = GENERIC_MESSAGE;
-	if (gb == httpGCode)
+	if (&gb == httpGCode)
 	{
 		type = HTTP_MESSAGE;
 	}
-	else if (gb == telnetGCode)
+	else if (&gb == telnetGCode)
 	{
 		type = TELNET_MESSAGE;
 	}
-	else if (gb == serialGCode)
+	else if (&gb == serialGCode)
 	{
 		type = HOST_MESSAGE;
 	}
 
-	const char* response = (gb->Seen('M') && gb->GetIValue() == 998) ? "rs " : "ok";
+	const char* response = (gb.Seen('M') && gb.GetIValue() == 998) ? "rs " : "ok";
 	const char* emulationType = 0;
 
 	switch (c)
@@ -2275,7 +2236,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, const char* reply)
 
 		case marlin:
 			// We don't need to handle M20 here because we always allocate an output buffer for that one
-			if (gb->Seen('M') && gb->GetIValue() == 28)
+			if (gb.Seen('M') && gb.GetIValue() == 28)
 			{
 				platform->Message(type, response);
 				platform->Message(type, "\n");
@@ -2284,7 +2245,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, const char* reply)
 				return;
 			}
 
-			if ((gb->Seen('M') && gb->GetIValue() == 105) || (gb->Seen('M') && gb->GetIValue() == 998))
+			if ((gb.Seen('M') && gb.GetIValue() == 105) || (gb.Seen('M') && gb.GetIValue() == 998))
 			{
 				platform->Message(type, response);
 				platform->Message(type, " ");
@@ -2293,7 +2254,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, const char* reply)
 				return;
 			}
 
-			if (reply[0] != 0 && !DoingFileMacro())
+			if (reply[0] != 0 && !gb.IsDoingFileMacro())
 			{
 				platform->Message(type, reply);
 				platform->Message(type, "\n");
@@ -2331,37 +2292,37 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, const char* reply)
 	}
 }
 
-void GCodes::HandleReply(GCodeBuffer *gb, bool error, OutputBuffer *reply)
+void GCodes::HandleReply(GCodeBuffer& gb, bool error, OutputBuffer *reply)
 {
 	// Although unlikely, it's possible that we get a nullptr reply. Don't proceed if this is the case
-	if (gb == nullptr || reply == nullptr)
+	if (reply == nullptr)
 	{
 		return;
 	}
 
 	// Second UART device, e.g. dc42's PanelDue. Do NOT use emulation for this one!
-	if (gb == auxGCode || (stackPointer != 0 && stack[0].gb == auxGCode))
+	if (&gb == auxGCode)
 	{
 		platform->AppendAuxReply(reply);
 		return;
 	}
 
-	const Compatibility c = (gb == serialGCode || gb == telnetGCode) ? platform->Emulating() : me;
+	const Compatibility c = (&gb == serialGCode || &gb == telnetGCode) ? platform->Emulating() : me;
 	MessageType type = GENERIC_MESSAGE;
-	if (gb == httpGCode)
+	if (&gb == httpGCode)
 	{
 		type = HTTP_MESSAGE;
 	}
-	else if (gb == telnetGCode)
+	else if (&gb == telnetGCode)
 	{
 		type = TELNET_MESSAGE;
 	}
-	else if (gb == serialGCode)
+	else if (&gb == serialGCode)
 	{
 		type = HOST_MESSAGE;
 	}
 
-	const char* response = (gb->Seen('M') && gb->GetIValue() == 998) ? "rs " : "ok";
+	const char* response = (gb.Seen('M') && gb.GetIValue() == 998) ? "rs " : "ok";
 	const char* emulationType = nullptr;
 
 	switch (c)
@@ -2376,7 +2337,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, OutputBuffer *reply)
 			return;
 
 		case marlin:
-			if (gb->Seen('M') && gb->GetIValue() == 20)
+			if (gb.Seen('M') && gb.GetIValue() == 20)
 			{
 				platform->Message(type, "Begin file list\n");
 				platform->Message(type, reply);
@@ -2386,7 +2347,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, OutputBuffer *reply)
 				return;
 			}
 
-			if (gb->Seen('M') && gb->GetIValue() == 28)
+			if (gb.Seen('M') && gb.GetIValue() == 28)
 			{
 				platform->Message(type, response);
 				platform->Message(type, "\n");
@@ -2394,7 +2355,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, OutputBuffer *reply)
 				return;
 			}
 
-			if ((gb->Seen('M') && gb->GetIValue() == 105) || (gb->Seen('M') && gb->GetIValue() == 998))
+			if ((gb.Seen('M') && gb.GetIValue() == 105) || (gb.Seen('M') && gb.GetIValue() == 998))
 			{
 				platform->Message(type, response);
 				platform->Message(type, " ");
@@ -2402,7 +2363,7 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, OutputBuffer *reply)
 				return;
 			}
 
-			if (reply->Length() != 0 && !DoingFileMacro())
+			if (reply->Length() != 0 && !gb.IsDoingFileMacro())
 			{
 				platform->Message(type, reply);
 				platform->Message(type, "\n");
@@ -2443,22 +2404,22 @@ void GCodes::HandleReply(GCodeBuffer *gb, bool error, OutputBuffer *reply)
 }
 
 // Set PID parameters (M301 or M304 command). 'heater' is the default heater number to use.
-void GCodes::SetPidParameters(GCodeBuffer *gb, int heater, StringRef& reply)
+void GCodes::SetPidParameters(GCodeBuffer& gb, int heater, StringRef& reply)
 {
-	if (gb->Seen('H'))
+	if (gb.Seen('H'))
 	{
-		heater = gb->GetIValue();
+		heater = gb.GetIValue();
 	}
 
 	if (heater >= 0 && heater < HEATERS)
 	{
 		PidParameters pp = platform->GetPidParameters(heater);
 		bool seen = false;
-		gb->TryGetFValue('P', pp.kP, seen);
-		gb->TryGetFValue('I', pp.kI, seen);
-		gb->TryGetFValue('D', pp.kD, seen);
-		gb->TryGetFValue('T', pp.kT, seen);
-		gb->TryGetFValue('S', pp.kS, seen);
+		gb.TryGetFValue('P', pp.kP, seen);
+		gb.TryGetFValue('I', pp.kI, seen);
+		gb.TryGetFValue('D', pp.kD, seen);
+		gb.TryGetFValue('T', pp.kT, seen);
+		gb.TryGetFValue('S', pp.kS, seen);
 
 		if (seen)
 		{
@@ -2472,11 +2433,11 @@ void GCodes::SetPidParameters(GCodeBuffer *gb, int heater, StringRef& reply)
 	}
 }
 
-void GCodes::SetHeaterParameters(GCodeBuffer *gb, StringRef& reply)
+void GCodes::SetHeaterParameters(GCodeBuffer& gb, StringRef& reply)
 {
-	if (gb->Seen('P'))
+	if (gb.Seen('P'))
 	{
-		int heater = gb->GetIValue();
+		int heater = gb.GetIValue();
 		if (heater >= 0 && heater < HEATERS)
 		{
 			Thermistor& th = platform->GetThermistor(heater);
@@ -2488,29 +2449,29 @@ void GCodes::SetHeaterParameters(GCodeBuffer *gb, StringRef& reply)
 			float shC = th.GetShc();
 			float seriesR = th.GetSeriesR();
 
-			gb->TryGetFValue('T', r25, seen);
-			gb->TryGetFValue('B', beta, seen);
-			gb->TryGetFValue('C', shC, seen);
-			gb->TryGetFValue('R', seriesR, seen);
+			gb.TryGetFValue('T', r25, seen);
+			gb.TryGetFValue('B', beta, seen);
+			gb.TryGetFValue('C', shC, seen);
+			gb.TryGetFValue('R', seriesR, seen);
 			if (seen)
 			{
 				th.SetParameters(r25, beta, shC, seriesR);
 			}
 
-			if (gb->Seen('L'))
+			if (gb.Seen('L'))
 			{
-				th.SetLowOffset((int8_t)constrain<int>(gb->GetIValue(), -100, 100));
+				th.SetLowOffset((int8_t)constrain<int>(gb.GetIValue(), -100, 100));
 				seen = true;
 			}
-			if (gb->Seen('H'))
+			if (gb.Seen('H'))
 			{
-				th.SetHighOffset((int8_t)constrain<int>(gb->GetIValue(), -100, 100));
+				th.SetHighOffset((int8_t)constrain<int>(gb.GetIValue(), -100, 100));
 				seen = true;
 			}
 
-			if (gb->Seen('X'))
+			if (gb.Seen('X'))
 			{
-				int thermistor = gb->GetIValue();
+				int thermistor = gb.GetIValue();
 				if (   (0 <= thermistor && thermistor < HEATERS)
 					|| ((int)FirstThermocoupleChannel <= thermistor && thermistor < (int)(FirstThermocoupleChannel + MaxSpiTempSensors))
 					|| ((int)FirstRtdChannel <= thermistor && thermistor < (int)(FirstRtdChannel + MaxSpiTempSensors))
@@ -2603,30 +2564,28 @@ bool GCodes::RetractFilament(bool retract)
 // If the code to act on is completed, this returns true,
 // otherwise false.  It is called repeatedly for a given
 // code until it returns true for that code.
-bool GCodes::ActOnCode(GCodeBuffer *gb, StringRef& reply)
+bool GCodes::ActOnCode(GCodeBuffer& gb, StringRef& reply)
 {
 	// Discard empty buffers right away
-	if (gb->IsEmpty())
+	if (gb.IsEmpty())
 	{
 		return true;
 	}
 
-	gbCurrent = gb;
-
 	// M-code parameters might contain letters T and G, e.g. in filenames.
 	// dc42 assumes that G-and T-code parameters never contain the letter M.
 	// Therefore we must check for an M-code first.
-	if (gb->Seen('M'))
+	if (gb.Seen('M'))
 	{
 		return HandleMcode(gb, reply);
 	}
 	// dc42 doesn't think a G-code parameter ever contains letter T, or a T-code ever contains letter G.
 	// So it doesn't matter in which order we look for them.
-	if (gb->Seen('G'))
+	if (gb.Seen('G'))
 	{
 		return HandleGcode(gb, reply);
 	}
-	if (gb->Seen('T'))
+	if (gb.Seen('T'))
 	{
 		return HandleTcode(gb, reply);
 	}
@@ -2636,12 +2595,12 @@ bool GCodes::ActOnCode(GCodeBuffer *gb, StringRef& reply)
 	return true;
 }
 
-bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
+bool GCodes::HandleGcode(GCodeBuffer& gb, StringRef& reply)
 {
 	bool result = true;
 	bool error = false;
 
-	int code = gb->GetIValue();
+	int code = gb.GetIValue();
 	if (simulationMode != 0 && code != 0 && code != 1 && code != 4 && code != 10 && code != 20 && code != 21 && code != 90 && code != 91 && code != 92)
 	{
 		return true;			// we only simulate some gcodes
@@ -2651,12 +2610,16 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 	{
 	case 0: // There are no rapid moves...
 	case 1: // Ordinary move
+		if (!LockMovement(gb))
+		{
+			return false;
+		}
 		{
 			// Check for 'R' parameter here to go back to the coordinates at which the print was paused
 			// NOTE: restore point 2 (tool change) won't work when changing tools on dual axis machines because of X axis mapping.
 			// We could possibly fix this by saving the virtual X axis position instead of the physical axis positions.
 			// However, slicers normally command the tool to the correct place after a tool change, so we don't need this feature anyway.
-			int rParam = (gb->Seen('R')) ? gb->GetIValue() : 0;
+			int rParam = (gb.Seen('R')) ? gb.GetIValue() : 0;
 			RestorePoint *rp = (rParam == 1) ? &pauseRestorePoint : (rParam == 2) ? &toolChangeRestorePoint : nullptr;
 			if (rp != nullptr)
 			{
@@ -2666,7 +2629,7 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 				}
 				for (size_t axis = 0; axis < numAxes; ++axis)
 				{
-					float offset = gb->Seen(axisLetters[axis]) ? gb->GetFValue() * distanceScale : 0.0;
+					float offset = gb.Seen(axisLetters[axis]) ? gb.GetFValue() * distanceScale : 0.0;
 					moveBuffer.coords[axis] = rp->moveCoords[axis] + offset;
 				}
 				// For now we don't handle extrusion at the same time
@@ -2674,7 +2637,7 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 				{
 					moveBuffer.coords[drive] = 0.0;
 				}
-				moveBuffer.feedRate = (gb->Seen(feedrateLetter)) ? gb->GetFValue() : feedRate;
+				moveBuffer.feedRate = (gb.Seen(feedrateLetter)) ? gb.GetFValue() : feedRate;
 				moveBuffer.filePos = noFilePosition;
 				moveBuffer.usePressureAdvance = false;
 				moveAvailable = true;
@@ -2684,7 +2647,7 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 				int res = SetUpMove(gb, reply);
 				if (res == 2)
 				{
-					state = GCodeState::waitingForMoveToComplete;
+					gb.MachineState().state = GCodeState::waitingForMoveToComplete;
 				}
 				result = (res != 0);
 			}
@@ -2696,17 +2659,28 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 10: // Set/report offsets and temperatures, or retract
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			SetOrReportOffsets(reply, gb);
+			if (!SetOrReportOffsets(gb, reply))
+			{
+				return false;
+			}
 		}
 		else
 		{
+			if (!LockMovement(gb))
+			{
+				return false;
+			}
 			result = RetractFilament(true);
 		}
 		break;
 
 	case 11: // Un-retract
+		if (!LockMovement(gb))
+		{
+			return false;
+		}
 		result = RetractFilament(false);
 		break;
 
@@ -2723,7 +2697,7 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 30: // Z probe/manually set at a position and set that as point P
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -2739,7 +2713,7 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 31: // Return the probe value, or set probe variables
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -2747,18 +2721,18 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 32: // Probe Z at multiple positions and generate the bed transform
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
 
 		// Try to execute bed.g
-		if (!DoFileMacro(BED_EQUATION_G, reprap.GetMove()->IsDeltaMode()))
+		if (!DoFileMacro(gb, BED_EQUATION_G, reprap.GetMove()->IsDeltaMode()))
 		{
 			// If we get here then we are not on a delta printer and there is no bed.g file
 			if (GetAxisIsHomed(X_AXIS) && GetAxisIsHomed(Y_AXIS))
 			{
-				state = GCodeState::setBed1;		// no bed.g file, so use the coordinates specified by M557
+				gb.MachineState().state = GCodeState::setBed1;		// no bed.g file, so use the coordinates specified by M557
 			}
 			else
 			{
@@ -2770,15 +2744,11 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 90: // Absolute coordinates
-		// DC 2014-07-21 we no longer change the extruder settings in response to G90/G91 commands
-		//drivesRelative = false;
-		axesRelative = false;
+		gb.MachineState().axesRelative = false;
 		break;
 
 	case 91: // Relative coordinates
-		// DC 2014-07-21 we no longer change the extruder settings in response to G90/G91 commands
-		//drivesRelative = true; // Non-axis movements (i.e. extruders)
-		axesRelative = true;   // Axis movements (i.e. X, Y and Z)
+		gb.MachineState().axesRelative = true;   // Axis movements (i.e. X, Y and Z)
 		break;
 
 	case 92: // Set position
@@ -2787,21 +2757,23 @@ bool GCodes::HandleGcode(GCodeBuffer* gb, StringRef& reply)
 
 	default:
 		error = true;
-		reply.printf("invalid G Code: %s", gb->Buffer());
+		reply.printf("invalid G Code: %s", gb.Buffer());
 	}
-	if (result && state == GCodeState::normal)
+
+	if (result && gb.MachineState().state == GCodeState::normal)
 	{
+		UnlockAll(gb);
 		HandleReply(gb, error, reply.Pointer());
 	}
 	return result;
 }
 
-bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
+bool GCodes::HandleMcode(GCodeBuffer& gb, StringRef& reply)
 {
 	bool result = true;
 	bool error = false;
 
-	int code = gb->GetIValue();
+	int code = gb.GetIValue();
 	if (simulationMode != 0 && (code < 20 || code > 37) && code != 0 && code != 1 && code != 82 && code != 83 && code != 105 && code != 111 && code != 112 && code != 122 && code != 408 && code != 999)
 	{
 		return true;			// we don't yet simulate most M codes
@@ -2811,7 +2783,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	{
 	case 0: // Stop
 	case 1: // Sleep
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -2824,7 +2796,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				// If we are cancelling a paused print with M0 and cancel.g exists then run it and do nothing else
 				if (code == 0)
 				{
-					if (DoFileMacro(CANCEL_G, false))
+					if (DoFileMacro(gb, CANCEL_G, false))
 					{
 						break;
 					}
@@ -2832,17 +2804,17 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 		}
 
-		state = (code == 0) ? GCodeState::stopping : GCodeState::sleeping;
-		DoFileMacro((code == 0) ? STOP_G : SLEEP_G, false);
+		gb.MachineState().state = (code == 0) ? GCodeState::stopping : GCodeState::sleeping;
+		DoFileMacro(gb, (code == 0) ? STOP_G : SLEEP_G, false);
 		break;
 
 #if SUPPORT_ROLAND
 	case 3: // Spin spindle
 		if (reprap.GetRoland()->Active())
 		{
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
-				result = reprap.GetRoland()->ProcessSpindle(gb->GetFValue());
+				result = reprap.GetRoland()->ProcessSpindle(gb.GetFValue());
 			}
 		}
 		break;
@@ -2850,7 +2822,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 18: // Motors off
 	case 84:
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -2858,7 +2830,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
 					SetAxisNotHomed(axis);
 					platform->DisableDrive(axis);
@@ -2866,11 +2838,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 
-			if (gb->Seen(extrudeLetter))
+			if (gb.Seen(extrudeLetter))
 			{
 				long int eDrive[MaxExtruders];
 				size_t eCount = numExtruders;
-				gb->GetLongArray(eDrive, eCount);
+				gb.GetLongArray(eDrive, eCount);
 				for (size_t i = 0; i < eCount; i++)
 				{
 					seen = true;
@@ -2884,11 +2856,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
 				seen = true;
 
-				float idleTimeout = gb->GetFValue();
+				float idleTimeout = gb.GetFValue();
 				if (idleTimeout < 0.0)
 				{
 					reply.copy("Idle timeouts cannot be negative!");
@@ -2908,10 +2880,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 20:		// List files on SD card
+		if (!LockFileSystem(gb))		// don't allow more than one at a time to avoid contention on output buffers
+		{
+			return false;
+		}
 		{
 			OutputBuffer *fileResponse;
-			int sparam = (gb->Seen('S')) ? gb->GetIValue() : 0;
-			const char* dir = (gb->Seen('P')) ? gb->GetString() : platform->GetGCodeDir();
+			const int sparam = (gb.Seen('S')) ? gb.GetIValue() : 0;
+			const char* dir = (gb.Seen('P')) ? gb.GetString() : platform->GetGCodeDir();
 
 			if (sparam == 2)
 			{
@@ -2935,7 +2911,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					fileResponse->copy("GCode files:\n");
 				}
 
-				bool encapsulateList = ((gb != serialGCode && gb != telnetGCode) || platform->Emulating() != marlin);
+				bool encapsulateList = ((&gb != serialGCode && &gb != telnetGCode) || platform->Emulating() != marlin);
 				FileInfo fileInfo;
 				if (platform->GetMassStorage()->FindFirst(dir, fileInfo))
 				{
@@ -2963,46 +2939,55 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 
+			UnlockAll(gb);
 			HandleReply(gb, false, fileResponse);
 			return true;
 		}
 
 	case 21: // Initialise SD card
+		if (!LockFileSystem(gb))		// don't allow more than one at a time to avoid contention on output buffers
+		{
+			return false;
+		}
 		{
-			size_t card = (gb->Seen('P')) ? gb->GetIValue() : 0;
+			size_t card = (gb.Seen('P')) ? gb.GetIValue() : 0;
 			result = platform->GetMassStorage()->Mount(card, reply, true);
 		}
 		break;
 
 	case 22: // Release SD card
+		if (!LockFileSystem(gb))		// don't allow more than one at a time to avoid contention on output buffers
 		{
-			size_t card = (gb->Seen('P')) ? gb->GetIValue() : 0;
+			return false;
+		}
+		{
+			size_t card = (gb.Seen('P')) ? gb.GetIValue() : 0;
 			result = platform->GetMassStorage()->Unmount(card, reply);
 		}
 		break;
 
 	case 23: // Set file to print
 	case 32: // Select file and start SD print
-		if (isPaused)
+		if (fileGCode->OriginalMachineState().fileState.IsLive())
 		{
-			reply.copy("Cannot set file to print, because another print is still paused. Run M0 or M1 first.");
+			reply.copy("Cannot set file to print, because a file is already being printed");
+			error = true;
 			break;
 		}
 
-		if (code == 32 && !AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (code == 32 && !LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
-
 		{
-			const char* filename = gb->GetUnprecedentedString();
+			const char* filename = gb.GetUnprecedentedString();
 			if (filename != nullptr)
 			{
 				QueueFileToPrint(filename);
 				if (fileToPrint.IsLive())
 				{
 					reprap.GetPrintMonitor()->StartingPrint(filename);
-					if (platform->Emulating() == marlin && (gb == serialGCode || gb == telnetGCode))
+					if (platform->Emulating() == marlin && (&gb == serialGCode || &gb == telnetGCode))
 					{
 						reply.copy("File opened\nFile selected");
 					}
@@ -3014,7 +2999,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 					if (code == 32)
 					{
-						fileBeingPrinted.MoveFrom(fileToPrint);
+						fileGCode->OriginalMachineState().fileState.MoveFrom(fileToPrint);
 						reprap.GetPrintMonitor()->StartedPrint();
 					}
 				}
@@ -3027,15 +3012,15 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 24: // Print/resume-printing the selected file
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
 
 		if (isPaused)
 		{
-			state = GCodeState::resuming1;
-			DoFileMacro(RESUME_G);
+			gb.MachineState().state = GCodeState::resuming1;
+			DoFileMacro(gb, RESUME_G);
 		}
 		else if (!fileToPrint.IsLive())
 		{
@@ -3044,14 +3029,16 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		}
 		else
 		{
-			fileBeingPrinted.MoveFrom(fileToPrint);
+			fileGCode->OriginalMachineState().fileState.MoveFrom(fileToPrint);
 			reprap.GetPrintMonitor()->StartedPrint();
 		}
 		break;
 
 	case 226: // Gcode Initiated Pause
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
+		{
 			return false;
+		}
 		// no break
 
 	case 25: // Pause the print
@@ -3065,29 +3052,24 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			reply.copy("Cannot pause print, because no file is being printed!");
 			error = true;
 		}
-		else if (doingFileMacro && gb != fileMacroGCode)
-		{
-			reply.copy("Cannot pause macro files, wait for it to complete first!");
-			error = true;
-		}
 		else
 		{
-			DoPause(code == 25 && gb != fileGCode);
+			DoPause(code == 25 && &gb != fileGCode);
 		}
 		break;
 
 	case 26: // Set SD position
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			const FilePosition value = gb->GetIValue();
+			const FilePosition value = gb.GetIValue();
 			if (value < 0)
 			{
 				reply.copy("SD positions can't be negative!");
 				error = true;
 			}
-			else if (fileBeingPrinted.IsLive())
+			else if (fileGCode->OriginalMachineState().fileState.IsLive())
 			{
-				if (!fileBeingPrinted.Seek(value))
+				if (!fileGCode->OriginalMachineState().fileState.Seek(value))
 				{
 					reply.copy("The specified SD position is invalid!");
 					error = true;
@@ -3118,6 +3100,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		if (reprap.GetPrintMonitor()->IsPrinting())
 		{
 			// Pronterface keeps sending M27 commands if "Monitor status" is checked, and it specifically expects the following response syntax
+			FileData& fileBeingPrinted = fileGCode->OriginalMachineState().fileState;
 			reply.printf("SD printing byte %lu/%lu", fileBeingPrinted.GetPosition(), fileBeingPrinted.Length());
 		}
 		else
@@ -3128,10 +3111,10 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 28: // Write to file
 		{
-			const char* str = gb->GetUnprecedentedString();
+			const char* str = gb.GetUnprecedentedString();
 			if (str != nullptr)
 			{
-				bool ok = OpenFileToWrite(platform->GetGCodeDir(), str, gb);
+				bool ok = OpenFileToWrite(gb, platform->GetGCodeDir(), str);
 				if (ok)
 				{
 					reply.printf("Writing to file: %s", str);
@@ -3151,7 +3134,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 30:	// Delete file
 		{
-			const char *filename = gb->GetUnprecedentedString();
+			const char *filename = gb.GetUnprecedentedString();
 			if (filename != nullptr)
 			{
 				DeleteFile(filename);
@@ -3162,13 +3145,18 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		// For case 32, see case 24
 
 	case 36:	// Return file information
+		if (!LockFileSystem(gb))									// getting file info takes several calls and isn't reentrant
+		{
+			return false;
+		}
 		{
-			const char* filename = gb->GetUnprecedentedString(true);	// get filename, or nullptr if none provided
+			const char* filename = gb.GetUnprecedentedString(true);	// get filename, or nullptr if none provided
 			OutputBuffer *fileInfoResponse;
 			result = reprap.GetPrintMonitor()->GetFileInfoResponse(filename, fileInfoResponse);
 			if (result)
 			{
 				fileInfoResponse->cat('\n');
+				UnlockAll(gb);
 				HandleReply(gb, false, fileInfoResponse);
 				return true;
 			}
@@ -3176,15 +3164,15 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 37:	// Simulation mode on/off
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+			if (!LockMovementAndWaitForStandstill(gb))
 			{
 				return false;
 			}
 
 			bool wasSimulating = (simulationMode != 0);
-			simulationMode = (uint8_t)gb->GetIValue();
+			simulationMode = (uint8_t)gb.GetIValue();
 			reprap.GetMove()->Simulate(simulationMode);
 
 			if (simulationMode != 0)
@@ -3216,10 +3204,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 38: // Report SHA1 of file
+		if (!LockFileSystem(gb))								// getting file hash takes several calls and isn't reentrant
+		{
+			return false;
+		}
 		if (fileBeingHashed == nullptr)
 		{
 			// See if we can open the file and start hashing
-			const char* filename = gb->GetUnprecedentedString(true);
+			const char* filename = gb.GetUnprecedentedString(true);
 			if (StartHash(filename))
 			{
 				// Hashing is now in progress...
@@ -3239,16 +3231,16 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 42:	// Turn an output pin on or off
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			const int logicalPin = gb->GetIValue();
+			const int logicalPin = gb.GetIValue();
 			Pin pin;
 			bool invert;
 			if (platform->GetFirmwarePin(logicalPin, PinAccess::pwm, pin, invert))
 			{
-				if (gb->Seen('S'))
+				if (gb.Seen('S'))
 				{
-					float val = gb->GetFValue();
+					float val = gb.GetFValue();
 					if (val > 1.0)
 					{
 						val /= 255.0;
@@ -3275,30 +3267,32 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 81:	// ATX power off
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
+		{
 			return false;
+		}
 		platform->SetAtxPower(false);
 		break;
 
 	case 82:	// Use absolute extruder positioning
-		if (drivesRelative)		// don't reset the absolute extruder position if it was already absolute
+		if (gb.MachineState().drivesRelative)		// don't reset the absolute extruder position if it was already absolute
 		{
 			for (size_t extruder = 0; extruder < MaxExtruders; extruder++)
 			{
 				lastRawExtruderPosition[extruder] = 0.0;
 			}
-			drivesRelative = false;
+			gb.MachineState().drivesRelative = false;
 		}
 		break;
 
 	case 83:	// Use relative extruder positioning
-		if (!drivesRelative)	// don't reset the absolute extruder position if it was already relative
+		if (!gb.MachineState().drivesRelative)	// don't reset the absolute extruder position if it was already relative
 		{
 			for (size_t extruder = 0; extruder < MaxExtruders; extruder++)
 			{
 				lastRawExtruderPosition[extruder] = 0.0;
 			}
-			drivesRelative = true;
+			gb.MachineState().drivesRelative = true;
 		}
 		break;
 
@@ -3308,10 +3302,6 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 92: // Set/report steps/mm for some axes
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
-		{
-			return false;
-		}
 		{
 			// Save the current positions as we may need them later
 			float positionNow[DRIVES];
@@ -3321,19 +3311,27 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					platform->SetDriveStepsPerUnit(axis, gb->GetFValue());
+					if (!LockMovementAndWaitForStandstill(gb))
+					{
+						return false;
+					}
+					platform->SetDriveStepsPerUnit(axis, gb.GetFValue());
 					seen = true;
 				}
 			}
 
-			if (gb->Seen(extrudeLetter))
+			if (gb.Seen(extrudeLetter))
 			{
+				if (!LockMovementAndWaitForStandstill(gb))
+				{
+					return false;
+				}
 				seen = true;
 				float eVals[MaxExtruders];
 				size_t eCount = numExtruders;
-				gb->GetFloatArray(eVals, eCount, true);
+				gb.GetFloatArray(eVals, eCount, true);
 
 				// The user may not have as many extruders as we allow for, so just set the ones for which a value is provided
 				for (size_t e = 0; e < eCount; e++)
@@ -3366,35 +3364,25 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 98: // Call Macro/Subprogram
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
-		{
-			return false;
-		}
-
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			DoFileMacro(gb->GetString());
+			DoFileMacro(gb, gb.GetString());
 		}
 		break;
 
 	case 99: // Return from Macro/Subprogram
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
-		{
-			return false;
-		}
-
-		FileMacroCyclesReturn();
+		FileMacroCyclesReturn(gb);
 		break;
 
 	case 104: // Deprecated.  This sets the active temperature of every heater of the active tool
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			float temperature = gb->GetFValue();
+			float temperature = gb.GetFValue();
 			Tool* tool;
-			if (gb->Seen('T'))
+			if (gb.Seen('T'))
 			{
-				int toolNumber = gb->GetIValue();
-				toolNumber += gb->GetToolNumberAdjust();
+				int toolNumber = gb.GetIValue();
+				toolNumber += gb.GetToolNumberAdjust();
 				tool = reprap.GetTool(toolNumber);
 			}
 			else
@@ -3446,7 +3434,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		{
 			bool seenFanNum = false;
 			int32_t fanNum = 0;			// Default to the first fan
-			gb->TryGetIValue('P', fanNum, seenFanNum);
+			gb.TryGetIValue('P', fanNum, seenFanNum);
 			if (fanNum < 0 || fanNum > (int)NUM_FANS)
 			{
 				reply.printf("Fan number %d is invalid, must be between 0 and %u", fanNum, NUM_FANS);
@@ -3456,9 +3444,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				bool seen = false;
 				Fan& fan = platform->GetFan(fanNum);
 
-				if (gb->Seen('I'))		// Invert cooling
+				if (gb.Seen('I'))		// Invert cooling
 				{
-					const int invert = gb->GetIValue();
+					const int invert = gb.GetIValue();
 					if (invert < 0)
 					{
 						fan.Disable();
@@ -3470,36 +3458,36 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					seen = true;
 				}
 
-				if (gb->Seen('F'))		// Set PWM frequency
+				if (gb.Seen('F'))		// Set PWM frequency
 				{
-					fan.SetPwmFrequency(gb->GetFValue());
+					fan.SetPwmFrequency(gb.GetFValue());
 					seen = true;
 				}
 
-				if (gb->Seen('T'))		// Set thermostatic trigger temperature
+				if (gb.Seen('T'))		// Set thermostatic trigger temperature
 				{
 					seen = true;
-					fan.SetTriggerTemperature(gb->GetFValue());
+					fan.SetTriggerTemperature(gb.GetFValue());
 				}
 
-				if (gb->Seen('B'))		// Set blip time
+				if (gb.Seen('B'))		// Set blip time
 				{
 					seen = true;
-					fan.SetBlipTime(gb->GetFValue());
+					fan.SetBlipTime(gb.GetFValue());
 				}
 
-				if (gb->Seen('L'))		// Set minimum speed
+				if (gb.Seen('L'))		// Set minimum speed
 				{
 					seen = true;
-					fan.SetMinValue(gb->GetFValue());
+					fan.SetMinValue(gb.GetFValue());
 				}
 
-				if (gb->Seen('H'))		// Set thermostatically-controller heaters
+				if (gb.Seen('H'))		// Set thermostatically-controller heaters
 				{
 					seen = true;
 					long heaters[HEATERS];
 					size_t numH = HEATERS;
-					gb->GetLongArray(heaters, numH);
+					gb.GetLongArray(heaters, numH);
 					// Note that M106 H-1 disables thermostatic mode. The following code implements that automatically.
 					uint16_t hh = 0;
 					for (size_t h = 0; h < numH; ++h)
@@ -3517,9 +3505,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					fan.SetHeatersMonitored(hh);
 				}
 
-				if (gb->Seen('S'))		// Set new fan value - process this after processing 'H' or it may not be acted on
+				if (gb.Seen('S'))		// Set new fan value - process this after processing 'H' or it may not be acted on
 				{
-					const float f = constrain<float>(gb->GetFValue(), 0.0, 255.0);
+					const float f = constrain<float>(gb.GetFValue(), 0.0, 255.0);
 					if (seen || seenFanNum)
 					{
 						platform->SetFanValue(fanNum, f);
@@ -3532,9 +3520,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 						SetMappedFanSpeed();
 					}
 				}
-				else if (gb->Seen('R'))
+				else if (gb.Seen('R'))
 				{
-					const int i = gb->GetIValue();
+					const int i = gb.GetIValue();
 					switch(i)
 					{
 					case 0:
@@ -3580,23 +3568,26 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		platform->SetFanValue(0, 0.0);		//T3P3 as deprecated only applies to fan0
 		break;
 
-	case 109: // Deprecated in RRF, but widely generated by slicers
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+	case 108: // Cancel waiting for temperature
+		if (isWaiting)
 		{
-			return false;
+			cancelWait = true;
 		}
+		break;
+
+	case 109: // Deprecated in RRF, but widely generated by slicers
 		{
 			float temperature;
 			bool waitWhenCooling;
-			if (gb->Seen('R'))
+			if (gb.Seen('R'))
 			{
 				waitWhenCooling = true;
-				temperature = gb->GetFValue();
+				temperature = gb.GetFValue();
 			}
-			else if (gb->Seen('S'))
+			else if (gb.Seen('S'))
 			{
 				waitWhenCooling = false;
-				temperature = gb->GetFValue();
+				temperature = gb.GetFValue();
 			}
 			else
 			{
@@ -3604,10 +3595,10 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 
 			Tool *tool;
-			if (gb->Seen('T'))
+			if (gb.Seen('T'))
 			{
-				int toolNumber = gb->GetIValue();
-				toolNumber += gb->GetToolNumberAdjust();
+				int toolNumber = gb.GetIValue();
+				toolNumber += gb.GetToolNumberAdjust();
 				tool = reprap.GetTool(toolNumber);
 			}
 			else
@@ -3620,7 +3611,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 			SetToolHeaters(tool, temperature);
-			result = ToolHeatersAtSetTemperatures(tool, waitWhenCooling);
+			if (cancelWait || ToolHeatersAtSetTemperatures(tool, waitWhenCooling))
+			{
+				cancelWait = isWaiting = false;
+				break;
+			}
+			// In Marlin emulation mode we should return some sort of (undocumented) message here every second...
+			isWaiting = true;
+			return false;
 		}
 		break;
 
@@ -3628,12 +3626,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 111: // Debug level
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			bool dbv = (gb->GetIValue() != 0);
-			if (gb->Seen('P'))
+			bool dbv = (gb.GetIValue() != 0);
+			if (gb.Seen('P'))
 			{
-				reprap.SetDebug(static_cast<Module>(gb->GetIValue()), dbv);
+				reprap.SetDebug(static_cast<Module>(gb.GetIValue()), dbv);
 			}
 			else
 			{
@@ -3655,9 +3653,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 115: // Print firmware version or set hardware type
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			platform->SetBoardType((BoardType)gb->GetIValue());
+			platform->SetBoardType((BoardType)gb.GetIValue());
 		}
 		else
 		{
@@ -3673,67 +3671,72 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		}
 		break;
 
-	case 116: // Wait for everything, especially set temperatures
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
-		{
-			return false;
-		}
-
+	case 116: // Wait for set temperatures
 		{
 			bool seen = false;
-			if (gb->Seen('P'))
+			if (gb.Seen('P'))
 			{
 				// Wait for the heaters associated with the specified tool to be ready
-				int toolNumber = gb->GetIValue();
-				toolNumber += gb->GetToolNumberAdjust();
-				if (!ToolHeatersAtSetTemperatures(reprap.GetTool(toolNumber), true))
+				int toolNumber = gb.GetIValue();
+				toolNumber += gb.GetToolNumberAdjust();
+				if (!cancelWait && !ToolHeatersAtSetTemperatures(reprap.GetTool(toolNumber), true))
 				{
+					isWaiting = true;
 					return false;
 				}
 				seen = true;
 			}
 
-			if (gb->Seen('H'))
+			if (gb.Seen('H'))
 			{
 				// Wait for specified heaters to be ready
 				long heaters[HEATERS];
 				size_t heaterCount = HEATERS;
-				gb->GetLongArray(heaters, heaterCount);
-				for(size_t i=0; i<heaterCount; i++)
+				gb.GetLongArray(heaters, heaterCount);
+				if (!cancelWait)
 				{
-					if (!reprap.GetHeat()->HeaterAtSetTemperature(heaters[i], true))
+					for (size_t i=0; i<heaterCount; i++)
 					{
-						return false;
+						if (!reprap.GetHeat()->HeaterAtSetTemperature(heaters[i], true))
+						{
+							isWaiting = true;
+							return false;
+						}
 					}
 				}
 				seen = true;
 			}
 
-			if (gb->Seen('C'))
+			if (gb.Seen('C'))
 			{
 				// Wait for chamber heater to be ready
 				const int8_t chamberHeater = reprap.GetHeat()->GetChamberHeater();
 				if (chamberHeater != -1)
 				{
-					if (!reprap.GetHeat()->HeaterAtSetTemperature(chamberHeater, true))
+					if (!cancelWait && !reprap.GetHeat()->HeaterAtSetTemperature(chamberHeater, true))
 					{
+						isWaiting = true;
 						return false;
 					}
 				}
 				seen = true;
 			}
 
-			if (!seen)
+			// Wait for all heaters to be ready
+			if (!seen && !cancelWait && !reprap.GetHeat()->AllHeatersAtSetTemperatures(true))
 			{
-				// Wait for all heaters to be ready
-				result = reprap.GetHeat()->AllHeatersAtSetTemperatures(true);
+				isWaiting = true;
+				return false;
 			}
+
+			// If we get here, there is nothing more to wait for
+			cancelWait = isWaiting = false;
 		}
 		break;
 
 	case 117:	// Display message
 		{
-			const char *msg = gb->GetUnprecedentedString(true);
+			const char *msg = gb.GetUnprecedentedString(true);
 			reprap.SetMessage((msg == nullptr) ? "" : msg);
 		}
 		break;
@@ -3748,27 +3751,19 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 120:
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
-		{
-			return false;
-		}
-		Push();
+		Push(gb);
 		break;
 
 	case 121:
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
-		{
-			return false;
-		}
-		Pop();
+		Pop(gb);
 		break;
 
 	case 122:
 		{
-			int val = (gb->Seen('P')) ? gb->GetIValue() : 0;
+			int val = (gb.Seen('P')) ? gb.GetIValue() : 0;
 			if (val == 0)
 			{
-				reprap.Diagnostics(gb->GetResponseMessageType());
+				reprap.Diagnostics(gb.GetResponseMessageType());
 			}
 			else
 			{
@@ -3778,9 +3773,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 135: // Set PID sample interval
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			platform->SetHeatSampleTime(gb->GetFValue() * 0.001);  // Value is in milliseconds; we want seconds
+			platform->SetHeatSampleTime(gb.GetFValue() * 0.001);  // Value is in milliseconds; we want seconds
 		}
 		else
 		{
@@ -3791,9 +3786,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 140: // Set bed temperature
 		{
 			int8_t bedHeater;
-			if (gb->Seen('H'))
+			if (gb.Seen('H'))
 			{
-				bedHeater = gb->GetIValue();
+				bedHeater = gb.GetIValue();
 				if (bedHeater < 0)
 				{
 					// Make sure we stay within reasonable boundaries...
@@ -3827,9 +3822,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 
-			if(gb->Seen('S'))
+			if(gb.Seen('S'))
 			{
-				float temperature = gb->GetFValue();
+				float temperature = gb.GetFValue();
 				if (temperature < NEARLY_ABS_ZERO)
 				{
 					reprap.GetHeat()->SwitchOff(bedHeater);
@@ -3840,9 +3835,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 					reprap.GetHeat()->Activate(bedHeater);
 				}
 			}
-			if(gb->Seen('R'))
+			if(gb.Seen('R'))
 			{
-				reprap.GetHeat()->SetStandbyTemperature(bedHeater, gb->GetFValue());
+				reprap.GetHeat()->SetStandbyTemperature(bedHeater, gb.GetFValue());
 			}
 		}
 		break;
@@ -3850,11 +3845,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 141: // Chamber temperature
 		{
 			bool seen = false;
-			if (gb->Seen('H'))
+			if (gb.Seen('H'))
 			{
 				seen = true;
 
-				int heater = gb->GetIValue();
+				int heater = gb.GetIValue();
 				if (heater < 0)
 				{
 					const int8_t currentHeater = reprap.GetHeat()->GetChamberHeater();
@@ -3876,14 +3871,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
 				seen = true;
 
 				const int8_t currentHeater = reprap.GetHeat()->GetChamberHeater();
 				if (currentHeater != -1)
 				{
-					float temperature = gb->GetFValue();
+					float temperature = gb.GetFValue();
 
 					if (temperature < NEARLY_ABS_ZERO)
 					{
@@ -3918,23 +3913,31 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 143: // Set temperature limit
-		if (gb->Seen('S'))
 		{
-			float limit = gb->GetFValue();
-			if (limit > BAD_LOW_TEMPERATURE && limit < BAD_ERROR_TEMPERATURE)
+			const int heater = (gb.Seen('H')) ? gb.GetIValue() : 1;		// default to extruder 1 if no heater number provided
+			if (heater < 0 || heater >= HEATERS)
 			{
-				platform->SetTemperatureLimit(limit);
+				reply.copy("Invalid heater number");
+				error = true;
+			}
+			else if (gb.Seen('S'))
+			{
+				const float limit = gb.GetFValue();
+				if (limit > BAD_LOW_TEMPERATURE && limit < BAD_ERROR_TEMPERATURE)
+				{
+					reprap.GetHeat()->SetTemperatureLimit(heater, limit);
+				}
+				else
+				{
+					reply.copy("Invalid temperature limit");
+					error = true;
+				}
 			}
 			else
 			{
-				reply.copy("Invalid temperature limit");
-				error = true;
+				reply.printf("Temperature limit for heater %d is %.1fC", heater, reprap.GetHeat()->GetTemperatureLimit(heater));
 			}
 		}
-		else
-		{
-			reply.printf("Temperature limit is %.1fC", platform->GetTemperatureLimit());
-		}
 		break;
 
 	case 144: // Set bed to standby
@@ -3949,26 +3952,21 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 190: // Set bed temperature and wait
 	case 191: // Set chamber temperature and wait
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())	// tell Move not to wait for more moves
-		{
-			return false;
-		}
-
 		{
 			const int8_t heater = (code == 191) ? reprap.GetHeat()->GetChamberHeater() : reprap.GetHeat()->GetBedHeater();
 			if (heater >= 0)
 			{
 				float temperature;
 				bool waitWhenCooling;
-				if (gb->Seen('R'))
+				if (gb.Seen('R'))
 				{
 					waitWhenCooling = true;
-					temperature = gb->GetFValue();
+					temperature = gb.GetFValue();
 				}
-				else if (gb->Seen('S'))
+				else if (gb.Seen('S'))
 				{
 					waitWhenCooling = false;
-					temperature = gb->GetFValue();
+					temperature = gb.GetFValue();
 				}
 				else
 				{
@@ -3977,7 +3975,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 				reprap.GetHeat()->SetActiveTemperature(heater, temperature);
 				reprap.GetHeat()->Activate(heater);
-				result = reprap.GetHeat()->HeaterAtSetTemperature(heater, waitWhenCooling);
+				if (cancelWait || reprap.GetHeat()->HeaterAtSetTemperature(heater, waitWhenCooling))
+				{
+					cancelWait = isWaiting = false;
+					break;
+				}
+				// In Marlin emulation mode we should return some sort of (undocumented) message here every second...
+				isWaiting = true;
+				return false;
 			}
 		}
 		break;
@@ -3987,29 +3992,29 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					platform->SetAcceleration(axis, gb->GetFValue() * distanceScale);
+					platform->SetAcceleration(axis, gb.GetFValue() * distanceScale);
 					seen = true;
 				}
 			}
 
-			if (gb->Seen(extrudeLetter))
+			if (gb.Seen(extrudeLetter))
 			{
 				seen = true;
 				float eVals[MaxExtruders];
 				size_t eCount = numExtruders;
-				gb->GetFloatArray(eVals, eCount, true);
+				gb.GetFloatArray(eVals, eCount, true);
 				for (size_t e = 0; e < eCount; e++)
 				{
 					platform->SetAcceleration(numAxes + e, eVals[e] * distanceScale);
 				}
 			}
 
-			if (gb->Seen('P'))
+			if (gb.Seen('P'))
 			{
 				// Set max average printing acceleration
-				platform->SetMaxAverageAcceleration(gb->GetFValue() * distanceScale);
+				platform->SetMaxAverageAcceleration(gb.GetFValue() * distanceScale);
 				seen = true;
 			}
 
@@ -4037,19 +4042,19 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; ++axis)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					platform->SetMaxFeedrate(axis, gb->GetFValue() * distanceScale * secondsToMinutes); // G Code feedrates are in mm/minute; we need mm/sec
+					platform->SetMaxFeedrate(axis, gb.GetFValue() * distanceScale * secondsToMinutes); // G Code feedrates are in mm/minute; we need mm/sec
 					seen = true;
 				}
 			}
 
-			if (gb->Seen(extrudeLetter))
+			if (gb.Seen(extrudeLetter))
 			{
 				seen = true;
 				float eVals[MaxExtruders];
 				size_t eCount = numExtruders;
-				gb->GetFloatArray(eVals, eCount, true);
+				gb.GetFloatArray(eVals, eCount, true);
 				for (size_t e = 0; e < eCount; e++)
 				{
 					platform->SetMaxFeedrate(numAxes + e, eVals[e] * distanceScale * secondsToMinutes);
@@ -4085,29 +4090,29 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 207: // Set firmware retraction details
 		{
 			bool seen = false;
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
-				retractLength = max<float>(gb->GetFValue(), 0.0);
+				retractLength = max<float>(gb.GetFValue(), 0.0);
 				seen = true;
 			}
-			if (gb->Seen('R'))
+			if (gb.Seen('R'))
 			{
-				retractExtra = max<float>(gb->GetFValue(), -retractLength);
+				retractExtra = max<float>(gb.GetFValue(), -retractLength);
 				seen = true;
 			}
-			if (gb->Seen('F'))
+			if (gb.Seen('F'))
 			{
-				unRetractSpeed = retractSpeed = max<float>(gb->GetFValue(), 60.0);
+				unRetractSpeed = retractSpeed = max<float>(gb.GetFValue(), 60.0);
 				seen = true;
 			}
-			if (gb->Seen('T'))	// must do this one after 'F'
+			if (gb.Seen('T'))	// must do this one after 'F'
 			{
-				unRetractSpeed = max<float>(gb->GetFValue(), 60.0);
+				unRetractSpeed = max<float>(gb.GetFValue(), 60.0);
 				seen = true;
 			}
-			if (gb->Seen('Z'))
+			if (gb.Seen('Z'))
 			{
-				retractHop = max<float>(gb->GetFValue(), 0.0);
+				retractHop = max<float>(gb.GetFValue(), 0.0);
 				seen = true;
 			}
 			if (!seen)
@@ -4120,13 +4125,13 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 208: // Set/print maximum axis lengths. If there is an S parameter with value 1 then we set the min value, else we set the max value.
 		{
-			bool setMin = (gb->Seen('S') ? (gb->GetIValue() == 1) : false);
+			bool setMin = (gb.Seen('S') ? (gb.GetIValue() == 1) : false);
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					float value = gb->GetFValue() * distanceScale;
+					float value = gb.GetFValue() * distanceScale;
 					if (setMin)
 					{
 						platform->SetAxisMinimum(axis, value);
@@ -4158,9 +4163,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 220:	// Set/report speed factor override percentage
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			float newSpeedFactor = (gb->GetFValue() / 100.0) * secondsToMinutes;	// include the conversion from mm/minute to mm/second
+			float newSpeedFactor = (gb.GetFValue() / 100.0) * secondsToMinutes;	// include the conversion from mm/minute to mm/second
 			if (newSpeedFactor > 0.0)
 			{
 				feedRate *= newSpeedFactor / speedFactor;
@@ -4186,14 +4191,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 221:	// Set/report extrusion factor override percentage
 		{
 			int extruder = 0;
-			if (gb->Seen('D'))	// D parameter (if present) selects the extruder number
+			if (gb.Seen('D'))	// D parameter (if present) selects the extruder number
 			{
-				extruder = gb->GetIValue();
+				extruder = gb.GetIValue();
 			}
 
-			if (gb->Seen('S'))	// S parameter sets the override percentage
+			if (gb.Seen('S'))	// S parameter sets the override percentage
 			{
-				float extrusionFactor = gb->GetFValue() / 100.0;
+				float extrusionFactor = gb.GetFValue() / 100.0;
 				if (extruder >= 0 && (size_t)extruder < numExtruders && extrusionFactor >= 0.0)
 				{
 					if (moveAvailable && !moveBuffer.isFirmwareRetraction)
@@ -4214,23 +4219,23 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		// For case 226, see case 25
 
 	case 280:	// Servos
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			const int servoIndex = gb->GetIValue();
+			const int servoIndex = gb.GetIValue();
 			Pin servoPin;
 			bool invert;
 			if (platform->GetFirmwarePin(servoIndex, PinAccess::servo, servoPin, invert))
 			{
-				if (gb->Seen('I'))
+				if (gb.Seen('I'))
 				{
-					if (gb->GetIValue() > 0)
+					if (gb.GetIValue() > 0)
 					{
 						invert = !invert;
 					}
 				}
-				if (gb->Seen('S'))
+				if (gb.Seen('S'))
 				{
-					float angleOrWidth = gb->GetFValue();
+					float angleOrWidth = gb.GetFValue();
 					if (angleOrWidth < 0.0)
 					{
 						// Disable the servo by setting the pulse width to zero
@@ -4266,8 +4271,8 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 300:	// Beep
 		{
-			int ms = (gb->Seen('P')) ? gb->GetIValue() : 1000;			// time in milliseconds
-			int freq = (gb->Seen('S')) ? gb->GetIValue() : 4600;		// 4600Hz produces the loudest sound on a PanelDue
+			int ms = (gb.Seen('P')) ? gb.GetIValue() : 1000;			// time in milliseconds
+			int freq = (gb.Seen('S')) ? gb.GetIValue() : 4600;		// 4600Hz produces the loudest sound on a PanelDue
 			reprap.Beep(freq, ms);
 		}
 		break;
@@ -4277,9 +4282,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 302: // Allow, deny or report cold extrudes
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			if (gb->GetIValue() > 0)
+			if (gb.GetIValue() > 0)
 			{
 				reprap.GetHeat()->AllowColdExtrude();
 			}
@@ -4296,12 +4301,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 303: // Run PID tuning
-		if (gb->Seen('H'))
+		if (gb.Seen('H'))
 		{
-			const size_t heater = gb->GetIValue();
-			const float temperature = (gb->Seen('S')) ? gb->GetFValue() : 225.0;
-			const float maxPwm = (gb->Seen('P')) ? gb->GetFValue() : 0.5;
-			if (heater < HEATERS && maxPwm >= 0.1 && maxPwm <= 1.0 && temperature >= 55.0 && temperature <= platform->GetTemperatureLimit())
+			const size_t heater = gb.GetIValue();
+			const float temperature = (gb.Seen('S')) ? gb.GetFValue() : 225.0;
+			const float maxPwm = (gb.Seen('P')) ? gb.GetFValue() : 0.5;
+			if (heater < HEATERS && maxPwm >= 0.1 && maxPwm <= 1.0 && temperature >= 55.0 && temperature <= reprap.GetHeat()->GetTemperatureLimit(heater))
 			{
 				reprap.GetHeat()->StartAutoTune(heater, temperature, maxPwm, reply);
 			}
@@ -4331,9 +4336,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 307: // Set heater process model parameters
-		if (gb->Seen('H'))
+		if (gb.Seen('H'))
 		{
-			size_t heater = gb->GetIValue();
+			size_t heater = gb.GetIValue();
 			if (heater < HEATERS)
 			{
 				const FopDt& model = reprap.GetHeat()->GetHeaterModel(heater);
@@ -4341,11 +4346,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				float gain = model.GetGain(), tc = model.GetTimeConstant(), td = model.GetDeadTime(), maxPwm = model.GetMaxPwm();
 				int32_t dontUsePid = model.UsePid() ? 0 : 1;
 
-				gb->TryGetFValue('A', gain, seen);
-				gb->TryGetFValue('C', tc, seen);
-				gb->TryGetFValue('D', td, seen);
-				gb->TryGetIValue('B', dontUsePid, seen);
-				gb->TryGetFValue('S', maxPwm, seen);
+				gb.TryGetFValue('A', gain, seen);
+				gb.TryGetFValue('C', tc, seen);
+				gb.TryGetFValue('D', td, seen);
+				gb.TryGetIValue('B', dontUsePid, seen);
+				gb.TryGetFValue('S', maxPwm, seen);
 
 				if (seen)
 				{
@@ -4386,25 +4391,25 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 350: // Set/report microstepping
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
-		{
-			return false;
-		}
 		{
 			// interp is current an int not a bool, because we use special values of interp to set the chopper control register
 			int interp = 0;
-			if (gb->Seen('I'))
+			if (gb.Seen('I'))
 			{
-				interp = gb->GetIValue();
+				interp = gb.GetIValue();
 			}
 
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
+					if (!LockMovementAndWaitForStandstill(gb))
+					{
+						return false;
+					}
 					seen = true;
-					int microsteps = gb->GetIValue();
+					int microsteps = gb.GetIValue();
 					if (ChangeMicrostepping(axis, microsteps, interp))
 					{
 						SetAxisNotHomed(axis);
@@ -4417,12 +4422,16 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 
-			if (gb->Seen(extrudeLetter))
+			if (gb.Seen(extrudeLetter))
 			{
+				if (!LockMovementAndWaitForStandstill(gb))
+				{
+					return false;
+				}
 				seen = true;
 				long eVals[MaxExtruders];
 				size_t eCount = numExtruders;
-				gb->GetLongArray(eVals, eCount);
+				gb.GetLongArray(eVals, eCount);
 				for (size_t e = 0; e < eCount; e++)
 				{
 					if (!ChangeMicrostepping(numAxes + e, (int)eVals[e], interp))
@@ -4454,7 +4463,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 400: // Wait for current moves to finish
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -4464,14 +4473,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		{
 			bool seen = false;
 
-			if (gb->Seen('N'))
+			if (gb.Seen('N'))
 			{
-				platform->SetFilamentWidth(gb->GetFValue());
+				platform->SetFilamentWidth(gb.GetFValue());
 				seen = true;
 			}
-			if (gb->Seen('D'))
+			if (gb.Seen('D'))
 			{
-				platform->SetNozzleDiameter(gb->GetFValue());
+				platform->SetNozzleDiameter(gb.GetFValue());
 				seen = true;
 			}
 
@@ -4484,8 +4493,8 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 408: // Get status in JSON format
 		{
-			int type = gb->Seen('S') ? gb->GetIValue() : 0;
-			int seq = gb->Seen('R') ? gb->GetIValue() : -1;
+			int type = gb.Seen('S') ? gb.GetIValue() : 0;
+			int seq = gb.Seen('R') ? gb.GetIValue() : -1;
 
 			OutputBuffer *statusResponse = nullptr;
 			switch (type)
@@ -4498,7 +4507,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				case 2:
 				case 3:
 				case 4:
-					statusResponse = reprap.GetStatusResponse(type - 1, (gb == auxGCode) ? ResponseSource::AUX : ResponseSource::Generic);
+					statusResponse = reprap.GetStatusResponse(type - 1, (&gb == auxGCode) ? ResponseSource::AUX : ResponseSource::Generic);
 					break;
 
 				case 5:
@@ -4508,6 +4517,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 			if (statusResponse != nullptr)
 			{
+				UnlockAll(gb);
 				statusResponse->cat('\n');
 				HandleReply(gb, false, statusResponse);
 				return true;
@@ -4521,9 +4531,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 501: // Load parameters from EEPROM
 		reprap.GetPlatform()->ReadNvData();
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			reprap.GetPlatform()->SetAutoSave(gb->GetIValue() > 0);
+			reprap.GetPlatform()->SetAutoSave(gb.GetIValue() > 0);
 		}
 		break;
 
@@ -4533,6 +4543,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 503: // List variable settings
 		{
+			if (!LockFileSystem(gb))
+			{
+				return false;
+			}
+
 			// Need a valid output buffer to continue...
 			OutputBuffer *configResponse;
 			if (!OutputBuffer::Allocate(configResponse))
@@ -4570,6 +4585,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 				f->Close();
 
+				UnlockAll(gb);
 				HandleReply(gb, false, configResponse);
 				return true;
 			}
@@ -4577,7 +4593,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 540: // Set/report MAC address
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
 			SetMACAddress(gb);
 		}
@@ -4589,9 +4605,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 550: // Set/report machine name
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			reprap.SetName(gb->GetString());
+			reprap.SetName(gb.GetString());
 		}
 		else
 		{
@@ -4600,32 +4616,32 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 551: // Set password (no option to report it)
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			reprap.SetPassword(gb->GetString());
+			reprap.SetPassword(gb.GetString());
 		}
 		break;
 
 	case 552: // Enable/Disable network and/or Set/Get IP address
 		{
 			bool seen = false;
-			if (gb->Seen('P'))
+			if (gb.Seen('P'))
 			{
 				seen = true;
 				SetEthernetAddress(gb, code);
 			}
 
-			if (gb->Seen('R'))
+			if (gb.Seen('R'))
 			{
-				reprap.GetNetwork()->SetHttpPort(gb->GetIValue());
+				reprap.GetNetwork()->SetHttpPort(gb.GetIValue());
 				seen = true;
 			}
 
 			// Process this one last in case the IP address is changed and the network enabled in the same command
-			if (gb->Seen('S')) // Has the user turned the network on or off?
+			if (gb.Seen('S')) // Has the user turned the network on or off?
 			{
 				seen = true;
-				if (gb->GetIValue() != 0)
+				if (gb.GetIValue() != 0)
 				{
 					reprap.GetNetwork()->Enable();
 				}
@@ -4648,7 +4664,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 553: // Set/Get netmask
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
 			SetEthernetAddress(gb, code);
 		}
@@ -4660,7 +4676,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 554: // Set/Get gateway
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
 			SetEthernetAddress(gb, code);
 		}
@@ -4672,9 +4688,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 555: // Set/report firmware type to emulate
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			platform->SetEmulating((Compatibility) gb->GetIValue());
+			platform->SetEmulating((Compatibility) gb.GetIValue());
 		}
 		else
 		{
@@ -4709,14 +4725,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 556: // Axis compensation (we support only X, Y, Z)
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			float value = gb->GetFValue();
+			float value = gb.GetFValue();
 			for (size_t axis = 0; axis <= Z_AXIS; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					reprap.GetMove()->SetAxisCompensation(axis, gb->GetFValue() / value);
+					reprap.GetMove()->SetAxisCompensation(axis, gb.GetFValue() / value);
 				}
 			}
 		}
@@ -4729,9 +4745,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 557: // Set/report Z probe point coordinates
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			int point = gb->GetIValue();
+			int point = gb.GetIValue();
 			if (point < 0 || (unsigned int)point >= MAX_PROBE_POINTS)
 			{
 				reprap.GetPlatform()->Message(GENERIC_MESSAGE, "Z probe point index out of range.\n");
@@ -4739,14 +4755,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			else
 			{
 				bool seen = false;
-				if (gb->Seen(axisLetters[X_AXIS]))
+				if (gb.Seen(axisLetters[X_AXIS]))
 				{
-					reprap.GetMove()->SetXBedProbePoint(point, gb->GetFValue());
+					reprap.GetMove()->SetXBedProbePoint(point, gb.GetFValue());
 					seen = true;
 				}
-				if (gb->Seen(axisLetters[Y_AXIS]))
+				if (gb.Seen(axisLetters[Y_AXIS]))
 				{
-					reprap.GetMove()->SetYBedProbePoint(point, gb->GetFValue());
+					reprap.GetMove()->SetYBedProbePoint(point, gb.GetFValue());
 					seen = true;
 				}
 
@@ -4764,9 +4780,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			uint32_t zProbeAxes = platform->GetZProbeAxes();
 			for (size_t axis = 0; axis < numAxes; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					if (gb->GetIValue() > 0)
+					if (gb.GetIValue() > 0)
 					{
 						zProbeAxes |= (1u << axis);
 					}
@@ -4783,35 +4799,35 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			}
 
 			// We must get and set the Z probe type first before setting the dive height etc., because different probe types may have different parameters
-			if (gb->Seen('P'))		// probe type
+			if (gb.Seen('P'))		// probe type
 			{
-				platform->SetZProbeType(gb->GetIValue());
+				platform->SetZProbeType(gb.GetIValue());
 				seenType = true;
 			}
 
 			ZProbeParameters params = platform->GetZProbeParameters();
-			gb->TryGetFValue('H', params.diveHeight, seenParam);		// dive height
+			gb.TryGetFValue('H', params.diveHeight, seenParam);		// dive height
 
-			if (gb->Seen('F'))		// feed rate i.e. probing speed
+			if (gb.Seen('F'))		// feed rate i.e. probing speed
 			{
-				params.probeSpeed = gb->GetFValue() * secondsToMinutes;
+				params.probeSpeed = gb.GetFValue() * secondsToMinutes;
 				seenParam = true;
 			}
 
-			if (gb->Seen('T'))		// travel speed to probe point
+			if (gb.Seen('T'))		// travel speed to probe point
 			{
-				params.travelSpeed = gb->GetFValue() * secondsToMinutes;
+				params.travelSpeed = gb.GetFValue() * secondsToMinutes;
 				seenParam = true;
 			}
 
-			if (gb->Seen('I'))
+			if (gb.Seen('I'))
 			{
-				params.invertReading = (gb->GetIValue() != 0);
+				params.invertReading = (gb.GetIValue() != 0);
 				seenParam = true;
 			}
 
-			gb->TryGetFValue('S', params.param1, seenParam);	// extra parameter for experimentation
-			gb->TryGetFValue('R', params.param2, seenParam);	// extra parameter for experimentation
+			gb.TryGetFValue('S', params.param1, seenParam);	// extra parameter for experimentation
+			gb.TryGetFValue('R', params.param2, seenParam);	// extra parameter for experimentation
 
 			if (seenParam)
 			{
@@ -4841,8 +4857,8 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 559: // Upload config.g or another gcode file to put in the sys directory
 	{
-		const char* str = (gb->Seen('P') ? gb->GetString() : platform->GetConfigFile());
-		bool ok = OpenFileToWrite(platform->GetSysDir(), str, gb);
+		const char* str = (gb.Seen('P') ? gb.GetString() : platform->GetConfigFile());
+		bool ok = OpenFileToWrite(gb, platform->GetSysDir(), str);
 		if (ok)
 		{
 			reply.printf("Writing to file: %s", str);
@@ -4857,8 +4873,8 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 560: // Upload reprap.htm or another web interface file
 	{
-		const char* str = (gb->Seen('P') ? gb->GetString() : INDEX_PAGE_FILE);
-		bool ok = OpenFileToWrite(platform->GetWebDir(), str, gb);
+		const char* str = (gb.Seen('P') ? gb.GetString() : INDEX_PAGE_FILE);
+		bool ok = OpenFileToWrite(gb, platform->GetWebDir(), str);
 		if (ok)
 		{
 			reply.printf("Writing to file: %s", str);
@@ -4876,9 +4892,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 562: // Reset temperature fault - use with great caution
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			int heater = gb->GetIValue();
+			int heater = gb.GetIValue();
 			if (heater >= 0 && heater < HEATERS)
 			{
 				reprap.ClearTemperatureFault(heater);
@@ -4896,9 +4912,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 564: // Think outside the box?
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			limitAxes = (gb->GetIValue() != 0);
+			limitAxes = (gb.GetIValue() != 0);
 		}
 		else
 		{
@@ -4911,19 +4927,19 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		bool seen = false;
 		for (size_t axis = 0; axis < numAxes; axis++)
 		{
-			if (gb->Seen(axisLetters[axis]))
+			if (gb.Seen(axisLetters[axis]))
 			{
-				platform->SetInstantDv(axis, gb->GetFValue() * distanceScale * secondsToMinutes); // G Code feedrates are in mm/minute; we need mm/sec
+				platform->SetInstantDv(axis, gb.GetFValue() * distanceScale * secondsToMinutes); // G Code feedrates are in mm/minute; we need mm/sec
 				seen = true;
 			}
 		}
 
-		if (gb->Seen(extrudeLetter))
+		if (gb.Seen(extrudeLetter))
 		{
 			seen = true;
 			float eVals[MaxExtruders];
 			size_t eCount = numExtruders;
-			gb->GetFloatArray(eVals, eCount, true);
+			gb.GetFloatArray(eVals, eCount, true);
 			for (size_t e = 0; e < eCount; e++)
 			{
 				platform->SetInstantDv(numAxes + e, eVals[e] * distanceScale * secondsToMinutes);
@@ -4948,20 +4964,20 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 567: // Set/report tool mix ratios
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			int8_t tNumber = gb->GetIValue();
+			int8_t tNumber = gb.GetIValue();
 			Tool* tool = reprap.GetTool(tNumber);
 			if (tool != NULL)
 			{
-				if (gb->Seen(extrudeLetter))
+				if (gb.Seen(extrudeLetter))
 				{
 					float eVals[MaxExtruders];
 					size_t eCount = tool->DriveCount();
-					gb->GetFloatArray(eVals, eCount, false);
+					gb.GetFloatArray(eVals, eCount, false);
 					if (eCount != tool->DriveCount())
 					{
-						reply.printf("Setting mix ratios - wrong number of E drives: %s", gb->Buffer());
+						reply.printf("Setting mix ratios - wrong number of E drives: %s", gb.Buffer());
 					}
 					else
 					{
@@ -4983,14 +4999,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 568: // Turn on/off automatic tool mixing
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			Tool* tool = reprap.GetTool(gb->GetIValue());
+			Tool* tool = reprap.GetTool(gb.GetIValue());
 			if (tool != NULL)
 			{
-				if (gb->Seen('S'))
+				if (gb.Seen('S'))
 				{
-					tool->SetMixing(gb->GetIValue() != 0);
+					tool->SetMixing(gb.GetIValue() != 0);
 				}
 				else
 				{
@@ -5001,36 +5017,44 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 569: // Set/report axis direction
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			size_t drive = gb->GetIValue();
+			size_t drive = gb.GetIValue();
 			if (drive < DRIVES)
 			{
 				bool seen = false;
-				if (gb->Seen('S'))
+				if (gb.Seen('S'))
 				{
-					platform->SetDirectionValue(drive, gb->GetIValue() != 0);
+					if (!LockMovementAndWaitForStandstill(gb))
+					{
+						return false;
+					}
+					platform->SetDirectionValue(drive, gb.GetIValue() != 0);
 					seen = true;
 				}
-				if (gb->Seen('R'))
+				if (gb.Seen('R'))
 				{
-					platform->SetEnableValue(drive, gb->GetIValue() != 0);
+					if (!LockMovementAndWaitForStandstill(gb))
+					{
+						return false;
+					}
+					platform->SetEnableValue(drive, gb.GetIValue() != 0);
 					seen = true;
 				}
-				if (gb->Seen('T'))
+				if (gb.Seen('T'))
 				{
-					platform->SetDriverStepTiming(drive, gb->GetFValue());
+					platform->SetDriverStepTiming(drive, gb.GetFValue());
 					seen = true;
 				}
 				bool badParameter = false;
 				for (size_t axis = 0; axis < numAxes; ++axis)
 				{
-					if (gb->Seen(axisLetters[axis]))
+					if (gb.Seen(axisLetters[axis]))
 					{
 						badParameter = true;
 					}
 				}
-				if (gb->Seen(extrudeLetter))
+				if (gb.Seen(extrudeLetter))
 				{
 					badParameter = true;
 				}
@@ -5050,16 +5074,16 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 570: // Set/report heater timeout
-		if (gb->Seen('H'))
+		if (gb.Seen('H'))
 		{
-			const size_t heater = gb->GetIValue();
+			const size_t heater = gb.GetIValue();
 			bool seen = false;
 			if (heater < HEATERS)
 			{
 				float maxTempExcursion, maxFaultTime;
 				reprap.GetHeat()->GetHeaterProtection(heater, maxTempExcursion, maxFaultTime);
-				gb->TryGetFValue('P', maxFaultTime, seen);
-				gb->TryGetFValue('T', maxTempExcursion, seen);
+				gb.TryGetFValue('P', maxFaultTime, seen);
+				gb.TryGetFValue('T', maxTempExcursion, seen);
 				if (seen)
 				{
 					reprap.GetHeat()->SetHeaterProtection(heater, maxTempExcursion, maxFaultTime);
@@ -5070,16 +5094,16 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 		}
-		else if (gb->Seen('S'))
+		else if (gb.Seen('S'))
 		{
 			reply.copy("M570 S parameter is no longer required or supported");
 		}
 		break;
 
 	case 571: // Set output on extrude
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			platform->SetExtrusionAncilliaryPWM(gb->GetFValue());
+			platform->SetExtrusionAncilliaryPWM(gb.GetFValue());
 		}
 		else
 		{
@@ -5088,13 +5112,13 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 572: // Set/report elastic compensation
-		if (gb->Seen('D'))
+		if (gb.Seen('D'))
 		{
 			// New usage: specify the extruder drive using the D parameter
-			size_t extruder = gb->GetIValue();
-			if (gb->Seen('S'))
+			size_t extruder = gb.GetIValue();
+			if (gb.Seen('S'))
 			{
-				platform->SetPressureAdvance(extruder, gb->GetFValue());
+				platform->SetPressureAdvance(extruder, gb.GetFValue());
 			}
 			else
 			{
@@ -5104,9 +5128,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 573: // Report heater average PWM
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			int heater = gb->GetIValue();
+			int heater = gb.GetIValue();
 			if (heater >= 0 && heater < HEATERS)
 			{
 				reply.printf("Average heater %d PWM: %.3f", heater, reprap.GetHeat()->GetAveragePWM(heater));
@@ -5117,12 +5141,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	case 574: // Set endstop configuration
 		{
 			bool seen = false;
-			bool logicLevel = (gb->Seen('S')) ? (gb->GetIValue() != 0) : true;
+			bool logicLevel = (gb.Seen('S')) ? (gb.GetIValue() != 0) : true;
 			for (size_t axis = 0; axis < numAxes; ++axis)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					int ival = gb->GetIValue();
+					int ival = gb.GetIValue();
 					if (ival >= 0 && ival <= 3)
 					{
 						platform->SetEndStopConfiguration(axis, (EndStopType) ival, logicLevel);
@@ -5147,20 +5171,20 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 575: // Set communications parameters
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			size_t chan = gb->GetIValue();
+			size_t chan = gb.GetIValue();
 			if (chan < NUM_SERIAL_CHANNELS)
 			{
 				bool seen = false;
-				if (gb->Seen('B'))
+				if (gb.Seen('B'))
 				{
-					platform->SetBaudRate(chan, gb->GetIValue());
+					platform->SetBaudRate(chan, gb.GetIValue());
 					seen = true;
 				}
-				if (gb->Seen('S'))
+				if (gb.Seen('S'))
 				{
-					uint32_t val = gb->GetIValue();
+					uint32_t val = gb.GetIValue();
 					platform->SetCommsProperties(chan, val);
 					switch (chan)
 					{
@@ -5186,11 +5210,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 577: // Wait until endstop is triggered
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
 			// Determine trigger type
 			EndStopHit triggerCondition;
-			switch (gb->GetIValue())
+			switch (gb.GetIValue())
 			{
 				case 1:
 					triggerCondition = EndStopHit::lowHit;
@@ -5209,7 +5233,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			// Axis endstops
 			for (size_t axis=0; axis < numAxes; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
 					if (platform->Stopped(axis) != triggerCondition)
 					{
@@ -5222,9 +5246,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			// Extruder drives
 			size_t eDriveCount = MaxExtruders;
 			long eDrives[MaxExtruders];
-			if (gb->Seen(extrudeLetter))
+			if (gb.Seen(extrudeLetter))
 			{
-				gb->GetLongArray(eDrives, eDriveCount);
+				gb.GetLongArray(eDrives, eDriveCount);
 				for(size_t extruder = 0; extruder < eDriveCount; extruder++)
 				{
 					const size_t eDrive = eDrives[extruder];
@@ -5252,9 +5276,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			return false;
 		}
 
-		if (gb->Seen('S')) // Need to handle the 'P' parameter too; see http://reprap.org/wiki/G-code#M578:_Fire_inkjet_bits
+		if (gb.Seen('S')) // Need to handle the 'P' parameter too; see http://reprap.org/wiki/G-code#M578:_Fire_inkjet_bits
 		{
-			platform->Inkjet(gb->GetIValue());
+			platform->Inkjet(gb.GetIValue());
 		}
 		break;
 #endif
@@ -5264,7 +5288,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; axis++)
 			{
-				gb->TryGetFValue(axisLetters[axis], axisScaleFactors[axis], seen);
+				gb.TryGetFValue(axisLetters[axis], axisScaleFactors[axis], seen);
 			}
 
 			if (!seen)
@@ -5282,14 +5306,14 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 #if SUPPORT_ROLAND
 	case 580: // (De)Select Roland mill
-		if (gb->Seen('R'))
+		if (gb.Seen('R'))
 		{
-			if (gb->GetIValue())
+			if (gb.GetIValue())
 			{
 				reprap.GetRoland()->Activate();
-				if (gb->Seen('P'))
+				if (gb.Seen('P'))
 				{
-					result = reprap.GetRoland()->RawWrite(gb->GetString());
+					result = reprap.GetRoland()->RawWrite(gb.GetString());
 				}
 			}
 			else
@@ -5306,9 +5330,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 581: // Configure external trigger
 	case 582: // Check external trigger
-		if (gb->Seen('T'))
+		if (gb.Seen('T'))
 		{
-			unsigned int triggerNumber = gb->GetIValue();
+			unsigned int triggerNumber = gb.GetIValue();
 			if (triggerNumber < MaxTriggers)
 			{
 				if (code == 582)
@@ -5322,32 +5346,32 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				else
 				{
 					bool seen = false;
-					if (gb->Seen('C'))
+					if (gb.Seen('C'))
 					{
 						seen = true;
-						triggers[triggerNumber].condition = gb->GetIValue();
+						triggers[triggerNumber].condition = gb.GetIValue();
 					}
 					else if (triggers[triggerNumber].IsUnused())
 					{
 						triggers[triggerNumber].condition = 0;		// this is a new trigger, so set no condition
 					}
-					if (gb->Seen('S'))
+					if (gb.Seen('S'))
 					{
 						seen = true;
-						int sval = gb->GetIValue();
+						int sval = gb.GetIValue();
 						TriggerMask triggerMask = 0;
 						for (size_t axis = 0; axis < numAxes; ++axis)
 						{
-							if (gb->Seen(axisLetters[axis]))
+							if (gb.Seen(axisLetters[axis]))
 							{
 								triggerMask |= (1u << axis);
 							}
 						}
-						if (gb->Seen(extrudeLetter))
+						if (gb.Seen(extrudeLetter))
 						{
 							long eStops[MaxExtruders];
 							size_t numEntries = MaxExtruders;
-							gb->GetLongArray(eStops, numEntries);
+							gb.GetLongArray(eStops, numEntries);
 							for (size_t i = 0; i < numEntries; ++i)
 							{
 								if (eStops[i] >= 0 && (unsigned long)eStops[i] < MaxExtruders)
@@ -5404,7 +5428,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 584: // Set axis/extruder to stepper driver(s) mapping
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())	// we also rely on this to retrieve the current motor positions to moveBuffer
+		if (!LockMovementAndWaitForStandstill(gb))	// we also rely on this to retrieve the current motor positions to moveBuffer
 		{
 			return false;
 		}
@@ -5412,12 +5436,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			bool seen = false, badDrive = false;
 			for (size_t drive = 0; drive < MAX_AXES; ++drive)
 			{
-				if (gb->Seen(axisLetters[drive]))
+				if (gb.Seen(axisLetters[drive]))
 				{
 					seen = true;
 					size_t numValues = MaxDriversPerAxis;
 					long drivers[MaxDriversPerAxis];
-					gb->GetLongArray(drivers, numValues);
+					gb.GetLongArray(drivers, numValues);
 
 					// Check all the driver numbers are in range
 					bool badAxis = false;
@@ -5455,12 +5479,12 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				}
 			}
 
-			if (gb->Seen(extrudeLetter))
+			if (gb.Seen(extrudeLetter))
 			{
 				seen = true;
 				size_t numValues = DRIVES - numAxes;
 				long drivers[MaxExtruders];
-				gb->GetLongArray(drivers, numValues);
+				gb.GetLongArray(drivers, numValues);
 				numExtruders = numValues;
 				for (size_t i = 0; i < numValues; ++i)
 				{
@@ -5505,7 +5529,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 665: // Set delta configuration
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -5517,44 +5541,44 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			bool wasInDeltaMode = params.IsDeltaMode();						// remember whether we were in delta mode
 			bool seen = false;
 
-			if (gb->Seen('L'))
+			if (gb.Seen('L'))
 			{
-				params.SetDiagonal(gb->GetFValue() * distanceScale);
+				params.SetDiagonal(gb.GetFValue() * distanceScale);
 				seen = true;
 			}
-			if (gb->Seen('R'))
+			if (gb.Seen('R'))
 			{
-				params.SetRadius(gb->GetFValue() * distanceScale);
+				params.SetRadius(gb.GetFValue() * distanceScale);
 				seen = true;
 			}
-			if (gb->Seen('B'))
+			if (gb.Seen('B'))
 			{
-				params.SetPrintRadius(gb->GetFValue() * distanceScale);
+				params.SetPrintRadius(gb.GetFValue() * distanceScale);
 				seen = true;
 			}
-			if (gb->Seen('X'))
+			if (gb.Seen('X'))
 			{
 				// X tower position correction
-				params.SetXCorrection(gb->GetFValue());
+				params.SetXCorrection(gb.GetFValue());
 				seen = true;
 			}
-			if (gb->Seen('Y'))
+			if (gb.Seen('Y'))
 			{
 				// Y tower position correction
-				params.SetYCorrection(gb->GetFValue());
+				params.SetYCorrection(gb.GetFValue());
 				seen = true;
 			}
-			if (gb->Seen('Z'))
+			if (gb.Seen('Z'))
 			{
 				// Y tower position correction
-				params.SetZCorrection(gb->GetFValue());
+				params.SetZCorrection(gb.GetFValue());
 				seen = true;
 			}
 
 			// The homed height must be done last, because it gets recalculated when some of the other factors are changed
-			if (gb->Seen('H'))
+			if (gb.Seen('H'))
 			{
-				params.SetHomedHeight(gb->GetFValue() * distanceScale);
+				params.SetHomedHeight(gb.GetFValue() * distanceScale);
 				seen = true;
 			}
 
@@ -5589,36 +5613,36 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 666: // Set delta endstop adjustments
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
 		{
 			DeltaParameters& params = reprap.GetMove()->AccessDeltaParams();
 			bool seen = false;
-			if (gb->Seen('X'))
+			if (gb.Seen('X'))
 			{
-				params.SetEndstopAdjustment(X_AXIS, gb->GetFValue());
+				params.SetEndstopAdjustment(X_AXIS, gb.GetFValue());
 				seen = true;
 			}
-			if (gb->Seen('Y'))
+			if (gb.Seen('Y'))
 			{
-				params.SetEndstopAdjustment(Y_AXIS, gb->GetFValue());
+				params.SetEndstopAdjustment(Y_AXIS, gb.GetFValue());
 				seen = true;
 			}
-			if (gb->Seen('Z'))
+			if (gb.Seen('Z'))
 			{
-				params.SetEndstopAdjustment(Z_AXIS, gb->GetFValue());
+				params.SetEndstopAdjustment(Z_AXIS, gb.GetFValue());
 				seen = true;
 			}
-			if (gb->Seen('A'))
+			if (gb.Seen('A'))
 			{
-				params.SetXTilt(gb->GetFValue() * 0.01);
+				params.SetXTilt(gb.GetFValue() * 0.01);
 				seen = true;
 			}
-			if (gb->Seen('B'))
+			if (gb.Seen('B'))
 			{
-				params.SetYTilt(gb->GetFValue() * 0.01);
+				params.SetYTilt(gb.GetFValue() * 0.01);
 				seen = true;
 			}
 
@@ -5636,7 +5660,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		break;
 
 	case 667: // Set CoreXY mode
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -5645,16 +5669,16 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			bool seen = false;
 			float positionNow[DRIVES];
 			move->GetCurrentUserPosition(positionNow, 0);					// get the current position, we may need it later
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
-				move->SetCoreXYMode(gb->GetIValue());
+				move->SetCoreXYMode(gb.GetIValue());
 				seen = true;
 			}
 			for (size_t axis = 0; axis < numAxes; ++axis)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					move->SetCoreAxisFactor(axis, gb->GetFValue());
+					move->SetCoreAxisFactor(axis, gb.GetFValue());
 					seen = true;
 				}
 			}
@@ -5681,10 +5705,10 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			struct tm * const timeInfo = gmtime(&now);
 			bool seen = false;
 
-			if (gb->Seen('P'))
+			if (gb.Seen('P'))
 			{
 				// Set date
-				const char * const dateString = gb->GetString();
+				const char * const dateString = gb.GetString();
 				if (strptime(dateString, "%Y-%m-%d", timeInfo) != nullptr)
 				{
 					if (!platform->SetDate(mktime(timeInfo)))
@@ -5704,10 +5728,10 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				seen = true;
 			}
 
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
 				// Set time
-				const char * const timeString = gb->GetString();
+				const char * const timeString = gb.GetString();
 				if (strptime(timeString, "%H:%M:%S", timeInfo) != nullptr)
 				{
 					if (!platform->SetTime(mktime(timeInfo)))
@@ -5746,26 +5770,31 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 906: // Set/report Motor currents
 	case 913: // Set/report motor current percent
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
-		{
-			return false;
-		}
 		{
 			bool seen = false;
 			for (size_t axis = 0; axis < numAxes; axis++)
 			{
-				if (gb->Seen(axisLetters[axis]))
+				if (gb.Seen(axisLetters[axis]))
 				{
-					platform->SetMotorCurrent(axis, gb->GetFValue(), code == 913);
+					if (!LockMovementAndWaitForStandstill(gb))
+					{
+						return false;
+					}
+					platform->SetMotorCurrent(axis, gb.GetFValue(), code == 913);
 					seen = true;
 				}
 			}
 
-			if (gb->Seen(extrudeLetter))
+			if (gb.Seen(extrudeLetter))
 			{
+				if (!LockMovementAndWaitForStandstill(gb))
+				{
+					return false;
+				}
+
 				float eVals[MaxExtruders];
 				size_t eCount = numExtruders;
-				gb->GetFloatArray(eVals, eCount, true);
+				gb.GetFloatArray(eVals, eCount, true);
 				// 2014-09-29 DC42: we no longer insist that the user supplies values for all possible extruder drives
 				for (size_t e = 0; e < eCount; e++)
 				{
@@ -5774,9 +5803,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 				seen = true;
 			}
 
-			if (code == 906 && gb->Seen('I'))
+			if (code == 906 && gb.Seen('I'))
 			{
-				float idleFactor = gb->GetFValue();
+				float idleFactor = gb.GetFValue();
 				if (idleFactor >= 0 && idleFactor <= 100.0)
 				{
 					platform->SetIdleCurrentFactor(idleFactor/100.0);
@@ -5810,9 +5839,9 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	case 912: // Set electronics temperature monitor adjustment
 		// Currently we ignore the P parameter (i.e. temperature measurement channel)
-		if (gb->Seen('S'))
+		if (gb.Seen('S'))
 		{
-			platform->SetMcuTemperatureAdjust(gb->GetFValue());
+			platform->SetMcuTemperatureAdjust(gb.GetFValue());
 		}
 		else
 		{
@@ -5823,7 +5852,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 	// For case 913, see 906
 
 	case 997: // Perform firmware update
-		if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+		if (!LockMovementAndWaitForStandstill(gb))
 		{
 			return false;
 		}
@@ -5833,11 +5862,11 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		if (firmwareUpdateModuleMap == 0)					// have we worked out which modules to update?
 		{
 			// Find out which modules we have been asked to update
-			if (gb->Seen('S'))
+			if (gb.Seen('S'))
 			{
 				long modulesToUpdate[3];
 				size_t numUpdateModules = ARRAY_SIZE(modulesToUpdate);
-				gb->GetLongArray(modulesToUpdate, numUpdateModules);
+				gb.GetLongArray(modulesToUpdate, numUpdateModules);
 				for (size_t i = 0; i < numUpdateModules; ++i)
 				{
 					long t = modulesToUpdate[i];
@@ -5882,15 +5911,15 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 			return false;
 		}
 
-		state = GCodeState::flashing1;
+		gb.MachineState().state = GCodeState::flashing1;
 		break;
 
 	case 998:
 		// The input handling code replaces the gcode by this when it detects a checksum error.
 		// Since we have no way of asking for the line to be re-sent, just report an error.
-		if (gb->Seen('P'))
+		if (gb.Seen('P'))
 		{
-			int val = gb->GetIValue();
+			int val = gb.GetIValue();
 			if (val != 0)
 			{
 				reply.printf("Checksum error on line %d", val);
@@ -5903,7 +5932,7 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 		if (result)
 		{
 			reprap.EmergencyStop();			// this disables heaters and drives - Duet WiFi pre-production boards need drives disabled here
-			uint16_t reason = (gb->Seen('P') && StringStartsWith(gb->GetString(), "ERASE"))
+			uint16_t reason = (gb.Seen('P') && StringStartsWith(gb.GetString(), "ERASE"))
 											? (uint16_t)SoftwareResetReason::erase
 											: (uint16_t)SoftwareResetReason::user;
 			platform->SoftwareReset(reason);			// doesn't return
@@ -5912,50 +5941,53 @@ bool GCodes::HandleMcode(GCodeBuffer* gb, StringRef& reply)
 
 	default:
 		error = true;
-		reply.printf("unsupported command: %s", gb->Buffer());
+		reply.printf("unsupported command: %s", gb.Buffer());
 	}
 
-	// Note that we send a reply to M105 requests even if the status is not 'normal', because we reply to these requests even when we are in other states
-	if (result && (state == GCodeState::normal || GCodeBuffer::IsPollCode(code)))
+	if (result && gb.MachineState().state == GCodeState::normal)
 	{
+		UnlockAll(gb);
 		HandleReply(gb, error, reply.Pointer());
 	}
 	return result;
 }
 
-bool GCodes::HandleTcode(GCodeBuffer* gb, StringRef& reply)
+bool GCodes::HandleTcode(GCodeBuffer& gb, StringRef& reply)
 {
-	if (!AllMovesAreFinishedAndMoveBufferIsLoaded())
+	if (!LockMovementAndWaitForStandstill(gb))
 	{
 		return false;
 	}
 
-	newToolNumber = gb->GetIValue();
-	newToolNumber += gb->GetToolNumberAdjust();
+	newToolNumber = gb.GetIValue();
+	newToolNumber += gb.GetToolNumberAdjust();
+
+	// TODO for the tool change restore point to be useful, we should undo any X axis mapping and remove any tool offsets
 	for (size_t drive = 0; drive < DRIVES; ++drive)
 	{
 		toolChangeRestorePoint.moveCoords[drive] = moveBuffer.coords[drive];
 	}
 	toolChangeRestorePoint.feedRate = feedRate;
 
-	if (simulationMode != 0)						// we don't yet simulate any T codes
+	if (simulationMode == 0)						// we don't yet simulate any T codes
 	{
-		HandleReply(gb, false, "");
-	}
-	else
-	{
-		// If old and new are the same we no longer follow the sequence. Use can deselect and then reselect the tool if he wants the macros run.
 		const Tool * const oldTool = reprap.GetCurrentTool();
+		// If old and new are the same we no longer follow the sequence. User can deselect and then reselect the tool if he wants the macros run.
 		if (oldTool->Number() != newToolNumber)
 		{
-			state = GCodeState::toolChange1;
+			gb.MachineState().state = GCodeState::toolChange1;
 			if (oldTool != nullptr && AllAxesAreHomed())
 			{
 				scratchString.printf("tfree%d.g", oldTool->Number());
-				DoFileMacro(scratchString.Pointer(), false);
+				DoFileMacro(gb, scratchString.Pointer(), false);
 			}
+			return true;							// proceeding with state machine, so don't unlock or send a reply
 		}
 	}
+
+	// If we get here, we have finished
+	UnlockAll(gb);
+	HandleReply(gb, false, "");
 	return true;
 }
 
@@ -5978,7 +6010,7 @@ void GCodes::CancelPrint()
 	isPaused = false;
 
 	fileGCode->Init();
-
+	FileData& fileBeingPrinted = fileGCode->OriginalMachineState().fileState;
 	if (fileBeingPrinted.IsLive())
 	{
 		fileBeingPrinted.Close();
@@ -6020,13 +6052,13 @@ bool GCodes::IsPaused() const
 
 bool GCodes::IsPausing() const
 {
-	const GCodeState topState = (stackPointer == 0) ? state : stack[0].state;
+	const GCodeState topState = fileGCode->OriginalMachineState().state;
 	return topState == GCodeState::pausing1 || topState == GCodeState::pausing2;
 }
 
 bool GCodes::IsResuming() const
 {
-	const GCodeState topState = (stackPointer == 0) ? state : stack[0].state;
+	const GCodeState topState = fileGCode->OriginalMachineState().state;
 	return topState == GCodeState::resuming1 || topState == GCodeState::resuming2 || topState == GCodeState::resuming3;
 }
 
@@ -6146,4 +6178,78 @@ void GCodes::SetAllAxesNotHomed()
 	axesHomed = 0;
 }
 
+// Resource locking/unlocking
+
+// Lock the resource, returning true if success
+bool GCodes::LockResource(const GCodeBuffer& gb, Resource r)
+{
+	if (resourceOwners[r] == &gb)
+	{
+		return true;
+	}
+	if (resourceOwners[r] == nullptr)
+	{
+		resourceOwners[r] = &gb;
+		gb.MachineState().lockedResources |= (1 << r);
+		return true;
+	}
+	return false;
+}
+
+bool GCodes::LockHeater(const GCodeBuffer& gb, int heater)
+{
+	if (heater >= 0 && heater < HEATERS)
+	{
+		return LockResource(gb, HeaterResourceBase + heater);
+	}
+	return true;
+}
+
+bool GCodes::LockFan(const GCodeBuffer& gb, int fan)
+{
+	if (fan >= 0 && fan < (int)NUM_FANS)
+	{
+		return LockResource(gb, FanResourceBase + fan);
+	}
+	return true;
+}
+
+// Lock the unshareable parts of the file system
+bool GCodes::LockFileSystem(const GCodeBuffer &gb)
+{
+	return LockResource(gb, FileSystemResource);
+}
+
+// Lock movement
+bool GCodes::LockMovement(const GCodeBuffer& gb)
+{
+	return LockResource(gb, MoveResource);
+}
+
+// Lock movement and wait for pending moves to finish
+bool GCodes::LockMovementAndWaitForStandstill(const GCodeBuffer& gb)
+{
+	bool b = LockMovement(gb);
+	if (b)
+	{
+		b = AllMovesAreFinishedAndMoveBufferIsLoaded();
+	}
+	return b;
+}
+
+// Release all locks, except those that were owned when the current macro was started
+void GCodes::UnlockAll(const GCodeBuffer& gb)
+{
+	const GCodeMachineState * const mc = gb.MachineState().previous;
+	const uint32_t resourcesToKeep = (mc == nullptr) ? 0 : mc->lockedResources;
+	for (size_t i = 0; i < NumResources; ++i)
+	{
+		if (resourceOwners[i] == &gb && ((1 << i) & resourcesToKeep) == 0)
+		{
+			resourceOwners[i] = nullptr;
+			gb.MachineState().lockedResources &= ~(1 << i);
+		}
+	}
+}
+
 // End
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index 4397384e..140569f3 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -25,54 +25,17 @@ Licence: GPL
 #include "GCodeBuffer.h"
 #include "Libraries/sha1/sha1.h"
 
-const unsigned int StackSize = 5;
-
 const char feedrateLetter = 'F';						// GCode feedrate
 const char extrudeLetter = 'E'; 						// GCode extrude
 
 // Type for specifying which endstops we want to check
 typedef uint16_t EndstopChecks;							// must be large enough to hold a bitmap of drive numbers or ZProbeActive
 const EndstopChecks ZProbeActive = 1 << 15;				// must be distinct from 1 << (any drive number)
+const EndstopChecks LogProbeChanges = 1 << 14;			// must be distinct from 1 << (any drive number)
 
 const float minutesToSeconds = 60.0;
 const float secondsToMinutes = 1.0/minutesToSeconds;
 
-// Enumeration to list all the possible states that the Gcode processing machine may be in
-enum class GCodeState
-{
-	normal,												// not doing anything and ready to process a new GCode
-	waitingForMoveToComplete,							// doing a homing move, so we must wait for it to finish before processing another GCode
-	homing,
-	setBed1,
-	setBed2,
-	setBed3,
-	toolChange1,
-	toolChange2,
-	toolChange3,
-	pausing1,
-	pausing2,
-	resuming1,
-	resuming2,
-	resuming3,
-	flashing1,
-	flashing2,
-	stopping,
-	sleeping
-};
-
-// Small class to stack the state when we execute a macro file
-class GCodeMachineState
-{
-public:
-	GCodeState state;
-	GCodeBuffer *gb;									// this may be null when executing config.g
-	float feedrate;
-	FileData fileState;
-	bool drivesRelative;
-	bool axesRelative;
-	bool doingFileMacro;
-};
-
 typedef uint16_t TriggerMask;
 
 struct Trigger
@@ -125,8 +88,9 @@ public:
     bool DoingFileMacro() const;										// Or still busy processing a macro file?
     float FractionOfFilePrinted() const;								// Get fraction of file printed
     void Diagnostics(MessageType mtype);								// Send helpful information out
-    bool HaveIncomingData() const;										// Is there something that we have to do?
-	size_t GetStackPointer() const;										// Returns the current stack pointer
+
+    bool RunConfigFile(const char* fileName);							// Start running the config file
+    bool IsRunningConfigFile() const;									// Are we still running the config file?
 
 	bool GetAxisIsHomed(unsigned int axis) const						// Has the axis been homed?
     	{ return (axesHomed & (1 << axis)) != 0; }
@@ -149,7 +113,6 @@ public:
 	bool IsRunning() const;
 
     bool AllAxesAreHomed() const;										// Return true if all axes are homed
-    bool DoFileMacro(const char* fileName, bool reportMissing = true);	// Run a GCode macro in a file, optionally report error if not found
 
     void CancelPrint();													// Cancel the current print
 
@@ -161,150 +124,166 @@ public:
     static const char axisLetters[MAX_AXES]; 							// 'X', 'Y', 'Z'
 
 private:
-    enum class CannedMoveType : uint8_t { none, relative, absolute };
-
-    struct RestorePoint
-    {
-    	float moveCoords[DRIVES];
-    	float feedRate;
-    	RestorePoint() { Init(); }
-    	void Init();
-    };
-
-    void FillGCodeBuffers();											// Get new data into the gcode buffers
-    void StartNextGCode(StringRef& reply);								// Fetch a new GCode and process it
-    void DoFilePrint(GCodeBuffer* gb, StringRef& reply);				// Get G Codes from a file and print them
-    bool AllMovesAreFinishedAndMoveBufferIsLoaded();					// Wait for move queue to exhaust and the current position is loaded
-    bool DoCannedCycleMove(EndstopChecks ce);							// Do a move from an internally programmed canned cycle
-    void FileMacroCyclesReturn();										// End a macro
-    bool ActOnCode(GCodeBuffer* gb, StringRef& reply);					// Do a G, M or T Code
-    bool HandleGcode(GCodeBuffer* gb, StringRef& reply);				// Do a G code
-    bool HandleMcode(GCodeBuffer* gb, StringRef& reply);				// Do an M code
-    bool HandleTcode(GCodeBuffer* gb, StringRef& reply);				// Do a T code
-    int SetUpMove(GCodeBuffer* gb, StringRef& reply);					// Pass a move on to the Move module
-    bool DoDwell(GCodeBuffer *gb);										// Wait for a bit
-    bool DoDwellTime(float dwell);										// Really wait for a bit
-    bool DoHome(GCodeBuffer *gb, StringRef& reply, bool& error);		// Home some axes
-    bool DoSingleZProbeAtPoint(int probePointIndex, float heightAdjust); // Probe at a given point
-    bool DoSingleZProbe(bool reportOnly, float heightAdjust);			// Probe where we are
-    int DoZProbe(float distance);										// Do a Z probe cycle up to the maximum specified distance
-    bool SetSingleZProbeAtAPosition(GCodeBuffer *gb, StringRef& reply);	// Probes at a given position - see the comment at the head of the function itself
-    void SetBedEquationWithProbe(int sParam, StringRef& reply);			// Probes a series of points and sets the bed equation
-    bool SetPrintZProbe(GCodeBuffer *gb, StringRef& reply);				// Either return the probe value, or set its threshold
-    void SetOrReportOffsets(StringRef& reply, GCodeBuffer *gb);			// Deal with a G10
-    bool SetPositions(GCodeBuffer *gb);									// Deal with a G92
-    bool LoadMoveBufferFromGCode(GCodeBuffer *gb, int moveType);		// Set up a move for the Move class
-    bool NoHome() const;												// Are we homing and not finished?
-    void Push();														// Push feedrate etc on the stack
-    void Pop();															// Pop feedrate etc
-    void DisableDrives();												// Turn the motors off
-    void SetEthernetAddress(GCodeBuffer *gb, int mCode);				// Does what it says
-    void SetMACAddress(GCodeBuffer *gb);								// Deals with an M540
-	void HandleReply(GCodeBuffer *gb, bool error, const char *reply);	// Handle G-Code replies
-	void HandleReply(GCodeBuffer *gb, bool error, OutputBuffer *reply);
-    bool OpenFileToWrite(const char* directory,							// Start saving GCodes in a file
-    		const char* fileName, GCodeBuffer *gb);
-    void WriteGCodeToFile(GCodeBuffer *gb);								// Write this GCode into a file
-    bool SendConfigToLine();											// Deal with M503
-    void WriteHTMLToFile(char b, GCodeBuffer *gb);						// Save an HTML file (usually to upload a new web interface)
-    bool OffsetAxes(GCodeBuffer *gb);									// Set offsets - deprecated, use G10
-    void SetPidParameters(GCodeBuffer *gb, int heater, StringRef& reply);	// Set the P/I/D parameters for a heater
-    void SetHeaterParameters(GCodeBuffer *gb, StringRef& reply);		 // Set the thermistor and ADC parameters for a heater
-    void ManageTool(GCodeBuffer *gb, StringRef& reply);					// Create a new tool definition
-    void SetToolHeaters(Tool *tool, float temperature);					// Set all a tool's heaters to the temperature.  For M104...
-    bool ToolHeatersAtSetTemperatures(const Tool *tool, bool waitWhenCooling) const;	// Wait for the heaters associated with the specified tool to reach their set temperatures
-    void SetAllAxesNotHomed();											// Flag all axes as not homed
-    void SetPositions(float positionNow[DRIVES]);						// Set the current position to be this
-    const char *TranslateEndStopResult(EndStopHit es);					// Translate end stop result to text
-    bool RetractFilament(bool retract);									// Retract or un-retract filaments
-    bool ChangeMicrostepping(size_t drive, int microsteps, int mode) const;	// Change microstepping on the specified drive
-    void ListTriggers(StringRef reply, TriggerMask mask);				// Append a list of trigger endstops to a message
-    bool CheckTriggers();												// Check for and execute triggers
-    void DoEmergencyStop();												// Execute an emergency stop
-    void DoPause(bool externalToFile);									// Pause the print
-    void SetMappedFanSpeed();											// Set the speeds of fans mapped for the current tool
-
-    Platform* platform;							// The RepRap machine
-    Webserver* webserver;						// The webserver class
-
-    GCodeBuffer* httpGCode;						// The sources...
-	GCodeBuffer* telnetGCode;					// ...
-    GCodeBuffer* fileGCode;						// ...
-    GCodeBuffer* serialGCode;					// ...
-    GCodeBuffer* auxGCode;						// this one is for the LCD display on the async serial interface
-    GCodeBuffer* fileMacroGCode;				// ...
-    GCodeBuffer *gbCurrent;
-
-    bool active;								// Live and running?
-    bool isPaused;								// true if the print has been paused
-    bool dwellWaiting;							// We are in a dwell
-    bool moveAvailable;							// Have we seen a move G Code and set it up?
-    float dwellTime;							// How long a pause for a dwell (seconds)?
-    float feedRate;								// The feed rate of the last G0/G1 command that had an F parameter
-    RawMove moveBuffer;							// Move details to pass to Move class
-    RestorePoint simulationRestorePoint;		// The position and feed rate when we started a simulation
-    RestorePoint pauseRestorePoint;				// The position and feed rate when we paused the print
-    RestorePoint toolChangeRestorePoint;		// The position and feed rate when we freed a tool
-    GCodeState state;							// The main state variable of the GCode state machine
-	bool drivesRelative;
-	bool axesRelative;
-    GCodeMachineState stack[StackSize];			// State that we save when calling macro files
-    unsigned int stackPointer;					// Push and Pop stack pointer
-    size_t numAxes;								// How many axes we have
-    size_t numExtruders;						// How many extruders we have, or may have
+	enum class CannedMoveType : uint8_t { none, relative, absolute };
+
+	struct RestorePoint
+	{
+		float moveCoords[DRIVES];
+		float feedRate;
+		RestorePoint() { Init(); }
+		void Init();
+	};
+
+	// Resources that can be locked.
+	// To avoid deadlock, if you need multiple resources then you must lock them in increasing numerical order.
+	typedef unsigned int Resource;
+	static const Resource MoveResource = 0;								// Movement system, including canned cycle variables
+	static const Resource FileSystemResource = 1;						// Non-sharable parts of the file system
+	static const Resource HeaterResourceBase = 2;
+	static const Resource FanResourceBase = HeaterResourceBase + HEATERS;
+	static const size_t NumResources = FanResourceBase + NUM_FANS;
+
+	static_assert(NumResources <= 32, "Too many resources to keep a bitmap of them in class GCodeMachineState");
+
+	bool LockResource(const GCodeBuffer& gb, Resource r);				// Lock the resource, returning true if success
+	bool LockHeater(const GCodeBuffer& gb, int heater);
+	bool LockFan(const GCodeBuffer& gb, int fan);
+	bool LockFileSystem(const GCodeBuffer& gb);							// Lock the unshareable parts of the file system
+	bool LockMovement(const GCodeBuffer& gb);							// Lock movement
+	bool LockMovementAndWaitForStandstill(const GCodeBuffer& gb);		// Lock movement and wait for pending moves to finish
+	void UnlockAll(const GCodeBuffer& gb);								// Release all locks
+
+	void StartNextGCode(GCodeBuffer& gb, StringRef& reply);				// Fetch a new or old GCode and process it
+	void DoFilePrint(GCodeBuffer& gb, StringRef& reply);				// Get G Codes from a file and print them
+	bool AllMovesAreFinishedAndMoveBufferIsLoaded();					// Wait for move queue to exhaust and the current position is loaded
+    bool DoFileMacro(GCodeBuffer& gb, const char* fileName, bool reportMissing = true);	// Run a GCode macro in a file, optionally report error if not found
+	bool DoCannedCycleMove(GCodeBuffer& gb, EndstopChecks ce);			// Do a move from an internally programmed canned cycle
+	void FileMacroCyclesReturn(GCodeBuffer& gb);						// End a macro
+	bool ActOnCode(GCodeBuffer& gb, StringRef& reply);					// Do a G, M or T Code
+	bool HandleGcode(GCodeBuffer& gb, StringRef& reply);				// Do a G code
+	bool HandleMcode(GCodeBuffer& gb, StringRef& reply);				// Do an M code
+	bool HandleTcode(GCodeBuffer& gb, StringRef& reply);				// Do a T code
+	int SetUpMove(GCodeBuffer& gb, StringRef& reply);					// Pass a move on to the Move module
+	bool DoDwell(GCodeBuffer& gb);										// Wait for a bit
+	bool DoDwellTime(float dwell);										// Really wait for a bit
+	bool DoHome(GCodeBuffer& gb, StringRef& reply, bool& error);		// Home some axes
+	bool DoSingleZProbeAtPoint(GCodeBuffer& gb, int probePointIndex, float heightAdjust);	// Probe at a given point
+	bool DoSingleZProbe(GCodeBuffer& gb, bool reportOnly, float heightAdjust);	// Probe where we are
+	int DoZProbe(GCodeBuffer& gb, float distance);						// Do a Z probe cycle up to the maximum specified distance
+	bool SetSingleZProbeAtAPosition(GCodeBuffer& gb, StringRef& reply);	// Probes at a given position - see the comment at the head of the function itself
+	void SetBedEquationWithProbe(int sParam, StringRef& reply);			// Probes a series of points and sets the bed equation
+	bool SetPrintZProbe(GCodeBuffer& gb, StringRef& reply);				// Either return the probe value, or set its threshold
+	bool SetOrReportOffsets(GCodeBuffer& gb, StringRef& reply);			// Deal with a G10
+	bool SetPositions(GCodeBuffer& gb);									// Deal with a G92
+	bool LoadMoveBufferFromGCode(GCodeBuffer& gb, int moveType);		// Set up a move for the Move class
+	bool NoHome() const;												// Are we homing and not finished?
+	bool Push(GCodeBuffer& gb);											// Push feedrate etc on the stack
+	void Pop(GCodeBuffer& gb);											// Pop feedrate etc
+	void DisableDrives();												// Turn the motors off
+	void SetEthernetAddress(GCodeBuffer& gb, int mCode);				// Does what it says
+	void SetMACAddress(GCodeBuffer& gb);								// Deals with an M540
+	void HandleReply(GCodeBuffer& gb, bool error, const char *reply);	// Handle G-Code replies
+	void HandleReply(GCodeBuffer& gb, bool error, OutputBuffer *reply);
+	bool OpenFileToWrite(GCodeBuffer& gb, const char* directory,							// Start saving GCodes in a file
+			const char* fileName);
+	void WriteGCodeToFile(GCodeBuffer& gb);								// Write this GCode into a file
+	bool SendConfigToLine();											// Deal with M503
+	void WriteHTMLToFile(GCodeBuffer& gb, char b);						// Save an HTML file (usually to upload a new web interface)
+	bool OffsetAxes(GCodeBuffer& gb);									// Set offsets - deprecated, use G10
+	void SetPidParameters(GCodeBuffer& gb, int heater, StringRef& reply);	// Set the P/I/D parameters for a heater
+	void SetHeaterParameters(GCodeBuffer& gb, StringRef& reply);		 // Set the thermistor and ADC parameters for a heater
+	void ManageTool(GCodeBuffer& gb, StringRef& reply);					// Create a new tool definition
+	void SetToolHeaters(Tool *tool, float temperature);					// Set all a tool's heaters to the temperature.  For M104...
+	bool ToolHeatersAtSetTemperatures(const Tool *tool, bool waitWhenCooling) const;	// Wait for the heaters associated with the specified tool to reach their set temperatures
+	void SetAllAxesNotHomed();											// Flag all axes as not homed
+	void SetPositions(float positionNow[DRIVES]);						// Set the current position to be this
+	const char *TranslateEndStopResult(EndStopHit es);					// Translate end stop result to text
+	bool RetractFilament(bool retract);									// Retract or un-retract filaments
+	bool ChangeMicrostepping(size_t drive, int microsteps, int mode) const;	// Change microstepping on the specified drive
+	void ListTriggers(StringRef reply, TriggerMask mask);				// Append a list of trigger endstops to a message
+	void CheckTriggers();												// Check for and execute triggers
+	void DoEmergencyStop();												// Execute an emergency stop
+	void DoPause(bool externalToFile);									// Pause the print
+	void SetMappedFanSpeed();											// Set the speeds of fans mapped for the current tool
+
+	Platform* platform;													// The RepRap machine
+	Webserver* webserver;												// The webserver class
+
+	GCodeBuffer* gcodeSources[6];										// The various sources of gcodes
+
+	GCodeBuffer*& httpGCode = gcodeSources[0];
+	GCodeBuffer*& telnetGCode = gcodeSources[1];
+	GCodeBuffer*& fileGCode = gcodeSources[2];
+	GCodeBuffer*& serialGCode = gcodeSources[3];
+	GCodeBuffer*& auxGCode = gcodeSources[4];							// This one is for the LCD display on the async serial interface
+	GCodeBuffer*& daemonGCode = gcodeSources[5];						// Used for executing config.g and trigger macro files
+	size_t nextGcodeSource;												// The one to check next
+
+	const GCodeBuffer* resourceOwners[NumResources];					// Which gcode buffer owns each resource
+
+	bool active;								// Live and running?
+	bool isPaused;								// true if the print has been paused
+	bool dwellWaiting;							// We are in a dwell
+	bool moveAvailable;							// Have we seen a move G Code and set it up?
+	float dwellTime;							// How long a pause for a dwell (seconds)?
+	float feedRate;								// The feed rate of the last G0/G1 command that had an F parameter
+	RawMove moveBuffer;							// Move details to pass to Move class
+	RestorePoint simulationRestorePoint;		// The position and feed rate when we started a simulation
+	RestorePoint pauseRestorePoint;				// The position and feed rate when we paused the print
+	RestorePoint toolChangeRestorePoint;		// The position and feed rate when we freed a tool
+	size_t numAxes;								// How many axes we have
+	size_t numExtruders;						// How many extruders we have, or may have
 	float axisScaleFactors[MAX_AXES];			// Scale XYZ coordinates by this factor (for Delta configurations)
-    float lastRawExtruderPosition[MaxExtruders]; // Extruder position of the last move fed into the Move class
-    float rawExtruderTotalByDrive[MaxExtruders]; // Total extrusion amount fed to Move class since starting print, before applying extrusion factor, per drive
-    float rawExtruderTotal;						// Total extrusion amount fed to Move class since starting print, before applying extrusion factor, summed over all drives
+	float lastRawExtruderPosition[MaxExtruders]; // Extruder position of the last move fed into the Move class
+	float rawExtruderTotalByDrive[MaxExtruders]; // Total extrusion amount fed to Move class since starting print, before applying extrusion factor, per drive
+	float rawExtruderTotal;						// Total extrusion amount fed to Move class since starting print, before applying extrusion factor, summed over all drives
 	float record[DRIVES];						// Temporary store for move positions
 	float cannedMoveCoords[DRIVES];				// Where to go or how much to move by in a canned cycle move, last is feed rate
 	float cannedFeedRate;						// How fast to do it
 	CannedMoveType cannedMoveType[DRIVES];		// Is this drive involved in a canned cycle move?
 	bool offSetSet;								// Are any axis offsets non-zero?
-    float distanceScale;						// MM or inches
-    FileData fileBeingPrinted;
-    FileData fileToPrint;
-    FileStore* fileBeingWritten;				// A file to write G Codes (or sometimes HTML) to
-    uint16_t toBeHomed;							// Bitmap of axes still to be homed
-    bool doingFileMacro;						// Are we executing a macro file?
-    int oldToolNumber, newToolNumber;			// Tools being changed
-    const char* eofString;						// What's at the end of an HTML file?
-    uint8_t eofStringCounter;					// Check the...
-    uint8_t eofStringLength;					// ... EoF string as we read.
-    int probeCount;								// Counts multiple probe points
-    int8_t cannedCycleMoveCount;				// Counts through internal (i.e. not macro) canned cycle moves
-    bool cannedCycleMoveQueued;					// True if a canned cycle move has been set
-    float longWait;								// Timer for things that happen occasionally (seconds)
-    bool limitAxes;								// Don't think outside the box.
-    uint32_t axesHomed;							// Bitmap of which axes have been homed
-    float pausedFanValues[NUM_FANS];			// Fan speeds when the print was paused
-    float lastDefaultFanSpeed;					// Last speed given in a M106 command with on fan number
-    float speedFactor;							// speed factor, including the conversion from mm/min to mm/sec, normally 1/60
-    float extrusionFactors[MaxExtruders];		// extrusion factors (normally 1.0)
-
-    // Z probe
-    float lastProbedZ;							// the last height at which the Z probe stopped
-    bool zProbesSet;							// True if all Z probing is done and we can set the bed equation
-    volatile bool zProbeTriggered;				// Set by the step ISR when a move is aborted because the Z probe is triggered
-
-    float simulationTime;						// Accumulated simulation time
-    uint8_t simulationMode;						// 0 = not simulating, 1 = simulating, >1 are simulation modes for debugging
-    FilePosition filePos;						// The position we got up to in the file being printed
-
-    // Firmware retraction settings
-    float retractLength, retractExtra;			// retraction length and extra length to un-retract
-    float retractSpeed;							// retract speed in mm/min
-    float unRetractSpeed;						// un=retract speed in mm/min
-    float retractHop;							// Z hop when retracting
-
-    // Triggers
-    Trigger triggers[MaxTriggers];				// Trigger conditions
-    TriggerMask lastEndstopStates;				// States of the endstop inputs last time we looked
-    static_assert(MaxTriggers <= 32, "Too many triggers");
-    uint32_t triggersPending;					// Bitmap of triggers pending but not yet executed
-
-    // Firmware update
-    uint8_t firmwareUpdateModuleMap;			// Bitmap of firmware modules to be updated
+	float distanceScale;						// MM or inches
+	FileData fileToPrint;
+	FileStore* fileBeingWritten;				// A file to write G Codes (or sometimes HTML) to
+	uint16_t toBeHomed;							// Bitmap of axes still to be homed
+	int oldToolNumber, newToolNumber;			// Tools being changed
+	const char* eofString;						// What's at the end of an HTML file?
+	uint8_t eofStringCounter;					// Check the...
+	uint8_t eofStringLength;					// ... EoF string as we read.
+	int probeCount;								// Counts multiple probe points
+	int8_t cannedCycleMoveCount;				// Counts through internal (i.e. not macro) canned cycle moves
+	bool cannedCycleMoveQueued;					// True if a canned cycle move has been set
+	float longWait;								// Timer for things that happen occasionally (seconds)
+	bool limitAxes;								// Don't think outside the box.
+	uint32_t axesHomed;							// Bitmap of which axes have been homed
+	float pausedFanValues[NUM_FANS];			// Fan speeds when the print was paused
+	float lastDefaultFanSpeed;					// Last speed given in a M106 command with on fan number
+	float speedFactor;							// speed factor, including the conversion from mm/min to mm/sec, normally 1/60
+	float extrusionFactors[MaxExtruders];		// extrusion factors (normally 1.0)
+
+	// Z probe
+	float lastProbedZ;							// the last height at which the Z probe stopped
+	bool zProbesSet;							// True if all Z probing is done and we can set the bed equation
+	volatile bool zProbeTriggered;				// Set by the step ISR when a move is aborted because the Z probe is triggered
+
+	float simulationTime;						// Accumulated simulation time
+	uint8_t simulationMode;						// 0 = not simulating, 1 = simulating, >1 are simulation modes for debugging
+	FilePosition filePos;						// The position we got up to in the file being printed
+
+	// Firmware retraction settings
+	float retractLength, retractExtra;			// retraction length and extra length to un-retract
+	float retractSpeed;							// retract speed in mm/min
+	float unRetractSpeed;						// un=retract speed in mm/min
+	float retractHop;							// Z hop when retracting
+
+	// Triggers
+	Trigger triggers[MaxTriggers];				// Trigger conditions
+	TriggerMask lastEndstopStates;				// States of the endstop inputs last time we looked
+	static_assert(MaxTriggers <= 32, "Too many triggers");
+	uint32_t triggersPending;					// Bitmap of triggers pending but not yet executed
+
+	// Firmware update
+	uint8_t firmwareUpdateModuleMap;			// Bitmap of firmware modules to be updated
 	bool isFlashing;							// Is a new firmware binary going to be flashed?
 
 	// SHA1 hashing
@@ -312,27 +291,17 @@ private:
 	SHA1Context hash;
 	bool StartHash(const char* filename);
 	bool AdvanceHash(StringRef &reply);
+
+	// Misc
+	bool isWaiting;								// True if waiting to reach temperature
+	bool cancelWait;							// Set true to cancel waiting
 };
 
 //*****************************************************************************************************
 
 inline bool GCodes::DoingFileMacro() const
 {
-	return doingFileMacro;
-}
-
-inline bool GCodes::HaveIncomingData() const
-{
-	return fileBeingPrinted.IsLive() ||
-			webserver->GCodeAvailable(WebSource::HTTP) ||
-			webserver->GCodeAvailable(WebSource::Telnet) ||
-			platform->GCodeAvailable(SerialSource::USB) ||
-			platform->GCodeAvailable(SerialSource::AUX);
-}
-
-inline size_t GCodes::GetStackPointer() const
-{
-	return stackPointer;
+	return fileGCode->IsDoingFileMacro();
 }
 
 #endif
diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp
index 256ad1a2..aad8b75b 100644
--- a/src/Heating/Heat.cpp
+++ b/src/Heating/Heat.cpp
@@ -36,18 +36,18 @@ void Heat::Init()
 	{
 		if (heater == bedHeater || heater == chamberHeater)
 		{
-			pids[heater]->Init(DefaultBedHeaterGain, DefaultBedHeaterTimeConstant, DefaultBedHeaterDeadTime, false);
+			pids[heater]->Init(DefaultBedHeaterGain, DefaultBedHeaterTimeConstant, DefaultBedHeaterDeadTime, DefaultBedTemperatureLimit, false);
 		}
 #ifndef DUET_NG
 		else if (heater == HEATERS - 1)
 		{
 			// Heater 6 pin is shared with fan 1. By default we support fan 1, so disable heater 6.
-			pids[heater]->Init(-1.0, -1.0, -1.0, true);
+			pids[heater]->Init(-1.0, -1.0, -1.0, DefaultExtruderTemperatureLimit, true);
 		}
 #endif
 		else
 		{
-			pids[heater]->Init(DefaultHotEndHeaterGain, DefaultHotEndHeaterTimeConstant, DefaultHotEndHeaterDeadTime, true);
+			pids[heater]->Init(DefaultHotEndHeaterGain, DefaultHotEndHeaterTimeConstant, DefaultHotEndHeaterDeadTime, DefaultExtruderTemperatureLimit, true);
 		}
 	}
 	lastTime = millis() - platform->HeatSampleInterval();		// flag the PIDS as due for spinning
@@ -171,6 +171,19 @@ float Heat::GetStandbyTemperature(int8_t heater) const
 	return (heater >= 0 && heater < HEATERS) ? pids[heater]->GetStandbyTemperature() : ABS_ZERO;
 }
 
+void Heat::SetTemperatureLimit(int8_t heater, float t)
+{
+	if (heater >= 0 && heater < HEATERS)
+	{
+		pids[heater]->SetTemperatureLimit(t);
+	}
+}
+
+float Heat::GetTemperatureLimit(int8_t heater) const
+{
+	return (heater >= 0 && heater < HEATERS) ? pids[heater]->GetTemperatureLimit() : ABS_ZERO;
+}
+
 float Heat::GetTemperature(int8_t heater) const
 {
 	return (heater >= 0 && heater < HEATERS) ? pids[heater]->GetTemperature() : ABS_ZERO;
@@ -264,4 +277,22 @@ void Heat::GetAutoTuneStatus(StringRef& reply) const
 	}
 }
 
+// Get the highest temperature limit of any heater
+float Heat::GetHighestTemperatureLimit() const
+{
+	float limit = ABS_ZERO;
+	for (size_t h = 0; h < HEATERS; ++h)
+	{
+		if (h < reprap.GetToolHeatersInUse() || (int)h == bedHeater || (int)h == chamberHeater)
+		{
+			const float t = pids[h]->GetTemperatureLimit();
+			if (t > limit)
+			{
+				limit = t;
+			}
+		}
+	}
+	return limit;
+}
+
 // End
diff --git a/src/Heating/Heat.h b/src/Heating/Heat.h
index 1ec813a7..6290cd16 100644
--- a/src/Heating/Heat.h
+++ b/src/Heating/Heat.h
@@ -58,6 +58,8 @@ public:
 	float GetActiveTemperature(int8_t heater) const;
 	void SetStandbyTemperature(int8_t heater, float t);
 	float GetStandbyTemperature(int8_t heater) const;
+	void SetTemperatureLimit(int8_t heater, float t);
+	float GetTemperatureLimit(int8_t heater) const;
 	void Activate(int8_t heater);								// Turn on a heater
 	void Standby(int8_t heater);								// Set a heater idle
 	float GetTemperature(int8_t heater) const;					// Get the temperature of a heater
@@ -106,6 +108,8 @@ public:
 	bool IsHeaterEnabled(size_t heater) const					// Is this heater enabled?
 	pre(heater < HEATERS);
 
+	float GetHighestTemperatureLimit() const;					// Get the highest temperature limit of any heater
+
 private:
 	Platform* platform;											// The instance of the RepRap hardware class
 	PID* pids[HEATERS];											// A PID controller for each heater
diff --git a/src/Heating/Pid.cpp b/src/Heating/Pid.cpp
index 836d009f..1cab2a77 100644
--- a/src/Heating/Pid.cpp
+++ b/src/Heating/Pid.cpp
@@ -31,8 +31,9 @@ PID::PID(Platform* p, int8_t h) : platform(p), heater(h), mode(HeaterMode::off)
 {
 }
 
-void PID::Init(float pGain, float pTc, float pTd, bool usePid)
+void PID::Init(float pGain, float pTc, float pTd, float tempLimit, bool usePid)
 {
+	temperatureLimit = tempLimit;
 	maxTempExcursion = DefaultMaxTempExcursion;
 	maxHeatingFaultTime = DefaultMaxHeatingFaultTime;
 	model.SetParameters(pGain, pTc, pTd, 1.0, usePid);
@@ -108,7 +109,7 @@ TemperatureError PID::ReadTemperature()
 		{
 			err = TemperatureError::openCircuit;
 		}
-		else if (temperature > platform->GetTemperatureLimit())
+		else if (temperature > temperatureLimit)
 		{
 			err = TemperatureError::tooHigh;
 		}
@@ -397,7 +398,7 @@ void PID::Spin()
 
 void PID::SetActiveTemperature(float t)
 {
-	if (t > platform->GetTemperatureLimit())
+	if (t > temperatureLimit)
 	{
 		platform->MessageF(GENERIC_MESSAGE, "Error: Temperature %.1f too high for heater %d!\n", t, heater);
 	}
@@ -413,7 +414,7 @@ void PID::SetActiveTemperature(float t)
 
 void PID::SetStandbyTemperature(float t)
 {
-	if (t > platform->GetTemperatureLimit())
+	if (t > temperatureLimit)
 	{
 		platform->MessageF(GENERIC_MESSAGE, "Error: Temperature %.1f too high for heater %d!\n", t, heater);
 	}
diff --git a/src/Heating/Pid.h b/src/Heating/Pid.h
index 67e019c4..17002877 100644
--- a/src/Heating/Pid.h
+++ b/src/Heating/Pid.h
@@ -36,13 +36,15 @@ class PID
 public:
 
 	PID(Platform* p, int8_t h);
-	void Init(float pGain, float pTc, float pTd, bool usePid);	// (Re)Set everything to start
+	void Init(float pGain, float pTc, float pTd, float tempLimit, bool usePid);	// (Re)Set everything to start
 	void Reset();
 	void Spin();									// Called in a tight loop to keep things running
 	void SetActiveTemperature(float t);
 	float GetActiveTemperature() const;
 	void SetStandbyTemperature(float t);
 	float GetStandbyTemperature() const;
+	void SetTemperatureLimit(float t);
+	float GetTemperatureLimit() const;
 	void Activate();								// Switch from idle to active
 	void Standby();									// Switch from active to idle
 	bool Active() const;							// Are we active?
@@ -60,6 +62,7 @@ public:
 
 	const FopDt& GetModel() const					// Get the process model
 		{ return model; }
+
 	bool SetModel(float gain, float tc, float td, float maxPwm, bool usePid);	// Set the process model
 
 	bool IsModelUsed() const						// Is the model being used to determine the PID parameters?
@@ -93,6 +96,7 @@ private:
 	Platform* platform;								// The instance of the class that is the RepRap hardware
 	float activeTemperature;						// The required active temperature
 	float standbyTemperature;						// The required standby temperature
+	float temperatureLimit;							// The maximum allowed temperature for this heater
 	float maxTempExcursion;							// The maximum temperature excursion permitted while maintaining the setpoint
 	float maxHeatingFaultTime;						// How long a heater fault is permitted to persist before a heater fault is raised
 	float temperature;								// The current temperature
@@ -147,6 +151,16 @@ inline float PID::GetStandbyTemperature() const
 	return standbyTemperature;
 }
 
+inline void PID::SetTemperatureLimit(float t)
+{
+	temperatureLimit = t;
+}
+
+inline float PID::GetTemperatureLimit() const
+{
+	return temperatureLimit;
+}
+
 inline float PID::GetTemperature() const
 {
 	return temperature;
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index cf91f8bd..0eeea2b6 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -55,6 +55,35 @@ static size_t savedMovePointer = 0;
 
 #endif
 
+#if DDA_LOG_PROBE_CHANGES
+
+size_t DDA::numLoggedProbePositions = 0;
+int32_t DDA::loggedProbePositions[MIN_AXES * MaxLoggedProbePositions];
+bool DDA::probeTriggered = false;
+
+void DDA::LogProbePosition()
+{
+	if (numLoggedProbePositions < MaxLoggedProbePositions)
+	{
+		int32_t *p = loggedProbePositions + (numLoggedProbePositions * MIN_AXES);
+		for (size_t drive = 0; drive < MIN_AXES; ++drive)
+		{
+			DriveMovement& dm = ddm[drive];
+			if (dm.state == DMState::moving)
+			{
+				p[drive] = endPoint[drive] - dm.GetNetStepsLeft();
+			}
+			else
+			{
+				p[drive] = endPoint[drive];
+			}
+		}
+		++numLoggedProbePositions;
+	}
+}
+
+#endif
+
 DDA::DDA(DDA* n) : next(n), prev(nullptr), state(empty)
 {
 	memset(ddm, 0, sizeof(ddm));	//DEBUG to clear stepError field
@@ -149,7 +178,7 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 	bool realMove = false, xyMoving = false;
 	const bool isSpecialDeltaMove = (move->IsDeltaMode() && !doMotorMapping);
 	float accelerations[DRIVES];
-	const float *normalAccelerations = reprap.GetPlatform()->Accelerations();
+	const float * const normalAccelerations = reprap.GetPlatform()->Accelerations();
 	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	for (size_t drive = 0; drive < DRIVES; drive++)
 	{
@@ -262,7 +291,7 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 				{
 					// Pure Z movement. We can't use the main calculation because it divides by a2plusb2.
 					dm.direction = (directionVector[Z_AXIS] >= 0.0);
-					dm.mp.delta.reverseStartStep = dm.totalSteps + 1;
+					dm.reverseStartStep = dm.totalSteps + 1;
 				}
 				else
 				{
@@ -279,11 +308,11 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 						// We may be almost at the peak height already, in which case we don't really have a reversal.
 						if (numStepsUp < 1 || (dm.direction && (uint32_t)numStepsUp <= dm.totalSteps))
 						{
-							dm.mp.delta.reverseStartStep = dm.totalSteps + 1;
+							dm.reverseStartStep = dm.totalSteps + 1;
 						}
 						else
 						{
-							dm.mp.delta.reverseStartStep = (uint32_t)numStepsUp + 1;
+							dm.reverseStartStep = (uint32_t)numStepsUp + 1;
 
 							// Correct the initial direction and the total number of steps
 							if (dm.direction)
@@ -301,7 +330,7 @@ bool DDA::Init(const GCodes::RawMove &nextMove, bool doMotorMapping)
 					}
 					else
 					{
-						dm.mp.delta.reverseStartStep = dm.totalSteps + 1;
+						dm.reverseStartStep = dm.totalSteps + 1;
 					}
 				}
 			}
@@ -538,7 +567,7 @@ void DDA::RecalculateMove()
 	canPause = (endStopsToCheck == 0);
 	if (canPause && endSpeed != 0.0)
 	{
-		const Platform *p = reprap.GetPlatform();
+		const Platform * const p = reprap.GetPlatform();
 		for (size_t drive = 0; drive < DRIVES; ++drive)
 		{
 			if (ddm[drive].state == DMState::moving && endSpeed * fabsf(directionVector[drive]) > p->ActualInstantDv(drive))
@@ -760,10 +789,10 @@ void DDA::Prepare()
 				// Check for sensible values, print them if they look dubious
 				if (reprap.Debug(moduleDda)
 					&& (   dm.totalSteps > 1000000
-						|| dm.mp.cart.reverseStartStep < dm.mp.cart.decelStartStep
-						|| (dm.mp.cart.reverseStartStep <= dm.totalSteps
-								&& dm.mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA > (int64_t)(dm.mp.cart.twoCsquaredTimesMmPerStepDivA * dm.mp.cart.reverseStartStep))
-					  )
+						|| dm.reverseStartStep < dm.mp.cart.decelStartStep
+						|| (dm.reverseStartStep <= dm.totalSteps
+							&& dm.mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA > (int64_t)(dm.mp.cart.twoCsquaredTimesMmPerStepDivA * dm.reverseStartStep))
+					   )
 				   )
 				{
 					DebugPrint();
@@ -828,6 +857,69 @@ void DDA::Prepare()
 	state = frozen;					// must do this last so that the ISR doesn't start executing it before we have finished setting it up
 }
 
+// Take a unit positive-hyperquadrant vector, and return the factor needed to obtain
+// length of the vector as projected to touch box[].
+float DDA::VectorBoxIntersection(const float v[], const float box[], size_t dimensions)
+{
+	// Generate a vector length that is guaranteed to exceed the size of the box
+	float biggerThanBoxDiagonal = 2.0*Magnitude(box, dimensions);
+	float magnitude = biggerThanBoxDiagonal;
+	for (size_t d = 0; d < dimensions; d++)
+	{
+		if (biggerThanBoxDiagonal*v[d] > box[d])
+		{
+			float a = box[d]/v[d];
+			if (a < magnitude)
+			{
+				magnitude = a;
+			}
+		}
+	}
+	return magnitude;
+}
+
+// Normalise a vector with dim1 dimensions so that it is unit in the first dim2 dimensions, and also return its previous magnitude in dim2 dimensions
+float DDA::Normalise(float v[], size_t dim1, size_t dim2)
+{
+	float magnitude = Magnitude(v, dim2);
+	if (magnitude <= 0.0)
+	{
+		return 0.0;
+	}
+	Scale(v, 1.0/magnitude, dim1);
+	return magnitude;
+}
+
+// Return the magnitude of a vector
+float DDA::Magnitude(const float v[], size_t dimensions)
+{
+	float magnitude = 0.0;
+	for (size_t d = 0; d < dimensions; d++)
+	{
+		magnitude += v[d]*v[d];
+	}
+	magnitude = sqrtf(magnitude);
+	return magnitude;
+}
+
+// Multiply a vector by a scalar
+void DDA::Scale(float v[], float scale, size_t dimensions)
+{
+	for(size_t d = 0; d < dimensions; d++)
+	{
+		v[d] = scale*v[d];
+	}
+}
+
+// Move a vector into the positive hyperquadrant
+void DDA::Absolute(float v[], size_t dimensions)
+{
+	for(size_t d = 0; d < dimensions; d++)
+	{
+		v[d] = fabsf(v[d]);
+	}
+}
+
 // The remaining functions are speed-critical, so use full optimisation
 #pragma GCC optimize ("O3")
 
@@ -839,12 +931,15 @@ pre(state == frozen)
 	moveStartTime = tim;
 	state = executing;
 
-	if (firstDM == nullptr)
+#if DDA_LOG_PROBE_CHANGES
+	if ((endStopsToCheck & LogProbeChanges) != 0)
 	{
-		// No steps are pending. This should not happen!
-		return true;	// schedule another interrupt immediately
+		numLoggedProbePositions = 0;
+		probeTriggered = false;
 	}
-	else
+#endif
+
+	if (firstDM != nullptr)
 	{
 		unsigned int extrusions = 0, retractions = 0;		// bitmaps of extruding and retracting drives
 		const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
@@ -887,7 +982,7 @@ pre(state == frozen)
 			}
 		}
 
-		Platform *platform = reprap.GetPlatform();
+		Platform * const platform = reprap.GetPlatform();
 		if (extruding)
 		{
 			platform->ExtrudeOn();
@@ -901,11 +996,10 @@ pre(state == frozen)
 		{
 			return platform->ScheduleInterrupt(firstDM->nextStepTime + moveStartTime);
 		}
-		else
-		{
-			return true;
-		}
 	}
+
+	// No steps are pending. This should not happen, except perhaps for an extrude-only move when extrusion is prohibited
+	return true;	// schedule another interrupt immediately
 }
 
 extern uint32_t maxReps;
@@ -915,7 +1009,7 @@ extern uint32_t maxReps;
 // This must be as fast as possible, because it determines the maximum movement speed.
 bool DDA::Step()
 {
-	Platform *platform = reprap.GetPlatform();
+	Platform * const platform = reprap.GetPlatform();
 	uint32_t lastStepPulseTime = platform->GetInterruptClocks();
 	bool repeat;
 	uint32_t numReps = 0;
@@ -946,6 +1040,33 @@ bool DDA::Step()
 				}
 			}
 
+#if DDA_LOG_PROBE_CHANGES
+			else if ((endStopsToCheck & LogProbeChanges) != 0)
+			{
+				switch (platform->GetZProbeResult())
+				{
+				case EndStopHit::lowHit:
+					if (!probeTriggered)
+					{
+						probeTriggered = true;
+						LogProbePosition();
+					}
+					break;
+
+				case EndStopHit::lowNear:
+				case EndStopHit::noStop:
+					if (probeTriggered)
+					{
+						probeTriggered = false;
+						LogProbePosition();
+					}
+					break;
+
+				default:
+					break;
+				}
+			}
+#endif
 			const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 			for (size_t drive = 0; drive < numAxes; ++drive)
 			{
@@ -1089,15 +1210,7 @@ void DDA::StopDrive(size_t drive)
 	DriveMovement& dm = ddm[drive];
 	if (dm.state == DMState::moving)
 	{
-		int32_t stepsLeft = dm.totalSteps - dm.nextStep + 1;
-		if (dm.direction)
-		{
-			endPoint[drive] -= stepsLeft;			// we were going forwards
-		}
-		else
-		{
-			endPoint[drive] += stepsLeft;			// we were going backwards
-		}
+		endPoint[drive] -= dm.GetNetStepsLeft();
 		dm.state = DMState::idle;
 		if (drive < reprap.GetGCodes()->GetNumAxes())
 		{
@@ -1184,67 +1297,4 @@ DriveMovement *DDA::RemoveDM(size_t drive)
 	return nullptr;
 }
 
-// Take a unit positive-hyperquadrant vector, and return the factor needed to obtain
-// length of the vector as projected to touch box[].
-float DDA::VectorBoxIntersection(const float v[], const float box[], size_t dimensions)
-{
-	// Generate a vector length that is guaranteed to exceed the size of the box
-	float biggerThanBoxDiagonal = 2.0*Magnitude(box, dimensions);
-	float magnitude = biggerThanBoxDiagonal;
-	for (size_t d = 0; d < dimensions; d++)
-	{
-		if (biggerThanBoxDiagonal*v[d] > box[d])
-		{
-			float a = box[d]/v[d];
-			if (a < magnitude)
-			{
-				magnitude = a;
-			}
-		}
-	}
-	return magnitude;
-}
-
-// Normalise a vector with dim1 dimensions so that it is unit in the first dim2 dimensions, and also return its previous magnitude in dim2 dimensions
-float DDA::Normalise(float v[], size_t dim1, size_t dim2)
-{
-	float magnitude = Magnitude(v, dim2);
-	if (magnitude <= 0.0)
-	{
-		return 0.0;
-	}
-	Scale(v, 1.0/magnitude, dim1);
-	return magnitude;
-}
-
-// Return the magnitude of a vector
-float DDA::Magnitude(const float v[], size_t dimensions)
-{
-	float magnitude = 0.0;
-	for (size_t d = 0; d < dimensions; d++)
-	{
-		magnitude += v[d]*v[d];
-	}
-	magnitude = sqrtf(magnitude);
-	return magnitude;
-}
-
-// Multiply a vector by a scalar
-void DDA::Scale(float v[], float scale, size_t dimensions)
-{
-	for(size_t d = 0; d < dimensions; d++)
-	{
-		v[d] = scale*v[d];
-	}
-}
-
-// Move a vector into the positive hyperquadrant
-void DDA::Absolute(float v[], size_t dimensions)
-{
-	for(size_t d = 0; d < dimensions; d++)
-	{
-		v[d] = fabsf(v[d]);
-	}
-}
-
 // End
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index e0463705..642a1526 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -10,6 +10,12 @@
 
 #include "DriveMovement.h"
 
+#ifdef DUET_NG
+#define DDA_LOG_PROBE_CHANGES	1
+#else
+#define DDA_LOG_PROBE_CHANGES	0		// save memory on the wired Duet
+#endif
+
 /**
  * This defines a single linear movement of the print head
  */
@@ -80,6 +86,12 @@ public:
 
 	static void PrintMoves();										// print saved moves for debugging
 
+#if DDA_LOG_PROBE_CHANGES
+	static const size_t MaxLoggedProbePositions = 40;
+	static size_t numLoggedProbePositions;
+	static int32_t loggedProbePositions[MIN_AXES * MaxLoggedProbePositions];
+#endif
+
 private:
 	void RecalculateMove();
 	void CalcNewSpeeds();
@@ -140,6 +152,12 @@ private:
 	uint32_t clocksNeeded;					// in clocks
 	uint32_t moveStartTime;					// clock count at which the move was started
 
+#if DDA_LOG_PROBE_CHANGES
+	static bool probeTriggered;
+
+	void LogProbePosition();
+#endif
+
     DriveMovement* firstDM;					// the contained DM that needs the first step
 
 	DriveMovement ddm[DRIVES];				// These describe the state of each drive movement
diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp
index 30fb68ab..447d7aac 100644
--- a/src/Movement/DriveMovement.cpp
+++ b/src/Movement/DriveMovement.cpp
@@ -39,7 +39,7 @@ void DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& param
 	}
 
 	// No reverse phase
-	mp.cart.reverseStartStep = totalSteps + 1;
+	reverseStartStep = totalSteps + 1;
 	mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA = 0;
 }
 
@@ -106,7 +106,7 @@ void DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, bo
 	if (dda.decelDistance * stepsPerMm < 0.5)		// if less than 1 deceleration step
 	{
 		totalSteps = (uint)max<int32_t>(netSteps, 0);
-		mp.cart.decelStartStep = mp.cart.reverseStartStep = netSteps + 1;
+		mp.cart.decelStartStep = reverseStartStep = netSteps + 1;
 		topSpeedTimesCdivAPlusDecelStartClocks = 0;
 		mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA = 0;
 		twoDistanceToStopTimesCsquaredDivA = 0;
@@ -130,27 +130,27 @@ void DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params, bo
 		{
 			// No reverse phase
 			totalSteps = (uint)max<int32_t>(netSteps, 0);
-			mp.cart.reverseStartStep = netSteps + 1;
+			reverseStartStep = netSteps + 1;
 			mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA = 0;
 		}
 		else
 		{
-			mp.cart.reverseStartStep = (initialDecelSpeed < 0.0)
-									? mp.cart.decelStartStep
-									: (twoDistanceToStopTimesCsquaredDivA/mp.cart.twoCsquaredTimesMmPerStepDivA) + 1;
+			reverseStartStep = (initialDecelSpeed < 0.0)
+								? mp.cart.decelStartStep
+								: (twoDistanceToStopTimesCsquaredDivA/mp.cart.twoCsquaredTimesMmPerStepDivA) + 1;
 			// Because the step numbers are rounded down, we may sometimes get a situation in which netSteps = 1 and reverseStartStep = 1.
 			// This would lead to totalSteps = -1, which must be avoided.
-			const int32_t overallSteps = (int32_t)(2 * (mp.cart.reverseStartStep - 1)) - netSteps;
+			const int32_t overallSteps = (int32_t)(2 * (reverseStartStep - 1)) - netSteps;
 			if (overallSteps > 0)
 			{
 				totalSteps = overallSteps;
 				mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA =
-						(int64_t)((2 * (mp.cart.reverseStartStep - 1)) * mp.cart.twoCsquaredTimesMmPerStepDivA) - (int64_t)twoDistanceToStopTimesCsquaredDivA;
+						(int64_t)((2 * (reverseStartStep - 1)) * mp.cart.twoCsquaredTimesMmPerStepDivA) - (int64_t)twoDistanceToStopTimesCsquaredDivA;
 			}
 			else
 			{
 				totalSteps = (uint)max<int32_t>(netSteps, 0);
-				mp.cart.reverseStartStep = totalSteps + 1;
+				reverseStartStep = totalSteps + 1;
 				mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA = 0;
 			}
 		}
@@ -161,25 +161,24 @@ void DriveMovement::DebugPrint(char c, bool isDeltaMovement) const
 {
 	if (state != DMState::idle)
 	{
-		debugPrintf("DM%c%s dir=%c steps=%u next=%u interval=%u sstcda=%u "
+		debugPrintf("DM%c%s dir=%c steps=%u next=%u rev=%u interval=%u sstcda=%u "
 					"acmadtcdts=%d tstcdapdsc=%u 2dtstc2diva=%" PRIu64 "\n",
-					c, (state == DMState::stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, stepInterval, startSpeedTimesCdivA,
+					c, (state == DMState::stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, reverseStartStep, stepInterval, startSpeedTimesCdivA,
 					accelClocksMinusAccelDistanceTimesCdivTopSpeed, topSpeedTimesCdivAPlusDecelStartClocks, twoDistanceToStopTimesCsquaredDivA);
 
 		if (isDeltaMovement)
 		{
-			debugPrintf("revss=%d hmz0sK=%d minusAaPlusBbTimesKs=%d dSquaredMinusAsquaredMinusBsquared=%" PRId64 "\n"
+			debugPrintf("hmz0sK=%d minusAaPlusBbTimesKs=%d dSquaredMinusAsquaredMinusBsquared=%" PRId64 "\n"
 						"2c2mmsdak=%u asdsk=%u dsdsk=%u mmstcdts=%u\n",
-						mp.delta.reverseStartStep, mp.delta.hmz0sK, mp.delta.minusAaPlusBbTimesKs, mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared,
+						mp.delta.hmz0sK, mp.delta.minusAaPlusBbTimesKs, mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared,
 						mp.delta.twoCsquaredTimesMmPerStepDivAK, mp.delta.accelStopDsK, mp.delta.decelStartDsK, mp.delta.mmPerStepTimesCdivtopSpeedK
 						);
 		}
 		else
 		{
-			debugPrintf("accelStopStep=%u decelStartStep=%u revStartStep=%u nextStep=%u nextStepTime=%u 2CsqtMmPerStepDivA=%" PRIu64 "\n",
-						mp.cart.accelStopStep, mp.cart.decelStartStep, mp.cart.reverseStartStep, nextStep, nextStepTime, mp.cart.twoCsquaredTimesMmPerStepDivA
-						);
-			debugPrintf(" mmPerStepTimesCdivtopSpeed=%u fmsdmtstdca2=%" PRId64 "\n",
+			debugPrintf("accelStopStep=%u decelStartStep=%u 2CsqtMmPerStepDivA=%" PRIu64 "\n"
+						"mmPerStepTimesCdivtopSpeed=%u fmsdmtstdca2=%" PRId64 "\n",
+						mp.cart.accelStopStep, mp.cart.decelStartStep, mp.cart.twoCsquaredTimesMmPerStepDivA,
 						mp.cart.mmPerStepTimesCdivtopSpeed, mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivA
 						);
 		}
@@ -203,8 +202,8 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 	uint32_t shiftFactor;
 	if (stepInterval < DDA::MinCalcIntervalCartesian)
 	{
-		uint32_t stepsToLimit = ((nextStep <= mp.cart.reverseStartStep && mp.cart.reverseStartStep <= totalSteps)
-									? mp.cart.reverseStartStep
+		uint32_t stepsToLimit = ((nextStep <= reverseStartStep && reverseStartStep <= totalSteps)
+									? reverseStartStep
 									: totalSteps
 								) - nextStep;
 		if (stepInterval < DDA::MinCalcIntervalCartesian/4 && stepsToLimit > 8)
@@ -242,7 +241,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 		// steady speed phase
 		nextStepTime = (uint32_t)((int32_t)(((uint64_t)mp.cart.mmPerStepTimesCdivtopSpeed * nextCalcStep)/K1) + accelClocksMinusAccelDistanceTimesCdivTopSpeed);
 	}
-	else if (nextCalcStep < mp.cart.reverseStartStep)
+	else if (nextCalcStep < reverseStartStep)
 	{
 		// deceleration phase, not reversed yet
 		uint64_t temp = mp.cart.twoCsquaredTimesMmPerStepDivA * nextCalcStep;
@@ -254,7 +253,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 	else
 	{
 		// deceleration phase, reversing or already reversed
-		if (nextCalcStep == mp.cart.reverseStartStep)
+		if (nextCalcStep == reverseStartStep)
 		{
 			direction = !direction;
 			if (live)
@@ -299,8 +298,8 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 	uint32_t shiftFactor;
 	if (stepInterval < DDA::MinCalcIntervalDelta)
 	{
-		const uint32_t stepsToLimit = ((nextStep < mp.delta.reverseStartStep && mp.delta.reverseStartStep <= totalSteps)
-										? mp.delta.reverseStartStep
+		const uint32_t stepsToLimit = ((nextStep < reverseStartStep && reverseStartStep <= totalSteps)
+										? reverseStartStep
 										: totalSteps
 									  ) - nextStep;
 		if (stepInterval < DDA::MinCalcIntervalDelta/8 && stepsToLimit > 16)
@@ -330,7 +329,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0)
 	}
 	stepsTillRecalc = (1u << shiftFactor) - 1;					// store number of additional steps to generate
 
-	if (nextStep == mp.delta.reverseStartStep)
+	if (nextStep == reverseStartStep)
 	{
 		direction = false;
 		if (live)
diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h
index 5dbe866c..e30020bd 100644
--- a/src/Movement/DriveMovement.h
+++ b/src/Movement/DriveMovement.h
@@ -40,6 +40,7 @@ public:
 	void PrepareExtruder(const DDA& dda, const PrepParams& params, bool doCompensation);
 	void ReduceSpeed(const DDA& dda, float inverseSpeedFactor);
 	void DebugPrint(char c, bool withDelta) const;
+	int32_t GetNetStepsLeft() const;
 
 private:
 	bool CalcNextStepTimeCartesianFull(const DDA &dda, bool live);
@@ -63,6 +64,7 @@ public:
 
 	// These values change as the step is executed
 	uint32_t nextStep;									// number of steps already done
+	uint32_t reverseStartStep;							// the step number for which we need to reverse direction due to pressure advance or delta movement
 	uint32_t nextStepTime;								// how many clocks after the start of this move the next step is due
 	uint32_t stepInterval;								// how many clocks between steps
 	DriveMovement *nextDM;								// link to next DM that needs a step
@@ -78,7 +80,6 @@ public:
 			// The following depend on how the move is executed, so they must be set up in Prepare()
 			uint32_t accelStopStep;						// the first step number at which we are no longer accelerating
 			uint32_t decelStartStep;					// the first step number at which we are decelerating
-			uint32_t reverseStartStep;					// the first step number for which we need to reverse direction due to pressure advance
 			uint32_t mmPerStepTimesCdivtopSpeed;		// mmPerStepInHyperCuboidSpace * clock / topSpeed
 
 			// The following only need to be stored per-drive if we are supporting pressure advance
@@ -89,7 +90,6 @@ public:
 		{
 			// The following don't depend on how the move is executed, so they can be set up in Init
 			int64_t dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared;
-			uint32_t reverseStartStep;
 			int32_t hmz0sK;								// the starting step position less the starting Z height, multiplied by the Z movement fraction and K (can go negative)
 			int32_t minusAaPlusBbTimesKs;
 			uint32_t twoCsquaredTimesMmPerStepDivAK;	// this could be stored in the DDA if all towers use the same steps/mm
@@ -150,4 +150,16 @@ inline bool DriveMovement::CalcNextStepTimeDelta(const DDA &dda, bool live)
 	return false;
 }
 
+// Return the number of net steps left for the move in the forwards direction.
+inline int32_t DriveMovement::GetNetStepsLeft() const
+{
+	const int32_t netStepsLeft =
+			(  (nextStep >= reverseStartStep || reverseStartStep >= totalSteps)
+			 ? totalSteps									// no reverse due, or we have already reversed
+			 : 2 * reverseStartStep - totalSteps			// we have yet to reverse
+			)
+			- nextStep + 1;
+	return (direction) ? netStepsLeft : -netStepsLeft;
+}
+
 #endif /* DRIVEMOVEMENT_H_ */
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 6cf2bb65..b1235bba 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -423,13 +423,28 @@ extern uint64_t lastNum;
 
 void Move::Diagnostics(MessageType mtype)
 {
-	reprap.GetPlatform()->Message(mtype, "=== Move ===\n");
-	reprap.GetPlatform()->MessageF(mtype, "MaxReps: %u, StepErrors: %u, MaxWait: %ums, Underruns: %u, %u\n",
+	Platform * const p = reprap.GetPlatform();
+	p->Message(mtype, "=== Move ===\n");
+	p->MessageF(mtype, "MaxReps: %u, StepErrors: %u, MaxWait: %ums, Underruns: %u, %u\n",
 											maxReps, stepErrors, longestGcodeWaitInterval, numLookaheadUnderruns, numPrepareUnderruns);
 	maxReps = 0;
 	numLookaheadUnderruns = numPrepareUnderruns = 0;
 	longestGcodeWaitInterval = 0;
 
+#if DDA_LOG_PROBE_CHANGES
+	// Temporary code to print Z probe trigger positions
+	p->Message(mtype, "Probe change coordinates:");
+	char ch = ' ';
+	for (size_t i = 0; i < DDA::numLoggedProbePositions; ++i)
+	{
+		float xyzPos[MIN_AXES];
+		MachineToEndPoint(DDA::loggedProbePositions + (MIN_AXES * i), xyzPos, MIN_AXES);
+		p->MessageF(mtype, "%c%.2f,%.2f", ch, xyzPos[X_AXIS], xyzPos[Y_AXIS]);
+		ch = ',';
+	}
+	p->Message(mtype, "\n");
+#endif
+
 #if 0
 	// For debugging
 	if (sqCount != 0)
@@ -1170,7 +1185,7 @@ void Move::DeltaProbeInterrupt()
 		}
 
 		bool dir = deltaProbe.GetDirection();
-		Platform *platform = reprap.GetPlatform();
+		Platform * const platform = reprap.GetPlatform();
 		platform->SetDirection(X_AXIS, dir);
 		platform->SetDirection(Y_AXIS, dir);
 		platform->SetDirection(Z_AXIS, dir);
@@ -1535,7 +1550,7 @@ int Move::DoDeltaProbe(float frequency, float amplitude, float rate, float dista
 		const uint32_t firstInterruptTime = deltaProbe.Start();
 		if (firstInterruptTime != 0xFFFFFFFF)
 		{
-			Platform *platform = reprap.GetPlatform();
+			Platform * const platform = reprap.GetPlatform();
 			platform->EnableDrive(X_AXIS);
 			platform->EnableDrive(Y_AXIS);
 			platform->EnableDrive(Z_AXIS);
diff --git a/src/Platform.cpp b/src/Platform.cpp
index b04ac586..391b7331 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -369,7 +369,6 @@ void Platform::Init()
 				THERMISTOR_SERIES_RS);										// initialise the thermistor parameters
 		thermistorFilters[heater].Init(0);
 	}
-	SetTemperatureLimit(DEFAULT_TEMPERATURE_LIMIT);
 
 	// Fans
 	InitFans();
@@ -450,14 +449,8 @@ bool Platform::AnyFileOpen(const FATFS *fs) const
 	return false;
 }
 
-void Platform::SetTemperatureLimit(float t)
-{
-	temperatureLimit = t;
-}
-
 // Specify which thermistor channel a particular heater uses
 void Platform::SetThermistorNumber(size_t heater, size_t thermistor)
-pre(heater < HEATERS && thermistor < HEATERS)
 {
 	heaterTempChannels[heater] = thermistor;
 
@@ -1635,7 +1628,6 @@ void Platform::SetPidParameters(size_t heater, const PidParameters& params)
 		{
 			WriteNvData();
 		}
-		SetTemperatureLimit(temperatureLimit);		// recalculate the thermistor resistance at max allowed temperature for the tick ISR
 	}
 }
 const PidParameters& Platform::GetPidParameters(size_t heater) const
@@ -2221,11 +2213,24 @@ void Platform::AppendAuxReply(const char *msg)
 	// Discard this response if either no aux device is attached or if the response is empty
 	if (msg[0] != 0 && HaveAux())
 	{
-		// Regular text-based responses for AUX are currently stored and processed by M105/M408
-		if (auxGCodeReply != nullptr || OutputBuffer::Allocate(auxGCodeReply))
+		if (msg[0] == '{')
+		{
+			// JSON responses are always sent directly to the AUX device
+			OutputBuffer *buf;
+			if (OutputBuffer::Allocate(buf))
+			{
+				buf->copy(msg);
+				auxOutput->Push(buf);
+			}
+		}
+		else
 		{
-			auxSeq++;
-			auxGCodeReply->cat(msg);
+			// Regular text-based responses for AUX are currently stored and processed by M105/M408
+			if (auxGCodeReply != nullptr || OutputBuffer::Allocate(auxGCodeReply))
+			{
+				auxSeq++;
+				auxGCodeReply->cat(msg);
+			}
 		}
 	}
 }
@@ -2304,22 +2309,6 @@ void Platform::Message(MessageType type, const char *message)
 					usbOutput->Push(usbOutputBuffer);
 				}
 
-				// Check if we need to write the indentation chars first
-				const size_t stackPointer = reprap.GetGCodes()->GetStackPointer();
-				if (stackPointer > 0)
-				{
-					// First, make sure we get the indentation right
-					char indentation[StackSize * 2 + 1];
-					for(size_t i = 0; i < stackPointer * 2; i++)
-					{
-						indentation[i] = ' ';
-					}
-					indentation[stackPointer * 2] = 0;
-
-					// Append the indentation string
-					usbOutputBuffer->cat(indentation);
-				}
-
 				// Append the message string
 				usbOutputBuffer->cat(message);
 			}
@@ -2667,7 +2656,7 @@ bool Platform::GetFirmwarePin(int logicalPin, PinAccess access, Pin& firmwarePin
 		}
 		else if (access == PinAccess::pwm || access == PinAccess::servo)
 		{
-			desiredMode = OUTPUT_PWM;
+			desiredMode = (invert) ? OUTPUT_PWM_HIGH : OUTPUT_PWM_LOW;
 		}
 		else
 		{
diff --git a/src/Platform.h b/src/Platform.h
index 7d20dc00..958f5557 100644
--- a/src/Platform.h
+++ b/src/Platform.h
@@ -481,10 +481,10 @@ public:
 	void SetPhysicalDrives(size_t drive, uint32_t physicalDrives);
 	uint32_t GetPhysicalDrives(size_t drive) const;
 	void SetDirection(size_t drive, bool direction);
-	void SetDirectionValue(size_t drive, bool dVal);
-	bool GetDirectionValue(size_t drive) const;
-	void SetEnableValue(size_t drive, bool eVal);
-	bool GetEnableValue(size_t drive) const;
+	void SetDirectionValue(size_t driver, bool dVal);
+	bool GetDirectionValue(size_t driver) const;
+	void SetEnableValue(size_t driver, bool eVal);
+	bool GetEnableValue(size_t driver) const;
 	void EnableDriver(size_t driver);
 	void DisableDriver(size_t driver);
 	void EnableDrive(size_t drive);
@@ -565,26 +565,43 @@ public:
 
 	// Heat and temperature
 
-	float GetTemperature(size_t heater, TemperatureError& err); // Result is in degrees Celsius
+	float GetTemperature(size_t heater, TemperatureError& err) // Result is in degrees Celsius
+	pre (heater < HEATERS);
+
 	float GetZProbeTemperature();							// Get our best estimate of the Z probe temperature
-	void SetHeater(size_t heater, float power);				// power is a fraction in [0,1]
+
+	void SetHeater(size_t heater, float power)				// power is a fraction in [0,1]
+	pre (heater < HEATERS);
+
 	uint32_t HeatSampleInterval() const;
 	void SetHeatSampleTime(float st);
 	float GetHeatSampleTime() const;
 	void SetPidParameters(size_t heater, const PidParameters& params);
-	const PidParameters& GetPidParameters(size_t heater) const;
+
+	const PidParameters& GetPidParameters(size_t heater) const
+	pre (heater < HEATERS);
+
 	Thermistor& GetThermistor(size_t heater)
 	pre (heater < HEATERS)
 	{
 		return thermistors[heater];
 	}
-	void SetThermistorNumber(size_t heater, size_t thermistor);
-	int GetThermistorNumber(size_t heater) const;
-	bool IsThermistorChannel(uint8_t heater) const;
-	bool IsThermocoupleChannel(uint8_t heater) const;
-	bool IsRtdChannel(uint8_t heater) const;
-	void SetTemperatureLimit(float t);
-	float GetTemperatureLimit() const { return temperatureLimit; }
+
+	void SetThermistorNumber(size_t heater, size_t thermistor)
+	pre(heater < HEATERS; thermistor < HEATERS);
+
+	int GetThermistorNumber(size_t heater) const
+	pre (heater < HEATERS);
+
+	bool IsThermistorChannel(uint8_t heater) const
+	pre(heater < HEATERS);
+
+	bool IsThermocoupleChannel(uint8_t heater) const
+	pre(heater < HEATERS);
+
+	bool IsRtdChannel(uint8_t heater) const
+	pre(heater < HEATERS);
+
 	void UpdateConfiguredHeaters();
 	bool AnyHeaterHot(uint16_t heaters, float t);			// called to see if we need to turn on the hot end fan
 
@@ -722,7 +739,9 @@ private:
 
 	void SetDriverCurrent(size_t driver, float current, bool isPercent);
 	void UpdateMotorCurrent(size_t driver);
-	void SetDriverDirection(uint8_t driver, bool direction);
+	void SetDriverDirection(uint8_t driver, bool direction)
+	pre(driver < DRIVES);
+
 	static uint32_t CalcDriverBitmap(size_t driver);			// calculate the step bit for this driver
 
 	volatile DriverStatus driverState[DRIVES];
@@ -786,7 +805,6 @@ private:
 	Pin spiTempSenseCsPins[MaxSpiTempSensors];
 	uint32_t configuredHeaters;										// bitmask of all heaters in use
 	uint32_t heatSampleTicks;
-	float temperatureLimit;
 
 	// Fans
 
@@ -1048,21 +1066,19 @@ inline bool Platform::GetDirectionValue(size_t drive) const
 
 inline void Platform::SetDriverDirection(uint8_t driver, bool direction)
 {
-	if (driver < DRIVES)
-	{
-		bool d = (direction == FORWARDS) ? directions[driver] : !directions[driver];
-		digitalWrite(DIRECTION_PINS[driver], d);
-	}
+	bool d = (direction == FORWARDS) ? directions[driver] : !directions[driver];
+	digitalWrite(DIRECTION_PINS[driver], d);
 }
 
-inline void Platform::SetEnableValue(size_t drive, bool eVal)
+inline void Platform::SetEnableValue(size_t driver, bool eVal)
 {
-	enableValues[drive] = eVal;
+	enableValues[driver] = eVal;
+	DisableDriver(driver);				// disable the drive, because the enable polarity may have been wrong before
 }
 
-inline bool Platform::GetEnableValue(size_t drive) const
+inline bool Platform::GetEnableValue(size_t driver) const
 {
-	return enableValues[drive];
+	return enableValues[driver];
 }
 
 inline float Platform::AxisMaximum(size_t axis) const
diff --git a/src/Reprap.cpp b/src/Reprap.cpp
index 1a60b022..a7df8e3f 100644
--- a/src/Reprap.cpp
+++ b/src/Reprap.cpp
@@ -59,9 +59,9 @@ void RepRap::Init()
 		configFile = platform->GetDefaultFile();
 	}
 
-	if (gCodes->DoFileMacro(configFile, false))
+	if (gCodes->RunConfigFile(configFile))
 	{
-		while (gCodes->DoingFileMacro())
+		while (gCodes->IsRunningConfigFile())
 		{
 			// GCodes::Spin will read the macro and ensure DoingFileMacro returns false when it's done
 			Spin();
@@ -730,8 +730,8 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		response->catf(",\"coldExtrudeTemp\":%1.f", heat->ColdExtrude() ? 0 : HOT_ENOUGH_TO_EXTRUDE);
 		response->catf(",\"coldRetractTemp\":%1.f", heat->ColdExtrude() ? 0 : HOT_ENOUGH_TO_RETRACT);
 
-		// Maximum hotend temperature
-		response->catf(",\"tempLimit\":%1.f", platform->GetTemperatureLimit());
+		// Maximum hotend temperature - DWC just wants the highest one
+		response->catf(",\"tempLimit\":%1.f", heat->GetHighestTemperatureLimit());
 
 		// Endstops
 		uint16_t endstops = 0;
@@ -755,7 +755,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		size_t mountedCards = 0;
 		for(size_t i = 0; i < NumSdCards; i++)
 		{
-			if (reprap.GetPlatform()->GetMassStorage()->IsDriveMounted(i))
+			if (platform->GetMassStorage()->IsDriveMounted(i))
 			{
 				mountedCards |= (1 << i);
 			}
@@ -1329,14 +1329,14 @@ OutputBuffer *RepRap::GetFilelistResponse(const char *dir)
 	}
 
 	// If the requested volume is not mounted, report an error
-	if (!reprap.GetPlatform()->GetMassStorage()->CheckDriveMounted(dir))
+	if (!platform->GetMassStorage()->CheckDriveMounted(dir))
 	{
 		response->copy("{\"err\":1}");
 		return response;
 	}
 
 	// Check if the directory exists
-	if (!reprap.GetPlatform()->GetMassStorage()->DirectoryExists(dir))
+	if (!platform->GetMassStorage()->DirectoryExists(dir))
 	{
 		response->copy("{\"err\":2}");
 		return response;
diff --git a/src/Tool.cpp b/src/Tool.cpp
index 4c4e7c53..a7abb201 100644
--- a/src/Tool.cpp
+++ b/src/Tool.cpp
@@ -173,7 +173,7 @@ float Tool::MaxFeedrate() const
 	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	for (size_t d = 0; d < driveCount; d++)
 	{
-		float mf = reprap.GetPlatform()->MaxFeedrate(drives[d] + numAxes);
+		const float mf = reprap.GetPlatform()->MaxFeedrate(drives[d] + numAxes);
 		if (mf > result)
 		{
 			result = mf;
@@ -193,7 +193,7 @@ float Tool::InstantDv() const
 	const size_t numAxes = reprap.GetGCodes()->GetNumAxes();
 	for (size_t d = 0; d < driveCount; d++)
 	{
-		float idv = reprap.GetPlatform()->ActualInstantDv(drives[d] + numAxes);
+		const float idv = reprap.GetPlatform()->ActualInstantDv(drives[d] + numAxes);
 		if (idv < result)
 		{
 			result = idv;
@@ -306,7 +306,7 @@ void Tool::SetVariables(const float* standby, const float* active)
 		}
 		else
 		{
-			const float temperatureLimit = reprap.GetPlatform()->GetTemperatureLimit();
+			const float temperatureLimit = reprap.GetHeat()->GetTemperatureLimit(heaters[heater]);
 			if (active[heater] < temperatureLimit)
 			{
 				activeTemperatures[heater] = active[heater];