ARM i.MX tzic: add handle_irq function

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

2 files changed, 27 insertions, 1 deletions

diff --git a/arch/arm/plat-mxc/include/mach/common.h b/arch/arm/plat-mxc/include/mach/common.h
index 6a6182df1870..893ec911e666 100644
--- a/arch/arm/plat-mxc/include/mach/common.h
+++ b/arch/arm/plat-mxc/include/mach/common.h
@@ -74,6 +74,7 @@ extern int mx53_revision(void);
 extern int mx53_display_revision(void);
 
 void avic_handle_irq(struct pt_regs *);
+void tzic_handle_irq(struct pt_regs *);
 
 #define imx1_handle_irq avic_handle_irq
 #define imx21_handle_irq avic_handle_irq
@@ -81,5 +82,8 @@ void avic_handle_irq(struct pt_regs *);
 #define imx27_handle_irq avic_handle_irq
 #define imx31_handle_irq avic_handle_irq
 #define imx35_handle_irq avic_handle_irq
+#define imx50_handle_irq tzic_handle_irq
+#define imx51_handle_irq tzic_handle_irq
+#define imx53_handle_irq tzic_handle_irq
 
 #endif
diff --git a/arch/arm/plat-mxc/tzic.c b/arch/arm/plat-mxc/tzic.c
index f257fccdc394..b7a272d16d37 100644
--- a/arch/arm/plat-mxc/tzic.c
+++ b/arch/arm/plat-mxc/tzic.c
@@ -42,7 +42,7 @@
 #define TZIC_SRCCLAR0   0x0280  /* Source Clear Register 0 */
 #define TZIC_PRIORITY0  0x0400  /* Priority Register 0 */
 #define TZIC_PND0       0x0D00  /* Pending Register 0 */
-#define TZIC_HIPND0     0x0D80  /* High Priority Pending Register */
+#define TZIC_HIPND(i)   (0x0D80+ ((i) << 2))    /* High Priority Pending Register */
 #define TZIC_WAKEUP0(i) (0x0E00 + ((i) << 2))   /* Wakeup Config Register */
 #define TZIC_SWINT      0x0F00  /* Software Interrupt Rigger Register */
 #define TZIC_ID0        0x0FD0  /* Indentification Register 0 */
@@ -96,6 +96,28 @@ static __init void tzic_init_gc(unsigned int irq_start)
         irq_setup_generic_chip(gc, IRQ_MSK(32), 0, IRQ_NOREQUEST, 0);
 }
 
+asmlinkage void __exception_irq_entry tzic_handle_irq(struct pt_regs *regs)
+{
+        u32 stat;
+        int i, irqofs, handled;
+
+        do {
+                handled = 0;
+
+                for (i = 0; i < 4; i++) {
+                        stat = __raw_readl(tzic_base + TZIC_HIPND(i)) &
+                                __raw_readl(tzic_base + TZIC_INTSEC0(i));
+
+                        while (stat) {
+                                handled = 1;
+                                irqofs = fls(stat) - 1;
+                                handle_IRQ(irqofs + i * 32, regs);
+                                stat &= ~(1 << irqofs);
+                        }
+                }
+        } while (handled);
+}
+
 /*
  * This function initializes the TZIC hardware and disables all the
  * interrupts. It registers the interrupt enable and disable functions