2 files changed, 51 insertions, 1 deletions

diff --git a/arch/s390/kernel/ipl.c b/arch/s390/kernel/ipl.c
index a36bea1188d9..d2e6a0a56ade 100644
--- a/arch/s390/kernel/ipl.c
+++ b/arch/s390/kernel/ipl.c
@@ -1037,13 +1037,15 @@ static void do_reset_calls(void)
 }
 
 extern void reset_mcck_handler(void);
+extern void reset_pgm_handler(void);
 
 void s390_reset_system(void)
 {
         struct _lowcore *lc;
 
-        /* Stack for interrupt/machine check handler */
         lc = (struct _lowcore *)(unsigned long) store_prefix();
+
+        /* Stack for interrupt/machine check handler */
         lc->panic_stack = S390_lowcore.panic_stack;
 
         /* Disable prefixing */
@@ -1056,5 +1058,11 @@ void s390_reset_system(void)
         S390_lowcore.mcck_new_psw.mask = PSW_KERNEL_BITS & ~PSW_MASK_MCHECK;
         S390_lowcore.mcck_new_psw.addr =
                 PSW_ADDR_AMODE | (unsigned long) &reset_mcck_handler;
+
+        /* Set new program check handler */
+        S390_lowcore.program_new_psw.mask = PSW_KERNEL_BITS & ~PSW_MASK_MCHECK;
+        S390_lowcore.program_new_psw.addr =
+                PSW_ADDR_AMODE | (unsigned long) &reset_pgm_handler;
+
         do_reset_calls();
 }
diff --git a/arch/s390/kernel/reset.S b/arch/s390/kernel/reset.S
index be8688c0665c..8a87355161fa 100644
--- a/arch/s390/kernel/reset.S
+++ b/arch/s390/kernel/reset.S
@@ -3,6 +3,7 @@
  *
  *    Copyright (C) IBM Corp. 2006
  *    Author(s): Heiko Carstens <heiko.carstens@de.ibm.com>
+ *               Michael Holzheu <holzheu@de.ibm.com>
  */
 
 #include <asm/ptrace.h>
@@ -27,6 +28,26 @@ reset_mcck_handler:
 s390_reset_mcck_handler:
         .quad   0
 
+        .globl  reset_pgm_handler
+reset_pgm_handler:
+        stmg    %r0,%r15,__LC_SAVE_AREA
+        basr    %r13,0
+0:      lg      %r15,__LC_PANIC_STACK   # load panic stack
+        aghi    %r15,-STACK_FRAME_OVERHEAD
+        lg      %r1,s390_reset_pgm_handler-0b(%r13)
+        ltgr    %r1,%r1
+        jz      1f
+        basr    %r14,%r1
+        lmg     %r0,%r15,__LC_SAVE_AREA
+        lpswe   __LC_PGM_OLD_PSW
+1:      lpswe   disabled_wait_psw-0b(%r13)
+        .globl s390_reset_pgm_handler
+s390_reset_pgm_handler:
+        .quad   0
+        .align  8
+disabled_wait_psw:
+        .quad   0x0002000180000000,0x0000000000000000 + reset_pgm_handler
+
 #else /* CONFIG_64BIT */
 
         .globl  reset_mcck_handler
@@ -45,4 +66,25 @@ reset_mcck_handler:
 s390_reset_mcck_handler:
         .long   0
 
+        .globl  reset_pgm_handler
+reset_pgm_handler:
+        stm     %r0,%r15,__LC_SAVE_AREA
+        basr    %r13,0
+0:      l       %r15,__LC_PANIC_STACK   # load panic stack
+        ahi     %r15,-STACK_FRAME_OVERHEAD
+        l       %r1,s390_reset_pgm_handler-0b(%r13)
+        ltr     %r1,%r1
+        jz      1f
+        basr    %r14,%r1
+        lm      %r0,%r15,__LC_SAVE_AREA
+        lpsw    __LC_PGM_OLD_PSW
+
+1:      lpsw    disabled_wait_psw-0b(%r13)
+        .globl  s390_reset_pgm_handler
+s390_reset_pgm_handler:
+        .long   0
+disabled_wait_psw:
+        .align 8
+        .long   0x000a0000,0x00000000 + reset_pgm_handler
+
 #endif /* CONFIG_64BIT */