3 files changed, 21 insertions, 12 deletions

diff --git a/arch/mips/include/asm/kdebug.h b/arch/mips/include/asm/kdebug.h
index 6a9af5fcb5d7..cba22ab7ad4d 100644
--- a/arch/mips/include/asm/kdebug.h
+++ b/arch/mips/include/asm/kdebug.h
@@ -10,7 +10,8 @@ enum die_val {
         DIE_RI,
         DIE_PAGE_FAULT,
         DIE_BREAK,
-        DIE_SSTEPBP
+        DIE_SSTEPBP,
+        DIE_MSAFP
 };
 
 #endif /* _ASM_MIPS_KDEBUG_H */
diff --git a/arch/mips/kernel/genex.S b/arch/mips/kernel/genex.S
index 86e22422d08c..af42e7003f12 100644
--- a/arch/mips/kernel/genex.S
+++ b/arch/mips/kernel/genex.S
@@ -360,21 +360,15 @@ NESTED(nmi_handler, PT_SIZE, sp)
         .set    mips1
         SET_HARDFLOAT
         cfc1    a1, fcr31
-        li      a2, ~(0x3f << 12)
-        and     a2, a1
-        ctc1    a2, fcr31
         .set    pop
-        TRACE_IRQS_ON
-        STI
+        CLI
+        TRACE_IRQS_OFF
         .endm
 
         .macro  __build_clear_msa_fpe
         _cfcmsa a1, MSA_CSR
-        li      a2, ~(0x3f << 12)
-        and     a1, a1, a2
-        _ctcmsa MSA_CSR, a1
-        TRACE_IRQS_ON
-        STI
+        CLI
+        TRACE_IRQS_OFF
         .endm
 
         .macro  __build_clear_ade
diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index 8943ebe4d154..5b4d711f878d 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -788,6 +788,11 @@ asmlinkage void do_fpe(struct pt_regs *regs, unsigned long fcr31)
         if (notify_die(DIE_FP, "FP exception", regs, 0, regs_to_trapnr(regs),
                        SIGFPE) == NOTIFY_STOP)
                 goto out;
+
+        /* Clear FCSR.Cause before enabling interrupts */
+        write_32bit_cp1_register(CP1_STATUS, fcr31 & ~FPU_CSR_ALL_X);
+        local_irq_enable();
+
         die_if_kernel("FP exception in kernel code", regs);
 
         if (fcr31 & FPU_CSR_UNI_X) {
@@ -1393,13 +1398,22 @@ out:
         exception_exit(prev_state);
 }
 
-asmlinkage void do_msa_fpe(struct pt_regs *regs)
+asmlinkage void do_msa_fpe(struct pt_regs *regs, unsigned int msacsr)
 {
         enum ctx_state prev_state;
 
         prev_state = exception_enter();
+        if (notify_die(DIE_MSAFP, "MSA FP exception", regs, 0,
+                       regs_to_trapnr(regs), SIGFPE) == NOTIFY_STOP)
+                goto out;
+
+        /* Clear MSACSR.Cause before enabling interrupts */
+        write_msa_csr(msacsr & ~MSA_CSR_CAUSEF);
+        local_irq_enable();
+
         die_if_kernel("do_msa_fpe invoked from kernel context!", regs);
         force_sig(SIGFPE, current);
+out:
         exception_exit(prev_state);
 }