1 files changed, 27 insertions, 14 deletions

diff --git a/arch/avr32/mach-at32ap/pio.c b/arch/avr32/mach-at32ap/pio.c
index ef2561e15399..ed81a8bcb22d 100644
--- a/arch/avr32/mach-at32ap/pio.c
+++ b/arch/avr32/mach-at32ap/pio.c
@@ -50,35 +50,48 @@ static struct pio_device *gpio_to_pio(unsigned int gpio)
 }
 
 /* Pin multiplexing API */
+static DEFINE_SPINLOCK(pio_lock);
 
-void __init at32_select_periph(unsigned int pin, unsigned int periph,
-                               unsigned long flags)
+void __init at32_select_periph(unsigned int port, u32 pin_mask,
+                               unsigned int periph, unsigned long flags)
 {
         struct pio_device *pio;
-        unsigned int pin_index = pin & 0x1f;
-        u32 mask = 1 << pin_index;
 
-        pio = gpio_to_pio(pin);
+        /* assign and verify pio */
+        pio = gpio_to_pio(port);
         if (unlikely(!pio)) {
-                printk("pio: invalid pin %u\n", pin);
+                printk(KERN_WARNING "pio: invalid port %u\n", port);
                 goto fail;
         }
 
-        if (unlikely(test_and_set_bit(pin_index, &pio->pinmux_mask)
-                         || gpiochip_is_requested(&pio->chip, pin_index))) {
-                printk("%s: pin %u is busy\n", pio->name, pin_index);
+        /* Test if any of the requested pins is already muxed */
+        spin_lock(&pio_lock);
+        if (unlikely(pio->pinmux_mask & pin_mask)) {
+                printk(KERN_WARNING "%s: pin(s) busy (requested 0x%x, busy 0x%x)\n",
+                       pio->name, pin_mask, pio->pinmux_mask & pin_mask);
+                spin_unlock(&pio_lock);
                 goto fail;
         }
 
-        pio_writel(pio, PUER, mask);
+        pio->pinmux_mask |= pin_mask;
+
+        /* enable pull ups */
+        pio_writel(pio, PUER, pin_mask);
+
+        /* select either peripheral A or B */
         if (periph)
-                pio_writel(pio, BSR, mask);
+                pio_writel(pio, BSR, pin_mask);
         else
-                pio_writel(pio, ASR, mask);
+                pio_writel(pio, ASR, pin_mask);
+
+        /* enable peripheral control */
+        pio_writel(pio, PDR, pin_mask);
 
-        pio_writel(pio, PDR, mask);
+        /* Disable pull ups if not requested. */
         if (!(flags & AT32_GPIOF_PULLUP))
-                pio_writel(pio, PUDR, mask);
+                pio_writel(pio, PUDR, pin_mask);
+
+        spin_unlock(&pio_lock);
 
         return;