1 files changed, 153 insertions, 0 deletions
diff --git a/lib/vsprintf.c b/lib/vsprintf.c
new file mode 100644
index 0000000..a566b83
--- /dev/null
+++ b/lib/vsprintf.c
@@ -0,0 +1,153 @@
+#include <stdarg.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+enum TYPE {
+  TYPE_INT      = 1,
+  TYPE_UINT     = 2,
+  TYPE_HEX      = 3,
+  TYPE_STRING   = 4,
+  TYPE_PERCENT  = 5
+};
+
+enum FLAGS {
+  FL_ZERO     = 1, /* zero padded */
+  FL_ALT      = 2, /* alternate form (effect is type dependent) */
+  FL_UPPER    = 4  /* uppercase */
+};
+
+static void hex(char **str, char flags, int fwidth, int size, int x) {
+  bool seen;
+  int i, y, len;
+  char c;
+  char *prefl = "0x";
+  char *prefu = "0X";
+
+  /* print the prefix, if the alt flag is set */
+  if(flags & FL_ALT) {
+    if(flags & FL_UPPER) {
+      while(*prefu)
+        *(*str)++ = *prefu++;
+    } else {
+      while(*prefl)
+        *(*str)++ = *prefl++;
+    }
+  }
+
+  /* calculate the number of digits */
+  len = 0;
+  y = x;
+  seen = false;
+  for(i = 0; i < sizeof(y) << 1; i++) {
+    c = ((unsigned int) y >> 28);
+    if(c)
+      seen = true;
+    if(c || seen)
+      len++;
+    y = y << 4;
+  }
+
+  /* print the padding characters (if any) */
+  c = ' ';
+  if(flags & FL_ZERO)
+    c = '0';
+  for(i = 0; i < fwidth - len; i++)
+    *(*str)++ = c;
+
+  /* actually print the digits */
+  seen = false;
+  for(i = 0; i < sizeof(x) << 1; i++) {
+    c = ((unsigned int) x >> 28) + '0';
+    if(c > '9') {
+      c += 0x27;
+      if(flags & FL_UPPER)
+        c -= 0x20;
+    }
+
+    if(c != '0')
+      seen = true;
+    if(c != '0' || seen)
+      *(*str)++ = c;
+
+    x = x << 4;
+  }
+}
+
+int vsprintf(char *str, char *fmt, va_list ap) {
+  int x, fwidth;
+  char flags;
+  char *s;
+  char *start = str;
+  uint8_t type;
+
+  while(*fmt) {
+    if(*fmt != '%') {
+      *str++ = *fmt++;
+      continue;
+    }
+
+    type = 0;
+    flags = 0;
+    fwidth = 0;
+
+    fmt++;
+    while(1) {
+      switch(*fmt) {
+        case '#':
+          flags |= FL_ALT;
+          break;
+        case '%':
+          type = TYPE_PERCENT;
+          goto done;
+        case 's':
+          type = TYPE_STRING;
+          goto done;
+        case 'x':
+          type = TYPE_HEX;
+          goto done;
+        case 'X':
+          type = TYPE_HEX;
+          flags |= FL_UPPER;
+          goto done;
+        default:
+          /* the first zero enables zero-padding */
+          if(*fmt == '0' && !fwidth) {
+            flags |= FL_ZERO;
+            goto next;
+          }
+
+          /* subsequent numbers indicate field width */
+          if(*fmt >= '0' && *fmt <= '9') {
+            fwidth = (fwidth * 10) + (*fmt - '0');
+            goto next;
+          }
+
+          goto done;
+      }
+next:
+      fmt++;
+    }
+
+done:
+    fmt++;
+
+    switch(type) {
+      case TYPE_STRING:
+        s = va_arg(ap, char*);
+        while(*s)
+          *str++ = *s++;
+        break;
+      case TYPE_HEX:
+        x = va_arg(ap, int);
+        hex(&str, flags, fwidth, 4, x);
+        break;
+      case TYPE_PERCENT:
+        *str++ = '%';
+        break;
+    }
+  }
+
+  *str = 0;
+
+  return (str - start);
+}