From 3e8d63c29cb7b2943920ee8d1917b7309f06782c Mon Sep 17 00:00:00 2001
From: Joachim Wiberg <troglobit@gmail.com>
Date: Sun, 7 Feb 2021 13:15:46 +0100
Subject: [PATCH] Support for multiple (3) console= on kernel cmdline

Some targets, like Raspberry Pi, have both serial and HDMI console
enabled.  Depending on the order of arguments on the kernel cmdline
either one were selected the default 'stderr', which finit used.

This patch adds support for parsing the console= string(s) and
opening multiple outputs for system startup/progress.  In the
end, however, a getty still needs to be started on each of the
consoles to use.

Signed-off-by: Joachim Wiberg <troglobit@gmail.com>
---
 src/conf.c    |  3 +++
 src/helpers.c | 67 ++++++++++++++++++++++++++++++++++++++++++++++-----
 src/helpers.h |  3 +++
 3 files changed, 67 insertions(+), 6 deletions(-)

diff --git a/src/conf.c b/src/conf.c
index 4d9c95dc..5ed26ab9 100644
--- a/src/conf.c
+++ b/src/conf.c
@@ -89,6 +89,9 @@ static void parse_arg(char *arg, int *dbg)
 
 	if (string_compare(arg, "splash"))
 		splash = 1;
+
+	if (string_match(arg, "console="))
+		add_console(arg);
 }
 
 void conf_parse_cmdline(int argc, char *argv[])
diff --git a/src/helpers.c b/src/helpers.c
index 547eab10..89e32cd2 100644
--- a/src/helpers.c
+++ b/src/helpers.c
@@ -67,6 +67,62 @@ static const char *status2[] = STATUS_CLASS;
 #define CHOOSE(x,y,z) z
 static const char *color[] = STATUS_CLASS;
 
+/*
+ * System consoles, can be given multiple times on the kernel cmdline.
+ * We support only three currently.
+ */
+#define MAX_CONS 3
+
+static int fds[MAX_CONS];
+static int num_cons = 0;
+
+
+void add_console(char *str)
+{
+	char *cons, *ptr;
+	char path[80];
+
+	if (num_cons == NELEMS(fds)) {
+		_e("too many consoles, max %d supported", NELEMS(fds));
+		return;
+	}
+
+	cons = strchr(str, '=');
+	if (!cons)
+		return;
+	cons++;
+
+	/* drop any options */
+	ptr = strchr(cons, ',');
+	if (ptr)
+		*ptr = 0;
+
+	snprintf(path, sizeof(path), "/dev/%s", cons);
+	fds[num_cons] = open(path, O_WRONLY);
+	if (fds[num_cons] < 0) {
+		_pe("Failed opening console %s", path);
+		return;
+	}
+	num_cons++;
+}
+
+ssize_t cprintf(const char *fmt, ...)
+{
+	const size_t len = strlen(fmt) * 2;
+	char buf[len < 256 ? 256 : len];
+	size_t size;
+	va_list ap;
+	int i;
+
+	va_start(ap, fmt);
+	size = vsnprintf(buf, sizeof(buf), fmt, ap);
+	va_end(ap);
+
+	for (i = 0; i < num_cons; i++)
+		(void)write(fds[i], buf, size);
+
+	return size;
+}
 
 char *strip_line(char *line)
 {
@@ -162,8 +218,7 @@ void print_banner(const char *heading)
 		pad(buf, sizeof(buf), "═", wmax);
 	}
 	strlcat(buf, "\e[0m\n", sizeof(buf));
-
-	(void)write(STDERR_FILENO, buf, strlen(buf));
+	cprintf("%s", buf);
 }
 
 static size_t print_timestamp(char *buf, size_t len)
@@ -221,9 +276,9 @@ void printv(const char *fmt, va_list ap)
 	vsnprintf(&buf[len], sizeof(buf) - len, fmt, ap);
 
 	if (progress_style == 1)
-		fprintf(stderr, "\r%s ", pad(buf, sizeof(buf), ".", sizeof(buf)));
+		cprintf("\r%s ", pad(buf, sizeof(buf), ".", sizeof(buf)));
 	else
-		fprintf(stderr, "\r\e[2K%s%s", status(3), buf);
+		cprintf("\r\e[2K%s%s", status(3), buf);
 }
 
 void print(int rc, const char *fmt, ...)
@@ -243,9 +298,9 @@ void print(int rc, const char *fmt, ...)
 		return;
 
 	if (progress_style == 1)
-		fprintf(stderr, "%s\n", status(rc));
+		cprintf("%s\n", status(rc));
 	else
-		fprintf(stderr, "\r%s\n", status(rc));
+		cprintf("\r%s\n", status(rc));
 }
 
 void print_desc(char *action, char *desc)
diff --git a/src/helpers.h b/src/helpers.h
index 0fff3428..68d6112e 100644
--- a/src/helpers.h
+++ b/src/helpers.h
@@ -50,6 +50,9 @@ void          endfsent  (void);
 struct fstab *getfsent  (void);
 #endif	/* HAVE_GETFSENT */
 
+void    add_console     (char *str);
+ssize_t cprintf         (const char *fmt, ...);
+
 char   *strip_line      (char *line);
 
 int     getty           (char *tty, speed_t speed, char *term, char *user);