openrisc: add l.lwa/l.swa emulation

This adds an emulation layer for implementations
that lack the l.lwa and l.swa instructions.
It handles these instructions both in kernel space and
user space.

Signed-off-by: Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>
[shorne@gmail.com: Added delay slot pc adjust logic]
Signed-off-by: Stafford Horne <shorne@gmail.com>

1 files changed, 183 insertions, 0 deletions

diff --git a/arch/openrisc/kernel/traps.c b/arch/openrisc/kernel/traps.c
index a4574cb4b0fb..7907b6cf5d8a 100644
--- 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 */