1 files changed, 32 insertions, 20 deletions

diff --git a/drivers/i2c/algos/i2c-algo-bit.c b/drivers/i2c/algos/i2c-algo-bit.c
index 8a5f5825bb72..7f0a0a62cf60 100644
--- a/drivers/i2c/algos/i2c-algo-bit.c
+++ b/drivers/i2c/algos/i2c-algo-bit.c
@@ -357,13 +357,29 @@ static int sendbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msg)
 	return wrcount;
 }
 
+static int acknak(struct i2c_adapter *i2c_adap, int is_ack)
+{
+	struct i2c_algo_bit_data *adap = i2c_adap->algo_data;
+
+	/* assert: sda is high */
+	if (is_ack)		/* send ack */
+		setsda(adap, 0);
+	udelay((adap->udelay + 1) / 2);
+	if (sclhi(adap) < 0) {	/* timeout */
+		dev_err(&i2c_adap->dev, "readbytes: ack/nak timeout\n");
+		return -ETIMEDOUT;
+	}
+	scllo(adap);
+	return 0;
+}
+
 static int readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msg)
 {
 	int inval;
 	int rdcount=0;   	/* counts bytes read */
-	struct i2c_algo_bit_data *adap = i2c_adap->algo_data;
 	unsigned char *temp = msg->buf;
 	int count = msg->len;
+	const unsigned flags = msg->flags;
 
 	while (count > 0) {
 		inval = i2c_inb(i2c_adap);
@@ -377,28 +393,12 @@ static int readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msg)
 		temp++;
 		count--;
 
-		if (msg->flags & I2C_M_NO_RD_ACK) {
-			bit_dbg(2, &i2c_adap->dev, "i2c_inb: 0x%02x\n",
-				inval);
-			continue;
-		}
-
-		/* assert: sda is high */
-		if (count)		/* send ack */
-			setsda(adap, 0);
-		udelay((adap->udelay + 1) / 2);
-		bit_dbg(2, &i2c_adap->dev, "i2c_inb: 0x%02x %s\n", inval,
-			count ? "A" : "NA");
-		if (sclhi(adap)<0) {	/* timeout */
-			dev_err(&i2c_adap->dev, "readbytes: timeout at ack\n");
-			return -ETIMEDOUT;
-		};
-		scllo(adap);
-
 		/* Some SMBus transactions require that we receive the
 		   transaction length as the first read byte. */
-		if (rdcount == 1 && (msg->flags & I2C_M_RECV_LEN)) {
+		if (rdcount == 1 && (flags & I2C_M_RECV_LEN)) {
 			if (inval <= 0 || inval > I2C_SMBUS_BLOCK_MAX) {
+				if (!(flags & I2C_M_NO_RD_ACK))
+					acknak(i2c_adap, 0);
 				dev_err(&i2c_adap->dev, "readbytes: invalid "
 					"block length (%d)\n", inval);
 				return -EREMOTEIO;
@@ -409,6 +409,18 @@ static int readbytes(struct i2c_adapter *i2c_adap, struct i2c_msg *msg)
 			count += inval;
 			msg->len += inval;
 		}
+
+		bit_dbg(2, &i2c_adap->dev, "readbytes: 0x%02x %s\n",
+			inval,
+			(flags & I2C_M_NO_RD_ACK)
+				? "(no ack/nak)"
+				: (count ? "A" : "NA"));
+
+		if (!(flags & I2C_M_NO_RD_ACK)) {
+			inval = acknak(i2c_adap, count);
+			if (inval < 0)
+				return inval;
+		}
 	}
 	return rdcount;
 }