diff --git a/src/platforms/native/platform.c b/src/platforms/native/platform.c
index 338cd85..1804e17 100644
--- a/src/platforms/native/platform.c
+++ b/src/platforms/native/platform.c
@@ -304,57 +304,3 @@ static void setup_vbus_irq(void)
 
 	exti15_10_isr();
 }
-
-#ifdef ENABLE_DEBUG
-enum {
-	RDI_SYS_OPEN = 0x01,
-	RDI_SYS_WRITE = 0x05,
-	RDI_SYS_ISTTY = 0x09,
-};
-
-int rdi_write(int fn, const char *buf, size_t len)
-{
-	(void)fn;
-	if (debug_bmp)
-		return len - usbuart_debug_write(buf, len);
-
-	return 0;
-}
-
-struct ex_frame {
-	union {
-		int syscall;
-		int retval;
-	};
-	const int *params;
-	uint32_t r2, r3, r12, lr, pc;
-};
-
-void debug_monitor_handler_c(struct ex_frame *sp)
-{
-	/* Return to after breakpoint instruction */
-	sp->pc += 2;
-
-	switch (sp->syscall) {
-	case RDI_SYS_OPEN:
-		sp->retval = 1;
-		break;
-	case RDI_SYS_WRITE:
-		sp->retval = rdi_write(sp->params[0], (void*)sp->params[1], sp->params[2]);
-		break;
-	case RDI_SYS_ISTTY:
-		sp->retval = 1;
-		break;
-	default:
-		sp->retval = -1;
-	}
-
-}
-
-asm(".globl debug_monitor_handler\n"
-    ".thumb_func\n"
-    "debug_monitor_handler: \n"
-    "    mov r0, sp\n"
-    "    b debug_monitor_handler_c\n");
-
-#endif
diff --git a/src/platforms/stlink/Makefile.inc b/src/platforms/stlink/Makefile.inc
index 874695f..18486e8 100644
--- a/src/platforms/stlink/Makefile.inc
+++ b/src/platforms/stlink/Makefile.inc
@@ -8,11 +8,17 @@ CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-I platforms/stm32
 LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs \
 	-lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
-	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc -lnosys \
+	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc \
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m3 -Wl,-gc-sections \
 	-L../libopencm3/lib
 LDFLAGS = $(LDFLAGS_BOOT) -Wl,-Ttext=0x8002000
 
+ifeq ($(ENABLE_DEBUG), 1)
+LDFLAGS += --specs=rdimon.specs
+else
+LDFLAGS += --specs=nosys.specs
+endif
+
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
diff --git a/src/platforms/stlink/platform.c b/src/platforms/stlink/platform.c
index 5177b1c..5e2ca3b 100644
--- a/src/platforms/stlink/platform.c
+++ b/src/platforms/stlink/platform.c
@@ -28,6 +28,7 @@
 
 #include <libopencm3/stm32/rcc.h>
 #include <libopencm3/cm3/scb.h>
+#include <libopencm3/cm3/scs.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/usart.h>
 #include <libopencm3/usb/usbd.h>
@@ -47,6 +48,11 @@ int platform_hwversion(void)
 void platform_init(void)
 {
 	rev = detect_rev();
+	SCS_DEMCR |= SCS_DEMCR_VC_MON_EN;
+#ifdef ENABLE_DEBUG
+	void initialise_monitor_handles(void);
+	initialise_monitor_handles();
+#endif
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
 	if (rev == 0) {
 		led_idle_run = GPIO8;
@@ -77,7 +83,9 @@ void platform_init(void)
 	if (rev > 1) /* Reconnect USB */
 		gpio_set(GPIOA, GPIO15);
 	cdcacm_init();
-	usbuart_init();
+	/* Don't enable UART if we're being debugged. */
+	if (!(SCS_DEMCR & SCS_DEMCR_TRCENA))
+		usbuart_init();
 }
 
 void platform_srst_set_val(bool assert)
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index 26ee153..f531444 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -33,6 +33,11 @@
 #include <libopencm3/stm32/f1/memorymap.h>
 #include <libopencm3/usb/usbd.h>
 
+#ifdef ENABLE_DEBUG
+# define PLATFORM_HAS_DEBUG
+# define USBUART_DEBUG
+#endif
+
 #define BOARD_IDENT       "Black Magic Probe (STLINK), (Firmware " FIRMWARE_VERSION ")"
 #define BOARD_IDENT_DFU   "Black Magic (Upgrade) for STLink/Discovery, (Firmware " FIRMWARE_VERSION ")"
 #define BOARD_IDENT_UPD   "Black Magic (DFU Upgrade) for STLink/Discovery, (Firmware " FIRMWARE_VERSION ")"
@@ -102,7 +107,13 @@
 #define USBUSART_TIM_IRQ NVIC_TIM4_IRQ
 #define USBUSART_TIM_ISR tim4_isr
 
-#define DEBUG(...)
+#ifdef ENABLE_DEBUG
+extern bool debug_bmp;
+int usbuart_debug_write(const char *buf, size_t len);
+# define DEBUG printf
+#else
+# define DEBUG(...)
+#endif
 
 extern uint16_t led_idle_run;
 #define LED_IDLE_RUN            led_idle_run
diff --git a/src/platforms/stm32/usbuart.c b/src/platforms/stm32/usbuart.c
index ed74d36..a4a772e 100644
--- a/src/platforms/stm32/usbuart.c
+++ b/src/platforms/stm32/usbuart.c
@@ -250,3 +250,57 @@ void USBUSART_TIM_ISR(void)
 	/* process FIFO */
 	usbuart_run();
 }
+
+#ifdef ENABLE_DEBUG
+enum {
+	RDI_SYS_OPEN = 0x01,
+	RDI_SYS_WRITE = 0x05,
+	RDI_SYS_ISTTY = 0x09,
+};
+
+int rdi_write(int fn, const char *buf, size_t len)
+{
+	(void)fn;
+	if (debug_bmp)
+		return len - usbuart_debug_write(buf, len);
+
+	return 0;
+}
+
+struct ex_frame {
+	union {
+		int syscall;
+		int retval;
+	};
+	const int *params;
+	uint32_t r2, r3, r12, lr, pc;
+};
+
+void debug_monitor_handler_c(struct ex_frame *sp)
+{
+	/* Return to after breakpoint instruction */
+	sp->pc += 2;
+
+	switch (sp->syscall) {
+	case RDI_SYS_OPEN:
+		sp->retval = 1;
+		break;
+	case RDI_SYS_WRITE:
+		sp->retval = rdi_write(sp->params[0], (void*)sp->params[1], sp->params[2]);
+		break;
+	case RDI_SYS_ISTTY:
+		sp->retval = 1;
+		break;
+	default:
+		sp->retval = -1;
+	}
+
+}
+
+asm(".globl debug_monitor_handler\n"
+    ".thumb_func\n"
+    "debug_monitor_handler: \n"
+    "    mov r0, sp\n"
+    "    b debug_monitor_handler_c\n");
+
+#endif
diff --git a/src/platforms/swlink/Makefile.inc b/src/platforms/swlink/Makefile.inc
index 1a979c5..aeee95a 100644
--- a/src/platforms/swlink/Makefile.inc
+++ b/src/platforms/swlink/Makefile.inc
@@ -8,11 +8,17 @@ CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-I platforms/stm32
 LDFLAGS_BOOT := $(LDFLAGS) --specs=nano.specs \
 	-lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
-	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc -lnosys \
+	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc\
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m3 -Wl,-gc-sections \
 	-L../libopencm3/lib
 LDFLAGS = $(LDFLAGS_BOOT) -Wl,-Ttext=0x8002000
 
+ifeq ($(ENABLE_DEBUG), 1)
+LDFLAGS += --specs=rdimon.specs
+else
+LDFLAGS += --specs=nosys.specs
+endif
+
 VPATH += platforms/stm32
 
 SRC += 	cdcacm.c	\
diff --git a/src/platforms/swlink/platform.c b/src/platforms/swlink/platform.c
index aace5a1..651ab9b 100644
--- a/src/platforms/swlink/platform.c
+++ b/src/platforms/swlink/platform.c
@@ -28,6 +28,7 @@
 
 #include <libopencm3/stm32/f1/rcc.h>
 #include <libopencm3/cm3/scb.h>
+#include <libopencm3/cm3/scs.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/usart.h>
 #include <libopencm3/usb/usbd.h>
@@ -36,6 +37,11 @@
 void platform_init(void)
 {
 	uint32_t data;
+	SCS_DEMCR |= SCS_DEMCR_VC_MON_EN;
+#ifdef ENABLE_DEBUG
+	void initialise_monitor_handles(void);
+	initialise_monitor_handles();
+#endif
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
 
 	/* Enable peripherals */
@@ -83,6 +89,9 @@ void platform_init(void)
 
 	platform_timing_init();
 	cdcacm_init();
+	/* Don't enable UART if we're being debugged. */
+	if (!(SCS_DEMCR & SCS_DEMCR_TRCENA))
+		usbuart_init();
 	usbuart_init();
 }
 
diff --git a/src/platforms/swlink/platform.h b/src/platforms/swlink/platform.h
index eef60b6..8c2be74 100644
--- a/src/platforms/swlink/platform.h
+++ b/src/platforms/swlink/platform.h
@@ -106,7 +106,15 @@
 #define TRACE_IC_IN TIM_IC_IN_TI2
 #define TRACE_TRIG_IN TIM_SMCR_TS_IT1FP2
 
-#define DEBUG(...)
+#ifdef ENABLE_DEBUG
+# define PLATFORM_HAS_DEBUG
+# define USBUART_DEBUG
+extern bool debug_bmp;
+int usbuart_debug_write(const char *buf, size_t len);
+# define DEBUG printf
+#else
+# define DEBUG(...)
+#endif
 
 #define SET_RUN_STATE(state)	{running_status = (state);}
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, LED_IDLE_RUN, state);}