1 files changed, 146 insertions, 0 deletions
diff --git a/lib/print.c b/lib/print.c
new file mode 100644
index 0000000..45c7671
--- /dev/null
+++ b/lib/print.c
@@ -0,0 +1,146 @@
+#include "print.h"
+#include "device/uart.h"
+#include "jove.h"
+#include "string.h"
+#include <stddef.h>
+#include <stdbool.h>
+
+int 
+kprintf(const char *fmt, ...)
+{
+    va_list ap;
+    va_start(ap, fmt);
+    int r = kvprintf(fmt, ap);
+    va_end(ap);
+    return r;
+}
+
+int 
+ksprintf(char *s, const char *fmt, ...)
+{
+    va_list ap;
+    va_start(ap, fmt);
+    int r = kvsprintf(s, fmt, ap);
+    va_end(ap);
+    return r;
+}
+
+int 
+ksnprintf(char *s, int size, const char *fmt, ...)
+{
+    va_list ap;
+    va_start(ap, fmt);
+    int r = kvsnprintf(s, size, fmt, ap);
+    va_end(ap);
+    return r;
+}
+
+int kvprintf(const char *fmt, va_list ap)
+{
+    va_list ap_dup;
+    va_copy(ap_dup, ap);
+    int size = kvsnprintf(0, 0, fmt, ap);
+    char buffer[size];
+    kvsnprintf(buffer, size, fmt, ap_dup);
+
+#ifdef ENABLE_UART
+    uart_write(&_initDirectory, _initData.log_object, buffer, size - 1);
+#endif
+    return size;
+}
+
+int 
+kvsprintf(char *s, const char *fmt, va_list ap)
+{
+    return kvsnprintf(s, 65536, fmt, ap);
+}
+
+int 
+kvsnprintf(char *s, int size, const char *fmt, va_list ap)
+{
+    const char *fmtc = fmt;
+    int wsize = 0;
+    do{
+        if(*fmtc != '%') {
+            if(s != NULL && wsize < size) s[wsize] = *fmtc;
+            wsize++;
+            continue;
+        }
+
+        bool sign = true;
+        bool caps = true;
+        int radix = 10;
+        int lpad = 0;
+        int paramw = sizeof(int);
+
+        if(*(++fmtc) == 0) goto done;
+        switch(*fmtc) {
+            case 'c':
+                if(s != NULL && wsize < size) s[wsize] = va_arg(ap, int);
+                wsize++;
+                break;
+            case 'p': //fallthrough;
+                paramw = sizeof(uintptr_t);
+                lpad = paramw * 2;
+            case 'X': //fallthrough
+            case 'x': //fallthrough
+                sign = false;
+                radix = 16;
+                goto printint;
+                break;
+            case 'u': //fallthrough
+                sign = false;
+                goto printint;
+                break;
+            case 'i': 
+                goto printint; 
+                break;
+            case 's':
+                goto printstr;
+                break;
+            default:
+            break;
+        }
+        continue;
+printstr:
+        {
+            char *arg_s = va_arg(ap, char*);
+            if(arg_s == 0) arg_s = "(NULL)";
+            size_t ssize = strlen(arg_s);
+            for(char *c = arg_s; *c; c++) {
+                if(s != NULL && wsize < size) s[wsize] = *c;
+                wsize++;
+            }
+        }
+        continue;
+printint: 
+        {
+            int isize = 1;
+            void *iptr = NULL;
+            if(s != NULL) {
+                iptr = &s[wsize];
+            }
+            switch(paramw) {
+                case sizeof(int): isize = ltostr(iptr, size - wsize, va_arg(ap, int), sign, radix); break;
+                case sizeof(uintptr_t): isize = ltostr(iptr, size - wsize, va_arg(ap, long), sign, radix); break;
+                default: break;
+            }
+            if(lpad) {
+            if(isize < lpad) {
+                    lpad -= isize - 1;
+                    if(s != 0) {
+                        memmove(&s[wsize + lpad], &s[wsize], isize);
+                        memset(&s[wsize], '0', lpad);
+                    }
+                    isize += lpad;
+
+                }
+            }
+            wsize += isize;
+            continue;
+        }
+    }while(*(fmtc++) != 0);
+done:
+    if(s != NULL && wsize < size) s[wsize++] = 0;
+    return wsize;
+}