1 files changed, 78 insertions, 7 deletions

diff --git a/arch/avr32/mach-at32ap/pio.c b/arch/avr32/mach-at32ap/pio.c
index d3aabfca8598..f1280ed8ed6d 100644
--- a/arch/avr32/mach-at32ap/pio.c
+++ b/arch/avr32/mach-at32ap/pio.c
@@ -25,27 +25,98 @@ struct pio_device {
         void __iomem *regs;
         const struct platform_device *pdev;
         struct clk *clk;
-        u32 alloc_mask;
+        u32 pinmux_mask;
         char name[32];
 };
 
 static struct pio_device pio_dev[MAX_NR_PIO_DEVICES];
 
-void portmux_set_func(unsigned int portmux_id, unsigned int pin_id,
-                      unsigned int function_id)
+static struct pio_device *gpio_to_pio(unsigned int gpio)
 {
         struct pio_device *pio;
-        u32 mask = 1 << pin_id;
+        unsigned int index;
 
-        BUG_ON(portmux_id >= MAX_NR_PIO_DEVICES);
+        index = gpio >> 5;
+        if (index >= MAX_NR_PIO_DEVICES)
+                return NULL;
+        pio = &pio_dev[index];
+        if (!pio->regs)
+                return NULL;
 
-        pio = &pio_dev[portmux_id];
+        return pio;
+}
+
+/* Pin multiplexing API */
+
+void __init at32_select_periph(unsigned int pin, 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);
+        if (unlikely(!pio)) {
+                printk("pio: invalid pin %u\n", pin);
+                goto fail;
+        }
 
-        if (function_id)
+        if (unlikely(test_and_set_bit(pin_index, &pio->pinmux_mask))) {
+                printk("%s: pin %u is busy\n", pio->name, pin_index);
+                goto fail;
+        }
+
+        pio_writel(pio, PUER, mask);
+        if (periph)
                 pio_writel(pio, BSR, mask);
         else
                 pio_writel(pio, ASR, mask);
+
         pio_writel(pio, PDR, mask);
+        if (!(flags & AT32_GPIOF_PULLUP))
+                pio_writel(pio, PUDR, mask);
+
+        return;
+
+fail:
+        dump_stack();
+}
+
+void __init at32_select_gpio(unsigned int pin, unsigned long flags)
+{
+        struct pio_device *pio;
+        unsigned int pin_index = pin & 0x1f;
+        u32 mask = 1 << pin_index;
+
+        pio = gpio_to_pio(pin);
+        if (unlikely(!pio)) {
+                printk("pio: invalid pin %u\n", pin);
+                goto fail;
+        }
+
+        if (unlikely(test_and_set_bit(pin_index, &pio->pinmux_mask))) {
+                printk("%s: pin %u is busy\n", pio->name, pin_index);
+                goto fail;
+        }
+
+        pio_writel(pio, PUER, mask);
+        if (flags & AT32_GPIOF_HIGH)
+                pio_writel(pio, SODR, mask);
+        else
+                pio_writel(pio, CODR, mask);
+        if (flags & AT32_GPIOF_OUTPUT)
+                pio_writel(pio, OER, mask);
+        else
+                pio_writel(pio, ODR, mask);
+
+        pio_writel(pio, PER, mask);
+        if (!(flags & AT32_GPIOF_PULLUP))
+                pio_writel(pio, PUDR, mask);
+
+        return;
+
+fail:
+        dump_stack();
 }
 
 static int __init pio_probe(struct platform_device *pdev)