s390/sclp: move early printk code to drivers

Move the early sclp printk code to the drivers folder where also the
rest of the sclp code can be found. This way it is possible to use the
sclp private header files for further cleanups.

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>

2 files changed, 228 insertions, 1 deletions

diff --git a/drivers/s390/char/Makefile b/drivers/s390/char/Makefile
index 41e28b23b26a..0c443e26835d 100644
--- a/drivers/s390/char/Makefile
+++ b/drivers/s390/char/Makefile
@@ -2,9 +2,23 @@
 # S/390 character devices
 #
 
+ifdef CONFIG_FUNCTION_TRACER
+# Do not trace early setup code
+CFLAGS_REMOVE_sclp_early_core.o	= $(CC_FLAGS_FTRACE)
+endif
+
+GCOV_PROFILE_sclp_early_core.o		:= n
+KCOV_INSTRUMENT_sclp_early_core.o	:= n
+UBSAN_SANITIZE_sclp_early_core.o	:= n
+
+ifneq ($(CC_FLAGS_MARCH),-march=z900)
+CFLAGS_REMOVE_sclp_early_core.o	+= $(CC_FLAGS_MARCH)
+CFLAGS_sclp_early_core.o		+= -march=z900
+endif
+
 obj-y += ctrlchar.o keyboard.o defkeymap.o sclp.o sclp_rw.o sclp_quiesce.o \
 	 sclp_cmd.o sclp_config.o sclp_cpi_sys.o sclp_ocf.o sclp_ctl.o \
-	 sclp_early.o
+	 sclp_early.o sclp_early_core.o
 
 obj-$(CONFIG_TN3270) += raw3270.o
 obj-$(CONFIG_TN3270_CONSOLE) += con3270.o
diff --git a/drivers/s390/char/sclp_early_core.c b/drivers/s390/char/sclp_early_core.c
new file mode 100644
index 000000000000..f9c5b02d2685
--- /dev/null
+++ b/drivers/s390/char/sclp_early_core.c
@@ -0,0 +1,213 @@
+/*
+ *    Copyright IBM Corp. 2015
+ *    Author(s): Martin Schwidefsky <schwidefsky@de.ibm.com>
+ */
+#include <linux/kernel.h>
+#include <asm/ebcdic.h>
+#include <asm/irq.h>
+#include <asm/lowcore.h>
+#include <asm/processor.h>
+#include <asm/sclp.h>
+
+#define EVTYP_VT220MSG_MASK	0x00000040
+#define EVTYP_MSG_MASK		0x40000000
+
+static char _sclp_work_area[4096] __aligned(PAGE_SIZE) __section(data);
+static bool have_vt220 __section(data);
+static bool have_linemode __section(data);
+
+static void _sclp_wait_int(void)
+{
+	unsigned long psw_mask, addr, flags;
+	psw_t psw_ext_save, psw_wait;
+	union ctlreg0 cr0, cr0_new;
+
+	raw_local_irq_save(flags);
+	__ctl_store(cr0.val, 0, 0);
+	cr0_new.val = cr0.val & ~CR0_IRQ_SUBCLASS_MASK;
+	cr0_new.lap = 0;
+	cr0_new.sssm = 1;
+	__ctl_load(cr0_new.val, 0, 0);
+
+	psw_ext_save = S390_lowcore.external_new_psw;
+	psw_mask = __extract_psw();
+	S390_lowcore.external_new_psw.mask = psw_mask;
+	psw_wait.mask = psw_mask | PSW_MASK_EXT | PSW_MASK_WAIT;
+	S390_lowcore.ext_int_code = 0;
+
+	do {
+		asm volatile(
+			"	larl	%[addr],0f\n"
+			"	stg	%[addr],%[psw_wait_addr]\n"
+			"	stg	%[addr],%[psw_ext_addr]\n"
+			"	lpswe	%[psw_wait]\n"
+			"0:\n"
+			: [addr] "=&d" (addr),
+			  [psw_wait_addr] "=Q" (psw_wait.addr),
+			  [psw_ext_addr] "=Q" (S390_lowcore.external_new_psw.addr)
+			: [psw_wait] "Q" (psw_wait)
+			: "cc", "memory");
+	} while (S390_lowcore.ext_int_code != EXT_IRQ_SERVICE_SIG);
+
+	S390_lowcore.external_new_psw = psw_ext_save;
+	__ctl_load(cr0.val, 0, 0);
+	raw_local_irq_restore(flags);
+}
+
+static int _sclp_servc(unsigned int cmd, char *sccb)
+{
+	unsigned int cc;
+
+	do {
+		asm volatile(
+			"	.insn	rre,0xb2200000,%1,%2\n"
+			"	ipm	%0\n"
+			: "=d" (cc) : "d" (cmd), "a" (sccb)
+			: "cc", "memory");
+		cc >>= 28;
+		if (cc == 3)
+			return -EINVAL;
+		_sclp_wait_int();
+	} while (cc != 0);
+	return (*(unsigned short *)(sccb + 6) == 0x20) ? 0 : -EIO;
+}
+
+static int _sclp_setup(int disable)
+{
+	static unsigned char init_sccb[] = {
+		0x00, 0x1c,
+		0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00,
+		0x00, 0x04,
+		0x80, 0x00, 0x00, 0x00,	0x40, 0x00, 0x00, 0x40,
+		0x00, 0x00, 0x00, 0x00,	0x00, 0x00, 0x00, 0x00
+	};
+	unsigned int *masks;
+	int rc;
+
+	memcpy(_sclp_work_area, init_sccb, 28);
+	masks = (unsigned int *)(_sclp_work_area + 12);
+	if (disable)
+		memset(masks, 0, 16);
+	/* SCLP write mask */
+	rc = _sclp_servc(0x00780005, _sclp_work_area);
+	if (rc)
+		return rc;
+	have_vt220 = masks[2] & EVTYP_VT220MSG_MASK;
+	have_linemode = masks[2] & EVTYP_MSG_MASK;
+	return 0;
+}
+
+/* Output multi-line text using SCLP Message interface. */
+static void _sclp_print_lm(const char *str, unsigned int len)
+{
+	static unsigned char write_head[] = {
+		/* sccb header */
+		0x00, 0x52,					/* 0 */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00,		/* 2 */
+		/* evbuf */
+		0x00, 0x4a,					/* 8 */
+		0x02, 0x00, 0x00, 0x00,				/* 10 */
+		/* mdb */
+		0x00, 0x44,					/* 14 */
+		0x00, 0x01,					/* 16 */
+		0xd4, 0xc4, 0xc2, 0x40,				/* 18 */
+		0x00, 0x00, 0x00, 0x01,				/* 22 */
+		/* go */
+		0x00, 0x38,					/* 26 */
+		0x00, 0x01,					/* 28 */
+		0x00, 0x00, 0x00, 0x00,				/* 30 */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	/* 34 */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	/* 42 */
+		0x00, 0x00, 0x00, 0x00,				/* 50 */
+		0x00, 0x00,					/* 54 */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	/* 56 */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	/* 64 */
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,	/* 72 */
+		0x00, 0x00,					/* 80 */
+	};
+	static unsigned char write_mto[] = {
+		/* mto	*/
+		0x00, 0x0a,					/* 0 */
+		0x00, 0x04,					/* 2 */
+		0x10, 0x00,					/* 4 */
+		0x00, 0x00, 0x00, 0x00				/* 6 */
+	};
+	unsigned char *ptr, *end_ptr, ch;
+	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;
+	do {
+		if (ptr + sizeof(write_mto) > end_ptr)
+			break;
+		memcpy(ptr, write_mto, sizeof(write_mto));
+		for (count = sizeof(write_mto); num < len; count++) {
+			num++;
+			ch = *str++;
+			if (ch == 0x0a)
+				break;
+			if (ptr > end_ptr)
+				break;
+			ptr[count] = _ascebc[ch];
+		}
+		/* Update length fields in mto, mdb, evbuf and sccb */
+		*(unsigned short *) ptr = count;
+		*(unsigned short *)(_sclp_work_area + 14) += count;
+		*(unsigned short *)(_sclp_work_area + 8) += count;
+		*(unsigned short *)(_sclp_work_area + 0) += count;
+		ptr += count;
+	} while (num < len);
+
+	/* SCLP write data */
+	_sclp_servc(0x00760005, _sclp_work_area);
+}
+
+/* Output multi-line text (plus a newline) using SCLP VT220
+ * interface.
+ */
+static void _sclp_print_vt220(const char *str, unsigned int len)
+{
+	static unsigned char const write_head[] = {
+		/* sccb header */
+		0x00, 0x0e,
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		/* evbuf header */
+		0x00, 0x06,
+		0x1a, 0x00, 0x00, 0x00,
+	};
+
+	if (sizeof(write_head) + len >= sizeof(_sclp_work_area))
+		len = sizeof(_sclp_work_area) - sizeof(write_head) - 1;
+
+	memcpy(_sclp_work_area, write_head, sizeof(write_head));
+	memcpy(_sclp_work_area + sizeof(write_head), str, len);
+	_sclp_work_area[sizeof(write_head) + len] = '\n';
+
+	/* Update length fields in evbuf and sccb headers */
+	*(unsigned short *)(_sclp_work_area + 8) += len + 1;
+	*(unsigned short *)(_sclp_work_area + 0) += len + 1;
+
+	/* SCLP write data */
+	(void)_sclp_servc(0x00760005, _sclp_work_area);
+}
+
+/* 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, unsigned int len)
+{
+	if (_sclp_setup(0) != 0)
+		return;
+	if (have_linemode)
+		_sclp_print_lm(str, len);
+	if (have_vt220)
+		_sclp_print_vt220(str, len);
+	_sclp_setup(1);
+}
+
+void _sclp_print_early(const char *str)
+{
+	__sclp_print_early(str, strlen(str));
+}