diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8d770c4..e2add76 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,6 +12,13 @@
 # Globally enable some compiler sanity
 add_compile_options(-fwrapv -fno-strict-aliasing -fsigned-char)
 
+# Enable tracing if wanted, on by default
+option(ENABLE_TRACING "Enable trace logging for debugging" ON)
+if(ENABLE_TRACING)
+	add_compile_options(-DMBED_CONF_MBED_TRACE_ENABLE=1)
+	add_compile_options(-DMBED_TRACE_MAX_LEVEL=TRACE_LEVEL_DEBUG)
+endif()
+
 # Create Tardis project
 project(Tardis)
 file(GLOB SRC_FILES "${PROJECT_SOURCE_DIR}/src/*.cpp")
diff --git a/include/Filesystem.h b/include/Filesystem.h
index 9b5ec30..4bfc6e0 100644
--- a/include/Filesystem.h
+++ b/include/Filesystem.h
@@ -9,7 +9,7 @@
 #include "FileSystem.h"
 #include "mbed.h"
 
-void mountFilesystem(mbed::BlockDevice &bd, mbed::FileSystem &fs);
-void unmountFilesystem(mbed::BlockDevice &bd, mbed::FileSystem &fs);
+int mountFilesystem(mbed::BlockDevice &bd, mbed::FileSystem &fs);
+int unmountFilesystem(mbed::BlockDevice &bd, mbed::FileSystem &fs);
 
 #endif
diff --git a/include/FlashErase.h b/include/FlashErase.h
index 250c777..83aad9d 100644
--- a/include/FlashErase.h
+++ b/include/FlashErase.h
@@ -8,6 +8,6 @@
 
 #include "MainBD.h"
 
-void doErase(mbed::BlockDevice &bd);
+int doErase(mbed::BlockDevice &bd);
 
 #endif
diff --git a/src/Filesystem.cpp b/src/Filesystem.cpp
index a580a81..0128272 100644
--- a/src/Filesystem.cpp
+++ b/src/Filesystem.cpp
@@ -9,47 +9,55 @@
 #include <errno.h>
 #include <functional>
 
+#include "mbed-trace/mbed_trace.h"
+#define TRACE_GROUP "FS"
+
 // Maximum number of elements in buffer
 #define BUFFER_MAX_LEN 10
 #define FORCE_REFORMAT false
 
 // function to attempt to mount the filesystem and
 // reformat and mount it again should it fail
-void mountFilesystem(mbed::BlockDevice &bd, mbed::FileSystem &fs) {
+int mountFilesystem(mbed::BlockDevice &bd, mbed::FileSystem &fs) {
 	int err;
 	// Try to mount the filesystem
-	printf("Mounting the filesystem... ");
+	tr_info("Mounting the filesystem... ");
 	fflush(stdout);
 	err = fs.mount(&bd);
-	printf("%s\n", (err ? "Fail :(" : "OK"));
+	tr_info("%s", (err ? "Fail :(" : "OK"));
 	if (err || FORCE_REFORMAT) {
 		// Reformat if we can't mount the filesystem
-		printf("formatting... ");
+		tr_info("formatting... ");
 		fflush(stdout);
 		err = fs.reformat(&bd);
-		printf("%s\n", (err ? "Fail :(" : "OK"));
+		tr_info("%s", (err ? "Fail :(" : "OK"));
 		if (err) {
-			error("error: %s (%d)\n", strerror(-err), err);
+			tr_err("error: %s (%d)", strerror(-err), err);
+			return err;
 		}
 		// Try to mount the filesystem again
-		printf("Mounting the filesystem... ");
+		tr_info("Mounting the filesystem... ");
 		fflush(stdout);
 		err = fs.mount(&bd);
-		printf("%s\n", (err ? "Fail :(" : "OK"));
+		tr_info("%s", (err ? "Fail :(" : "OK"));
 		if (err) {
-			error("error: %s (%d)\n", strerror(-err), err);
+			tr_err("error: %s (%d)", strerror(-err), err);
+			return err;
 		}
 	}
+	return 0;
 }
 
 // function to unmount the filesystem
-void unmountFilesystem(mbed::BlockDevice &bd, mbed::FileSystem &fs) {
+int unmountFilesystem(mbed::BlockDevice &bd, mbed::FileSystem &fs) {
 	// Try to unmount the filesystem
-	printf("Unmounting the filesystem... ");
+	tr_info("Unmounting the filesystem... ");
 	fflush(stdout);
 	int err = fs.unmount();
-	printf("%s\n", (err ? "Fail :(" : "OK"));
+	tr_info("%s", (err ? "Fail :(" : "OK"));
 	if (err) {
-		error("error: %s (%d)\n", strerror(-err), err);
+		tr_err("error: %s (%d)", strerror(-err), err);
+		return err;
 	}
+	return 0;
 }
diff --git a/src/FlashErase.cpp b/src/FlashErase.cpp
index a84f71d..9042514 100644
--- a/src/FlashErase.cpp
+++ b/src/FlashErase.cpp
@@ -8,29 +8,36 @@
 #include <errno.h>
 #include <functional>
 
+#include "mbed-trace/mbed_trace.h"
+#define TRACE_GROUP "FLER"
+
 // function to erase the flash
-void doErase(mbed::BlockDevice &bd) {
-	printf("Initializing the block device... ");
+int doErase(mbed::BlockDevice &bd) {
+	tr_info("Initializing the block device... ");
 	fflush(stdout);
 	int err = bd.init();
-	printf("%s\n", (err ? "Fail :(" : "OK"));
+	tr_info("%s", (err ? "Fail :(" : "OK"));
 	if (err) {
-		error("error: %s (%d)\n", strerror(-err), err);
+		tr_err("error: %s (%d)", strerror(-err), err);
+		return err;
 	}
 
-	printf("Erasing the block device... ");
+	tr_info("Erasing the block device... ");
 	fflush(stdout);
 	err = bd.erase(0, bd.size());
-	printf("%s\n", (err ? "Fail :(" : "OK"));
+	tr_info("%s", (err ? "Fail :(" : "OK"));
 	if (err) {
-		error("error: %s (%d)\n", strerror(-err), err);
+		tr_err("error: %s (%d)", strerror(-err), err);
+		return err;
 	}
 
-	printf("Deinitializing the block device... ");
+	tr_info("Deinitializing the block device... ");
 	fflush(stdout);
 	err = bd.deinit();
-	printf("%s\n", (err ? "Fail :(" : "OK"));
+	tr_info("%s", (err ? "Fail :(" : "OK"));
 	if (err) {
-		error("error: %s (%d)\n", strerror(-err), err);
+		tr_err("error: %s (%d)", strerror(-err), err);
+		return err;
 	}
+	return 0;
 }
diff --git a/src/MyUSBMSD.cpp b/src/MyUSBMSD.cpp
index 08d6bbc..3015a93 100644
--- a/src/MyUSBMSD.cpp
+++ b/src/MyUSBMSD.cpp
@@ -7,6 +7,9 @@
 #include "ButtonThread.h"
 #include "mbed.h"
 
+#include "mbed-trace/mbed_trace.h"
+#define TRACE_GROUP "USBM"
+
 // create a custom usb mass storage subclass to override
 // the product description
 
@@ -26,17 +29,16 @@
 }
 
 void doUSBMSD(mbed::BlockDevice &bd) {
-	printf("Switching to the usb mass storage mode...\n");
+	tr_info("Switching to the usb mass storage mode...");
 	MyUSBMSD usb(&bd, true, 0x1209, 0x10);
 
 	while (true) {
 		int presses = waitForPresses(0s);
 		usb.process();
 		if (presses == 1) {
-			printf("Disconnecting the usb mass storage "
-			       "device...\n");
+			tr_info("Disconnecting the usb mass storage device...");
 			usb.disconnect();
-			printf("Switched usb mass storage mode off.\n");
+			tr_info("Switched usb mass storage mode off.");
 			break;
 		}
 	}
diff --git a/src/main.cpp b/src/main.cpp
index 742b0d4..bd46c6f 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -10,10 +10,40 @@
 #include "MainBD.h"
 #include "MainFilesystem.h"
 #include "MyUSBMSD.h"
+#include "mbed-trace/mbed_trace.h"
 #include "mbed.h"
+#define TRACE_GROUP "APP"
+
+#if MBED_CONF_MBED_TRACE_ENABLE
+// mutex setup for tracing in thread safe mode
+
+static Mutex traceMutex;
+
+static void trace_mutex_wait() { traceMutex.lock(); }
+
+static void trace_mutex_release() { traceMutex.unlock(); }
+
+void setup_tracing(void) {
+	char trace_filters[] = "APP,FS,FLER,SNTP,USBM";
+	mbed_trace_mutex_wait_function_set(trace_mutex_wait);
+	mbed_trace_mutex_release_function_set(trace_mutex_release);
+	mbed_trace_init();
+	mbed_trace_include_filters_set(trace_filters);
+	tr_debug("Debug tracing enabled");
+	tr_info("Info tracing enabled");
+	tr_warn("Warning tracing enabled");
+	tr_err("Error tracing enabled");
+}
+#else
+void setup_tracing(void) {
+	// do nothing
+}
+#endif
 
 int main() {
-	printf("Project Tardis\n");
+	setup_tracing();
+
+	tr_info("Project Tardis");
 
 	mountFilesystem(mainBD, mainFS);