2 files changed, 20 insertions, 4 deletions

diff --git a/arch/s390/kernel/entry.S b/arch/s390/kernel/entry.S
index d5e3e6007447..bea9ee37ac9d 100644
--- a/arch/s390/kernel/entry.S
+++ b/arch/s390/kernel/entry.S
@@ -535,8 +535,16 @@ pgm_no_vtime2:
         l       %r3,__LC_PGM_ILC        # load program interruption code
         la      %r8,0x7f
         nr      %r8,%r3                 # clear per-event-bit and ilc
-        be      BASED(pgm_exit)         # only per or per+check ?
-        b       BASED(pgm_do_call)
+        be      BASED(pgm_exit2)        # only per or per+check ?
+        l       %r7,BASED(.Ljump_table)
+        sll     %r8,2
+        l       %r7,0(%r8,%r7)          # load address of handler routine
+        la      %r2,SP_PTREGS(%r15)     # address of register-save area
+        basr    %r14,%r7                # branch to interrupt-handler
+pgm_exit2:
+        TRACE_IRQS_ON
+        stosm   __SF_EMPTY(%r15),0x03   # reenable interrupts
+        b       BASED(sysc_return)
 
 #
 # it was a single stepped SVC that is causing all the trouble
diff --git a/arch/s390/kernel/entry64.S b/arch/s390/kernel/entry64.S
index e7192e1cb678..8bccec15ea90 100644
--- a/arch/s390/kernel/entry64.S
+++ b/arch/s390/kernel/entry64.S
@@ -544,8 +544,16 @@ pgm_no_vtime2:
         lgf     %r3,__LC_PGM_ILC        # load program interruption code
         lghi    %r8,0x7f
         ngr     %r8,%r3                 # clear per-event-bit and ilc
-        je      pgm_exit
-        j       pgm_do_call
+        je      pgm_exit2
+        sll     %r8,3
+        larl    %r1,pgm_check_table
+        lg      %r1,0(%r8,%r1)          # load address of handler routine
+        la      %r2,SP_PTREGS(%r15)     # address of register-save area
+        basr    %r14,%r1                # branch to interrupt-handler
+pgm_exit2:
+        TRACE_IRQS_ON
+        stosm   __SF_EMPTY(%r15),0x03   # reenable interrupts
+        j       sysc_return
 
 #
 # it was a single stepped SVC that is causing all the trouble