Initial code to support precise stopping on CAN expansion boards

8 files changed, 386 insertions, 251 deletions

diff --git a/src/CAN/CanDriversData.cpp b/src/CAN/CanDriversData.cpp
new file mode 100644
index 00000000..f985110f
--- /dev/null
+++ b/src/CAN/CanDriversData.cpp
@@ -0,0 +1,57 @@
+/*
+ * CanDriversData.cpp
+ *
+ *  Created on: 23 Dec 2021
+ *      Author: David
+ */
+
+#include "CanDriversData.h"
+
+#if SUPPORT_CAN_EXPANSION
+
+// Insert a new entry, keeping the list ordered
+void CanDriversList::AddEntry(DriverId driver) noexcept
+{
+	if (numEntries < ARRAY_SIZE(drivers))
+	{
+		// We could do a binary search here but the number of CAN drivers supported isn't huge, so linear search instead
+		size_t insertPoint = 0;
+		while (insertPoint < numEntries && drivers[insertPoint] < driver)
+		{
+			++insertPoint;
+		}
+
+		if (insertPoint == numEntries)
+		{
+			drivers[numEntries] = driver;
+			++numEntries;
+		}
+		else if (drivers[insertPoint] != driver)
+		{
+			memmove(drivers + (insertPoint + 1), drivers + insertPoint, (numEntries - insertPoint) * sizeof(drivers[0]));
+			drivers[insertPoint] = driver;
+			++numEntries;
+		}
+	}
+}
+
+// Get the details of the drivers on the next board and advance startFrom beyond the entries for this board
+CanAddress CanDriversList::GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept
+{
+	driversBitmap.Clear();
+	if (startFrom >= numEntries)
+	{
+		return CanId::NoAddress;
+	}
+	const CanAddress boardAddress = drivers[startFrom].boardAddress;
+	do
+	{
+		driversBitmap.SetBit(drivers[startFrom].localDriver);
+		++startFrom;
+	} while (startFrom < numEntries && drivers[startFrom].boardAddress == boardAddress);
+	return boardAddress;
+}
+
+#endif
+
+// End
diff --git a/src/CAN/CanDriversData.h b/src/CAN/CanDriversData.h
new file mode 100644
index 00000000..42057bca
--- /dev/null
+++ b/src/CAN/CanDriversData.h
@@ -0,0 +1,97 @@
+/*
+ * CanDriversData.h
+ *
+ *  Created on: 23 Dec 2021
+ *      Author: David
+ */
+
+#ifndef SRC_CAN_CANDRIVERSDATA_H_
+#define SRC_CAN_CANDRIVERSDATA_H_
+
+#include "RepRapFirmware.h"
+
+#if SUPPORT_CAN_EXPANSION
+
+typedef Bitmap<uint16_t> CanDriversBitmap;
+
+// Class to accumulate a set of values relating to CAN-connected drivers
+template<class T> class CanDriversData
+{
+public:
+	CanDriversData() noexcept;
+	void AddEntry(DriverId id, T val) noexcept;
+	size_t GetNumEntries() const noexcept { return numEntries; }
+	CanAddress GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept;
+	T GetElement(size_t n) const pre(n < GetnumEntries()) noexcept { return data[n].val; }
+
+private:
+	struct DriverDescriptor
+	{
+		DriverId driver;
+		T val;
+	};
+
+	size_t numEntries;
+	DriverDescriptor data[MaxCanDrivers];
+};
+
+// Class to represent a set of CAN-connected drivers with no associated data
+class CanDriversList
+{
+public:
+	CanDriversList() noexcept : numEntries(0) { }
+	void Clear() noexcept { numEntries = 0; }
+	void AddEntry(DriverId id) noexcept;
+	size_t GetNumEntries() const noexcept { return numEntries; }
+	bool IsEmpty() const noexcept { return numEntries == 0; }
+	CanAddress GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept;
+
+private:
+	size_t numEntries;
+	DriverId drivers[MaxCanDrivers];
+};
+
+// Members of template class CanDriversData
+template<class T> CanDriversData<T>::CanDriversData() noexcept
+{
+	numEntries = 0;
+}
+
+// Insert a new entry, keeping the list ordered by driver ID
+template<class T> void CanDriversData<T>::AddEntry(DriverId driver, T val) noexcept
+{
+	if (numEntries < ARRAY_SIZE(data))
+	{
+		// We could do a binary search here but the number of CAN drivers supported isn't huge, so linear search instead
+		size_t insertPoint = 0;
+		while (insertPoint < numEntries && data[insertPoint].driver < driver)
+		{
+			++insertPoint;
+		}
+		memmove(data + (insertPoint + 1), data + insertPoint, (numEntries - insertPoint) * sizeof(data[0]));
+		data[insertPoint].driver = driver;
+		data[insertPoint].val = val;
+		++numEntries;
+	}
+}
+
+// Get the details of the drivers on the next board and advance startFrom beyond the entries for this board
+template<class T> CanAddress CanDriversData<T>::GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept
+{
+	driversBitmap.Clear();
+	if (startFrom >= numEntries)
+	{
+		return CanId::NoAddress;
+	}
+	const CanAddress boardAddress = data[startFrom].driver.boardAddress;
+	do
+	{
+		driversBitmap.SetBit(data[startFrom].driver.localDriver);
+		++startFrom;
+	} while (startFrom < numEntries && data[startFrom].driver.boardAddress == boardAddress);
+	return boardAddress;
+}
+
+#endif
+
+#endif /* SRC_CAN_CANDRIVERSDATA_H_ */
diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index 20c36877..a44d757d 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -561,49 +561,6 @@ extern "C" [[noreturn]] void CanClockLoop(void *) noexcept
 	}
 }
 
-// Insert a new entry, keeping the list ordered
-void CanDriversList::AddEntry(DriverId driver) noexcept
-{
-	if (numEntries < ARRAY_SIZE(drivers))
-	{
-		// We could do a binary search here but the number of CAN drivers supported isn't huge, so linear search instead
-		size_t insertPoint = 0;
-		while (insertPoint < numEntries && drivers[insertPoint] < driver)
-		{
-			++insertPoint;
-		}
-
-		if (insertPoint == numEntries)
-		{
-			drivers[numEntries] = driver;
-			++numEntries;
-		}
-		else if (drivers[insertPoint] != driver)
-		{
-			memmove(drivers + (insertPoint + 1), drivers + insertPoint, (numEntries - insertPoint) * sizeof(drivers[0]));
-			drivers[insertPoint] = driver;
-			++numEntries;
-		}
-	}
-}
-
-// Get the details of the drivers on the next board and advance startFrom beyond the entries for this board
-CanAddress CanDriversList::GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept
-{
-	driversBitmap.Clear();
-	if (startFrom >= numEntries)
-	{
-		return CanId::NoAddress;
-	}
-	const CanAddress boardAddress = drivers[startFrom].boardAddress;
-	do
-	{
-		driversBitmap.SetBit(drivers[startFrom].localDriver);
-		++startFrom;
-	} while (startFrom < numEntries && drivers[startFrom].boardAddress == boardAddress);
-	return boardAddress;
-}
-
 // Members of namespace CanInterface, and associated local functions
 
 template<class T> static GCodeResult SetRemoteDriverValues(const CanDriversData<T>& data, const StringRef& reply, CanMessageType mt) noexcept
diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h
index 5281e879..a1c320ac 100644
--- a/src/CAN/CanInterface.h
+++ b/src/CAN/CanInterface.h
@@ -14,50 +14,13 @@
 
 #include <CanId.h>
 #include <CanMessageFormats.h>
+#include "CanDriversData.h"
 
 class CanMessageBuffer;
 class DDA;
 class DriveMovement;
 struct PrepParams;
 
-typedef Bitmap<uint16_t> CanDriversBitmap;
-
-// Class to accumulate a set of values relating to CAN-connected drivers
-template<class T>class CanDriversData
-{
-public:
-	CanDriversData() noexcept;
-	void AddEntry(DriverId id, T val) noexcept;
-	size_t GetNumEntries() const noexcept { return numEntries; }
-	CanAddress GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept;
-	T GetElement(size_t n) const pre(n < GetnumEntries()) noexcept { return data[n].val; }
-
-private:
-	struct DriverDescriptor
-	{
-		DriverId driver;
-		T val;
-	};
-
-	size_t numEntries;
-	DriverDescriptor data[MaxCanDrivers];
-};
-
-class CanDriversList
-{
-public:
-	CanDriversList() noexcept : numEntries(0) { }
-	void Clear() noexcept { numEntries = 0; }
-	void AddEntry(DriverId id) noexcept;
-	size_t GetNumEntries() const noexcept { return numEntries; }
-	bool IsEmpty() const noexcept { return numEntries == 0; }
-	CanAddress GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept;
-
-private:
-	size_t numEntries;
-	DriverId drivers[MaxCanDrivers];
-};
-
 namespace CanInterface
 {
 	// Note: GetCanAddress() in this namespace is now declared in RepRapFirmware.h to overcome ordering issues
@@ -155,47 +118,6 @@ namespace ODrive {
 #endif
 }
 
-// Members of template class CanDriversData
-template<class T> CanDriversData<T>::CanDriversData() noexcept
-{
-	numEntries = 0;
-}
-
-// Insert a new entry, keeping the list ordered
-template<class T> void CanDriversData<T>::AddEntry(DriverId driver, T val) noexcept
-{
-	if (numEntries < ARRAY_SIZE(data))
-	{
-		// We could do a binary search here but the number of CAN drivers supported isn't huge, so linear search instead
-		size_t insertPoint = 0;
-		while (insertPoint < numEntries && data[insertPoint].driver < driver)
-		{
-			++insertPoint;
-		}
-		memmove(data + (insertPoint + 1), data + insertPoint, (numEntries - insertPoint) * sizeof(data[0]));
-		data[insertPoint].driver = driver;
-		data[insertPoint].val = val;
-		++numEntries;
-	}
-}
-
-// Get the details of the drivers on the next board and advance startFrom beyond the entries for this board
-template<class T> CanAddress CanDriversData<T>::GetNextBoardDriverBitmap(size_t& startFrom, CanDriversBitmap& driversBitmap) const noexcept
-{
-	driversBitmap.Clear();
-	if (startFrom >= numEntries)
-	{
-		return CanId::NoAddress;
-	}
-	const CanAddress boardAddress = data[startFrom].driver.boardAddress;
-	do
-	{
-		driversBitmap.SetBit(data[startFrom].driver.localDriver);
-		++startFrom;
-	} while (startFrom < numEntries && data[startFrom].driver.boardAddress == boardAddress);
-	return boardAddress;
-}
-
 #endif
 
 #endif /* SRC_CAN_CANINTERFACE_H_ */
diff --git a/src/CAN/CanMotion.cpp b/src/CAN/CanMotion.cpp
index 6d311d41..47bb2f27 100644
--- a/src/CAN/CanMotion.cpp
+++ b/src/CAN/CanMotion.cpp
@@ -12,29 +12,44 @@
 #include <CanMessageBuffer.h>
 #include <CanMessageFormats.h>
 #include "CanInterface.h"
+#include <General/FreelistManager.h>
 
-static CanMessageBuffer *movementBufferList = nullptr;
-static CanMessageBuffer *urgentMessageBuffer = nullptr;
-static uint32_t currentMoveClocks;
-
-#if 0
-static unsigned int numMotionMessagesSentLast = 0;
-#endif
+namespace CanMotion
+{
+	enum class DriverStopState : uint8_t { inactive = 0, active, stopRequested, stopSent };
 
-static volatile uint32_t hiccupToInsert = 0;
-static CanDriversList driversToStop[2];
-static size_t driversToStopIndexBeingFilled = 0;
-static size_t indexOfNextDriverToStop = 0;
-static volatile bool stopAllFlag = false;
-static bool doingStopAll = false;
-static LargeBitmap<CanId::MaxCanAddress + 1> boardsActiveInLastMove;
-static uint8_t nextSeq[CanId::MaxCanAddress + 1] = { 0 };
+	// Class to record drivers active and requests to stop them
+	class DriversStopList
+	{
+	public:
+		DECLARE_FREELIST_NEW_DELETE(DriversStopList)
+
+		DriversStopList(DriversStopList *p_next, CanAddress p_ba) noexcept : next(p_next), boardAddress(p_ba) { }
+
+		DriversStopList *next;
+		CanAddress boardAddress;
+		uint8_t numDrivers;
+		volatile DriverStopState stopStates[MaxLinearDriversPerCanSlave];
+		volatile int32_t stopSteps[MaxLinearDriversPerCanSlave];
+	};
+
+	static CanMessageBuffer urgentMessageBuffer(nullptr);
+	static CanMessageBuffer *movementBufferList = nullptr;
+	static DriversStopList *volatile stopList = nullptr;
+	static uint32_t currentMoveClocks;
+	static volatile uint32_t hiccupToInsert = 0;
+	static Mutex stopListMutex;
+	static uint8_t nextSeq[CanId::MaxCanAddress + 1] = { 0 };
+
+	static CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexcept;
+	static void InternalStopDriverWhenProvisional(DriverId driver) noexcept;
+	static bool InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noexcept;
+}
 
 void CanMotion::Init() noexcept
 {
 	movementBufferList = nullptr;
-	urgentMessageBuffer = CanMessageBuffer::Allocate();
-	boardsActiveInLastMove.ClearAll();
+	stopListMutex.Create("stopList");
 }
 
 // This is called by DDA::Prepare at the start of preparing a movement
@@ -51,9 +66,23 @@ void CanMotion::StartMovement() noexcept
 		movementBufferList = p->next;
 		CanMessageBuffer::Free(p);
 	}
+
+	// Free up any stop list items left over from the previous move
+	MutexLocker lock(stopListMutex);
+
+	for (;;)
+	{
+		DriversStopList *p = stopList;
+		if (p == nullptr)
+		{
+			break;
+		}
+		stopList = p->next;
+		delete p;
+	}
 }
 
-CanMessageBuffer *GetBuffer(const PrepParams& params, DriverId canDriver) noexcept
+CanMessageBuffer *CanMotion::GetBuffer(const PrepParams& params, DriverId canDriver) noexcept
 {
 	if (canDriver.localDriver >= MaxLinearDriversPerCanSlave)
 	{
@@ -178,34 +207,18 @@ void CanMotion::AddExtruderMovement(const PrepParams& params, DriverId canDriver
 #endif
 
 // This is called by DDA::Prepare when all DMs for CAN drives have been processed. Return the calculated move time in steps, or 0 if there are no CAN moves
-uint32_t CanMotion::FinishMovement(uint32_t moveStartTime, bool simulating) noexcept
+uint32_t CanMotion::FinishMovement(uint32_t moveStartTime, bool simulating, bool checkingEndstops) noexcept
 {
-	boardsActiveInLastMove.ClearAll();
 	CanMessageBuffer *buf = movementBufferList;
 	if (buf == nullptr)
 	{
 		return 0;
 	}
 
-#if 0
-	numMotionMessagesSentLast = 0;
-#endif
+	MutexLocker lock(stopListMutex);
+
 	do
 	{
-		boardsActiveInLastMove.SetBit(buf->id.Dst());					//TODO should we set this if there were no steps for drives on the board, just drives to be enabled?
-#if USE_REMOTE_INPUT_SHAPING
-		buf->msg.moveLinearShaped.whenToExecute = moveStartTime;
-		uint8_t& seq = nextSeq[buf->id.Dst()];
-		buf->msg.moveLinearShaped.seq = seq;
-		seq = (seq + 1) & 0x7F;
-		buf->dataLength = buf->msg.moveLinearShaped.GetActualDataLength();
-#else
-		buf->msg.moveLinear.whenToExecute = moveStartTime;
-		uint8_t& seq = nextSeq[buf->id.Dst()];
-		buf->msg.moveLinear.seq = seq;
-		seq = (seq + 1) & 0x7F;
-		buf->dataLength = buf->msg.moveLinear.GetActualDataLength();
-#endif
 		CanMessageBuffer * const nextBuffer = buf->next;				// must get this before sending the buffer, because sending the buffer releases it
 		if (simulating)
 		{
@@ -213,11 +226,30 @@ uint32_t CanMotion::FinishMovement(uint32_t moveStartTime, bool simulating) noex
 		}
 		else
 		{
+#if USE_REMOTE_INPUT_SHAPING
+			CanMessageMovementLinear& msg = buf->msg.moveLinearShaped;
+#else
+			CanMessageMovementLinear& msg = buf->msg.moveLinear;
+#endif
+			msg.whenToExecute = moveStartTime;
+			uint8_t& seq = nextSeq[buf->id.Dst()];
+			msg.seq = seq;
+			seq = (seq + 1) & 0x7F;
+			buf->dataLength = msg.GetActualDataLength();
+			if (checkingEndstops)
+			{
+				// Set up the stop list
+				DriversStopList * const sl = new DriversStopList(stopList, buf->id.Dst());
+				const size_t nd = msg.numDrivers;
+				sl->numDrivers = (uint8_t)nd;
+				for (size_t i = 0; i < nd; ++i)
+				{
+					sl->stopStates[i] = (msg.perDrive[i].steps != 0) ? DriverStopState::active : DriverStopState::inactive;
+				}
+				stopList = sl;
+			}
 			CanInterface::SendMotion(buf);								// queues the buffer for sending and frees it when done
 		}
-#if 0
-		++numMotionMessagesSentLast;
-#endif
 		buf = nextBuffer;
 	} while (buf != nullptr);
 
@@ -231,51 +263,31 @@ bool CanMotion::CanPrepareMove() noexcept
 }
 
 // This is called by the CanSender task to check if we have any urgent messages to send
+// The only urgent messages we may have currently are messages to stop drivers.
 CanMessageBuffer *CanMotion::GetUrgentMessage() noexcept
 {
-	if (stopAllFlag)
-	{
-		// Send a broadcast Stop All message first, followed by individual ones
-		driversToStop[driversToStopIndexBeingFilled].Clear();
-		driversToStop[driversToStopIndexBeingFilled ^ 1].Clear();
-		auto msg = urgentMessageBuffer->SetupBroadcastMessage<CanMessageStopMovement>(CanInterface::GetCanAddress());
-		msg->whichDrives = 0xFFFF;
-		doingStopAll = true;
-		stopAllFlag = false;
-		indexOfNextDriverToStop = 0;
-		return urgentMessageBuffer;
-	}
+	MutexLocker lock(stopListMutex);			// make sure the list isn't being changed while we traverse it
 
-	if (doingStopAll)
+	for (DriversStopList *sl = stopList; sl != nullptr; sl = sl->next)
 	{
-		const unsigned int nextBoard = boardsActiveInLastMove.FindLowestSetBit();
-		if (nextBoard < boardsActiveInLastMove.NumBits())
+		auto msg = urgentMessageBuffer.SetupRequestMessage<CanMessageStopMovement>(0, CanInterface::GetCanAddress(), sl->boardAddress);
+		uint16_t driversBeingStopped = 0;
+		size_t numDriversStopped = 0;
+		for (size_t driver = 0; driver < sl->numDrivers; ++driver)
 		{
-			boardsActiveInLastMove.ClearBit(nextBoard);
-			auto msg = urgentMessageBuffer->SetupRequestMessage<CanMessageStopMovement>(0, CanInterface::GetCanAddress(), nextBoard);
-			msg->whichDrives = 0xFFFF;
-			return urgentMessageBuffer;
+			if (sl->stopStates[driver] == DriverStopState::stopRequested)
+			{
+				driversBeingStopped |= 1u << driver;
+				msg->finalStepCounts[numDriversStopped++] = sl->stopSteps[driver];
+				sl->stopStates[driver] = DriverStopState::stopSent;
+			}
 		}
-		doingStopAll = false;
-	}
-
-	if (driversToStop[driversToStopIndexBeingFilled ^ 1].GetNumEntries() == 0 && driversToStop[driversToStopIndexBeingFilled].GetNumEntries() != 0)
-	{
-		driversToStopIndexBeingFilled  = driversToStopIndexBeingFilled ^ 1;
-	}
-
-	if (driversToStop[driversToStopIndexBeingFilled ^ 1].GetNumEntries() != 0)
-	{
-		CanDriversBitmap drivers;
-		const CanAddress board = driversToStop[driversToStopIndexBeingFilled ^ 1].GetNextBoardDriverBitmap(indexOfNextDriverToStop, drivers);
-		if (board != CanId::NoAddress)
+		if (driversBeingStopped != 0)
 		{
-			auto msg = urgentMessageBuffer->SetupRequestMessage<CanMessageStopMovement>(0, CanInterface::GetCanAddress(), board);
-			msg->whichDrives = drivers.GetRaw();
-			return urgentMessageBuffer;
+			msg->whichDrives = driversBeingStopped;
+			urgentMessageBuffer.dataLength = msg->GetActualDataLength(numDriversStopped);
+			return &urgentMessageBuffer;
 		}
-		driversToStop[driversToStopIndexBeingFilled ^ 1].Clear();
-		indexOfNextDriverToStop = 0;
 	}
 
 	return nullptr;
@@ -289,64 +301,116 @@ void CanMotion::InsertHiccup(uint32_t numClocks) noexcept
 	CanInterface::WakeAsyncSenderFromIsr();
 }
 
-void CanMotion::StopDriver(bool isBeingPrepared, DriverId driver) noexcept
+void CanMotion::InternalStopDriverWhenProvisional(DriverId driver) noexcept
 {
-	if (isBeingPrepared)
+	// Search for the correct movement buffer
+	CanMessageBuffer* buf = movementBufferList;
+	while (buf != nullptr && buf->id.Dst() != driver.boardAddress)
 	{
-		// Search for the correct movement buffer
-		CanMessageBuffer* buf = movementBufferList;
-		while (buf != nullptr && buf->id.Dst() != driver.boardAddress)
-		{
-			buf = buf->next;
-		}
+		buf = buf->next;
+	}
 
-		if (buf != nullptr)
-		{
+	if (buf != nullptr)
+	{
 #if USE_REMOTE_INPUT_SHAPING
-			buf->msg.moveLinearShaped.perDrive[driver.localDriver].steps = 0;
+		buf->msg.moveLinearShaped.perDrive[driver.localDriver].steps = 0;
 #else
-			buf->msg.moveLinear.perDrive[driver.localDriver].steps = 0;
+		buf->msg.moveLinear.perDrive[driver.localDriver].steps = 0;
 #endif
-		}
 	}
-	else
+}
+
+bool CanMotion::InternalStopDriverWhenMoving(DriverId driver, int32_t steps) noexcept
+{
+	DriversStopList *sl = stopList;
+	while (sl != nullptr)
 	{
-		driversToStop[driversToStopIndexBeingFilled].AddEntry(driver);
-		CanInterface::WakeAsyncSenderFromIsr();
+		if (sl->boardAddress == driver.boardAddress)
+		{
+			if (sl->stopStates[driver.localDriver] == DriverStopState::active)			// if active and stop not yet requested
+			{
+				sl->stopSteps[driver.localDriver] = steps;								// must assign this one first
+				sl->stopStates[driver.localDriver] = DriverStopState::stopRequested;
+				return true;
+			}
+			break;																		// we found the right board, no point in searching further
+		}
+		sl = sl->next;
 	}
+	return false;
 }
 
-void CanMotion::StopAxis(bool isBeingPrepared, size_t axis) noexcept
+// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true
+void CanMotion::StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcept
 {
-	const AxisDriversConfig& cfg = reprap.GetPlatform().GetAxisDriversConfig(axis);
-	if (isBeingPrepared)
+	if (dda.GetState() == DDA::DDAState::provisional)
+	{
+		InternalStopDriverWhenProvisional(driver);
+	}
+	else
 	{
-		for (size_t i = 0; i < cfg.numDrivers; ++i)
+		const DriveMovement * const dm = dda.FindDM(axis);
+		if (dm != nullptr)
 		{
-			const DriverId driver = cfg.driverNumbers[i];
-			if (driver.IsRemote())
+			if (InternalStopDriverWhenMoving(driver, dm->GetNetStepsTaken()))
 			{
-				StopDriver(true, driver);
+				CanInterface::WakeAsyncSenderFromIsr();
 			}
 		}
 	}
-	else if (!stopAllFlag)
+}
+
+// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true
+void CanMotion::StopAxis(const DDA& dda, size_t axis) noexcept
+{
+	const Platform& p = reprap.GetPlatform();
+	if (axis < reprap.GetGCodes().GetTotalAxes())
 	{
-		for (size_t i = 0; i < cfg.numDrivers; ++i)
+		const AxisDriversConfig& cfg = p.GetAxisDriversConfig(axis);
+		if (dda.GetState() == DDA::DDAState::provisional)
 		{
-			const DriverId driver = cfg.driverNumbers[i];
-			if (driver.IsRemote())
+			for (size_t i = 0; i < cfg.numDrivers; ++i)
 			{
-				driversToStop[driversToStopIndexBeingFilled].AddEntry(driver);
+				const DriverId driver = cfg.driverNumbers[i];
+				if (driver.IsRemote())
+				{
+					InternalStopDriverWhenProvisional(driver);
+				}
+			}
+		}
+		else
+		{
+			const DriveMovement * const dm = dda.FindDM(axis);
+			if (dm != nullptr)
+			{
+				bool somethingStopped = false;
+				const uint32_t steps = dm->GetNetStepsTaken();
+				for (size_t i = 0; i < cfg.numDrivers; ++i)
+				{
+					const DriverId driver = cfg.driverNumbers[i];
+					if (driver.IsRemote() && InternalStopDriverWhenMoving(driver, steps))
+					{
+						somethingStopped = true;
+					}
+				}
+				if (somethingStopped)
+				{
+					CanInterface::WakeAsyncSenderFromIsr();
+				}
 			}
 		}
-		CanInterface::WakeAsyncSenderFromIsr();
+	}
+	else
+	{
+		const DriverId driver = p.GetExtruderDriver(LogicalDriveToExtruder(axis));
+		StopDriver(dda, axis, driver);
 	}
 }
 
-void CanMotion::StopAll(bool isBeingPrepared) noexcept
+// This is called from the step ISR with isBeingPrepared false, or from the Move task with isBeingPrepared true
+void CanMotion::StopAll(const DDA& dda) noexcept
 {
-	if (isBeingPrepared)
+	if (dda.GetState() == DDA::DDAState::provisional)
 	{
 		// We still send the messages so that the drives get enabled, but we set the steps to zero
 		for (CanMessageBuffer *buf = movementBufferList; buf != nullptr; buf = buf->next)
@@ -368,8 +432,49 @@ void CanMotion::StopAll(bool isBeingPrepared) noexcept
 	}
 	else
 	{
-		stopAllFlag = true;
-		CanInterface::WakeAsyncSenderFromIsr();
+		// Loop through the axes that are actually moving
+		const GCodes& gc = reprap.GetGCodes();
+		const size_t totalAxes = gc.GetTotalAxes();
+		const Platform& p = reprap.GetPlatform();
+		bool somethingStopped = false;
+		for (size_t axis = 0; axis < totalAxes; ++axis)
+		{
+			const DriveMovement* const dm = dda.FindDM(axis);
+			if (dm != nullptr)
+			{
+				const uint32_t steps = dm->GetNetStepsTaken();
+				const AxisDriversConfig& cfg = p.GetAxisDriversConfig(axis);
+				for (size_t i = 0; i < cfg.numDrivers; ++i)
+				{
+					const DriverId driver = cfg.driverNumbers[i];
+					if (driver.IsRemote() && InternalStopDriverWhenMoving(driver, steps))
+					{
+						somethingStopped = true;
+					}
+				}
+			}
+		}
+		const size_t numExtruders = gc.GetNumExtruders();
+		for (size_t extruder = 0; extruder < numExtruders; ++extruder)
+		{
+			const DriverId driver = p.GetExtruderDriver(extruder);
+			if (driver.IsRemote())
+			{
+				const DriveMovement* const dm = dda.FindDM(extruder);
+				if (dm != nullptr)
+				{
+					if (InternalStopDriverWhenMoving(driver, dm->GetNetStepsTaken()))
+					{
+						somethingStopped = true;
+					}
+				}
+			}
+		}
+
+		if (somethingStopped)
+		{
+			CanInterface::WakeAsyncSenderFromIsr();
+		}
 	}
 }
 
diff --git a/src/CAN/CanMotion.h b/src/CAN/CanMotion.h
index 7b6b0d32..e02b0f76 100644
--- a/src/CAN/CanMotion.h
+++ b/src/CAN/CanMotion.h
@@ -22,17 +22,18 @@ namespace CanMotion
 	void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps) noexcept;
 	void AddExtruderMovement(const PrepParams& params, DriverId canDriver, float extrusion, bool usePressureAdvance) noexcept;
 #else
-	void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance = false) noexcept;
+	void AddMovement(const PrepParams& params, DriverId canDriver, int32_t steps, bool usePressureAdvance) noexcept;
 #endif
-	uint32_t FinishMovement(uint32_t moveStartTime, bool simulating) noexcept;
+	uint32_t FinishMovement(uint32_t moveStartTime, bool simulating, bool checkingEndstops) noexcept;
 	bool CanPrepareMove() noexcept;
 	CanMessageBuffer *GetUrgentMessage() noexcept;
 
 	// The next 4 functions may be called from the step ISR, so they can't send CAN messages directly
 	void InsertHiccup(uint32_t numClocks) noexcept;
-	void StopDriver(bool isBeingPrepared, DriverId driver) noexcept;
-	void StopAxis(bool isBeingPrepared, size_t axis) noexcept;
-	void StopAll(bool isBeingPrepared) noexcept;
+	void StopAll(const DDA& dda) noexcept;
+	void StopAxis(const DDA& dda, size_t axis) noexcept;
+	void StopDriver(const DDA& dda, size_t axis, DriverId driver) noexcept
+		pre(driver.IsRemote());
 }
 
 #endif
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp
index 26358f6d..c0a81800 100644
--- a/src/Movement/DDA.cpp
+++ b/src/Movement/DDA.cpp
@@ -1374,7 +1374,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept
 #if SUPPORT_CAN_EXPANSION
 						if (driver.IsRemote())
 						{
-							CanMotion::AddMovement(params, driver, delta);
+							CanMotion::AddMovement(params, driver, delta, false);
 						}
 						else
 #endif
@@ -1444,7 +1444,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept
 					const DriverId driver = config.driverNumbers[i];
 					if (driver.IsRemote())
 					{
-						CanMotion::AddMovement(params, driver, delta);
+						CanMotion::AddMovement(params, driver, delta, false);
 					}
 				}
 #endif
@@ -1510,7 +1510,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept
 						const DriverId driver = config.driverNumbers[i];
 						if (driver.IsRemote())
 						{
-							CanMotion::AddMovement(params, driver, delta);
+							CanMotion::AddMovement(params, driver, delta, false);
 						}
 					}
 #endif
@@ -1614,7 +1614,7 @@ void DDA::Prepare(SimulationMode simMode) noexcept
 		}
 
 #if SUPPORT_CAN_EXPANSION
-		const uint32_t canClocksNeeded = CanMotion::FinishMovement(afterPrepare.moveStartTime, simMode != SimulationMode::off);
+		const uint32_t canClocksNeeded = CanMotion::FinishMovement(afterPrepare.moveStartTime, simMode != SimulationMode::off, flags.checkEndstops);
 		if (canClocksNeeded > clocksNeeded)
 		{
 			// Due to rounding error in the calculations, we quite often calculate the CAN move as being longer than our previously-calculated value, normally by just one clock.
@@ -1784,10 +1784,6 @@ float DDA::NormaliseLinearMotion(AxesBitmap linearAxes) noexcept
 // Either this move is currently executing (DDARing.currentDDA == this) and the state is 'executing', or we have almost finished preparing it and the state is 'provisional'.
 void DDA::CheckEndstops(Platform& platform) noexcept
 {
-#if SUPPORT_CAN_EXPANSION
-	const bool fromPrepare = (state == DDAState::provisional);		// determine this before anything sets the state to 'completed'
-#endif
-
 	for (;;)
 	{
 		const EndstopHitDetails hitDetails = platform.GetEndstops().CheckEndstops();
@@ -1796,7 +1792,7 @@ void DDA::CheckEndstops(Platform& platform) noexcept
 		case EndstopHitAction::stopAll:
 			MoveAborted();											// set the state to completed and recalculate the endpoints
 #if SUPPORT_CAN_EXPANSION
-			CanMotion::StopAll(fromPrepare);
+			CanMotion::StopAll(*this);
 #endif
 			if (hitDetails.isZProbe)
 			{
@@ -1819,11 +1815,11 @@ void DDA::CheckEndstops(Platform& platform) noexcept
 #if SUPPORT_CAN_EXPANSION
 			if (state == completed)									// if the call to StopDrive flagged the move as completed
 			{
-				CanMotion::StopAll(fromPrepare);
+				CanMotion::StopAll(*this);
 			}
 			else
 			{
-				CanMotion::StopAxis(fromPrepare, hitDetails.axis);
+				CanMotion::StopAxis(*this, hitDetails.axis);
 			}
 #endif
 			if (hitDetails.setAxisLow)
@@ -1842,7 +1838,7 @@ void DDA::CheckEndstops(Platform& platform) noexcept
 #if SUPPORT_CAN_EXPANSION
 			if (hitDetails.driver.IsRemote())
 			{
-				CanMotion::StopDriver(fromPrepare, hitDetails.driver);
+				CanMotion::StopDriver(*this, hitDetails.axis, hitDetails.driver);
 			}
 			else
 #endif
diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h
index c787b40b..94798d77 100644
--- a/src/Movement/DDA.h
+++ b/src/Movement/DDA.h
@@ -182,6 +182,7 @@ public:
 	uint32_t GetStepInterval(size_t axis, uint32_t microstepShift) const noexcept;	// Get the current full step interval for this axis or extruder
 #endif
 
+	DriveMovement *FindDM(size_t drive) const noexcept;								// find the DM for a drive if there is one even if it is completed
 	void CheckEndstops(Platform& platform) noexcept;
 
 	void DebugPrint(const char *tag) const noexcept;								// print the DDA only
@@ -231,7 +232,6 @@ public:
 #endif
 
 private:
-	DriveMovement *FindDM(size_t drive) const noexcept;						// find the DM for a drive if there is one even if it is completed
 	DriveMovement *FindActiveDM(size_t drive) const noexcept;				// find the DM for a drive if there is one but only if it is active
 	void RecalculateMove(DDARing& ring) noexcept SPEED_CRITICAL;
 	void MatchSpeeds() noexcept SPEED_CRITICAL;
@@ -351,7 +351,7 @@ private:
 	// These three could possibly be moved into afterPrepare
 	DriveMovement* activeDMs;						// list of associated DMs that need steps, in step time order
 	DriveMovement* completedDMs;					// list of associated DMs that don't need any more steps
-	MoveSegment* shapedSegments;						// linked list of move segments used by axis DMs
+	MoveSegment* shapedSegments;					// linked list of move segments used by axis DMs
 	MoveSegment* unshapedSegments;					// linked list of move segments used by extruder DMs
 };