1 files changed, 24 insertions, 19 deletions

diff --git a/drivers/i2c/busses/i2c-bfin-twi.c b/drivers/i2c/busses/i2c-bfin-twi.c
index c2e6b7849e8d..4799c6886946 100644
--- a/drivers/i2c/busses/i2c-bfin-twi.c
+++ b/drivers/i2c/busses/i2c-bfin-twi.c
@@ -130,21 +130,25 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
                         }
                         iface->transPtr++;
                         iface->readNum--;
-                } else if (iface->manual_stop) {
-                        /* Temporary workaround to avoid possible bus stall -
-                         * Flush FIFO before issuing the STOP condition
-                         */
-                        read_RCV_DATA16(iface);
-                        write_MASTER_CTL(iface,
-                                read_MASTER_CTL(iface) | STOP);
-                } else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
-                           iface->cur_msg + 1 < iface->msg_num) {
-                        if (iface->pmsg[iface->cur_msg + 1].flags & I2C_M_RD)
-                                write_MASTER_CTL(iface,
-                                        read_MASTER_CTL(iface) | RSTART | MDIR);
-                        else
+                }
+
+                if (iface->readNum == 0) {
+                        if (iface->manual_stop) {
+                                /* Temporary workaround to avoid possible bus stall -
+                                 * Flush FIFO before issuing the STOP condition
+                                 */
+                                read_RCV_DATA16(iface);
                                 write_MASTER_CTL(iface,
-                                        (read_MASTER_CTL(iface) | RSTART) & ~MDIR);
+                                        read_MASTER_CTL(iface) | STOP);
+                        } else if (iface->cur_mode == TWI_I2C_MODE_REPEAT &&
+                                        iface->cur_msg + 1 < iface->msg_num) {
+                                if (iface->pmsg[iface->cur_msg + 1].flags & I2C_M_RD)
+                                        write_MASTER_CTL(iface,
+                                                read_MASTER_CTL(iface) | RSTART | MDIR);
+                                else
+                                        write_MASTER_CTL(iface,
+                                                (read_MASTER_CTL(iface) | RSTART) & ~MDIR);
+                        }
                 }
         }
         if (twi_int_status & MERR) {
@@ -245,12 +249,13 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
                                 }
                         }
 
-                        if (iface->pmsg[iface->cur_msg].len <= 255)
-                                        write_MASTER_CTL(iface,
+                        if (iface->pmsg[iface->cur_msg].len <= 255) {
+                                write_MASTER_CTL(iface,
                                         (read_MASTER_CTL(iface) &
                                         (~(0xff << 6))) |
-                                (iface->pmsg[iface->cur_msg].len << 6));
-                        else {
+                                        (iface->pmsg[iface->cur_msg].len << 6));
+                                iface->manual_stop = 0;
+                        } else {
                                 write_MASTER_CTL(iface,
                                         (read_MASTER_CTL(iface) |
                                         (0xff << 6)));
@@ -264,8 +269,8 @@ static void bfin_twi_handle_interrupt(struct bfin_twi_iface *iface,
                         write_INT_MASK(iface, 0);
                         write_MASTER_CTL(iface, 0);
                 }
+                complete(&iface->complete);
         }
-        complete(&iface->complete);
 }
 
 /* Interrupt handler */