ARM: 6154/1: nomadik-gpio: refactor nmk_gpio_set_mode

Extract out a portion of nmk_gpio_set_mode() that can be called with the
lock already held.  This will be used from the pincfg API.

Cc: Alessandro Rubini <rubini@unipv.it>
Acked-by: NLinus Walleij <linus.walleij@stericsson.com>
Signed-off-by: NRabin Vincent <rabin.vincent@stericsson.com>
Signed-off-by: NRussell King <rmk+kernel@arm.linux.org.uk>

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;