[PATCH 3/5] gpio: thunderx: Configure GPIO pins at probe

[Piyush Malgujar <pmalgujar@marvell.com>]

Add support to configure GPIO pins using DTS 'pin-cfg'

Signed-off-by: Piyush Malgujar <pmalgujar@marvell.com>
---
 drivers/gpio/gpio-thunderx.c | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/drivers/gpio/gpio-thunderx.c b/drivers/gpio/gpio-thunderx.c
index bb2b40e4033b00134af35592b6b7c7f83cf6c737..451c412512450fea717937376002d2ba35d1c508 100644
--- a/drivers/gpio/gpio-thunderx.c
+++ b/drivers/gpio/gpio-thunderx.c
@@ -426,6 +426,32 @@ static void *thunderx_gpio_populate_parent_alloc_info(struct gpio_chip *chip,
 	return info;
 }
 
+static void thunderx_gpio_pinsel(struct device *dev,
+				 struct thunderx_gpio *txgpio)
+{
+	struct device_node *node;
+	const __be32 *pinsel;
+	int npins, rlen, i;
+	u32 pin, sel;
+
+	node = dev_of_node(dev);
+	if (!node)
+		return;
+
+	pinsel = of_get_property(node, "pin-cfg", &rlen);
+	if (!pinsel || rlen % 2)
+		return;
+
+	npins = rlen / sizeof(__be32) / 2;
+
+	for (i = 0; i < npins; i++) {
+		pin = of_read_number(pinsel++, 1);
+		sel = of_read_number(pinsel++, 1);
+		dev_dbg(dev, "Set GPIO pin %d CFG register to %x\n", pin, sel);
+		writeq(sel, txgpio->register_base + bit_cfg_reg(pin));
+	}
+}
+
 static int thunderx_gpio_probe(struct pci_dev *pdev,
 			       const struct pci_device_id *id)
 {
@@ -548,6 +574,9 @@ static int thunderx_gpio_probe(struct pci_dev *pdev,
 	if (err)
 		goto out;
 
+	/* Configure default functions of GPIO pins */
+	thunderx_gpio_pinsel(dev, txgpio);
+
 	/* Push on irq_data and the domain for each line. */
 	for (i = 0; i < ngpio; i++) {
 		struct irq_fwspec fwspec;
-- 
2.17.1