1 files changed, 60 insertions, 13 deletions

diff --git a/arch/arm/plat-mxc/tzic.c b/arch/arm/plat-mxc/tzic.c
index f257fccdc394..98308ec1f321 100644
--- a/arch/arm/plat-mxc/tzic.c
+++ b/arch/arm/plat-mxc/tzic.c
@@ -17,6 +17,7 @@
 #include <linux/io.h>
 
 #include <asm/mach/irq.h>
+#include <asm/exception.h>
 
 #include <mach/hardware.h>
 #include <mach/common.h>
@@ -42,7 +43,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 */
@@ -72,7 +73,34 @@ static int tzic_set_irq_fiq(unsigned int irq, unsigned int type)
 #define tzic_set_irq_fiq NULL
 #endif
 
-static unsigned int *wakeup_intr[4];
+#ifdef CONFIG_PM
+static void tzic_irq_suspend(struct irq_data *d)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
+	int idx = gc->irq_base >> 5;
+
+	__raw_writel(gc->wake_active, tzic_base + TZIC_WAKEUP0(idx));
+}
+
+static void tzic_irq_resume(struct irq_data *d)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
+	int idx = gc->irq_base >> 5;
+
+	__raw_writel(__raw_readl(tzic_base + TZIC_ENSET0(idx)),
+		     tzic_base + TZIC_WAKEUP0(idx));
+}
+
+#else
+#define tzic_irq_suspend NULL
+#define tzic_irq_resume NULL
+#endif
+
+static struct mxc_extra_irq tzic_extra_irq = {
+#ifdef CONFIG_FIQ
+	.set_irq_fiq = tzic_set_irq_fiq,
+#endif
+};
 
 static __init void tzic_init_gc(unsigned int irq_start)
 {
@@ -82,20 +110,43 @@ static __init void tzic_init_gc(unsigned int irq_start)
 
 	gc = irq_alloc_generic_chip("tzic", 1, irq_start, tzic_base,
 				    handle_level_irq);
-	gc->private = tzic_set_irq_fiq;
+	gc->private = &tzic_extra_irq;
 	gc->wake_enabled = IRQ_MSK(32);
-	wakeup_intr[idx] = &gc->wake_active;
 
 	ct = gc->chip_types;
 	ct->chip.irq_mask = irq_gc_mask_disable_reg;
 	ct->chip.irq_unmask = irq_gc_unmask_enable_reg;
 	ct->chip.irq_set_wake = irq_gc_set_wake;
+	ct->chip.irq_suspend = tzic_irq_suspend;
+	ct->chip.irq_resume = tzic_irq_resume;
 	ct->regs.disable = TZIC_ENCLEAR0(idx);
 	ct->regs.enable = TZIC_ENSET0(idx);
 
 	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
@@ -138,23 +189,19 @@ void __init tzic_init_irq(void __iomem *irqbase)
 /**
  * tzic_enable_wake() - enable wakeup interrupt
  *
- * @param is_idle		1 if called in idle loop (ENSET0 register);
- *				0 to be used when called from low power entry
  * @return			0 if successful; non-zero otherwise
  */
-int tzic_enable_wake(int is_idle)
+int tzic_enable_wake(void)
 {
-	unsigned int i, v;
+	unsigned int i;
 
 	__raw_writel(1, tzic_base + TZIC_DSMINT);
 	if (unlikely(__raw_readl(tzic_base + TZIC_DSMINT) == 0))
 		return -EAGAIN;
 
-	for (i = 0; i < 4; i++) {
-		v = is_idle ? __raw_readl(tzic_base + TZIC_ENSET0(i)) :
-			*wakeup_intr[i];
-		__raw_writel(v, tzic_base + TZIC_WAKEUP0(i));
-	}
+	for (i = 0; i < 4; i++)
+		__raw_writel(__raw_readl(tzic_base + TZIC_ENSET0(i)),
+			     tzic_base + TZIC_WAKEUP0(i));
 
 	return 0;
 }