s390: provide sclp based boot console

Use the early sclp code to provide a boot console. This boot console
is available if the kernel parameter "earlyprintk" has been specified,
just like it works for other architectures that also provide an early
boot console.

This makes debugging of early problems much easier, since now we
finally have working console output even before memory detection is
running.

The boot console will be automatically disabled as soon as another
console will be registered.

Reviewed-by: Peter Oberparleiter <oberpar@linux.vnet.ibm.com>
Signed-off-by: Heiko Carstens <heiko.carstens@de.ibm.com>
Signed-off-by: Martin Schwidefsky <schwidefsky@de.ibm.com>

3 files changed, 52 insertions, 10 deletions

diff --git a/arch/s390/kernel/Makefile b/arch/s390/kernel/Makefile
index 36b5101c8606..edbc62e04027 100644
--- a/arch/s390/kernel/Makefile
+++ b/arch/s390/kernel/Makefile
@@ -76,7 +76,7 @@ obj-$(CONFIG_AUDIT)		+= audit.o
 compat-obj-$(CONFIG_AUDIT)	+= compat_audit.o
 obj-$(CONFIG_COMPAT)		+= compat_linux.o compat_signal.o
 obj-$(CONFIG_COMPAT)		+= compat_wrapper.o $(compat-obj-y)
-
+obj-$(CONFIG_EARLY_PRINTK)	+= early_printk.o
 obj-$(CONFIG_STACKTRACE)	+= stacktrace.o
 obj-$(CONFIG_KPROBES)		+= kprobes.o
 obj-$(CONFIG_FUNCTION_TRACER)	+= mcount.o ftrace.o
diff --git a/arch/s390/kernel/early_printk.c b/arch/s390/kernel/early_printk.c
new file mode 100644
index 000000000000..54a4dc582b81
--- /dev/null
+++ b/arch/s390/kernel/early_printk.c
@@ -0,0 +1,35 @@
+/*
+ *    Copyright IBM Corp. 2017
+ */
+
+#include <linux/console.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <asm/sclp.h>
+
+static void sclp_early_write(struct console *con, const char *s, unsigned int len)
+{
+	__sclp_print_early(s, len);
+}
+
+static struct console sclp_early_console = {
+	.name  = "earlysclp",
+	.write = sclp_early_write,
+	.flags = CON_PRINTBUFFER | CON_BOOT,
+	.index = -1,
+};
+
+static int __init setup_early_printk(char *buf)
+{
+	if (early_console)
+		return 0;
+	/* Accept only "earlyprintk" and "earlyprintk=sclp" */
+	if (buf && strncmp(buf, "sclp", 4))
+		return 0;
+	if (!sclp.has_linemode && !sclp.has_vt220)
+		return 0;
+	early_console = &sclp_early_console;
+	register_console(early_console);
+	return 0;
+}
+early_param("earlyprintk", setup_early_printk);
diff --git a/arch/s390/kernel/sclp.c b/arch/s390/kernel/sclp.c
index 53e391fe8577..f9c5b02d2685 100644
--- a/arch/s390/kernel/sclp.c
+++ b/arch/s390/kernel/sclp.c
@@ -98,7 +98,7 @@ static int _sclp_setup(int disable)
 }
 
 /* Output multi-line text using SCLP Message interface. */
-static void _sclp_print_lm(const char *str)
+static void _sclp_print_lm(const char *str, unsigned int len)
 {
 	static unsigned char write_head[] = {
 		/* sccb header */
@@ -133,8 +133,9 @@ static void _sclp_print_lm(const char *str)
 		0x00, 0x00, 0x00, 0x00				/* 6 */
 	};
 	unsigned char *ptr, *end_ptr, ch;
-	unsigned int count;
+	unsigned int count, num;
 
+	num = 0;
 	memcpy(_sclp_work_area, write_head, sizeof(write_head));
 	ptr = _sclp_work_area + sizeof(write_head);
 	end_ptr = _sclp_work_area + sizeof(_sclp_work_area) - 1;
@@ -142,7 +143,9 @@ static void _sclp_print_lm(const char *str)
 		if (ptr + sizeof(write_mto) > end_ptr)
 			break;
 		memcpy(ptr, write_mto, sizeof(write_mto));
-		for (count = sizeof(write_mto); (ch = *str++) != 0; count++) {
+		for (count = sizeof(write_mto); num < len; count++) {
+			num++;
+			ch = *str++;
 			if (ch == 0x0a)
 				break;
 			if (ptr > end_ptr)
@@ -155,7 +158,7 @@ static void _sclp_print_lm(const char *str)
 		*(unsigned short *)(_sclp_work_area + 8) += count;
 		*(unsigned short *)(_sclp_work_area + 0) += count;
 		ptr += count;
-	} while (ch != 0);
+	} while (num < len);
 
 	/* SCLP write data */
 	_sclp_servc(0x00760005, _sclp_work_area);
@@ -164,7 +167,7 @@ static void _sclp_print_lm(const char *str)
 /* Output multi-line text (plus a newline) using SCLP VT220
  * interface.
  */
-static void _sclp_print_vt220(const char *str)
+static void _sclp_print_vt220(const char *str, unsigned int len)
 {
 	static unsigned char const write_head[] = {
 		/* sccb header */
@@ -174,7 +177,6 @@ static void _sclp_print_vt220(const char *str)
 		0x00, 0x06,
 		0x1a, 0x00, 0x00, 0x00,
 	};
-	size_t len = strlen(str);
 
 	if (sizeof(write_head) + len >= sizeof(_sclp_work_area))
 		len = sizeof(_sclp_work_area) - sizeof(write_head) - 1;
@@ -194,13 +196,18 @@ static void _sclp_print_vt220(const char *str)
 /* Output one or more lines of text on the SCLP console (VT220 and /
  * or line-mode). All lines get terminated; no need for a trailing LF.
  */
-void _sclp_print_early(const char *str)
+void __sclp_print_early(const char *str, unsigned int len)
 {
 	if (_sclp_setup(0) != 0)
 		return;
 	if (have_linemode)
-		_sclp_print_lm(str);
+		_sclp_print_lm(str, len);
 	if (have_vt220)
-		_sclp_print_vt220(str);
+		_sclp_print_vt220(str, len);
 	_sclp_setup(1);
 }
+
+void _sclp_print_early(const char *str)
+{
+	__sclp_print_early(str, strlen(str));
+}