[PATCH v2] gpio: gpio-pca953x, Add get_multiple function

[Paul Thomas <pthomas8589@gmail.com>]

Implement a get_multiple function for gpio-pca953x. If a driver
leaves get_multiple unimplemented then gpio_chip_get_multiple()
in gpiolib.c takes care of it by calling chip->get() as needed.
For i2c chips this is very inefficient. For example if you do an
8-bit read then instead of a single i2c transaction there are
8 transactions reading the same byte!

This has been tested with max7312 chips on a 5.2 kernel.

Signed-off-by: Paul Thomas <pthomas8589@gmail.com>
---
 changes from v1: rebased to 5.7-rc1

 drivers/gpio/gpio-pca953x.c | 37 +++++++++++++++++++++++++++++++++++++
 1 file changed, 37 insertions(+)

diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index 5638b4e5355f..6317510b0dc3 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -115,6 +115,7 @@ MODULE_DEVICE_TABLE(acpi, pca953x_acpi_ids);
 
 #define MAX_BANK 5
 #define BANK_SZ 8
+#define BANK_SFT 3 /* ilog2(BANK_SZ) */
 #define MAX_LINE	(MAX_BANK * BANK_SZ)
 
 #define NBANK(chip) DIV_ROUND_UP(chip->gpio_chip.ngpio, BANK_SZ)
@@ -466,6 +467,41 @@ static int pca953x_gpio_get_direction(struct gpio_chip *gc, unsigned off)
 	return GPIO_LINE_DIRECTION_OUT;
 }
 
+static int pca953x_gpio_get_multiple(struct gpio_chip *gc,
+				      unsigned long *mask, unsigned long *bits)
+{
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
+	unsigned int reg_val;
+	int offset, value, i, ret = 0;
+	u8 inreg;
+
+	/* Force offset outside the range of i so that
+	 * at least the first relevant register is read
+	 */
+	offset = gc->ngpio;
+	for_each_set_bit(i, mask, gc->ngpio) {
+		/* whenever i goes into a new bank update inreg
+		 * and read the register
+		 */
+		if ((offset >> BANK_SFT) != (i >> BANK_SFT)) {
+			offset = i;
+			inreg = pca953x_recalc_addr(chip, chip->regs->input,
+						    offset, true, false);
+			mutex_lock(&chip->i2c_lock);
+			ret = regmap_read(chip->regmap, inreg, &reg_val);
+			mutex_unlock(&chip->i2c_lock);
+			if (ret < 0)
+				return ret;
+		}
+		/* reg_val is relative to the last read byte,
+		 * so only shift the relative bits
+		 */
+		value = (reg_val >> (i % 8)) & 0x01;
+		__assign_bit(i, bits, value);
+	}
+	return ret;
+}
+
 static void pca953x_gpio_set_multiple(struct gpio_chip *gc,
 				      unsigned long *mask, unsigned long *bits)
 {
@@ -551,6 +587,7 @@ static void pca953x_setup_gpio(struct pca953x_chip *chip, int gpios)
 	gc->get = pca953x_gpio_get_value;
 	gc->set = pca953x_gpio_set_value;
 	gc->get_direction = pca953x_gpio_get_direction;
+	gc->get_multiple = pca953x_gpio_get_multiple;
 	gc->set_multiple = pca953x_gpio_set_multiple;
 	gc->set_config = pca953x_gpio_set_config;
 	gc->can_sleep = true;
-- 
2.17.1