1 files changed, 42 insertions, 36 deletions

diff --git a/drivers/i2c/omap24xx_i2c.c b/drivers/i2c/omap24xx_i2c.c
index d176b5d..35201c3 100644
--- a/drivers/i2c/omap24xx_i2c.c
+++ b/drivers/i2c/omap24xx_i2c.c
@@ -218,7 +218,7 @@ read_exit:
 static int i2c_write_byte (u8 devaddr, u8 regoffset, u8 value)
 {
 	int i2c_error = 0;
-	u16 status, stat;
+	u16 status;
 
 	/* wait until bus not busy */
 	wait_for_bb ();
@@ -231,49 +231,55 @@ static int i2c_write_byte (u8 devaddr, u8 regoffset, u8 value)
 	writew (I2C_CON_EN | I2C_CON_MST | I2C_CON_STT | I2C_CON_TRX |
 		I2C_CON_STP, &i2c_base->con);
 
-	/* wait until state change */
-	status = wait_for_pin ();
-
-	if (status & I2C_STAT_XRDY) {
-#if defined(CONFIG_OMAP243X) || defined(CONFIG_OMAP34XX) || \
-    defined(CONFIG_OMAP44XX)
-		/* send out 1 byte */
-		writeb (regoffset, &i2c_base->data);
-		writew (I2C_STAT_XRDY, &i2c_base->stat);
-
-		status = wait_for_pin ();
-		if ((status & I2C_STAT_XRDY)) {
-			/* send out next 1 byte */
-			writeb (value, &i2c_base->data);
-			writew (I2C_STAT_XRDY, &i2c_base->stat);
-		} else {
+	while (1) {
+		status = wait_for_pin();
+		if (status == 0 || status & I2C_STAT_NACK) {
 			i2c_error = 1;
+			goto write_exit;
 		}
+		if (status & I2C_STAT_XRDY) {
+#if defined(CONFIG_OMAP243X) || defined(CONFIG_OMAP34XX) || \
+    defined(CONFIG_OMAP44XX)
+			/* send register offset */
+			writeb(regoffset, &i2c_base->data);
+			writew(I2C_STAT_XRDY, &i2c_base->stat);
+
+			while (1) {
+				status = wait_for_pin();
+				if (status == 0 || status & I2C_STAT_NACK) {
+					i2c_error = 1;
+					goto write_exit;
+				}
+				if (status & I2C_STAT_XRDY) {
+					/* send data */
+					writeb(value, &i2c_base->data);
+					writew(I2C_STAT_XRDY, &i2c_base->stat);
+				}
+				if (status & I2C_STAT_ARDY) {
+					writew(I2C_STAT_ARDY, &i2c_base->stat);
+					break;
+				}
+			}
+			break;
 #else
-		/* send out two bytes */
-		writew ((value << 8) + regoffset, &i2c_base->data);
+			/* send out two bytes */
+			writew((value << 8) + regoffset, &i2c_base->data);
+			writew(I2C_STAT_XRDY, &i2c_base->stat);
 #endif
-		/* must have enough delay to allow BB bit to go low */
-		udelay (50000);
-		if (readw (&i2c_base->stat) & I2C_STAT_NACK) {
-			i2c_error = 1;
 		}
-	} else {
-		i2c_error = 1;
+		if (status & I2C_STAT_ARDY) {
+			writew(I2C_STAT_ARDY, &i2c_base->stat);
+			break;
+		}
 	}
 
-	if (!i2c_error) {
-		int eout = 200;
+	wait_for_bb();
 
-		writew (I2C_CON_EN, &i2c_base->con);
-		while ((stat = readw (&i2c_base->stat)) || (readw (&i2c_base->con) & I2C_CON_MST)) {
-			udelay (1000);
-			/* have to read to clear intrrupt */
-			writew (0xFFFF, &i2c_base->stat);
-			if(--eout == 0) /* better leave with error than hang */
-				break;
-		}
-	}
+	status = readw(&i2c_base->stat);
+	if (status & I2C_STAT_NACK)
+		i2c_error = 1;
+
+write_exit:
 	flush_fifo();
 	writew (0xFFFF, &i2c_base->stat);
 	writew (0, &i2c_base->cnt);