Patchwork [kvm-unit-tests,4/5] s390x: Use interrupts in SCLP and add line mode

login
register
mail settings
Submitter Janosch Frank
Date Dec. 4, 2018, 1:48 p.m.
Message ID <20181204134838.5841-5-frankja@linux.ibm.com>
Download mbox | patch
Permalink /patch/671909/
State New
Headers show

Comments

Janosch Frank - Dec. 4, 2018, 1:48 p.m.
z/VM isn't fond of vt220, so we need line mode when running under z/VM.

Also we need to properly implement interrupt handling for SCLP,
because on z/VM and LPAR SCLP calls are not synchronous!

Signed-off-by: Janosch Frank <frankja@linux.ibm.com>
---
 lib/s390x/asm/arch_def.h  |   1 +
 lib/s390x/asm/interrupt.h |   2 +
 lib/s390x/interrupt.c     |  12 ++-
 lib/s390x/io.c            |   2 +-
 lib/s390x/sclp-ascii.c    | 222 ++++++++++++++++++++++++++++++++++++++--------
 lib/s390x/sclp.c          |  42 +++++++++
 lib/s390x/sclp.h          |  71 ++++++++++++++-
 7 files changed, 311 insertions(+), 41 deletions(-)
David Hildenbrand - Dec. 4, 2018, 5:08 p.m.
On 04.12.18 14:48, Janosch Frank wrote:
> z/VM isn't fond of vt220, so we need line mode when running under z/VM.
> 
> Also we need to properly implement interrupt handling for SCLP,
> because on z/VM and LPAR SCLP calls are not synchronous!

Can you fix the latter in a separate patch? (makes it easier to review)

> 
> Signed-off-by: Janosch Frank <frankja@linux.ibm.com>
> ---
>  lib/s390x/asm/arch_def.h  |   1 +
>  lib/s390x/asm/interrupt.h |   2 +
>  lib/s390x/interrupt.c     |  12 ++-
>  lib/s390x/io.c            |   2 +-
>  lib/s390x/sclp-ascii.c    | 222 ++++++++++++++++++++++++++++++++++++++--------
>  lib/s390x/sclp.c          |  42 +++++++++
>  lib/s390x/sclp.h          |  71 ++++++++++++++-
>  7 files changed, 311 insertions(+), 41 deletions(-)
> 
> diff --git a/lib/s390x/asm/arch_def.h b/lib/s390x/asm/arch_def.h
> index d2cd727..4bbb428 100644
> --- a/lib/s390x/asm/arch_def.h
> +++ b/lib/s390x/asm/arch_def.h
> @@ -15,6 +15,7 @@ struct psw {
>  	uint64_t	addr;
>  };
>  
> +#define PSW_MASK_EXT			0x0100000000000000UL
>  #define PSW_MASK_DAT			0x0400000000000000UL
>  #define PSW_MASK_PSTATE			0x0001000000000000UL
>  
> diff --git a/lib/s390x/asm/interrupt.h b/lib/s390x/asm/interrupt.h
> index 013709f..de15d9e 100644
> --- a/lib/s390x/asm/interrupt.h
> +++ b/lib/s390x/asm/interrupt.h
> @@ -11,6 +11,8 @@
>  #define _ASMS390X_IRQ_H_
>  #include <asm/arch_def.h>
>  
> +#define EXT_IRQ_SERVICE_SIG 0x2401
> +
>  void handle_pgm_int(void);
>  void handle_ext_int(void);
>  void handle_mcck_int(void);
> diff --git a/lib/s390x/interrupt.c b/lib/s390x/interrupt.c
> index cf0a794..7118577 100644
> --- a/lib/s390x/interrupt.c
> +++ b/lib/s390x/interrupt.c
> @@ -12,6 +12,7 @@
>  #include <libcflat.h>
>  #include <asm/interrupt.h>
>  #include <asm/barrier.h>
> +#include <sclp.h>
>  
>  static bool pgm_int_expected;
>  static struct lowcore *lc;
> @@ -107,8 +108,15 @@ void handle_pgm_int(void)
>  
>  void handle_ext_int(void)
>  {
> -	report_abort("Unexpected external call interrupt: at %#lx",
> -		     lc->ext_old_psw.addr);
> +	if (lc->ext_int_code != EXT_IRQ_SERVICE_SIG)
> +		report_abort("Unexpected external call interrupt: at %#lx",
> +			     lc->ext_old_psw.addr);
> +	else {
> +		lc->ext_old_psw.mask &= ~PSW_MASK_EXT;
> +		lc->sw_int_cr0 &= ~(1UL << 9);
> +		sclp_handle_ext();
> +		lc->ext_int_code = 0;
> +	}
>  }
>  
>  void handle_mcck_int(void)
> diff --git a/lib/s390x/io.c b/lib/s390x/io.c
> index 7bca637..72041ed 100644
> --- a/lib/s390x/io.c
> +++ b/lib/s390x/io.c
> @@ -44,8 +44,8 @@ void setup(void)
>  {
>  	setup_args_progname(ipl_args);
>  	setup_facilities();
> -	sclp_ascii_setup();
>  	sclp_memory_setup();
> +	sclp_ascii_setup();
>  }
>  
>  void exit(int code)
> diff --git a/lib/s390x/sclp-ascii.c b/lib/s390x/sclp-ascii.c
> index 893ca17..5e23207 100644
> --- a/lib/s390x/sclp-ascii.c
> +++ b/lib/s390x/sclp-ascii.c
> @@ -11,58 +11,206 @@
>  #include <libcflat.h>
>  #include <string.h>
>  #include <asm/page.h>
> +#include <asm/arch_def.h>
> +#include <asm/io.h>
>  #include "sclp.h"
>  
> -char _sccb[PAGE_SIZE] __attribute__((__aligned__(4096)));
>  
> -/* Perform service call. Return 0 on success, non-zero otherwise. */
> -int sclp_service_call(unsigned int command, void *sccb)
> +/*
> + * ASCII (IBM PC 437) -> EBCDIC 037
> + */
> +static uint8_t _ascebc[256] =
>  {
> -        int cc;
> + /*00 NUL   SOH   STX   ETX   EOT   ENQ   ACK   BEL */
> +     0x00, 0x01, 0x02, 0x03, 0x37, 0x2D, 0x2E, 0x2F,
> + /*08  BS    HT    LF    VT    FF    CR    SO    SI */
> + /*              ->NL                               */
> +     0x16, 0x05, 0x15, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
> + /*10 DLE   DC1   DC2   DC3   DC4   NAK   SYN   ETB */
> +     0x10, 0x11, 0x12, 0x13, 0x3C, 0x3D, 0x32, 0x26,
> + /*18 CAN    EM   SUB   ESC    FS    GS    RS    US */
> + /*                               ->IGS ->IRS ->IUS */
> +     0x18, 0x19, 0x3F, 0x27, 0x22, 0x1D, 0x1E, 0x1F,
> + /*20  SP     !     "     #     $     %     &     ' */
> +     0x40, 0x5A, 0x7F, 0x7B, 0x5B, 0x6C, 0x50, 0x7D,
> + /*28   (     )     *     +     ,     -    .      / */
> +     0x4D, 0x5D, 0x5C, 0x4E, 0x6B, 0x60, 0x4B, 0x61,
> + /*30   0     1     2     3     4     5     6     7 */
> +     0xF0, 0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7,
> + /*38   8     9     :     ;     <     =     >     ? */
> +     0xF8, 0xF9, 0x7A, 0x5E, 0x4C, 0x7E, 0x6E, 0x6F,
> + /*40   @     A     B     C     D     E     F     G */
> +     0x7C, 0xC1, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7,
> + /*48   H     I     J     K     L     M     N     O */
> +     0xC8, 0xC9, 0xD1, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6,
> + /*50   P     Q     R     S     T     U     V     W */
> +     0xD7, 0xD8, 0xD9, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6,
> + /*58   X     Y     Z     [     \     ]     ^     _ */
> +     0xE7, 0xE8, 0xE9, 0xBA, 0xE0, 0xBB, 0xB0, 0x6D,
> + /*60   `     a     b     c     d     e     f     g */
> +     0x79, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
> + /*68   h     i     j     k     l     m     n     o */
> +     0x88, 0x89, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96,
> + /*70   p     q     r     s     t     u     v     w */
> +     0x97, 0x98, 0x99, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6,
> + /*78   x     y     z     {     |     }     ~    DL */
> +     0xA7, 0xA8, 0xA9, 0xC0, 0x4F, 0xD0, 0xA1, 0x07,
> + /*80*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*88*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*90*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*98*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*A0*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*A8*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*B0*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*B8*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*C0*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*C8*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*D0*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*D8*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*E0        sz						*/
> +     0x3F, 0x59, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*E8*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*F0*/
> +     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
> + /*F8*/
> +     0x90, 0x3F, 0x3F, 0x3F, 0x3F, 0xEA, 0x3F, 0xFF
> +};
>  
> -        asm volatile(
> -                "       .insn   rre,0xb2200000,%1,%2\n"  /* servc %1,%2 */
> -                "       ipm     %0\n"
> -                "       srl     %0,28"
> -                : "=&d" (cc) : "d" (command), "a" (__pa(sccb))
> -                : "cc", "memory");
> -        if (cc == 3)
> -                return -1;
> -        if (cc == 2)
> -                return -1;
> -        return 0;
> -}
> +static bool initialized = false;
>  
> -static void sclp_set_write_mask(void)
> -{
> -    WriteEventMask *sccb = (void *)_sccb;
> -
> -    sccb->h.length = sizeof(WriteEventMask);
> -    sccb->mask_length = sizeof(unsigned int);
> -    sccb->receive_mask = SCLP_EVENT_MASK_MSG_ASCII;
> -    sccb->cp_receive_mask = SCLP_EVENT_MASK_MSG_ASCII;
> -    sccb->send_mask = SCLP_EVENT_MASK_MSG_ASCII;
> -    sccb->cp_send_mask = SCLP_EVENT_MASK_MSG_ASCII;
> -
> -    sclp_service_call(SCLP_CMD_WRITE_EVENT_MASK, sccb);
> -}
> -
> -void sclp_ascii_setup(void)
> -{
> -    sclp_set_write_mask();
> -}
> -
> -void sclp_print(const char *str)
> +static void sclp_print_ascii(const char *str)
>  {
>      int len = strlen(str);
>      WriteEventData *sccb = (void *)_sccb;
>  
> +    while (sclp_busy)
> +	    /* Wait for SCLP request to complete */;
> +    sclp_busy = true;
> +    memset(sccb, 0, sizeof(*sccb));
>      sccb->h.length = sizeof(WriteEventData) + len;
>      sccb->h.function_code = SCLP_FC_NORMAL_WRITE;
>      sccb->ebh.length = sizeof(EventBufferHeader) + len;
>      sccb->ebh.type = SCLP_EVENT_ASCII_CONSOLE_DATA;
> -    sccb->ebh.flags = 0;
>      memcpy(sccb->data, str, len);
>  
>      sclp_service_call(SCLP_CMD_WRITE_EVENT_DATA, sccb);
>  }
> +
> +static void sclp_print_lm(const char *str)
> +{
> +	unsigned char *ptr, *end, ch;
> +	unsigned int count, offset, len;
> +	struct write_sccb *sccb;
> +	struct msg_buf *msg;
> +	struct mdb *mdb;
> +	struct mto *mto;
> +	struct go *go;
> +
> +	while (sclp_busy)
> +		/* Wait for SCLP request to complete */;
> +	sclp_busy = true;
> +	sccb = (struct write_sccb *) _sccb;
> +	end = (unsigned char *) sccb + 4096 - 1;
> +	memset(sccb, 0, sizeof(*sccb));
> +	ptr = (unsigned char *) &sccb->msg.mdb.mto;
> +	len = strlen(str);
> +	offset = 0;
> +	do {
> +		for (count = sizeof(*mto); offset < len; count++) {
> +			ch = str[offset++];
> +			if ((ch == 0x0a) || (ptr + count > end))
> +				break;
> +			ptr[count] = _ascebc[ch];
> +		}
> +		mto = (struct mto *) ptr;
> +		memset(mto, 0, sizeof(*mto));
> +		mto->length = count;
> +		mto->type = 4;
> +		mto->line_type_flags = LNTPFLGS_ENDTEXT;
> +		ptr += count;
> +	} while ((offset < len) && (ptr + sizeof(*mto) <= end));
> +	len = ptr - (unsigned char *) sccb;
> +	sccb->header.length = len - offsetof(struct write_sccb, header);
> +	msg = &sccb->msg;
> +	msg->header.type = EVTYP_MSG;
> +	msg->header.length = len - offsetof(struct write_sccb, msg.header);
> +	mdb = &msg->mdb;
> +	mdb->header.type = 1;
> +	mdb->header.tag = 0xD4C4C240;
> +	mdb->header.revision_code = 1;
> +	mdb->header.length = len - offsetof(struct write_sccb, msg.mdb.header);
> +	go = &mdb->go;
> +	go->length = sizeof(*go);
> +	go->type = 1;
> +	sclp_service_call(SCLP_CMD_WRITE_EVENT_DATA, sccb);
> +}
> +
> +/*
> + * SCLP needs to be initialized by setting a send and receive mask,
> + * indicating which messages the control program (we) want(s) to
> + * send/receive.
> + */
> +static bool sclp_set_write_mask(void)
> +{
> +    WriteEventMask *sccb = (void *)_sccb;
> +
> +	while (sclp_busy)
> +		/* Wait for SCLP request to complete */;
> +	sclp_busy = true;
> +	memset(_sccb, 0, sizeof(*sccb));
> +	sccb->h.length = sizeof(WriteEventMask);
> +	sccb->h.function_code = 0;
> +	sccb->mask_length = sizeof(sccb_mask_t);
> +
> +    /* For now we don't process sclp input. */
> +    sccb->cp_receive_mask = 0;
> +    /* We send ASCII and line mode. */
> +    sccb->cp_send_mask = SCLP_EVENT_MASK_MSG_ASCII | SCLP_EVENT_MASK_MSG;
> +
> +	sclp_service_call(SCLP_CMD_WRITE_EVENT_MASK, sccb);
> +	while (sclp_busy) {
> +	}
> +	/* TODO: exit()? */
> +	if (sccb->h.response_code != SCLP_RC_NORMAL_COMPLETION)
> +		return false;
> +	else {
> +		initialized = true;
> +		return true;
> +	}
> +}
> +
> +void sclp_ascii_setup(void)
> +{
> +	sclp_set_write_mask();
> +}

Can you avoid fixing alignment while introducing new changes?

E.g. this function didn't change at all. If you want to fix alignment,
please move patch5 in front of this patch and fix alignment in the whole
file.

Nice to see that this runs with little changes under LPAR and even z/VM :)

BTW, did you find any inconsistencies running the tests on LPAR / z/VM?

Patch

diff --git a/lib/s390x/asm/arch_def.h b/lib/s390x/asm/arch_def.h
index d2cd727..4bbb428 100644
--- a/lib/s390x/asm/arch_def.h
+++ b/lib/s390x/asm/arch_def.h
@@ -15,6 +15,7 @@  struct psw {
 	uint64_t	addr;
 };
 
+#define PSW_MASK_EXT			0x0100000000000000UL
 #define PSW_MASK_DAT			0x0400000000000000UL
 #define PSW_MASK_PSTATE			0x0001000000000000UL
 
diff --git a/lib/s390x/asm/interrupt.h b/lib/s390x/asm/interrupt.h
index 013709f..de15d9e 100644
--- a/lib/s390x/asm/interrupt.h
+++ b/lib/s390x/asm/interrupt.h
@@ -11,6 +11,8 @@ 
 #define _ASMS390X_IRQ_H_
 #include <asm/arch_def.h>
 
+#define EXT_IRQ_SERVICE_SIG 0x2401
+
 void handle_pgm_int(void);
 void handle_ext_int(void);
 void handle_mcck_int(void);
diff --git a/lib/s390x/interrupt.c b/lib/s390x/interrupt.c
index cf0a794..7118577 100644
--- a/lib/s390x/interrupt.c
+++ b/lib/s390x/interrupt.c
@@ -12,6 +12,7 @@ 
 #include <libcflat.h>
 #include <asm/interrupt.h>
 #include <asm/barrier.h>
+#include <sclp.h>
 
 static bool pgm_int_expected;
 static struct lowcore *lc;
@@ -107,8 +108,15 @@  void handle_pgm_int(void)
 
 void handle_ext_int(void)
 {
-	report_abort("Unexpected external call interrupt: at %#lx",
-		     lc->ext_old_psw.addr);
+	if (lc->ext_int_code != EXT_IRQ_SERVICE_SIG)
+		report_abort("Unexpected external call interrupt: at %#lx",
+			     lc->ext_old_psw.addr);
+	else {
+		lc->ext_old_psw.mask &= ~PSW_MASK_EXT;
+		lc->sw_int_cr0 &= ~(1UL << 9);
+		sclp_handle_ext();
+		lc->ext_int_code = 0;
+	}
 }
 
 void handle_mcck_int(void)
diff --git a/lib/s390x/io.c b/lib/s390x/io.c
index 7bca637..72041ed 100644
--- a/lib/s390x/io.c
+++ b/lib/s390x/io.c
@@ -44,8 +44,8 @@  void setup(void)
 {
 	setup_args_progname(ipl_args);
 	setup_facilities();
-	sclp_ascii_setup();
 	sclp_memory_setup();
+	sclp_ascii_setup();
 }
 
 void exit(int code)
diff --git a/lib/s390x/sclp-ascii.c b/lib/s390x/sclp-ascii.c
index 893ca17..5e23207 100644
--- a/lib/s390x/sclp-ascii.c
+++ b/lib/s390x/sclp-ascii.c
@@ -11,58 +11,206 @@ 
 #include <libcflat.h>
 #include <string.h>
 #include <asm/page.h>
+#include <asm/arch_def.h>
+#include <asm/io.h>
 #include "sclp.h"
 
-char _sccb[PAGE_SIZE] __attribute__((__aligned__(4096)));
 
-/* Perform service call. Return 0 on success, non-zero otherwise. */
-int sclp_service_call(unsigned int command, void *sccb)
+/*
+ * ASCII (IBM PC 437) -> EBCDIC 037
+ */
+static uint8_t _ascebc[256] =
 {
-        int cc;
+ /*00 NUL   SOH   STX   ETX   EOT   ENQ   ACK   BEL */
+     0x00, 0x01, 0x02, 0x03, 0x37, 0x2D, 0x2E, 0x2F,
+ /*08  BS    HT    LF    VT    FF    CR    SO    SI */
+ /*              ->NL                               */
+     0x16, 0x05, 0x15, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
+ /*10 DLE   DC1   DC2   DC3   DC4   NAK   SYN   ETB */
+     0x10, 0x11, 0x12, 0x13, 0x3C, 0x3D, 0x32, 0x26,
+ /*18 CAN    EM   SUB   ESC    FS    GS    RS    US */
+ /*                               ->IGS ->IRS ->IUS */
+     0x18, 0x19, 0x3F, 0x27, 0x22, 0x1D, 0x1E, 0x1F,
+ /*20  SP     !     "     #     $     %     &     ' */
+     0x40, 0x5A, 0x7F, 0x7B, 0x5B, 0x6C, 0x50, 0x7D,
+ /*28   (     )     *     +     ,     -    .      / */
+     0x4D, 0x5D, 0x5C, 0x4E, 0x6B, 0x60, 0x4B, 0x61,
+ /*30   0     1     2     3     4     5     6     7 */
+     0xF0, 0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7,
+ /*38   8     9     :     ;     <     =     >     ? */
+     0xF8, 0xF9, 0x7A, 0x5E, 0x4C, 0x7E, 0x6E, 0x6F,
+ /*40   @     A     B     C     D     E     F     G */
+     0x7C, 0xC1, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7,
+ /*48   H     I     J     K     L     M     N     O */
+     0xC8, 0xC9, 0xD1, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6,
+ /*50   P     Q     R     S     T     U     V     W */
+     0xD7, 0xD8, 0xD9, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6,
+ /*58   X     Y     Z     [     \     ]     ^     _ */
+     0xE7, 0xE8, 0xE9, 0xBA, 0xE0, 0xBB, 0xB0, 0x6D,
+ /*60   `     a     b     c     d     e     f     g */
+     0x79, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
+ /*68   h     i     j     k     l     m     n     o */
+     0x88, 0x89, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96,
+ /*70   p     q     r     s     t     u     v     w */
+     0x97, 0x98, 0x99, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6,
+ /*78   x     y     z     {     |     }     ~    DL */
+     0xA7, 0xA8, 0xA9, 0xC0, 0x4F, 0xD0, 0xA1, 0x07,
+ /*80*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*88*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*90*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*98*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*A0*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*A8*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*B0*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*B8*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*C0*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*C8*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*D0*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*D8*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*E0        sz						*/
+     0x3F, 0x59, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*E8*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*F0*/
+     0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F,
+ /*F8*/
+     0x90, 0x3F, 0x3F, 0x3F, 0x3F, 0xEA, 0x3F, 0xFF
+};
 
-        asm volatile(
-                "       .insn   rre,0xb2200000,%1,%2\n"  /* servc %1,%2 */
-                "       ipm     %0\n"
-                "       srl     %0,28"
-                : "=&d" (cc) : "d" (command), "a" (__pa(sccb))
-                : "cc", "memory");
-        if (cc == 3)
-                return -1;
-        if (cc == 2)
-                return -1;
-        return 0;
-}
+static bool initialized = false;
 
-static void sclp_set_write_mask(void)
-{
-    WriteEventMask *sccb = (void *)_sccb;
-
-    sccb->h.length = sizeof(WriteEventMask);
-    sccb->mask_length = sizeof(unsigned int);
-    sccb->receive_mask = SCLP_EVENT_MASK_MSG_ASCII;
-    sccb->cp_receive_mask = SCLP_EVENT_MASK_MSG_ASCII;
-    sccb->send_mask = SCLP_EVENT_MASK_MSG_ASCII;
-    sccb->cp_send_mask = SCLP_EVENT_MASK_MSG_ASCII;
-
-    sclp_service_call(SCLP_CMD_WRITE_EVENT_MASK, sccb);
-}
-
-void sclp_ascii_setup(void)
-{
-    sclp_set_write_mask();
-}
-
-void sclp_print(const char *str)
+static void sclp_print_ascii(const char *str)
 {
     int len = strlen(str);
     WriteEventData *sccb = (void *)_sccb;
 
+    while (sclp_busy)
+	    /* Wait for SCLP request to complete */;
+    sclp_busy = true;
+    memset(sccb, 0, sizeof(*sccb));
     sccb->h.length = sizeof(WriteEventData) + len;
     sccb->h.function_code = SCLP_FC_NORMAL_WRITE;
     sccb->ebh.length = sizeof(EventBufferHeader) + len;
     sccb->ebh.type = SCLP_EVENT_ASCII_CONSOLE_DATA;
-    sccb->ebh.flags = 0;
     memcpy(sccb->data, str, len);
 
     sclp_service_call(SCLP_CMD_WRITE_EVENT_DATA, sccb);
 }
+
+static void sclp_print_lm(const char *str)
+{
+	unsigned char *ptr, *end, ch;
+	unsigned int count, offset, len;
+	struct write_sccb *sccb;
+	struct msg_buf *msg;
+	struct mdb *mdb;
+	struct mto *mto;
+	struct go *go;
+
+	while (sclp_busy)
+		/* Wait for SCLP request to complete */;
+	sclp_busy = true;
+	sccb = (struct write_sccb *) _sccb;
+	end = (unsigned char *) sccb + 4096 - 1;
+	memset(sccb, 0, sizeof(*sccb));
+	ptr = (unsigned char *) &sccb->msg.mdb.mto;
+	len = strlen(str);
+	offset = 0;
+	do {
+		for (count = sizeof(*mto); offset < len; count++) {
+			ch = str[offset++];
+			if ((ch == 0x0a) || (ptr + count > end))
+				break;
+			ptr[count] = _ascebc[ch];
+		}
+		mto = (struct mto *) ptr;
+		memset(mto, 0, sizeof(*mto));
+		mto->length = count;
+		mto->type = 4;
+		mto->line_type_flags = LNTPFLGS_ENDTEXT;
+		ptr += count;
+	} while ((offset < len) && (ptr + sizeof(*mto) <= end));
+	len = ptr - (unsigned char *) sccb;
+	sccb->header.length = len - offsetof(struct write_sccb, header);
+	msg = &sccb->msg;
+	msg->header.type = EVTYP_MSG;
+	msg->header.length = len - offsetof(struct write_sccb, msg.header);
+	mdb = &msg->mdb;
+	mdb->header.type = 1;
+	mdb->header.tag = 0xD4C4C240;
+	mdb->header.revision_code = 1;
+	mdb->header.length = len - offsetof(struct write_sccb, msg.mdb.header);
+	go = &mdb->go;
+	go->length = sizeof(*go);
+	go->type = 1;
+	sclp_service_call(SCLP_CMD_WRITE_EVENT_DATA, sccb);
+}
+
+/*
+ * SCLP needs to be initialized by setting a send and receive mask,
+ * indicating which messages the control program (we) want(s) to
+ * send/receive.
+ */
+static bool sclp_set_write_mask(void)
+{
+    WriteEventMask *sccb = (void *)_sccb;
+
+	while (sclp_busy)
+		/* Wait for SCLP request to complete */;
+	sclp_busy = true;
+	memset(_sccb, 0, sizeof(*sccb));
+	sccb->h.length = sizeof(WriteEventMask);
+	sccb->h.function_code = 0;
+	sccb->mask_length = sizeof(sccb_mask_t);
+
+    /* For now we don't process sclp input. */
+    sccb->cp_receive_mask = 0;
+    /* We send ASCII and line mode. */
+    sccb->cp_send_mask = SCLP_EVENT_MASK_MSG_ASCII | SCLP_EVENT_MASK_MSG;
+
+	sclp_service_call(SCLP_CMD_WRITE_EVENT_MASK, sccb);
+	while (sclp_busy) {
+	}
+	/* TODO: exit()? */
+	if (sccb->h.response_code != SCLP_RC_NORMAL_COMPLETION)
+		return false;
+	else {
+		initialized = true;
+		return true;
+	}
+}
+
+void sclp_ascii_setup(void)
+{
+	sclp_set_write_mask();
+}
+
+void sclp_print(const char *str)
+{
+	if (!initialized)
+		return;
+	/*
+	 * z/VM advertises a vt220 console which is not functional:
+	 * (response code 05F0, "not active because of the state of
+	 * the machine"). Hence testing the masks would only work if
+	 * we also use stsi data to distinguish z/VM.
+	 *
+	 * Let's rather print on all available consoles.
+	 */
+	sclp_print_ascii(str);
+	sclp_print_lm(str);
+	while (sclp_busy)
+		/* Wait for SCLP request to complete */;
+}
diff --git a/lib/s390x/sclp.c b/lib/s390x/sclp.c
index 1d4a010..cd0e5e5 100644
--- a/lib/s390x/sclp.c
+++ b/lib/s390x/sclp.c
@@ -23,6 +23,9 @@  static uint64_t storage_increment_size;
 static uint64_t max_ram_size;
 static uint64_t ram_size;
 
+char _sccb[PAGE_SIZE] __attribute__((__aligned__(4096)));
+volatile bool sclp_busy = false;
+
 static void mem_init(phys_addr_t mem_end)
 {
 	phys_addr_t freemem_start = (phys_addr_t)&stacktop;
@@ -30,6 +33,42 @@  static void mem_init(phys_addr_t mem_end)
 	phys_alloc_init(freemem_start, mem_end - freemem_start);
 }
 
+static void sclp_setup_int(void)
+{
+	uint64_t mask;
+
+	ctl_set_bit(0, 9);
+
+	mask = extract_psw_mask();
+	mask |= PSW_MASK_EXT;
+	load_psw_mask(mask);
+}
+
+void sclp_handle_ext(void)
+{
+	ctl_clear_bit(0, 9);
+	sclp_busy = false;
+}
+
+/* Perform service call. Return 0 on success, non-zero otherwise. */
+int sclp_service_call(unsigned int command, void *sccb)
+{
+        int cc;
+
+	sclp_setup_int();
+        asm volatile(
+                "       .insn   rre,0xb2200000,%1,%2\n"  /* servc %1,%2 */
+                "       ipm     %0\n"
+                "       srl     %0,28"
+                : "=&d" (cc) : "d" (command), "a" (__pa(sccb))
+                : "cc", "memory");
+        if (cc == 3)
+                return -1;
+        if (cc == 2)
+                return -1;
+        return 0;
+}
+
 void sclp_memory_setup(void)
 {
 	ReadInfo *ri = (void *)_sccb;
@@ -37,7 +76,10 @@  void sclp_memory_setup(void)
 	int cc;
 
 	ri->h.length = SCCB_SIZE;
+	sclp_busy = true;
 	sclp_service_call(SCLP_CMDW_READ_SCP_INFO_FORCED, ri);
+	while (sclp_busy)
+		/* Wait for SCLP request to complete */;
 
 	/* calculate the storage increment size */
 	rnsize = ri->rnsize;
diff --git a/lib/s390x/sclp.h b/lib/s390x/sclp.h
index 21d482b..f550afb 100644
--- a/lib/s390x/sclp.h
+++ b/lib/s390x/sclp.h
@@ -174,6 +174,7 @@  typedef struct SCCB {
 /* SCLP event masks */
 #define SCLP_EVENT_MASK_SIGNAL_QUIESCE          0x00000008
 #define SCLP_EVENT_MASK_MSG_ASCII               0x00000040
+#define SCLP_EVENT_MASK_MSG          		0x40000000
 
 #define SCLP_UNCONDITIONAL_READ                 0x00
 #define SCLP_SELECTIVE_READ                     0x01
@@ -207,9 +208,77 @@  typedef struct ReadEventData {
     uint32_t mask;
 } __attribute__((packed)) ReadEventData;
 
+#define MDBTYP_GO               0x0001
+#define MDBTYP_MTO              0x0004
+#define EVTYP_MSG               0x02
+#define LNTPFLGS_CNTLTEXT       0x8000
+#define LNTPFLGS_LABELTEXT      0x4000
+#define LNTPFLGS_DATATEXT       0x2000
+#define LNTPFLGS_ENDTEXT        0x1000
+#define LNTPFLGS_PROMPTTEXT     0x0800
+
+typedef uint32_t sccb_mask_t;
+
+/* SCLP line mode console related structures. */
+
+struct mto {
+	u16 length;
+	u16 type;
+	u16 line_type_flags;
+	u8  alarm_control;
+	u8  _reserved[3];
+} __attribute__((packed));
+
+struct go {
+	u16 length;
+	u16 type;
+	u32 domid;
+	u8  hhmmss_time[8];
+	u8  th_time[3];
+	u8  reserved_0;
+	u8  dddyyyy_date[7];
+	u8  _reserved_1;
+	u16 general_msg_flags;
+	u8  _reserved_2[10];
+	u8  originating_system_name[8];
+	u8  job_guest_name[8];
+} __attribute__((packed));
+
+struct mdb_header {
+	u16 length;
+	u16 type;
+	u32 tag;
+	u32 revision_code;
+} __attribute__((packed));
+
+struct mdb {
+	struct mdb_header header;
+	struct go go;
+	struct mto mto;
+} __attribute__((packed));
+
+struct evbuf_header {
+	u16	length;
+	u8	type;
+	u8	flags;
+	u16	_reserved;
+} __attribute__((packed));
+
+struct msg_buf {
+	struct evbuf_header header;
+	struct mdb mdb;
+} __attribute__((packed));
+
+struct write_sccb {
+	struct SCCBHeader header;
+	struct msg_buf msg;
+} __packed;
+
+extern char _sccb[];
+volatile bool sclp_busy;
+void sclp_handle_ext(void);
 void sclp_ascii_setup(void);
 void sclp_print(const char *str);
-extern char _sccb[];
 int sclp_service_call(unsigned int command, void *sccb);
 void sclp_memory_setup(void);