1 files changed, 247 insertions, 52 deletions

diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index 9296b2673b52..be8713908125 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -19,6 +19,7 @@
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/pci.h>
+#include <linux/gpio.h>
 
 #include <linux/sm501.h>
 #include <linux/sm501-regs.h>
@@ -31,10 +32,29 @@ struct sm501_device {
         struct platform_device          pdev;
 };
 
+struct sm501_gpio;
+
+struct sm501_gpio_chip {
+        struct gpio_chip        gpio;
+        struct sm501_gpio       *ourgpio;       /* to get back to parent. */
+        void __iomem            *regbase;
+};
+
+struct sm501_gpio {
+        struct sm501_gpio_chip  low;
+        struct sm501_gpio_chip  high;
+        spinlock_t              lock;
+
+        unsigned int             registered : 1;
+        void __iomem            *regs;
+        struct resource         *regs_res;
+};
+
 struct sm501_devdata {
         spinlock_t                       reg_lock;
         struct mutex                     clock_lock;
         struct list_head                 devices;
+        struct sm501_gpio                gpio;
 
         struct device                   *dev;
         struct resource                 *io_res;
@@ -42,6 +62,7 @@ struct sm501_devdata {
         struct resource                 *regs_claim;
         struct sm501_platdata           *platdata;
 
+
         unsigned int                     in_suspend;
         unsigned long                    pm_misc;
 
@@ -52,6 +73,7 @@ struct sm501_devdata {
         unsigned int                     rev;
 };
 
+
 #define MHZ (1000 * 1000)
 
 #ifdef DEBUG
@@ -276,58 +298,6 @@ unsigned long sm501_modify_reg(struct device *dev,
 
 EXPORT_SYMBOL_GPL(sm501_modify_reg);
 
-unsigned long sm501_gpio_get(struct device *dev,
-                             unsigned long gpio)
-{
-        struct sm501_devdata *sm = dev_get_drvdata(dev);
-        unsigned long result;
-        unsigned long reg;
-
-        reg = (gpio > 32) ? SM501_GPIO_DATA_HIGH : SM501_GPIO_DATA_LOW;
-        result = readl(sm->regs + reg);
-
-        result >>= (gpio & 31);
-        return result & 1UL;
-}
-
-EXPORT_SYMBOL_GPL(sm501_gpio_get);
-
-void sm501_gpio_set(struct device *dev,
-                    unsigned long gpio,
-                    unsigned int to,
-                    unsigned int dir)
-{
-        struct sm501_devdata *sm = dev_get_drvdata(dev);
-
-        unsigned long bit = 1 << (gpio & 31);
-        unsigned long base;
-        unsigned long save;
-        unsigned long val;
-
-        base = (gpio > 32) ? SM501_GPIO_DATA_HIGH : SM501_GPIO_DATA_LOW;
-        base += SM501_GPIO;
-
-        spin_lock_irqsave(&sm->reg_lock, save);
-
-        val = readl(sm->regs + base) & ~bit;
-        if (to)
-                val |= bit;
-        writel(val, sm->regs + base);
-
-        val = readl(sm->regs + SM501_GPIO_DDR_LOW) & ~bit;
-        if (dir)
-                val |= bit;
-
-        writel(val, sm->regs + SM501_GPIO_DDR_LOW);
-        sm501_sync_regs(sm);
-
-        spin_unlock_irqrestore(&sm->reg_lock, save);
-
-}
-
-EXPORT_SYMBOL_GPL(sm501_gpio_set);
-
-
 /* sm501_unit_power
  *
  * alters the power active gate to set specific units on or off
@@ -906,6 +876,226 @@ static int sm501_register_display(struct sm501_devdata *sm,
         return sm501_register_device(sm, pdev);
 }
 
+#ifdef CONFIG_MFD_SM501_GPIO
+
+static inline struct sm501_gpio_chip *to_sm501_gpio(struct gpio_chip *gc)
+{
+        return container_of(gc, struct sm501_gpio_chip, gpio);
+}
+
+static inline struct sm501_devdata *sm501_gpio_to_dev(struct sm501_gpio *gpio)
+{
+        return container_of(gpio, struct sm501_devdata, gpio);
+}
+
+static int sm501_gpio_get(struct gpio_chip *chip, unsigned offset)
+
+{
+        struct sm501_gpio_chip *smgpio = to_sm501_gpio(chip);
+        unsigned long result;
+
+        result = readl(smgpio->regbase + SM501_GPIO_DATA_LOW);
+        result >>= offset;
+
+        return result & 1UL;
+}
+
+static void sm501_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
+
+{
+        struct sm501_gpio_chip *smchip = to_sm501_gpio(chip);
+        struct sm501_gpio *smgpio = smchip->ourgpio;
+        unsigned long bit = 1 << offset;
+        void __iomem *regs = smchip->regbase;
+        unsigned long save;
+        unsigned long val;
+
+        dev_dbg(sm501_gpio_to_dev(smgpio)->dev, "%s(%p,%d)\n",
+                __func__, chip, offset);
+
+        spin_lock_irqsave(&smgpio->lock, save);
+
+        val = readl(regs + SM501_GPIO_DATA_LOW) & ~bit;
+        if (value)
+                val |= bit;
+        writel(val, regs);
+
+        sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+        spin_unlock_irqrestore(&smgpio->lock, save);
+}
+
+static int sm501_gpio_input(struct gpio_chip *chip, unsigned offset)
+{
+        struct sm501_gpio_chip *smchip = to_sm501_gpio(chip);
+        struct sm501_gpio *smgpio = smchip->ourgpio;
+        void __iomem *regs = smchip->regbase;
+        unsigned long bit = 1 << offset;
+        unsigned long save;
+        unsigned long ddr;
+
+        dev_info(sm501_gpio_to_dev(smgpio)->dev, "%s(%p,%d)\n",
+                 __func__, chip, offset);
+
+        spin_lock_irqsave(&smgpio->lock, save);
+
+        ddr = readl(regs + SM501_GPIO_DDR_LOW);
+        writel(ddr & ~bit, regs + SM501_GPIO_DDR_LOW);
+
+        sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+        spin_unlock_irqrestore(&smgpio->lock, save);
+
+        return 0;
+}
+
+static int sm501_gpio_output(struct gpio_chip *chip,
+                             unsigned offset, int value)
+{
+        struct sm501_gpio_chip *smchip = to_sm501_gpio(chip);
+        struct sm501_gpio *smgpio = smchip->ourgpio;
+        unsigned long bit = 1 << offset;
+        void __iomem *regs = smchip->regbase;
+        unsigned long save;
+        unsigned long val;
+        unsigned long ddr;
+
+        dev_dbg(sm501_gpio_to_dev(smgpio)->dev, "%s(%p,%d,%d)\n",
+                __func__, chip, offset, value);
+
+        spin_lock_irqsave(&smgpio->lock, save);
+
+        val = readl(regs + SM501_GPIO_DATA_LOW);
+        if (value)
+                val |= bit;
+        else
+                val &= ~bit;
+        writel(val, regs);
+
+        ddr = readl(regs + SM501_GPIO_DDR_LOW);
+        writel(ddr | bit, regs + SM501_GPIO_DDR_LOW);
+
+        sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+        writel(val, regs + SM501_GPIO_DATA_LOW);
+
+        sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+        spin_unlock_irqrestore(&smgpio->lock, save);
+
+        return 0;
+}
+
+static struct gpio_chip gpio_chip_template = {
+        .ngpio                  = 32,
+        .direction_input        = sm501_gpio_input,
+        .direction_output       = sm501_gpio_output,
+        .set                    = sm501_gpio_set,
+        .get                    = sm501_gpio_get,
+};
+
+static int __devinit sm501_gpio_register_chip(struct sm501_devdata *sm,
+                                              struct sm501_gpio *gpio,
+                                              struct sm501_gpio_chip *chip)
+{
+        struct sm501_platdata *pdata = sm->platdata;
+        struct gpio_chip *gchip = &chip->gpio;
+        unsigned base = pdata->gpio_base;
+
+        memcpy(chip, &gpio_chip_template, sizeof(struct gpio_chip));
+
+        if (chip == &gpio->high) {
+                base += 32;
+                chip->regbase = gpio->regs + SM501_GPIO_DATA_HIGH;
+                gchip->label  = "SM501-HIGH";
+        } else {
+                chip->regbase = gpio->regs + SM501_GPIO_DATA_LOW;
+                gchip->label  = "SM501-LOW";
+        }
+
+        gchip->base   = base;
+        chip->ourgpio = gpio;
+
+        return gpiochip_add(gchip);
+}
+
+static int sm501_register_gpio(struct sm501_devdata *sm)
+{
+        struct sm501_gpio *gpio = &sm->gpio;
+        resource_size_t iobase = sm->io_res->start + SM501_GPIO;
+        int ret;
+        int tmp;
+
+        dev_dbg(sm->dev, "registering gpio block %08llx\n",
+                (unsigned long long)iobase);
+
+        spin_lock_init(&gpio->lock);
+
+        gpio->regs_res = request_mem_region(iobase, 0x20, "sm501-gpio");
+        if (gpio->regs_res == NULL) {
+                dev_err(sm->dev, "gpio: failed to request region\n");
+                return -ENXIO;
+        }
+
+        gpio->regs = ioremap(iobase, 0x20);
+        if (gpio->regs == NULL) {
+                dev_err(sm->dev, "gpio: failed to remap registers\n");
+                ret = -ENXIO;
+                goto err_mapped;
+        }
+
+        /* Register both our chips. */
+
+        ret = sm501_gpio_register_chip(sm, gpio, &gpio->low);
+        if (ret) {
+                dev_err(sm->dev, "failed to add low chip\n");
+                goto err_mapped;
+        }
+
+        ret = sm501_gpio_register_chip(sm, gpio, &gpio->high);
+        if (ret) {
+                dev_err(sm->dev, "failed to add high chip\n");
+                goto err_low_chip;
+        }
+
+        gpio->registered = 1;
+
+        return 0;
+
+ err_low_chip:
+        tmp = gpiochip_remove(&gpio->low.gpio);
+        if (tmp) {
+                dev_err(sm->dev, "cannot remove low chip, cannot tidy up\n");
+                return ret;
+        }
+
+ err_mapped:
+        release_resource(gpio->regs_res);
+        kfree(gpio->regs_res);
+
+        return ret;
+}
+
+static void sm501_gpio_remove(struct sm501_devdata *sm)
+{
+        int ret;
+
+        ret = gpiochip_remove(&sm->gpio.low.gpio);
+        if (ret)
+                dev_err(sm->dev, "cannot remove low chip, cannot tidy up\n");
+
+        ret = gpiochip_remove(&sm->gpio.high.gpio);
+        if (ret)
+                dev_err(sm->dev, "cannot remove high chip, cannot tidy up\n");
+}
+
+#else
+static int sm501_register_gpio(struct sm501_devdata *sm)
+{
+        return 0;
+}
+
+static void sm501_gpio_remove(struct sm501_devdata *sm)
+{
+}
+#endif
+
 /* sm501_dbg_regs
  *
  * Debug attribute to attach to parent device to show core registers
@@ -1059,6 +1249,8 @@ static int sm501_init_dev(struct sm501_devdata *sm)
                         sm501_register_usbhost(sm, &mem_avail);
                 if (idata->devices & (SM501_USE_UART0 | SM501_USE_UART1))
                         sm501_register_uart(sm, idata->devices);
+                if (idata->devices & SM501_USE_GPIO)
+                        sm501_register_gpio(sm);
         }
 
         ret = sm501_check_clocks(sm);
@@ -1366,6 +1558,9 @@ static void sm501_dev_remove(struct sm501_devdata *sm)
                 sm501_remove_sub(sm, smdev);
 
         device_remove_file(sm->dev, &dev_attr_dbg_regs);
+
+        if (sm->gpio.registered)
+                sm501_gpio_remove(sm);
 }
 
 static void sm501_pci_remove(struct pci_dev *dev)