2 files changed, 154 insertions, 0 deletions
diff --git a/lib/src/fmt.c b/lib/src/fmt.c
new file mode 100644
index 0000000..be1ca3a
--- /dev/null
+++ b/lib/src/fmt.c
@@ -0,0 +1,117 @@
+// Copyright (c) 2020 Johannes Stoelp
+
+#include <fmt.h>
+
+static const char* num2dec(char* buf, unsigned long len, unsigned long long num) {
+    char* pbuf = buf + len - 1;
+    *pbuf = '\0';
+
+    if (num == 0) {
+        *(--pbuf) = '0';
+    }
+
+    while (num > 0 && pbuf != buf) {
+        char d = (num % 10) + '0';
+        *(--pbuf) = d;
+        num /= 10;
+    }
+    return pbuf;
+}
+
+static const char* num2hex(char* buf, unsigned long len, unsigned long long num) {
+    char* pbuf = buf + len - 1;
+    *pbuf = '\0';
+
+    if (num == 0) {
+        *(--pbuf) = '0';
+    }
+
+    while (num > 0 && pbuf != buf) {
+        char d = (num & 0xf);
+        *(--pbuf) = d + (d > 9 ? 'a' - 10 : '0');
+        num >>= 4;
+    }
+    return pbuf;
+}
+
+int vfmt(char* buf, unsigned long len, const char* fmt, va_list ap) {
+    unsigned i = 0;
+
+#define put(c)           \
+    {                    \
+        char _c = (c);   \
+        if (i < len) {   \
+            buf[i] = _c; \
+        }                \
+        ++i;             \
+    }
+
+#define puts(s)    \
+    while (*s) {   \
+        put(*s++); \
+    }
+
+    char scratch[16];
+    int l_cnt = 0;
+
+    while (*fmt) {
+        if (*fmt != '%') {
+            put(*fmt++);
+            continue;
+        }
+
+        l_cnt = 0;
+
+    continue_fmt:
+        switch (*(++fmt /* constume '%' */)) {
+            case 'l':
+                ++l_cnt;
+                goto continue_fmt;
+            case 'd': {
+                long val = l_cnt > 0 ? va_arg(ap, long) : va_arg(ap, int);
+                if (val < 0) {
+                    val *= -1;
+                    put('-');
+                }
+                const char* ptr = num2dec(scratch, sizeof(scratch), val);
+                puts(ptr);
+            } break;
+            case 'x': {
+                unsigned long val = l_cnt > 0 ? va_arg(ap, unsigned long) : va_arg(ap, unsigned);
+                const char* ptr = num2hex(scratch, sizeof(scratch), val);
+                puts(ptr);
+            } break;
+            case 's': {
+                const char* ptr = va_arg(ap, const char*);
+                puts(ptr);
+            } break;
+            case 'p': {
+                const void* val = va_arg(ap, const void*);
+                const char* ptr = num2hex(scratch, sizeof(scratch), (unsigned long long)val);
+                put('0');
+                put('x');
+                puts(ptr);
+            } break;
+            default:
+                put(*fmt);
+                break;
+        }
+        ++fmt;
+    }
+
+#undef puts
+#undef put
+
+    if (buf) {
+        i < len ? (buf[i] = '\0') : (buf[len - 1] = '\0');
+    }
+    return i;
+}
+
+int fmt(char* buf, unsigned long len, const char* fmt, ...) {
+    va_list ap;
+    va_start(ap, fmt);
+    int ret = vfmt(buf, len, fmt, ap);
+    va_end(ap);
+    return ret;
+}
diff --git a/lib/src/io.c b/lib/src/io.c
new file mode 100644
index 0000000..efe938b
--- /dev/null
+++ b/lib/src/io.c
@@ -0,0 +1,37 @@
+// Copyright (c) 2020 Johannes Stoelp
+
+#include <io.h>
+#include <fmt.h>
+#include <syscall.h>
+
+#include <asm/unistd.h>
+
+// `pfmt` uses fixed-size buffer on the stack for formating the message
+// (for simplicity and since we don't impl buffered I/O).
+//
+// NOTE: This allows to specify a large buffer on the stack, but for
+// the purpose of this study that's fine, we are cautious.
+#define MAX_PRINTF_LEN 128
+
+#define FD_STDOUT 1
+#define FD_STDERR 2
+
+int pfmt(const char* fmt, ...) {
+    char buf[MAX_PRINTF_LEN];
+
+    va_list ap;
+    va_start(ap, fmt);
+    int ret = vfmt(buf, sizeof(buf), fmt, ap);
+    va_end(ap);
+
+    if (ret > MAX_PRINTF_LEN - 1) {
+        syscall3(__NR_write, FD_STDERR, buf, MAX_PRINTF_LEN - 1);
+
+        static const char warn[] = "\npfmt: Message truncated, max length can be configured by defining MAX_PRINTF_LEN\n";
+        syscall3(__NR_write, FD_STDOUT, warn, sizeof(warn));
+        return MAX_PRINTF_LEN - 1;
+    }
+
+    syscall3(__NR_write, FD_STDOUT, buf, ret);
+    return ret;
+}