1 files changed, 384 insertions, 133 deletions

diff --git a/drivers/gpio/basic_mmio_gpio.c b/drivers/gpio/basic_mmio_gpio.c
index 3addea65894e..8152e9f516b0 100644
--- a/drivers/gpio/basic_mmio_gpio.c
+++ b/drivers/gpio/basic_mmio_gpio.c
@@ -45,6 +45,7 @@ o        `                     ~~~~\___/~~~~    ` controller in FPGA is ,.`
  */
 
 #include <linux/init.h>
+#include <linux/err.h>
 #include <linux/bug.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -61,102 +62,101 @@ o        `                     ~~~~\___/~~~~    ` controller in FPGA is ,.`
 #include <linux/mod_devicetable.h>
 #include <linux/basic_mmio_gpio.h>
 
-struct bgpio_chip {
-        struct gpio_chip gc;
-        void __iomem *reg_dat;
-        void __iomem *reg_set;
-        void __iomem *reg_clr;
-
-        /* Number of bits (GPIOs): <register width> * 8. */
-        int bits;
-
-        /*
-         * Some GPIO controllers work with the big-endian bits notation,
-         * e.g. in a 8-bits register, GPIO7 is the least significant bit.
-         */
-        int big_endian_bits;
-
-        /*
-         * Used to lock bgpio_chip->data. Also, this is needed to keep
-         * shadowed and real data registers writes together.
-         */
-        spinlock_t lock;
-
-        /* Shadowed data register to clear/set bits safely. */
-        unsigned long data;
-};
+static void bgpio_write8(void __iomem *reg, unsigned long data)
+{
+        writeb(data, reg);
+}
 
-static struct bgpio_chip *to_bgpio_chip(struct gpio_chip *gc)
+static unsigned long bgpio_read8(void __iomem *reg)
 {
-        return container_of(gc, struct bgpio_chip, gc);
+        return readb(reg);
 }
 
-static unsigned long bgpio_in(struct bgpio_chip *bgc)
+static void bgpio_write16(void __iomem *reg, unsigned long data)
 {
-        switch (bgc->bits) {
-        case 8:
-                return __raw_readb(bgc->reg_dat);
-        case 16:
-                return __raw_readw(bgc->reg_dat);
-        case 32:
-                return __raw_readl(bgc->reg_dat);
-#if BITS_PER_LONG >= 64
-        case 64:
-                return __raw_readq(bgc->reg_dat);
-#endif
-        }
-        return -EINVAL;
+        writew(data, reg);
 }
 
-static void bgpio_out(struct bgpio_chip *bgc, void __iomem *reg,
-                      unsigned long data)
+static unsigned long bgpio_read16(void __iomem *reg)
 {
-        switch (bgc->bits) {
-        case 8:
-                __raw_writeb(data, reg);
-                return;
-        case 16:
-                __raw_writew(data, reg);
-                return;
-        case 32:
-                __raw_writel(data, reg);
-                return;
+        return readw(reg);
+}
+
+static void bgpio_write32(void __iomem *reg, unsigned long data)
+{
+        writel(data, reg);
+}
+
+static unsigned long bgpio_read32(void __iomem *reg)
+{
+        return readl(reg);
+}
+
 #if BITS_PER_LONG >= 64
-        case 64:
-                __raw_writeq(data, reg);
-                return;
-#endif
-        }
+static void bgpio_write64(void __iomem *reg, unsigned long data)
+{
+        writeq(data, reg);
 }
 
+static unsigned long bgpio_read64(void __iomem *reg)
+{
+        return readq(reg);
+}
+#endif /* BITS_PER_LONG >= 64 */
+
 static unsigned long bgpio_pin2mask(struct bgpio_chip *bgc, unsigned int pin)
 {
-        if (bgc->big_endian_bits)
-                return 1 << (bgc->bits - 1 - pin);
-        else
-                return 1 << pin;
+        return 1 << pin;
+}
+
+static unsigned long bgpio_pin2mask_be(struct bgpio_chip *bgc,
+                                       unsigned int pin)
+{
+        return 1 << (bgc->bits - 1 - pin);
 }
 
 static int bgpio_get(struct gpio_chip *gc, unsigned int gpio)
 {
         struct bgpio_chip *bgc = to_bgpio_chip(gc);
 
-        return bgpio_in(bgc) & bgpio_pin2mask(bgc, gpio);
+        return bgc->read_reg(bgc->reg_dat) & bgc->pin2mask(bgc, gpio);
 }
 
 static void bgpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
         struct bgpio_chip *bgc = to_bgpio_chip(gc);
-        unsigned long mask = bgpio_pin2mask(bgc, gpio);
+        unsigned long mask = bgc->pin2mask(bgc, gpio);
         unsigned long flags;
 
-        if (bgc->reg_set) {
-                if (val)
-                        bgpio_out(bgc, bgc->reg_set, mask);
-                else
-                        bgpio_out(bgc, bgc->reg_clr, mask);
-                return;
-        }
+        spin_lock_irqsave(&bgc->lock, flags);
+
+        if (val)
+                bgc->data |= mask;
+        else
+                bgc->data &= ~mask;
+
+        bgc->write_reg(bgc->reg_dat, bgc->data);
+
+        spin_unlock_irqrestore(&bgc->lock, flags);
+}
+
+static void bgpio_set_with_clear(struct gpio_chip *gc, unsigned int gpio,
+                                 int val)
+{
+        struct bgpio_chip *bgc = to_bgpio_chip(gc);
+        unsigned long mask = bgc->pin2mask(bgc, gpio);
+
+        if (val)
+                bgc->write_reg(bgc->reg_set, mask);
+        else
+                bgc->write_reg(bgc->reg_clr, mask);
+}
+
+static void bgpio_set_set(struct gpio_chip *gc, unsigned int gpio, int val)
+{
+        struct bgpio_chip *bgc = to_bgpio_chip(gc);
+        unsigned long mask = bgc->pin2mask(bgc, gpio);
+        unsigned long flags;
 
         spin_lock_irqsave(&bgc->lock, flags);
 
@@ -165,103 +165,352 @@ static void bgpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
         else
                 bgc->data &= ~mask;
 
-        bgpio_out(bgc, bgc->reg_dat, bgc->data);
+        bgc->write_reg(bgc->reg_set, bgc->data);
 
         spin_unlock_irqrestore(&bgc->lock, flags);
 }
 
+static int bgpio_simple_dir_in(struct gpio_chip *gc, unsigned int gpio)
+{
+        return 0;
+}
+
+static int bgpio_simple_dir_out(struct gpio_chip *gc, unsigned int gpio,
+                                int val)
+{
+        gc->set(gc, gpio, val);
+
+        return 0;
+}
+
 static int bgpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
+        struct bgpio_chip *bgc = to_bgpio_chip(gc);
+        unsigned long flags;
+
+        spin_lock_irqsave(&bgc->lock, flags);
+
+        bgc->dir &= ~bgc->pin2mask(bgc, gpio);
+        bgc->write_reg(bgc->reg_dir, bgc->dir);
+
+        spin_unlock_irqrestore(&bgc->lock, flags);
+
         return 0;
 }
 
 static int bgpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-        bgpio_set(gc, gpio, val);
+        struct bgpio_chip *bgc = to_bgpio_chip(gc);
+        unsigned long flags;
+
+        gc->set(gc, gpio, val);
+
+        spin_lock_irqsave(&bgc->lock, flags);
+
+        bgc->dir |= bgc->pin2mask(bgc, gpio);
+        bgc->write_reg(bgc->reg_dir, bgc->dir);
+
+        spin_unlock_irqrestore(&bgc->lock, flags);
+
         return 0;
 }
 
-static int __devinit bgpio_probe(struct platform_device *pdev)
+static int bgpio_dir_in_inv(struct gpio_chip *gc, unsigned int gpio)
 {
-        const struct platform_device_id *platid = platform_get_device_id(pdev);
-        struct device *dev = &pdev->dev;
-        struct bgpio_pdata *pdata = dev_get_platdata(dev);
-        struct bgpio_chip *bgc;
-        struct resource *res_dat;
-        struct resource *res_set;
-        struct resource *res_clr;
-        resource_size_t dat_sz;
-        int bits;
-        int ret;
+        struct bgpio_chip *bgc = to_bgpio_chip(gc);
+        unsigned long flags;
 
-        res_dat = platform_get_resource_byname(pdev, IORESOURCE_MEM, "dat");
-        if (!res_dat)
-                return -EINVAL;
+        spin_lock_irqsave(&bgc->lock, flags);
 
-        dat_sz = resource_size(res_dat);
-        if (!is_power_of_2(dat_sz))
-                return -EINVAL;
+        bgc->dir |= bgc->pin2mask(bgc, gpio);
+        bgc->write_reg(bgc->reg_dir, bgc->dir);
+
+        spin_unlock_irqrestore(&bgc->lock, flags);
+
+        return 0;
+}
 
-        bits = dat_sz * 8;
-        if (bits > BITS_PER_LONG)
+static int bgpio_dir_out_inv(struct gpio_chip *gc, unsigned int gpio, int val)
+{
+        struct bgpio_chip *bgc = to_bgpio_chip(gc);
+        unsigned long flags;
+
+        gc->set(gc, gpio, val);
+
+        spin_lock_irqsave(&bgc->lock, flags);
+
+        bgc->dir &= ~bgc->pin2mask(bgc, gpio);
+        bgc->write_reg(bgc->reg_dir, bgc->dir);
+
+        spin_unlock_irqrestore(&bgc->lock, flags);
+
+        return 0;
+}
+
+static int bgpio_setup_accessors(struct device *dev,
+                                 struct bgpio_chip *bgc,
+                                 bool be)
+{
+
+        switch (bgc->bits) {
+        case 8:
+                bgc->read_reg   = bgpio_read8;
+                bgc->write_reg  = bgpio_write8;
+                break;
+        case 16:
+                bgc->read_reg   = bgpio_read16;
+                bgc->write_reg  = bgpio_write16;
+                break;
+        case 32:
+                bgc->read_reg   = bgpio_read32;
+                bgc->write_reg  = bgpio_write32;
+                break;
+#if BITS_PER_LONG >= 64
+        case 64:
+                bgc->read_reg   = bgpio_read64;
+                bgc->write_reg  = bgpio_write64;
+                break;
+#endif /* BITS_PER_LONG >= 64 */
+        default:
+                dev_err(dev, "unsupported data width %u bits\n", bgc->bits);
                 return -EINVAL;
+        }
 
-        bgc = devm_kzalloc(dev, sizeof(*bgc), GFP_KERNEL);
-        if (!bgc)
-                return -ENOMEM;
+        bgc->pin2mask = be ? bgpio_pin2mask_be : bgpio_pin2mask;
+
+        return 0;
+}
+
+/*
+ * Create the device and allocate the resources.  For setting GPIO's there are
+ * three supported configurations:
+ *
+ *      - single input/output register resource (named "dat").
+ *      - set/clear pair (named "set" and "clr").
+ *      - single output register resource and single input resource ("set" and
+ *      dat").
+ *
+ * For the single output register, this drives a 1 by setting a bit and a zero
+ * by clearing a bit.  For the set clr pair, this drives a 1 by setting a bit
+ * in the set register and clears it by setting a bit in the clear register.
+ * The configuration is detected by which resources are present.
+ *
+ * For setting the GPIO direction, there are three supported configurations:
+ *
+ *      - simple bidirection GPIO that requires no configuration.
+ *      - an output direction register (named "dirout") where a 1 bit
+ *      indicates the GPIO is an output.
+ *      - an input direction register (named "dirin") where a 1 bit indicates
+ *      the GPIO is an input.
+ */
+static int bgpio_setup_io(struct bgpio_chip *bgc,
+                          void __iomem *dat,
+                          void __iomem *set,
+                          void __iomem *clr)
+{
 
-        bgc->reg_dat = devm_ioremap(dev, res_dat->start, dat_sz);
+        bgc->reg_dat = dat;
         if (!bgc->reg_dat)
-                return -ENOMEM;
+                return -EINVAL;
+
+        if (set && clr) {
+                bgc->reg_set = set;
+                bgc->reg_clr = clr;
+                bgc->gc.set = bgpio_set_with_clear;
+        } else if (set && !clr) {
+                bgc->reg_set = set;
+                bgc->gc.set = bgpio_set_set;
+        } else {
+                bgc->gc.set = bgpio_set;
+        }
+
+        bgc->gc.get = bgpio_get;
+
+        return 0;
+}
 
-        res_set = platform_get_resource_byname(pdev, IORESOURCE_MEM, "set");
-        res_clr = platform_get_resource_byname(pdev, IORESOURCE_MEM, "clr");
-        if (res_set && res_clr) {
-                if (resource_size(res_set) != resource_size(res_clr) ||
-                                resource_size(res_set) != dat_sz)
-                        return -EINVAL;
-
-                bgc->reg_set = devm_ioremap(dev, res_set->start, dat_sz);
-                bgc->reg_clr = devm_ioremap(dev, res_clr->start, dat_sz);
-                if (!bgc->reg_set || !bgc->reg_clr)
-                        return -ENOMEM;
-        } else if (res_set || res_clr) {
+static int bgpio_setup_direction(struct bgpio_chip *bgc,
+                                 void __iomem *dirout,
+                                 void __iomem *dirin)
+{
+        if (dirout && dirin) {
                 return -EINVAL;
+        } else if (dirout) {
+                bgc->reg_dir = dirout;
+                bgc->gc.direction_output = bgpio_dir_out;
+                bgc->gc.direction_input = bgpio_dir_in;
+        } else if (dirin) {
+                bgc->reg_dir = dirin;
+                bgc->gc.direction_output = bgpio_dir_out_inv;
+                bgc->gc.direction_input = bgpio_dir_in_inv;
+        } else {
+                bgc->gc.direction_output = bgpio_simple_dir_out;
+                bgc->gc.direction_input = bgpio_simple_dir_in;
         }
 
-        spin_lock_init(&bgc->lock);
+        return 0;
+}
 
-        bgc->bits = bits;
-        bgc->big_endian_bits = !strcmp(platid->name, "basic-mmio-gpio-be");
-        bgc->data = bgpio_in(bgc);
+int __devexit bgpio_remove(struct bgpio_chip *bgc)
+{
+        int err = gpiochip_remove(&bgc->gc);
 
-        bgc->gc.ngpio = bits;
-        bgc->gc.direction_input = bgpio_dir_in;
-        bgc->gc.direction_output = bgpio_dir_out;
-        bgc->gc.get = bgpio_get;
-        bgc->gc.set = bgpio_set;
+        kfree(bgc);
+
+        return err;
+}
+EXPORT_SYMBOL_GPL(bgpio_remove);
+
+int __devinit bgpio_init(struct bgpio_chip *bgc,
+                         struct device *dev,
+                         unsigned long sz,
+                         void __iomem *dat,
+                         void __iomem *set,
+                         void __iomem *clr,
+                         void __iomem *dirout,
+                         void __iomem *dirin,
+                         bool big_endian)
+{
+        int ret;
+
+        if (!is_power_of_2(sz))
+                return -EINVAL;
+
+        bgc->bits = sz * 8;
+        if (bgc->bits > BITS_PER_LONG)
+                return -EINVAL;
+
+        spin_lock_init(&bgc->lock);
         bgc->gc.dev = dev;
         bgc->gc.label = dev_name(dev);
+        bgc->gc.base = -1;
+        bgc->gc.ngpio = bgc->bits;
 
-        if (pdata)
-                bgc->gc.base = pdata->base;
-        else
-                bgc->gc.base = -1;
+        ret = bgpio_setup_io(bgc, dat, set, clr);
+        if (ret)
+                return ret;
 
-        dev_set_drvdata(dev, bgc);
+        ret = bgpio_setup_accessors(dev, bgc, big_endian);
+        if (ret)
+                return ret;
 
-        ret = gpiochip_add(&bgc->gc);
+        ret = bgpio_setup_direction(bgc, dirout, dirin);
         if (ret)
-                dev_err(dev, "gpiochip_add() failed: %d\n", ret);
+                return ret;
+
+        bgc->data = bgc->read_reg(bgc->reg_dat);
 
         return ret;
 }
+EXPORT_SYMBOL_GPL(bgpio_init);
+
+#ifdef CONFIG_GPIO_BASIC_MMIO
 
-static int __devexit bgpio_remove(struct platform_device *pdev)
+static void __iomem *bgpio_map(struct platform_device *pdev,
+                               const char *name,
+                               resource_size_t sane_sz,
+                               int *err)
 {
-        struct bgpio_chip *bgc = dev_get_drvdata(&pdev->dev);
+        struct device *dev = &pdev->dev;
+        struct resource *r;
+        resource_size_t start;
+        resource_size_t sz;
+        void __iomem *ret;
+
+        *err = 0;
+
+        r = platform_get_resource_byname(pdev, IORESOURCE_MEM, name);
+        if (!r)
+                return NULL;
 
-        return gpiochip_remove(&bgc->gc);
+        sz = resource_size(r);
+        if (sz != sane_sz) {
+                *err = -EINVAL;
+                return NULL;
+        }
+
+        start = r->start;
+        if (!devm_request_mem_region(dev, start, sz, r->name)) {
+                *err = -EBUSY;
+                return NULL;
+        }
+
+        ret = devm_ioremap(dev, start, sz);
+        if (!ret) {
+                *err = -ENOMEM;
+                return NULL;
+        }
+
+        return ret;
+}
+
+static int __devinit bgpio_pdev_probe(struct platform_device *pdev)
+{
+        struct device *dev = &pdev->dev;
+        struct resource *r;
+        void __iomem *dat;
+        void __iomem *set;
+        void __iomem *clr;
+        void __iomem *dirout;
+        void __iomem *dirin;
+        unsigned long sz;
+        bool be;
+        int err;
+        struct bgpio_chip *bgc;
+        struct bgpio_pdata *pdata = dev_get_platdata(dev);
+
+        r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "dat");
+        if (!r)
+                return -EINVAL;
+
+        sz = resource_size(r);
+
+        dat = bgpio_map(pdev, "dat", sz, &err);
+        if (!dat)
+                return err ? err : -EINVAL;
+
+        set = bgpio_map(pdev, "set", sz, &err);
+        if (err)
+                return err;
+
+        clr = bgpio_map(pdev, "clr", sz, &err);
+        if (err)
+                return err;
+
+        dirout = bgpio_map(pdev, "dirout", sz, &err);
+        if (err)
+                return err;
+
+        dirin = bgpio_map(pdev, "dirin", sz, &err);
+        if (err)
+                return err;
+
+        be = !strcmp(platform_get_device_id(pdev)->name, "basic-mmio-gpio-be");
+
+        bgc = devm_kzalloc(&pdev->dev, sizeof(*bgc), GFP_KERNEL);
+        if (!bgc)
+                return -ENOMEM;
+
+        err = bgpio_init(bgc, dev, sz, dat, set, clr, dirout, dirin, be);
+        if (err)
+                return err;
+
+        if (pdata) {
+                bgc->gc.base = pdata->base;
+                if (pdata->ngpio > 0)
+                        bgc->gc.ngpio = pdata->ngpio;
+        }
+
+        platform_set_drvdata(pdev, bgc);
+
+        return gpiochip_add(&bgc->gc);
+}
+
+static int __devexit bgpio_pdev_remove(struct platform_device *pdev)
+{
+        struct bgpio_chip *bgc = platform_get_drvdata(pdev);
+
+        return bgpio_remove(bgc);
 }
 
 static const struct platform_device_id bgpio_id_table[] = {
@@ -276,21 +525,23 @@ static struct platform_driver bgpio_driver = {
                 .name = "basic-mmio-gpio",
         },
         .id_table = bgpio_id_table,
-        .probe = bgpio_probe,
-        .remove = __devexit_p(bgpio_remove),
+        .probe = bgpio_pdev_probe,
+        .remove = __devexit_p(bgpio_pdev_remove),
 };
 
-static int __init bgpio_init(void)
+static int __init bgpio_platform_init(void)
 {
         return platform_driver_register(&bgpio_driver);
 }
-module_init(bgpio_init);
+module_init(bgpio_platform_init);
 
-static void __exit bgpio_exit(void)
+static void __exit bgpio_platform_exit(void)
 {
         platform_driver_unregister(&bgpio_driver);
 }
-module_exit(bgpio_exit);
+module_exit(bgpio_platform_exit);
+
+#endif /* CONFIG_GPIO_BASIC_MMIO */
 
 MODULE_DESCRIPTION("Driver for basic memory-mapped GPIO controllers");
 MODULE_AUTHOR("Anton Vorontsov <cbouatmailru@gmail.com>");