[PATCH 1/3] riscv: Support for early console.

[Jinglin Wen]

This feature is mainly used for debugging during the early startup
process. Currently, the implementation of this function is based
on the sbi interface.

By setting the CONFIG_RISCV_EARLY_CONSOLE option, this
function can be enabled, which subsequently sets the log level
to CONSOLE_LOGLEVEL_MOTORMOUT.

Signed-off-by: Jinglin Wen <jinglin.wen at shingroup.cn>
---
 arch/riscv/include/asm/early_console.h |  23 ++++++
 arch/riscv/kernel/Makefile             |   1 +
 arch/riscv/kernel/early_console.c      | 108 +++++++++++++++++++++++++
 arch/riscv/kernel/setup.c              |   2 +
 4 files changed, 134 insertions(+)
 create mode 100644 arch/riscv/include/asm/early_console.h
 create mode 100644 arch/riscv/kernel/early_console.c

diff --git a/arch/riscv/include/asm/early_console.h b/arch/riscv/include/asm/early_console.h
new file mode 100644
index 000000000000..0683a42e9207
--- /dev/null
+++ b/arch/riscv/include/asm/early_console.h
@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
+#ifndef _ASM_EARLY_CONSOLE_H
+#define _ASM_EARLY_CONSOLE_H
+#ifdef __KERNEL__
+
+#include <linux/compiler.h>
+#include <linux/init.h>
+
+void __init early_console_init(void);
+
+/* early_console libs */
+void early_console_puts(const char *s);
+int early_console_write(const char *s, int n);
+void early_console_printf(const char *fmt, ...);
+void early_console_progress(char *s, unsigned short hex);
+
+#ifdef CONFIG_RISCV_EARLY_CONSOLE_SBI
+void __init hvc_sbi_early_init(void (**putc)(char c));
+#endif /* CONFIG_HVC_RISCV_SBI */
+
+#endif /* __KERNEL__ */
+#endif /* _ASM_EARLY_CONSOLE_H */
diff --git a/arch/riscv/kernel/Makefile b/arch/riscv/kernel/Makefile
index 81d94a8ee10f..ef037e3762f1 100644
--- a/arch/riscv/kernel/Makefile
+++ b/arch/riscv/kernel/Makefile
@@ -48,6 +48,7 @@ obj-y	+= ptrace.o
 obj-y	+= reset.o
 obj-y	+= return_address.o
 obj-y	+= setup.o
+obj-y	+= early_console.o
 obj-y	+= signal.o
 obj-y	+= syscall_table.o
 obj-y	+= sys_riscv.o
diff --git a/arch/riscv/kernel/early_console.c b/arch/riscv/kernel/early_console.c
new file mode 100644
index 000000000000..64f3a5705413
--- /dev/null
+++ b/arch/riscv/kernel/early_console.c
@@ -0,0 +1,108 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Early console support for RISCV
+ */
+
+#include <linux/stdarg.h>
+#include <linux/types.h>
+#include <linux/console.h>
+#include <asm/sbi.h>
+#include <asm/early_console.h>
+
+/* interface for early console output characters */
+void (*riscv_early_console_putc)(char c);
+
+void early_console_puts(const char *s)
+{
+	if (riscv_early_console_putc) {
+		char c;
+
+		if (s && *s != '\0') {
+			while ((c = *s++) != '\0')
+				riscv_early_console_putc(c);
+		}
+	}
+}
+
+int early_console_write(const char *s, int n)
+{
+	int remain = n;
+	char c;
+
+	if (!riscv_early_console_putc)
+		return 0;
+
+	if (s && *s != '\0') {
+		while (((c = *s++) != '\0') && (remain-- > 0))
+			riscv_early_console_putc(c);
+	}
+
+	return n - remain;
+}
+
+#define EARLY_CONSOLE_BUFSIZE 256
+void early_console_printf(const char *fmt, ...)
+{
+	if (riscv_early_console_putc) {
+		char buf[EARLY_CONSOLE_BUFSIZE];
+		va_list args;
+
+		va_start(args, fmt);
+		vsnprintf(buf, EARLY_CONSOLE_BUFSIZE, fmt, args);
+		early_console_puts(buf);
+		va_end(args);
+	}
+}
+
+void __init early_console_progress(char *s, unsigned short hex)
+{
+	early_console_puts(s);
+	early_console_puts("\n");
+}
+
+/*
+ * Console based on early console
+ */
+static void riscv_early_console_write(struct console *con, const char *s,
+		unsigned int n)
+{
+	early_console_write(s, n);
+}
+
+static struct console riscv_early_console = {
+	.name	= "riscv_early_con",
+	.write	= riscv_early_console_write,
+	.flags	= CON_PRINTBUFFER | CON_ENABLED | CON_BOOT | CON_ANYTIME,
+	.index	= 0,
+};
+
+static void __init register_early_console(void)
+{
+	if (!riscv_early_console_putc)
+		return;
+
+	add_preferred_console("riscv_early_con", 0, NULL);
+	register_console(&riscv_early_console);
+}
+
+/*
+ * This is called after sbi_init.
+ */
+void __init early_console_init(void)
+{
+	/*
+	 * Set riscv_early_console_putc.
+	 * If there are other output interfaces, you can add corresponding code
+	 * to initialize riscv_early_console_putc.
+	 */
+#if defined(CONFIG_RISCV_EARLY_CONSOLE_SBI)
+	/* using the sbi */
+	hvc_sbi_early_init(&riscv_early_console_putc);
+#else
+	/* using other */
+#endif
+
+	console_loglevel = CONSOLE_LOGLEVEL_MOTORMOUTH;
+	register_early_console();
+}
+
diff --git a/arch/riscv/kernel/setup.c b/arch/riscv/kernel/setup.c
index 4f73c0ae44b2..1b48630f0861 100644
--- a/arch/riscv/kernel/setup.c
+++ b/arch/riscv/kernel/setup.c
@@ -36,6 +36,7 @@
 #include <asm/thread_info.h>
 #include <asm/kasan.h>
 #include <asm/efi.h>
+#include <asm/early_console.h>
 
 #include "head.h"
 
@@ -255,6 +256,7 @@ void __init setup_arch(char **cmdline_p)
 
 	early_ioremap_setup();
 	sbi_init();
+	early_console_init();
 	jump_label_init();
 	parse_early_param();
 
-- 
2.25.1