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));
+}