[PATCH] i2c: imx: check busy bit when START/STOP

[Richard Zhao <linuxzsc-Re5JQEeQqe8AvxtiuMwx3w@public.gmane.org>]

After START/RESTART, wait for busy bit to be set and
after STOP, wait for busy bit to be clear.

Signed-off-by: Richard Zhao <linuxzsc-Re5JQEeQqe8AvxtiuMwx3w@public.gmane.org>
---
 drivers/i2c/busses/i2c-imx.c |   46 +++++++++++++++++++++++++++++------------
 1 files changed, 32 insertions(+), 14 deletions(-)

diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 4afba3e..156cc95 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -125,14 +125,19 @@ struct imx_i2c_struct {
 /** 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) {
+	temp = readb(i2c_imx->base + IMX_I2C_I2SR);
+	while (1) {
+		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__);
@@ -144,6 +149,7 @@ static int i2c_imx_bus_busy(struct imx_i2c_struct *i2c_imx)
 			return -EIO;
 		}
 		schedule();
+		temp = readb(i2c_imx->base + IMX_I2C_I2SR);
 	}
 
 	return 0;
@@ -179,20 +185,32 @@ 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__);
 
 	/* Enable I2C controller */
+	writeb(0, i2c_imx->base + IMX_I2C_I2SR);
 	writeb(I2CR_IEN, i2c_imx->base + IMX_I2C_I2CR);
+
+	result = i2c_imx_bus_busy(i2c_imx, 0);
+	if (result)
+		return result;
+
 	/* 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;
+
 	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)
@@ -202,16 +220,16 @@ static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
 	/* Stop I2C transaction */
 	dev_dbg(&i2c_imx->adapter.dev, "<%s>\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);
-	/* 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);
+
+	i2c_imx_bus_busy(i2c_imx, 0);
+
 	/* Disable I2C controller */
 	writeb(0, i2c_imx->base + IMX_I2C_I2CR);
 }
@@ -344,7 +362,7 @@ static int i2c_imx_read(struct imx_i2c_struct *i2c_imx, struct i2c_msg *msgs)
 			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);
 		} else if (i == (msgs->len - 2)) {
 			dev_dbg(&i2c_imx->adapter.dev,
@@ -370,14 +388,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 +401,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);
-- 
1.6.0.4