2 files changed, 59 insertions, 37 deletions

diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 4afba3ec2a61..e3654d683e15 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -120,19 +120,26 @@ struct imx_i2c_struct {
         wait_queue_head_t       queue;
         unsigned long           i2csr;
         unsigned int            disable_delay;
+        int                     stopped;
+        unsigned int            ifdr; /* IMX_I2C_IFDR */
 };
 
 /** Functions for IMX I2C adapter driver ***************************************
 *******************************************************************************/
 
-static int i2c_imx_bus_busy(struct imx_i2c_struct *i2c_imx)
+static int i2c_imx_bus_busy(struct imx_i2c_struct *i2c_imx, int for_busy)
 {
         unsigned long orig_jiffies = jiffies;
+        unsigned int temp;
 
         dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 
-        /* wait for bus not busy */
-        while (readb(i2c_imx->base + IMX_I2C_I2SR) & I2SR_IBB) {
+        while (1) {
+                temp = readb(i2c_imx->base + IMX_I2C_I2SR);
+                if (for_busy && (temp & I2SR_IBB))
+                        break;
+                if (!for_busy && !(temp & I2SR_IBB))
+                        break;
                 if (signal_pending(current)) {
                         dev_dbg(&i2c_imx->adapter.dev,
                                 "<%s> I2C Interrupted\n", __func__);
@@ -179,41 +186,62 @@ static int i2c_imx_acked(struct imx_i2c_struct *i2c_imx)
         return 0;
 }
 
-static void i2c_imx_start(struct imx_i2c_struct *i2c_imx)
+static int i2c_imx_start(struct imx_i2c_struct *i2c_imx)
 {
         unsigned int temp = 0;
+        int result;
 
         dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 
+        clk_enable(i2c_imx->clk);
+        writeb(i2c_imx->ifdr, i2c_imx->base + IMX_I2C_IFDR);
         /* Enable I2C controller */
+        writeb(0, i2c_imx->base + IMX_I2C_I2SR);
         writeb(I2CR_IEN, i2c_imx->base + IMX_I2C_I2CR);
+
+        /* Wait controller to be stable */
+        udelay(50);
+
         /* Start I2C transaction */
         temp = readb(i2c_imx->base + IMX_I2C_I2CR);
         temp |= I2CR_MSTA;
         writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+        result = i2c_imx_bus_busy(i2c_imx, 1);
+        if (result)
+                return result;
+        i2c_imx->stopped = 0;
+
         temp |= I2CR_IIEN | I2CR_MTX | I2CR_TXAK;
         writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+        return result;
 }
 
 static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
 {
         unsigned int temp = 0;
 
-        /* Stop I2C transaction */
-        dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
-        temp = readb(i2c_imx->base + IMX_I2C_I2CR);
-        temp &= ~I2CR_MSTA;
-        writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
-        /* setup chip registers to defaults */
-        writeb(I2CR_IEN, i2c_imx->base + IMX_I2C_I2CR);
-        writeb(0, i2c_imx->base + IMX_I2C_I2SR);
-        /*
-         * This delay caused by an i.MXL hardware bug.
-         * If no (or too short) delay, no "STOP" bit will be generated.
-         */
-        udelay(i2c_imx->disable_delay);
+        if (!i2c_imx->stopped) {
+                /* Stop I2C transaction */
+                dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
+                temp = readb(i2c_imx->base + IMX_I2C_I2CR);
+                temp &= ~(I2CR_MSTA | I2CR_MTX);
+                writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+                i2c_imx->stopped = 1;
+        }
+        if (cpu_is_mx1()) {
+                /*
+                 * This delay caused by an i.MXL hardware bug.
+                 * If no (or too short) delay, no "STOP" bit will be generated.
+                 */
+                udelay(i2c_imx->disable_delay);
+        }
+
+        if (!i2c_imx->stopped)
+                i2c_imx_bus_busy(i2c_imx, 0);
+
         /* Disable I2C controller */
         writeb(0, i2c_imx->base + IMX_I2C_I2CR);
+        clk_disable(i2c_imx->clk);
 }
 
 static void __init i2c_imx_set_clk(struct imx_i2c_struct *i2c_imx,
@@ -233,8 +261,8 @@ static void __init i2c_imx_set_clk(struct imx_i2c_struct *i2c_imx,
         else
                 for (i = 0; i2c_clk_div[i][0] < div; i++);
 
-        /* Write divider value to register */
-        writeb(i2c_clk_div[i][1], i2c_imx->base + IMX_I2C_IFDR);
+        /* Store divider value */
+        i2c_imx->ifdr = i2c_clk_div[i][1];
 
         /*
          * There dummy delay is calculated.
@@ -341,11 +369,15 @@ static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs)
                 if (result)
                         return result;
                 if (i == (msgs->len - 1)) {
+                        /* It must generate STOP before read I2DR to prevent
+                           controller from generating another clock cycle */
                         dev_dbg(&i2c_imx->adapter.dev,
                                 "<%s> clear MSTA\n", __func__);
                         temp = readb(i2c_imx->base + IMX_I2C_I2CR);
-                        temp &= ~I2CR_MSTA;
+                        temp &= ~(I2CR_MSTA | I2CR_MTX);
                         writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+                        i2c_imx_bus_busy(i2c_imx, 0);
+                        i2c_imx->stopped = 1;
                 } else if (i == (msgs->len - 2)) {
                         dev_dbg(&i2c_imx->adapter.dev,
                                 "<%s> set TXAK\n", __func__);
@@ -370,14 +402,11 @@ static int i2c_imx_xfer(struct i2c_adapter *adapter,
 
         dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 
-        /* Check if i2c bus is not busy */
-        result = i2c_imx_bus_busy(i2c_imx);
+        /* Start I2C transfer */
+        result = i2c_imx_start(i2c_imx);
         if (result)
                 goto fail0;
 
-        /* Start I2C transfer */
-        i2c_imx_start(i2c_imx);
-
         /* read/write data */
         for (i = 0; i < num; i++) {
                 if (i) {
@@ -386,6 +415,9 @@ static int i2c_imx_xfer(struct i2c_adapter *adapter,
                         temp = readb(i2c_imx->base + IMX_I2C_I2CR);
                         temp |= I2CR_RSTA;
                         writeb(temp, i2c_imx->base + IMX_I2C_I2CR);
+                        result =  i2c_imx_bus_busy(i2c_imx, 1);
+                        if (result)
+                                goto fail0;
                 }
                 dev_dbg(&i2c_imx->adapter.dev,
                         "<%s> transfer message: %d\n", __func__, i);
@@ -500,7 +532,6 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
                 dev_err(&pdev->dev, "can't get I2C clock\n");
                 goto fail3;
         }
-        clk_enable(i2c_imx->clk);
 
         /* Request IRQ */
         ret = request_irq(i2c_imx->irq, i2c_imx_isr, 0, pdev->name, i2c_imx);
@@ -549,7 +580,6 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 fail5:
         free_irq(i2c_imx->irq, i2c_imx);
 fail4:
-        clk_disable(i2c_imx->clk);
         clk_put(i2c_imx->clk);
 fail3:
         release_mem_region(i2c_imx->res->start, resource_size(res));
@@ -586,8 +616,6 @@ static int __exit i2c_imx_remove(struct platform_device *pdev)
         if (pdata && pdata->exit)
                 pdata->exit(&pdev->dev);
 
-        /* Disable I2C clock */
-        clk_disable(i2c_imx->clk);
         clk_put(i2c_imx->clk);
 
         release_mem_region(i2c_imx->res->start, resource_size(i2c_imx->res));
diff --git a/drivers/i2c/busses/i2c-mpc.c b/drivers/i2c/busses/i2c-mpc.c
index d325e86e3103..f627001108b8 100644
--- a/drivers/i2c/busses/i2c-mpc.c
+++ b/drivers/i2c/busses/i2c-mpc.c
@@ -365,9 +365,6 @@ static int mpc_write(struct mpc_i2c *i2c, int target,
         unsigned timeout = i2c->adap.timeout;
         u32 flags = restart ? CCR_RSTA : 0;
 
-        /* Start with MEN */
-        if (!restart)
-                writeccr(i2c, CCR_MEN);
         /* Start as master */
         writeccr(i2c, CCR_MIEN | CCR_MEN | CCR_MSTA | CCR_MTX | flags);
         /* Write target byte */
@@ -396,9 +393,6 @@ static int mpc_read(struct mpc_i2c *i2c, int target,
         int i, result;
         u32 flags = restart ? CCR_RSTA : 0;
 
-        /* Start with MEN */
-        if (!restart)
-                writeccr(i2c, CCR_MEN);
         /* Switch to read - restart */
         writeccr(i2c, CCR_MIEN | CCR_MEN | CCR_MSTA | CCR_MTX | flags);
         /* Write target address byte - this time with the read flag set */
@@ -425,9 +419,9 @@ static int mpc_read(struct mpc_i2c *i2c, int target,
                 /* Generate txack on next to last byte */
                 if (i == length - 2)
                         writeccr(i2c, CCR_MIEN | CCR_MEN | CCR_MSTA | CCR_TXAK);
-                /* Generate stop on last byte */
+                /* Do not generate stop on last byte */
                 if (i == length - 1)
-                        writeccr(i2c, CCR_MIEN | CCR_MEN | CCR_TXAK);
+                        writeccr(i2c, CCR_MIEN | CCR_MEN | CCR_MSTA | CCR_MTX);
                 data[i] = readb(i2c->base + MPC_I2C_DR);
         }