Refactored LockMovementAndWaitForStandstill for multiple motion systems

13 files changed, 292 insertions, 268 deletions

diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index 9cb8b0c0..7e2f6a33 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -979,7 +979,7 @@ pre(driver.IsRemote())
 	case 0:
 		if (gb.SeenAny("RS"))
 		{
-			if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+			if (!reprap.GetGCodes().LockAllMovementSystemsAndWaitForStandstill(gb))
 			{
 				return GCodeResult::notFinished;
 			}
@@ -993,7 +993,7 @@ pre(driver.IsRemote())
 	case 1:
 		if (gb.SeenAny("STERID"))
 		{
-			if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+			if (!reprap.GetGCodes().LockAllMovementSystemsAndWaitForStandstill(gb))
 			{
 				return GCodeResult::notFinished;
 			}
diff --git a/src/Endstops/EndstopsManager.cpp b/src/Endstops/EndstopsManager.cpp
index feb6b9de..acf8869b 100644
--- a/src/Endstops/EndstopsManager.cpp
+++ b/src/Endstops/EndstopsManager.cpp
@@ -376,7 +376,7 @@ GCodeResult EndstopsManager::HandleM574(GCodeBuffer& gb, const StringRef& reply,
 	}
 
 	// If we get here then axes were specified so we are setting endstop parameters
-	if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+	if (!reprap.GetGCodes().LockAllMovementSystemsAndWaitForStandstill(gb))
 	{
 		return GCodeResult::notFinished;
 	}
diff --git a/src/Fans/LedStripDriver.cpp b/src/Fans/LedStripDriver.cpp
index ccb6ae35..81c2a65a 100644
--- a/src/Fans/LedStripDriver.cpp
+++ b/src/Fans/LedStripDriver.cpp
@@ -508,7 +508,7 @@ GCodeResult LedStripDriver::SetColours(GCodeBuffer& gb, const StringRef& reply)
 	// Interrupts are disabled while bit-banging data, which will mess up the step timing. So make sure movement has stopped if we are going to use bit-banging
 	if (MustStopMovement(gb))
 	{
-		if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+		if (!reprap.GetGCodes().LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			return GCodeResult::notFinished;
 		}
diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp
index e9c5fa17..cf3b7b7b 100644
--- a/src/GCodes/GCodes.cpp
+++ b/src/GCodes/GCodes.cpp
@@ -254,10 +254,18 @@ void GCodes::Reset() noexcept
 	g68Angle = g68Centre[0] = g68Centre[1] = 0.0;		// no coordinate rotation
 #endif
 
+#if SUPPORT_ASYNC_MOVES
+	reprap.GetMove().GetKinematics().GetAssumedInitialPosition(numVisibleAxes, lastKnownMachinePositions);
+#endif
+
 	for (MovementState& ms : moveStates)
 	{
 		ms.Reset();
+#if SUPPORT_ASYNC_MOVES
+		memcpyf(ms.coords, lastKnownMachinePositions, numVisibleAxes);
+#else
 		reprap.GetMove().GetKinematics().GetAssumedInitialPosition(numVisibleAxes, ms.coords);
+#endif
 		ToolOffsetInverseTransform(ms);
 	}
 
@@ -607,13 +615,18 @@ bool GCodes::DoFilePrint(GCodeBuffer& gb, const StringRef& reply) noexcept
 		if (gb.IsFileFinished())
 		{
 			const bool printFileFinished = (gb.LatestMachineState().GetPrevious() == nullptr);
-			if (!LockMovementAndWaitForStandstill(gb
 # if SUPPORT_ASYNC_MOVES
-													, printFileFinished
+			if (printFileFinished)
+			{
+				DoSync(gb);														// wait until the other input stream has caught up
+			}
+			else
 # endif
-													))				// wait until movement has finished and deferred command queue has caught up
 			{
-				return false;
+				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))			// wait until movement has finished and deferred command queue has caught up
+				{
+					return false;
+				}
 			}
 
 			if (printFileFinished)
@@ -750,16 +763,21 @@ bool GCodes::DoFilePrint(GCodeBuffer& gb, const StringRef& reply) noexcept
 				return true;
 			}
 
-			gb.Init();											// mark buffer as empty
+			gb.Init();															// mark buffer as empty
 
 			const bool printFileFinished = (gb.LatestMachineState().GetPrevious() == nullptr);
-			if (!LockMovementAndWaitForStandstill(gb
-#if SUPPORT_ASYNC_MOVES
-													, printFileFinished
-#endif
-													))			// wait until movement has finished and deferred command queue has caught up
+# if SUPPORT_ASYNC_MOVES
+			if (printFileFinished)
 			{
-				return false;
+				DoSync(gb);														// wait until the other input stream has caught up
+			}
+			else
+# endif
+			{
+				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))			// wait until movement has finished and deferred command queue has caught up
+				{
+					return false;
+				}
 			}
 
 			if (printFileFinished)
@@ -882,7 +900,7 @@ void GCodes::DoEmergencyStop() noexcept
 bool GCodes::DoSynchronousPause(GCodeBuffer& gb, PrintPausedReason reason, GCodeState newState) noexcept
 {
 	// Pausing because of a command in the file itself
-	if (!LockMovementAndWaitForStandstill(gb))
+	if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 	{
 		return false;
 	}
@@ -1503,22 +1521,47 @@ void GCodes::Diagnostics(MessageType mtype) noexcept
 	}
 }
 
+#if SUPPORT_ASYNC_MOVES
+
+// Lock the movement system that we currently use and wait for it to stop
+bool GCodes::LockCurrentMovementSystemAndWaitForStandstill(GCodeBuffer& gb) noexcept
+{
+	return LockMovementSystemAndWaitForStandstill(gb, gb.GetActiveQueueNumber());
+}
+
+// Lock all movement systems and wait for them to stop
+bool GCodes::LockAllMovementSystemsAndWaitForStandstill(GCodeBuffer& gb) noexcept
+{
+	unsigned int i = 0;
+	while (LockMovementSystemAndWaitForStandstill(gb, i))
+	{
+		++i;
+		if (i == NumMovementSystems)
+		{
+			return true;
+		}
+	}
+
+	// We failed to lock the ith movement system. To avoid possible deadlock we need to release any later locks that we have.
+	UnlockMovementFrom(gb, i + 1);
+	return false;
+}
+
+#endif
+
 // Lock movement and wait for pending moves to finish.
 // Return true if successful, false if we need to try again later.
 // As a side-effect it updates the user coordinates from the machine coordinates.
-bool GCodes::LockMovementAndWaitForStandstill(GCodeBuffer& gb
-#if SUPPORT_ASYNC_MOVES
-												, bool sync
-#endif
-															) noexcept
+
+bool GCodes::LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned int msNumber) noexcept
 {
 	// Lock movement to stop another source adding moves to the queue
-	if (!LockMovement(gb))
+	if (!LockResource(gb, MoveResourceBase + msNumber))
 	{
 		return false;
 	}
 
-	MovementState& ms = GetMovementState(gb);
+	MovementState& ms = moveStates[msNumber];
 	if (ms.segmentsLeft != 0)						// has the last move generated been fully transferred to the movement queue?
 	{
 		return false;								// if no
@@ -1530,71 +1573,16 @@ bool GCodes::LockMovementAndWaitForStandstill(GCodeBuffer& gb
 	case GCodeChannel::Queue2:
 		break;
 
-#if SUPPORT_ASYNC_MOVES
-	case GCodeChannel::File:
-		if (!reprap.GetMove().WaitingForAllMovesFinished(0))
-		{
-			return false;
-		}
-		if (!(QueuedGCode()->IsIdle() && moveStates[0].codeQueue->IsIdle()))
-		{
-			return false;
-		}
-
-		// Now that we know that pending commands for this queue are completed, we can try to sync with other GCode buffers
-		if (sync && !gb.ExecutingAll() && File2GCode()->IsDoingFile())
-		{
-			const bool ret = SyncWith(gb, *File2GCode());
-			if (ret)
-			{
-				gb.MotionStopped();
-			}
-			//if (!ret) { debugPrintf("Lock wait 7, queue %u\n", gb.GetQueueNumberToLock()); }
-			return ret;
-		}
-		break;
-
-	case GCodeChannel::File2:
-		if (!reprap.GetMove().WaitingForAllMovesFinished(1))
-		{
-			return false;
-		}
-		if (!(Queue2GCode()->IsIdle() && moveStates[1].codeQueue->IsIdle()))
-		{
-			return false;
-		}
-
-		// Now that we know that pending commands for this queue are completed, we can try to sync with other GCode buffers
-		if (sync && !gb.ExecutingAll() && FileGCode()->IsDoingFile())
-		{
-			const bool ret = SyncWith(gb, *FileGCode());
-			if (ret)
-			{
-				gb.MotionStopped();
-			}
-			return ret;
-		}
-		break;
-#endif
-
 	default:
-		if (   !reprap.GetMove().WaitingForAllMovesFinished(0)
-#if SUPPORT_ASYNC_MOVES
-			|| !reprap.GetMove().WaitingForAllMovesFinished(1)
-#endif
-		   )
+		if (!reprap.GetMove().WaitingForAllMovesFinished(msNumber))
 		{
 			return false;
 		}
-
-		if (   !(QueuedGCode()->IsIdle() && moveStates[0].codeQueue->IsIdle())
-#if SUPPORT_ASYNC_MOVES
-			&& !(Queue2GCode()->IsIdle() && moveStates[1].codeQueue->IsIdle())
-#endif
-		   )
+		if (!(QueuedGCode()->IsIdle() && moveStates[msNumber].codeQueue->IsIdle()))
 		{
 			return false;
 		}
+		break;
 	}
 
 	gb.MotionStopped();									// must do this after we have finished waiting, so that we don't stop waiting when executing G4
@@ -1880,7 +1868,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated) THROWS(GCodeExc
 		const int ival = gb.GetIValue();
 		if (ival >= 1 && ival <= 4)
 		{
-			if (!LockMovementAndWaitForStandstill(gb))
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 			{
 				return false;
 			}
@@ -2687,7 +2675,7 @@ void GCodes::FinaliseMove(GCodeBuffer& gb, MovementState& ms) noexcept
 // But the expected position was saved by buildObjects when the state changed from printing a cancelled object to printing a live object.
 bool GCodes::TravelToStartPoint(GCodeBuffer& gb) noexcept
 {
-	if (!LockMovementAndWaitForStandstill(gb))				// update the user position from the machine position
+	if (!LockCurrentMovementSystemAndWaitForStandstill(gb))				// update the user position from the machine position
 	{
 		return false;
 	}
@@ -3240,7 +3228,7 @@ GCodeResult GCodes::DoDwell(GCodeBuffer& gb) THROWS(GCodeException)
 	// This is so that G4 can be used in a trigger or daemon macro file without pausing motion, when the macro doesn't itself command any motion.
 	if (gb.WasMotionCommanded())
 	{
-		if (!LockMovementAndWaitForStandstillNoSync(gb))
+		if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			return GCodeResult::notFinished;
 		}
@@ -3550,7 +3538,7 @@ GCodeResult GCodes::ManageTool(GCodeBuffer& gb, const StringRef& reply)
 
 	if (seen)
 	{
-		if (!LockMovementAndWaitForStandstill(gb))
+		if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			return GCodeResult::notFinished;
 		}
@@ -4733,57 +4721,57 @@ bool GCodes::LockFileSystem(const GCodeBuffer &gb) noexcept
 	return LockResource(gb, FileSystemResource);
 }
 
+#if SUPPORT_ASYNC_MOVES
+
 // The movement lock is special because we have one for each motion system
-// Lock movement
+
+// Lock the movement system that we currently use
 bool GCodes::LockMovement(const GCodeBuffer& gb) noexcept
 {
-#if SUPPORT_ASYNC_MOVES
-	return LockResource(gb, MoveResourceBase + gb.GetQueueNumberToLock());
-#else
-	return LockResource(gb, MoveResourceBase);
-#endif
+	return LockMovement(gb, gb.GetQueueNumberToLock());
 }
 
 // Lock movement on all motion systems
 bool GCodes::LockAllMovement(const GCodeBuffer& gb) noexcept
 {
-#if SUPPORT_ASYNC_MOVES
-	for (Resource r = MoveResourceBase; r < MoveResourceBase + NumMovementSystems; ++r)
+	for (unsigned int i = 0; i < NumMovementSystems; ++i)
 	{
-		if (!LockResource(gb, r))
+		if (!LockMovement(gb, i))
 		{
+			UnlockMovementFrom(gb, i + 1);			// release any higher locks we own to avoid deadlock
 			return false;
 		}
 	}
 	return true;
-#else
-	return LockMovement(gb);
-#endif
 }
 
-// Grab the movement lock even if another channel owns it
-void GCodes::GrabMovement(const GCodeBuffer& gb) noexcept
+// Release movement locks greater than the specified one
+void GCodes::UnlockMovementFrom(const GCodeBuffer& gb, unsigned int msNumber) noexcept
 {
-#if SUPPORT_ASYNC_MOVES
-	GrabResource(gb, MoveResourceBase + gb.GetQueueNumberToLock());
-#else
-	GrabResource(gb, MoveResourceBase);
-#endif
+	while (msNumber < NumMovementSystems)
+	{
+		UnlockMovement(gb, msNumber);
+		++msNumber;
+	}
 }
 
-// Release the movement lock
+// Release all movement locks that we own
 void GCodes::UnlockMovement(const GCodeBuffer& gb) noexcept
 {
-#if SUPPORT_ASYNC_MOVES
-	for (Resource r = MoveResourceBase; r < MoveResourceBase + NumMovementSystems; ++r)
+	UnlockMovementFrom(gb, 0);
+}
+
+// Grab all movement locks even if other channels owns them
+void GCodes::GrabMovement(const GCodeBuffer& gb) noexcept
+{
+	for (unsigned int i = 0; i < NumMovementSystems; ++i)
 	{
-		UnlockResource(gb, r);
+		GrabResource(gb, MoveResourceBase + i);
 	}
-#else
-	UnlockResource(gb, MoveResourceBase);
-#endif
 }
 
+#endif
+
 // Unlock the resource if we own it
 void GCodes::UnlockResource(const GCodeBuffer& gb, Resource r) noexcept
 {
@@ -4942,10 +4930,14 @@ void GCodes::AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap a
 }
 
 // Synchronise motion systems and update user coordinates.
-// This is called after we have checked that the motion system for thisGb has completed all moves.
 // Return true if synced, false if we need to wait longer.
 bool GCodes::SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept
 {
+	if (!LockCurrentMovementSystemAndWaitForStandstill(thisGb))
+	{
+		return false;
+	}
+
 	switch (thisGb.syncState)
 	{
 	case GCodeBuffer::SyncState::running:
@@ -4992,7 +4984,7 @@ bool GCodes::SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept
 				thisGb.syncState = GCodeBuffer::SyncState::running;
 				return true;
 			}
-			// We are not the primary, so wait for the other onput channel to complete the current command
+			// We are not the primary, so wait for the other output channel to complete the current command
 			return otherGb.IsLaterThan(thisGb);
 		}
 	}
@@ -5000,6 +4992,14 @@ bool GCodes::SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept
 	return false;			// unreachable code, to keep Eclipse happy
 }
 
+// Synchronise the other motion system with this one. Return true if done, false if we need to wait for it to catch up.
+bool GCodes::DoSync(GCodeBuffer& gb) noexcept
+{
+	return (&gb == FileGCode()) ? SyncWith(gb, *File2GCode())
+			: (&gb == File2GCode()) ? SyncWith(gb, *FileGCode())
+				: true;
+}
+
 void GCodes::UpdateUserCoordinatesAndReleaseOwnedAxes(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept
 {
 	// Get the position of all axes by combining positions from the queues
diff --git a/src/GCodes/GCodes.h b/src/GCodes/GCodes.h
index e495349a..d8952f57 100644
--- a/src/GCodes/GCodes.h
+++ b/src/GCodes/GCodes.h
@@ -187,12 +187,9 @@ public:
 	const char *GetAxisLetters() const noexcept { return axisLetters; }			// Return a null-terminated string of axis letters indexed by drive
 	size_t GetAxisNumberForLetter(const char axisLetter) const noexcept;
 	MachineType GetMachineType() const noexcept { return machineType; }
-	bool LockMovementAndWaitForStandstill(GCodeBuffer& gb
-#if SUPPORT_ASYNC_MOVES
-											, bool sync = true
-#endif
-															) noexcept;			// Lock movement and wait for pending moves to finish
-	bool LockMovementAndWaitForStandstillNoSync(GCodeBuffer& gb) noexcept;		// Lock movement and wait for pending moves to finish but don't sync if using multiple movement queues
+	bool LockMovementSystemAndWaitForStandstill(GCodeBuffer& gb, unsigned int msNumber) noexcept;	// Lock a movement system and wait for pending moves to finish
+	bool LockCurrentMovementSystemAndWaitForStandstill(GCodeBuffer& gb) noexcept;	// Lock movement and wait for pending moves to finish
+	bool LockAllMovementSystemsAndWaitForStandstill(GCodeBuffer& gb) noexcept;	// Lock movement and wait for all motion systems to reach standstill
 
 #if SUPPORT_DIRECT_LCD
 	void SetPrimarySpeedFactor(float factor) noexcept;							// Set the speed factor
@@ -342,12 +339,17 @@ private:
 
 	bool LockResource(const GCodeBuffer& gb, Resource r) noexcept;				// Lock the resource, returning true if success
 	bool LockFileSystem(const GCodeBuffer& gb) noexcept;						// Lock the unshareable parts of the file system
-	bool LockMovement(const GCodeBuffer& gb) noexcept;							// Lock movement
-	bool LockAllMovement(const GCodeBuffer& gb) noexcept;						// Lock movement on all queues
+	bool LockMovement(const GCodeBuffer& gb) noexcept;							// Lock the movement system we are using
+	bool LockMovement(const GCodeBuffer& gb, unsigned int msNumber) noexcept;	// Lock a particular movement system
+	bool LockAllMovement(const GCodeBuffer& gb) noexcept;						// Lock all movement systems
 	void GrabResource(const GCodeBuffer& gb, Resource r) noexcept;				// Grab a resource even if it is already owned
-	void GrabMovement(const GCodeBuffer& gb) noexcept;							// Grab the movement lock even if it is already owned
+	void GrabMovement(const GCodeBuffer& gb) noexcept;							// Grab all movement locks even if they are already owned
 	void UnlockResource(const GCodeBuffer& gb, Resource r) noexcept;			// Unlock the resource if we own it
-	void UnlockMovement(const GCodeBuffer& gb) noexcept;						// Unlock the movement resource if we own it
+	void UnlockMovement(const GCodeBuffer& gb) noexcept;						// Unlock the movement system we are using, if we own it
+	void UnlockMovement(const GCodeBuffer& gb, unsigned int msNumber) noexcept;	// Unlock a particular movement system, if we own it
+#if SUPPORT_ASYNC_MOVES
+	void UnlockMovementFrom(const GCodeBuffer& gb, unsigned int firstMsNumber) noexcept;	// Release movement locks greater or equal to than the specified one
+#endif
 
 	bool SpinGCodeBuffer(GCodeBuffer& gb) noexcept;								// Do some work on an input channel
 	bool StartNextGCode(GCodeBuffer& gb, const StringRef& reply) noexcept;		// Fetch a new or old GCode and process it
@@ -546,10 +548,13 @@ private:
 	MovementState& GetMovementState(const GCodeBuffer& gb) noexcept;			// Get a reference to the movement state associated with the specified GCode buffer
 
 #if SUPPORT_ASYNC_MOVES
-	GCodeResult SelectMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);	// Handle M596
-	GCodeResult CollisionAvoidance(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);		// Handle M597
+	GCodeResult SelectMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);		// Handle M596
+	GCodeResult CollisionAvoidance(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);			// Handle M597
+	GCodeResult SyncMovementSystems(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);		// Handle M598
+	GCodeResult ExecuteM400(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);				// Handle M400
 	void AllocateAxes(const GCodeBuffer& gb, MovementState& ms, AxesBitmap axes) THROWS(GCodeException);	// allocate axes to a movement state
-	bool SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept;	// synchronise motion systems
+	bool DoSync(GCodeBuffer& gb) noexcept;																	// sync with the other stream returning true if done, false if we need to wait for it
+	bool SyncWith(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept;								// synchronise motion systems
 	void UpdateUserCoordinatesAndReleaseOwnedAxes(GCodeBuffer& thisGb, const GCodeBuffer& otherGb) noexcept;
 #endif
 
@@ -716,6 +721,7 @@ private:
 #if SUPPORT_ASYNC_MOVES
 	AxesBitmap axesAndExtrudersMoved;			// axes and extruders that have moved since the last sync
 	CollisionAvoider collisionChecker;
+	float lastKnownMachinePositions[MaxAxes];
 #endif
 
 #if HAS_MASS_STORAGE
@@ -750,18 +756,28 @@ inline float GCodes::GetTotalBabyStepOffset(size_t axis) const noexcept
 	return currentBabyStepOffsets[axis];
 }
 
-#if SUPPORT_ASYNC_MOVES
+// Lock a particular movement system
+inline bool GCodes::LockMovement(const GCodeBuffer& gb, unsigned int msNumber) noexcept
+{
+	return LockResource(gb, MoveResourceBase + msNumber);
+}
 
-inline bool GCodes::LockMovementAndWaitForStandstillNoSync(GCodeBuffer& gb) noexcept
+// Unlock a particular movement system, if we own it
+inline void GCodes::UnlockMovement(const GCodeBuffer& gb, unsigned int msNumber) noexcept
 {
-	return LockMovementAndWaitForStandstill(gb, false);
+	return UnlockResource(gb, MoveResourceBase + msNumber);
 }
 
-#else
+#if !SUPPORT_ASYNC_MOVES
+
+inline bool GCodes::LockCurrentMovementSystemAndWaitForStandstill(GCodeBuffer& gb) noexcept
+{
+	return LockMovementSystemAndWaitForStandstill(gb, 0);
+}
 
-inline bool GCodes::LockMovementAndWaitForStandstillNoSync(GCodeBuffer& gb) noexcept
+inline bool GCodes::LockAllMovementSystemsAndWaitForStandstill(GCodeBuffer& gb) noexcept
 {
-	return LockMovementAndWaitForStandstill(gb);
+	return LockMovementSystemAndWaitForStandstill(gb, 0);
 }
 
 // Get a reference to the movement state associated with the specified GCode buffer
@@ -780,6 +796,30 @@ inline const MovementState& GCodes::GetCurrentMovementState(const ObjectExplorat
 	return moveStates[0];
 }
 
+// Lock the movement system we are using
+inline bool GCodes::LockMovement(const GCodeBuffer& gb) noexcept
+{
+	return LockResource(gb, MoveResourceBase);
+}
+
+// Lock all movement systems
+inline bool GCodes::LockAllMovement(const GCodeBuffer& gb) noexcept
+{
+	return LockResource(gb, MoveResourceBase);
+}
+
+// Unlock the movement system we are using, if we own it
+inline void GCodes::UnlockMovement(const GCodeBuffer& gb) noexcept
+{
+	return UnlockResource(gb, MoveResourceBase);
+}
+
+// Grab all movement locks even if they are already owned
+inline void GCodes::GrabMovement(const GCodeBuffer& gb) noexcept
+{
+	GrabResource(gb, MoveResourceBase);
+}
+
 #endif
 
 //*****************************************************************************************************
diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp
index b9aa5be3..8b0f75ed 100644
--- a/src/GCodes/GCodes2.cpp
+++ b/src/GCodes/GCodes2.cpp
@@ -285,7 +285,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 		case 17:	// Select XY plane for G2/G3
 		case 18:	// Select XZ plane
 		case 19:	// Select YZ plane
-			if (!LockMovementAndWaitForStandstill(gb))			// do this in case a G2 or G3 command is in progress
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))			// do this in case a G2 or G3 command is in progress
 			{
 				return false;
 			}
@@ -301,7 +301,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 		case 28: // Home
-			if (!LockMovementAndWaitForStandstill(gb))
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 			{
 				return false;
 			}
@@ -310,7 +310,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 		case 29: // Grid-based bed probing
-			if (!LockMovementAndWaitForStandstill(gb))			// do this first to make sure that a new grid isn't being defined
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))			// do this first to make sure that a new grid isn't being defined
 			{
 				return false;
 			}
@@ -375,7 +375,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 		case 30: // Z probe/manually set at a position and set that as point P
-			if (!LockMovementAndWaitForStandstill(gb))
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 			{
 				return false;
 			}
@@ -398,7 +398,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 		case 32: // Probe Z at multiple positions and generate the bed transform
-			if (!LockMovementAndWaitForStandstill(gb))
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 			{
 				return false;
 			}
@@ -414,7 +414,7 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 		case 38: // Straight probe - move until either the probe is triggered or the commanded move ends
-			if (!LockMovementAndWaitForStandstill(gb))
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 			{
 				return false;
 			}
@@ -464,12 +464,12 @@ bool GCodes::HandleGcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			break;
 
 #if SUPPORT_COORDINATE_ROTATION
-		case 68:
+		case 68:	// Coordinate rotation
 			result = HandleG68(gb, reply);
 			break;
 
-		case 69:
-			if (!LockMovementAndWaitForStandstill(gb))
+		case 69:	// Cancel coordinate rotation
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 			{
 				return false;
 			}
@@ -557,34 +557,10 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			// These commands are executed by all GCode processors, at least to start with
 			break;
 
-		case 17:
-		case 18:
-		case 81:
-		case 84:
-		case 190:
-		case 191:
-		case 206:
-		case 375:
-		case 451:
-		case 452:
-		case 453:
-		case 561:
-		case 574:
-		case 665:
-		case 666:
-		case 669:
-		case 671:
-		case 918:
-			// These commands cause synchronisation but are then executed by just the primary processor. The code to implement the command also calls LockMovementAndWaitForStandstill.
-			if (!LockMovementAndWaitForStandstill(gb))
-			{
-				return false;
-			}
-			// no break
 		default:
 			// All remaining commands are executed by the primary processor only
 			HandleReply(gb, GCodeResult::ok, "");
-			return true;			// we don't simulate most M codes
+			return true;
 		}
 	}
 #endif
@@ -658,10 +634,17 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				if (gb.IsFileChannel())
 				{
 					// Stopping a job because of a command in the file
-					if (!LockMovementAndWaitForStandstill(gb))		// wait until everything has stopped and deferred command queue has caught up
+#if SUPPORT_ASYNC_MOVES
+					if (!DoSync(gb))
 					{
 						return false;
 					}
+#else
+					if (!LockCurrentMovementSystemAndWaitForStandstill(gb))	// wait until everything has stopped and deferred command queue has caught up
+					{
+						return false;
+					}
+#endif
 					if (&gb == FileGCode())
 					{
 						isWaiting = cancelWait = false;				// we may have been waiting for temperatures to be reached
@@ -671,7 +654,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				else if (pauseState == PauseState::paused)
 				{
 					// Cancelling a print that has been paused
-					if (!LockMovementAndWaitForStandstill(gb))		// make sure everything has stopped
+					if (!LockAllMovementSystemsAndWaitForStandstill(gb))	// make sure everything has stopped
 					{
 						return false;
 					}
@@ -808,7 +791,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			case 17: // Motors on
 			case 18: // Motors off
 			case 84:
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -995,7 +978,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					break;
 				}
 
-				if (code == 32 && !LockMovementAndWaitForStandstill(gb))
+				if (code == 32 && !LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -1045,7 +1028,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				}
 				else
 				{
-					if (!LockMovementAndWaitForStandstill(gb))
+					if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 					{
 						return false;
 					}
@@ -1442,7 +1425,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 81:	// ATX power off
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -1478,7 +1461,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					{
 						if (gb.Seen(axisLetters[axis]))
 						{
-							if (!LockMovementAndWaitForStandstillNoSync(gb))
+							if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 							{
 								return false;
 							}
@@ -1492,7 +1475,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 
 					if (gb.Seen(extrudeLetter))
 					{
-						if (!LockMovementAndWaitForStandstillNoSync(gb))
+						if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 						{
 							return false;
 						}
@@ -1669,7 +1652,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 						MoveResourceBase
 #endif
 						);
-					if (!LockMovementAndWaitForStandstill(gb))		// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
+					if (!LockCurrentMovementSystemAndWaitForStandstill(gb))		// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
 					{
 						return false;
 					}
@@ -1738,7 +1721,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					if (code == 109 && ms.currentTool == nullptr)
 					{
 						// Switch to the tool
-						if (!LockMovementAndWaitForStandstill(gb))
+						if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 						{
 							return false;
 						}
@@ -1893,7 +1876,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 116: // Wait for set temperatures
-				if (!LockMovementAndWaitForStandstillNoSync(gb))	// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
+				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))	// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
 				{
 					return false;
 				}
@@ -2228,7 +2211,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 
 			case 190: // Set bed temperature and wait
 			case 191: // Set chamber temperature and wait
-				if (!LockMovementAndWaitForStandstill(gb))			// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
+				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))		// wait until movement has finished and deferred command queue has caught up to avoid out-of-order execution
 				{
 					return false;
 				}
@@ -2731,7 +2714,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					bool seen = false;
 					if (gb.Seen('P'))
 					{
-						if (!LockMovementAndWaitForStandstillNoSync(gb))
+						if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 						{
 							return false;
 						}
@@ -2740,7 +2723,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					}
 					if (gb.Seen('S'))
 					{
-						if (!LockMovementAndWaitForStandstillNoSync(gb))
+						if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 						{
 							return false;
 						}
@@ -2749,7 +2732,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					}
 					if (gb.Seen('R'))
 					{
-						if (!LockMovementAndWaitForStandstillNoSync(gb))
+						if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 						{
 							return false;
 						}
@@ -2810,7 +2793,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					{
 						if (gb.Seen(axisLetters[axis]))
 						{
-							if (!LockMovementAndWaitForStandstillNoSync(gb))
+							if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 							{
 								return false;
 							}
@@ -2832,7 +2815,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 
 					if (gb.Seen(extrudeLetter))
 					{
-						if (!LockMovementAndWaitForStandstillNoSync(gb))
+						if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 						{
 							return false;
 						}
@@ -2885,7 +2868,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 375: // Load grid and height map from file and enable compensation
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -2912,10 +2895,14 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 400: // Wait for current moves to finish
-				if (!LockMovementAndWaitForStandstill(gb))
+#if SUPPORT_ASYNC_MOVES
+				result = ExecuteM400(gb, reply);
+#else
+				if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 				{
 					return false;
 				}
+#endif
 				break;
 
 			case 401: // Deploy Z probe
@@ -3012,7 +2999,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 451: // Select FFF printer mode
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -3025,7 +3012,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 
 #if SUPPORT_LASER
 			case 452: // Select laser mode
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -3060,7 +3047,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 #endif
 
 			case 453: // Select CNC mode
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -3131,7 +3118,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			case 502: // Revert to default "factory settings" ignoring values in config-override.g
 				if (!gb.LatestMachineState().runningM502)							// avoid recursion
 				{
-					if (!LockMovementAndWaitForStandstillNoSync(gb))
+					if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 					{
 						return false;
 					}
@@ -3196,7 +3183,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				if (gb.Seen('P'))
 				{
 					// Lock movement to try to prevent other threads opening system files while we change the system path
-					if (!LockMovementAndWaitForStandstillNoSync(gb))
+					if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 					{
 						return false;
 					}
@@ -3431,7 +3418,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 #endif
 
 			case 561: // Set identity transform and disable height map
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -3595,11 +3582,6 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			// case 573 was report heater average PWM but is no longer supported because you can use "echo heat/heaters[N].avgPwm" instead
 
 			case 574: // Set endstop configuration
-				// We may be about to delete endstops, so make sure we are not executing a move that uses them
-				if (!LockMovementAndWaitForStandstill(gb))
-				{
-					return false;
-				}
 				result = platform.GetEndstops().HandleM574(gb, reply, outBuf);
 				break;
 
@@ -3863,6 +3845,10 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			case 597:	// Collision avoidance
 				result = CollisionAvoidance(gb, reply);
 				break;
+
+			case 598:	// Sync
+				result = SyncMovementSystems(gb, reply);
+				break;
 #endif
 			// For cases 600 and 601, see 226
 
@@ -3870,7 +3856,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 
 #if SUPPORT_LINEAR_DELTA
 			case 665: // Set delta configuration
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -3913,7 +3899,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 666: // Set delta endstop adjustments
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -3935,7 +3921,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 				break;
 
 			case 669:	// Set kinematics and parameters for non-delta kinematics
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -3993,7 +3979,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 #endif
 
 			case 671:	// Set Z leadscrew positions
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -4014,7 +4000,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 					reply.copy("Insufficient axes configured");
 					result = GCodeResult::error;
 				}
-				else if (!LockMovementAndWaitForStandstillNoSync(gb))
+				else if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					result = GCodeResult::notFinished;
 				}
@@ -4369,7 +4355,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 #endif
 							   )
 							{
-								if (!LockMovementAndWaitForStandstillNoSync(gb))
+								if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 								{
 									return false;
 								}
@@ -4387,7 +4373,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 #endif
 						   )
 						{
-							if (!LockMovementAndWaitForStandstillNoSync(gb))
+							if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 							{
 								return false;
 							}
@@ -4560,7 +4546,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 			case 918: // Configure direct-connect display
 # ifdef DUET_NG
 				// On Duet 2 configuring the display may affect the number of supported stepper drivers, so wait until there is no movement
-				if (!LockMovementAndWaitForStandstill(gb))
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 				{
 					return false;
 				}
@@ -4782,11 +4768,7 @@ bool GCodes::HandleTcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx
 
 	if (seen)
 	{
-		if (!LockMovementAndWaitForStandstill(gb
-#if SUPPORT_ASYNC_MOVES
-												, false		// the other motion system can run concurrently with the tool change
-#endif
-														))
+		if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			return false;
 		}
diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp
index b69a2101..d91df708 100644
--- a/src/GCodes/GCodes3.cpp
+++ b/src/GCodes/GCodes3.cpp
@@ -82,7 +82,7 @@ GCodeResult GCodes::SetPositions(GCodeBuffer& gb, const StringRef& reply) THROWS
 			const float axisValue = gb.GetFValue();
 			if (axesIncluded.IsEmpty())
 			{
-				if (!LockMovementAndWaitForStandstill(gb))	// lock movement and get current coordinates
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))	// lock movement and get current coordinates
 				{
 					return GCodeResult::notFinished;
 				}
@@ -134,7 +134,7 @@ GCodeResult GCodes::OffsetAxes(GCodeBuffer& gb, const StringRef& reply)
 	{
 		if (gb.Seen(axisLetters[axis]))
 		{
-			if (!LockMovementAndWaitForStandstill(gb))
+			if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 			{
 				return GCodeResult::notFinished;
 			}
@@ -188,7 +188,7 @@ GCodeResult GCodes::GetSetWorkplaceCoordinates(GCodeBuffer& gb, const StringRef&
 			const float coord = gb.GetDistance();
 			if (!seen)
 			{
-				if (!LockMovementAndWaitForStandstill(gb))						// make sure the user coordinates are stable and up to date
+				if (!LockAllMovementSystemsAndWaitForStandstill(gb))			// make sure the user coordinates are stable and up to date
 				{
 					return GCodeResult::notFinished;
 				}
@@ -305,7 +305,7 @@ GCodeResult GCodes::ChangeSimulationMode(GCodeBuffer& gb, const StringRef &reply
 {
 	if (newSimMode != simulationMode)
 	{
-		if (!LockMovementAndWaitForStandstill(gb))
+		if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			return GCodeResult::notFinished;
 		}
@@ -395,7 +395,7 @@ GCodeResult GCodes::CheckTrigger(GCodeBuffer& gb, const StringRef& reply) THROWS
 // Deal with a M584
 GCodeResult GCodes::DoDriveMapping(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
 {
-	if (!LockMovementAndWaitForStandstill(gb))				// we also rely on this to retrieve the current motor positions to moveBuffer
+	if (!LockAllMovementSystemsAndWaitForStandstill(gb))		// we also rely on this to retrieve the current motor positions to moveBuffer
 	{
 		return GCodeResult::notFinished;
 	}
@@ -764,7 +764,7 @@ GCodeResult GCodes::ProbeTool(GCodeBuffer& gb, const StringRef& reply) THROWS(GC
 		return GCodeResult::error;
 	}
 
-	if (!LockMovementAndWaitForStandstill(gb))
+	if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 	{
 		return GCodeResult::notFinished;
 	}
@@ -858,7 +858,7 @@ GCodeResult GCodes::FindCenterOfCavity(GCodeBuffer& gb, const StringRef& reply)
 		return GCodeResult::error;
 	}
 
-	if (!LockMovementAndWaitForStandstill(gb))
+	if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 	{
 		return GCodeResult::notFinished;
 	}
@@ -983,7 +983,7 @@ GCodeResult GCodes::SetDateTime(GCodeBuffer& gb, const StringRef& reply) THROWS(
 // Handle M997
 GCodeResult GCodes::UpdateFirmware(GCodeBuffer& gb, const StringRef &reply)
 {
-	if (!LockMovementAndWaitForStandstill(gb))
+	if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 	{
 		return GCodeResult::notFinished;
 	}
@@ -1295,7 +1295,7 @@ GCodeResult GCodes::ConfigureLocalDriverBasicParameters(GCodeBuffer& gb, const S
 {
 	if (gb.SeenAny("RS"))
 	{
-		if (!LockMovementAndWaitForStandstill(gb))
+		if (!LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			return GCodeResult::notFinished;
 		}
@@ -1513,7 +1513,7 @@ GCodeResult GCodes::ConfigureLocalDriverBasicParameters(GCodeBuffer& gb, const S
 // Handle G68
 GCodeResult GCodes::HandleG68(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
 {
-	if (!LockMovementAndWaitForStandstill(gb))
+	if (!LockCurrentMovementSystemAndWaitForStandstill(gb))
 	{
 		return GCodeResult::notFinished;
 	}
diff --git a/src/GCodes/GCodes4.cpp b/src/GCodes/GCodes4.cpp
index 7c89ca06..4a9e169f 100644
--- a/src/GCodes/GCodes4.cpp
+++ b/src/GCodes/GCodes4.cpp
@@ -45,7 +45,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 	{
 	case GCodeState::waitingForSpecialMoveToComplete:
 	case GCodeState::abortWhenMovementFinished:
-		if (   LockMovementAndWaitForStandstill(gb)		// movement should already be locked, but we need to wait for standstill and fetch the current position
+		if (   LockCurrentMovementSystemAndWaitForStandstill(gb)		// movement should already be locked, but we need to wait for standstill and fetch the current position
 #if SUPPORT_CAN_EXPANSION
 			&& CanMotion::FinishedReverting()
 #endif
@@ -88,7 +88,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 			break;
 
 		case SegmentedMoveState::aborted:					// move terminated abnormally
-			if (!LockMovementAndWaitForStandstill(gb))		// update the the user position from the machine position at which we stop
+			if (!LockCurrentMovementSystemAndWaitForStandstill(gb))	// update the the user position from the machine position at which we stop
 			{
 				break;
 			}
@@ -133,7 +133,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::probingToolOffset4:					// executing M585, probing move has started
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			if (m585Settings.useProbe)
 			{
@@ -169,7 +169,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::findCenterOfCavity1:						// Executing M675 using a Z probe, have already deployed the probe
 	case GCodeState::probingToolOffset1:						// Executing M585 using a probe, which we have deployed
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			lastProbedTime = millis();							// start the recovery timer
 			const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber);
@@ -202,7 +202,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::findCenterOfCavity3:						// Executing M675, min probing move has started
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber);
 			zp->SetProbing(false);
@@ -223,7 +223,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::findCenterOfCavity4:						// Executing M675, backoff move from min has started
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			if (SetupM675ProbingMove(gb, false))
 			{
@@ -240,7 +240,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::findCenterOfCavity5:						// Executing M675, max probing move has started
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			reprap.GetHeat().SuspendHeaters(false);
 			const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber);
@@ -261,7 +261,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::findCenterOfCavity6:						// Executing M675, move to centre has started
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			gb.SetState(GCodeState::normal);
 			RetractZProbe(gb);
@@ -305,7 +305,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::homing2:
-		if (LockMovementAndWaitForStandstill(gb))		// movement should already be locked, but we need to wait for the previous homing move to complete
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))		// movement should already be locked, but we need to wait for the previous homing move to complete
 		{
 			// Test whether the previous homing move homed any axes
 			if (toBeHomed.Disjoint(axesHomed))
@@ -351,11 +351,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::toolChange1:						// release the old tool (if any), then run tpre for the new tool
 	case GCodeState::m109ToolChange1:					// release the old tool (if any), then run tpre for the new tool
-		if (LockMovementAndWaitForStandstill(gb
-#if SUPPORT_ASYNC_MOVES
-										, false
-#endif
-												))		// wait for tfree.g to finish executing
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))	// wait for tfree.g to finish executing
 		{
 			if (ms.currentTool != nullptr)
 			{
@@ -378,11 +374,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::toolChange2:						// select the new tool if it exists and run tpost
 	case GCodeState::m109ToolChange2:					// select the new tool if it exists and run tpost
-		if (LockMovementAndWaitForStandstill(gb
-#if SUPPORT_ASYNC_MOVES
-										, false
-#endif
-												))		// wait for tpre.g to finish executing
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))	// wait for tpre.g to finish executing
 		{
 			ms.SelectTool(ms.newToolNumber, IsSimulating());
 			UpdateCurrentUserPosition(gb);				// get the actual position of the new tool
@@ -399,11 +391,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::toolChangeComplete:
 	case GCodeState::m109ToolChangeComplete:
-		if (LockMovementAndWaitForStandstill(gb
-#if SUPPORT_ASYNC_MOVES
-										, false
-#endif
-												))		// wait for the move to height to finish
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))	// wait for the move to height to finish
 		{
 			gb.LatestMachineState().feedRate = ms.toolChangeRestorePoint.feedRate;
 			// We don't restore the default fan speed in case the user wants to use a different one for the new tool
@@ -435,7 +423,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::pausing1:
 	case GCodeState::eventPausing1:
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			gb.AdvanceState();
 			if (AllAxesAreHomed())
@@ -446,7 +434,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::filamentChangePause1:
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			gb.AdvanceState();
 			if (AllAxesAreHomed())
@@ -461,7 +449,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::pausing2:
 	case GCodeState::filamentChangePause2:
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			reply.printf((gb.GetState() == GCodeState::filamentChangePause2) ? "Printing paused for filament change at" : "Printing paused at");
 			for (size_t axis = 0; axis < numVisibleAxes; ++axis)
@@ -478,7 +466,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::eventPausing2:
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			pauseState = PauseState::paused;
 #if HAS_SBC_INTERFACE
@@ -492,7 +480,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 	case GCodeState::resuming2:
 		// Here when we have just finished running the resume macro file.
 		// Move the head back to the paused location
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			const float currentZ = ms.coords[Z_AXIS];
 			for (size_t axis = 0; axis < numVisibleAxes; ++axis)
@@ -519,7 +507,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::resuming3:
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			// We no longer restore the paused fan speeds automatically on resuming, because that messes up the print cooling fan speed if a tool change has been done
 			// They can be restored manually in resume.g if required
@@ -540,7 +528,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::cancelling:
-		if (LockMovementAndWaitForStandstill(gb))		// wait until cancel.g has completely finished
+		if (LockAllMovementSystemsAndWaitForStandstill(gb))		// wait until cancel.g has completely finished
 		{
 			pauseState = PauseState::notPaused;
 			gb.SetState(GCodeState::normal);
@@ -621,7 +609,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::stopping:			// here when a print has finished, need to execute stop.g
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 #if SUPPORT_ASYNC_MOVES
 			gb.ExecuteAll();			// only fileGCode gets here so it needs to execute moves for all commands
@@ -683,7 +671,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::gridProbing2a:		// ready to probe the current grid probe point (we return to this state when doing the second and subsequent taps)
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			gb.AdvanceState();
 			if (platform.GetZProbeOrDefault(currentZProbeNumber)->GetProbeType() == ZProbeType::blTouch)
@@ -694,7 +682,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::gridProbing2b:		// ready to probe the current grid probe point
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			lastProbedTime = millis();														// start the recovery timer
 			const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber);
@@ -753,7 +741,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::gridProbing4:	// ready to lift the probe after probing the current grid probe point
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			doingManualBedProbe = false;
 			++tapsDone;
@@ -802,7 +790,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::gridProbing5:	// finished probing a point and moved back to the dive height
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			// See whether we need to do any more taps
 			const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber);
@@ -944,7 +932,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::probingAtPoint1:
 		// The move to raise/lower the head to the correct dive height has been commanded.
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			// Head is at the dive height but needs to be moved to the correct XY position. The XY coordinates have already been stored.
 			SetMoveBufferDefaults(ms);
@@ -963,7 +951,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 	case GCodeState::probingAtPoint2a:								// note we return to this state when doing the second and subsequent taps
 		// Executing G30 with a P parameter. The move to put the head at the specified XY coordinates has been commanded.
 		// OR initial state when executing G30 with no P parameter (must call InitialiseTaps first)
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			gb.AdvanceState();
 			if (platform.GetZProbeOrDefault(currentZProbeNumber)->GetProbeType() == ZProbeType::blTouch)	// bltouch needs to be redeployed prior to each probe point
@@ -974,7 +962,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::probingAtPoint2b:
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			// Head has finished moving to the correct XY position and BLTouch has been deployed
 			lastProbedTime = millis();								// start the probe recovery timer
@@ -1042,7 +1030,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::probingAtPoint4:
 		// Executing G30. The probe wasn't triggered at the start of the move, and the probing move has been commanded.
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			// Probing move has stopped
 			reprap.GetHeat().SuspendHeaters(false);
@@ -1134,7 +1122,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::probingAtPoint5:
 		// Here when we have moved the head back up to the dive height
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			// See whether we need to do any more taps
 			const auto zp = platform.GetZProbeOrDefault(currentZProbeNumber);
@@ -1200,7 +1188,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::probingAtPoint6:
 		// Here when we have finished probing and have retracted the probe if necessary
-		if (LockMovementAndWaitForStandstill(gb))		// retracting the Z probe
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))		// retracting the Z probe
 		{
 			if (g30SValue == 1)
 			{
@@ -1256,7 +1244,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::straightProbe0:					// ready to deploy the probe
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			gb.AdvanceState();
 			currentZProbeNumber = straightProbeSettings.GetZProbeToUse();
@@ -1265,7 +1253,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 		break;
 
 	case GCodeState::straightProbe1:
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			const auto zp = platform.GetEndstops().GetZProbe(straightProbeSettings.GetZProbeToUse());
 			lastProbedTime = millis();			// start the probe recovery timer
@@ -1333,7 +1321,7 @@ void GCodes::RunStateMachine(GCodeBuffer& gb, const StringRef& reply) noexcept
 
 	case GCodeState::straightProbe3:
 		// Executing G38. The probe wasn't in target state at the start of the move, and the probing move has been commanded.
-		if (LockMovementAndWaitForStandstill(gb))
+		if (LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			// Probing move has stopped
 			reprap.GetHeat().SuspendHeaters(false);
diff --git a/src/GCodes/GCodes5.cpp b/src/GCodes/GCodes5.cpp
index 833b7ff7..79db28f4 100644
--- a/src/GCodes/GCodes5.cpp
+++ b/src/GCodes/GCodes5.cpp
@@ -57,6 +57,14 @@ void GCodes::ReportToolTemperatures(const StringRef& reply, const Tool *tool, bo
 
 #if SUPPORT_ASYNC_MOVES
 
+// Handle M400
+GCodeResult GCodes::ExecuteM400(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
+{
+	const unsigned int param = (gb.Seen('P')) ? gb.GetLimitedUIValue('P', 2) : 0;
+	const bool finished = (param == 1) ? LockAllMovementSystemsAndWaitForStandstill(gb) : LockCurrentMovementSystemAndWaitForStandstill(gb);
+	return (finished) ? GCodeResult::ok : GCodeResult::notFinished;
+}
+
 // Handle M596
 GCodeResult GCodes::SelectMovementQueue(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
 {
@@ -130,6 +138,12 @@ GCodeResult GCodes::CollisionAvoidance(GCodeBuffer& gb, const StringRef& reply)
 	return GCodeResult::ok;
 }
 
+// Handle M598
+GCodeResult GCodes::SyncMovementSystems(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException)
+{
+	return (DoSync(gb)) ? GCodeResult::ok : GCodeResult::notFinished;
+}
+
 #endif
 
 GCodeResult GCodes::HandleM486(GCodeBuffer &gb, const StringRef &reply, OutputBuffer*& buf) THROWS(GCodeException)
diff --git a/src/Movement/AxisShaper.cpp b/src/Movement/AxisShaper.cpp
index d5ea5eee..5c9e94df 100644
--- a/src/Movement/AxisShaper.cpp
+++ b/src/Movement/AxisShaper.cpp
@@ -77,7 +77,7 @@ GCodeResult AxisShaper::Configure(GCodeBuffer& gb, const StringRef& reply) THROW
 	// Changing just the minimum acceleration is OK because no other variables depend on it.
 	if (gb.SeenAny("FSPHT"))
 	{
-		if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+		if (!reprap.GetGCodes().LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			return GCodeResult::notFinished;
 		}
diff --git a/src/Movement/DDARing.cpp b/src/Movement/DDARing.cpp
index 5297f924..5b375636 100644
--- a/src/Movement/DDARing.cpp
+++ b/src/Movement/DDARing.cpp
@@ -121,7 +121,7 @@ GCodeResult DDARing::ConfigureMovementQueue(GCodeBuffer& gb, const StringRef& re
 	gb.TryGetUIValue('R', gracePeriod, seen);
 	if (seen)
 	{
-		if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+		if (!reprap.GetGCodes().LockAllMovementSystemsAndWaitForStandstill(gb))
 		{
 			return GCodeResult::notFinished;
 		}
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index c8aed193..26277d12 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -1043,7 +1043,7 @@ GCodeResult Move::ConfigurePressureAdvance(GCodeBuffer& gb, const StringRef& rep
 	if (gb.Seen('S'))
 	{
 		const float advance = gb.GetNonNegativeFValue();
-		if (!reprap.GetGCodes().LockMovementAndWaitForStandstill(gb))
+		if (!reprap.GetGCodes().LockCurrentMovementSystemAndWaitForStandstill(gb))
 		{
 			return GCodeResult::notFinished;
 		}
diff --git a/src/SBC/SbcInterface.cpp b/src/SBC/SbcInterface.cpp
index 0a203999..766def66 100644
--- a/src/SBC/SbcInterface.cpp
+++ b/src/SBC/SbcInterface.cpp
@@ -451,7 +451,7 @@ void SbcInterface::ExchangeData() noexcept
 			{
 				GCodeBuffer * const gb = reprap.GetGCodes().GetGCodeBuffer(channel);
 				MutexLocker locker(gb->mutex, SbcYieldTimeout);
-				if (locker.IsAcquired() && reprap.GetGCodes().LockMovementAndWaitForStandstill(*gb))
+				if (locker.IsAcquired() && reprap.GetGCodes().LockCurrentMovementSystemAndWaitForStandstill(*gb))
 				{
 					transfer.WriteLocked(channel);
 				}