libpayload: console: Allow output drivers to print whole strings at once

The console output driver framework in libpayload is currently built on
the putchar primitive, meaning that every driver's function gets called
one character at a time. This becomes an issue when we add drivers that
could output multiple characters at a time, but have a high constant
overhead per invocation (such as the planned GDB stub, which needs to
wrap a special frame around output strings and wait for an
acknowledgement from the server).

This patch adds a new 'write' function pointer to the
console_output_driver structure as an alternative to 'putchar'. Output
drivers need to provide at least one of the two ('write' is preferred if
available). The CBMEM console driver is ported as a proof of concept
(since it's our most performace-critical driver and should in theory
benefit the most from less function pointer invocations, although it's
probably still negligible compared to the big sprawling mess that is
printf()).

Even with this fix, the problem remains that printf() was written with
the putchar primitive in mind. Even though normal text already contains
an optimization to allow multiple characters at a time, almost all
formatting directives cause their output (including things like
padding whitespace) to be putchar()ed one character at a time.
Therefore, this patch reworks parts of the output code (especially
number printing) to all but remove that inefficiency (directives still
invoke an extra write() call, but at least not one per character). Since
I'm touching printf() core code anyway, I also tried to salvage what I
could from that weird, broken "return negative on error" code path (not
that any of our current output drivers can trigger it anyway).

A final consequence of this patch is that the responsibility to prepend
line feeds with carriage returns is moved into the output driver
implementations. Doing this only makes sense for drivers with explicit
cursor position control (i.e. serial or video), and things like the
CBMEM console that appears like a normal file to the system really have
no business containing carriage returns (we don't want people to
accidentally associate us with Windows, now, do we?).

BUG=chrome-os-partner:18390
TEST=Made sure video and CBMEM console still look good, tried printf()
with as many weird edge-case strings as I could find and compared serial
output as well as sprintf() return value.

Original-Change-Id: Ie05ae489332a0103461620f5348774b6d4afd91a
Original-Signed-off-by: Julius Werner <jwerner@chromium.org>
Original-Reviewed-on: https://chromium-review.googlesource.com/196384
Original-Reviewed-by: Hung-Te Lin <hungte@chromium.org>
Original-Reviewed-by: David Hendricks <dhendrix@chromium.org>
(cherry picked from commit ab1ef0c07736fe1aa3e0baaf02d258731e6856c0)
Signed-off-by: Marc Jones <marc.jones@se-eng.com>

Change-Id: I78f5aedf6d0c3665924995cdab691ee0162de404
Reviewed-on: http://review.coreboot.org/7880
Tested-by: build bot (Jenkins)
Reviewed-by: Patrick Georgi <pgeorgi@google.com>

7 files changed, 150 insertions, 165 deletions

diff --git a/payloads/libpayload/drivers/cbmem_console.c b/payloads/libpayload/drivers/cbmem_console.c
index 0d3b35fccb..687344400c 100644
--- a/payloads/libpayload/drivers/cbmem_console.c
+++ b/payloads/libpayload/drivers/cbmem_console.c
@@ -40,7 +40,7 @@ static struct cbmem_console *cbmem_console_p;
 
 static struct console_output_driver cbmem_console_driver =
 {
-	.putchar = &cbmem_console_putc
+	.write = &cbmem_console_write,
 };
 
 void cbmem_console_init(void)
@@ -50,11 +50,11 @@ void cbmem_console_init(void)
 		console_add_output_driver(&cbmem_console_driver);
 }
 
-void cbmem_console_putc(unsigned int data)
+void cbmem_console_write(const void *buffer, size_t count)
 {
-	// Bail out if the buffer is full.
-	if (cbmem_console_p->cursor >= cbmem_console_p->size)
+	if (cbmem_console_p->cursor + count >= cbmem_console_p->size)
 		return;
 
-	cbmem_console_p->body[cbmem_console_p->cursor++] = data;
+	memcpy(cbmem_console_p->body + cbmem_console_p->cursor, buffer, count);
+	cbmem_console_p->cursor += count;
 }
diff --git a/payloads/libpayload/drivers/serial/8250.c b/payloads/libpayload/drivers/serial/8250.c
index a4c1b1a2ab..0651f5201f 100644
--- a/payloads/libpayload/drivers/serial/8250.c
+++ b/payloads/libpayload/drivers/serial/8250.c
@@ -137,6 +137,8 @@ void serial_putchar(unsigned int c)
 		return;
 	while ((serial_read_reg(0x05) & 0x20) == 0) ;
 	serial_write_reg(c, 0x00);
+	if (c == '\n')
+		serial_putchar('\r');
 }
 
 int serial_havechar(void)
diff --git a/payloads/libpayload/drivers/serial/tegra.c b/payloads/libpayload/drivers/serial/tegra.c
index bcf7b1925d..fa5ff16526 100644
--- a/payloads/libpayload/drivers/serial/tegra.c
+++ b/payloads/libpayload/drivers/serial/tegra.c
@@ -65,6 +65,8 @@ void serial_putchar(unsigned int c)
 {
 	while (!(readb(&uart_regs->lsr) & TEGRA_UART_LSR_THRE));
 	writeb(c, &uart_regs->thr);
+	if (c == '\n')
+		serial_putchar('\r');
 }
 
 int serial_havechar(void)
diff --git a/payloads/libpayload/drivers/video/video.c b/payloads/libpayload/drivers/video/video.c
index ed47234345..ce0374d6a3 100644
--- a/payloads/libpayload/drivers/video/video.c
+++ b/payloads/libpayload/drivers/video/video.c
@@ -141,6 +141,7 @@ void video_console_putchar(unsigned int ch)
 		break;
 
 	case '\n':
+		cursorx = 0;
 		cursory++;
 		break;
 
diff --git a/payloads/libpayload/include/libpayload.h b/payloads/libpayload/include/libpayload.h
index d6258de3ac..824e96e8c2 100644
--- a/payloads/libpayload/include/libpayload.h
+++ b/payloads/libpayload/include/libpayload.h
@@ -212,7 +212,7 @@ void video_console_set_cursor(unsigned int cursorx, unsigned int cursory);
  * @{
  */
 void cbmem_console_init(void);
-void cbmem_console_putc(unsigned int data);
+void cbmem_console_write(const void *buffer, size_t count);
 /** @} */
 
 /* drivers/option.c */
@@ -255,6 +255,7 @@ typedef enum {
 } console_input_type;
 
 void console_init(void);
+void console_write(const void *buffer, size_t count);
 int putchar(unsigned int c);
 int puts(const char *s);
 int havekey(void);
@@ -276,6 +277,7 @@ struct console_output_driver;
 struct console_output_driver {
 	struct console_output_driver *next;
 	void (*putchar) (unsigned int);
+	void (*write) (const void *, size_t);
 };
 
 void console_add_output_driver(struct console_output_driver *out);
diff --git a/payloads/libpayload/libc/console.c b/payloads/libpayload/libc/console.c
index 59d5bccb48..8c0664d125 100644
--- a/payloads/libpayload/libc/console.c
+++ b/payloads/libpayload/libc/console.c
@@ -37,6 +37,7 @@ static console_input_type last_getchar_input_type;
 
 void console_add_output_driver(struct console_output_driver *out)
 {
+	die_if(!out->putchar && !out->write, "Need at least one output func\n");
 	out->next = console_out;
 	console_out = out;
 }
@@ -63,33 +64,33 @@ void console_init(void)
 #endif
 }
 
-static void device_putchar(unsigned char c)
+void console_write(const void *buffer, size_t count)
 {
+	const char *ptr;
 	struct console_output_driver *out;
 	for (out = console_out; out != 0; out = out->next)
-		out->putchar(c);
+		if (out->write)
+			out->write(buffer, count);
+		else
+			for (ptr = buffer; (void *)ptr < buffer + count; ptr++)
+				out->putchar(*ptr);
 }
 
-int putchar(unsigned int c)
+int putchar(unsigned int i)
 {
-	c &= 0xff;
-	if (c == '\n')
-		device_putchar('\r');
-	device_putchar(c);
-	return c;
+	unsigned char c = (unsigned char)i;
+	console_write(&c, 1);
+	return (int)c;
 }
 
 int puts(const char *s)
 {
-	int n = 0;
+	size_t size = strlen(s);
 
-	while (*s) {
-		putchar(*s++);
-		n++;
-	}
+	console_write(s, size);
 
 	putchar('\n');
-	return n + 1;
+	return size + 1;
 }
 
 int havekey(void)
diff --git a/payloads/libpayload/libc/printf.c b/payloads/libpayload/libc/printf.c
index bc2c41c255..8f76ccf286 100644
--- a/payloads/libpayload/libc/printf.c
+++ b/payloads/libpayload/libc/printf.c
@@ -69,11 +69,10 @@ struct printf_spec {
 #define __PRINTF_FLAG_NEGATIVE		0x00000100
 
 /**
- * Buffer big enough for 64-bit number printed in base 2, sign, prefix and 0
- * to terminate string (last one is only for better testing end of buffer by
- * zero-filling subroutine).
+ * Buffer big enough for 64-bit number printed in base 2, sign, and prefix.
+ * Add some more to support sane amounts of zero-padding.
  */
-#define PRINT_NUMBER_BUFFER_SIZE	(64 + 5)
+#define PRINT_BUFFER_SIZE		(64 + 1 + 2 + 13)
 
 /** Enumeration of possible arguments types. */
 typedef enum {
@@ -128,6 +127,26 @@ static int printf_putchar(int c, struct printf_spec *ps)
 	return ps->write(&ch, 1, ps->data);
 }
 
+/* Print spaces for padding. Ignores negative counts. */
+static int print_spaces(int count, struct printf_spec *ps)
+{
+	int tmp, ret;
+	char buffer[PRINT_BUFFER_SIZE];
+
+	if (count <= 0)
+		return 0;
+
+	memset(buffer, ' ', MIN(PRINT_BUFFER_SIZE, count));
+	for (tmp = count; tmp > PRINT_BUFFER_SIZE; tmp -= PRINT_BUFFER_SIZE)
+		if ((ret = printf_putnchars(buffer, PRINT_BUFFER_SIZE, ps)) < 0)
+			return ret;
+
+	if ((ret = printf_putnchars(buffer, tmp, ps)) < 0)
+		return ret;
+
+	return count;
+}
+
 /**
  * Print one formatted character.
  *
@@ -139,21 +158,24 @@ static int printf_putchar(int c, struct printf_spec *ps)
  */
 static int print_char(char c, int width, uint64_t flags, struct printf_spec *ps)
 {
-	int counter = 0;
+	int retval;
+	int counter = 1;
 
 	if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
-		while (--width > 0) {
-			if (printf_putchar(' ', ps) > 0)
-				++counter;
-		}
+		if ((retval = print_spaces(width - 1, ps)) < 0)
+			return retval;
+		else
+			counter += retval;
 	}
 
-	if (printf_putchar(c, ps) > 0)
-		counter++;
+	if ((retval = printf_putchar(c, ps)) < 0)
+		return retval;
 
-	while (--width > 0) {
-		if (printf_putchar(' ', ps) > 0)
-			++counter;
+	if (flags & __PRINTF_FLAG_LEFTALIGNED) {
+		if ((retval = print_spaces(width - 1, ps)) < 0)
+			return retval;
+		else
+			counter += retval;
 	}
 
 	return counter;
@@ -185,19 +207,21 @@ static int print_string(char *s, int width, unsigned int precision,
 	width -= precision;
 
 	if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
-		while (width-- > 0) {
-			if (printf_putchar(' ', ps) == 1)
-				counter++;
-		}
+		if ((retval = print_spaces(width, ps)) < 0)
+			return retval;
+		else
+			counter += retval;
 	}
 
 	if ((retval = printf_putnchars(s, MIN(size, precision), ps)) < 0)
-		return -counter;
+		return retval;
 	counter += retval;
 
-	while (width-- > 0) {
-		if (printf_putchar(' ', ps) == 1)
-			++counter;
+	if (flags & __PRINTF_FLAG_LEFTALIGNED) {
+		if ((retval = print_spaces(width, ps)) < 0)
+			return retval;
+		else
+			counter += retval;
 	}
 
 	return counter;
@@ -209,7 +233,7 @@ static int print_string(char *s, int width, unsigned int precision,
  * Print significant digits of a number in given base.
  *
  * @param num		Number to print.
- * @param width		Width modifier.h
+ * @param width		Width modifier.
  * @param precision	Precision modifier.
  * @param base		Base to print the number in (must be between 2 and 16).
  * @param flags		Flags that modify the way the number is printed.
@@ -220,49 +244,34 @@ static int print_number(uint64_t num, int width, int precision, int base,
 			uint64_t flags, struct printf_spec *ps)
 {
 	const char *digits = digits_small;
-	char d[PRINT_NUMBER_BUFFER_SIZE];
-	char *ptr = &d[PRINT_NUMBER_BUFFER_SIZE - 1];
-	int size = 0;		/* Size of number with all prefixes and signs. */
-	int number_size;	/* Size of plain number. */
+	char d[PRINT_BUFFER_SIZE];
+	char *ptr = &d[PRINT_BUFFER_SIZE];
+	int size = 0;			/* Size of the string in ptr */
+	int counter = 0;		/* Amount of actually printed bytes. */
 	char sgn;
 	int retval;
-	int counter = 0;
 
 	if (flags & __PRINTF_FLAG_BIGCHARS)
 		digits = digits_big;
 
-	*ptr-- = 0;		/* Put zero at end of string. */
-
 	if (num == 0) {
-		*ptr-- = '0';
+		*--ptr = '0';
 		size++;
 	} else {
 		do {
-			*ptr-- = digits[num % base];
+			*--ptr = digits[num % base];
 			size++;
 		} while (num /= base);
 	}
 
-	number_size = size;
+	/* Both precision and LEFTALIGNED overrule ZEROPADDED. */
+	if ((flags & __PRINTF_FLAG_LEFTALIGNED) || precision)
+		flags &= ~__PRINTF_FLAG_ZEROPADDED;
 
-	/*
-	 * Collect the sum of all prefixes/signs/... to calculate padding and
-	 * leading zeroes.
-	 */
-	if (flags & __PRINTF_FLAG_PREFIX) {
-		switch (base) {
-		case 2:	/* Binary formating is not standard, but useful. */
-			size += 2;
-			break;
-		case 8:
-			size++;
-			break;
-		case 16:
-			size += 2;
-			break;
-		}
-	}
+	/* Fix precision now since it doesn't count prefixes/signs. */
+	precision -= size;
 
+	/* Reserve size for prefixes/signs before filling up padding. */
 	sgn = 0;
 	if (flags & __PRINTF_FLAG_SIGNED) {
 		if (flags & __PRINTF_FLAG_NEGATIVE) {
@@ -276,87 +285,75 @@ static int print_number(uint64_t num, int width, int precision, int base,
 			size++;
 		}
 	}
-
-	if (flags & __PRINTF_FLAG_LEFTALIGNED)
-		flags &= ~__PRINTF_FLAG_ZEROPADDED;
-
-	/*
-	 * If the number is left-aligned or precision is specified then
-	 * zero-padding is ignored.
-	 */
-	if (flags & __PRINTF_FLAG_ZEROPADDED) {
-		if ((precision == 0) && (width > size))
-			precision = width - size + number_size;
-	}
-
-	/* Print leading spaces. */
-	if (number_size > precision) {
-		/* Print the whole number not only a part. */
-		precision = number_size;
-	}
-
-	width -= precision + size - number_size;
-
-	if (!(flags & __PRINTF_FLAG_LEFTALIGNED)) {
-		while (width-- > 0) {
-			if (printf_putchar(' ', ps) == 1)
-				counter++;
+	if (flags & __PRINTF_FLAG_PREFIX) {
+		switch (base) {
+		case 2:	/* Binary formating is not standard, but useful. */
+			size += 2;
+			break;
+		case 8:
+			size++;
+			break;
+		case 16:
+			size += 2;
+			break;
 		}
 	}
 
-	/* Print sign. */
-	if (sgn) {
-		if (printf_putchar(sgn, ps) == 1)
-			counter++;
+	/* If this is still set we didn't have a precision, so repurpose it */
+	if (flags & __PRINTF_FLAG_ZEROPADDED)
+		precision = width - size;
+
+	/* Pad smaller numbers with 0 (larger numbers lead to precision < 0). */
+	if (precision > 0) {
+		precision = MIN(precision, PRINT_BUFFER_SIZE - size);
+		ptr -= precision;
+		size += precision;
+		memset(ptr, '0', precision);
 	}
 
-	/* Print prefix. */
+	/* Add sign and prefix (we adjusted size for this beforehand). */
 	if (flags & __PRINTF_FLAG_PREFIX) {
 		switch (base) {
 		case 2:	/* Binary formating is not standard, but useful. */
-			if (printf_putchar('0', ps) == 1)
-				counter++;
-			if (flags & __PRINTF_FLAG_BIGCHARS) {
-				if (printf_putchar('B', ps) == 1)
-					counter++;
-			} else {
-				if (printf_putchar('b', ps) == 1)
-					counter++;
-			}
+			*--ptr = (flags & __PRINTF_FLAG_BIGCHARS) ? 'B' : 'b';
+			*--ptr = '0';
 			break;
 		case 8:
-			if (printf_putchar('o', ps) == 1)
-				counter++;
+			*--ptr = '0';
 			break;
 		case 16:
-			if (printf_putchar('0', ps) == 1)
-				counter++;
-			if (flags & __PRINTF_FLAG_BIGCHARS) {
-				if (printf_putchar('X', ps) == 1)
-					counter++;
-			} else {
-				if (printf_putchar('x', ps) == 1)
-					counter++;
-			}
+			*--ptr = (flags & __PRINTF_FLAG_BIGCHARS) ? 'X' : 'x';
+			*--ptr = '0';
 			break;
 		}
 	}
-
-	/* Print leading zeroes. */
-	precision -= number_size;
-	while (precision-- > 0) {
-		if (printf_putchar('0', ps) == 1)
-			counter++;
+	if (sgn)
+		*--ptr = sgn;
+
+	/* Pad with spaces up to width, try to avoid extra putnchar if we can */
+	width -= size;
+	if (width > 0 && !(flags & __PRINTF_FLAG_LEFTALIGNED)) {
+		int tmp = MIN(width, PRINT_BUFFER_SIZE - size);
+		ptr -= tmp;
+		size += tmp;
+		memset(ptr, ' ', tmp);
+		if ((retval = print_spaces(width - tmp, ps)) < 0)
+			return retval;
+		else
+			counter += retval;
 	}
 
-	/* Print number itself. */
-	if ((retval = printf_putstr(++ptr, ps)) > 0)
-		counter += retval;
+	/* Now print the whole thing at once. */
+	if ((retval = printf_putnchars(ptr, size, ps)) < 0)
+		return retval;
+	counter += retval;
 
-	/* Print ending spaces. */
-	while (width-- > 0) {
-		if (printf_putchar(' ', ps) == 1)
-			counter++;
+	/* Edge case: left-aligned with width (should be rare). */
+	if (flags & __PRINTF_FLAG_LEFTALIGNED) {
+		if ((retval = print_spaces(width, ps)) < 0)
+			return retval;
+		else
+			counter += retval;
 	}
 
 	return counter;
@@ -468,10 +465,8 @@ static int printf_core(const char *fmt, struct printf_spec *ps, va_list ap)
 			/* Print common characters if any processed. */
 			if (i > j) {
 				if ((retval = printf_putnchars(&fmt[j],
-				    (size_t) (i - j), ps)) < 0) {
-					counter = -counter;
-					goto out;	/* Error */
-				}
+				    (size_t) (i - j), ps)) < 0)
+				    	return retval;
 				counter += retval;
 			}
 
@@ -574,20 +569,15 @@ static int printf_core(const char *fmt, struct printf_spec *ps, va_list ap)
 			/* String and character conversions */
 			case 's':
 				if ((retval = print_string(va_arg(ap, char *),
-				    width, precision, flags, ps)) < 0) {
-					counter = -counter;
-					goto out;
-				};
+				    width, precision, flags, ps)) < 0)
+				    	return retval;
 				counter += retval;
 				j = i + 1;
 				goto next_char;
 			case 'c':
 				c = va_arg(ap, unsigned int);
-				retval = print_char(c, width, flags, ps);
-				if (retval < 0) {
-					counter = -counter;
-					goto out;
-				};
+				if ((retval = print_char(c, width, flags, ps)) < 0)
+					return retval;
 				counter += retval;
 				j = i + 1;
 				goto next_char;
@@ -654,9 +644,6 @@ static int printf_core(const char *fmt, struct printf_spec *ps, va_list ap)
 				size = sizeof(void *);
 				number = (uint64_t) (unsigned long)va_arg(ap, void *);
 				break;
-			default:	/* Unknown qualifier */
-				counter = -counter;
-				goto out;
 			}
 
 			if (flags & __PRINTF_FLAG_SIGNED) {
@@ -674,10 +661,8 @@ static int printf_core(const char *fmt, struct printf_spec *ps, va_list ap)
 			}
 
 			if ((retval = print_number(number, width, precision,
-						   base, flags, ps)) < 0) {
-				counter = -counter;
-				goto out;
-			}
+						   base, flags, ps)) < 0)
+				return retval;
 
 			counter += retval;
 			j = i + 1;
@@ -688,15 +673,11 @@ next_char:
 
 	if (i > j) {
 		if ((retval = printf_putnchars(&fmt[j],
-		    (u64) (i - j), ps)) < 0) {
-			counter = -counter;
-			goto out;	/* Error */
-
-		}
+		    (u64) (i - j), ps)) < 0)
+		    	return retval;
 		counter += retval;
 	}
 
-out:
 	return counter;
 }
 
@@ -830,12 +811,8 @@ int printf(const char *fmt, ...)
 
 static int vprintf_write(const char *str, size_t count, void *unused)
 {
-	size_t i;
-
-	for (i = 0; i < count; i++)
-		putchar(str[i]);
-
-	return i;
+	console_write(str, count);
+	return count;
 }
 
 int vprintf(const char *fmt, va_list ap)