1 files changed, 28 insertions, 8 deletions

diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index da6639707ea3..7ba31770d773 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -128,6 +128,8 @@ struct omap_i2c_dev {
         size_t                  buf_len;
         struct i2c_adapter      adapter;
         unsigned                rev1:1;
+        unsigned                idle:1;
+        u16                     iestate;        /* Saved interrupt register */
 };
 
 static inline void omap_i2c_write_reg(struct omap_i2c_dev *i2c_dev,
@@ -174,18 +176,30 @@ static void omap_i2c_put_clocks(struct omap_i2c_dev *dev)
         }
 }
 
-static void omap_i2c_enable_clocks(struct omap_i2c_dev *dev)
+static void omap_i2c_unidle(struct omap_i2c_dev *dev)
 {
         if (dev->iclk != NULL)
                 clk_enable(dev->iclk);
         clk_enable(dev->fclk);
+        if (dev->iestate)
+                omap_i2c_write_reg(dev, OMAP_I2C_IE_REG, dev->iestate);
+        dev->idle = 0;
 }
 
-static void omap_i2c_disable_clocks(struct omap_i2c_dev *dev)
+static void omap_i2c_idle(struct omap_i2c_dev *dev)
 {
+        u16 iv;
+
+        dev->idle = 1;
+        dev->iestate = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
+        omap_i2c_write_reg(dev, OMAP_I2C_IE_REG, 0);
+        if (dev->rev1)
+                iv = omap_i2c_read_reg(dev, OMAP_I2C_IV_REG);   /* Read clears */
+        else
+                omap_i2c_write_reg(dev, OMAP_I2C_STAT_REG, dev->iestate);
+        clk_disable(dev->fclk);
         if (dev->iclk != NULL)
                 clk_disable(dev->iclk);
-        clk_disable(dev->fclk);
 }
 
 static int omap_i2c_init(struct omap_i2c_dev *dev)
@@ -360,7 +374,7 @@ omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
         int i;
         int r;
 
-        omap_i2c_enable_clocks(dev);
+        omap_i2c_unidle(dev);
 
         if ((r = omap_i2c_wait_for_bb(dev)) < 0)
                 goto out;
@@ -374,7 +388,7 @@ omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
         if (r == 0)
                 r = num;
 out:
-        omap_i2c_disable_clocks(dev);
+        omap_i2c_idle(dev);
         return r;
 }
 
@@ -403,6 +417,9 @@ omap_i2c_rev1_isr(int this_irq, void *dev_id)
         struct omap_i2c_dev *dev = dev_id;
         u16 iv, w;
 
+        if (dev->idle)
+                return IRQ_NONE;
+
         iv = omap_i2c_read_reg(dev, OMAP_I2C_IV_REG);
         switch (iv) {
         case 0x00:      /* None */
@@ -457,6 +474,9 @@ omap_i2c_isr(int this_irq, void *dev_id)
         u16 stat, w;
         int count = 0;
 
+        if (dev->idle)
+                return IRQ_NONE;
+
         bits = omap_i2c_read_reg(dev, OMAP_I2C_IE_REG);
         while ((stat = (omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG))) & bits) {
                 dev_dbg(dev->dev, "IRQ (ISR = 0x%04x)\n", stat);
@@ -575,7 +595,7 @@ omap_i2c_probe(struct platform_device *pdev)
         if ((r = omap_i2c_get_clocks(dev)) != 0)
                 goto err_free_mem;
 
-        omap_i2c_enable_clocks(dev);
+        omap_i2c_unidle(dev);
 
         if (cpu_is_omap15xx())
                 dev->rev1 = omap_i2c_read_reg(dev, OMAP_I2C_REV_REG) < 0x20;
@@ -610,7 +630,7 @@ omap_i2c_probe(struct platform_device *pdev)
                 goto err_free_irq;
         }
 
-        omap_i2c_disable_clocks(dev);
+        omap_i2c_idle(dev);
 
         return 0;
 
@@ -618,7 +638,7 @@ err_free_irq:
         free_irq(dev->irq, dev);
 err_unuse_clocks:
         omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, 0);
-        omap_i2c_disable_clocks(dev);
+        omap_i2c_idle(dev);
         omap_i2c_put_clocks(dev);
 err_free_mem:
         platform_set_drvdata(pdev, NULL);