1 files changed, 44 insertions, 0 deletions
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