1 files changed, 13 insertions, 22 deletions

diff --git a/arch/arm/plat-mxc/gpio.c b/arch/arm/plat-mxc/gpio.c
index d65ebe303b9f..71437c61cfd7 100644
--- a/arch/arm/plat-mxc/gpio.c
+++ b/arch/arm/plat-mxc/gpio.c
@@ -3,7 +3,7 @@
  * Copyright 2008 Juergen Beisert, kernel@pengutronix.de
  *
  * Based on code from Freescale,
- * Copyright 2004-2006 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -38,7 +38,6 @@ static int gpio_table_size;
 #define GPIO_ICR2	(cpu_is_mx1_mx2() ? 0x2C : 0x10)
 #define GPIO_IMR	(cpu_is_mx1_mx2() ? 0x30 : 0x14)
 #define GPIO_ISR	(cpu_is_mx1_mx2() ? 0x34 : 0x18)
-#define GPIO_ISR	(cpu_is_mx1_mx2() ? 0x34 : 0x18)
 
 #define GPIO_INT_LOW_LEV	(cpu_is_mx1_mx2() ? 0x3 : 0x0)
 #define GPIO_INT_HIGH_LEV	(cpu_is_mx1_mx2() ? 0x2 : 0x1)
@@ -140,16 +139,13 @@ static void mxc_flip_edge(struct mxc_gpio_port *port, u32 gpio)
 	val = __raw_readl(reg);
 	edge = (val >> (bit << 1)) & 3;
 	val &= ~(0x3 << (bit << 1));
-	switch (edge) {
-	case GPIO_INT_HIGH_LEV:
+	if (edge == GPIO_INT_HIGH_LEV) {
 		edge = GPIO_INT_LOW_LEV;
 		pr_debug("mxc: switch GPIO %d to low trigger\n", gpio);
-		break;
-	case GPIO_INT_LOW_LEV:
+	} else if (edge == GPIO_INT_LOW_LEV) {
 		edge = GPIO_INT_HIGH_LEV;
 		pr_debug("mxc: switch GPIO %d to high trigger\n", gpio);
-		break;
-	default:
+	} else {
 		pr_err("mxc: invalid configuration for GPIO %d: %x\n",
 		       gpio, edge);
 		return;
@@ -157,25 +153,20 @@ static void mxc_flip_edge(struct mxc_gpio_port *port, u32 gpio)
 	__raw_writel(val | (edge << (bit << 1)), reg);
 }
 
-/* handle n interrupts in one status register */
+/* handle 32 interrupts in one status register */
 static void mxc_gpio_irq_handler(struct mxc_gpio_port *port, u32 irq_stat)
 {
-	u32 gpio_irq_no;
-
-	gpio_irq_no = port->virtual_irq_start;
-	for (; irq_stat != 0; irq_stat >>= 1, gpio_irq_no++) {
-		u32 gpio = irq_to_gpio(gpio_irq_no);
+	u32 gpio_irq_no_base = port->virtual_irq_start;
 
-		if ((irq_stat & 1) == 0)
-			continue;
+	while (irq_stat != 0) {
+		int irqoffset = fls(irq_stat) - 1;
 
-		BUG_ON(!(irq_desc[gpio_irq_no].handle_irq));
+		if (port->both_edges & (1 << irqoffset))
+			mxc_flip_edge(port, irqoffset);
 
-		if (port->both_edges & (1 << (gpio & 31)))
-			mxc_flip_edge(port, gpio);
+		generic_handle_irq(gpio_irq_no_base + irqoffset);
 
-		irq_desc[gpio_irq_no].handle_irq(gpio_irq_no,
-				&irq_desc[gpio_irq_no]);
+		irq_stat &= ~(1 << irqoffset);
 	}
 }
 
@@ -297,7 +288,7 @@ int __init mxc_gpio_init(struct mxc_gpio_port *port, int cnt)
 		/* its a serious configuration bug when it fails */
 		BUG_ON( gpiochip_add(&port[i].chip) < 0 );
 
-		if (cpu_is_mx1() || cpu_is_mx3() || cpu_is_mx25()) {
+		if (cpu_is_mx1() || cpu_is_mx3() || cpu_is_mx25() || cpu_is_mx51()) {
 			/* setup one handler for each entry */
 			set_irq_chained_handler(port[i].irq, mx3_gpio_irq_handler);
 			set_irq_data(port[i].irq, &port[i]);