gpio: gpio-generic: add flag to read out output value from reg_set

[firefly-linux-kernel-4.4.55.git] / drivers / gpio / gpio-generic.c

diff --git a/drivers/gpio/gpio-generic.c b/drivers/gpio/gpio-generic.c
index b92a690f5765c37457147b83ecac3cb810b5641c..9bda3727fac12df7480f451057ed2513fcae2618 100644 (file)
--- a/drivers/gpio/gpio-generic.c
+++ b/drivers/gpio/gpio-generic.c
@@ -135,6 +135,17 @@ static unsigned long bgpio_pin2mask_be(struct bgpio_chip *bgc,
        return 1 << (bgc->bits - 1 - pin);
 }
 
+static int bgpio_get_set(struct gpio_chip *gc, unsigned int gpio)
+{
+       struct bgpio_chip *bgc = to_bgpio_chip(gc);
+       unsigned long pinmask = bgc->pin2mask(bgc, gpio);
+
+       if (bgc->dir & pinmask)
+               return bgc->read_reg(bgc->reg_set) & pinmask;
+       else
+               return bgc->read_reg(bgc->reg_dat) & pinmask;
+}
+
 static int bgpio_get(struct gpio_chip *gc, unsigned int gpio)
 {
        struct bgpio_chip *bgc = to_bgpio_chip(gc);
@@ -416,7 +427,8 @@ static int bgpio_setup_accessors(struct device *dev,
 static int bgpio_setup_io(struct bgpio_chip *bgc,
                          void __iomem *dat,
                          void __iomem *set,
-                         void __iomem *clr)
+                         void __iomem *clr,
+                         unsigned long flags)
 {
 
        bgc->reg_dat = dat;
@@ -437,7 +449,11 @@ static int bgpio_setup_io(struct bgpio_chip *bgc,
                bgc->gc.set_multiple = bgpio_set_multiple;
        }
 
-       bgc->gc.get = bgpio_get;
+       if (!(flags & BGPIOF_UNREADABLE_REG_SET) &&
+           (flags & BGPIOF_READ_OUTPUT_REG_SET))
+               bgc->gc.get = bgpio_get_set;
+       else
+               bgc->gc.get = bgpio_get;
 
        return 0;
 }
@@ -500,7 +516,7 @@ int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
        bgc->gc.ngpio = bgc->bits;
        bgc->gc.request = bgpio_request;
 
-       ret = bgpio_setup_io(bgc, dat, set, clr);
+       ret = bgpio_setup_io(bgc, dat, set, clr, flags);
        if (ret)
                return ret;