1 files changed, 138 insertions, 0 deletions
diff --git a/lib/printf.c b/lib/printf.c
new file mode 100644
index 0000000..4c26fe4
--- /dev/null
+++ b/lib/printf.c
@@ -0,0 +1,138 @@
+#include "printf.h"
+#include "string.h"
+#include "init.h"
+#include "hal/obj.h"
+#include <stdint.h>
+
+int kprintf(const char *fmt, ...)
+{
+    va_list ap;
+    va_start(ap, fmt);
+    int r = kvprintf(fmt, ap);
+    va_end(ap);
+    return r;
+}
+
+#define SPRINTF_PUTC(str, size, i, c) \
+    if(str && i < size) str[i] = (c); \
+    i++
+
+static int
+convert_as_int(
+        char *str,
+        size_t size,
+        int stri,
+        int field_width,
+        uintmax_t value,
+        int radix,
+        int unsign)
+{
+    unsigned digits = 0;
+    int ostri = stri;
+    if((intmax_t)value < 0 && !unsign) {
+        SPRINTF_PUTC(str, size, stri, '-');
+        value = 0 - (intmax_t)value;
+    }
+
+    for(uintmax_t i = value; i != 0; i /= radix) digits++;
+    if(value == 0) digits++;
+
+    if(field_width < digits) field_width = digits;
+    for(uintmax_t i = value, digiti = 0; digiti < digits; i /= radix) {
+        char i_mod = (char)(i % radix);
+        int stri_digiti = stri + (field_width - digiti) - 1;
+
+        SPRINTF_PUTC(str, size, stri_digiti,
+                (i_mod > 9 ? (i_mod + 'a' - 10) : (i_mod + '0')));
+        digiti++;
+    }
+
+    return field_width + (stri - ostri) + 1;
+}
+
+int 
+vsnprintf(
+        char *str, size_t size, 
+        const char *fmt, va_list ap)
+{
+    int stri = 0;
+    for(const char *fmtc = fmt; *fmtc; fmtc++) {
+        if(*fmtc != '%') {
+            SPRINTF_PUTC(str, size, stri, *fmtc);
+            continue;
+        }
+        fmtc++;
+
+        //TODO: other format specifiers.
+        int field_width = 0;
+        char field_pad = ' ';
+
+        int lenspec = 4;
+        
+        int radix = 10;
+        int unsign = 0;
+        int caps = 0;
+        switch(*fmtc) {
+            case 's':
+                goto string_conversion;
+            case 'b':
+                radix = 2;
+                goto int_conversion;
+                break;
+            case 'o':
+                radix = 8;
+                goto int_conversion;
+                break;
+            case 'p':
+                lenspec = sizeof(uintptr_t);
+                field_width = lenspec * 2;
+                field_pad = '0';
+            case 'X':
+                caps = 1;
+            case 'x':
+                unsign = 1;
+                radix = 16;
+                goto int_conversion;
+                break;
+            case 'u':
+                unsign = 1;
+            case 'd':
+            case 'i':
+                goto int_conversion;
+                break;
+        }
+string_conversion:
+        {
+            const char *strarg = va_arg(ap, const char*);
+            for(const char *strargc = strarg; *strargc; strargc++) {
+                SPRINTF_PUTC(str, size, stri, *strargc);
+            }
+        } continue;
+int_conversion: 
+        {
+            uintmax_t value = 0;
+            switch(lenspec) {
+                case 1: value = (uintmax_t)(int8_t)va_arg(ap, int); break;
+                case 2: value = (uintmax_t)(int16_t)va_arg(ap, int); break;
+                default:
+                case 4: value = (uintmax_t)va_arg(ap, unsigned int); break;
+                case 8: value = (uintmax_t)va_arg(ap, uintmax_t); break;
+            }
+            size_t pad_stri = stri;
+            for(size_t i = 0; i < field_width; i++) {
+                SPRINTF_PUTC(str, size, pad_stri, field_pad);
+            }
+
+            int advance = convert_as_int(str, size, stri, field_width, value, radix, unsign);
+            if(caps && str) {
+                for(int i = 0; i < advance; i++) {
+                    int stra = stri + i;
+                    if(stra < size) str[stra] = c_toupper(str[stra]);
+                }
+            }
+            stri += advance;
+        } continue;
+    }
+    if(str && stri < size) str[stri] = 0;
+    return stri;
+}