2 files changed, 31 insertions, 28 deletions

diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S
index 0f82098c9bfe..68e41defef9d 100644
--- a/arch/arm/kernel/entry-armv.S
+++ b/arch/arm/kernel/entry-armv.S
@@ -197,13 +197,6 @@ __dabt_svc:
         @ IRQs off again before pulling preserved data off the stack
         @
         disable_irq_notrace
-
-#ifdef CONFIG_TRACE_IRQFLAGS
-        tst     r5, #PSR_I_BIT
-        bleq    trace_hardirqs_on
-        tst     r5, #PSR_I_BIT
-        blne    trace_hardirqs_off
-#endif
         svc_exit r5                             @ return from exception
  UNWIND(.fnend          )
 ENDPROC(__dabt_svc)
@@ -223,12 +216,7 @@ __irq_svc:
         blne    svc_preempt
 #endif
 
-#ifdef CONFIG_TRACE_IRQFLAGS
-        @ The parent context IRQs must have been enabled to get here in
-        @ the first place, so there's no point checking the PSR I bit.
-        bl      trace_hardirqs_on
-#endif
-        svc_exit r5                             @ return from exception
+        svc_exit r5, irq = 1                    @ return from exception
  UNWIND(.fnend          )
 ENDPROC(__irq_svc)
 
@@ -305,12 +293,6 @@ __und_svc_finish:
         @ restore SPSR and restart the instruction
         @
         ldr     r5, [sp, #S_PSR]                @ Get SVC cpsr
-#ifdef CONFIG_TRACE_IRQFLAGS
-        tst     r5, #PSR_I_BIT
-        bleq    trace_hardirqs_on
-        tst     r5, #PSR_I_BIT
-        blne    trace_hardirqs_off
-#endif
         svc_exit r5                             @ return from exception
  UNWIND(.fnend          )
 ENDPROC(__und_svc)
@@ -325,13 +307,6 @@ __pabt_svc:
         @ IRQs off again before pulling preserved data off the stack
         @
         disable_irq_notrace
-
-#ifdef CONFIG_TRACE_IRQFLAGS
-        tst     r5, #PSR_I_BIT
-        bleq    trace_hardirqs_on
-        tst     r5, #PSR_I_BIT
-        blne    trace_hardirqs_off
-#endif
         svc_exit r5                             @ return from exception
  UNWIND(.fnend          )
 ENDPROC(__pabt_svc)
diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S
index 9a8531eadd3d..0bf15e5c40a2 100644
--- a/arch/arm/kernel/entry-header.S
+++ b/arch/arm/kernel/entry-header.S
@@ -74,7 +74,21 @@
         .endm
 
 #ifndef CONFIG_THUMB2_KERNEL
-        .macro  svc_exit, rpsr
+        .macro  svc_exit, rpsr, irq = 0
+        .if     \irq != 0
+#ifdef CONFIG_TRACE_IRQFLAGS
+        @ The parent context IRQs must have been enabled to get here in
+        @ the first place, so there's no point checking the PSR I bit.
+        bl      trace_hardirqs_on
+#endif
+        .else
+#ifdef CONFIG_TRACE_IRQFLAGS
+        tst     \rpsr, #PSR_I_BIT
+        bleq    trace_hardirqs_on
+        tst     \rpsr, #PSR_I_BIT
+        blne    trace_hardirqs_off
+#endif
+        .endif
         msr     spsr_cxsf, \rpsr
 #if defined(CONFIG_CPU_V6)
         ldr     r0, [sp]
@@ -120,7 +134,21 @@
         mov     pc, \reg
         .endm
 #else   /* CONFIG_THUMB2_KERNEL */
-        .macro  svc_exit, rpsr
+        .macro  svc_exit, rpsr, irq = 0
+        .if     \irq != 0
+#ifdef CONFIG_TRACE_IRQFLAGS
+        @ The parent context IRQs must have been enabled to get here in
+        @ the first place, so there's no point checking the PSR I bit.
+        bl      trace_hardirqs_on
+#endif
+        .else
+#ifdef CONFIG_TRACE_IRQFLAGS
+        tst     \rpsr, #PSR_I_BIT
+        bleq    trace_hardirqs_on
+        tst     \rpsr, #PSR_I_BIT
+        blne    trace_hardirqs_off
+#endif
+        .endif
         ldr     lr, [sp, #S_SP]                 @ top of the stack
         ldrd    r0, r1, [sp, #S_LR]             @ calling lr and pc
         clrex                                   @ clear the exclusive monitor