--- /dev/null
+/* adapted from http://www.efgh.com/software/gprintf.htm */
+/* public domain */
+
+#include <stdarg.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#include "kernel.h"
+#include "tty.h"
+
+#define BITS_PER_BYTE 8
+
+#define F_THOUSANDS 0x1
+#define F_LEFTJUSTIFY 0x2
+#define F_USESIGN 0x4
+#define F_SPACE 0x8
+#define F_ALTERNATE 0x10
+#define F_ZEROPAD 0x20
+#define F_CAPITAL 0x40
+/* F_PRECISION set if precision specified in format */
+#define F_PRECISION 0x80
+
+#if 0
+static unsigned char digits[17] = "01234567890abcdef";
+static unsigned char capdigits[17] = "01234567890ABCDEF";
+#endif
+
+struct parameters {
+ int n; /* number of output chars */
+ unsigned flags;
+ int width;
+ int precision;
+ int modifier;
+
+ short len;
+ int (*output_function)(void *, int);
+ void *output_pointer;
+ char *str;
+ char buf[BITS_PER_BYTE * sizeof(uintmax_t) + 1];
+};
+
+static int strlenk(char *s) {
+ int len = 0;
+ while (*s++) {
+ len++;
+ }
+ return len;
+}
+
+static void output(struct parameters *p, char *s) {
+ int justify = p->width - p->len;
+ char jc = ' ';
+
+ if (justify < 0) {
+ justify = 0;
+ }
+
+ if (justify && p->flags & F_ZEROPAD) {
+ jc = '0';
+ }
+
+ if (justify && !(p->flags & F_LEFTJUSTIFY)) {
+ while (justify--) {
+ terminal.putchar(jc);
+ p->n++;
+ }
+ }
+
+ while (p->len--) {
+ terminal.putchar(*s++);
+ p->n++;
+ }
+
+ while (justify-- > 0) {
+ terminal.putchar(' ');
+ p->n++;
+ }
+}
+
+static inline int get_decimal(const char *str, int *val) {
+ int ans = 0;
+ int used = 0;
+ int neg = 0;
+ char digit;
+
+ if (*str == '-') {
+ str++;
+ neg = 1;
+ used++;
+ }
+
+ while (digit = *str++) {
+ if (digit >= '0' && digit <= '9') {
+ ans *= 10;
+ ans += (digit - '0');
+ used++;
+ } else {
+ break;
+ }
+ }
+ if (neg) {
+ ans = -ans;
+ }
+ *val = ans;
+ return used;
+}
+
+int printk(const char *fmt, ...) {
+ va_list ap;
+ int n;
+ va_start(ap, fmt);
+ n = printkv(fmt, ap);
+ va_end(ap);
+ return n;
+}
+
+int printkv(const char *fmt, va_list ap) {
+ struct parameters p = { 0 };
+ struct parameters z = { 0 };
+ unsigned char fc;
+ int get_flags;
+
+ while (fc = *fmt++) {
+ if (fc != '%') {
+ terminal.putchar(fc);
+ p.n++;
+ continue;
+ }
+ p = z;
+
+ /* it's a conversion, get flags */
+ get_flags = 1;
+ while (get_flags) {
+ fc = *fmt++;
+ switch (fc) {
+ case 0:
+ /* we're done */
+ /* this is an error though */
+ return p.n;
+ break;
+ case '#':
+ p.flags |= F_ALTERNATE;
+ break;
+ case '0':
+ p.flags |= F_ZEROPAD;
+ break;
+ case '-':
+ p.flags |= F_LEFTJUSTIFY;
+ break;
+ case ' ':
+ p.flags |= F_SPACE;
+ break;
+ case '+':
+ p.flags |= F_USESIGN;
+ break;
+ /* posix, not C, so maybe need a define */
+ case '.':
+ p.flags |= F_THOUSANDS;
+ break;
+ default:
+ fmt--;
+ get_flags = 0;
+ break;
+ }
+ }
+
+ /* get minimum field width */
+ if (fc >= '1' && fc <= '9') {
+ fmt += get_decimal(fmt, &p.width);
+ } else if (*fmt == '*') {
+ fmt++;
+ if (*fmt >= '1' && *fmt <= '9') {
+ int lenarg = 0;
+ fmt += get_decimal(fmt, &lenarg);
+ if (*fmt != '$') {
+ /* TODO ERROR */
+ }
+ fmt++; /* skip past the $ */
+ } else {
+ p.width = va_arg(ap, int);
+ if (p.width < 0) {
+ p.width = -p.width;
+ p.flags |= F_LEFTJUSTIFY;
+ }
+ }
+ }
+
+ /* get precision */
+ if (*fmt == '.') {
+ fmt++;
+ p.precision = 0;
+ p.flags |= F_PRECISION;
+
+ if (fc >= '0' && fc <= '9') {
+ fmt += get_decimal(fmt, &p.width);
+ } else if (*fmt == '*') {
+ fmt++;
+ if (*fmt >= '1' && *fmt <= '9') {
+ int arg = 0;
+ fmt += get_decimal(fmt, &arg);
+ if (*fmt != '$') {
+ /* TODO ERROR */
+ }
+ fmt++; /* skip past the $ */
+ } else {
+ p.precision = va_arg(ap, int);
+ if (p.precision < 0) {
+ p.precision = 0;
+ p.flags &= ~F_PRECISION;
+ }
+ }
+ }
+ }
+
+ /* get length modifier */
+ switch (*fmt) {
+ case 'L':
+ case 'j':
+ case 'z':
+ case 't':
+ p.modifier = *fmt++;
+ break;
+ case 'h':
+ p.modifier = *fmt++;
+ if (*fmt == 'h') {
+ fmt++;
+ p.modifier = 'H';
+ }
+ break;
+ case 'l':
+ p.modifier = *fmt++;
+ if (*fmt == 'l') {
+ fmt++;
+ p.modifier = 'L';
+ }
+ break;
+ default:
+ break;
+ }
+
+ /* conversion specifier */
+ int base = 2;
+ uintmax_t i;
+ union {
+ uintmax_t uim;
+ int i;
+ char c;
+ unsigned u;
+ unsigned char uc;
+ unsigned short us;
+ unsigned long ul;
+ unsigned long long ull;
+ size_t size;
+ ptrdiff_t pd;
+ } arg;
+
+ switch (*fmt) {
+ case '%':
+ terminal.putchar('%');
+ p.n++;
+ fmt++;
+ break;
+ case 'c':
+ if (p.modifier == 'l') {
+ /* wide int wint_t */
+ break;
+ }
+ arg.i = va_arg(ap, int);
+ p.buf[0] = (unsigned char) arg.i;
+ p.len = 1;
+ output(&p, p.buf);
+ fmt++;
+ break;
+ case 's':
+ p.str = va_arg(ap, char *);
+ /* TODO may not be properly terminated if precision is given */
+ p.len = strlenk(p.str);
+ /* precision, if given, is maximum characters
+ * for an 's' conversion */
+ if (p.flags & F_PRECISION && p.len > p.precision) {
+ p.len = p.precision;
+ }
+ /* TODO don't zero pad strings */
+ output(&p, p.str);
+ fmt++;
+ break;
+ case 'X':
+ p.flags |= F_CAPITAL;
+ case 'x':
+ base += 6;
+ case 'u':
+ base += 2;
+ case 'o':
+ base += 6;
+ switch (p.modifier) {
+ case 'H':
+ arg.u = va_arg(ap, unsigned int);
+ i = (uintmax_t) (unsigned char) arg.uc;
+ break;
+ case 'h':
+ arg.u = va_arg(ap, unsigned int);
+ i = (uintmax_t) (unsigned short) arg.us;
+ break;
+ case 'l':
+ arg.ul = va_arg(ap, unsigned long);
+ i = (uintmax_t) arg.ul;
+ break;
+ case 'L':
+ arg.ull = va_arg(ap, unsigned long long);
+ i = (uintmax_t) arg.ull;
+ break;
+ case 'j':
+ i = va_arg(ap, uintmax_t);
+ break;
+ case 'z':
+ arg.size = va_arg(ap, size_t);
+ i = (uintmax_t) arg.size;
+ break;
+ case 't':
+ arg.pd = va_arg(ap, ptrdiff_t);
+ i = (uintmax_t) arg.pd;
+ break;
+ default:
+ arg.u = va_arg(ap, unsigned);
+ i = (uintmax_t) arg.u;
+ break;
+ }
+ p.str = p.buf + sizeof(p.buf) - 1;
+ *p.str = 0;
+ p.str--;
+ do {
+ int c;
+ c = i % base + '0';
+ if (c > '9') {
+ if (p.flags & F_CAPITAL)
+ c += 'A'-'9'-1;
+ else
+ c += 'a'-'9'-1;
+ }
+ *p.str-- = c;
+ p.len++;
+ } while ( i /= base);
+ output(&p, p.str+1);
+ fmt++;
+ break;
+ case 'd':
+ case 'i':
+ base = 10;
+
+ default:
+ break;
+ }
+ /* output number here ? */
+
+ }
+ terminal.update_cursor();
+
+ return p.n;
+}
projects / zos / blobdiff