MIPS: Loongson-3: Add some unaligned instructions emulation

1, Add unaligned gslq, gssq, gslqc1, gssqc1 emulation;
2, Add unaligned gsl{h, w, d}x, gss{h, w, d}x emulation;
3, Add unaligned gslwxc1, gsswxc1, gsldxc1, gssdxc1 emulation.

Signed-off-by: Huacai Chen <chenhc@lemote.com>
Signed-off-by: Pei Huang <huangpei@loongson.cn>
Reviewed-by: Jiaxun Yang <jiaxun.yang@flygoat.com>
Signed-off-by: Thomas Bogendoerfer <tsbogend@alpha.franken.de>

diff --git a/arch/mips/include/uapi/asm/inst.h b/arch/mips/include/uapi/asm/inst.h
index eaa3a80affdf756e7921ad73ba66c961823cc83f..98f97c85e0594d8336255185c4f2415ae9cef54b 100644 (file)
--- a/arch/mips/include/uapi/asm/inst.h
+++ b/arch/mips/include/uapi/asm/inst.h
@@ -988,6 +988,30 @@ struct mm16_r5_format {            /* Load/store from stack pointer format */
        ;))))
 };
 
+/*
+ * Loongson-3 overridden COP2 instruction formats (32-bit length)
+ */
+struct loongson3_lswc2_format {        /* Loongson-3 overridden lwc2/swc2 Load/Store format */
+       __BITFIELD_FIELD(unsigned int opcode : 6,
+       __BITFIELD_FIELD(unsigned int base : 5,
+       __BITFIELD_FIELD(unsigned int rt : 5,
+       __BITFIELD_FIELD(unsigned int fr : 1,
+       __BITFIELD_FIELD(unsigned int offset : 9,
+       __BITFIELD_FIELD(unsigned int ls : 1,
+       __BITFIELD_FIELD(unsigned int rq : 5,
+       ;)))))))
+};
+
+struct loongson3_lsdc2_format {        /* Loongson-3 overridden ldc2/sdc2 Load/Store format */
+       __BITFIELD_FIELD(unsigned int opcode : 6,
+       __BITFIELD_FIELD(unsigned int base : 5,
+       __BITFIELD_FIELD(unsigned int rt : 5,
+       __BITFIELD_FIELD(unsigned int index : 5,
+       __BITFIELD_FIELD(unsigned int offset : 8,
+       __BITFIELD_FIELD(unsigned int opcode1 : 3,
+       ;))))))
+};
+
 /*
  * MIPS16e instruction formats (16-bit length)
  */
@@ -1088,6 +1112,8 @@ union mips_instruction {
        struct mm16_rb_format mm16_rb_format;
        struct mm16_r3_format mm16_r3_format;
        struct mm16_r5_format mm16_r5_format;
+       struct loongson3_lswc2_format loongson3_lswc2_format;
+       struct loongson3_lsdc2_format loongson3_lsdc2_format;
 };
 
 union mips16e_instruction {

diff --git a/arch/mips/loongson64/cop2-ex.c b/arch/mips/loongson64/cop2-ex.c
index 9efdfe430ff0b4b178521c4d2f8c3809950eb50f..af0600dfe83c6e328c24d31036eddfcda905b296 100644 (file)
--- a/arch/mips/loongson64/cop2-ex.c
+++ b/arch/mips/loongson64/cop2-ex.c
@@ -14,17 +14,29 @@
 #include <linux/sched.h>
 #include <linux/notifier.h>
 #include <linux/ptrace.h>
+#include <linux/uaccess.h>
+#include <linux/sched/signal.h>
 
 #include <asm/fpu.h>
 #include <asm/cop2.h>
+#include <asm/inst.h>
+#include <asm/branch.h>
 #include <asm/current.h>
 #include <asm/mipsregs.h>
 
 static int loongson_cu2_call(struct notifier_block *nfb, unsigned long action,
        void *data)
 {
-       int fpu_owned;
+       unsigned int res, fpu_owned;
+       unsigned long ra, value, value_next;
+       union mips_instruction insn;
        int fr = !test_thread_flag(TIF_32BIT_FPREGS);
+       struct pt_regs *regs = (struct pt_regs *)data;
+       void __user *addr = (void __user *)regs->cp0_badvaddr;
+       unsigned int __user *pc = (unsigned int __user *)exception_epc(regs);
+
+       ra = regs->regs[31];
+       __get_user(insn.word, pc);
 
        switch (action) {
        case CU2_EXCEPTION:
@@ -49,9 +61,284 @@ static int loongson_cu2_call(struct notifier_block *nfb, unsigned long action,
                preempt_enable();
 
                return NOTIFY_STOP;     /* Don't call default notifier */
+
+       case CU2_LWC2_OP:
+               if (insn.loongson3_lswc2_format.ls == 0)
+                       goto sigbus;
+
+               if (insn.loongson3_lswc2_format.fr == 0) {      /* gslq */
+                       if (!access_ok(addr, 16))
+                               goto sigbus;
+
+                       LoadDW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       LoadDW(addr + 8, value_next, res);
+                       if (res)
+                               goto fault;
+
+                       regs->regs[insn.loongson3_lswc2_format.rt] = value;
+                       regs->regs[insn.loongson3_lswc2_format.rq] = value_next;
+                       compute_return_epc(regs);
+               } else {                                        /* gslqc1 */
+                       if (!access_ok(addr, 16))
+                               goto sigbus;
+
+                       lose_fpu(1);
+                       LoadDW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       LoadDW(addr + 8, value_next, res);
+                       if (res)
+                               goto fault;
+
+                       set_fpr64(current->thread.fpu.fpr,
+                               insn.loongson3_lswc2_format.rt, value);
+                       set_fpr64(current->thread.fpu.fpr,
+                               insn.loongson3_lswc2_format.rq, value_next);
+                       compute_return_epc(regs);
+                       own_fpu(1);
+               }
+               return NOTIFY_STOP;     /* Don't call default notifier */
+
+       case CU2_SWC2_OP:
+               if (insn.loongson3_lswc2_format.ls == 0)
+                       goto sigbus;
+
+               if (insn.loongson3_lswc2_format.fr == 0) {      /* gssq */
+                       if (!access_ok(addr, 16))
+                               goto sigbus;
+
+                       /* write upper 8 bytes first */
+                       value_next = regs->regs[insn.loongson3_lswc2_format.rq];
+
+                       StoreDW(addr + 8, value_next, res);
+                       if (res)
+                               goto fault;
+                       value = regs->regs[insn.loongson3_lswc2_format.rt];
+
+                       StoreDW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       compute_return_epc(regs);
+               } else {                                        /* gssqc1 */
+                       if (!access_ok(addr, 16))
+                               goto sigbus;
+
+                       lose_fpu(1);
+                       value_next = get_fpr64(current->thread.fpu.fpr,
+                                       insn.loongson3_lswc2_format.rq);
+
+                       StoreDW(addr + 8, value_next, res);
+                       if (res)
+                               goto fault;
+
+                       value = get_fpr64(current->thread.fpu.fpr,
+                                       insn.loongson3_lswc2_format.rt);
+
+                       StoreDW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       compute_return_epc(regs);
+                       own_fpu(1);
+               }
+               return NOTIFY_STOP;     /* Don't call default notifier */
+
+       case CU2_LDC2_OP:
+               switch (insn.loongson3_lsdc2_format.opcode1) {
+               /*
+                * Loongson-3 overridden ldc2 instructions.
+                * opcode1              instruction
+                *   0x1          gslhx: load 2 bytes to GPR
+                *   0x2          gslwx: load 4 bytes to GPR
+                *   0x3          gsldx: load 8 bytes to GPR
+                *   0x6          gslwxc1: load 4 bytes to FPR
+                *   0x7          gsldxc1: load 8 bytes to FPR
+                */
+               case 0x1:
+                       if (!access_ok(addr, 2))
+                               goto sigbus;
+
+                       LoadHW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       compute_return_epc(regs);
+                       regs->regs[insn.loongson3_lsdc2_format.rt] = value;
+                       break;
+               case 0x2:
+                       if (!access_ok(addr, 4))
+                               goto sigbus;
+
+                       LoadW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       compute_return_epc(regs);
+                       regs->regs[insn.loongson3_lsdc2_format.rt] = value;
+                       break;
+               case 0x3:
+                       if (!access_ok(addr, 8))
+                               goto sigbus;
+
+                       LoadDW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       compute_return_epc(regs);
+                       regs->regs[insn.loongson3_lsdc2_format.rt] = value;
+                       break;
+               case 0x6:
+                       die_if_kernel("Unaligned FP access in kernel code", regs);
+                       BUG_ON(!used_math());
+                       if (!access_ok(addr, 4))
+                               goto sigbus;
+
+                       lose_fpu(1);
+                       LoadW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       set_fpr64(current->thread.fpu.fpr,
+                                       insn.loongson3_lsdc2_format.rt, value);
+                       compute_return_epc(regs);
+                       own_fpu(1);
+
+                       break;
+               case 0x7:
+                       die_if_kernel("Unaligned FP access in kernel code", regs);
+                       BUG_ON(!used_math());
+                       if (!access_ok(addr, 8))
+                               goto sigbus;
+
+                       lose_fpu(1);
+                       LoadDW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       set_fpr64(current->thread.fpu.fpr,
+                                       insn.loongson3_lsdc2_format.rt, value);
+                       compute_return_epc(regs);
+                       own_fpu(1);
+                       break;
+
+               }
+               return NOTIFY_STOP;     /* Don't call default notifier */
+
+       case CU2_SDC2_OP:
+               switch (insn.loongson3_lsdc2_format.opcode1) {
+               /*
+                * Loongson-3 overridden sdc2 instructions.
+                * opcode1              instruction
+                *   0x1          gsshx: store 2 bytes from GPR
+                *   0x2          gsswx: store 4 bytes from GPR
+                *   0x3          gssdx: store 8 bytes from GPR
+                *   0x6          gsswxc1: store 4 bytes from FPR
+                *   0x7          gssdxc1: store 8 bytes from FPR
+                */
+               case 0x1:
+                       if (!access_ok(addr, 2))
+                               goto sigbus;
+
+                       compute_return_epc(regs);
+                       value = regs->regs[insn.loongson3_lsdc2_format.rt];
+
+                       StoreHW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       break;
+               case 0x2:
+                       if (!access_ok(addr, 4))
+                               goto sigbus;
+
+                       compute_return_epc(regs);
+                       value = regs->regs[insn.loongson3_lsdc2_format.rt];
+
+                       StoreW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       break;
+               case 0x3:
+                       if (!access_ok(addr, 8))
+                               goto sigbus;
+
+                       compute_return_epc(regs);
+                       value = regs->regs[insn.loongson3_lsdc2_format.rt];
+
+                       StoreDW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       break;
+
+               case 0x6:
+                       die_if_kernel("Unaligned FP access in kernel code", regs);
+                       BUG_ON(!used_math());
+
+                       if (!access_ok(addr, 4))
+                               goto sigbus;
+
+                       lose_fpu(1);
+                       value = get_fpr64(current->thread.fpu.fpr,
+                                       insn.loongson3_lsdc2_format.rt);
+
+                       StoreW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       compute_return_epc(regs);
+                       own_fpu(1);
+
+                       break;
+               case 0x7:
+                       die_if_kernel("Unaligned FP access in kernel code", regs);
+                       BUG_ON(!used_math());
+
+                       if (!access_ok(addr, 8))
+                               goto sigbus;
+
+                       lose_fpu(1);
+                       value = get_fpr64(current->thread.fpu.fpr,
+                                       insn.loongson3_lsdc2_format.rt);
+
+                       StoreDW(addr, value, res);
+                       if (res)
+                               goto fault;
+
+                       compute_return_epc(regs);
+                       own_fpu(1);
+
+                       break;
+               }
+               return NOTIFY_STOP;     /* Don't call default notifier */
        }
 
        return NOTIFY_OK;               /* Let default notifier send signals */
+
+fault:
+       /* roll back jump/branch */
+       regs->regs[31] = ra;
+       regs->cp0_epc = (unsigned long)pc;
+       /* Did we have an exception handler installed? */
+       if (fixup_exception(regs))
+               return NOTIFY_STOP;     /* Don't call default notifier */
+
+       die_if_kernel("Unhandled kernel unaligned access", regs);
+       force_sig(SIGSEGV);
+
+       return NOTIFY_STOP;     /* Don't call default notifier */
+
+sigbus:
+       die_if_kernel("Unhandled kernel unaligned access", regs);
+       force_sig(SIGBUS);
+
+       return NOTIFY_STOP;     /* Don't call default notifier */
 }
 
 static int __init loongson_cu2_setup(void)