Merge tag 'openrisc-for-linus' of git://github.com/openrisc/linux

[karo-tx-linux.git] / arch / openrisc / kernel / traps.c

diff --git a/arch/openrisc/kernel/traps.c b/arch/openrisc/kernel/traps.c
index d29c41bfbffaab232cf21d781aed6893fde12c06..7e81ad258bca39bf20867060cf012bd24bed346e 100644 (file)
--- a/arch/openrisc/kernel/traps.c
+++ b/arch/openrisc/kernel/traps.c
@@ -40,6 +40,8 @@
 extern char _etext, _stext;
 
 int kstack_depth_to_print = 0x180;
+int lwa_flag;
+unsigned long __user *lwa_addr;
 
 static inline int valid_stack_ptr(struct thread_info *tinfo, void *p)
 {
@@ -334,10 +336,191 @@ asmlinkage void do_bus_fault(struct pt_regs *regs, unsigned long address)
        }
 }
 
+static inline int in_delay_slot(struct pt_regs *regs)
+{
+#ifdef CONFIG_OPENRISC_NO_SPR_SR_DSX
+       /* No delay slot flag, do the old way */
+       unsigned int op, insn;
+
+       insn = *((unsigned int *)regs->pc);
+       op = insn >> 26;
+       switch (op) {
+       case 0x00: /* l.j */
+       case 0x01: /* l.jal */
+       case 0x03: /* l.bnf */
+       case 0x04: /* l.bf */
+       case 0x11: /* l.jr */
+       case 0x12: /* l.jalr */
+               return 1;
+       default:
+               return 0;
+       }
+#else
+       return regs->sr & SPR_SR_DSX;
+#endif
+}
+
+static inline void adjust_pc(struct pt_regs *regs, unsigned long address)
+{
+       int displacement;
+       unsigned int rb, op, jmp;
+
+       if (unlikely(in_delay_slot(regs))) {
+               /* In delay slot, instruction at pc is a branch, simulate it */
+               jmp = *((unsigned int *)regs->pc);
+
+               displacement = sign_extend32(((jmp) & 0x3ffffff) << 2, 27);
+               rb = (jmp & 0x0000ffff) >> 11;
+               op = jmp >> 26;
+
+               switch (op) {
+               case 0x00: /* l.j */
+                       regs->pc += displacement;
+                       return;
+               case 0x01: /* l.jal */
+                       regs->pc += displacement;
+                       regs->gpr[9] = regs->pc + 8;
+                       return;
+               case 0x03: /* l.bnf */
+                       if (regs->sr & SPR_SR_F)
+                               regs->pc += 8;
+                       else
+                               regs->pc += displacement;
+                       return;
+               case 0x04: /* l.bf */
+                       if (regs->sr & SPR_SR_F)
+                               regs->pc += displacement;
+                       else
+                               regs->pc += 8;
+                       return;
+               case 0x11: /* l.jr */
+                       regs->pc = regs->gpr[rb];
+                       return;
+               case 0x12: /* l.jalr */
+                       regs->pc = regs->gpr[rb];
+                       regs->gpr[9] = regs->pc + 8;
+                       return;
+               default:
+                       break;
+               }
+       } else {
+               regs->pc += 4;
+       }
+}
+
+static inline void simulate_lwa(struct pt_regs *regs, unsigned long address,
+                               unsigned int insn)
+{
+       unsigned int ra, rd;
+       unsigned long value;
+       unsigned long orig_pc;
+       long imm;
+
+       const struct exception_table_entry *entry;
+
+       orig_pc = regs->pc;
+       adjust_pc(regs, address);
+
+       ra = (insn >> 16) & 0x1f;
+       rd = (insn >> 21) & 0x1f;
+       imm = (short)insn;
+       lwa_addr = (unsigned long __user *)(regs->gpr[ra] + imm);
+
+       if ((unsigned long)lwa_addr & 0x3) {
+               do_unaligned_access(regs, address);
+               return;
+       }
+
+       if (get_user(value, lwa_addr)) {
+               if (user_mode(regs)) {
+                       force_sig(SIGSEGV, current);
+                       return;
+               }
+
+               if ((entry = search_exception_tables(orig_pc))) {
+                       regs->pc = entry->fixup;
+                       return;
+               }
+
+               /* kernel access in kernel space, load it directly */
+               value = *((unsigned long *)lwa_addr);
+       }
+
+       lwa_flag = 1;
+       regs->gpr[rd] = value;
+}
+
+static inline void simulate_swa(struct pt_regs *regs, unsigned long address,
+                               unsigned int insn)
+{
+       unsigned long __user *vaddr;
+       unsigned long orig_pc;
+       unsigned int ra, rb;
+       long imm;
+
+       const struct exception_table_entry *entry;
+
+       orig_pc = regs->pc;
+       adjust_pc(regs, address);
+
+       ra = (insn >> 16) & 0x1f;
+       rb = (insn >> 11) & 0x1f;
+       imm = (short)(((insn & 0x2200000) >> 10) | (insn & 0x7ff));
+       vaddr = (unsigned long __user *)(regs->gpr[ra] + imm);
+
+       if (!lwa_flag || vaddr != lwa_addr) {
+               regs->sr &= ~SPR_SR_F;
+               return;
+       }
+
+       if ((unsigned long)vaddr & 0x3) {
+               do_unaligned_access(regs, address);
+               return;
+       }
+
+       if (put_user(regs->gpr[rb], vaddr)) {
+               if (user_mode(regs)) {
+                       force_sig(SIGSEGV, current);
+                       return;
+               }
+
+               if ((entry = search_exception_tables(orig_pc))) {
+                       regs->pc = entry->fixup;
+                       return;
+               }
+
+               /* kernel access in kernel space, store it directly */
+               *((unsigned long *)vaddr) = regs->gpr[rb];
+       }
+
+       lwa_flag = 0;
+       regs->sr |= SPR_SR_F;
+}
+
+#define INSN_LWA       0x1b
+#define INSN_SWA       0x33
+
 asmlinkage void do_illegal_instruction(struct pt_regs *regs,
                                       unsigned long address)
 {
        siginfo_t info;
+       unsigned int op;
+       unsigned int insn = *((unsigned int *)address);
+
+       op = insn >> 26;
+
+       switch (op) {
+       case INSN_LWA:
+               simulate_lwa(regs, address, insn);
+               return;
+
+       case INSN_SWA:
+               simulate_swa(regs, address, insn);
+               return;
+
+       default:
+               break;
+       }
 
        if (user_mode(regs)) {
                /* Send a SIGILL */