diff mbox series

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

Message ID 20200831150910.317171-5-frankja@linux.ibm.com (mailing list archive)
State New, archived
Headers show
Series pc-bios: s390x: Cleanup part 2 | expand

Commit Message

Janosch Frank Aug. 31, 2020, 3:09 p.m. UTC
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(-)

Comments

Thomas Huth Sept. 1, 2020, 5:22 p.m. UTC | #1
On 31/08/2020 17.09, Janosch Frank wrote:
> 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(-)

Patch looks basically fine to me, I just got some questions for my
understanding below...

> 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)

Can't you specify the external_new_code and io_new_code in the
external_new_psw / io_new_psw directly? Or is our relocation code not
good enough for this?

> +        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

Why not a "je load_ewait" again, like in the consume_sclp_int handler?

> +        /* 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
> 

In case you respin, could you maybe add some local #defines for 0x1f0
and 0x1b0 ?

 Thomas
Janosch Frank Sept. 2, 2020, 10:30 a.m. UTC | #2
On 9/1/20 7:22 PM, Thomas Huth wrote:
> On 31/08/2020 17.09, Janosch Frank wrote:
>> 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(-)
> 
> Patch looks basically fine to me, I just got some questions for my
> understanding below...
> 
>> 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)
> 
> Can't you specify the external_new_code and io_new_code in the
> external_new_psw / io_new_psw directly? Or is our relocation code not
> good enough for this?

Initially I had some problems with this. I just had another try and it
seems to work well, but as the testing infrastructure doesn't really
work, I can't vouch for that.

> 
>> +        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
> 
> Why not a "je load_ewait" again, like in the consume_sclp_int handler?

Great question

> 
>> +        /* 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
>>
> 
> In case you respin, could you maybe add some local #defines for 0x1f0
> and 0x1b0 ?

At the top of this file?

> 
>  Thomas
>
Thomas Huth Sept. 2, 2020, 4:55 p.m. UTC | #3
On 02/09/2020 12.30, Janosch Frank wrote:
> On 9/1/20 7:22 PM, Thomas Huth wrote:
>> On 31/08/2020 17.09, Janosch Frank wrote:
>>> 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(-)
>>
>> Patch looks basically fine to me, I just got some questions for my
>> understanding below...
>>
>>> 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)
>>
>> Can't you specify the external_new_code and io_new_code in the
>> external_new_psw / io_new_psw directly? Or is our relocation code not
>> good enough for this?
> 
> Initially I had some problems with this. I just had another try and it
> seems to work well, but as the testing infrastructure doesn't really
> work, I can't vouch for that.

You could maybe dump the memory in both cases to see whether
external_new_psw and io_new_psw contain the same values before and after
the change?

>> In case you respin, could you maybe add some local #defines for 0x1f0
>> and 0x1b0 ?
> 
> At the top of this file?

Yes, please.

 Thomas
diff mbox series

Patch

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