6 files changed, 39 insertions, 18 deletions

diff --git a/arch/xtensa/include/asm/traps.h b/arch/xtensa/include/asm/traps.h
index 28f33a8b7f5f..3ad151aee6af 100644
--- a/arch/xtensa/include/asm/traps.h
+++ b/arch/xtensa/include/asm/traps.h
@@ -65,4 +65,13 @@ static inline void spill_registers(void)
 #endif
 }
 
+struct debug_table {
+        /* Pointer to debug exception handler */
+        void (*debug_exception)(void);
+        /* Temporary register save area */
+        unsigned long debug_save[1];
+};
+
+void debug_exception(void);
+
 #endif /* _XTENSA_TRAPS_H */
diff --git a/arch/xtensa/kernel/asm-offsets.c b/arch/xtensa/kernel/asm-offsets.c
index b123ace3b67c..8fd46c6404ea 100644
--- a/arch/xtensa/kernel/asm-offsets.c
+++ b/arch/xtensa/kernel/asm-offsets.c
@@ -23,6 +23,7 @@
 #include <linux/kbuild.h>
 
 #include <asm/ptrace.h>
+#include <asm/traps.h>
 #include <asm/uaccess.h>
 
 int main(void)
@@ -117,5 +118,10 @@ int main(void)
         DEFINE(_CLONE_UNTRACED, CLONE_UNTRACED);
         DEFINE(PG_ARCH_1, PG_arch_1);
 
+        /* struct debug_table */
+        DEFINE(DT_DEBUG_EXCEPTION,
+               offsetof(struct debug_table, debug_exception));
+        DEFINE(DT_DEBUG_SAVE, offsetof(struct debug_table, debug_save));
+
         return 0;
 }
diff --git a/arch/xtensa/kernel/entry.S b/arch/xtensa/kernel/entry.S
index db5c1765b413..ab7904fd027f 100644
--- a/arch/xtensa/kernel/entry.S
+++ b/arch/xtensa/kernel/entry.S
@@ -789,36 +789,32 @@ ENTRY(debug_exception)
 
         movi    a2, 1 << PS_EXCM_BIT
         or      a2, a0, a2
-        movi    a0, debug_exception     # restore a3, debug jump vector
         wsr     a2, ps
-        xsr     a0, SREG_EXCSAVE + XCHAL_DEBUGLEVEL
 
         /* Switch to kernel/user stack, restore jump vector, and save a0 */
 
         bbsi.l  a2, PS_UM_BIT, 2f       # jump if user mode
 
         addi    a2, a1, -16-PT_SIZE     # assume kernel stack
+3:
+        l32i    a0, a3, DT_DEBUG_SAVE
+        s32i    a1, a2, PT_AREG1
         s32i    a0, a2, PT_AREG0
         movi    a0, 0
-        s32i    a1, a2, PT_AREG1
         s32i    a0, a2, PT_DEPC         # mark it as a regular exception
+        xsr     a3, SREG_EXCSAVE + XCHAL_DEBUGLEVEL
         xsr     a0, depc
         s32i    a3, a2, PT_AREG3
         s32i    a0, a2, PT_AREG2
         mov     a1, a2
+
+        rsr     a2, ps
+        bbsi.l  a2, PS_UM_BIT, _user_exception
         j       _kernel_exception
 
 2:      rsr     a2, excsave1
         l32i    a2, a2, EXC_TABLE_KSTK  # load kernel stack pointer
-        s32i    a0, a2, PT_AREG0
-        movi    a0, 0
-        s32i    a1, a2, PT_AREG1
-        s32i    a0, a2, PT_DEPC
-        xsr     a0, depc
-        s32i    a3, a2, PT_AREG3
-        s32i    a0, a2, PT_AREG2
-        mov     a1, a2
-        j       _user_exception
+        j       3b
 
         /* Debug exception while in exception mode. */
 1:      j       1b      // FIXME!!
diff --git a/arch/xtensa/kernel/head.S b/arch/xtensa/kernel/head.S
index 05e1df943856..bc4f4bf05099 100644
--- a/arch/xtensa/kernel/head.S
+++ b/arch/xtensa/kernel/head.S
@@ -197,11 +197,6 @@ ENTRY(_startup)
         wsr     a2, ps                  # (enable reg-windows; progmode stack)
         rsync
 
-        /* Set up EXCSAVE[DEBUGLEVEL] to point to the Debug Exception Handler.*/
-
-        movi    a2, debug_exception
-        wsr     a2, SREG_EXCSAVE + XCHAL_DEBUGLEVEL
-
 #ifdef CONFIG_SMP
         /*
          * Notice that we assume with SMP that cores have PRID
diff --git a/arch/xtensa/kernel/traps.c b/arch/xtensa/kernel/traps.c
index a8b8dae4139a..e4764f216a7a 100644
--- a/arch/xtensa/kernel/traps.c
+++ b/arch/xtensa/kernel/traps.c
@@ -157,6 +157,8 @@ COPROCESSOR(7),
 
 DEFINE_PER_CPU(unsigned long, exc_table[EXC_TABLE_SIZE/4]);
 
+DEFINE_PER_CPU(struct debug_table, debug_table);
+
 void die(const char*, struct pt_regs*, long);
 
 static inline void
@@ -372,6 +374,15 @@ static void trap_init_excsave(void)
         __asm__ __volatile__("wsr  %0, excsave1\n" : : "a" (excsave1));
 }
 
+static void trap_init_debug(void)
+{
+        unsigned long debugsave = (unsigned long)this_cpu_ptr(&debug_table);
+
+        this_cpu_ptr(&debug_table)->debug_exception = debug_exception;
+        __asm__ __volatile__("wsr %0, excsave" __stringify(XCHAL_DEBUGLEVEL)
+                             :: "a"(debugsave));
+}
+
 /*
  * Initialize dispatch tables.
  *
@@ -415,12 +426,14 @@ void __init trap_init(void)
 
         /* Initialize EXCSAVE_1 to hold the address of the exception table. */
         trap_init_excsave();
+        trap_init_debug();
 }
 
 #ifdef CONFIG_SMP
 void secondary_trap_init(void)
 {
         trap_init_excsave();
+        trap_init_debug();
 }
 #endif
 
diff --git a/arch/xtensa/kernel/vectors.S b/arch/xtensa/kernel/vectors.S
index fc25318e75ad..332e9d635fb6 100644
--- a/arch/xtensa/kernel/vectors.S
+++ b/arch/xtensa/kernel/vectors.S
@@ -601,7 +601,9 @@ ENDPROC(window_overflow_restore_a0_fixup)
 
 ENTRY(_DebugInterruptVector)
 
-        xsr     a0, SREG_EXCSAVE + XCHAL_DEBUGLEVEL
+        xsr     a3, SREG_EXCSAVE + XCHAL_DEBUGLEVEL
+        s32i    a0, a3, DT_DEBUG_SAVE
+        l32i    a0, a3, DT_DEBUG_EXCEPTION
         jx      a0
 
 ENDPROC(_DebugInterruptVector)