1 files changed, 117 insertions, 55 deletions

diff --git a/src/shared/printf.c b/src/shared/printf.c
index b50d4ef..47d56f8 100644
--- a/src/shared/printf.c
+++ b/src/shared/printf.c
@@ -1,71 +1,133 @@
+#include <stdbool.h>
 #include <shared/mem.h>
 #include <shared/printf.h>
 
+struct out_state {
+	void (*back)(void *, const char *, size_t);
+	void *backarg;
+	int written;
+};
+
+static int output(struct out_state *os, const char *buf, size_t len) {
+	if (len) os->back(os->backarg, buf, len);
+	os->written += len;
+	return os->written;
+}
+
+static void output_c(struct out_state *os, char c) {
+	output(os, &c, 1);
+}
+
+
+struct mods {
+	char fill_char;
+	size_t field_width;
+};
+
+static void pad(struct out_state *os, struct mods *m, size_t len) {
+	for (size_t i = 0; len + i < m->field_width; i++)
+		output(os, &m->fill_char, 1);
+}
+
+static void output_uint(struct out_state *os, struct mods *m, unsigned long long n) {
+	// TODO static assert sizeof(unsigned long long) <= 8
+	char buf[20];
+	size_t pos = sizeof(buf);
+
+	if (!n) {
+		buf[--pos] = '0';
+	} else while (n) {
+		unsigned long long q = n / 10, r = n % 10;
+		buf[--pos] = r + '0';
+		n = q;
+	}
+	pad(os, m, sizeof(buf) - pos);
+	output(os, buf + pos, sizeof(buf) - pos);
+}
+
+
 int __printf_internal(const char *fmt, va_list argp,
-		void (*back)(void*, const char*, size_t), void *back_arg)
+		void (*back)(void*, const char*, size_t), void *backarg)
 {
-	const char *seg = fmt; // beginning of the current segment
-	int total = 0;
+	const char *seg = fmt; /* beginning of the current non-modifier streak */
+	struct out_state os = {
+		.back = back,
+		.backarg = backarg,
+	};
 
 	for (;;) {
 		char c = *fmt++;
+		if (c == '\0') {
+			return output(&os, seg, fmt - seg - 1);
+		}
+		if (c != '%') continue;
+		output(&os, seg, fmt - seg - 1);
+
+		struct mods m = {
+			.fill_char = ' ',
+			.field_width = 0,
+		};
+
+		for (bool modifier = true; modifier;) {
+			c = *fmt++;
+			switch (c) {
+				case '0':
+					m.fill_char = '0';
+					break;
+				default:
+					modifier = false;
+					break;
+			}
+		}
+
+		while ('0' <= c && c <= '9') {
+			m.field_width *= 10;
+			m.field_width += c - '0';
+			c = *fmt++;
+		}
+
+		// TODO length modifiers
+
 		switch (c) {
-			case '\0':
-				back(back_arg, seg, fmt - seg - 1);
-				return total + (fmt - seg - 1);
+			unsigned long n, len;
 
-			case '%':
-				back(back_arg, seg, fmt - seg - 1);
-				total += fmt - seg - 1;
-				size_t pad_len = 0;
-
-				c = *fmt++;
-				while (1) {
-					switch (c) {
-						case '0':
-							pad_len = *fmt++ - '0'; // can skip over the null byte, idc
-							break;
-						default:
-							goto modifier_break;
-					}
-					c = *fmt++;
+			case 'c':
+				output_c(&os, va_arg(argp, int));
+				break;
+
+			case 's':
+				const char *s = va_arg(argp, char*);
+				if (s) {
+					len = strlen(s);
+					pad(&os, &m, len);
+					output(&os, s, len);
+				} else {
+					pad(&os, &m, 0);
 				}
-modifier_break:
-				switch (c) {
-					case 'c':
-						char c = va_arg(argp, int);
-						back(back_arg, &c, 1);
-						total += 1;
-						break;
-
-					case 's':
-						const char *s = va_arg(argp, char*);
-						if (s) {
-							back(back_arg, s, strlen(s));
-							total += strlen(s);
-						}
-						break;
-
-					case 'x':
-						unsigned long n = va_arg(argp, long);
-						size_t i = 4; // nibbles * 4
-						while (n >> i && i < (sizeof(n) * 8))
-							i += 4;
-
-						if (i < pad_len * 4)
-							i = pad_len * 4;
-
-						while (i > 0) {
-							i -= 4;
-							char h = '0' + ((n >> i) & 0xf);
-							if (h > '9') h += 'a' - '9' - 1;
-							back(back_arg, &h, 1);
-							total++;
-						}
-						break;
+				break;
+
+			case 'x':
+				n = va_arg(argp, unsigned long);
+				len = 1;
+				while (n >> (len * 4) && (len * 4) < (sizeof(n) * 8))
+					len++;
+				pad(&os, &m, len);
+				while (len-- > 0) {
+					char h = '0' + ((n >> (len * 4)) & 0xf);
+					if (h > '9') h += 'a' - '9' - 1;
+					output_c(&os, h);
 				}
-				seg = fmt;
+				break;
+
+			case 'u':
+				n = va_arg(argp, unsigned long);
+				output_uint(&os, &m, n);
+				break;
+
+			case '%':
+				output(&os, "%", 1);
 				break;
 		}
+		seg = fmt;
 	}
 }