[U-Boot] [PATCH 1/3] bcm2835_gpio: Add support for pinmux

[Alexander Graf]

On the bcm2835 the GPIO IP block is responsible to control pin muxing
of the configurable pins on the chip.

This adds a simple helper function that allows a device driver to set
pin muxing according to device tree configuration on those devices.

Signed-off-by: Alexander Graf <agraf at suse.de>
---
 arch/arm/mach-bcm283x/include/mach/gpio.h |  1 +
 drivers/gpio/bcm2835_gpio.c               | 40 +++++++++++++++++++++++++++++++
 2 files changed, 41 insertions(+)

diff --git a/arch/arm/mach-bcm283x/include/mach/gpio.h b/arch/arm/mach-bcm283x/include/mach/gpio.h
index daaee52f81..751594d09f 100644
--- a/arch/arm/mach-bcm283x/include/mach/gpio.h
+++ b/arch/arm/mach-bcm283x/include/mach/gpio.h
@@ -62,5 +62,6 @@ struct bcm2835_gpio_platdata {
 };
 
 int bcm2835_gpio_get_func_id(struct udevice *dev, unsigned gpio);
+void bcm2835_gpio_set_pinmux(struct udevice *dev, int handle);
 
 #endif /* _BCM2835_GPIO_H_ */
diff --git a/drivers/gpio/bcm2835_gpio.c b/drivers/gpio/bcm2835_gpio.c
index beaa21853a..209cbed9e6 100644
--- a/drivers/gpio/bcm2835_gpio.c
+++ b/drivers/gpio/bcm2835_gpio.c
@@ -83,6 +83,46 @@ int bcm2835_gpio_get_func_id(struct udevice *dev, unsigned gpio)
 	return (val >> BCM2835_GPIO_FSEL_SHIFT(gpio) & BCM2835_GPIO_FSEL_MASK);
 }
 
+static void bcm2835_gpio_set_func_id(struct udevice *dev, unsigned gpio, int func)
+{
+	struct bcm2835_gpios *gpios = dev_get_priv(dev);
+	u32 val;
+
+	val = readl(&gpios->reg->gpfsel[BCM2835_GPIO_FSEL_BANK(gpio)]);
+	val &= ~(BCM2835_GPIO_FSEL_MASK << BCM2835_GPIO_FSEL_SHIFT(gpio));
+	val |= (func << BCM2835_GPIO_FSEL_SHIFT(gpio));
+	writel(val, &gpios->reg->gpfsel[BCM2835_GPIO_FSEL_BANK(gpio)]);
+}
+
+void bcm2835_gpio_set_pinmux(struct udevice *dev, int handle)
+{
+	int node = fdt_node_offset_by_phandle(gd->fdt_blob, handle);
+	u32 pins[16];
+	int len;
+	u32 func;
+	int i;
+
+	if (!node)
+		goto err;
+
+	func = fdtdec_get_int(gd->fdt_blob, node, "brcm,function", -1);
+	if (func <= 0)
+		goto err;
+
+	len = fdtdec_get_int_array_count(gd->fdt_blob, node, "brcm,pins", pins,
+					 ARRAY_SIZE(pins));
+	if (len <= 0)
+		goto err;
+
+	for (i = 0; i < len; i++)
+		bcm2835_gpio_set_func_id(dev, pins[i], func);
+
+	return;
+err:
+	printf("Failed to pinmux phandle 0x%x\n", handle);
+	return;
+}
+
 static int bcm2835_gpio_get_function(struct udevice *dev, unsigned offset)
 {
 	int funcid = bcm2835_gpio_get_func_id(dev, offset);
-- 
2.12.3