i2c: octeon: Support I2C_M_RECV_LEN

[cascardo/linux.git] / drivers / i2c / busses / i2c-octeon.c

diff --git a/drivers/i2c/busses/i2c-octeon.c b/drivers/i2c/busses/i2c-octeon.c
index 9240037..46fb6c4 100644 (file)
--- a/drivers/i2c/busses/i2c-octeon.c
+++ b/drivers/i2c/busses/i2c-octeon.c
@@ -315,16 +315,17 @@ static int octeon_i2c_write(struct octeon_i2c *i2c, int target,
  * @i2c: The struct octeon_i2c
  * @target: Target address
  * @data: Pointer to the location to store the data
- * @length: Length of the data
+ * @rlength: Length of the data
+ * @recv_len: flag for length byte
  *
  * The address is sent over the bus, then the data is read.
  *
  * Returns 0 on success, otherwise a negative errno.
  */
 static int octeon_i2c_read(struct octeon_i2c *i2c, int target,
-                          u8 *data, int length)
+                          u8 *data, u16 *rlength, bool recv_len)
 {
-       int i, result;
+       int i, result, length = *rlength;
        u8 tmp;
 
        if (length < 1)
@@ -363,7 +364,17 @@ static int octeon_i2c_read(struct octeon_i2c *i2c, int target,
                        return result;
 
                data[i] = octeon_i2c_read_sw(i2c, SW_TWSI_EOP_TWSI_DATA);
+               if (recv_len && i == 0) {
+                       if (data[i] > I2C_SMBUS_BLOCK_MAX + 1) {
+                               dev_err(i2c->dev,
+                                       "%s: read len > I2C_SMBUS_BLOCK_MAX %d\n",
+                                       __func__, data[i]);
+                               return -EPROTO;
+                       }
+                       length += data[i];
+               }
        }
+       *rlength = length;
        return 0;
 }
 
@@ -390,7 +401,7 @@ static int octeon_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
                         pmsg->len, pmsg->addr, i + 1, num);
                if (pmsg->flags & I2C_M_RD)
                        ret = octeon_i2c_read(i2c, pmsg->addr, pmsg->buf,
-                                             pmsg->len);
+                                             &pmsg->len, pmsg->flags & I2C_M_RECV_LEN);
                else
                        ret = octeon_i2c_write(i2c, pmsg->addr, pmsg->buf,
                                               pmsg->len);
@@ -402,7 +413,8 @@ static int octeon_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 
 static u32 octeon_i2c_functionality(struct i2c_adapter *adap)
 {
-       return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+       return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
+              I2C_FUNC_SMBUS_READ_BLOCK_DATA | I2C_SMBUS_BLOCK_PROC_CALL;
 }
 
 static const struct i2c_algorithm octeon_i2c_algo = {