Move printf() formatting to its own file to enable re-use

Also add snprintf(), and %X format code.

BUG=chrome-os-partner:10206
TEST=timerinfo; should print correctly.  'ectool battery' on host side should print same serial as 'battery' on EC console.

Change-Id: I5c9f69d1a20ee5d0a59440c122655adbf62c9aea
Signed-off-by: Randall Spangler <rspangler@chromium.org>
Reviewed-on: https://gerrit.chromium.org/gerrit/24635
Reviewed-by: Simon Glass <sjg@chromium.org>
Reviewed-by: Rong Chang <rongchang@chromium.org>

1 files changed, 236 insertions, 0 deletions

diff --git a/common/printf.c b/common/printf.c
new file mode 100644
index 0000000000..af393daa0b
--- /dev/null
+++ b/common/printf.c
@@ -0,0 +1,236 @@
+/* Copyright (c) 2012 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/* Printf-like functionality for Chrome EC */
+
+#include "printf.h"
+#include "timer.h"
+#include "util.h"
+
+static const char error_str[] = "ERROR";
+
+int vfnprintf(int (*addchar)(void *context, int c), void *context,
+	      const char *format, va_list args)
+{
+	char intbuf[34];
+		/* Longest uint64 in decimal = 20
+		 * longest uint32 in binary  = 32
+		 */
+	int dropped_chars = 0;
+	int is_left;
+	int pad_zero;
+	int pad_width;
+	char *vstr;
+	int vlen;
+
+	while (*format && !dropped_chars) {
+		int c = *format++;
+
+		/* Copy normal characters */
+		if (c != '%') {
+			dropped_chars |= addchar(context, c);
+			continue;
+		}
+
+		/* Get first format character */
+		c = *format++;
+
+		/* Send "%" for "%%" input */
+		if (c == '%' || c == '\0') {
+			dropped_chars |= addchar(context, '%');
+			continue;
+		}
+
+		/* Handle %c */
+		if (c == 'c') {
+			c = va_arg(args, int);
+			dropped_chars |= addchar(context, c);
+			continue;
+		}
+
+		/* Handle left-justification ("%-5s") */
+		is_left = (c == '-');
+		if (is_left)
+			c = *format++;
+
+		/* Handle padding with 0's */
+		pad_zero = (c == '0');
+		if (pad_zero)
+			c = *format++;
+
+		/* Count padding length */
+		pad_width = 0;
+		while (c >= '0' && c <= '9') {
+			pad_width = (10 * pad_width) + c - '0';
+			c = *format++;
+		}
+		if (pad_width > 80) {
+			/* Sanity check for width failed */
+			format = error_str;
+			continue;
+		}
+
+		if (c == 's') {
+			vstr = va_arg(args, char *);
+			if (vstr == NULL)
+				vstr = "(NULL)";
+		} else {
+			uint64_t v;
+			int is_negative = 0;
+			int is_64bit = 0;
+			int base = 10;
+			int fixed_point = 0;
+
+			/* Handle fixed point numbers */
+			if (c == '.') {
+				c = *format++;
+				if (c < '0' || c > '9') {
+					format = error_str;
+					continue;
+				}
+				fixed_point = c - '0';
+				c = *format++;
+			}
+
+			if (c == 'l') {
+				is_64bit = 1;
+				c = *format++;
+			}
+
+			/* Special-case: %T = current time */
+			if (c == 'T') {
+				v = get_time().val;
+				is_64bit = 1;
+				fixed_point = 6;
+			} else if (is_64bit) {
+				v = va_arg(args, uint64_t);
+			} else {
+				v = va_arg(args, uint32_t);
+			}
+
+			switch (c) {
+			case 'd':
+				if (is_64bit) {
+					if ((int64_t)v < 0) {
+						is_negative = 1;
+						if (v != (1ULL << 63))
+							v = -v;
+					}
+				} else {
+					if ((int)v < 0) {
+						is_negative = 1;
+						if (v != (1ULL << 31))
+							v = -(int)v;
+					}
+				}
+				break;
+			case 'u':
+			case 'T':
+				break;
+			case 'X':
+			case 'x':
+			case 'p':
+				base = 16;
+				break;
+			case 'b':
+				base = 2;
+				break;
+			default:
+				format = error_str;
+			}
+			if (format == error_str)
+				continue; /* Bad format specifier */
+
+			/* Convert integer to string, starting at end of
+			 * buffer and working backwards. */
+			vstr = intbuf + sizeof(intbuf) - 1;
+			*(vstr) = '\0';
+
+			/* Handle digits to right of decimal for fixed point
+			 * numbers. */
+			for (vlen = 0; vlen < fixed_point; vlen++)
+				*(--vstr) = '0' + uint64divmod(&v, 10);
+			if (fixed_point)
+				*(--vstr) = '.';
+
+			if (!v)
+				*(--vstr) = '0';
+
+			while (v) {
+				int digit = uint64divmod(&v, base);
+				if (digit < 10)
+					*(--vstr) = '0' + digit;
+				else if (c == 'X')
+					*(--vstr) = 'A' + digit - 9;
+				else
+					*(--vstr) = 'a' + digit - 9;
+			}
+
+			if (is_negative)
+				*(--vstr) = '-';
+		}
+
+		/* Copy string (or stringified integer) */
+		vlen = strlen(vstr);
+		while (vlen < pad_width && !is_left) {
+			dropped_chars |= addchar(context, pad_zero ? '0' : ' ');
+			vlen++;
+		}
+		while (*vstr)
+			dropped_chars |= addchar(context, *vstr++);
+		while (vlen < pad_width && is_left) {
+			dropped_chars |= addchar(context, ' ');
+			vlen++;
+		}
+	}
+
+	/* Successful if we consumed all output */
+	return dropped_chars ? EC_ERROR_OVERFLOW : EC_SUCCESS;
+}
+
+
+/* Context for snprintf() */
+struct snprintf_context {
+	char *str;
+	int size;
+};
+
+
+/* Add a character to the string */
+static int snprintf_addchar(void *context, int c)
+{
+	struct snprintf_context *ctx = (struct snprintf_context *)context;
+
+	if (!ctx->size)
+		return 1;
+
+	*(ctx->str++) = c;
+	ctx->size--;
+	return 0;
+}
+
+
+/* Print formatted outut to a string */
+int snprintf(char *str, int size, const char *format, ...)
+{
+	struct snprintf_context ctx;
+	va_list args;
+	int rv;
+
+	if (!str || !size)
+		return EC_ERROR_INVAL;
+
+	ctx.str = str;
+	ctx.size = size - 1;  /* Reserve space for terminating '\0' */
+
+	va_start(args, format);
+	rv = vfnprintf(snprintf_addchar, &ctx, format, args);
+	va_end(args);
+
+	/* Terminate string */
+	*ctx.str = '\0';
+
+	return rv;
+}