From 966f360515e387fb79223a75986aa7b79878268a Mon Sep 17 00:00:00 2001
From: Gareth McMullin <gareth@blacksphere.co.nz>
Date: Sun, 25 Sep 2016 15:55:30 -0700
Subject: [PATCH] debug: Use semihosting if available or output via usbuart.

---
 src/exception.c                   |  1 +
 src/platforms/native/Makefile.inc |  8 +++-
 src/platforms/native/platform.c   | 61 +++++++++++++++++++++++++++++++
 src/platforms/native/platform.h   |  4 +-
 src/platforms/stm32/usbuart.c     | 39 ++++----------------
 5 files changed, 78 insertions(+), 35 deletions(-)

diff --git a/src/exception.c b/src/exception.c
index 3d43f99d..0242cec4 100644
--- a/src/exception.c
+++ b/src/exception.c
@@ -26,6 +26,7 @@ struct exception *innermost_exception;
 void raise_exception(uint32_t type, const char *msg)
 {
 	struct exception *e;
+	DEBUG("Exception: %s\n", msg);
 	for (e = innermost_exception; e; e = e->outer) {
 		if (e->mask & type) {
 			e->type = type;
diff --git a/src/platforms/native/Makefile.inc b/src/platforms/native/Makefile.inc
index ba08e712..63b24ded 100644
--- a/src/platforms/native/Makefile.inc
+++ b/src/platforms/native/Makefile.inc
@@ -7,11 +7,17 @@ CFLAGS += -Istm32/include -mcpu=cortex-m3 -mthumb \
 	-Iplatforms/stm32
 
 LDFLAGS_BOOT := $(LDFLAGS) -lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
-	-Wl,-T,platforms/stm32/blackmagic.ld -nostartfiles -lc -lnosys \
+	-Wl,-T,platforms/stm32/blackmagic.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/native/platform.c b/src/platforms/native/platform.c
index c8c3edf4..8e0cde38 100644
--- a/src/platforms/native/platform.c
+++ b/src/platforms/native/platform.c
@@ -29,6 +29,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/exti.h>
 #include <libopencm3/stm32/usart.h>
@@ -89,6 +90,12 @@ int platform_hwversion(void)
 
 void platform_init(void)
 {
+	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 */
@@ -287,3 +294,57 @@ 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/native/platform.h b/src/platforms/native/platform.h
index 282b7671..9062e27d 100644
--- a/src/platforms/native/platform.h
+++ b/src/platforms/native/platform.h
@@ -149,9 +149,9 @@
 
 #ifdef ENABLE_DEBUG
 extern bool debug_bmp;
-void usbuart_debug_outf(const char *fmt, ...);
+int usbuart_debug_write(const char *buf, size_t len);
 
-#define DEBUG(...) if (debug_bmp) {usbuart_debug_outf("bmp: ");usbuart_debug_outf(__VA_ARGS__);}
+#define DEBUG printf
 #else
 #define DEBUG(...)
 #endif
diff --git a/src/platforms/stm32/usbuart.c b/src/platforms/stm32/usbuart.c
index 17d02c17..77c959b1 100644
--- a/src/platforms/stm32/usbuart.c
+++ b/src/platforms/stm32/usbuart.c
@@ -193,44 +193,19 @@ void usbuart_usb_out_cb(usbd_device *dev, uint8_t ep)
 }
 
 #ifdef USBUART_DEBUG
-#include <stdarg.h>
-
-/* Function to output debug data to usbuart port (ttyACM1 on linux) */
-void usbuart_debug_outf(const char *fmt, ...)
+int usbuart_debug_write(const char *buf, size_t len)
 {
-	va_list ap;
-	char *buf, *tmp;
-
-	va_start(ap, fmt);
-	if (vasprintf(&buf, fmt, ap) < 0)
-		return;
-	tmp = buf;
-	while( *tmp != 0 )
-	{
-		if( *tmp == '\n' && *(tmp-1) != '\r' )
-		{
-			/* insert into FIFO */
+	for (size_t i = 0; i < len; i++) {
+		if (buf[i] == '\n') {
 			buf_rx[buf_rx_in++] = '\r';
-
-			/* wrap out pointer */
-			if (buf_rx_in >= FIFO_SIZE)
-			{
-				buf_rx_in = 0;
-			}
-		}
-		/* insert into FIFO */
-		buf_rx[buf_rx_in++] = *(tmp++);
-
-		/* wrap out pointer */
-		if (buf_rx_in >= FIFO_SIZE)
-		{
-			buf_rx_in = 0;
+			buf_rx_in %= FIFO_SIZE;
 		}
+		buf_rx[buf_rx_in++] = buf[i];
+		buf_rx_in %= FIFO_SIZE;
 	}
 	/* enable deferred processing if we put data in the FIFO */
 	timer_enable_irq(USBUSART_TIM, TIM_DIER_UIE);
-	free(buf);
-	va_end(ap);
+	return len;
 }
 #endif