1 files changed, 102 insertions, 28 deletions

diff --git a/drivers/gpio/gpio-mmio.c b/drivers/gpio/gpio-mmio.c
index f7da40e46c55..f9042bcc27a4 100644
--- a/drivers/gpio/gpio-mmio.c
+++ b/drivers/gpio/gpio-mmio.c
@@ -126,20 +126,16 @@ static unsigned long bgpio_read32be(void __iomem *reg)
         return ioread32be(reg);
 }
 
-static unsigned long bgpio_pin2mask(struct gpio_chip *gc, unsigned int pin)
+static unsigned long bgpio_line2mask(struct gpio_chip *gc, unsigned int line)
 {
-        return BIT(pin);
-}
-
-static unsigned long bgpio_pin2mask_be(struct gpio_chip *gc,
-                                       unsigned int pin)
-{
-        return BIT(gc->bgpio_bits - 1 - pin);
+        if (gc->be_bits)
+                return BIT(gc->bgpio_bits - 1 - line);
+        return BIT(line);
 }
 
 static int bgpio_get_set(struct gpio_chip *gc, unsigned int gpio)
 {
-        unsigned long pinmask = gc->pin2mask(gc, gpio);
+        unsigned long pinmask = bgpio_line2mask(gc, gpio);
 
         if (gc->bgpio_dir & pinmask)
                 return !!(gc->read_reg(gc->reg_set) & pinmask);
@@ -147,9 +143,76 @@ static int bgpio_get_set(struct gpio_chip *gc, unsigned int gpio)
                 return !!(gc->read_reg(gc->reg_dat) & pinmask);
 }
 
+/*
+ * This assumes that the bits in the GPIO register are in native endianness.
+ * We only assign the function pointer if we have that.
+ */
+static int bgpio_get_set_multiple(struct gpio_chip *gc, unsigned long *mask,
+                                  unsigned long *bits)
+{
+        unsigned long get_mask = 0;
+        unsigned long set_mask = 0;
+        int bit = 0;
+
+        while ((bit = find_next_bit(mask, gc->ngpio, bit)) != gc->ngpio) {
+                if (gc->bgpio_dir & BIT(bit))
+                        set_mask |= BIT(bit);
+                else
+                        get_mask |= BIT(bit);
+        }
+
+        if (set_mask)
+                *bits |= gc->read_reg(gc->reg_set) & set_mask;
+        if (get_mask)
+                *bits |= gc->read_reg(gc->reg_dat) & get_mask;
+
+        return 0;
+}
+
 static int bgpio_get(struct gpio_chip *gc, unsigned int gpio)
 {
-        return !!(gc->read_reg(gc->reg_dat) & gc->pin2mask(gc, gpio));
+        return !!(gc->read_reg(gc->reg_dat) & bgpio_line2mask(gc, gpio));
+}
+
+/*
+ * This only works if the bits in the GPIO register are in native endianness.
+ * It is dirt simple and fast in this case. (Also the most common case.)
+ */
+static int bgpio_get_multiple(struct gpio_chip *gc, unsigned long *mask,
+                              unsigned long *bits)
+{
+
+        *bits = gc->read_reg(gc->reg_dat) & *mask;
+        return 0;
+}
+
+/*
+ * With big endian mirrored bit order it becomes more tedious.
+ */
+static int bgpio_get_multiple_be(struct gpio_chip *gc, unsigned long *mask,
+                                 unsigned long *bits)
+{
+        unsigned long readmask = 0;
+        unsigned long val;
+        int bit;
+
+        /* Create a mirrored mask */
+        bit = 0;
+        while ((bit = find_next_bit(mask, gc->ngpio, bit)) != gc->ngpio)
+                readmask |= bgpio_line2mask(gc, bit);
+
+        /* Read the register */
+        val = gc->read_reg(gc->reg_dat) & readmask;
+
+        /*
+         * Mirror the result into the "bits" result, this will give line 0
+         * in bit 0 ... line 31 in bit 31 for a 32bit register.
+         */
+        bit = 0;
+        while ((bit = find_next_bit(&val, gc->ngpio, bit)) != gc->ngpio)
+                *bits |= bgpio_line2mask(gc, bit);
+
+        return 0;
 }
 
 static void bgpio_set_none(struct gpio_chip *gc, unsigned int gpio, int val)
@@ -158,7 +221,7 @@ static void bgpio_set_none(struct gpio_chip *gc, unsigned int gpio, int val)
 
 static void bgpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-        unsigned long mask = gc->pin2mask(gc, gpio);
+        unsigned long mask = bgpio_line2mask(gc, gpio);
         unsigned long flags;
 
         spin_lock_irqsave(&gc->bgpio_lock, flags);
@@ -176,7 +239,7 @@ static void bgpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 static void bgpio_set_with_clear(struct gpio_chip *gc, unsigned int gpio,
                                  int val)
 {
-        unsigned long mask = gc->pin2mask(gc, gpio);
+        unsigned long mask = bgpio_line2mask(gc, gpio);
 
         if (val)
                 gc->write_reg(gc->reg_set, mask);
@@ -186,7 +249,7 @@ static void bgpio_set_with_clear(struct gpio_chip *gc, unsigned int gpio,
 
 static void bgpio_set_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-        unsigned long mask = gc->pin2mask(gc, gpio);
+        unsigned long mask = bgpio_line2mask(gc, gpio);
         unsigned long flags;
 
         spin_lock_irqsave(&gc->bgpio_lock, flags);
@@ -216,9 +279,9 @@ static void bgpio_multiple_get_masks(struct gpio_chip *gc,
                         break;
                 if (__test_and_clear_bit(i, mask)) {
                         if (test_bit(i, bits))
-                                *set_mask |= gc->pin2mask(gc, i);
+                                *set_mask |= bgpio_line2mask(gc, i);
                         else
-                                *clear_mask |= gc->pin2mask(gc, i);
+                                *clear_mask |= bgpio_line2mask(gc, i);
                 }
         }
 }
@@ -294,7 +357,7 @@ static int bgpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 
         spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-        gc->bgpio_dir &= ~gc->pin2mask(gc, gpio);
+        gc->bgpio_dir &= ~bgpio_line2mask(gc, gpio);
         gc->write_reg(gc->reg_dir, gc->bgpio_dir);
 
         spin_unlock_irqrestore(&gc->bgpio_lock, flags);
@@ -305,7 +368,7 @@ static int bgpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 static int bgpio_get_dir(struct gpio_chip *gc, unsigned int gpio)
 {
         /* Return 0 if output, 1 of input */
-        return !(gc->read_reg(gc->reg_dir) & gc->pin2mask(gc, gpio));
+        return !(gc->read_reg(gc->reg_dir) & bgpio_line2mask(gc, gpio));
 }
 
 static int bgpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
@@ -316,7 +379,7 @@ static int bgpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 
         spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-        gc->bgpio_dir |= gc->pin2mask(gc, gpio);
+        gc->bgpio_dir |= bgpio_line2mask(gc, gpio);
         gc->write_reg(gc->reg_dir, gc->bgpio_dir);
 
         spin_unlock_irqrestore(&gc->bgpio_lock, flags);
@@ -330,7 +393,7 @@ static int bgpio_dir_in_inv(struct gpio_chip *gc, unsigned int gpio)
 
         spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-        gc->bgpio_dir |= gc->pin2mask(gc, gpio);
+        gc->bgpio_dir |= bgpio_line2mask(gc, gpio);
         gc->write_reg(gc->reg_dir, gc->bgpio_dir);
 
         spin_unlock_irqrestore(&gc->bgpio_lock, flags);
@@ -346,7 +409,7 @@ static int bgpio_dir_out_inv(struct gpio_chip *gc, unsigned int gpio, int val)
 
         spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-        gc->bgpio_dir &= ~gc->pin2mask(gc, gpio);
+        gc->bgpio_dir &= ~bgpio_line2mask(gc, gpio);
         gc->write_reg(gc->reg_dir, gc->bgpio_dir);
 
         spin_unlock_irqrestore(&gc->bgpio_lock, flags);
@@ -357,12 +420,11 @@ static int bgpio_dir_out_inv(struct gpio_chip *gc, unsigned int gpio, int val)
 static int bgpio_get_dir_inv(struct gpio_chip *gc, unsigned int gpio)
 {
         /* Return 0 if output, 1 if input */
-        return !!(gc->read_reg(gc->reg_dir) & gc->pin2mask(gc, gpio));
+        return !!(gc->read_reg(gc->reg_dir) & bgpio_line2mask(gc, gpio));
 }
 
 static int bgpio_setup_accessors(struct device *dev,
                                  struct gpio_chip *gc,
-                                 bool bit_be,
                                  bool byte_be)
 {
 
@@ -406,8 +468,6 @@ static int bgpio_setup_accessors(struct device *dev,
                 return -EINVAL;
         }
 
-        gc->pin2mask = bit_be ? bgpio_pin2mask_be : bgpio_pin2mask;
-
         return 0;
 }
 
@@ -462,10 +522,24 @@ static int bgpio_setup_io(struct gpio_chip *gc,
         }
 
         if (!(flags & BGPIOF_UNREADABLE_REG_SET) &&
-            (flags & BGPIOF_READ_OUTPUT_REG_SET))
+            (flags & BGPIOF_READ_OUTPUT_REG_SET)) {
                 gc->get = bgpio_get_set;
-        else
+                if (!gc->be_bits)
+                        gc->get_multiple = bgpio_get_set_multiple;
+                /*
+                 * We deliberately avoid assigning the ->get_multiple() call
+                 * for big endian mirrored registers which are ALSO reflecting
+                 * their value in the set register when used as output. It is
+                 * simply too much complexity, let the GPIO core fall back to
+                 * reading each line individually in that fringe case.
+                 */
+        } else {
                 gc->get = bgpio_get;
+                if (gc->be_bits)
+                        gc->get_multiple = bgpio_get_multiple_be;
+                else
+                        gc->get_multiple = bgpio_get_multiple;
+        }
 
         return 0;
 }
@@ -526,13 +600,13 @@ int bgpio_init(struct gpio_chip *gc, struct device *dev,
         gc->base = -1;
         gc->ngpio = gc->bgpio_bits;
         gc->request = bgpio_request;
+        gc->be_bits = !!(flags & BGPIOF_BIG_ENDIAN);
 
         ret = bgpio_setup_io(gc, dat, set, clr, flags);
         if (ret)
                 return ret;
 
-        ret = bgpio_setup_accessors(dev, gc, flags & BGPIOF_BIG_ENDIAN,
-                                    flags & BGPIOF_BIG_ENDIAN_BYTE_ORDER);
+        ret = bgpio_setup_accessors(dev, gc, flags & BGPIOF_BIG_ENDIAN_BYTE_ORDER);
         if (ret)
                 return ret;