1 files changed, 253 insertions, 0 deletions
diff --git a/roms/qboot/printf.c b/roms/qboot/printf.c
new file mode 100644
index 000000000..bfbd15c1b
--- /dev/null
+++ b/roms/qboot/printf.c
@@ -0,0 +1,253 @@
+#include "bios.h"
+#include "stdio.h"
+#include "string.h"
+#include "stdlib.h"
+#include "ioport.h"
+
+typedef struct pstream {
+    char *buffer;
+    int remain;
+    int added;
+} pstream_t;
+
+typedef struct strprops {
+    char pad;
+    int npad;
+} strprops_t;
+
+static void addchar(pstream_t *p, char c)
+{
+    if (p->remain) {
+	*p->buffer++ = c;
+	--p->remain;
+    }
+    ++p->added;
+}
+
+int puts(const char *c)
+{
+    int n = 0;
+    while (c[n])
+        outb(0x3f8, c[n++]);
+    return n;
+}
+
+void print_str(pstream_t *p, const char *s, strprops_t props)
+{
+    const char *s_orig = s;
+    int npad = props.npad;
+
+    if (npad > 0) {
+	npad -= strlen(s_orig);
+	while (npad > 0) {
+	    addchar(p, props.pad);
+	    --npad;
+	}
+    }
+
+    while (*s)
+	addchar(p, *s++);
+
+    if (npad < 0) {
+	props.pad = ' '; /* ignore '0' flag with '-' flag */
+	npad += strlen(s_orig);
+	while (npad < 0) {
+	    addchar(p, props.pad);
+	    ++npad;
+	}
+    }
+}
+
+static char digits[16] = "0123456789abcdef";
+
+void print_int(pstream_t *ps, long n, int base, strprops_t props)
+{
+    char buf[sizeof(long) * 3 + 2], *p = buf;
+    int s = 0, i;
+
+    if (n < 0) {
+	n = -n;
+	s = 1;
+    }
+
+    while (n) {
+	*p++ = digits[n % base];
+	n /= base;
+    }
+
+    if (s)
+	*p++ = '-';
+
+    if (p == buf)
+	*p++ = '0';
+
+    for (i = 0; i < (p - buf) / 2; ++i) {
+	char tmp;
+
+	tmp = buf[i];
+	buf[i] = p[-1-i];
+	p[-1-i] = tmp;
+    }
+
+    *p = 0;
+
+    print_str(ps, buf, props);
+}
+
+void print_unsigned(pstream_t *ps, unsigned long n, int base,
+		    strprops_t props)
+{
+    char buf[sizeof(long) * 3 + 1], *p = buf;
+    int i;
+
+    while (n) {
+	*p++ = digits[n % base];
+	n /= base;
+    }
+
+    if (p == buf)
+	*p++ = '0';
+
+    for (i = 0; i < (p - buf) / 2; ++i) {
+	char tmp;
+
+	tmp = buf[i];
+	buf[i] = p[-1-i];
+	p[-1-i] = tmp;
+    }
+
+    *p = 0;
+
+    print_str(ps, buf, props);
+}
+
+static int fmtnum(const char **fmt)
+{
+    const char *f = *fmt;
+    int len = 0, num;
+
+    if (*f == '-')
+	++f, ++len;
+
+    while (*f >= '0' && *f <= '9')
+	++f, ++len;
+
+    num = atol(*fmt);
+    *fmt += len;
+    return num;
+}
+
+int vsnprintf(char *buf, int size, const char *fmt, va_list va)
+{
+    pstream_t s;
+
+    s.buffer = buf;
+    s.remain = size - 1;
+    s.added = 0;
+    while (*fmt) {
+	char f = *fmt++;
+	int nlong = 0;
+	strprops_t props;
+	memset(&props, 0, sizeof(props));
+	props.pad = ' ';
+
+	if (f != '%') {
+	    addchar(&s, f);
+	    continue;
+	}
+    morefmt:
+	f = *fmt++;
+	if (f == '%') {
+	    addchar(&s, '%');
+	    continue;
+	}
+	if (f == 'c') {
+            addchar(&s, va_arg(va, int));
+	    continue;
+	}
+	if (f == '\0') {
+	    --fmt;
+	    continue;
+	}
+	if (f == '0') {
+	    props.pad = '0';
+	    ++fmt;
+	    /* fall through */
+	}
+	if ((f >= '1' && f <= '9') || f == '-') {
+	    --fmt;
+	    props.npad = fmtnum(&fmt);
+	    goto morefmt;
+	}
+	if (f == 'l') {
+	    ++nlong;
+	    goto morefmt;
+	}
+	if (f == 'd') {
+	    switch (nlong) {
+	    case 0:
+		print_int(&s, va_arg(va, int), 10, props);
+		break;
+	    case 1:
+		print_int(&s, va_arg(va, long), 10, props);
+		break;
+	    default:
+		panic();
+		break;
+	    }
+	    continue;
+	}
+	if (f == 'x') {
+	    switch (nlong) {
+	    case 0:
+		print_unsigned(&s, va_arg(va, unsigned), 16, props);
+		break;
+	    case 1:
+		print_unsigned(&s, va_arg(va, unsigned long), 16, props);
+		break;
+	    default:
+		panic();
+		break;
+	    }
+	    continue;
+	}
+	if (f == 'p') {
+	    print_str(&s, "0x", props);
+	    print_unsigned(&s, (unsigned long)va_arg(va, void *), 16, props);
+	    continue;
+	}
+	if (f == 's') {
+	    print_str(&s, va_arg(va, const char *), props);
+	    continue;
+	}
+        addchar(&s, f);
+    }
+    *s.buffer = 0;
+    ++s.added;
+    return s.added;
+}
+
+
+int snprintf(char *buf, int size, const char *fmt, ...)
+{
+    va_list va;
+    int r;
+
+    va_start(va, fmt);
+    r = vsnprintf(buf, size, fmt, va);
+    va_end(va);
+    return r;
+}
+
+int printf(const char *fmt, ...)
+{
+    va_list va;
+    char buf[2000];
+    int r;
+
+    va_start(va, fmt);
+    r = vsnprintf(buf, sizeof buf, fmt, va);
+    va_end(va);
+    puts(buf);
+    return r;
+}