1 files changed, 115 insertions, 0 deletions
diff --git a/klib/sfmt.c b/klib/sfmt.c
new file mode 100644
index 0000000..519fbf4
--- /dev/null
+++ b/klib/sfmt.c
@@ -0,0 +1,115 @@
+#include "string.h"
+#include "format.h"
+
+char*
+sfmt(char *s, size_t limit, const char *fmt, ...)
+{
+    va_list ap;
+    va_start(ap, fmt);
+
+    s = svfmt(s, limit, fmt, ap);
+
+    va_end(ap);
+    return s;
+}
+
+char*
+svfmt(char *s, size_t limit, const char *fmt, va_list ap)
+{
+
+    size_t fmtlen = strlen(fmt);
+    size_t si = 0;
+    for(size_t fmti = 0; fmti < fmtlen; fmti++)
+    {
+        if(si >= limit) break;
+        if(fmt[fmti] != '%') {
+            s[si++] = fmt[fmti];
+            continue;
+        }
+        // TODO: Format flags
+        fmti++;
+
+        bool alt = false;
+        bool zpad = false;
+        for(;;fmti++){
+            if(fmt[fmti] == '#') { alt = true; continue; }
+            if(fmt[fmti] == '0') { zpad = true; continue; }
+            break;
+        }
+        int fwidth = 0;
+        if(fmt[fmti] >= '1' && fmt[fmti] <= '9')
+        {
+            for(;fmt[fmti] >= '0' && fmt[fmti] <= '9';fmti++) {
+                fwidth *= 10;
+                fwidth += fmt[fmti] - '0';
+            }
+        }
+        int precision = 0;
+        if(fmt[fmti] == '.')
+        {
+            fmti++;
+            for(;fmt[fmti] >= '0' && fmt[fmti] <= '9';fmti++) {
+                precision *= 10;
+                precision += fmt[fmti] - '0';
+            }
+        }
+
+        bool sgn = true;
+        bool upper = false;
+        int radix = 10;
+        switch(fmt[fmti])
+        {
+            case '%':
+                s[si++] = '%';
+                break;
+            case 'p':
+                radix = 16;
+                alt = true;
+            case 'b':
+                if(radix == 10) radix = 2;
+            case 'X':
+                upper = true;
+            case 'x':
+                if(radix == 10)
+                    radix = 16;
+            case 'o':
+                if(radix == 10)
+                    radix = 8;
+            case 'u':
+                sgn = false;
+            case 'i': {
+                if((radix == 8 || radix == 16) && alt) {
+                    s[si++] = '0';
+                    if(radix == 16) {
+                        s[si++] = 'x';
+                    }
+                    if(radix == 2) {
+                        s[si++] = 'b';
+                    }
+                }
+                size_t osi = si;
+                size_t nlen = ltostr(&s[si], limit - si, va_arg(ap, long), sgn, radix);
+                if(upper) sntoupper(&s[si], nlen);
+                si += nlen;
+
+                int lpad = fwidth - (int)nlen;
+                if(lpad > 0)
+                {
+                    if(lpad + osi >= limit) lpad = (limit - osi - 1);
+                    memmove(&s[osi + lpad], &s[osi], nlen);
+                    memset(&s[osi], zpad ? '0' : ' ', lpad);
+                    si += lpad;
+                }
+                      } break;
+            case 's': {
+                const char *str = va_arg(ap, char*);
+                size_t slen = strlen(str);
+                size_t wlen = slen > limit - si ? limit - si : slen;
+                for(size_t i = 0; i < wlen; i++)
+                    s[si++] = str[i];
+                      } break;
+        }
+    }
+    s[si] = 0;
+    return s;
+}