+#include <stdarg.h>
+#include <stdint.h>
+#include <string.h>
+
+void semi_put_char(char c)
+{
+ // This is tricky, we need to work around GCC bugs
+ volatile char cc = c;
+ asm volatile (
+ "mov r0, #0x03\n" /* SYS_WRITEC */
+ "mov r1, %[msg]\n"
+ "bkpt #0xAB\n"
+ :
+ : [msg] "r" (&cc)
+ : "r0", "r1"
+ );
+}
+
+void semi_write_string(char *c)
+{
+ asm volatile (
+ "mov r0, #0x04\n" /* SYS_WRITE0 */
+ "mov r1, %[msg]\n"
+ "bkpt #0xAB\n"
+ :
+ : [msg] "r" (c)
+ : "r0", "r1"
+ );
+}
+
+void debug_putc(int c)
+{
+ static char debug_buf[16];
+ static int debug_i;
+ debug_buf[debug_i++] = c;
+ if (c == '\n' || debug_i >= sizeof(debug_buf) - 1)
+ {
+ debug_buf[debug_i] = 0;
+ semi_write_string(debug_buf);
+ debug_i = 0;
+ }
+}
+
+void debug_puts(const char *s)
+{
+ while (*s)
+ debug_putc(*s++);
+}
+
+enum printf_flags {
+ PF_ZERO_PAD = 1,
+ PF_SIGNED = 2,
+ PF_NEGATIVE = 4,
+ PF_UPPERCASE = 8,
+ PF_LEFT = 16,
+};
+
+static void printf_string(const char *s, uint width, uint flags)
+{
+ uint len = strlen(s);
+ uint pad = (len < width) ? width - len : 0;
+ char pad_char = (flags & PF_ZERO_PAD) ? '0' : ' ';
+
+ if (flags & PF_LEFT)
+ debug_puts(s);
+ while (pad--)
+ debug_putc(pad_char);
+ if (!(flags & PF_LEFT))
+ debug_puts(s);
+}
+
+static void printf_number(uint i, uint width, uint flags, uint base)
+{
+ char buf[16];
+ char *w = buf + sizeof(buf);
+
+ if (flags & PF_SIGNED)
+ {
+ if ((int) i < 0)
+ {
+ i = - (int) i;
+ flags |= PF_NEGATIVE;
+ }
+ }
+
+ *--w = 0;
+ do
+ {
+ uint digit = i % base;
+ if (digit < 10)
+ *--w = '0' + digit;
+ else
+ *--w = ((flags & PF_UPPERCASE) ? 'A' : 'a') + digit - 10;
+ i /= base;
+ }
+ while (i);
+
+ if (flags & PF_NEGATIVE)
+ *--w = '-';
+
+ printf_string(w, width, flags);
+}
+
+void debug_printf(const char *fmt, ...)
+{
+ va_list args;
+ va_start(args, fmt);
+
+ while (*fmt)
+ {
+ int c = *fmt++;
+ if (c != '%')
+ {
+ debug_putc(c);
+ continue;
+ }
+
+ uint width = 0;
+ uint flags = 0;
+
+ if (*fmt == '-')
+ {
+ fmt++;
+ flags |= PF_LEFT;
+ }
+
+ if (*fmt == '0')
+ {
+ fmt++;
+ flags |= PF_ZERO_PAD;
+ }
+
+ while (*fmt >= '0' && *fmt <= '9')
+ width = 10*width + *fmt++ - '0';
+
+ c = *fmt++;
+ switch (c)
+ {
+ case 'd':
+ printf_number(va_arg(args, int), width, flags | PF_SIGNED, 10);
+ break;
+ case 'u':
+ printf_number(va_arg(args, int), width, flags, 10);
+ break;
+ case 'X':
+ flags |= PF_UPPERCASE;
+ // fall-thru
+ case 'x':
+ printf_number(va_arg(args, int), width, flags, 16);
+ break;
+ case 's':
+ printf_string(va_arg(args, char *), width, flags);
+ break;
+ default:
+ debug_putc(c);
+ continue;
+ }
+ }
+
+ va_end(args);
+}
+