omap i2c interrupt handler fixes

index 75bf3ad..ad8242a 100644
@@ -671,6 +671,27 @@ omap_i2c_rev1_isr(int this_irq, void *dev_id)
 #define omap_i2c_rev1_isr		NULL
 #endif
 
+/*
+ * OMAP3430 Errata 1.153: When an XRDY/XDR is hit, wait for XUDF before writing
+ * data to DATA_REG. Otherwise some data bytes can be lost while transferring
+ * them from the memory to the I2C interface.
+ */
+static int omap3430_workaround(struct omap_i2c_dev *dev, u16 *stat, int *err)
+{
+	while (!(*stat & OMAP_I2C_STAT_XUDF)) {
+		if (*stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
+			omap_i2c_ack_stat(dev, *stat & (OMAP_I2C_STAT_XRDY |
+							OMAP_I2C_STAT_XDR));
+			*err |= OMAP_I2C_STAT_XUDF;
+			return -1;
+		}
+		cpu_relax();
+		*stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+	}
+
+	return 0;
+}
+
 static irqreturn_t
 omap_i2c_isr(int this_irq, void *dev_id)
 {

@@ -794,25 +815,9 @@ complete:
 					break;
 				}
 
-				/*
-				 * OMAP3430 Errata 1.153: When an XRDY/XDR
-				 * is hit, wait for XUDF before writing data
-				 * to DATA_REG. Otherwise some data bytes can
-				 * be lost while transferring them from the
-				 * memory to the I2C interface.
-				 */
-
-				if (dev->rev <= OMAP_I2C_REV_ON_3430) {
-						while (!(stat & OMAP_I2C_STAT_XUDF)) {
-							if (stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
-								omap_i2c_ack_stat(dev, stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
-								err |= OMAP_I2C_STAT_XUDF;
-								goto complete;
-							}
-							cpu_relax();
-							stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
-						}
-				}
+				if (dev->rev <= OMAP_I2C_REV_ON_3430 &&
+				    omap3430_workaround(dev, &stat, &err))
+					goto complete;
 
 				omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
 			}

index ad8242a..b474c20 100644
@@ -678,7 +678,9 @@ omap_i2c_rev1_isr(int this_irq, void *dev_id)
  */
 static int omap3430_workaround(struct omap_i2c_dev *dev, u16 *stat, int *err)
 {
-	while (!(*stat & OMAP_I2C_STAT_XUDF)) {
+	unsigned long timeout = 10000;
+
+	while (!(*stat & OMAP_I2C_STAT_XUDF && --timeout)) {
 		if (*stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
 			omap_i2c_ack_stat(dev, *stat & (OMAP_I2C_STAT_XRDY |
 							OMAP_I2C_STAT_XDR));

@@ -689,6 +691,9 @@ static int omap3430_workaround(struct omap_i2c_dev *dev, u16 *stat, int *err)
 		*stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
 	}
 
+	if (!timeout)
+		dev_err(dev->dev, "timeout waiting on XUDF bit\n");
+
 	return 0;
 }
 

index 2d146ac..7d56a25 100644
@@ -678,6 +678,8 @@ omap_i2c_rev1_isr(int this_irq, void *dev_id)
  */
 static int errata_omap3_1p153(struct omap_i2c_dev *dev, u16 *stat, int *err)
 {
+	unsigned long timeout = jiffies + msecs_to_jiffies(1);
+
 	while (!(*stat & OMAP_I2C_STAT_XUDF)) {
 		if (*stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
 			omap_i2c_ack_stat(dev, *stat & (OMAP_I2C_STAT_XRDY |

@@ -685,6 +687,12 @@ static int errata_omap3_1p153(struct omap_i2c_dev *dev, u16 *stat, int *err)
 			*err |= OMAP_I2C_STAT_XUDF;
 			return -ETIMEDOUT;
 		}
+
+		if (time_after(jiffies, timeout)) {
+			dev_err(dev->dev, "timeout waiting on XUDF bit\n");
+			return 0;
+		}
+
 		cpu_relax();
 		*stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
 	}

index ef73483..00fd02e 100644
@@ -763,17 +763,25 @@ omap_i2c_rev1_isr(int this_irq, void *dev_id)
  */
 static int errata_omap3_1p153(struct omap_i2c_dev *dev, u16 *stat, int *err)
 {
-	while (!(*stat & OMAP_I2C_STAT_XUDF)) {
+	unsigned long timeout = 10000;
+
+	while (--timeout && !(*stat & OMAP_I2C_STAT_XUDF)) {
 		if (*stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
 			omap_i2c_ack_stat(dev, *stat & (OMAP_I2C_STAT_XRDY |
 							OMAP_I2C_STAT_XDR));
 			*err |= OMAP_I2C_STAT_XUDF;
 			return -ETIMEDOUT;
 		}
+
 		cpu_relax();
 		*stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
 	}
 
+	if (!timeout) {
+		dev_err(dev->dev, "timeout waiting on XUDF bit\n");
+		return 0;
+	}
+
 	return 0;
 }
 

index 75bf3ad..2d146ac 100644
@@ -671,6 +671,27 @@ omap_i2c_rev1_isr(int this_irq, void *dev_id)
 #define omap_i2c_rev1_isr		NULL
 #endif
 
+/*
+ * OMAP3430 Errata 1.153: When an XRDY/XDR is hit, wait for XUDF before writing
+ * data to DATA_REG. Otherwise some data bytes can be lost while transferring
+ * them from the memory to the I2C interface.
+ */
+static int errata_omap3_1p153(struct omap_i2c_dev *dev, u16 *stat, int *err)
+{
+	while (!(*stat & OMAP_I2C_STAT_XUDF)) {
+		if (*stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
+			omap_i2c_ack_stat(dev, *stat & (OMAP_I2C_STAT_XRDY |
+							OMAP_I2C_STAT_XDR));
+			*err |= OMAP_I2C_STAT_XUDF;
+			return -ETIMEDOUT;
+		}
+		cpu_relax();
+		*stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
+	}
+
+	return 0;
+}
+
 static irqreturn_t
 omap_i2c_isr(int this_irq, void *dev_id)
 {

@@ -794,25 +815,9 @@ complete:
 					break;
 				}
 
-				/*
-				 * OMAP3430 Errata 1.153: When an XRDY/XDR
-				 * is hit, wait for XUDF before writing data
-				 * to DATA_REG. Otherwise some data bytes can
-				 * be lost while transferring them from the
-				 * memory to the I2C interface.
-				 */
-
-				if (dev->rev <= OMAP_I2C_REV_ON_3430) {
-						while (!(stat & OMAP_I2C_STAT_XUDF)) {
-							if (stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
-								omap_i2c_ack_stat(dev, stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
-								err |= OMAP_I2C_STAT_XUDF;
-								goto complete;
-							}
-							cpu_relax();
-							stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
-						}
-				}
+				if ((dev->rev <= OMAP_I2C_REV_ON_3430) &&
+				    errata_omap3_1p153(dev, &stat, &err))
+					goto complete;
 
 				omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
 			}

@@ -38,6 +38,15 @@
 #include <linux/clk.h>
 #include <linux/io.h>
 
+/* Hack to enable zero length transfers and smbus quick until clean fix
+   is available */
+#define OMAP_HACK
+
+/* timeout waiting for the controller to respond */
+#define OMAP_I2C_TIMEOUT (msecs_to_jiffies(1000))
+
+
+
 /* I2C controller revisions */
 #define OMAP_I2C_REV_2			0x20
 

@@ -167,6 +176,11 @@
 	struct resource		*ioarea;
 	u32			speed;		/* Speed of bus in Khz */
 	u16			cmd_err;
+
+	u16                     cur_msg;
+	u16                     nbr_msg;
+	struct i2c_msg          *msgs;
+
 	u8			*buf;
 	size_t			buf_len;
 	struct i2c_adapter	adapter;

@@ -436,134 +450,142 @@
 /*
  * Low level master read/write transaction.
  */
-static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
+static void omap_i2c_xfer_msg(struct i2c_adapter *adap,
 			     struct i2c_msg *msg, int stop)
 {
 	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
-	int r;
+
+#ifdef OMAP_HACK
+        u8 zero_byte = 0;
+#endif
+
 	u16 w;
 
 	dev_dbg(dev->dev, "addr: 0x%04x, len: %d, flags: 0x%x, stop: %d\n",
 		msg->addr, msg->len, msg->flags, stop);
 
-	if (msg->len == 0)
-		return -EINVAL;
+#ifndef OMAP_HACK
+        if (msg->len == 0)
+                return -EINVAL;
+
+        omap_i2c_write_reg(dev, OMAP_I2C_SA_REG, msg->addr);
+
+        /* REVISIT: Could the STB bit of I2C_CON be used with probing? */
+        dev->buf = msg->buf;
+        dev->buf_len = msg->len;
+#else
 
-	omap_i2c_write_reg(dev, OMAP_I2C_SA_REG, msg->addr);
+        omap_i2c_write_reg(dev, OMAP_I2C_SA_REG, msg->addr);
+        /* REVISIT: Remove this hack when we can get I2C chips from board-*.c
+         *          files
+         * Sigh, seems we can't do zero length transactions. Thus, we
+         * can't probe for devices w/o actually sending/receiving at least
+         * a single byte. So we'll set count to 1 for the zero length
+         * transaction case and hope we don't cause grief for some
+         * arbitrary device due to random byte write/read during
+         * probes.
+         */
+        if (msg->len == 0) {
+                dev->buf = &zero_byte;
+                dev->buf_len = 1;
+        } else {
+                dev->buf = msg->buf;
+                dev->buf_len = msg->len;
+        }
+#endif
 
-	/* REVISIT: Could the STB bit of I2C_CON be used with probing? */
-	dev->buf = msg->buf;
-	dev->buf_len = msg->len;
 
 	omap_i2c_write_reg(dev, OMAP_I2C_CNT_REG, dev->buf_len);
 
-	/* Clear the FIFO Buffers */
-	w = omap_i2c_read_reg(dev, OMAP_I2C_BUF_REG);
-	w |= OMAP_I2C_BUF_RXFIF_CLR | OMAP_I2C_BUF_TXFIF_CLR;
-	omap_i2c_write_reg(dev, OMAP_I2C_BUF_REG, w);
+        /* Clear the FIFO Buffers */
+        w = omap_i2c_read_reg(dev, OMAP_I2C_BUF_REG);
+        w |= OMAP_I2C_BUF_RXFIF_CLR | OMAP_I2C_BUF_TXFIF_CLR;
+        omap_i2c_write_reg(dev, OMAP_I2C_BUF_REG, w);
+
+        dev->cmd_err = 0;
+
+        w = OMAP_I2C_CON_EN | OMAP_I2C_CON_MST | OMAP_I2C_CON_STT;
+
+        /* High speed configuration */
+        if (dev->speed > 400)
+                w |= OMAP_I2C_CON_OPMODE_HS;
+
+        if (msg->flags & I2C_M_TEN)
+                w |= OMAP_I2C_CON_XA;
+        if (!(msg->flags & I2C_M_RD))
+                w |= OMAP_I2C_CON_TRX;
+
+        if (!dev->b_hw && stop)
+                w |= OMAP_I2C_CON_STP;
+
+        omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, w);
+
+        if (dev->b_hw && stop) {
+                /* H/w behavior: dont write stt and stp together.. */
+                while (omap_i2c_read_reg(dev, OMAP_I2C_CON_REG) & OMAP_I2C_CON_STT) {
+                        /* Dont do anything - this will come in a couple of loops at max*/
+                }
+                w |= OMAP_I2C_CON_STP;
+                w &= ~OMAP_I2C_CON_STT;
+                omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, w);
+        }
+}
 
-	init_completion(&dev->cmd_complete);
-	dev->cmd_err = 0;
+/*
+ * Prepare controller for a transaction and call omap_i2c_xfer_msg
+ * to do the work during IRQ processing.
+ */
+static int
+omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
+{
+	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+	int r;
+	u16 w;
 
-	w = OMAP_I2C_CON_EN | OMAP_I2C_CON_MST | OMAP_I2C_CON_STT;
+	omap_i2c_unidle(dev);
 
-	/* High speed configuration */
-	if (dev->speed > 400)
-		w |= OMAP_I2C_CON_OPMODE_HS;
-
-	if (msg->flags & I2C_M_TEN)
-		w |= OMAP_I2C_CON_XA;
-	if (!(msg->flags & I2C_M_RD))
-		w |= OMAP_I2C_CON_TRX;
-
-	if (!dev->b_hw && stop)
-		w |= OMAP_I2C_CON_STP;
-
-	omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, w);
-
-	/*
-	 * Don't write stt and stp together on some hardware.
-	 */
-	if (dev->b_hw && stop) {
-		unsigned long delay = jiffies + OMAP_I2C_TIMEOUT;
-		u16 con = omap_i2c_read_reg(dev, OMAP_I2C_CON_REG);
-		while (con & OMAP_I2C_CON_STT) {
-			con = omap_i2c_read_reg(dev, OMAP_I2C_CON_REG);
-
-			/* Let the user know if i2c is in a bad state */
-			if (time_after(jiffies, delay)) {
-				dev_err(dev->dev, "controller timed out "
-				"waiting for start condition to finish\n");
-				return -ETIMEDOUT;
-			}
-			cpu_relax();
-		}
 
-		w |= OMAP_I2C_CON_STP;
-		w &= ~OMAP_I2C_CON_STT;
-		omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, w);
-	}
+	r = omap_i2c_wait_for_bb(dev);
+	if (r < 0)
+		goto out;
+
+	dev->cur_msg = 0;
+	dev->nbr_msg = num;
+	dev->msgs = msgs;
+
+	init_completion(&dev->cmd_complete);
+
+	omap_i2c_xfer_msg(adap, &msgs[0], num == 1);
+
 
-	/*
-	 * REVISIT: We should abort the transfer on signals, but the bus goes
-	 * into arbitration and we're currently unable to recover from it.
-	 */
 	r = wait_for_completion_timeout(&dev->cmd_complete,
 					OMAP_I2C_TIMEOUT);
-	dev->buf_len = 0;
-	if (r < 0)
-		return r;
 	if (r == 0) {
 		dev_err(dev->dev, "controller timed out\n");
 		omap_i2c_init(dev);
-		return -ETIMEDOUT;
+		r = -ETIMEDOUT;
 	}
-
-	if (likely(!dev->cmd_err))
-		return 0;
+	else
+		if (likely(!dev->cmd_err))
+			r = 0;
 
 	/* We have an error */
 	if (dev->cmd_err & (OMAP_I2C_STAT_AL | OMAP_I2C_STAT_ROVR |
 			    OMAP_I2C_STAT_XUDF)) {
 		omap_i2c_init(dev);
-		return -EIO;
+		r = -EIO;
 	}
 
 	if (dev->cmd_err & OMAP_I2C_STAT_NACK) {
-		if (msg->flags & I2C_M_IGNORE_NAK)
-			return 0;
-		if (stop) {
+               if (msgs[0].flags & I2C_M_IGNORE_NAK)
+                       r = 0;
+               else {
+
 			w = omap_i2c_read_reg(dev, OMAP_I2C_CON_REG);
 			w |= OMAP_I2C_CON_STP;
 			omap_i2c_write_reg(dev, OMAP_I2C_CON_REG, w);
+			r = -EREMOTEIO;
 		}
-		return -EREMOTEIO;
-	}
-	return -EIO;
-}
-
-
-/*
- * Prepare controller for a transaction and call omap_i2c_xfer_msg
- * to do the work during IRQ processing.
- */
-static int
-omap_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
-{
-	struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
-	int i;
-	int r;
-
-	omap_i2c_unidle(dev);
-
-	r = omap_i2c_wait_for_bb(dev);
-	if (r < 0)
-		goto out;
-
-	for (i = 0; i < num; i++) {
-		r = omap_i2c_xfer_msg(adap, &msgs[i], (i == (num - 1)));
-		if (r != 0)
-			break;
 	}
 
 	if (r == 0)

@@ -576,7 +598,11 @@
 static u32
 omap_i2c_func(struct i2c_adapter *adap)
 {
+#ifndef OMAP_HACK
 	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
+#else
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+#endif
 }
 
 static inline void

@@ -659,7 +685,7 @@
 	struct omap_i2c_dev *dev = dev_id;
 	u16 bits;
 	u16 stat, w;
-	int err, count = 0;
+	int count = 0;
 
 	if (dev->idle)
 		return IRQ_NONE;

@@ -672,7 +698,6 @@
 			break;
 		}
 
-		err = 0;
 complete:
 		/*
 		 * Ack the stat in one go, but [R/X]DR and [R/X]RDY should be

@@ -683,134 +708,110 @@
 				~(OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR |
 				OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
 
-		if (stat & OMAP_I2C_STAT_NACK) {
-			err |= OMAP_I2C_STAT_NACK;
-			omap_i2c_write_reg(dev, OMAP_I2C_CON_REG,
-					   OMAP_I2C_CON_STP);
-		}
-		if (stat & OMAP_I2C_STAT_AL) {
-			dev_err(dev->dev, "Arbitration lost\n");
-			err |= OMAP_I2C_STAT_AL;
-		}
-		if (stat & (OMAP_I2C_STAT_ARDY | OMAP_I2C_STAT_NACK |
-					OMAP_I2C_STAT_AL)) {
-			omap_i2c_ack_stat(dev, stat &
-				(OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR |
-				OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
-			omap_i2c_complete_cmd(dev, err);
-			return IRQ_HANDLED;
-		}
-		if (stat & (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR)) {
-			u8 num_bytes = 1;
-			if (dev->fifo_size) {
-				if (stat & OMAP_I2C_STAT_RRDY)
-					num_bytes = dev->fifo_size;
-				else    /* read RXSTAT on RDR interrupt */
-					num_bytes = (omap_i2c_read_reg(dev,
-							OMAP_I2C_BUFSTAT_REG)
-							>> 8) & 0x3F;
-			}
-			while (num_bytes) {
-				num_bytes--;
-				w = omap_i2c_read_reg(dev, OMAP_I2C_DATA_REG);
-				if (dev->buf_len) {
-					*dev->buf++ = w;
-					dev->buf_len--;
-					/* Data reg from 2430 is 8 bit wide */
-					if (!cpu_is_omap2430() &&
-							!cpu_is_omap34xx()) {
-						if (dev->buf_len) {
-							*dev->buf++ = w >> 8;
-							dev->buf_len--;
-						}
-					}
-				} else {
-					if (stat & OMAP_I2C_STAT_RRDY)
-						dev_err(dev->dev,
-							"RRDY IRQ while no data"
-								" requested\n");
-					if (stat & OMAP_I2C_STAT_RDR)
-						dev_err(dev->dev,
-							"RDR IRQ while no data"
-								" requested\n");
-					break;
-				}
-			}
-			omap_i2c_ack_stat(dev,
-				stat & (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR));
-			continue;
-		}
-		if (stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR)) {
-			u8 num_bytes = 1;
-			if (dev->fifo_size) {
-				if (stat & OMAP_I2C_STAT_XRDY)
-					num_bytes = dev->fifo_size;
-				else    /* read TXSTAT on XDR interrupt */
-					num_bytes = omap_i2c_read_reg(dev,
-							OMAP_I2C_BUFSTAT_REG)
-							& 0x3F;
-			}
-			while (num_bytes) {
-				num_bytes--;
-				w = 0;
-				if (dev->buf_len) {
-					w = *dev->buf++;
-					dev->buf_len--;
-					/* Data reg from  2430 is 8 bit wide */
-					if (!cpu_is_omap2430() &&
-							!cpu_is_omap34xx()) {
-						if (dev->buf_len) {
-							w |= *dev->buf++ << 8;
-							dev->buf_len--;
-						}
-					}
-				} else {
-					if (stat & OMAP_I2C_STAT_XRDY)
-						dev_err(dev->dev,
-							"XRDY IRQ while no "
-							"data to send\n");
-					if (stat & OMAP_I2C_STAT_XDR)
-						dev_err(dev->dev,
-							"XDR IRQ while no "
-							"data to send\n");
-					break;
-				}
-
-				/*
-				 * OMAP3430 Errata 1.153: When an XRDY/XDR
-				 * is hit, wait for XUDF before writing data
-				 * to DATA_REG. Otherwise some data bytes can
-				 * be lost while transferring them from the
-				 * memory to the I2C interface.
-				 */
-
-				if (dev->rev <= OMAP_I2C_REV_ON_3430) {
-						while (!(stat & OMAP_I2C_STAT_XUDF)) {
-							if (stat & (OMAP_I2C_STAT_NACK | OMAP_I2C_STAT_AL)) {
-								omap_i2c_ack_stat(dev, stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
-								err |= OMAP_I2C_STAT_XUDF;
-								goto complete;
-							}
-							cpu_relax();
-							stat = omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG);
-						}
-				}
+		if (stat & OMAP_I2C_STAT_ARDY) {
+			// the bus is avaiable for next msg.
+			dev->cur_msg ++;
+
+			// Stop if the last msg is sent or errors
+			if ((dev->cur_msg >= dev->nbr_msg) || dev->cmd_err)
+				omap_i2c_complete_cmd(dev, 0);
+			else
+				omap_i2c_xfer_msg(&dev->adapter,
+				&dev->msgs[dev->cur_msg], dev->cur_msg == (dev->nbr_msg - 1));
+			//omap_i2c_complete_cmd(dev, 0);
 
-				omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
-			}
-			omap_i2c_ack_stat(dev,
-				stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR));
 			continue;
 		}
-		if (stat & OMAP_I2C_STAT_ROVR) {
-			dev_err(dev->dev, "Receive overrun\n");
-			dev->cmd_err |= OMAP_I2C_STAT_ROVR;
-		}
-		if (stat & OMAP_I2C_STAT_XUDF) {
-			dev_err(dev->dev, "Transmit underflow\n");
-			dev->cmd_err |= OMAP_I2C_STAT_XUDF;
-		}
-	}
+
+                if (stat & (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR)) {
+                         u8 num_bytes = 1;
+                         if (dev->fifo_size) {
+                                 num_bytes = (stat & OMAP_I2C_STAT_RRDY) ? dev->fifo_size :
+                                                 omap_i2c_read_reg(dev, OMAP_I2C_BUFSTAT_REG);
+                         }
+                         while (num_bytes) {
+                                 num_bytes--;
+                                 w = omap_i2c_read_reg(dev, OMAP_I2C_DATA_REG);
+                                 if (dev->buf_len) {
+                                         *dev->buf++ = w;
+                                         dev->buf_len--;
+                                         /* data reg from 2430 is 8 bit wide */
+                                         if (!cpu_is_omap2430() &&
+                                                         !cpu_is_omap34xx()) {
+                                                 if (dev->buf_len) {
+                                                         *dev->buf++ = w >> 8;
+                                                         dev->buf_len--;
+                                                 }
+                                         }
+                                 } else {
+                                         if (stat & OMAP_I2C_STAT_RRDY)
+                                                 dev_err(dev->dev, "RRDY IRQ while no data"
+                                                                 "requested\n");
+                                         if (stat & OMAP_I2C_STAT_RDR)
+                                                 dev_err(dev->dev, "RDR IRQ while no data"
+                                                                 "requested\n");
+                                         break;
+                                 }
+                         }
+                         omap_i2c_ack_stat(dev, stat & (OMAP_I2C_STAT_RRDY | OMAP_I2C_STAT_RDR));
+                         continue;
+                }
+
+                if (stat & (OMAP_I2C_STAT_XRDY | OMAP_I2C_STAT_XDR)) {
+                        u8 num_bytes = 1;
+                        if (dev->fifo_size) {
+                                num_bytes = (stat & OMAP_I2C_STAT_XRDY) ? dev->fifo_size :
+                                                omap_i2c_read_reg(dev, OMAP_I2C_BUFSTAT_REG);
+                        }
+                        while (num_bytes) {
+                                num_bytes--;
+                                w = 0;
+                                if (dev->buf_len) {
+                                        w = *dev->buf++;
+                                        dev->buf_len--;
+                                        /* data reg from 2430 is 8 bit wide */
+                                        if (!cpu_is_omap2430() &&
+                                                        !cpu_is_omap34xx()) {
+                                                if (dev->buf_len) {
+                                                        w |= *dev->buf++ << 8;
+                                                        dev->buf_len--;
+                                                }
+                                        }
+                                } else {
+                                        if (stat & OMAP_I2C_STAT_XRDY)
+                                                dev_err(dev->dev, "XRDY IRQ while no"
+                                                                "data to send\n");
+                                        if (stat & OMAP_I2C_STAT_XDR)
+                                                dev_err(dev->dev, "XDR IRQ while no"
+                                                                "data to send\n");
+                                        break;
+                                }
+                                omap_i2c_write_reg(dev, OMAP_I2C_DATA_REG, w);
+                        }
+                        omap_i2c_ack_stat(dev, stat &
+                                        (OMAP_I2C_STAT_XRDY |
+                                                 OMAP_I2C_STAT_XDR));
+                        continue;
+                }
+                if (stat & OMAP_I2C_STAT_ROVR) {
+                        dev_err(dev->dev, "Receive overrun\n");
+                        dev->cmd_err |= OMAP_I2C_STAT_ROVR;
+                }
+                if (stat & OMAP_I2C_STAT_XUDF) {
+                        dev_err(dev->dev, "Transmit overflow\n");
+                        dev->cmd_err |= OMAP_I2C_STAT_XUDF;
+                }
+                if (stat & OMAP_I2C_STAT_NACK) {
+                        dev_err(dev->dev, "No Acknowledgement\n");
+                        omap_i2c_complete_cmd(dev, OMAP_I2C_STAT_NACK);
+                        omap_i2c_write_reg(dev, OMAP_I2C_CON_REG,
+                                           OMAP_I2C_CON_STP);
+                }
+                if (stat & OMAP_I2C_STAT_AL) {
+                        dev_err(dev->dev, "Arbitration lost\n");
+                        omap_i2c_complete_cmd(dev, OMAP_I2C_STAT_AL);
+                }
+        }
 
 	return count ? IRQ_HANDLED : IRQ_NONE;
 }