MIPS: MIPS16e: Support handling of delay slots.

Add logic needed to properly calculate exceptions for delay slots
when in MIPS16e mode.

Signed-off-by: Steven J. Hill <Steven.Hill@imgtec.com>

diff --git a/arch/mips/include/asm/branch.h b/arch/mips/include/asm/branch.h
index 40bb9eb..e28a3e0 100644 (file)
--- a/arch/mips/include/asm/branch.h
+++ b/arch/mips/include/asm/branch.h
@@ -16,6 +16,7 @@ extern int __compute_return_epc(struct pt_regs *regs);
 extern int __compute_return_epc_for_insn(struct pt_regs *regs,
                                         union mips_instruction insn);
 extern int __microMIPS_compute_return_epc(struct pt_regs *regs);
+extern int __MIPS16e_compute_return_epc(struct pt_regs *regs);
 
 
 static inline int delay_slot(struct pt_regs *regs)
@@ -41,6 +42,8 @@ static inline int compute_return_epc(struct pt_regs *regs)
        if (get_isa16_mode(regs->cp0_epc)) {
                if (cpu_has_mmips)
                        return __microMIPS_compute_return_epc(regs);
+               if (cpu_has_mips16)
+                       return __MIPS16e_compute_return_epc(regs);
                return regs->cp0_epc;
        }
 
@@ -52,4 +55,19 @@ static inline int compute_return_epc(struct pt_regs *regs)
        return __compute_return_epc(regs);
 }
 
+static inline int MIPS16e_compute_return_epc(struct pt_regs *regs,
+                                            union mips16e_instruction *inst)
+{
+       if (likely(!delay_slot(regs))) {
+               if (inst->ri.opcode == MIPS16e_extend_op) {
+                       regs->cp0_epc += 4;
+                       return 0;
+               }
+               regs->cp0_epc += 2;
+               return 0;
+       }
+
+       return __MIPS16e_compute_return_epc(regs);
+}
+
 #endif /* _ASM_BRANCH_H */

diff --git a/arch/mips/include/asm/inst.h b/arch/mips/include/asm/inst.h
index b27091e..22912f7 100644 (file)
--- a/arch/mips/include/asm/inst.h
+++ b/arch/mips/include/asm/inst.h
@@ -82,4 +82,7 @@ struct mm_decoded_insn {
        int micro_mips_mode;
 };
 
+/* Recode table from 16-bit register notation to 32-bit GPR. Do NOT export!!! */
+extern const int reg16to32[];
+
 #endif /* _ASM_INST_H */

diff --git a/arch/mips/kernel/branch.c b/arch/mips/kernel/branch.c
index a03836b..46c2ad0 100644 (file)
--- a/arch/mips/kernel/branch.c
+++ b/arch/mips/kernel/branch.c
@@ -20,13 +20,13 @@
 #include <asm/uaccess.h>
 
 /*
- * Calculate and return exception PC in case of branch delay
- * slot for microMIPS. It does not clear the ISA mode bit.
+ * Calculate and return exception PC in case of branch delay slot
+ * for microMIPS and MIPS16e. It does not clear the ISA mode bit.
  */
 int __isa_exception_epc(struct pt_regs *regs)
 {
-       long epc = regs->cp0_epc;
        unsigned short inst;
+       long epc = regs->cp0_epc;
 
        /* Calculate exception PC in branch delay slot. */
        if (__get_user(inst, (u16 __user *) msk_isa16_mode(epc))) {
@@ -34,8 +34,13 @@ int __isa_exception_epc(struct pt_regs *regs)
                force_sig(SIGSEGV, current);
                return epc;
        }
-
-       if (mm_insn_16bit(inst))
+       if (cpu_has_mips16) {
+               if (((union mips16e_instruction)inst).ri.opcode
+                               == MIPS16e_jal_op)
+                       epc += 4;
+               else
+                       epc += 2;
+       } else if (mm_insn_16bit(inst))
                epc += 2;
        else
                epc += 4;
@@ -101,6 +106,94 @@ sigsegv:
        return -EFAULT;
 }
 
+/*
+ * Compute return address and emulate branch in MIPS16e mode after an
+ * exception only. It does not handle compact branches/jumps and cannot
+ * be used in interrupt context. (Compact branches/jumps do not cause
+ * exceptions.)
+ */
+int __MIPS16e_compute_return_epc(struct pt_regs *regs)
+{
+       u16 __user *addr;
+       union mips16e_instruction inst;
+       u16 inst2;
+       u32 fullinst;
+       long epc;
+
+       epc = regs->cp0_epc;
+
+       /* Read the instruction. */
+       addr = (u16 __user *)msk_isa16_mode(epc);
+       if (__get_user(inst.full, addr)) {
+               force_sig(SIGSEGV, current);
+               return -EFAULT;
+       }
+
+       switch (inst.ri.opcode) {
+       case MIPS16e_extend_op:
+               regs->cp0_epc += 4;
+               return 0;
+
+               /*
+                *  JAL and JALX in MIPS16e mode
+                */
+       case MIPS16e_jal_op:
+               addr += 1;
+               if (__get_user(inst2, addr)) {
+                       force_sig(SIGSEGV, current);
+                       return -EFAULT;
+               }
+               fullinst = ((unsigned)inst.full << 16) | inst2;
+               regs->regs[31] = epc + 6;
+               epc += 4;
+               epc >>= 28;
+               epc <<= 28;
+               /*
+                * JAL:5 X:1 TARGET[20-16]:5 TARGET[25:21]:5 TARGET[15:0]:16
+                *
+                * ......TARGET[15:0].................TARGET[20:16]...........
+                * ......TARGET[25:21]
+                */
+               epc |=
+                   ((fullinst & 0xffff) << 2) | ((fullinst & 0x3e00000) >> 3) |
+                   ((fullinst & 0x1f0000) << 7);
+               if (!inst.jal.x)
+                       set_isa16_mode(epc);    /* Set ISA mode bit. */
+               regs->cp0_epc = epc;
+               return 0;
+
+               /*
+                *  J(AL)R(C)
+                */
+       case MIPS16e_rr_op:
+               if (inst.rr.func == MIPS16e_jr_func) {
+
+                       if (inst.rr.ra)
+                               regs->cp0_epc = regs->regs[31];
+                       else
+                               regs->cp0_epc =
+                                   regs->regs[reg16to32[inst.rr.rx]];
+
+                       if (inst.rr.l) {
+                               if (inst.rr.nd)
+                                       regs->regs[31] = epc + 2;
+                               else
+                                       regs->regs[31] = epc + 4;
+                       }
+                       return 0;
+               }
+               break;
+       }
+
+       /*
+        * All other cases have no branch delay slot and are 16-bits.
+        * Branches do not cause an exception.
+        */
+       regs->cp0_epc += 2;
+
+       return 0;
+}
+
 /**
  * __compute_return_epc_for_insn - Computes the return address and do emulate
  *                                 branch simulation, if required.