[PATCH] i2c: mv64xxx: Implement I2C_M_RECV_LEN and I2C_FUNC_SMBUS_READ_BLOCK_DATA

[Federico Fuga]

The i2c_mv64xxx driver doesn't implement the I2C_M_REC_LEN function
essential to allow blocks with variable length to be read from an i2c
 slave.
This is needed to implement the SMBus Read Block Data function.

This patch implements the function by changing the bytes_left and
msg len on the fly if the flag is specified.

It has been successfully tested on Allwinner A33 with a special
i2c chip that returns variable length blocks on reading.

Signed-off-by: Federico Fuga <fuga@xxxxxxxxxxxxxx>
Acked-by: Gregory CLEMENT <gregory.clement@xxxxxxxxxxx>
Link: https://lore.kernel.org/r/20200118115820.9080-1-fuga@xxxxxxxxxxxxxx
[groeck: Various fixes]
Signed-off-by: Guenter Roeck <linux@xxxxxxxxxxxx>
---
 drivers/i2c/busses/i2c-mv64xxx.c | 75 ++++++++++++++++++++++++++------
 1 file changed, 61 insertions(+), 14 deletions(-)

diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index 5c8e94b6cdb5..9dda90359966 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -341,7 +341,17 @@ static void mv64xxx_i2c_send_start(struct mv64xxx_i2c_data *drv_data)
 {
 	drv_data->msg = drv_data->msgs;
 	drv_data->byte_posn = 0;
-	drv_data->bytes_left = drv_data->msg->len;
+
+	/*
+	 * If we should retrieve the length from the buffer, make sure
+	 * to read enough bytes to avoid sending the STOP bit after
+	 * the read if the first byte
+	 */
+	if (drv_data->msg->flags & I2C_M_RECV_LEN)
+		drv_data->bytes_left = 3;
+	else
+		drv_data->bytes_left = drv_data->msg->len;
+
 	drv_data->aborting = 0;
 	drv_data->rc = 0;
 
@@ -350,6 +360,47 @@ static void mv64xxx_i2c_send_start(struct mv64xxx_i2c_data *drv_data)
 	       drv_data->reg_base + drv_data->reg_offsets.control);
 }
 
+static void
+mv64xxx_i2c_do_send_stop(struct mv64xxx_i2c_data *drv_data)
+{
+	drv_data->cntl_bits &= ~MV64XXX_I2C_REG_CONTROL_INTEN;
+	writel(drv_data->cntl_bits | MV64XXX_I2C_REG_CONTROL_STOP,
+	       drv_data->reg_base + drv_data->reg_offsets.control);
+	drv_data->block = 0;
+	if (drv_data->errata_delay)
+		udelay(5);
+
+	wake_up(&drv_data->waitq);
+}
+
+static void
+mv64xxx_i2c_do_read_data(struct mv64xxx_i2c_data *drv_data)
+{
+	u8 data;
+
+	data = readl(drv_data->reg_base + drv_data->reg_offsets.data);
+	drv_data->msg->buf[drv_data->byte_posn++] = data;
+
+	if (drv_data->msg->flags & I2C_M_RECV_LEN) {
+		if (!data || data > I2C_SMBUS_BLOCK_MAX) {
+			/*
+			 * FIXME
+			 * Abort and report error. Needs to be
+			 * verified/tested on real hardware.
+			 */
+			drv_data->rc = -EPROTO;
+			mv64xxx_i2c_do_send_stop(drv_data);
+			return;
+		}
+		drv_data->msg->flags &= ~I2C_M_RECV_LEN;
+		drv_data->bytes_left = data;
+		drv_data->msg->len = data + 1;
+	}
+
+	writel(drv_data->cntl_bits,
+	       drv_data->reg_base + drv_data->reg_offsets.control);
+}
+
 static void
 mv64xxx_i2c_do_action(struct mv64xxx_i2c_data *drv_data)
 {
@@ -400,23 +451,13 @@ mv64xxx_i2c_do_action(struct mv64xxx_i2c_data *drv_data)
 		break;
 
 	case MV64XXX_I2C_ACTION_RCV_DATA:
-		drv_data->msg->buf[drv_data->byte_posn++] =
-			readl(drv_data->reg_base + drv_data->reg_offsets.data);
-		writel(drv_data->cntl_bits,
-			drv_data->reg_base + drv_data->reg_offsets.control);
+		mv64xxx_i2c_do_read_data(drv_data);
 		break;
 
 	case MV64XXX_I2C_ACTION_RCV_DATA_STOP:
 		drv_data->msg->buf[drv_data->byte_posn++] =
 			readl(drv_data->reg_base + drv_data->reg_offsets.data);
-		drv_data->cntl_bits &= ~MV64XXX_I2C_REG_CONTROL_INTEN;
-		writel(drv_data->cntl_bits | MV64XXX_I2C_REG_CONTROL_STOP,
-			drv_data->reg_base + drv_data->reg_offsets.control);
-		drv_data->block = 0;
-		if (drv_data->errata_delay)
-			udelay(5);
-
-		wake_up(&drv_data->waitq);
+		mv64xxx_i2c_do_send_stop(drv_data);
 		break;
 
 	case MV64XXX_I2C_ACTION_INVALID:
@@ -680,6 +721,10 @@ mv64xxx_i2c_can_offload(struct mv64xxx_i2c_data *drv_data)
 	if (!drv_data->offload_enabled)
 		return false;
 
+	/* Offload not supported for block data transfers */
+	if (msgs[0].flags & I2C_M_RECV_LEN)
+		return false;
+
 	/*
 	 * We can offload a transaction consisting of a single
 	 * message, as long as the message has a length between 1 and
@@ -697,6 +742,7 @@ mv64xxx_i2c_can_offload(struct mv64xxx_i2c_data *drv_data)
 	    mv64xxx_i2c_valid_offload_sz(msgs) &&
 	    mv64xxx_i2c_valid_offload_sz(msgs + 1) &&
 	    !(msgs[0].flags & I2C_M_RD) &&
+	    !(msgs[1].flags & I2C_M_RECV_LEN) &&
 	    msgs[1].flags & I2C_M_RD)
 		return true;
 
@@ -713,7 +759,8 @@ mv64xxx_i2c_can_offload(struct mv64xxx_i2c_data *drv_data)
 static u32
 mv64xxx_i2c_functionality(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_10BIT_ADDR | I2C_FUNC_SMBUS_EMUL;
+	return I2C_FUNC_I2C | I2C_FUNC_10BIT_ADDR |
+		I2C_FUNC_SMBUS_READ_BLOCK_DATA | I2C_FUNC_SMBUS_EMUL;
 }
 
 static int
-- 
2.25.1