More RTOS work

Refactored mutex and task interface
Output channels, tool list and message box data are now thread safe

23 files changed, 453 insertions, 285 deletions

diff --git a/EdgeRelease/Duet2CombinedFirmware.bin b/EdgeRelease/Duet2CombinedFirmware.bin
new file mode 100644
index 00000000..56bb6a1e
--- /dev/null
+++ b/EdgeRelease/Duet2CombinedFirmware.bin
diff --git a/src/Heating/Heat.cpp b/src/Heating/Heat.cpp
index 977e0b28..ef3bc3f9 100644
--- a/src/Heating/Heat.cpp
+++ b/src/Heating/Heat.cpp
@@ -37,8 +37,7 @@ const uint32_t HeaterTaskStackSize = 128;			// task stack size in dwords
 const uint32_t HeaterTaskPriority = 1;
 
 static uint32_t heaterTaskStack[HeaterTaskStackSize];
-static TaskStorage heaterTaskBuffer;
-static TaskHandle heaterTaskHandle;
+static Task heaterTask;
 
 extern "C" void HeaterTask(void * pvParameters)
 {
@@ -145,7 +144,7 @@ void Heat::Init()
 	coldExtrude = false;
 
 #ifdef RTOS
-	heaterTaskHandle = RTOSIface::CreateTask(HeaterTask, "HEAT", ARRAY_SIZE(heaterTaskStack), nullptr, HeaterTaskPriority, heaterTaskStack, heaterTaskBuffer);
+	heaterTask.Create(HeaterTask, "HEAT", ARRAY_SIZE(heaterTaskStack), nullptr, HeaterTaskPriority, heaterTaskStack);
 #else
 	lastTime = millis() - platform.HeatSampleInterval();		// flag the PIDS as due for spinning
 	longWait = millis();
@@ -161,7 +160,7 @@ void Heat::Exit()
 	}
 
 #ifdef RTOS
-	RTOSIface::SuspendTask(heaterTaskHandle);
+	heaterTask.Suspend();
 #else
 	active = false;
 #endif
@@ -255,7 +254,7 @@ void Heat::Diagnostics(MessageType mtype)
 		}
 	}
 #ifdef RTOS
-	Tasks::TaskDiagnostics(mtype, heaterTaskHandle);
+	Tasks::TaskDiagnostics(mtype, heaterTask);
 #endif
 }
 
diff --git a/src/Heating/Sensors/SpiTemperatureSensor.cpp b/src/Heating/Sensors/SpiTemperatureSensor.cpp
index a1a4fc26..6c447714 100644
--- a/src/Heating/Sensors/SpiTemperatureSensor.cpp
+++ b/src/Heating/Sensors/SpiTemperatureSensor.cpp
@@ -31,7 +31,7 @@ TemperatureError SpiTemperatureSensor::DoSpiTransaction(const uint8_t dataOut[],
 	uint8_t rawBytes[8];
 	spi_status_t sts;
 	{
-		Locker lock(Tasks::GetSpiMutextHandle(), 50);
+		MutexLocker lock(Tasks::GetSpiMutex(), 50);
 		if (!lock)
 		{
 			return TemperatureError::busBusy;
diff --git a/src/Libraries/Fatfs/conf_fatfs.h b/src/Libraries/Fatfs/conf_fatfs.h
index 5f0b8496..e488f494 100644
--- a/src/Libraries/Fatfs/conf_fatfs.h
+++ b/src/Libraries/Fatfs/conf_fatfs.h
@@ -218,9 +218,10 @@
 
 #ifdef RTOS
 
-#define _FS_REENTRANT		1		/* 0:Disable or 1:Enable */
-#define _FS_TIMEOUT			1000	/* Timeout period in unit of time ticks */
-#define _SYNC_t				void*	/* O/S dependent type of sync object. e.g. HANDLE, OS_EVENT*, ID and etc.. */
+struct Mutex;
+#define _FS_REENTRANT		1					/* 0:Disable or 1:Enable */
+#define _FS_TIMEOUT			1000				/* Timeout period in unit of time ticks */
+#define _SYNC_t				const struct Mutex*	/* O/S dependent type of sync object. e.g. HANDLE, OS_EVENT*, ID and etc.. */
 
 #else
 
diff --git a/src/Libraries/Fatfs/diskio.cpp b/src/Libraries/Fatfs/diskio.cpp
index 925803c5..6ee696eb 100644
--- a/src/Libraries/Fatfs/diskio.cpp
+++ b/src/Libraries/Fatfs/diskio.cpp
@@ -99,7 +99,7 @@ DSTATUS disk_initialize(BYTE drv)
 	}
 #endif
 
-	Locker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutextHandle() : nullptr);
+	MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
 	Ctrl_status mem_status;
 
@@ -133,7 +133,7 @@ DSTATUS disk_initialize(BYTE drv)
  */
 DSTATUS disk_status(BYTE drv)
 {
-	Locker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutextHandle() : nullptr);
+	MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
 	switch (mem_test_unit_ready(drv)) {
 	case CTRL_GOOD:
@@ -159,7 +159,7 @@ DRESULT disk_read(BYTE drv, BYTE *buff, DWORD sector, BYTE count)
 {
 //	debugPrintf("R %u %u\n", sector, count);
 #if ACCESS_MEM_TO_RAM
-	Locker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutextHandle() : nullptr);
+	MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
 	uint8_t uc_sector_size = mem_sector_size(drv);
 	uint32_t ul_last_sector_num;
@@ -206,7 +206,7 @@ DRESULT disk_write(BYTE drv, BYTE const *buff, DWORD sector, BYTE count)
 {
 //	debugPrintf("W %u %u\n", sector, count);
 #if ACCESS_MEM_TO_RAM
-	Locker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutextHandle() : nullptr);
+	MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
 	uint8_t uc_sector_size = mem_sector_size(drv);
 	uint32_t ul_last_sector_num;
@@ -274,7 +274,7 @@ DRESULT disk_ioctl(BYTE drv, BYTE ctrl, void *buff)
 	/* Get the number of sectors on the disk (DWORD) */
 	case GET_SECTOR_COUNT:
 	{
-		Locker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutextHandle() : nullptr);
+		MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
 		uint32_t ul_last_sector_num;
 
@@ -290,7 +290,7 @@ DRESULT disk_ioctl(BYTE drv, BYTE ctrl, void *buff)
 	/* Get sectors on the disk (WORD) */
 	case GET_SECTOR_SIZE:
 	{
-		Locker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutextHandle() : nullptr);
+		MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
 		uint8_t uc_sector_size = mem_sector_size(drv);
 
@@ -311,7 +311,7 @@ DRESULT disk_ioctl(BYTE drv, BYTE ctrl, void *buff)
 	/* Make sure that data has been written */
 	case CTRL_SYNC:
 		{
-			Locker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutextHandle() : nullptr);
+			MutexLocker lock((drv >= SD_MMC_HSMCI_MEM_CNT) ? Tasks::GetSpiMutex() : nullptr);
 
 			if (mem_test_unit_ready(drv) == CTRL_GOOD) {
 				res = RES_OK;
diff --git a/src/Networking/Network.cpp b/src/Networking/Network.cpp
index 18586b72..3f729e14 100644
--- a/src/Networking/Network.cpp
+++ b/src/Networking/Network.cpp
@@ -50,6 +50,9 @@ Network::Network(Platform& p) : platform(p), responders(nullptr), nextResponderT
 // Note that Platform::Init() must be called before this to that Platform::IsDuetWiFi() returns the correct value
 void Network::Init()
 {
+	httpMutex.Create();
+	telnetMutex.Create();
+
 #if defined(DUET_NG)
 	interfaces[0] = (platform.IsDuetWiFi()) ? static_cast<NetworkInterface*>(new WiFiInterface(platform)) : static_cast<NetworkInterface*>(new W5500Interface(platform));
 #endif
@@ -386,21 +389,25 @@ bool Network::FindResponder(Socket *skt, NetworkProtocol protocol)
 
 void Network::HandleHttpGCodeReply(const char *msg)
 {
+	MutexLocker lock(httpMutex);
 	HttpResponder::HandleGCodeReply(msg);
 }
 
 void Network::HandleTelnetGCodeReply(const char *msg)
 {
+	MutexLocker lock(telnetMutex);
 	TelnetResponder::HandleGCodeReply(msg);
 }
 
 void Network::HandleHttpGCodeReply(OutputBuffer *buf)
 {
+	MutexLocker lock(httpMutex);
 	HttpResponder::HandleGCodeReply(buf);
 }
 
 void Network::HandleTelnetGCodeReply(OutputBuffer *buf)
 {
+	MutexLocker lock(telnetMutex);
 	TelnetResponder::HandleGCodeReply(buf);
 }
 
diff --git a/src/Networking/Network.h b/src/Networking/Network.h
index 308ec272..38d330b8 100644
--- a/src/Networking/Network.h
+++ b/src/Networking/Network.h
@@ -12,6 +12,7 @@
 #include "RepRapFirmware.h"
 #include "MessageType.h"
 #include "GCodes/GCodeResult.h"
+#include "RTOSIface.h"
 
 #if defined(SAME70_TEST_BOARD)
 const size_t NumNetworkInterfaces = 2;
@@ -86,6 +87,8 @@ private:
 	NetworkResponder *responders;
 	NetworkResponder *nextResponderToPoll;
 
+	Mutex httpMutex, telnetMutex;
+
 	char hostname[16];								// Limit DHCP hostname to 15 characters + terminating 0
 };
 
diff --git a/src/OutputMemory.cpp b/src/OutputMemory.cpp
index 5aab2db1..14cf2c7d 100644
--- a/src/OutputMemory.cpp
+++ b/src/OutputMemory.cpp
@@ -342,27 +342,29 @@ bool OutputBuffer::WriteToFile(FileData& f) const
 	}
 }
 
-// Allocates an output buffer instance which can be used for (large) string outputs
+// Allocates an output buffer instance which can be used for (large) string outputs. This must be thread safe.
 /*static*/ bool OutputBuffer::Allocate(OutputBuffer *&buf)
 {
-	const irqflags_t flags = cpu_irq_save();
-
-	if (freeOutputBuffers == nullptr)
 	{
-		reprap.GetPlatform().LogError(ErrorCode::OutputStarvation);
-		cpu_irq_restore(flags);
+		CriticalSectionLocker lock;
 
-		buf = nullptr;
-		return false;
-	}
+		buf = freeOutputBuffers;
+		if (buf != nullptr)
+		{
+			freeOutputBuffers = buf->next;
 
-	buf = freeOutputBuffers;
-	freeOutputBuffers = buf->next;
+			usedOutputBuffers++;
+			if (usedOutputBuffers > maxUsedOutputBuffers)
+			{
+				maxUsedOutputBuffers = usedOutputBuffers;
+			}
+		}
+	}
 
-	usedOutputBuffers++;
-	if (usedOutputBuffers > maxUsedOutputBuffers)
+	if (buf == nullptr)
 	{
-		maxUsedOutputBuffers = usedOutputBuffers;
+		reprap.GetPlatform().LogError(ErrorCode::OutputStarvation);
+		return false;
 	}
 
 	buf->next = nullptr;
@@ -371,7 +373,6 @@ bool OutputBuffer::WriteToFile(FileData& f) const
 	buf->references = 1; // Assume it's only used once by default
 	buf->isReferenced = false;
 
-	cpu_irq_restore(flags);
 	return true;
 }
 
@@ -438,7 +439,7 @@ bool OutputBuffer::WriteToFile(FileData& f) const
 // Releases an output buffer instance and returns the next entry from the chain
 /*static */ OutputBuffer *OutputBuffer::Release(OutputBuffer *buf)
 {
-	const irqflags_t flags = cpu_irq_save();
+	CriticalSectionLocker lock;
 	OutputBuffer * const nextBuffer = buf->next;
 
 	// If this one is reused by another piece of code, don't free it up
@@ -446,16 +447,14 @@ bool OutputBuffer::WriteToFile(FileData& f) const
 	{
 		buf->references--;
 		buf->bytesRead = 0;
-		cpu_irq_restore(flags);
-		return nextBuffer;
 	}
-
-	// Otherwise prepend it to the list of free output buffers again
-	buf->next = freeOutputBuffers;
-	freeOutputBuffers = buf;
-	usedOutputBuffers--;
-
-	cpu_irq_restore(flags);
+	else
+	{
+		// Otherwise prepend it to the list of free output buffers again
+		buf->next = freeOutputBuffers;
+		freeOutputBuffers = buf;
+		usedOutputBuffers--;
+	}
 	return nextBuffer;
 }
 
@@ -489,9 +488,8 @@ void OutputStack::Push(OutputBuffer *buffer)
 	if (buffer != nullptr)
 	{
 		buffer->whenQueued = millis();
-		const irqflags_t flags = cpu_irq_save();
+		CriticalSectionLocker lock;
 		items[count++] = buffer;
-		cpu_irq_restore(flags);
 	}
 }
 
@@ -503,14 +501,13 @@ OutputBuffer *OutputStack::Pop()
 		return nullptr;
 	}
 
-	const irqflags_t flags = cpu_irq_save();
+	CriticalSectionLocker lock;
 	OutputBuffer *item = items[0];
 	for(size_t i = 1; i < count; i++)
 	{
 		items[i - 1] = items[i];
 	}
 	count--;
-	cpu_irq_restore(flags);
 
 	return item;
 }
@@ -528,7 +525,7 @@ OutputBuffer *OutputStack::GetFirstItem() const
 // Set the first item of the stack. If it's NULL, then the first item will be removed
 void OutputStack::SetFirstItem(OutputBuffer *buffer)
 {
-	const irqflags_t flags = cpu_irq_save();
+	CriticalSectionLocker lock;
 	if (buffer == nullptr)
 	{
 		// If buffer is NULL, then the first item is removed from the stack
@@ -544,7 +541,6 @@ void OutputStack::SetFirstItem(OutputBuffer *buffer)
 		items[0] = buffer;
 		buffer->whenQueued = millis();
 	}
-	cpu_irq_restore(flags);
 }
 
 // Returns the last item from the stack or NULL if none is available
@@ -562,12 +558,11 @@ size_t OutputStack::DataLength() const
 {
 	size_t totalLength = 0;
 
-	const irqflags_t flags = cpu_irq_save();
+	CriticalSectionLocker lock;
 	for(size_t i = 0; i < count; i++)
 	{
 		totalLength += items[i]->Length();
 	}
-	cpu_irq_restore(flags);
 
 	return totalLength;
 }
@@ -593,12 +588,11 @@ void OutputStack::Append(OutputStack *stack)
 // Increase the number of references for each OutputBuffer on the stack
 void OutputStack::IncreaseReferences(size_t num)
 {
-	const irqflags_t flags = cpu_irq_save();
+	CriticalSectionLocker lock;
 	for(size_t i = 0; i < count; i++)
 	{
 		items[i]->IncreaseReferences(num);
 	}
-	cpu_irq_restore(flags);
 }
 
 // Release all buffers and clean up
diff --git a/src/Platform.cpp b/src/Platform.cpp
index 17f25317..7f391bc6 100644
--- a/src/Platform.cpp
+++ b/src/Platform.cpp
@@ -223,6 +223,8 @@ void Platform::Init()
 	commsParams[2] = 0;
 #endif
 
+	usbMutex.Create();
+	auxMutex.Create();
 	auxDetected = false;
 	auxSeq = 0;
 
@@ -230,6 +232,7 @@ void Platform::Init()
 	SERIAL_AUX_DEVICE.begin(baudRates[1]);		// this can't be done in the constructor because the Arduino port initialisation isn't complete at that point
 #ifdef SERIAL_AUX2_DEVICE
 	SERIAL_AUX2_DEVICE.begin(baudRates[2]);
+	aux2Mutex.Create();
 #endif
 
 	compatibility = Compatibility::marlin;		// default to Marlin because the common host programs expect the "OK" response to commands
@@ -1190,6 +1193,7 @@ void Platform::SetNetMask(uint8_t nm[])
 bool Platform::FlushAuxMessages()
 {
 	// Write non-blocking data to the AUX line
+	MutexLocker lock(auxMutex);
 	OutputBuffer *auxOutputBuffer = auxOutput->GetFirstItem();
 	if (auxOutputBuffer != nullptr)
 	{
@@ -1215,55 +1219,65 @@ bool Platform::FlushMessages()
 
 #ifdef SERIAL_AUX2_DEVICE
 	// Write non-blocking data to the second AUX line
-	OutputBuffer *aux2OutputBuffer = aux2Output->GetFirstItem();
-	if (aux2OutputBuffer != nullptr)
+	bool aux2hasMore;
 	{
-		size_t bytesToWrite = min<size_t>(SERIAL_AUX2_DEVICE.canWrite(), aux2OutputBuffer->BytesLeft());
-		if (bytesToWrite > 0)
+		MutexLocker lock(aux2MutexHandle);
+		OutputBuffer *aux2OutputBuffer = aux2Output->GetFirstItem();
+		if (aux2OutputBuffer != nullptr)
 		{
-			SERIAL_AUX2_DEVICE.write(aux2OutputBuffer->Read(bytesToWrite), bytesToWrite);
-		}
+			size_t bytesToWrite = min<size_t>(SERIAL_AUX2_DEVICE.canWrite(), aux2OutputBuffer->BytesLeft());
+			if (bytesToWrite > 0)
+			{
+				SERIAL_AUX2_DEVICE.write(aux2OutputBuffer->Read(bytesToWrite), bytesToWrite);
+			}
 
-		if (aux2OutputBuffer->BytesLeft() == 0)
-		{
-			aux2OutputBuffer = OutputBuffer::Release(aux2OutputBuffer);
-			aux2Output->SetFirstItem(aux2OutputBuffer);
+			if (aux2OutputBuffer->BytesLeft() == 0)
+			{
+				aux2OutputBuffer = OutputBuffer::Release(aux2OutputBuffer);
+				aux2Output->SetFirstItem(aux2OutputBuffer);
+			}
 		}
+		aux2hasMore = (aux2Output->GetFirstItem() != nullptr);
 	}
 #endif
 
 	// Write non-blocking data to the USB line
-	OutputBuffer *usbOutputBuffer = usbOutput->GetFirstItem();
-	if (usbOutputBuffer != nullptr)
+	bool usbHasMore;
 	{
-		if (!SERIAL_MAIN_DEVICE)
-		{
-			// If the USB port is not opened, free the data left for writing
-			OutputBuffer::ReleaseAll(usbOutputBuffer);
-			usbOutput->SetFirstItem(nullptr);
-		}
-		else
+		MutexLocker lock(usbMutex);
+		OutputBuffer *usbOutputBuffer = usbOutput->GetFirstItem();
+		if (usbOutputBuffer != nullptr)
 		{
-			// Write as much data as we can...
-			size_t bytesToWrite = min<size_t>(SERIAL_MAIN_DEVICE.canWrite(), usbOutputBuffer->BytesLeft());
-			if (bytesToWrite > 0)
+			if (!SERIAL_MAIN_DEVICE)
 			{
-				SERIAL_MAIN_DEVICE.write(usbOutputBuffer->Read(bytesToWrite), bytesToWrite);
+				// If the USB port is not opened, free the data left for writing
+				OutputBuffer::ReleaseAll(usbOutputBuffer);
+				usbOutput->SetFirstItem(nullptr);
 			}
-
-			if (usbOutputBuffer->BytesLeft() == 0 || usbOutputBuffer->GetAge() > SERIAL_MAIN_TIMEOUT)
+			else
 			{
-				usbOutputBuffer = OutputBuffer::Release(usbOutputBuffer);
-				usbOutput->SetFirstItem(usbOutputBuffer);
+				// Write as much data as we can...
+				size_t bytesToWrite = min<size_t>(SERIAL_MAIN_DEVICE.canWrite(), usbOutputBuffer->BytesLeft());
+				if (bytesToWrite > 0)
+				{
+					SERIAL_MAIN_DEVICE.write(usbOutputBuffer->Read(bytesToWrite), bytesToWrite);
+				}
+
+				if (usbOutputBuffer->BytesLeft() == 0 || usbOutputBuffer->GetAge() > SERIAL_MAIN_TIMEOUT)
+				{
+					usbOutputBuffer = OutputBuffer::Release(usbOutputBuffer);
+					usbOutput->SetFirstItem(usbOutputBuffer);
+				}
 			}
 		}
+		usbHasMore = (usbOutput->GetFirstItem() != nullptr);
 	}
 
 	return auxHasMore
 #ifdef SERIAL_AUX2_DEVICE
-		|| aux2Output->GetFirstItem() != nullptr
+		|| aux2HasMore
 #endif
-		|| usbOutput->GetFirstItem() != nullptr;
+		|| usbHasMore;
 }
 
 void Platform::Spin()
@@ -3323,6 +3337,7 @@ void Platform::AppendAuxReply(const char *msg, bool rawMessage)
 	// Discard this response if either no aux device is attached or if the response is empty
 	if (msg[0] != 0 && HaveAux())
 	{
+		MutexLocker lock(auxMutex);
 		if (rawMessage)
 		{
 			// Raw responses are sent directly to the AUX device
@@ -3352,23 +3367,27 @@ void Platform::AppendAuxReply(OutputBuffer *reply, bool rawMessage)
 	{
 		OutputBuffer::ReleaseAll(reply);
 	}
-	else if (rawMessage)
-	{
-		// JSON responses are always sent directly to the AUX device
-		// For big responses it makes sense to write big chunks of data in portions. Store this data here
-		auxOutput->Push(reply);
-	}
 	else
 	{
-		// Other responses are stored for M105/M408
-		auxSeq++;
-		if (auxGCodeReply == nullptr)
+		MutexLocker lock(auxMutex);
+		if (rawMessage)
 		{
-			auxGCodeReply = reply;
+			// JSON responses are always sent directly to the AUX device
+			// For big responses it makes sense to write big chunks of data in portions. Store this data here
+			auxOutput->Push(reply);
 		}
 		else
 		{
-			auxGCodeReply->Append(reply);
+			// Other responses are stored for M105/M408
+			auxSeq++;
+			if (auxGCodeReply == nullptr)
+			{
+				auxGCodeReply = reply;
+			}
+			else
+			{
+				auxGCodeReply->Append(reply);
+			}
 		}
 	}
 }
@@ -3405,6 +3424,7 @@ void Platform::RawMessage(MessageType type, const char *message)
 	if ((type & AuxMessage) != 0)
 	{
 #ifdef SERIAL_AUX2_DEVICE
+		MutexLocker lock(aux2Mutex);
 		// Message that is to be sent to the second auxiliary device (blocking)
 		if (!aux2Output->IsEmpty())
 		{
@@ -3423,6 +3443,7 @@ void Platform::RawMessage(MessageType type, const char *message)
 	if ((type & BlockingUsbMessage) != 0)
 	{
 		// Debug output sends messages in blocking mode. We now give up sending if we are close to software watchdog timeout.
+		MutexLocker lock(usbMutex);
 		const char *p = message;
 		size_t len = strlen(p);
 		while (SERIAL_MAIN_DEVICE && len != 0 && !reprap.SpinTimeoutImminent())
@@ -3436,6 +3457,7 @@ void Platform::RawMessage(MessageType type, const char *message)
 	else if ((type & UsbMessage) != 0)
 	{
 		// Message that is to be sent via the USB line (non-blocking)
+		MutexLocker lock(usbMutex);
 #if SUPPORT_SCANNER
 		if (!reprap.GetScanner().IsRegistered() || reprap.GetScanner().DoingGCodes())
 #endif
@@ -3522,12 +3544,14 @@ void Platform::Message(const MessageType type, OutputBuffer *buffer)
 		if ((type & AuxMessage) != 0)
 		{
 			// Send this message to the second UART device
+			MutexLocker lock(aux2Mutex);
 			aux2Output->Push(buffer);
 		}
 #endif
 
 		if ((type & (UsbMessage | BlockingUsbMessage)) != 0)
 		{
+			MutexLocker lock(usbMutex);
 			if (   !SERIAL_MAIN_DEVICE
 #if SUPPORT_SCANNER
 				|| (reprap.GetScanner().IsRegistered() && !reprap.GetScanner().DoingGCodes())
@@ -3548,24 +3572,23 @@ void Platform::Message(const MessageType type, OutputBuffer *buffer)
 
 void Platform::MessageF(MessageType type, const char *fmt, va_list vargs)
 {
-	char formatBuffer[FORMAT_STRING_LENGTH];
-	StringRef formatString(formatBuffer, ARRAY_SIZE(formatBuffer));
+	String<FORMAT_STRING_LENGTH> formatString;
 	if ((type & ErrorMessageFlag) != 0)
 	{
 		formatString.copy("Error: ");
-		formatString.vcatf(fmt, vargs);
+		formatString.GetRef().vcatf(fmt, vargs);
 	}
 	else if ((type & WarningMessageFlag) != 0)
 	{
 		formatString.copy("Warning: ");
-		formatString.vcatf(fmt, vargs);
+		formatString.GetRef().vcatf(fmt, vargs);
 	}
 	else
 	{
-		formatString.vprintf(fmt, vargs);
+		formatString.GetRef().vprintf(fmt, vargs);
 	}
 
-	RawMessage((MessageType)(type & ~(ErrorMessageFlag | WarningMessageFlag)), formatBuffer);
+	RawMessage((MessageType)(type & ~(ErrorMessageFlag | WarningMessageFlag)), formatString.c_str());
 }
 
 void Platform::MessageF(MessageType type, const char *fmt, ...)
@@ -3584,11 +3607,10 @@ void Platform::Message(MessageType type, const char *message)
 	}
 	else
 	{
-		char formatBuffer[FORMAT_STRING_LENGTH];
-		StringRef formatString(formatBuffer, ARRAY_SIZE(formatBuffer));
+		String<FORMAT_STRING_LENGTH> formatString;
 		formatString.copy(((type & ErrorMessageFlag) != 0) ? "Error: " : "Warning: ");
 		formatString.cat(message);
-		RawMessage((MessageType)(type & ~(ErrorMessageFlag | WarningMessageFlag)), formatBuffer);
+		RawMessage((MessageType)(type & ~(ErrorMessageFlag | WarningMessageFlag)), formatString.c_str());
 	}
 }
 
diff --git a/src/Platform.h b/src/Platform.h
index 32d3e270..ef2294a1 100644
--- a/src/Platform.h
+++ b/src/Platform.h
@@ -826,14 +826,20 @@ private:
   	// Serial/USB
 	uint32_t baudRates[NUM_SERIAL_CHANNELS];
 	uint8_t commsParams[NUM_SERIAL_CHANNELS];
+
 	OutputStack *auxOutput;
+	Mutex auxMutex;
+	OutputBuffer *auxGCodeReply;				// G-Code reply for AUX devices (special one because it is actually encapsulated before sending)
+	uint32_t auxSeq;							// Sequence number for AUX devices
+    bool auxDetected;							// Have we processed at least one G-Code from an AUX device?
+
 #ifdef SERIAL_AUX2_DEVICE
 	OutputStack *aux2Output;
+	Mutex aux2Mutex;
 #endif
+
 	OutputStack *usbOutput;
-    bool auxDetected;							// Have we processed at least one G-Code from an AUX device?
-	OutputBuffer *auxGCodeReply;				// G-Code reply for AUX devices (special one because it is actually encapsulated before sending)
-	uint32_t auxSeq;							// Sequence number for AUX devices
+	Mutex usbMutex;
 
 	// Files
 	MassStorage* massStorage;
diff --git a/src/RTOS work pending.txt b/src/RTOS work pending.txt
new file mode 100644
index 00000000..6edb4475
--- /dev/null
+++ b/src/RTOS work pending.txt
@@ -0,0 +1,36 @@
+Changes to RRF for RTOS
+
+[done] Include FreeRTOS library in the link library list and run RRF as a single FreeRTOS task
+[done] Run Heat as a separate task
+[done] Replace delay() by a version that calls FreeRTOS
+[done] Have hsmciIdleFunc suspend the calling task pending an interrupt
+[done] Use a mutex to protect fatfs from being reentered, preferably a separate mutex for each volume
+[done] Use a mutex to protect the shared SPI subsystem from reentrant calls, and use it the filesystem and the temperature sensor reading system
+[done] Use a mutex when allocating file handles
+[done] Use a mutex to protect the find first/next buffer
+[done] make getting file info thread safe
+[done] buffer allocation needs to be thread safe
+[done] tool list traversal to be thread safe
+[done] make USB and serial out channels thread safe
+[done] make message box output thread safe
+[done except for mutexes on network and telnet] Make Platform::Message thread safe
+Are there any aspects of Platform that are not thread-safe? e.g. GCodes setting stuff while network reports it?
+Get rid of shared scratchString
+Run Network as a separate task
+Run Move as a separate task. Probably needs a mutex to protect calls into it, e.g. babystepping and pause.
+Use DMA for SharedSPI transfers, suspend the calling task while waiting on a transfer
+Do a SAM4S version of FreeRTOS, then do a SAM4S_RTOS build of RRF
+Stop allocating file buffers on the stack?
+Find maximum stack sizes and optimise stacks
+Run all network responders as separate tasks? Would need mutex on network interface access
+
+Bugs
+
+[done] IAP fails to update firmware
+M122 output is too big for the available buffers, unless the Network task reads the output fast enough
+[done] Disconnects when getting file info forCandyBowlx8comments.gcode when running RTOS build
+File upload speed is a little lower when using RTOS - may be fixed when network is a separate task
+
+Deferred
+
+Use a mutex to protect HSMCI from reentrant calls (only needed if/when we support more than one HSMCI volume, which is unlikely but possoible on the SAME70)
diff --git a/src/RTOSIface.cpp b/src/RTOSIface.cpp
index 5d364739..dbde75c2 100644
--- a/src/RTOSIface.cpp
+++ b/src/RTOSIface.cpp
@@ -13,77 +13,107 @@
 # include "task.h"
 # include "semphr.h"
 
-static_assert(RTOSIface::TimeoutUnlimited == portMAX_DELAY, "Bad value for TimeoutUnlimited");
+static_assert(Mutex::TimeoutUnlimited == portMAX_DELAY, "Bad value for TimeoutUnlimited");
+
+void Mutex::Create()
+{
+	if (handle == nullptr)
+	{
+		handle = xSemaphoreCreateRecursiveMutexStatic(&storage);
+	}
+}
+
+bool Mutex::Take(uint32_t timeout) const
+{
+	return xSemaphoreTakeRecursive(handle, timeout) == pdTRUE;
+}
+
+bool Mutex::Release() const
+{
+	return xSemaphoreGiveRecursive(handle) == pdTRUE;
+}
+
+TaskHandle Mutex::GetHolder() const
+{
+	return static_cast<TaskHandle>(xSemaphoreGetMutexHolder(handle));
+}
+
+void Task::Create(TaskFunction_t pxTaskCode, const char * pcName, uint32_t ulStackDepth, void *pvParameters, unsigned int uxPriority, uint32_t * const puxStackBuffer)
+{
+	handle = xTaskCreateStatic(pxTaskCode, pcName, ulStackDepth, pvParameters, uxPriority, puxStackBuffer, &storage);
+}
+
+#else
+
+void Mutex::Create()
+{
+}
+
+bool Mutex::Take(uint32_t timeout) const
+{
+	return true;
+}
+
+bool Mutex::Release() const
+{
+	return true;
+}
+
+TaskHandle Mutex::GetHolder() const
+{
+	return nullptr;
+}
 
 #endif
 
-Locker::Locker(MutexHandle hnd)
+MutexLocker::MutexLocker(const Mutex *m, uint32_t timeout)
 {
-	handle = hnd;
+	handle = m;
 	acquired =
 #ifdef RTOS
-				hnd == nullptr || xSemaphoreTakeRecursive(hnd, portMAX_DELAY);
+				m == nullptr || m->Take(timeout);
 #else
 				true;
 #endif
 }
 
-Locker::Locker(MutexHandle hnd, uint32_t timeout)
+MutexLocker::MutexLocker(const Mutex& m, uint32_t timeout)
 {
-	handle = hnd;
+	handle = &m;
 	acquired =
 #ifdef RTOS
-				hnd == nullptr || xSemaphoreTakeRecursive(hnd, timeout);
+				m.Take(timeout);
 #else
 				true;
 #endif
 }
 
-Locker::~Locker()
+void MutexLocker::Release()
 {
 #ifdef RTOS
 	if (acquired && handle != nullptr)
 	{
-		xSemaphoreGiveRecursive(handle);
+		handle->Release();
+		acquired = false;
 	}
 #endif
 }
 
-namespace RTOSIface
+MutexLocker::~MutexLocker()
 {
-
+	Release();
 #ifdef RTOS
-
-	TaskHandle CreateTask(TaskFunction_t pxTaskCode, const char * pcName, uint32_t ulStackDepth, void *pvParameters, unsigned int uxPriority,
-							uint32_t * const puxStackBuffer, TaskStorage& taskBuffer)
-	{
-		return static_cast<TaskHandle>(xTaskCreateStatic(pxTaskCode, pcName, ulStackDepth, pvParameters, uxPriority, puxStackBuffer, &taskBuffer));
-	}
-
-	void SuspendTask(TaskHandle hnd)
-	{
-		vTaskSuspend(hnd);
-	}
-
-	MutexHandle CreateMutex(MutexStorage& st)
-	{
-		return static_cast<MutexHandle>(xSemaphoreCreateRecursiveMutexStatic(&st));
-	}
-
-	bool TakeMutex(MutexHandle hnd, uint32_t timeout)
+	if (acquired && handle != nullptr)
 	{
-		return xSemaphoreTakeRecursive(hnd, timeout) == pdTRUE;
+		handle->Release();
 	}
+#endif
+}
 
-	bool ReleaseMutex(MutexHandle hnd)
-	{
-		return xSemaphoreGiveRecursive(hnd) == pdTRUE;
-	}
+namespace RTOSIface
+{
 
-	TaskHandle GetMutexHolder(MutexHandle hnd)
-	{
-		return static_cast<TaskHandle>(xSemaphoreGetMutexHolder(hnd));
-	}
+#ifdef RTOS
 
 	TaskHandle GetCurrentTask()
 	{
@@ -92,26 +122,6 @@ namespace RTOSIface
 
 #else
 
-	MutexHandle CreateMutex(MutexStorage& st)
-	{
-		return static_cast<MutexHandle>((void *)&st);
-	}
-
-	bool TakeMutex(MutexHandle hnd, uint32_t timeout)
-	{
-		return true;
-	}
-
-	bool ReleaseMutex(MutexHandle hnd)
-	{
-		return true;
-	}
-
-	TaskHandle GetMutexHolder(MutexHandle hnd)
-	{
-		return nullptr;
-	}
-
 	TaskHandle GetCurrentTask()
 	{
 		return nullptr;
diff --git a/src/RTOSIface.h b/src/RTOSIface.h
index af21204f..4bcb4f31 100644
--- a/src/RTOSIface.h
+++ b/src/RTOSIface.h
@@ -15,54 +15,118 @@
 class Task_undefined;					// this class is never defined
 typedef Task_undefined *TaskHandle;
 
-class Mutex_undefined;					// this class is never defined
-typedef Mutex_undefined *MutexHandle;
-
 #ifdef RTOS
 # include "FreeRTOS.h"
 # include "task.h"
 # include "semphr.h"
+#else
+# include "asf.h"
+#endif
+
+class Mutex
+{
+public:
+	Mutex() { handle = nullptr; }
+
+	void Create();
+	bool Take(uint32_t timeout = TimeoutUnlimited) const;
+	bool Release() const;
+	TaskHandle GetHolder() const;
+
+	static constexpr uint32_t TimeoutUnlimited = 0xFFFFFFFF;
 
-typedef StaticSemaphore_t MutexStorage;
-typedef StaticTask_t TaskStorage;
+	Mutex(const Mutex&) = delete;
+	Mutex& operator=(const Mutex&) = delete;
+
+private:
 
+#ifdef RTOS
+	SemaphoreHandle_t handle;
+	StaticSemaphore_t storage;
 #else
+	void *handle;
+#endif
+
+};
+
+#ifdef RTOS
+
+class Task
+{
+public:
+	Task() { handle = nullptr; }
 
-typedef int MutexStorage;
-typedef int TaskStorage;
+	TaskHandle GetHandle() const { return static_cast<TaskHandle>(handle); }
+	void Create(TaskFunction_t pxTaskCode, const char * pcName, uint32_t ulStackDepth, void *pvParameters, unsigned int uxPriority, uint32_t * const puxStackBuffer);
+	void Suspend() { vTaskSuspend(handle); }
+
+	Task(const Task&) = delete;
+	Task& operator=(const Task&) = delete;
+
+private:
+	TaskHandle_t handle;
+	StaticTask_t storage;
+};
 
 #endif
 
 // Class to lock a mutex and automatically release it when it goes out of scope
 // If we pass a null mutex handle to the Locker constructor, it means there is nothing to lock and we pretend the lock has been acquired.
-class Locker
+class MutexLocker
 {
 public:
-	Locker(MutexHandle hnd);						// acquire lock, no timeout
-	Locker(MutexHandle hnd, uint32_t timeout);	// acquire lock with timeout
-	~Locker();
+	MutexLocker(const Mutex *pm, uint32_t timeout = Mutex::TimeoutUnlimited);	// acquire lock
+	MutexLocker(const Mutex& pm, uint32_t timeout = Mutex::TimeoutUnlimited);	// acquire lock
+	void Release();															// release the lock early (else gets released by destructor)
+	~MutexLocker();
 	operator bool() const { return acquired; }
 
+	MutexLocker(const MutexLocker&) = delete;
+	MutexLocker& operator=(const MutexLocker&) = delete;
+
 private:
-	MutexHandle handle;
+	const Mutex *handle;
 	bool acquired;
 };
 
+// Interface to RTOS or RTOS substitute
 namespace RTOSIface
 {
-	constexpr uint32_t TimeoutUnlimited = 0xFFFFFFFF;
+	TaskHandle GetCurrentTask();
 
-#if RTOS
-	TaskHandle CreateTask(TaskFunction_t pxTaskCode, const char * pcName, uint32_t ulStackDepth, void *pvParameters, unsigned int uxPriority,
-							uint32_t * const puxStackBuffer, TaskStorage& taskBuffer);
-	void SuspendTask(TaskHandle hnd);
+#ifndef RTOS
+	static volatile unsigned int criticalSectionNesting = 0;
 #endif
 
-	MutexHandle CreateMutex(MutexStorage& st);
-	bool TakeMutex(MutexHandle hnd, uint32_t timeout = TimeoutUnlimited);
-	bool ReleaseMutex(MutexHandle hnd);
-	TaskHandle GetMutexHolder(MutexHandle hnd);
-	TaskHandle GetCurrentTask();
+	inline void EnterCriticalSection()
+	{
+#ifdef RTOS
+		taskENTER_CRITICAL();
+#else
+		cpu_irq_disable();
+		++criticalSectionNesting;
+#endif
+	}
+
+	inline void LeaveCriticalSection()
+	{
+#ifdef RTOS
+		taskEXIT_CRITICAL();
+#else
+		--criticalSectionNesting;
+		if (criticalSectionNesting == 0)
+		{
+			cpu_irq_enable();
+		}
+#endif
+	}
 }
 
+class CriticalSectionLocker
+{
+public:
+	CriticalSectionLocker() { RTOSIface::EnterCriticalSection(); }
+	~CriticalSectionLocker() { RTOSIface::LeaveCriticalSection(); }
+};
+
 #endif /* SRC_RTOSIFACE_H_ */
diff --git a/src/RepRap.cpp b/src/RepRap.cpp
index a8108f72..9f38846a 100644
--- a/src/RepRap.cpp
+++ b/src/RepRap.cpp
@@ -135,6 +135,9 @@ RepRap::RepRap() : toolList(nullptr), currentTool(nullptr), lastWarningMillis(0)
 
 void RepRap::Init()
 {
+	toolListMutex.Create();
+	messageBoxMutex.Create();
+
 	// All of the following init functions must execute reasonably quickly before the watchdog times us out
 	platform->Init();
 	network->Init();
@@ -298,6 +301,7 @@ void RepRap::Spin()
 	const uint32_t now = millis();
 	if (now - lastWarningMillis >= MinimumWarningInterval)
 	{
+		MutexLocker lock(toolListMutex);
 		for (Tool *t = toolList; t != nullptr; t = t->Next())
 		{
 			if (t->DisplayColdExtrudeWarning())
@@ -352,18 +356,16 @@ void RepRap::EmergencyStop()
 	// Do not turn off ATX power here. If the nozzles are still hot, don't risk melting any surrounding parts...
 	//platform->SetAtxPower(false);
 
-	Tool* tool = toolList;
-	while (tool != nullptr)
+	// Deselect all tools (is this necessary?)
 	{
-		tool->Standby();
-		tool = tool->Next();
+		MutexLocker lock(toolListMutex);
+		for (Tool* tool = toolList; tool != nullptr; tool = tool->Next())
+		{
+			tool->Standby();
+		}
 	}
 
-	heat->Exit();
-	for (size_t heater = 0; heater < Heaters; heater++)
-	{
-		platform->SetHeater(heater, 0.0);
-	}
+	heat->Exit();		// this also turns off all heaters
 
 	// We do this twice, to avoid an interrupt switching a drive back on. move->Exit() should prevent interrupts doing this.
 	for (int i = 0; i < 2; i++)
@@ -423,6 +425,7 @@ void RepRap::PrintDebug()
 // The tool list is maintained in tool number order.
 void RepRap::AddTool(Tool* tool)
 {
+	MutexLocker lock(toolListMutex);
 	Tool** t = &toolList;
 	while(*t != nullptr && (*t)->Number() < tool->Number())
 	{
@@ -455,6 +458,7 @@ void RepRap::DeleteTool(Tool* tool)
 	}
 
 	// Purge any references to this tool
+	MutexLocker lock(toolListMutex);
 	for (Tool **t = &toolList; *t != nullptr; t = &((*t)->next))
 	{
 		if (*t == tool)
@@ -529,6 +533,7 @@ void RepRap::StandbyTool(int toolNumber, bool simulating)
 
 Tool* RepRap::GetTool(int toolNumber) const
 {
+	MutexLocker lock(toolListMutex);
 	Tool* tool = toolList;
 	while(tool != nullptr)
 	{
@@ -570,6 +575,7 @@ void RepRap::SetToolVariables(int toolNumber, const float* standbyTemperatures,
 
 bool RepRap::IsHeaterAssignedToTool(int8_t heater) const
 {
+	MutexLocker lock(toolListMutex);
 	for (Tool *tool = toolList; tool != nullptr; tool = tool->Next())
 	{
 		for (size_t i = 0; i < tool->HeaterCount(); i++)
@@ -718,6 +724,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		const bool sendMessage = (message[0] != 0);
 
 		float timeLeft = 0.0;
+		MutexLocker lock(messageBoxMutex);
 		if (displayMessageBox && boxTimer != 0)
 		{
 			timeLeft = (float)(boxTimeout) / 1000.0 - (float)(millis() - boxTimer) / 1000.0;
@@ -917,36 +924,39 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 
 		/* Tool temperatures */
 		response->cat("},\"tools\":{\"active\":[");
-		for (const Tool *tool = toolList; tool != nullptr; tool = tool->Next())
 		{
-			ch = '[';
-			for (size_t heater = 0; heater < tool->heaterCount; heater++)
+			MutexLocker lock(toolListMutex);
+			for (const Tool *tool = toolList; tool != nullptr; tool = tool->Next())
 			{
-				response->catf("%c%.1f", ch, (double)tool->activeTemperatures[heater]);
-				ch = ',';
-			}
-			response->cat((ch == '[') ? "[]" : "]");
+				ch = '[';
+				for (size_t heater = 0; heater < tool->heaterCount; heater++)
+				{
+					response->catf("%c%.1f", ch, (double)tool->activeTemperatures[heater]);
+					ch = ',';
+				}
+				response->cat((ch == '[') ? "[]" : "]");
 
-			if (tool->Next() != nullptr)
-			{
-				response->cat(",");
+				if (tool->Next() != nullptr)
+				{
+					response->cat(",");
+				}
 			}
-		}
 
-		response->cat("],\"standby\":[");
-		for (const Tool *tool = toolList; tool != nullptr; tool = tool->Next())
-		{
-			ch = '[';
-			for (size_t heater = 0; heater < tool->heaterCount; heater++)
+			response->cat("],\"standby\":[");
+			for (const Tool *tool = toolList; tool != nullptr; tool = tool->Next())
 			{
-				response->catf("%c%.1f", ch, (double)tool->standbyTemperatures[heater]);
-				ch = ',';
-			}
-			response->cat((ch == '[') ? "[]" : "]");
+				ch = '[';
+				for (size_t heater = 0; heater < tool->heaterCount; heater++)
+				{
+					response->catf("%c%.1f", ch, (double)tool->standbyTemperatures[heater]);
+					ch = ',';
+				}
+				response->cat((ch == '[') ? "[]" : "]");
 
-			if (tool->Next() != nullptr)
-			{
-				response->cat(",");
+				if (tool->Next() != nullptr)
+				{
+					response->cat(",");
+				}
 			}
 		}
 
@@ -1056,6 +1066,7 @@ OutputBuffer *RepRap::GetStatusResponse(uint8_t type, ResponseSource source)
 		/* Tool Mapping */
 		{
 			response->cat(",\"tools\":[");
+			MutexLocker lock(toolListMutex);
 			for (Tool *tool = toolList; tool != nullptr; tool = tool->Next())
 			{
 				// Number and Name
@@ -1490,25 +1501,29 @@ OutputBuffer *RepRap::GetLegacyStatusResponse(uint8_t type, int seq)
 	// We no longer send the amount of http buffer space here because the web interface doesn't use these forms of status response
 
 	// Deal with the message box, if there is one
-	float timeLeft = 0.0;
-	if (displayMessageBox && boxTimer != 0)
 	{
-		timeLeft = (float)(boxTimeout) / 1000.0 - (float)(millis() - boxTimer) / 1000.0;
-		displayMessageBox = (timeLeft > 0.0);
-	}
+		float timeLeft = 0.0;
+		MutexLocker lock(messageBoxMutex);
 
-	if (displayMessageBox)
-	{
-		response->catf(",\"msgBox.mode\":%d,\"msgBox.seq\":%" PRIu32 ",\"msgBox.timeout\":%.1f,\"msgBox.controls\":%" PRIu32 "",
-						boxMode, boxSeq, (double)timeLeft, boxControls);
-		response->cat(",\"msgBox.msg\":");
-		response->EncodeString(boxMessage.c_str(), boxMessage.Capacity(), false);
-		response->cat(",\"msgBox.title\":");
-		response->EncodeString(boxTitle.c_str(), boxTitle.Capacity(), false);
-	}
-	else
-	{
-		response->cat(",\"msgBox.mode\":-1");					// tell PanelDue that there is no active message box
+		if (displayMessageBox && boxTimer != 0)
+		{
+			timeLeft = (float)(boxTimeout) / 1000.0 - (float)(millis() - boxTimer) / 1000.0;
+			displayMessageBox = (timeLeft > 0.0);
+		}
+
+		if (displayMessageBox)
+		{
+			response->catf(",\"msgBox.mode\":%d,\"msgBox.seq\":%" PRIu32 ",\"msgBox.timeout\":%.1f,\"msgBox.controls\":%" PRIu32 "",
+							boxMode, boxSeq, (double)timeLeft, boxControls);
+			response->cat(",\"msgBox.msg\":");
+			response->EncodeString(boxMessage.c_str(), boxMessage.Capacity(), false);
+			response->cat(",\"msgBox.title\":");
+			response->EncodeString(boxTitle.c_str(), boxTitle.Capacity(), false);
+		}
+		else
+		{
+			response->cat(",\"msgBox.mode\":-1");					// tell PanelDue that there is no active message box
+		}
 	}
 
 	if (type == 2)
@@ -1812,6 +1827,7 @@ void RepRap::SetMessage(const char *msg)
 // Display a message box on the web interface
 void RepRap::SetAlert(const char *msg, const char *title, int mode, float timeout, AxesBitmap controls)
 {
+	MutexLocker lock(messageBoxMutex);
 	boxMessage.copy(msg);
 	boxTitle.copy(title);
 	boxMode = mode;
@@ -1825,6 +1841,7 @@ void RepRap::SetAlert(const char *msg, const char *title, int mode, float timeou
 // Clear pending message box
 void RepRap::ClearAlert()
 {
+	MutexLocker lock(messageBoxMutex);
 	displayMessageBox = false;
 }
 
@@ -1921,6 +1938,7 @@ unsigned int RepRap::GetProhibitedExtruderMovements(unsigned int extrusions, uns
 
 void RepRap::FlagTemperatureFault(int8_t dudHeater)
 {
+	MutexLocker lock(toolListMutex);
 	if (toolList != nullptr)
 	{
 		toolList->FlagTemperatureFault(dudHeater);
@@ -1930,6 +1948,7 @@ void RepRap::FlagTemperatureFault(int8_t dudHeater)
 void RepRap::ClearTemperatureFault(int8_t wasDudHeater)
 {
 	heat->ResetFault(wasDudHeater);
+	MutexLocker lock(toolListMutex);
 	if (toolList != nullptr)
 	{
 		toolList->ClearTemperatureFault(wasDudHeater);
@@ -1954,6 +1973,7 @@ bool RepRap::WriteToolSettings(FileStore *f) const
 {
 	// First write the settings of all tools except the current one and the command to select them if they are on standby
 	bool ok = true;
+	MutexLocker lock(toolListMutex);
 	for (const Tool *t = toolList; t != nullptr && ok; t = t->Next())
 	{
 		if (t != currentTool)
@@ -1974,6 +1994,7 @@ bool RepRap::WriteToolSettings(FileStore *f) const
 bool RepRap::WriteToolParameters(FileStore *f) const
 {
 	bool ok = true, written = false;
+	MutexLocker lock(toolListMutex);
 	for (const Tool *t = toolList; ok && t != nullptr; t = t->Next())
 	{
 		const AxesBitmap axesProbed = t->GetAxisOffsetsProbed();
diff --git a/src/RepRap.h b/src/RepRap.h
index dcdc62fe..e45b2cbe 100644
--- a/src/RepRap.h
+++ b/src/RepRap.h
@@ -23,6 +23,7 @@ Licence: GPL
 
 #include "RepRapFirmware.h"
 #include "MessageType.h"
+#include "RTOSIface.h"
 
 enum class ResponseSource
 {
@@ -141,6 +142,7 @@ private:
  	Display *display;
 #endif
 
+ 	Mutex toolListMutex, messageBoxMutex;
 	Tool* toolList;								// the tool list is sorted in order of increasing tool number
 	Tool* currentTool;
 	uint32_t lastWarningMillis;					// When we last sent a warning message for things that can happen very often
diff --git a/src/Storage/FileInfoParser.cpp b/src/Storage/FileInfoParser.cpp
index fcd78500..013caf1d 100644
--- a/src/Storage/FileInfoParser.cpp
+++ b/src/Storage/FileInfoParser.cpp
@@ -31,12 +31,12 @@ FileInfoParser::FileInfoParser()
 	: parseState(notParsing), fileBeingParsed(nullptr), accumulatedParseTime(0), accumulatedReadTime(0), accumulatedSeekTime(0), fileOverlapLength(0)
 {
 	parsedFileInfo.Init();
-	parserMutexHandle = RTOSIface::CreateMutex(parserMutexStorage);
+	parserMutex.Create();
 }
 
 bool FileInfoParser::GetFileInfo(const char *directory, const char *fileName, GCodeFileInfo& info, bool quitEarly)
 {
-	Locker lock(parserMutexHandle, MAX_FILEINFO_PROCESS_TIME);
+	MutexLocker lock(parserMutex, MAX_FILEINFO_PROCESS_TIME);
 	if (!lock)
 	{
 		return false;
diff --git a/src/Storage/FileInfoParser.h b/src/Storage/FileInfoParser.h
index 059d775c..49f3572e 100644
--- a/src/Storage/FileInfoParser.h
+++ b/src/Storage/FileInfoParser.h
@@ -68,8 +68,7 @@ private:
 	unsigned int FindFilamentUsed(const char* buf, size_t len, float *filamentUsed, size_t maxFilaments) const;
 
 	// We parse G-Code files in multiple stages. These variables hold the required information
-	MutexHandle parserMutexHandle;
-	MutexStorage parserMutexStorage;
+	Mutex parserMutex;
 
 	FileParseState parseState;
 	String<MaxFilenameLength> filenameBeingParsed;
diff --git a/src/Storage/MassStorage.cpp b/src/Storage/MassStorage.cpp
index 3026271d..1aa083ce 100644
--- a/src/Storage/MassStorage.cpp
+++ b/src/Storage/MassStorage.cpp
@@ -64,8 +64,8 @@ MassStorage::MassStorage(Platform* p) : freeWriteBuffers(nullptr)
 void MassStorage::Init()
 {
 	// Create the mutexes
-	fsMutexHandle = RTOSIface::CreateMutex(fsMutexStorage);
-	dirMutexHandle = RTOSIface::CreateMutex(dirMutexStorage);
+	fsMutex.Create();
+	dirMutex.Create();
 
 	for (size_t i = 0; i < NumFileWriteBuffers; ++i)
 	{
@@ -79,7 +79,7 @@ void MassStorage::Init()
 		inf.mounting = inf.isMounted = false;
 		inf.cdPin = SdCardDetectPins[card];
 		inf.cardState = CardDetectState::present;
-		inf.volMutexHandle = RTOSIface::CreateMutex(inf.volMutexStorage);
+		inf.volMutex.Create();
 	}
 
 	sd_mmc_init(SdWriteProtectPins, SdSpiCSPins);		// initialize SD MMC stack
@@ -101,7 +101,7 @@ void MassStorage::Init()
 
 FileWriteBuffer *MassStorage::AllocateWriteBuffer()
 {
-	Locker lock(fsMutexHandle);
+	MutexLocker lock(fsMutex);
 	if (freeWriteBuffers == nullptr)
 	{
 		return nullptr;
@@ -115,7 +115,7 @@ FileWriteBuffer *MassStorage::AllocateWriteBuffer()
 
 void MassStorage::ReleaseWriteBuffer(FileWriteBuffer *buffer)
 {
-	Locker lock(fsMutexHandle);
+	MutexLocker lock(fsMutex);
 	buffer->SetNext(freeWriteBuffers);
 	freeWriteBuffers = buffer;
 }
@@ -123,7 +123,7 @@ void MassStorage::ReleaseWriteBuffer(FileWriteBuffer *buffer)
 FileStore* MassStorage::OpenFile(const char* directory, const char* fileName, OpenMode mode)
 {
 	{
-		Locker lock(fsMutexHandle);
+		MutexLocker lock(fsMutex);
 		for (size_t i = 0; i < MAX_FILES; i++)
 		{
 			if (!files[i].inUse)
@@ -147,7 +147,7 @@ FileStore* MassStorage::OpenFile(const char* directory, const char* fileName, Op
 // Close all files
 void MassStorage::CloseAllFiles()
 {
-	Locker lock(fsMutexHandle);
+	MutexLocker lock(fsMutex);
 	for (FileStore& f : files)
 	{
 		while (f.inUse)
@@ -217,7 +217,7 @@ bool MassStorage::FindFirst(const char *directory, FileInfo &file_info)
 		loc[len - 1] = 0;
 	}
 
-	if (!RTOSIface::TakeMutex(dirMutexHandle, 10000))
+	if (!dirMutex.Take(10000))
 	{
 		return false;
 	}
@@ -250,7 +250,7 @@ bool MassStorage::FindFirst(const char *directory, FileInfo &file_info)
 		}
 	}
 
-	RTOSIface::ReleaseMutex(dirMutexHandle);
+	dirMutex.Release();
 	return false;
 }
 
@@ -258,7 +258,7 @@ bool MassStorage::FindFirst(const char *directory, FileInfo &file_info)
 // If it returns false then it also releases the mutex.
 bool MassStorage::FindNext(FileInfo &file_info)
 {
-	if (RTOSIface::GetMutexHolder(dirMutexHandle) != RTOSIface::GetCurrentTask())
+	if (dirMutex.GetHolder() != RTOSIface::GetCurrentTask())
 	{
 		return false;		// error, we don't hold the mutex
 	}
@@ -271,7 +271,7 @@ bool MassStorage::FindNext(FileInfo &file_info)
 	if (f_readdir(&findDir, &entry) != FR_OK || entry.fname[0] == 0)
 	{
 		//f_closedir(findDir);
-		RTOSIface::ReleaseMutex(dirMutexHandle);
+		dirMutex.Release();
 		return false;
 	}
 
@@ -290,9 +290,9 @@ bool MassStorage::FindNext(FileInfo &file_info)
 // Quit searching for files. Needed to avoid hanging on to the mutex.
 void MassStorage::AbandonFindNext()
 {
-	if (RTOSIface::GetMutexHolder(dirMutexHandle) == RTOSIface::GetCurrentTask())
+	if (dirMutex.GetHolder() == RTOSIface::GetCurrentTask())
 	{
-		RTOSIface::ReleaseMutex(dirMutexHandle);
+		dirMutex.Release();
 	}
 }
 
@@ -315,7 +315,7 @@ bool MassStorage::Delete(const char* directory, const char* fileName, bool silen
 
 	// Start new scope to lock the filesystem for the minimum time
 	{
-		Locker lock(fsMutexHandle);
+		MutexLocker lock(fsMutex);
 
 		// First check whether the file is open - don't allow it to be deleted if it is
 		FIL file;
@@ -461,8 +461,8 @@ GCodeResult MassStorage::Mount(size_t card, const StringRef& reply, bool reportS
 	}
 
 	SdCardInfo& inf = info[card];
-	Locker lock1(fsMutexHandle);
-	Locker lock2(inf.volMutexHandle);
+	MutexLocker lock1(fsMutex);
+	MutexLocker lock2(inf.volMutex);
 	if (!inf.mounting)
 	{
 		if (inf.isMounted)
@@ -568,7 +568,7 @@ bool MassStorage::CheckDriveMounted(const char* path)
 // Return true if any files are open on the file system
 bool MassStorage::AnyFileOpen(const FATFS *fs) const
 {
-	Locker lock(fsMutexHandle);
+	MutexLocker lock(fsMutex);
 	for (const FileStore & fil : files)
 	{
 		if (fil.IsOpenOn(fs))
@@ -583,7 +583,7 @@ bool MassStorage::AnyFileOpen(const FATFS *fs) const
 unsigned int MassStorage::InvalidateFiles(const FATFS *fs, bool doClose)
 {
 	unsigned int invalidated = 0;
-	Locker lock(fsMutexHandle);
+	MutexLocker lock(fsMutex);
 	for (FileStore & fil : files)
 	{
 		if (fil.Invalidate(fs, doClose))
@@ -618,8 +618,8 @@ bool MassStorage::IsCardDetected(size_t card) const
 bool MassStorage::InternalUnmount(size_t card, bool doClose)
 {
 	SdCardInfo& inf = info[card];
-	Locker lock1(fsMutexHandle);
-	Locker lock2(inf.volMutexHandle);
+	MutexLocker lock1(fsMutex);
+	MutexLocker lock2(inf.volMutex);
 	const bool invalidated = InvalidateFiles(&inf.fileSystem, doClose);
 	f_mount(card, nullptr);
 	memset(&inf.fileSystem, 0, sizeof(inf.fileSystem));
@@ -631,7 +631,7 @@ bool MassStorage::InternalUnmount(size_t card, bool doClose)
 unsigned int MassStorage::GetNumFreeFiles() const
 {
 	unsigned int numFreeFiles = 0;
-	Locker lock(fsMutexHandle);
+	MutexLocker lock(fsMutex);
 	for (const FileStore & fil : files)
 	{
 		if (!fil.inUse)
@@ -703,7 +703,7 @@ void MassStorage::Spin()
 
 	// Check if any files are supposed to be closed
 	{
-		Locker lock(fsMutexHandle);
+		MutexLocker lock(fsMutex);
 		for (FileStore & fil : files)
 		{
 			if (fil.closeRequested)
@@ -757,21 +757,21 @@ extern "C"
 	// Create a sync object. We already created it, we just need to copy the handle.
 	int ff_cre_syncobj (BYTE vol, _SYNC_t* psy)
 	{
-		*psy = reprap.GetPlatform().GetMassStorage()->GetVolumeMutexHandle(vol);
+		*psy = &reprap.GetPlatform().GetMassStorage()->GetVolumeMutex(vol);
 		return 1;
 	}
 
 	// Lock sync object
 	int ff_req_grant (_SYNC_t sy)
 	{
-		xSemaphoreTakeRecursive(sy, portMAX_DELAY);
+		sy->Take();
 		return 1;
 	}
 
 	// Unlock sync object
 	void ff_rel_grant (_SYNC_t sy)
 	{
-		xSemaphoreGiveRecursive(sy);
+		sy->Release();
 	}
 
 	// Delete a sync object
diff --git a/src/Storage/MassStorage.h b/src/Storage/MassStorage.h
index e73716f3..51d29c69 100644
--- a/src/Storage/MassStorage.h
+++ b/src/Storage/MassStorage.h
@@ -51,7 +51,7 @@ public:
 	void CloseAllFiles();
 	unsigned int GetNumFreeFiles() const;
 	void Spin();
-	MutexHandle GetVolumeMutexHandle(size_t vol) const { return info[vol].volMutexHandle; }
+	const Mutex& GetVolumeMutex(size_t vol) const { return info[vol].volMutex; }
 	bool GetFileInfo(const char *directory, const char *fileName, GCodeFileInfo& info, bool quitEarly) { return infoParser.GetFileInfo(directory, fileName, info, quitEarly); }
 
 	enum class InfoResult : uint8_t
@@ -87,8 +87,7 @@ private:
 		FATFS fileSystem;
 		uint32_t cdChangedTime;
 		uint32_t mountStartTime;
-		MutexHandle volMutexHandle;
-		MutexStorage volMutexStorage;
+		Mutex volMutex;
 		Pin cdPin;
 		bool mounting;
 		bool isMounted;
@@ -100,10 +99,7 @@ private:
 
 	SdCardInfo info[NumSdCards];
 
-	MutexHandle fsMutexHandle;
-	MutexHandle dirMutexHandle;
-	MutexStorage fsMutexStorage;
-	MutexStorage dirMutexStorage;
+	Mutex fsMutex, dirMutex;
 
 	FileInfoParser infoParser;
 	DIR findDir;
diff --git a/src/Tasks.cpp b/src/Tasks.cpp
index 6c072eae..a276196e 100644
--- a/src/Tasks.cpp
+++ b/src/Tasks.cpp
@@ -23,11 +23,9 @@ const uint32_t MainTaskStackSize = 2048;			// task stack size in dwords
 const uint32_t MainTaskPriority = 1;
 
 static StackType_t mainTaskStack[MainTaskStackSize];
-static StaticTask_t mainTaskBuffer;
-static TaskHandle_t mainTaskHandle;
+static Task mainTask;
 
-static MutexHandle spiMutexHandle;
-static MutexStorage spiMutexStorage;
+static Mutex spiMutex;
 
 extern "C" void MainTask(void * pvParameters);
 #endif
@@ -66,13 +64,13 @@ extern "C" void AppMain()
 
 #ifdef RTOS
 	// Create the startup task
-	mainTaskHandle = xTaskCreateStatic(MainTask, "MAIN", ARRAY_SIZE(mainTaskStack), nullptr, MainTaskPriority, mainTaskStack, &mainTaskBuffer);
+	mainTask.Create(MainTask, "MAIN", ARRAY_SIZE(mainTaskStack), nullptr, MainTaskPriority, mainTaskStack);
 	vTaskStartScheduler();			// doesn't return
 }
 
 extern "C" void MainTask(void *pvParameters)
 {
-	spiMutexHandle = RTOSIface::CreateMutex(spiMutexStorage);
+	spiMutex.Create();
 #endif
 	reprap.Init();
 	for (;;)
@@ -101,18 +99,22 @@ namespace Tasks
 		if (neverUsed != nullptr) { *neverUsed = stack_lwm - heapend; }
 	}
 
-	void TaskDiagnostics(MessageType mtype, TaskHandle_t ct)
+	static void TaskDiagnosticsFromHandle(MessageType mtype, TaskHandle_t ct)
 	{
 		TaskStatus_t taskDetails;
 		vTaskGetInfo(ct, &taskDetails, pdTRUE, eInvalid);
 		reprap.GetPlatform().MessageF(mtype, "Task %s: state %d stack rem %u\n", taskDetails.pcTaskName, (int)taskDetails.eCurrentState, (unsigned int)taskDetails.usStackHighWaterMark);
 	}
 
+	void TaskDiagnostics(MessageType mtype, const Task& ct)
+	{
+		TaskDiagnosticsFromHandle(mtype, ct.GetHandle());
+	}
+
 	// Write data about the current task
 	void CurrentTaskDiagnostics(MessageType mtype)
 	{
-		const TaskHandle_t ct = xTaskGetCurrentTaskHandle();
-		TaskDiagnostics(mtype, ct);
+		TaskDiagnosticsFromHandle(mtype, xTaskGetCurrentTaskHandle());
 	}
 
 #else
@@ -135,10 +137,10 @@ namespace Tasks
 
 #endif
 
-	MutexHandle GetSpiMutextHandle()
+	const Mutex *GetSpiMutex()
 	{
 #ifdef RTOS
-		return spiMutexHandle;
+		return &spiMutex;
 #else
 		return nullptr;
 #endif
diff --git a/src/Tasks.h b/src/Tasks.h
index 852149df..d529cfd3 100644
--- a/src/Tasks.h
+++ b/src/Tasks.h
@@ -16,12 +16,12 @@ namespace Tasks
 {
 #ifdef RTOS
 	void GetHandlerStackUsage(uint32_t* maxStack, uint32_t* neverUsed);
-	void TaskDiagnostics(MessageType mtype, TaskHandle_t ct);
+	void TaskDiagnostics(MessageType mtype, const Task& ct);
 	void CurrentTaskDiagnostics(MessageType mtype);
 #else
 	void GetStackUsage(uint32_t* currentStack, uint32_t* maxStack, uint32_t* neverUsed);
 #endif
-	MutexHandle GetSpiMutextHandle();
+	const Mutex *GetSpiMutex();
 }
 
 #endif /* SRC_TASKS_H_ */
diff --git a/src/Tools/Tool.cpp b/src/Tools/Tool.cpp
index c7fb4e53..581fd254 100644
--- a/src/Tools/Tool.cpp
+++ b/src/Tools/Tool.cpp
@@ -68,12 +68,16 @@ Tool * Tool::freelist = nullptr;
 	}
 
 	Tool *t;
-	if (freelist != nullptr)
 	{
+		CriticalSectionLocker lock;
 		t = freelist;
-		freelist = t->next;
+		if (t != nullptr)
+		{
+			freelist = t->next;
+		}
 	}
-	else
+
+	if (t == nullptr)
 	{
 		t = new Tool;
 	}
@@ -146,6 +150,8 @@ Tool * Tool::freelist = nullptr;
 		delete t->name;
 		t->name = nullptr;
 		t->filament = nullptr;
+
+		CriticalSectionLocker lock;
 		t->next = freelist;
 		freelist = t;
 	}
diff --git a/src/Version.h b/src/Version.h
index 9165a6e9..1ee1c97c 100644
--- a/src/Version.h
+++ b/src/Version.h
@@ -19,7 +19,7 @@
 #endif
 
 #ifndef DATE
-# define DATE "2018-04-01b1"
+# define DATE "2018-04-01b2"
 #endif
 
 #define AUTHORS "reprappro, dc42, chrishamm, t3p3, dnewman"