[PATCH v3 4/5] pc-bios: s390x: Save io and external new PSWs before overwriting them

[Janosch Frank <frankja@linux.ibm.com>]

Currently we always overwrite the mentioned exception new PSWs before
loading the enabled wait PSW. Let's save the PSW before overwriting
and restore it right before starting the loaded kernel.

Signed-off-by: Janosch Frank <frankja@linux.ibm.com>
---
 pc-bios/s390-ccw/jump2ipl.c |  4 +++
 pc-bios/s390-ccw/netmain.c  |  3 ++
 pc-bios/s390-ccw/start.S    | 62 +++++++++++++++++++++++++++----------
 3 files changed, 52 insertions(+), 17 deletions(-)

diff --git a/pc-bios/s390-ccw/jump2ipl.c b/pc-bios/s390-ccw/jump2ipl.c
index 5b8352d257..bb94ba7550 100644
--- a/pc-bios/s390-ccw/jump2ipl.c
+++ b/pc-bios/s390-ccw/jump2ipl.c
@@ -14,6 +14,7 @@
 #define RESET_PSW_MASK (PSW_MASK_SHORTPSW | PSW_MASK_64)
 #define RESET_PSW ((uint64_t)&jump_to_IPL_addr | RESET_PSW_MASK)
 
+extern uint64_t psw_save_io[], psw_save_ext[];
 static uint64_t *reset_psw = 0, save_psw, ipl_continue;
 
 void write_reset_psw(uint64_t psw)
@@ -59,6 +60,9 @@ void jump_to_IPL_code(uint64_t address)
     /* Ensure the guest output starts fresh */
     sclp_print("\n");
 
+    memcpy(&lowcore->io_new_psw, psw_save_io, 16);
+    memcpy(&lowcore->external_new_psw, psw_save_ext, 16);
+
     /*
      * HACK ALERT.
      * We use the load normal reset to keep r15 unchanged. jump_to_IPL_2
diff --git a/pc-bios/s390-ccw/netmain.c b/pc-bios/s390-ccw/netmain.c
index 056e93a818..74ef28fbc6 100644
--- a/pc-bios/s390-ccw/netmain.c
+++ b/pc-bios/s390-ccw/netmain.c
@@ -32,6 +32,7 @@
 #include <time.h>
 #include <pxelinux.h>
 
+#include "s390-arch.h"
 #include "s390-ccw.h"
 #include "cio.h"
 #include "virtio.h"
@@ -43,6 +44,8 @@
 extern char _start[];
 void write_iplb_location(void) {}
 
+LowCore *lowcore; /* Yes, this *is* a pointer to address 0 */
+
 #define KERNEL_ADDR             ((void *)0L)
 #define KERNEL_MAX_SIZE         ((long)_start)
 #define ARCH_COMMAND_LINE_SIZE  896              /* Taken from Linux kernel */
diff --git a/pc-bios/s390-ccw/start.S b/pc-bios/s390-ccw/start.S
index ce519300a1..939aac3a7c 100644
--- a/pc-bios/s390-ccw/start.S
+++ b/pc-bios/s390-ccw/start.S
@@ -34,7 +34,17 @@ remainder:
 	larl	%r2,memsetxc
 	ex	%r3,0(%r2)
 done:
-	j      main		/* And call C */
+        /* prepare i/o call handler */
+        larl  %r1, io_new_code
+        larl  %r2, io_new_psw
+        stg   %r1, 8(%r2)
+        mvc   0x1f0(16),0(%r2)
+        /* prepare external call handler */
+        larl  %r1, external_new_code
+        larl  %r2, external_new_psw
+        stg   %r1, 8(%r2)
+        mvc   0x1b0(16),0(%r2)
+        j      main		/* And call C */
 
 memsetxc:
 	xc	0(1,%r1),0(%r1)
@@ -64,13 +74,16 @@ consume_sclp_int:
         oi      6(%r15),0x2
         lctlg   %c0,%c0,0(%r15)
         /* prepare external call handler */
-        larl %r1, external_new_code
-        stg %r1, 0x1b8
-        larl %r1, external_new_mask
-        mvc 0x1b0(8),0(%r1)
-        /* load enabled wait PSW */
-        larl %r1, enabled_wait_psw
-        lpswe 0(%r1)
+        larl  %r1, external_new_psw
+        lghi  %r2, 0x1b0
+        /* Is the BIOS' external new PSW already set? */
+        clc   0(16, %r1), 0(%r2)
+        je    load_ewait
+        /* No, save old PSW and write BIOS PSW */
+        larl  %r3, psw_save_ext
+        mvc   0(16, %r3), 0x1b0
+        mvc   0x1b0(16),0(%r1)
+        j     load_ewait
 
 /*
  * void consume_io_int(void)
@@ -84,11 +97,20 @@ consume_io_int:
         oi    4(%r15), 0xff
         lctlg %c6,%c6,0(%r15)
         /* prepare i/o call handler */
-        larl  %r1, io_new_code
-        stg   %r1, 0x1f8
-        larl  %r1, io_new_mask
-        mvc   0x1f0(8),0(%r1)
-        /* load enabled wait PSW */
+        larl  %r1, io_new_psw
+        lghi  %r2, 0x1f0
+        /* Is the BIOS' PSW already set? */
+        larl  %r3, load_ewait
+        clc   0(16, %r1), 0(%r2)
+        bcr   8, %r3
+        /* No, save old PSW and write BIOS PSW */
+        larl  %r3, psw_save_io
+        mvc   0(16, %r3), 0x1f0
+        mvc   0x1f0(16),0(%r1)
+        j     load_ewait
+
+load_ewait:
+        /* PSW is the correct one, time to load the enabled wait PSW */
         larl  %r1, enabled_wait_psw
         lpswe 0(%r1)
 
@@ -107,11 +129,17 @@ io_new_code:
         br    %r14
 
         .align  8
+        .globl psw_save_io
+        .globl psw_save_ext
 disabled_wait_psw:
         .quad   0x0002000180000000,0x0000000000000000
 enabled_wait_psw:
         .quad   0x0302000180000000,0x0000000000000000
-external_new_mask:
-        .quad   0x0000000180000000
-io_new_mask:
-        .quad   0x0000000180000000
+external_new_psw:
+        .quad   0x0000000180000000,0
+io_new_psw:
+        .quad   0x0000000180000000,0
+psw_save_io:
+        .quad   0,0
+psw_save_ext:
+        .quad   0,0
-- 
2.25.1