[v2,1/2] gpio: Add gpio latch driver

diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 0642f579196f2..e4603810ec910 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -1690,6 +1690,12 @@  config GPIO_AGGREGATOR
 	      industrial control context, to be operated from userspace using
 	      the GPIO chardev interface.
 
+config GPIO_LATCH
+	tristate "GPIO latch driver"
+	help
+	  Say yes here to enable a driver for GPIO multiplexers based on latches
+	  connected to other GPIOs.
+
 config GPIO_MOCKUP
 	tristate "GPIO Testing Driver"
 	select IRQ_SIM
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index a0985d30f51bb..310fa08decc69 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -75,6 +75,7 @@  obj-$(CONFIG_GPIO_IT87)			+= gpio-it87.o
 obj-$(CONFIG_GPIO_IXP4XX)		+= gpio-ixp4xx.o
 obj-$(CONFIG_GPIO_JANZ_TTL)		+= gpio-janz-ttl.o
 obj-$(CONFIG_GPIO_KEMPLD)		+= gpio-kempld.o
+obj-$(CONFIG_GPIO_LATCH)		+= gpio-latch.o
 obj-$(CONFIG_GPIO_LOGICVC)		+= gpio-logicvc.o
 obj-$(CONFIG_GPIO_LOONGSON1)		+= gpio-loongson1.o
 obj-$(CONFIG_GPIO_LOONGSON)		+= gpio-loongson.o
diff --git a/drivers/gpio/gpio-latch.c b/drivers/gpio/gpio-latch.c
new file mode 100644
index 0000000000000..4068ed4066272
--- /dev/null
+++ b/drivers/gpio/gpio-latch.c
@@ -0,0 +1,190 @@ 
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * GPIO latch driver
+ *
+ *  Copyright (C) 2022 Sascha Hauer <s.hauer@pengutronix.de>
+ *
+ * This driver implements a GPIO (or better GPO as there is no input)
+ * multiplexer based on latches like this:
+ *
+ * CLK0 ----------------------.        ,--------.
+ * CLK1 -------------------.  `--------|>    #0 |
+ *                         |           |        |
+ * OUT0 ----------------+--|-----------|D0    Q0|-----|<
+ * OUT1 --------------+-|--|-----------|D1    Q1|-----|<
+ * OUT2 ------------+-|-|--|-----------|D2    Q2|-----|<
+ * OUT3 ----------+-|-|-|--|-----------|D3    Q3|-----|<
+ * OUT4 --------+-|-|-|-|--|-----------|D4    Q4|-----|<
+ * OUT5 ------+-|-|-|-|-|--|-----------|D5    Q5|-----|<
+ * OUT6 ----+-|-|-|-|-|-|--|-----------|D6    Q6|-----|<
+ * OUT7 --+-|-|-|-|-|-|-|--|-----------|D7    Q7|-----|<
+ *        | | | | | | | |  |           `--------'
+ *        | | | | | | | |  |
+ *        | | | | | | | |  |           ,--------.
+ *        | | | | | | | |  `-----------|>    #1 |
+ *        | | | | | | | |              |        |
+ *        | | | | | | | `--------------|D0    Q0|-----|<
+ *        | | | | | | `----------------|D1    Q1|-----|<
+ *        | | | | | `------------------|D2    Q2|-----|<
+ *        | | | | `--------------------|D3    Q3|-----|<
+ *        | | | `----------------------|D4    Q4|-----|<
+ *        | | `------------------------|D5    Q5|-----|<
+ *        | `--------------------------|D6    Q6|-----|<
+ *        `----------------------------|D7    Q7|-----|<
+ *                                     `--------'
+ *
+ * The above is just an example. The actual number of number of latches and
+ * the number of inputs per latch is derived from the number of GPIOs given
+ * in the corresponding device tree properties.
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/gpio/driver.h>
+#include <linux/platform_device.h>
+#include <linux/gpio/consumer.h>
+
+#include "gpiolib.h"
+
+struct gpio_latch_priv {
+	struct gpio_chip gc;
+	struct gpio_descs *clk_gpios;
+	struct gpio_descs *data_gpios;
+	spinlock_t lock;
+	int n_ports;
+	int n_pins;
+	unsigned int *shadow;
+	struct mutex mutex;
+	spinlock_t spinlock;
+};
+
+static int gpio_latch_get_direction(struct gpio_chip *gc, unsigned int offset)
+{
+	return GPIO_LINE_DIRECTION_OUT;
+}
+
+static void __gpio_latch_set(struct gpio_latch_priv *priv,
+			     void (* set)(struct gpio_desc *desc, int value),
+			     unsigned int offset, int val)
+{
+	int latch = offset / priv->n_pins;
+	int i;
+
+	if (val)
+		priv->shadow[latch] |= BIT(offset % priv->n_pins);
+	else
+		priv->shadow[latch] &= ~BIT(offset % priv->n_pins);
+
+	for (i = 0; i < priv->n_pins; i++)
+		set(priv->data_gpios->desc[i], priv->shadow[latch] & BIT(i));
+
+	set(priv->clk_gpios->desc[latch], 1);
+	set(priv->clk_gpios->desc[latch], 0);
+}
+
+static void gpio_latch_set(struct gpio_chip *gc, unsigned int offset, int val)
+{
+	struct gpio_latch_priv *priv = gpiochip_get_data(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->spinlock, flags);
+
+	__gpio_latch_set(priv, gpiod_set_value, offset, val);
+
+	spin_unlock_irqrestore(&priv->spinlock, flags);
+}
+
+static void gpio_latch_set_can_sleep(struct gpio_chip *gc, unsigned int offset, int val)
+{
+	struct gpio_latch_priv *priv = gpiochip_get_data(gc);
+
+	mutex_lock(&priv->mutex);
+
+	__gpio_latch_set(priv, gpiod_set_value_cansleep, offset, val);
+
+	mutex_unlock(&priv->mutex);
+}
+
+static bool gpio_latch_can_sleep(struct gpio_latch_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < priv->n_ports; i++)
+		if (gpiod_cansleep(priv->clk_gpios->desc[i]))
+			return true;
+
+	for (i = 0; i < priv->n_pins; i++)
+		if (gpiod_cansleep(priv->data_gpios->desc[i]))
+			return true;
+
+	return false;
+}
+
+static int gpio_latch_probe(struct platform_device *pdev)
+{
+	struct gpio_latch_priv *priv;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->clk_gpios = devm_gpiod_get_array(&pdev->dev, "clk", GPIOD_OUT_LOW);
+	if (IS_ERR(priv->clk_gpios))
+		return PTR_ERR(priv->clk_gpios);
+
+	priv->data_gpios = devm_gpiod_get_array(&pdev->dev, "data", GPIOD_OUT_LOW);
+	if (IS_ERR(priv->data_gpios))
+		return PTR_ERR(priv->data_gpios);
+
+	priv->n_ports = priv->clk_gpios->ndescs;
+	priv->n_pins = priv->data_gpios->ndescs;
+
+	priv->shadow = devm_kcalloc(&pdev->dev, priv->n_ports, sizeof(*priv->shadow),
+				    GFP_KERNEL);
+	if (!priv->shadow)
+		return -ENOMEM;
+
+	if (gpio_latch_can_sleep(priv)) {
+		priv->gc.can_sleep = true;
+		priv->gc.set = gpio_latch_set_can_sleep;
+		mutex_init(&priv->mutex);
+	} else {
+		priv->gc.can_sleep = false;
+		priv->gc.set = gpio_latch_set;
+		spin_lock_init(&priv->spinlock);
+	}
+
+	priv->gc.get_direction = gpio_latch_get_direction;
+	priv->gc.ngpio = priv->n_ports * priv->n_pins;
+	priv->gc.owner = THIS_MODULE;
+	priv->gc.base = -1;
+	priv->gc.parent = &pdev->dev;
+	priv->gc.of_node = pdev->dev.of_node;
+
+	platform_set_drvdata(pdev, priv);
+
+	return devm_gpiochip_add_data(&pdev->dev, &priv->gc, priv);
+}
+
+static const struct of_device_id gpio_latch_ids[] = {
+	{
+		.compatible	= "gpio-latch",
+	}, {
+		/* sentinel */
+	}
+};
+MODULE_DEVICE_TABLE(of, gpio_latch_ids);
+
+static struct platform_driver gpio_latch_driver = {
+	.driver	= {
+		.name		= "gpio-latch",
+		.of_match_table	= gpio_latch_ids,
+	},
+	.probe	= gpio_latch_probe,
+};
+module_platform_driver(gpio_latch_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Sascha Hauer <s.hauer@pengutronix.de>");
+MODULE_DESCRIPTION("GPIO latch driver");