MIPS: Emulate the BC1{EQ,NE}Z FPU instructions

MIPS R6 introduced the following two branch instructions for COP1:

BC1EQZ: Branch if Cop1 (FPR) Register Bit 0 is Equal to Zero
BC1NEZ: Branch if Cop1 (FPR) Register Bit 0 is Not Equal to Zero

Signed-off-by: Markos Chandras <markos.chandras@imgtec.com>

3 files changed, 101 insertions, 30 deletions

diff --git a/arch/mips/include/uapi/asm/inst.h b/arch/mips/include/uapi/asm/inst.h
index 5c9e14a903af..19d3bc1e6510 100644
--- a/arch/mips/include/uapi/asm/inst.h
+++ b/arch/mips/include/uapi/asm/inst.h
@@ -115,7 +115,8 @@ enum cop_op {
         mfhc_op       = 0x03, mtc_op        = 0x04,
         dmtc_op       = 0x05, ctc_op        = 0x06,
         mthc0_op      = 0x06, mthc_op       = 0x07,
-        bc_op         = 0x08, cop_op        = 0x10,
+        bc_op         = 0x08, bc1eqz_op     = 0x09,
+        bc1nez_op     = 0x0d, cop_op        = 0x10,
         copm_op       = 0x18
 };
 
diff --git a/arch/mips/kernel/branch.c b/arch/mips/kernel/branch.c
index 5121adaa34bd..f9cb13c56595 100644
--- a/arch/mips/kernel/branch.c
+++ b/arch/mips/kernel/branch.c
@@ -403,7 +403,7 @@ int __MIPS16e_compute_return_epc(struct pt_regs *regs)
 int __compute_return_epc_for_insn(struct pt_regs *regs,
                                    union mips_instruction insn)
 {
-        unsigned int bit, fcr31, dspcontrol;
+        unsigned int bit, fcr31, dspcontrol, reg;
         long epc = regs->cp0_epc;
         int ret = 0;
 
@@ -618,40 +618,83 @@ int __compute_return_epc_for_insn(struct pt_regs *regs,
          * And now the FPA/cp1 branch instructions.
          */
         case cop1_op:
-                preempt_disable();
-                if (is_fpu_owner())
-                        fcr31 = read_32bit_cp1_register(CP1_STATUS);
-                else
-                        fcr31 = current->thread.fpu.fcr31;
-                preempt_enable();
-
-                bit = (insn.i_format.rt >> 2);
-                bit += (bit != 0);
-                bit += 23;
-                switch (insn.i_format.rt & 3) {
-                case 0: /* bc1f */
-                case 2: /* bc1fl */
-                        if (~fcr31 & (1 << bit)) {
-                                epc = epc + 4 + (insn.i_format.simmediate << 2);
-                                if (insn.i_format.rt == 2)
-                                        ret = BRANCH_LIKELY_TAKEN;
-                        } else
+                if (cpu_has_mips_r6 &&
+                    ((insn.i_format.rs == bc1eqz_op) ||
+                     (insn.i_format.rs == bc1nez_op))) {
+                        if (!used_math()) { /* First time FPU user */
+                                ret = init_fpu();
+                                if (ret && NO_R6EMU) {
+                                        ret = -ret;
+                                        break;
+                                }
+                                ret = 0;
+                                set_used_math();
+                        }
+                        lose_fpu(1);    /* Save FPU state for the emulator. */
+                        reg = insn.i_format.rt;
+                        bit = 0;
+                        switch (insn.i_format.rs) {
+                        case bc1eqz_op:
+                                /* Test bit 0 */
+                                if (get_fpr32(&current->thread.fpu.fpr[reg], 0)
+                                    & 0x1)
+                                        bit = 1;
+                                break;
+                        case bc1nez_op:
+                                /* Test bit 0 */
+                                if (!(get_fpr32(&current->thread.fpu.fpr[reg], 0)
+                                      & 0x1))
+                                        bit = 1;
+                                break;
+                        }
+                        own_fpu(1);
+                        if (bit)
+                                epc = epc + 4 +
+                                        (insn.i_format.simmediate << 2);
+                        else
                                 epc += 8;
                         regs->cp0_epc = epc;
+
                         break;
+                } else {
 
-                case 1: /* bc1t */
-                case 3: /* bc1tl */
-                        if (fcr31 & (1 << bit)) {
-                                epc = epc + 4 + (insn.i_format.simmediate << 2);
-                                if (insn.i_format.rt == 3)
-                                        ret = BRANCH_LIKELY_TAKEN;
-                        } else
-                                epc += 8;
-                        regs->cp0_epc = epc;
+                        preempt_disable();
+                        if (is_fpu_owner())
+                                fcr31 = read_32bit_cp1_register(CP1_STATUS);
+                        else
+                                fcr31 = current->thread.fpu.fcr31;
+                        preempt_enable();
+
+                        bit = (insn.i_format.rt >> 2);
+                        bit += (bit != 0);
+                        bit += 23;
+                        switch (insn.i_format.rt & 3) {
+                        case 0: /* bc1f */
+                        case 2: /* bc1fl */
+                                if (~fcr31 & (1 << bit)) {
+                                        epc = epc + 4 +
+                                                (insn.i_format.simmediate << 2);
+                                        if (insn.i_format.rt == 2)
+                                                ret = BRANCH_LIKELY_TAKEN;
+                                } else
+                                        epc += 8;
+                                regs->cp0_epc = epc;
+                                break;
+
+                        case 1: /* bc1t */
+                        case 3: /* bc1tl */
+                                if (fcr31 & (1 << bit)) {
+                                        epc = epc + 4 +
+                                                (insn.i_format.simmediate << 2);
+                                        if (insn.i_format.rt == 3)
+                                                ret = BRANCH_LIKELY_TAKEN;
+                                } else
+                                        epc += 8;
+                                regs->cp0_epc = epc;
+                                break;
+                        }
                         break;
                 }
-                break;
 #ifdef CONFIG_CPU_CAVIUM_OCTEON
         case lwc2_op: /* This is bbit0 on Octeon */
                 if ((regs->regs[insn.i_format.rs] & (1ull<<insn.i_format.rt))
diff --git a/arch/mips/math-emu/cp1emu.c b/arch/mips/math-emu/cp1emu.c
index 7bbaefe0434d..798204e492fc 100644
--- a/arch/mips/math-emu/cp1emu.c
+++ b/arch/mips/math-emu/cp1emu.c
@@ -602,6 +602,33 @@ static int isBranchInstr(struct pt_regs *regs, struct mm_decoded_insn dec_insn,
 #endif
         case cop0_op:
         case cop1_op:
+                /* Need to check for R6 bc1nez and bc1eqz branches */
+                if (cpu_has_mips_r6 &&
+                    ((insn.i_format.rs == bc1eqz_op) ||
+                     (insn.i_format.rs == bc1nez_op))) {
+                        bit = 0;
+                        switch (insn.i_format.rs) {
+                        case bc1eqz_op:
+                                if (get_fpr32(&current->thread.fpu.fpr[insn.i_format.rt], 0) & 0x1)
+                                    bit = 1;
+                                break;
+                        case bc1nez_op:
+                                if (!(get_fpr32(&current->thread.fpu.fpr[insn.i_format.rt], 0) & 0x1))
+                                    bit = 1;
+                                break;
+                        }
+                        if (bit)
+                                *contpc = regs->cp0_epc +
+                                        dec_insn.pc_inc +
+                                        (insn.i_format.simmediate << 2);
+                        else
+                                *contpc = regs->cp0_epc +
+                                        dec_insn.pc_inc +
+                                        dec_insn.next_pc_inc;
+
+                        return 1;
+                }
+                /* R2/R6 compatible cop1 instruction. Fall through */
         case cop2_op:
         case cop1x_op:
                 if (insn.i_format.rs == bc_op) {