tm6000: fix i2c readings

From: Christopher Pascoe <c.pascoe@itee.uq.edu.au>

Signed-off-by: Mauro Carvalho Chehab <mchehab@redhat.com>

1 files changed, 26 insertions, 7 deletions

diff --git a/linux/drivers/staging/tm6000/tm6000-i2c.c b/linux/drivers/staging/tm6000/tm6000-i2c.c
index 751093eb7..d1ae9ffa0 100644
--- a/linux/drivers/staging/tm6000/tm6000-i2c.c
+++ b/linux/drivers/staging/tm6000/tm6000-i2c.c
@@ -95,7 +95,7 @@ static int tm6000_i2c_xfer(struct i2c_adapter *i2c_adap,
 {
 	struct tm6000_core *dev = i2c_adap->algo_data;
 	int addr, rc, i, byte;
-	u8 prev_reg = 0;
+	int prev_reg = -1;
 
 	if (num <= 0)
 		return 0;
@@ -114,11 +114,15 @@ static int tm6000_i2c_xfer(struct i2c_adapter *i2c_adap,
 	   out of message data.
 	 */
 			/* SMBus Read Byte command */
+			if (prev_reg < 0)
+				printk("XXX read from unknown prev_reg\n");
 			rc = tm6000_read_write_usb (dev,
 				USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 				REQ_16_SET_GET_I2CSEQ,
 				addr | (prev_reg << 8), 0,
 				msgs[i].buf, msgs[i].len);
+			if (prev_reg >= 0)
+				prev_reg += msgs[i].len;
 #if 0
 			if (rc>=0)
 				*msgs[i].buf=rc;
@@ -128,6 +132,24 @@ static int tm6000_i2c_xfer(struct i2c_adapter *i2c_adap,
 					printk(" %02x", msgs[i].buf[byte]);
 				}
 			}
+		} else if (i+1 < num && msgs[i].len == 2 &&
+			   (msgs[i+1].flags & I2C_M_RD) &&
+			   msgs[i].addr == msgs[i+1].addr) {
+			i2c_dprintk(2, "msg %d: write 2, read %d", i,
+				    msgs[i+1].len);
+			/* Write 2 Read N command */
+			rc = tm6000_read_write_usb (dev,
+				USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+				REQ_14_SET_GET_EEPROM_PAGE, /* XXX wrong name */
+				addr | msgs[i].buf[0] << 8, msgs[i].buf[1],
+				msgs[i+1].buf, msgs[i+1].len);
+			i++;
+			if (i2c_debug>=2) {
+				for (byte = 0; byte < msgs[i].len; byte++) {
+					printk(" %02x", msgs[i].buf[byte]);
+				}
+			}
+			prev_reg = -1;
 		} else {
 			/* write bytes */
 			if (i2c_debug>=2) {
@@ -139,6 +161,8 @@ static int tm6000_i2c_xfer(struct i2c_adapter *i2c_adap,
 			if(msgs[i].len == 1 && i+1 < num && msgs[i+1].flags & I2C_M_RD
 					    && msgs[i+1].addr == msgs[i].addr) {
 				prev_reg = msgs[i].buf[0];
+				if (i2c_debug >= 2)
+					printk("\n");
 				continue;
 			}
 
@@ -148,12 +172,7 @@ static int tm6000_i2c_xfer(struct i2c_adapter *i2c_adap,
 				addr|(*msgs[i].buf)<<8, 0,
 				msgs[i].buf+1, msgs[i].len-1);
 
-			if(msgs[i].len >= 1) {
-				prev_reg = msgs[i].buf[0];
-			}
-			else {
-				prev_reg = 0;
-			}
+			prev_reg = -1;
 		}
 		if (i2c_debug>=2)
 			printk("\n");