From 75491dd36b9483a6844355ffe6f8bda017918457 Mon Sep 17 00:00:00 2001
From: Josh Holtrop <jholtrop@gmail.com>
Date: Tue, 20 Oct 2020 19:30:23 -0400
Subject: [PATCH] add hos_printf and stream modules

---
 src/hos_printf.c | 138 +++++++++++++++++++++++++++++++++++++++++++++++
 src/hos_printf.h |  10 ++++
 src/stream.h     |  12 +++++
 3 files changed, 160 insertions(+)
 create mode 100644 src/hos_printf.c
 create mode 100644 src/hos_printf.h
 create mode 100644 src/stream.h

diff --git a/src/hos_printf.c b/src/hos_printf.c
new file mode 100644
index 0000000..681d3b6
--- /dev/null
+++ b/src/hos_printf.c
@@ -0,0 +1,138 @@
+#include "hos_printf.h"
+#include <stdint.h>
+#include <stdbool.h>
+#include "string.h"
+
+static size_t format_dec(char * buffer, int v)
+{
+    size_t sz = 0u;
+    bool printing = false;
+    if (v < 0)
+    {
+        buffer[sz++] = '-';
+        v = -v;
+    }
+    for (int div = 1000000000; div >= 1; div /= 10)
+    {
+        int digit = v / div;
+        v %= div;
+        if ((digit != 0) || (div == 1))
+        {
+            printing = true;
+        }
+        if (printing)
+        {
+            buffer[sz++] = digit + '0';
+        }
+    }
+    return sz;
+}
+
+static size_t format_udec(char * buffer, unsigned int v)
+{
+    size_t sz = 0u;
+    for (unsigned int div = 1000000000u; div >= 1u; div /= 10u)
+    {
+        unsigned int digit = v / div;
+        v %= div;
+        if ((digit != 0) || (sz > 0u) || (div == 1))
+        {
+            buffer[sz++] = digit + '0';
+        }
+    }
+    return sz;
+}
+
+static size_t format_hex(char * buffer, int v, bool upper)
+{
+    const char upper_hex[] = "0123456789ABCDEF";
+    const char lower_hex[] = "0123456789abcdef";
+    size_t sz = 0u;
+    for (int i = 28; i >= 0; i -= 4)
+    {
+        uint8_t n = (v >> i) & 0xFu;
+        if ((sz > 0u) || (n != 0u) || (i == 0))
+        {
+            buffer[sz] = upper ? upper_hex[n] : lower_hex[n];
+            sz++;
+        }
+    }
+    return sz;
+}
+
+void hos_vprintf(const stream_t * stream, const char * fmt, va_list va)
+{
+    bool in_conv = false;
+    char c;
+    char buffer[12];
+    while ((c = *fmt))
+    {
+        if (in_conv)
+        {
+            switch (c)
+            {
+            case '%':
+                stream->write1('%');
+                break;
+            case 'X':
+                {
+                    unsigned int v = va_arg(va, unsigned int);
+                    size_t length = format_hex(buffer, v, true);
+                    stream->write(buffer, length);
+                }
+                break;
+            case 'c':
+                {
+                    char ch = va_arg(va, int);
+                    stream->write1(ch);
+                }
+                break;
+            case 'd':
+                {
+                    int v = va_arg(va, int);
+                    size_t length = format_dec(buffer, v);
+                    stream->write(buffer, length);
+                }
+                break;
+            case 's':
+                {
+                    const char * s = va_arg(va, const char *);
+                    stream->write(s, strlen(s));
+                }
+                break;
+            case 'u':
+                {
+                    unsigned int v = va_arg(va, unsigned int);
+                    size_t length = format_udec(buffer, v);
+                    stream->write(buffer, length);
+                }
+                break;
+            case 'x':
+                {
+                    unsigned int v = va_arg(va, unsigned int);
+                    size_t length = format_hex(buffer, v, false);
+                    stream->write(buffer, length);
+                }
+                break;
+            }
+            in_conv = false;
+        }
+        else if (c == '%')
+        {
+            in_conv = true;
+        }
+        else
+        {
+            stream->write1(c);
+        }
+        fmt++;
+    }
+}
+
+void hos_printf(const stream_t * stream, const char * fmt, ...)
+{
+    va_list va;
+    va_start(va, fmt);
+    hos_vprintf(stream, fmt, va);
+    va_end(va);
+}
diff --git a/src/hos_printf.h b/src/hos_printf.h
new file mode 100644
index 0000000..64442cd
--- /dev/null
+++ b/src/hos_printf.h
@@ -0,0 +1,10 @@
+#ifndef HOS_PRINTF_H
+#define HOS_PRINTF_H
+
+#include "stream.h"
+#include <stdarg.h>
+
+void hos_vprintf(const stream_t * stream, const char * fmt, va_list va);
+void hos_printf(const stream_t * stream, const char * fmt, ...);
+
+#endif
diff --git a/src/stream.h b/src/stream.h
new file mode 100644
index 0000000..cbbf2fb
--- /dev/null
+++ b/src/stream.h
@@ -0,0 +1,12 @@
+#ifndef STREAM_H
+#define STREAM_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+typedef struct {
+    void (*write)(const uint8_t * src, size_t length);
+    void (*write1)(uint8_t c);
+} stream_t;
+
+#endif