4 files changed, 29 insertions, 4 deletions

diff --git a/arch/s390/include/asm/processor.h b/arch/s390/include/asm/processor.h
index a4b6229e5d4b..306c93c1b184 100644
--- a/arch/s390/include/asm/processor.h
+++ b/arch/s390/include/asm/processor.h
@@ -187,7 +187,6 @@ static inline void __load_psw(psw_t psw)
  * Set PSW mask to specified value, while leaving the
  * PSW addr pointing to the next instruction.
  */
-
 static inline void __load_psw_mask (unsigned long mask)
 {
         unsigned long addr;
@@ -212,6 +211,27 @@ static inline void __load_psw_mask (unsigned long mask)
                 : "=&d" (addr), "=Q" (psw) : "Q" (psw) : "memory", "cc");
 #endif /* __s390x__ */
 }
+
+/*
+ * Rewind PSW instruction address by specified number of bytes.
+ */
+static inline unsigned long __rewind_psw(psw_t psw, unsigned long ilc)
+{
+#ifndef __s390x__
+        if (psw.addr & PSW_ADDR_AMODE)
+                /* 31 bit mode */
+                return (psw.addr - ilc) | PSW_ADDR_AMODE;
+        /* 24 bit mode */
+        return (psw.addr - ilc) & ((1UL << 24) - 1);
+#else
+        unsigned long mask;
+
+        mask = (psw.mask & PSW_MASK_EA) ? -1UL :
+               (psw.mask & PSW_MASK_BA) ? (1UL << 31) - 1 :
+                                          (1UL << 24) - 1;
+        return (psw.addr - ilc) & mask;
+#endif
+}
  
 /*
  * Function to stop a processor until an interruption occurred
diff --git a/arch/s390/include/asm/ptrace.h b/arch/s390/include/asm/ptrace.h
index 93c907b4776f..2904cc66967a 100644
--- a/arch/s390/include/asm/ptrace.h
+++ b/arch/s390/include/asm/ptrace.h
@@ -236,6 +236,8 @@ typedef struct
 #define PSW_MASK_ASC            0x0000C000UL
 #define PSW_MASK_CC             0x00003000UL
 #define PSW_MASK_PM             0x00000F00UL
+#define PSW_MASK_EA             0x00000000UL
+#define PSW_MASK_BA             0x00000000UL
 
 #define PSW_ADDR_AMODE          0x80000000UL
 #define PSW_ADDR_INSN           0x7FFFFFFFUL
@@ -261,6 +263,8 @@ typedef struct
 #define PSW_MASK_ASC            0x0000C00000000000UL
 #define PSW_MASK_CC             0x0000300000000000UL
 #define PSW_MASK_PM             0x00000F0000000000UL
+#define PSW_MASK_EA             0x0000000100000000UL
+#define PSW_MASK_BA             0x0000000080000000UL
 
 #define PSW_ADDR_AMODE          0x0000000000000000UL
 #define PSW_ADDR_INSN           0xFFFFFFFFFFFFFFFFUL
diff --git a/arch/s390/kernel/signal.c b/arch/s390/kernel/signal.c
index e751cab80e04..058e372bada1 100644
--- a/arch/s390/kernel/signal.c
+++ b/arch/s390/kernel/signal.c
@@ -447,8 +447,9 @@ void do_signal(struct pt_regs *regs)
                         /* fallthrough */
                         case -ERESTARTNOINTR:
                                 regs->gprs[2] = regs->orig_gpr2;
-                                regs->psw.addr = regs->psw.addr -
-                                        (regs->svc_code >> 16);
+                                regs->psw.addr =
+                                        __rewind_psw(regs->psw,
+                                                     regs->svc_code >> 16);
                                 break;
                         }
                         /* No longer in a system call */
diff --git a/arch/s390/mm/fault.c b/arch/s390/mm/fault.c
index 9564fc779b27..a90fd91a9c72 100644
--- a/arch/s390/mm/fault.c
+++ b/arch/s390/mm/fault.c
@@ -393,7 +393,7 @@ void __kprobes do_protection_exception(struct pt_regs *regs, long pgm_int_code,
         int fault;
 
         /* Protection exception is suppressing, decrement psw address. */
-        regs->psw.addr -= (pgm_int_code >> 16);
+        regs->psw.addr = __rewind_psw(regs->psw, pgm_int_code >> 16);
         /*
          * Check for low-address protection.  This needs to be treated
          * as a special case because the translation exception code