1 files changed, 21 insertions, 1 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