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>

diff --git a/arch/openrisc/kernel/entry.S b/arch/openrisc/kernel/entry.S
index aac0bde..ba1a361 100644 (file)
--- a/arch/openrisc/kernel/entry.S
+++ b/arch/openrisc/kernel/entry.S
@@ -173,6 +173,11 @@ handler:                                                   ;\
        l.j     _ret_from_exception                             ;\
         l.nop
 
+/* clobbers 'reg' */
+#define CLEAR_LWA_FLAG(reg)            \
+       l.movhi reg,hi(lwa_flag)        ;\
+       l.ori   reg,reg,lo(lwa_flag)    ;\
+       l.sw    0(reg),r0
 /*
  * NOTE: one should never assume that SPR_EPC, SPR_ESR, SPR_EEAR
  *       contain the same values as when exception we're handling
@@ -193,6 +198,7 @@ EXCEPTION_ENTRY(_tng_kernel_start)
 /* ---[ 0x200: BUS exception ]------------------------------------------- */
 
 EXCEPTION_ENTRY(_bus_fault_handler)
+       CLEAR_LWA_FLAG(r3)
        /* r4: EA of fault (set by EXCEPTION_HANDLE) */
        l.jal   do_bus_fault
         l.addi  r3,r1,0 /* pt_regs */
@@ -202,11 +208,13 @@ EXCEPTION_ENTRY(_bus_fault_handler)
 
 /* ---[ 0x300: Data Page Fault exception ]------------------------------- */
 EXCEPTION_ENTRY(_dtlb_miss_page_fault_handler)
+       CLEAR_LWA_FLAG(r3)
        l.and   r5,r5,r0
        l.j     1f
         l.nop
 
 EXCEPTION_ENTRY(_data_page_fault_handler)
+       CLEAR_LWA_FLAG(r3)
        /* set up parameters for do_page_fault */
        l.ori   r5,r0,0x300                // exception vector
 1:
@@ -282,11 +290,13 @@ EXCEPTION_ENTRY(_data_page_fault_handler)
 
 /* ---[ 0x400: Insn Page Fault exception ]------------------------------- */
 EXCEPTION_ENTRY(_itlb_miss_page_fault_handler)
+       CLEAR_LWA_FLAG(r3)
        l.and   r5,r5,r0
        l.j     1f
         l.nop
 
 EXCEPTION_ENTRY(_insn_page_fault_handler)
+       CLEAR_LWA_FLAG(r3)
        /* set up parameters for do_page_fault */
        l.ori   r5,r0,0x400                // exception vector
 1:
@@ -304,6 +314,7 @@ EXCEPTION_ENTRY(_insn_page_fault_handler)
 /* ---[ 0x500: Timer exception ]----------------------------------------- */
 
 EXCEPTION_ENTRY(_timer_handler)
+       CLEAR_LWA_FLAG(r3)
        l.jal   timer_interrupt
         l.addi r3,r1,0 /* pt_regs */
 
@@ -313,6 +324,7 @@ EXCEPTION_ENTRY(_timer_handler)
 /* ---[ 0x600: Aligment exception ]-------------------------------------- */
 
 EXCEPTION_ENTRY(_alignment_handler)
+       CLEAR_LWA_FLAG(r3)
        /* r4: EA of fault (set by EXCEPTION_HANDLE) */
        l.jal   do_unaligned_access
         l.addi  r3,r1,0 /* pt_regs */
@@ -509,6 +521,7 @@ EXCEPTION_ENTRY(_external_irq_handler)
 //     l.sw    PT_SR(r1),r4
 1:
 #endif
+       CLEAR_LWA_FLAG(r3)
        l.addi  r3,r1,0
        l.movhi r8,hi(do_IRQ)
        l.ori   r8,r8,lo(do_IRQ)
@@ -556,8 +569,12 @@ ENTRY(_sys_call_handler)
         * they should be clobbered, otherwise
         */
        l.sw    PT_GPR3(r1),r3
-       /* r4 already saved */
-       /* r4 holds the EEAR address of the fault, load the original r4 */
+       /*
+        * r4 already saved
+        * r4 holds the EEAR address of the fault, use it as screatch reg and
+        * then load the original r4
+        */
+       CLEAR_LWA_FLAG(r4)
        l.lwz   r4,PT_GPR4(r1)
        l.sw    PT_GPR5(r1),r5
        l.sw    PT_GPR6(r1),r6
@@ -776,6 +793,7 @@ UNHANDLED_EXCEPTION(_vector_0xd00,0xd00)
 /* ---[ 0xe00: Trap exception ]------------------------------------------ */
 
 EXCEPTION_ENTRY(_trap_handler)
+       CLEAR_LWA_FLAG(r3)
        /* r4: EA of fault (set by EXCEPTION_HANDLE) */
        l.jal   do_trap
         l.addi  r3,r1,0 /* pt_regs */

diff --git a/arch/openrisc/kernel/process.c b/arch/openrisc/kernel/process.c
index d7990df..c49350b 100644 (file)
--- a/arch/openrisc/kernel/process.c
+++ b/arch/openrisc/kernel/process.c
@@ -226,6 +226,7 @@ int dump_fpu(struct pt_regs *regs, elf_fpregset_t * fpu)
 
 extern struct thread_info *_switch(struct thread_info *old_ti,
                                   struct thread_info *new_ti);
+extern int lwa_flag;
 
 struct task_struct *__switch_to(struct task_struct *old,
                                struct task_struct *new)
@@ -243,6 +244,8 @@ struct task_struct *__switch_to(struct task_struct *old,
        new_ti = new->stack;
        old_ti = old->stack;
 
+       lwa_flag = 0;
+
        current_thread_info_set[smp_processor_id()] = new_ti;
        last = (_switch(old_ti, new_ti))->task;
 

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