[5/6] console: rework flushing to the console driver

diff --git a/core/console-log.c b/core/console-log.c
index 642b39ca7988..0b8cef53fc64 100644
--- a/core/console-log.c
+++ b/core/console-log.c
@@ -26,6 +26,7 @@ 
 #include "stdio.h"
 #include "console.h"
 #include "timebase.h"
+#include "ctype.h"
 
 static int vprlog(int log_level, const char *fmt, va_list ap)
 {
@@ -44,6 +45,7 @@  static int vprlog(int log_level, const char *fmt, va_list ap)
 	if (log_level > (debug_descriptor.console_log_levels >> 4))
 		return 0;
 
+	/* if this changes parse_loghdr() needs to be updated too */
 	count = snprintf(buffer, sizeof(buffer), "[%5lu.%09lu,%d] ",
 			 tb_to_secs(tb), tb_remaining_nsecs(tb), log_level);
 	count+= vsnprintf(buffer+count, sizeof(buffer)-count, fmt, ap);
@@ -56,6 +58,75 @@  static int vprlog(int log_level, const char *fmt, va_list ap)
 	return count;
 }
 
+/*
+ * This parses a log entry to find it's log-level. If the log entry does not
+ * have a valid log header it'll return -1.
+ *
+ * d - run of decial digits
+ * l - log level digit
+ *
+ * Other characters are literals
+ */
+static const char pattern[] = "[d.d,l] ";
+#define LOGHDR_MAX (24 * 2 + 4)
+
+/*
+ * These functions live in console.c, but we need to access them from here.
+ * For parsing the log header.
+ */
+
+extern int conbuf_get(int offset);
+
+int __parse_loghdr(int offset, int *log_level);
+int __parse_loghdr(int offset, int *log_level)
+{
+	int i, run;
+
+	*log_level = -1;
+
+	for (i = 0; pattern[i]; i++) {
+		int c = conbuf_get(offset);
+		if (c < 0 || c == '\n')
+			return offset;
+
+		switch (pattern[i]) {
+		case 'd': /* match a run of digits/spaces */
+			for (run = 0; ; offset++, run++) {
+				c = conbuf_get(offset);
+				if (c < 0 || c == '\n' || run > LOGHDR_MAX)
+					return offset;
+
+				if (!isdigit(c) && !isspace(c))
+					break;
+			}
+
+			/* didn't match the pattern */
+			if (!run)
+				return offset;
+
+			break;
+
+		case 'l': /* extract log level */
+			if (!isdigit(c))
+				return offset;
+
+			*log_level = c - '0';
+			offset++;
+
+			break;
+
+		default: /* match a literal */
+			if (pattern[i] != c)
+				return offset;
+
+			offset++;
+		}
+	}
+
+	return offset;
+}
+
+
 /* we don't return anything as what on earth are we going to do
  * if we actually fail to print a log message? Print a log message about it?
  * Callers shouldn't care, prlog and friends should do something generically
diff --git a/core/console.c b/core/console.c
index b9129c9f3766..fbab5a38e21e 100644
--- a/core/console.c
+++ b/core/console.c
@@ -27,6 +27,15 @@ 
 #include <processor.h>
 #include <cpu.h>
 
+/*
+ * ring buffer pointers
+ *
+ * con_in = offset the next character will be written to
+ * con_out = offset of the next character to be flushed
+ *
+ * when con_in == con_out there is no data to be flushed
+ */
+
 static char *con_buf = (char *)INMEM_CON_START;
 static size_t con_in;
 static size_t con_out;
@@ -95,16 +104,50 @@  void clear_console(void)
 }
 
 /*
- * Flush the console buffer into the driver, returns true
- * if there is more to go.
- * Optionally can skip flushing to drivers, leaving messages
- * just in memory console.
+ * The prototypes here are because we want to use these in console-log.c
+ * The log header parsing lives in there, but we still need access to the
+ * log buffer.
+ */
+int conbuf_get(int offset);
+int conbuf_get(int offset)
+{
+	offset %= memcons.obuf_size;
+
+	if (offset == con_in)
+		return -1;
+
+	return con_buf[offset];
+}
+
+extern int __parse_loghdr(int offset, int *log_level);
+static int parse_loghdr(int start, int *log_level)
+{
+	int offset = __parse_loghdr(start, log_level);
+
+	while (1) {
+		int c = conbuf_get(offset);
+
+		if (c < 0)
+			break;
+		offset++;
+
+		if (c == '\n')
+			break;
+	}
+
+	return offset - start;
+}
+
+/*
+ * Flush the console buffer into the driver. Returns true
+ * if there is more to go, but that only happens when the
+ * underlying driver failed so don't call it again.
  */
-static bool __flush_console(bool flush_to_drivers)
+static bool __flush_console(bool flush_to_drivers __unused)
 {
+	int flush_lvl = debug_descriptor.console_log_levels & 0xf;
 	struct cpu_thread *cpu = this_cpu();
-	size_t req, len = 0;
-	static bool in_flush, more_flush;
+	static bool in_flush;
 
 	/* Is there anything to flush ? Bail out early if not */
 	if (con_in == con_out || !con_driver)
@@ -131,10 +174,9 @@  static bool __flush_console(bool flush_to_drivers)
 	 * concurrent attempts at flushing the same chunk of buffer
 	 * by other processors.
 	 */
-	if (in_flush) {
-		more_flush = true;
+	if (in_flush)
 		return false;
-	}
+
 	in_flush = true;
 
 	/*
@@ -148,25 +190,41 @@  static bool __flush_console(bool flush_to_drivers)
 	}
 
 	do {
-		more_flush = false;
-
-		if (con_out > con_in) {
-			req = INMEM_CON_OUT_LEN - con_out;
-			more_flush = true;
+		int start, req, len, log_lvl;
+
+		start = con_out;
+
+		/*
+		 * NB: Input that does not start with a valid log skiboot
+		 * log header is always flushed. This can happen due to
+		 * writes into the dummy OPAL console or because a line
+		 * was only partially flushed.
+		 */
+		req = parse_loghdr(start, &log_lvl);
+		if (log_lvl <= flush_lvl) {
+			/*
+			 * It this messages crosses the ring buffer edge then
+			 * truncate and write the rest on the next iteration.
+			 */
+			int end = (start + req) % memcons.obuf_size;
+			if (end < start)
+				req = memcons.obuf_size - start;
+
+			unlock(&con_lock);
+			len = con_driver->write(con_buf + con_out, req);
+			lock(&con_lock);
 		} else
-			req = con_in - con_out;
+			len = req;
 
-		unlock(&con_lock);
-		len = con_driver->write(con_buf + con_out, req);
-		lock(&con_lock);
+		con_out += len;
+		con_out %= memcons.obuf_size;
 
-		con_out = (con_out + len) % INMEM_CON_OUT_LEN;
-
-		/* write error? */
 		if (len < req)
-			break;
-	} while(more_flush);
+			goto bail;
+
+	} while (con_out != con_in);
 
+bail:
 	in_flush = false;
 	return con_out != con_in;
 }