5 files changed, 28 insertions, 4 deletions

diff --git a/arch/mips/include/asm/switch_to.h b/arch/mips/include/asm/switch_to.h
index b928b6f898cd..e92d6c4b5ed1 100644
--- a/arch/mips/include/asm/switch_to.h
+++ b/arch/mips/include/asm/switch_to.h
@@ -75,9 +75,12 @@ do {									\
 #endif
 
 #define __clear_software_ll_bit()                                       \
-do {                                                                    \
-        if (!__builtin_constant_p(cpu_has_llsc) || !cpu_has_llsc)       \
-                ll_bit = 0;                                             \
+do {    if (cpu_has_rw_llb) {                                           \
+                write_c0_lladdr(0);                                     \
+        } else {                                                        \
+                if (!__builtin_constant_p(cpu_has_llsc) || !cpu_has_llsc)\
+                        ll_bit = 0;                                     \
+        }                                                               \
 } while (0)
 
 #define switch_to(prev, next, last)                                     \
diff --git a/arch/mips/include/asm/thread_info.h b/arch/mips/include/asm/thread_info.h
index 99eea59604e9..fb68fd2714fb 100644
--- a/arch/mips/include/asm/thread_info.h
+++ b/arch/mips/include/asm/thread_info.h
@@ -28,7 +28,7 @@ struct thread_info {
         unsigned long           tp_value;       /* thread pointer */
         __u32                   cpu;            /* current CPU */
         int                     preempt_count;  /* 0 => preemptable, <0 => BUG */
-
+        int                     r2_emul_return; /* 1 => Returning from R2 emulator */
         mm_segment_t            addr_limit;     /*
                                                  * thread address space limit:
                                                  * 0x7fffffff for user-thead
diff --git a/arch/mips/kernel/asm-offsets.c b/arch/mips/kernel/asm-offsets.c
index b1d84bd4efb3..7b6c11aa1cae 100644
--- a/arch/mips/kernel/asm-offsets.c
+++ b/arch/mips/kernel/asm-offsets.c
@@ -97,6 +97,7 @@ void output_thread_info_defines(void)
         OFFSET(TI_TP_VALUE, thread_info, tp_value);
         OFFSET(TI_CPU, thread_info, cpu);
         OFFSET(TI_PRE_COUNT, thread_info, preempt_count);
+        OFFSET(TI_R2_EMUL_RET, thread_info, r2_emul_return);
         OFFSET(TI_ADDR_LIMIT, thread_info, addr_limit);
         OFFSET(TI_RESTART_BLOCK, thread_info, restart_block);
         OFFSET(TI_REGS, thread_info, regs);
diff --git a/arch/mips/kernel/entry.S b/arch/mips/kernel/entry.S
index d5ab21c3fd12..af41ba6db960 100644
--- a/arch/mips/kernel/entry.S
+++ b/arch/mips/kernel/entry.S
@@ -46,6 +46,11 @@ resume_userspace:
         local_irq_disable               # make sure we dont miss an
                                         # interrupt setting need_resched
                                         # between sampling and return
+#ifdef CONFIG_MIPSR2_TO_R6_EMULATOR
+        lw      k0, TI_R2_EMUL_RET($28)
+        bnez    k0, restore_all_from_r2_emul
+#endif
+
         LONG_L  a2, TI_FLAGS($28)       # current->work
         andi    t0, a2, _TIF_WORK_MASK  # (ignoring syscall_trace)
         bnez    t0, work_pending
@@ -114,6 +119,19 @@ restore_partial:		# restore partial frame
         RESTORE_SP_AND_RET
         .set    at
 
+#ifdef CONFIG_MIPSR2_TO_R6_EMULATOR
+restore_all_from_r2_emul:                       # restore full frame
+        .set    noat
+        sw      zero, TI_R2_EMUL_RET($28)       # reset it
+        RESTORE_TEMP
+        RESTORE_AT
+        RESTORE_STATIC
+        RESTORE_SOME
+        LONG_L  sp, PT_R29(sp)
+        eretnc
+        .set    at
+#endif
+
 work_pending:
         andi    t0, a2, _TIF_NEED_RESCHED # a2 is preloaded with TI_FLAGS
         beqz    t0, work_notifysig
diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index fc157322f609..afa447e5e97f 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -1039,12 +1039,14 @@ asmlinkage void do_ri(struct pt_regs *regs)
                         switch (status) {
                         case 0:
                         case SIGEMT:
+                                task_thread_info(current)->r2_emul_return = 1;
                                 return;
                         case SIGILL:
                                 goto no_r2_instr;
                         default:
                                 process_fpemu_return(status,
                                                      &current->thread.cp0_baduaddr);
+                                task_thread_info(current)->r2_emul_return = 1;
                                 return;
                         }
                 }