MXC: Lets handle IRQ by priority, defined with exported API function

Signed-off-by: Darius Augulis <augulis.darius@gmail.com>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

1 files changed, 11 insertions, 1 deletions

diff --git a/arch/arm/plat-mxc/include/mach/entry-macro.S b/arch/arm/plat-mxc/include/mach/entry-macro.S
index b542433afb1b..11632028f7d1 100644
--- a/arch/arm/plat-mxc/include/mach/entry-macro.S
+++ b/arch/arm/plat-mxc/include/mach/entry-macro.S
@@ -9,11 +9,17 @@
  * published by the Free Software Foundation.
  */
 
+#define AVIC_NIMASK     0x04
+
         @ this macro disables fast irq (not implemented)
         .macro  disable_fiq
         .endm
 
         .macro  get_irqnr_preamble, base, tmp
+        ldr     \base, =AVIC_IO_ADDRESS(AVIC_BASE_ADDR)
+#ifdef CONFIG_MXC_IRQ_PRIOR
+        ldr     r4, [\base, #AVIC_NIMASK]
+#endif
         .endm
 
         .macro  arch_ret_to_user, tmp1, tmp2
@@ -23,7 +29,6 @@
         @ and returns its number in irqnr
         @ and returns if an interrupt occured in irqstat
         .macro  get_irqnr_and_base, irqnr, irqstat, base, tmp
-        ldr     \base, =AVIC_IO_ADDRESS(AVIC_BASE_ADDR)
         @ Load offset & priority of the highest priority
         @ interrupt pending from AVIC_NIVECSR
         ldr     \irqstat, [\base, #0x40]
@@ -32,6 +37,11 @@
         mov     \irqnr, \irqstat, asr #16
         @ set zero flag if IRQ + 1 == 0
         adds    \tmp, \irqnr, #1
+#ifdef CONFIG_MXC_IRQ_PRIOR
+        bicne   \tmp, \irqstat, #0xFFFFFFE0
+        strne   \tmp, [\base, #AVIC_NIMASK]
+        streq   r4, [\base, #AVIC_NIMASK]
+#endif
         .endm
 
         @ irq priority table (not used)