3 files changed, 43 insertions, 19 deletions

diff --git a/arch/avr32/Kconfig b/arch/avr32/Kconfig
index 599ec166d833..b77abce10c7a 100644
--- a/arch/avr32/Kconfig
+++ b/arch/avr32/Kconfig
@@ -27,6 +27,12 @@ config GENERIC_HARDIRQS
 config STACKTRACE_SUPPORT
         def_bool y
 
+config LOCKDEP_SUPPORT
+        def_bool y
+
+config TRACE_IRQFLAGS_SUPPORT
+        def_bool y
+
 config HARDIRQS_SW_RESEND
         def_bool y
 
diff --git a/arch/avr32/kernel/entry-avr32b.S b/arch/avr32/kernel/entry-avr32b.S
index ccadfd9b438d..cc2a9b76a344 100644
--- a/arch/avr32/kernel/entry-avr32b.S
+++ b/arch/avr32/kernel/entry-avr32b.S
@@ -527,15 +527,14 @@ fault_exit_work:
 
         /* If we get a debug trap from privileged context we end up here */
 handle_debug_priv:
-        /* Fix up LR and SP in regs. r11 contains the mode we came from */
-        mfsr    r8, SYSREG_SR
-        mov     r9, r8
-        andh    r8, hi(~MODE_MASK)
-        or      r8, r11
-        mtsr    SYSREG_SR, r8
+        /* Fix up LR and SP in regs. r1 contains the mode we came from */
+        mfsr    r2, SYSREG_SR
+        mov     r3, r2
+        bfins   r2, r1, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
+        mtsr    SYSREG_SR, r2
         sub     pc, -2
         stdsp   sp[REG_LR], lr
-        mtsr    SYSREG_SR, r9
+        mtsr    SYSREG_SR, r3
         sub     pc, -2
         sub     r10, sp, -FRAME_SIZE_FULL
         stdsp   sp[REG_SP], r10
@@ -547,15 +546,19 @@ handle_debug_priv:
         popm    r10, r11
         mtsr    SYSREG_RAR_DBG, r10
         mtsr    SYSREG_RSR_DBG, r11
-        mfsr    r8, SYSREG_SR
-        mov     r9, r8
-        andh    r8, hi(~MODE_MASK)
-        andh    r11, hi(MODE_MASK)
-        or      r8, r11
-        mtsr    SYSREG_SR, r8
+#ifdef CONFIG_TRACE_IRQFLAGS
+        bld     r11, SYSREG_GM_OFFSET
+        brcc    1f
+        rcall   trace_hardirqs_on
+1:
+#endif
+        mfsr    r2, SYSREG_SR
+        mov     r3, r2
+        bfins   r2, r1, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
+        mtsr    SYSREG_SR, r2
         sub     pc, -2
         popm    lr
-        mtsr    SYSREG_SR, r9
+        mtsr    SYSREG_SR, r3
         sub     pc, -2
         sub     sp, -4          /* skip SP */
         popm    r0-r12
@@ -572,11 +575,14 @@ handle_debug_priv:
 handle_debug:
         sub     sp, 4           /* r12_orig */
         stmts   --sp, r0-lr
-        mfsr    r10, SYSREG_RAR_DBG
-        mfsr    r11, SYSREG_RSR_DBG
+        mfsr    r0, SYSREG_RAR_DBG
+        mfsr    r1, SYSREG_RSR_DBG
+#ifdef CONFIG_TRACE_IRQFLAGS
+        rcall   trace_hardirqs_off
+#endif
         unmask_exceptions
-        pushm   r10,r11
-        andh    r11, (MODE_MASK >> 16), COH
+        stm     --sp, r0, r1
+        bfextu  r1, r1, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
         brne    handle_debug_priv
 
         mov     r12, sp
@@ -591,6 +597,12 @@ debug_restore_all:
         mask_exceptions
         mtsr    SYSREG_RSR_DBG, r11
         mtsr    SYSREG_RAR_DBG, r10
+#ifdef CONFIG_TRACE_IRQFLAGS
+        bld     r11, SYSREG_GM_OFFSET
+        brcc    1f
+        rcall   trace_hardirqs_on
+1:
+#endif
         ldmts   sp++, r0-lr
         sub     sp, -4
         retd
@@ -675,7 +687,11 @@ irq_level\level:
         andl    r1, _TIF_WORK_MASK, COH
         brne    irq_exit_work
 
-1:      popm    r8-r9
+1:
+#ifdef CONFIG_TRACE_IRQFLAGS
+        rcall   trace_hardirqs_on
+#endif
+        popm    r8-r9
         mtsr    rar_int\level, r8
         mtsr    rsr_int\level, r9
         ldmts   sp++,r0-lr
diff --git a/include/asm-avr32/sysreg.h b/include/asm-avr32/sysreg.h
index dd21182b60e0..d4e0950170ca 100644
--- a/include/asm-avr32/sysreg.h
+++ b/include/asm-avr32/sysreg.h
@@ -93,6 +93,8 @@
 #define SYSREG_I3M_SIZE                         1
 #define SYSREG_EM_OFFSET                        21
 #define SYSREG_EM_SIZE                          1
+#define SYSREG_MODE_OFFSET                      22
+#define SYSREG_MODE_SIZE                        3
 #define SYSREG_M0_OFFSET                        22
 #define SYSREG_M0_SIZE                          1
 #define SYSREG_M1_OFFSET                        23