1 files changed, 28 insertions, 2 deletions

diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index 170f9d47c2f9..0e5761f12634 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -41,6 +41,7 @@ struct sm501_gpio_chip {
         struct gpio_chip        gpio;
         struct sm501_gpio       *ourgpio;       /* to get back to parent. */
         void __iomem            *regbase;
+        void __iomem            *control;       /* address of control reg. */
 };
 
 struct sm501_gpio {
@@ -908,6 +909,25 @@ static int sm501_gpio_get(struct gpio_chip *chip, unsigned offset)
         return result & 1UL;
 }
 
+static void sm501_gpio_ensure_gpio(struct sm501_gpio_chip *smchip,
+                                   unsigned long bit)
+{
+        unsigned long ctrl;
+
+        /* check and modify if this pin is not set as gpio. */
+
+        if (readl(smchip->control) & bit) {
+                dev_info(sm501_gpio_to_dev(smchip->ourgpio)->dev,
+                         "changing mode of gpio, bit %08lx\n", bit);
+
+                ctrl = readl(smchip->control);
+                ctrl &= ~bit;
+                writel(ctrl, smchip->control);
+
+                sm501_sync_regs(sm501_gpio_to_dev(smchip->ourgpio));
+        }
+}
+
 static void sm501_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 {
@@ -929,6 +949,8 @@ static void sm501_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
         writel(val, regs);
 
         sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+        sm501_gpio_ensure_gpio(smchip, bit);
+
         spin_unlock_irqrestore(&smgpio->lock, save);
 }
 
@@ -941,8 +963,8 @@ static int sm501_gpio_input(struct gpio_chip *chip, unsigned offset)
         unsigned long save;
         unsigned long ddr;
 
-        dev_info(sm501_gpio_to_dev(smgpio)->dev, "%s(%p,%d)\n",
-                 __func__, chip, offset);
+        dev_dbg(sm501_gpio_to_dev(smgpio)->dev, "%s(%p,%d)\n",
+                __func__, chip, offset);
 
         spin_lock_irqsave(&smgpio->lock, save);
 
@@ -950,6 +972,8 @@ static int sm501_gpio_input(struct gpio_chip *chip, unsigned offset)
         writel(ddr & ~bit, regs + SM501_GPIO_DDR_LOW);
 
         sm501_sync_regs(sm501_gpio_to_dev(smgpio));
+        sm501_gpio_ensure_gpio(smchip, bit);
+
         spin_unlock_irqrestore(&smgpio->lock, save);
 
         return 0;
@@ -1012,9 +1036,11 @@ static int __devinit sm501_gpio_register_chip(struct sm501_devdata *sm,
                 if (base > 0)
                         base += 32;
                 chip->regbase = gpio->regs + SM501_GPIO_DATA_HIGH;
+                chip->control = sm->regs + SM501_GPIO63_32_CONTROL;
                 gchip->label  = "SM501-HIGH";
         } else {
                 chip->regbase = gpio->regs + SM501_GPIO_DATA_LOW;
+                chip->control = sm->regs + SM501_GPIO31_0_CONTROL;
                 gchip->label  = "SM501-LOW";
         }