add printk

2 files changed, 56 insertions, 0 deletions

diff --git a/armv7m.c b/armv7m.c
index 60b243a..ba55642 100644
--- a/armv7m.c
+++ b/armv7m.c
@@ -46,6 +46,58 @@ void putdec(uint32_t v)
     if(!out) putc('0');
 }
 
+void printk(const char *fmt, ...)
+{
+    va_list args;
+    va_start(args, fmt);
+    vprintk(fmt, args);
+    va_end(args);
+}
+
+void vprintk(const char *fmt, va_list args)
+{
+    while(1) {
+        char c=*fmt++;
+        if(c=='\0') {
+            return;
+
+        } else if(c!='%') {
+            putc(c);
+
+        } else {
+specmod:
+            c = *fmt++;
+            
+            switch(c) {
+            case '\0': return;
+            case '%': putc(c); break;
+            case '0'...'9':
+            case ' ':
+                /* ignore modifiers */
+                goto specmod;
+            case 'u': {
+                unsigned v = va_arg(args, unsigned);
+                putdec(v);
+                break;
+            }
+            case 'x': {
+                unsigned v = va_arg(args, unsigned);
+                puthex(v);
+                break;
+            }
+            case 's': {
+                const char *v = va_arg(args, const char*);
+                puts(v);
+                break;
+            }
+            default:
+                putc('!');
+                break;
+            }
+        }
+    }
+}
+
 unsigned log2_ceil(uint32_t v)
 {
     unsigned r=0, c=0;
diff --git a/armv7m.h b/armv7m.h
index c357ae0..29d481e 100644
--- a/armv7m.h
+++ b/armv7m.h
@@ -2,6 +2,7 @@
 #define ARMV7m_h
 
 #include <stdint.h>
+#include <stdarg.h>
 
 #define UART(N) ((N)+(void*)0x4000c000)
 #define UART_DATA UART(0)
@@ -92,6 +93,9 @@ void putc(char c);
 
 void puts(const char *s);
 
+void printk(const char *fmt, ...) __attribute__((format(__printf__,1,2)));
+void vprintk(const char *fmt, va_list) __attribute__((format(__printf__,1,0)));
+
 void flush(void);
 
 extern const char hexchars[16];