1 files changed, 27 insertions, 30 deletions

diff --git a/arch/arm/plat-nomadik/gpio.c b/arch/arm/plat-nomadik/gpio.c
index 38fc3b5d9872..a8ac545ddadc 100644
--- a/arch/arm/plat-nomadik/gpio.c
+++ b/arch/arm/plat-nomadik/gpio.c
@@ -107,40 +107,37 @@ static void nmk_gpio_irq_ack(unsigned int irq)
         writel(nmk_gpio_get_bitmask(gpio), nmk_chip->addr + NMK_GPIO_IC);
 }
 
-static void nmk_gpio_irq_mask(unsigned int irq)
+static void __nmk_gpio_irq_modify(struct nmk_gpio_chip *nmk_chip,
+                                  int gpio, bool enable)
 {
-        int gpio;
-        struct nmk_gpio_chip *nmk_chip;
-        unsigned long flags;
-        u32 bitmask, reg;
-
-        gpio = NOMADIK_IRQ_TO_GPIO(irq);
-        nmk_chip = get_irq_chip_data(irq);
-        bitmask = nmk_gpio_get_bitmask(gpio);
-        if (!nmk_chip)
-                return;
+        u32 bitmask = nmk_gpio_get_bitmask(gpio);
+        u32 reg;
 
-        /* we must individually clear the two edges */
-        spin_lock_irqsave(&nmk_chip->lock, flags);
+        /* we must individually set/clear the two edges */
         if (nmk_chip->edge_rising & bitmask) {
                 reg = readl(nmk_chip->addr + NMK_GPIO_RIMSC);
-                reg &= ~bitmask;
+                if (enable)
+                        reg |= bitmask;
+                else
+                        reg &= ~bitmask;
                 writel(reg, nmk_chip->addr + NMK_GPIO_RIMSC);
         }
         if (nmk_chip->edge_falling & bitmask) {
                 reg = readl(nmk_chip->addr + NMK_GPIO_FIMSC);
-                reg &= ~bitmask;
+                if (enable)
+                        reg |= bitmask;
+                else
+                        reg &= ~bitmask;
                 writel(reg, nmk_chip->addr + NMK_GPIO_FIMSC);
         }
-        spin_unlock_irqrestore(&nmk_chip->lock, flags);
-};
+}
 
-static void nmk_gpio_irq_unmask(unsigned int irq)
+static void nmk_gpio_irq_modify(unsigned int irq, bool enable)
 {
         int gpio;
         struct nmk_gpio_chip *nmk_chip;
         unsigned long flags;
-        u32 bitmask, reg;
+        u32 bitmask;
 
         gpio = NOMADIK_IRQ_TO_GPIO(irq);
         nmk_chip = get_irq_chip_data(irq);
@@ -148,21 +145,21 @@ static void nmk_gpio_irq_unmask(unsigned int irq)
         if (!nmk_chip)
                 return;
 
-        /* we must individually set the two edges */
         spin_lock_irqsave(&nmk_chip->lock, flags);
-        if (nmk_chip->edge_rising & bitmask) {
-                reg = readl(nmk_chip->addr + NMK_GPIO_RIMSC);
-                reg |= bitmask;
-                writel(reg, nmk_chip->addr + NMK_GPIO_RIMSC);
-        }
-        if (nmk_chip->edge_falling & bitmask) {
-                reg = readl(nmk_chip->addr + NMK_GPIO_FIMSC);
-                reg |= bitmask;
-                writel(reg, nmk_chip->addr + NMK_GPIO_FIMSC);
-        }
+        __nmk_gpio_irq_modify(nmk_chip, gpio, enable);
         spin_unlock_irqrestore(&nmk_chip->lock, flags);
 }
 
+static void nmk_gpio_irq_mask(unsigned int irq)
+{
+        nmk_gpio_irq_modify(irq, false);
+};
+
+static void nmk_gpio_irq_unmask(unsigned int irq)
+{
+        nmk_gpio_irq_modify(irq, true);
+}
+
 static int nmk_gpio_irq_set_type(unsigned int irq, unsigned int type)
 {
         int gpio;