From 137489b6cd7e03031b0acb5d3abab4603decde04 Mon Sep 17 00:00:00 2001
From: Trygve Laugstøl <trygvis@inamo.no>
Date: Sun, 20 Dec 2015 15:10:55 +0100
Subject: o Adding serial1 for testing USART.

---
 debug.cpp | 44 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 44 insertions(+)
 create mode 100644 debug.cpp

(limited to 'debug.cpp')

diff --git a/debug.cpp b/debug.cpp
new file mode 100644
index 0000000..d44fc79
--- /dev/null
+++ b/debug.cpp
@@ -0,0 +1,44 @@
+#include "debug.h"
+#include "printf.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+static void append_char(void *v, char c) {
+    *(*((char **) v))++ = c;
+}
+
+void dbg_printf(const char *fmt, ...) {
+    char msg[100];
+    msg[0] = 0;
+
+    va_list va;
+    va_start(va, fmt);
+    char *a = msg;
+    char **b = &a;
+    tfp_format(b, append_char, fmt, va);
+    va_end(va);
+    append_char(b, '\0');
+
+    send_command(SYS_WRITE0, msg);
+}
+
+void dbg_putc(void *, char c) {
+    char cc = c;
+    send_command(SYS_WRITEC, &cc);
+}
+
+void send_command(enum SemihostingCmd command, void *message) {
+    int c = command;
+
+    __asm volatile (
+    "mov r0, %[cmd];"
+        "mov r1, %[msg];"
+        "bkpt #0xAB" : : [cmd] "r"(c), [msg] "r"(message) : "r0", "r1", "memory"
+    );
+}
+
+#ifdef __cplusplus
+};
+#endif
-- 
cgit v1.2.3