1 files changed, 17 insertions, 11 deletions

diff --git a/arch/arm/plat-nomadik/gpio.c b/arch/arm/plat-nomadik/gpio.c
index a678e887e0fe..7b7393f96a9c 100644
--- a/arch/arm/plat-nomadik/gpio.c
+++ b/arch/arm/plat-nomadik/gpio.c
@@ -46,6 +46,22 @@ struct nmk_gpio_chip {
         u32 edge_falling;
 };
 
+static void __nmk_gpio_set_mode(struct nmk_gpio_chip *nmk_chip,
+                                unsigned offset, int gpio_mode)
+{
+        u32 bit = 1 << offset;
+        u32 afunc, bfunc;
+
+        afunc = readl(nmk_chip->addr + NMK_GPIO_AFSLA) & ~bit;
+        bfunc = readl(nmk_chip->addr + NMK_GPIO_AFSLB) & ~bit;
+        if (gpio_mode & NMK_GPIO_ALT_A)
+                afunc |= bit;
+        if (gpio_mode & NMK_GPIO_ALT_B)
+                bfunc |= bit;
+        writel(afunc, nmk_chip->addr + NMK_GPIO_AFSLA);
+        writel(bfunc, nmk_chip->addr + NMK_GPIO_AFSLB);
+}
+
 static void __nmk_gpio_set_slpm(struct nmk_gpio_chip *nmk_chip,
                                 unsigned offset, enum nmk_gpio_slpm mode)
 {
@@ -139,23 +155,13 @@ int nmk_gpio_set_mode(int gpio, int gpio_mode)
 {
         struct nmk_gpio_chip *nmk_chip;
         unsigned long flags;
-        u32 afunc, bfunc, bit;
 
         nmk_chip = get_irq_chip_data(NOMADIK_GPIO_TO_IRQ(gpio));
         if (!nmk_chip)
                 return -EINVAL;
 
-        bit = 1 << (gpio - nmk_chip->chip.base);
-
         spin_lock_irqsave(&nmk_chip->lock, flags);
-        afunc = readl(nmk_chip->addr + NMK_GPIO_AFSLA) & ~bit;
-        bfunc = readl(nmk_chip->addr + NMK_GPIO_AFSLB) & ~bit;
-        if (gpio_mode & NMK_GPIO_ALT_A)
-                afunc |= bit;
-        if (gpio_mode & NMK_GPIO_ALT_B)
-                bfunc |= bit;
-        writel(afunc, nmk_chip->addr + NMK_GPIO_AFSLA);
-        writel(bfunc, nmk_chip->addr + NMK_GPIO_AFSLB);
+        __nmk_gpio_set_mode(nmk_chip, gpio - nmk_chip->chip.base, gpio_mode);
         spin_unlock_irqrestore(&nmk_chip->lock, flags);
 
         return 0;