nios2: rework trap handler

Redefine trap handler as below:

 0  N/A        reserved for system calls
 1  SIGUSR1    user-defined signal 1
 2  SIGUSR2    user-defined signal 2
 3  SIGILL     illegal instruction
 4..29         reserved (but implemented to raise SIGILL instead of being undefined)
30  SIGTRAP    KGDB
31  SIGTRAP    trace/breakpoint trap

Signed-off-by: Ley Foon Tan <lftan@altera.com>

2 files changed, 69 insertions, 36 deletions

diff --git a/arch/nios2/kernel/entry.S b/arch/nios2/kernel/entry.S
index 27b006c52e12..1e515ccd698e 100644
--- a/arch/nios2/kernel/entry.S
+++ b/arch/nios2/kernel/entry.S
@@ -92,35 +92,35 @@ exception_table:
 
 trap_table:
         .word   handle_system_call      /* 0  */
-        .word   instruction_trap        /* 1  */
-        .word   instruction_trap        /* 2  */
-        .word   instruction_trap        /* 3  */
-        .word   instruction_trap        /* 4  */
-        .word   instruction_trap        /* 5  */
-        .word   instruction_trap        /* 6  */
-        .word   instruction_trap        /* 7  */
-        .word   instruction_trap        /* 8  */
-        .word   instruction_trap        /* 9  */
-        .word   instruction_trap        /* 10 */
-        .word   instruction_trap        /* 11 */
-        .word   instruction_trap        /* 12 */
-        .word   instruction_trap        /* 13 */
-        .word   instruction_trap        /* 14 */
-        .word   instruction_trap        /* 15 */
-        .word   instruction_trap        /* 16 */
-        .word   instruction_trap        /* 17 */
-        .word   instruction_trap        /* 18 */
-        .word   instruction_trap        /* 19 */
-        .word   instruction_trap        /* 20 */
-        .word   instruction_trap        /* 21 */
-        .word   instruction_trap        /* 22 */
-        .word   instruction_trap        /* 23 */
-        .word   instruction_trap        /* 24 */
-        .word   instruction_trap        /* 25 */
-        .word   instruction_trap        /* 26 */
-        .word   instruction_trap        /* 27 */
-        .word   instruction_trap        /* 28 */
-        .word   instruction_trap        /* 29 */
+        .word   handle_trap_1           /* 1  */
+        .word   handle_trap_2           /* 2  */
+        .word   handle_trap_3           /* 3  */
+        .word   handle_trap_reserved    /* 4  */
+        .word   handle_trap_reserved    /* 5  */
+        .word   handle_trap_reserved    /* 6  */
+        .word   handle_trap_reserved    /* 7  */
+        .word   handle_trap_reserved    /* 8  */
+        .word   handle_trap_reserved    /* 9  */
+        .word   handle_trap_reserved    /* 10 */
+        .word   handle_trap_reserved    /* 11 */
+        .word   handle_trap_reserved    /* 12 */
+        .word   handle_trap_reserved    /* 13 */
+        .word   handle_trap_reserved    /* 14 */
+        .word   handle_trap_reserved    /* 15 */
+        .word   handle_trap_reserved    /* 16 */
+        .word   handle_trap_reserved    /* 17 */
+        .word   handle_trap_reserved    /* 18 */
+        .word   handle_trap_reserved    /* 19 */
+        .word   handle_trap_reserved    /* 20 */
+        .word   handle_trap_reserved    /* 21 */
+        .word   handle_trap_reserved    /* 22 */
+        .word   handle_trap_reserved    /* 23 */
+        .word   handle_trap_reserved    /* 24 */
+        .word   handle_trap_reserved    /* 25 */
+        .word   handle_trap_reserved    /* 26 */
+        .word   handle_trap_reserved    /* 27 */
+        .word   handle_trap_reserved    /* 28 */
+        .word   handle_trap_reserved    /* 29 */
 #ifdef CONFIG_KGDB
         .word   handle_kgdb_breakpoint  /* 30 KGDB breakpoint */
 #else
@@ -455,6 +455,19 @@ handle_kgdb_breakpoint:
         br      ret_from_exception
 #endif
 
+handle_trap_1:
+        call    handle_trap_1_c
+        br      ret_from_exception
+
+handle_trap_2:
+        call    handle_trap_2_c
+        br      ret_from_exception
+
+handle_trap_3:
+handle_trap_reserved:
+        call    handle_trap_3_c
+        br      ret_from_exception
+
 /*
  * Beware - when entering resume, prev (the current task) is
  * in r4, next (the new task) is in r5, don't change these
diff --git a/arch/nios2/kernel/traps.c b/arch/nios2/kernel/traps.c
index b7b97641a9a6..81f7da7b1d55 100644
--- a/arch/nios2/kernel/traps.c
+++ b/arch/nios2/kernel/traps.c
@@ -23,6 +23,17 @@
 
 static DEFINE_SPINLOCK(die_lock);
 
+static void _send_sig(int signo, int code, unsigned long addr)
+{
+        siginfo_t info;
+
+        info.si_signo = signo;
+        info.si_errno = 0;
+        info.si_code = code;
+        info.si_addr = (void __user *) addr;
+        force_sig_info(signo, &info, current);
+}
+
 void die(const char *str, struct pt_regs *regs, long err)
 {
         console_verbose();
@@ -39,16 +50,10 @@ void die(const char *str, struct pt_regs *regs, long err)
 
 void _exception(int signo, struct pt_regs *regs, int code, unsigned long addr)
 {
-        siginfo_t info;
-
         if (!user_mode(regs))
                 die("Exception in kernel mode", regs, signo);
 
-        info.si_signo = signo;
-        info.si_errno = 0;
-        info.si_code = code;
-        info.si_addr = (void __user *) addr;
-        force_sig_info(signo, &info, current);
+        _send_sig(signo, code, addr);
 }
 
 /*
@@ -183,3 +188,18 @@ asmlinkage void unhandled_exception(struct pt_regs *regs, int cause)
 
         pr_emerg("opcode: 0x%08lx\n", *(unsigned long *)(regs->ea));
 }
+
+asmlinkage void handle_trap_1_c(struct pt_regs *fp)
+{
+        _send_sig(SIGUSR1, 0, fp->ea);
+}
+
+asmlinkage void handle_trap_2_c(struct pt_regs *fp)
+{
+        _send_sig(SIGUSR2, 0, fp->ea);
+}
+
+asmlinkage void handle_trap_3_c(struct pt_regs *fp)
+{
+        _send_sig(SIGILL, ILL_ILLTRP, fp->ea);
+}