1 files changed, 12 insertions, 6 deletions

diff --git a/include/asm-arm/arch-iop13xx/entry-macro.S b/include/asm-arm/arch-iop13xx/entry-macro.S
index 94c50283dc56..a624a7870c64 100644
--- a/include/asm-arm/arch-iop13xx/entry-macro.S
+++ b/include/asm-arm/arch-iop13xx/entry-macro.S
@@ -19,21 +19,27 @@
         .macro  disable_fiq
         .endm
 
+        .macro get_irqnr_preamble, base, tmp
+        mrc     p15, 0, \tmp, c15, c1, 0
+        orr     \tmp, \tmp, #(1 << 6)
+        mcr     p15, 0, \tmp, c15, c1, 0        @ Enable cp6 access
+        .endm
+
         /*
          * Note: a 1-cycle window exists where iintvec will return the value
          * of iintbase, so we explicitly check for "bad zeros"
          */
         .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
-        mrc     p15, 0, \tmp, c15, c1, 0
-        orr     \tmp, \tmp, #(1 << 6)
-        mcr     p15, 0, \tmp, c15, c1, 0        @ Enable cp6 access
-
         mrc     p6, 0, \irqnr, c3, c2, 0        @ Read IINTVEC
         cmp     \irqnr, #0
         mrceq   p6, 0, \irqnr, c3, c2, 0        @ Re-read on potentially bad zero
         adds    \irqstat, \irqnr, #1            @ Check for 0xffffffff
         movne   \irqnr, \irqnr, lsr #2          @ Convert to irqnr
+        .endm
 
-        biceq   \tmp, \tmp, #(1 << 6)
-        mcreq   p15, 0, \tmp, c15, c1, 0        @ Disable cp6 access if no more interrupts
+        .macro arch_ret_to_user, tmp1, tmp2
+        mrc     p15, 0, \tmp1, c15, c1, 0
+        ands    \tmp2, \tmp1, #(1 << 6)
+        bicne   \tmp1, \tmp1, #(1 << 6)
+        mcrne   p15, 0, \tmp1, c15, c1, 0       @ Disable cp6 access
         .endm