1 files changed, 39 insertions, 2 deletions

diff --git a/arch/mips/kernel/genex.S b/arch/mips/kernel/genex.S
index c6ada98ee042..01dcbe38fa01 100644
--- a/arch/mips/kernel/genex.S
+++ b/arch/mips/kernel/genex.S
@@ -20,6 +20,7 @@
 #include <asm/stackframe.h>
 #include <asm/war.h>
 #include <asm/page.h>
+#include <asm/thread_info.h>
 
 #define PANIC_PIC(msg)                                  \
                 .set push;                              \
@@ -126,7 +127,42 @@ handle_vcei:
 
         __FINIT
 
+        .align  5       /* 32 byte rollback region */
+LEAF(r4k_wait)
+        .set    push
+        .set    noreorder
+        /* start of rollback region */
+        LONG_L  t0, TI_FLAGS($28)
+        nop
+        andi    t0, _TIF_NEED_RESCHED
+        bnez    t0, 1f
+         nop
+        nop
+        nop
+        .set    mips3
+        wait
+        /* end of rollback region (the region size must be power of two) */
+        .set    pop
+1:
+        jr      ra
+        END(r4k_wait)
+
+        .macro  BUILD_ROLLBACK_PROLOGUE handler
+        FEXPORT(rollback_\handler)
+        .set    push
+        .set    noat
+        MFC0    k0, CP0_EPC
+        PTR_LA  k1, r4k_wait
+        ori     k0, 0x1f        /* 32 byte rollback region */
+        xori    k0, 0x1f
+        bne     k0, k1, 9f
+        MTC0    k0, CP0_EPC
+9:
+        .set pop
+        .endm
+
         .align  5
+BUILD_ROLLBACK_PROLOGUE handle_int
 NESTED(handle_int, PT_SIZE, sp)
 #ifdef CONFIG_TRACE_IRQFLAGS
         /*
@@ -201,6 +237,7 @@ NESTED(except_vec_ejtag_debug, 0, sp)
  * This prototype is copied to ebase + n*IntCtl.VS and patched
  * to invoke the handler
  */
+BUILD_ROLLBACK_PROLOGUE except_vec_vi
 NESTED(except_vec_vi, 0, sp)
         SAVE_SOME
         SAVE_AT
@@ -245,8 +282,8 @@ NESTED(except_vec_vi_handler, 0, sp)
         and     t0, a0, t1
 #ifdef CONFIG_MIPS_MT_SMTC_IM_BACKSTOP
         mfc0    t2, CP0_TCCONTEXT
-        or      t0, t0, t2
-        mtc0    t0, CP0_TCCONTEXT
+        or      t2, t0, t2
+        mtc0    t2, CP0_TCCONTEXT
 #endif /* CONFIG_MIPS_MT_SMTC_IM_BACKSTOP */
         xor     t1, t1, t0
         mtc0    t1, CP0_STATUS