media: atomisp: simplify ov2680 array write logic

Instead of trying to send multiple bytes at the same time,
just go one by one, like the upstream driver does.

Signed-off-by: Mauro Carvalho Chehab <mchehab+huawei@kernel.org>

1 files changed, 13 insertions, 115 deletions

diff --git a/drivers/staging/media/atomisp/i2c/atomisp-ov2680.c b/drivers/staging/media/atomisp/i2c/atomisp-ov2680.c
index 6be359a4dc13..1cb55acf19e1 100644
--- a/drivers/staging/media/atomisp/i2c/atomisp-ov2680.c
+++ b/drivers/staging/media/atomisp/i2c/atomisp-ov2680.c
@@ -78,146 +78,44 @@ static int ov2680_read_reg(struct i2c_client *client,
 	return 0;
 }
 
-static int ov2680_i2c_write(struct i2c_client *client, u16 len, u8 *data)
-{
-	struct i2c_msg msg;
-	const int num_msg = 1;
-	int ret;
-
-	msg.addr = client->addr;
-	msg.flags = 0;
-	msg.len = len;
-	msg.buf = data;
-	ret = i2c_transfer(client->adapter, &msg, 1);
-
-	if (ret < 0)
-		dev_dbg(&client->dev,
-			"%s: i2c write reg=0x%02x, value 0x%02x, error %d\n",
-			__func__, data[0]*256 +data[1], data[2], ret);
-	return ret == num_msg ? 0 : ret;
-}
-
 static int ov2680_write_reg(struct i2c_client *client, unsigned int len,
 			    u16 reg, u16 val)
 {
 	u8 buf[6];
 	int ret;
 
-	if (len > 4)
+	if (len == 2)
+		put_unaligned_be16(val << (8 * (4 - len)), buf + 2);
+	else if (len == 1)
+		buf[2] = val;
+	else
 		return -EINVAL;
 
 	put_unaligned_be16(reg, buf);
-	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
+
 	ret = i2c_master_send(client, buf, len + 2);
 	if (ret != len + 2) {
-		dev_err(&client->dev, "write error: reg=0x%4x: %d\n", reg, ret);
+		dev_err(&client->dev, "write error %d reg 0x%04x, val 0x%02x: buf sent: %*ph\n",
+			ret, reg, val, len + 2, &buf);
 		return -EIO;
 	}
 
 	return 0;
 }
 
-/*
- * ov2680_write_reg_array - Initializes a list of OV2680 registers
- * @client: i2c driver client structure
- * @reglist: list of registers to be written
- *
- * This function initializes a list of registers. When consecutive addresses
- * are found in a row on the list, this function creates a buffer and sends
- * consecutive data in a single i2c_transfer().
- *
- * __ov2680_flush_reg_array, __ov2680_buf_reg_array() and
- * __ov2680_write_reg_is_consecutive() are internal functions to
- * ov2680_write_reg_array_fast() and should be not used anywhere else.
- *
- */
-
-static int __ov2680_flush_reg_array(struct i2c_client *client,
-				    struct ov2680_write_ctrl *ctrl)
-{
-	u16 size;
-	__be16 *data16 = (void *)&ctrl->buffer.addr;
-
-	if (ctrl->index == 0) {
-		dev_dbg(&client->dev,  "%s: *not* flushing reg_array\n",
-		        __func__);
-		return 0;
-	}
-
-	dev_dbg(&client->dev,  "%s: flushing reg_array\n", __func__);
-
-	size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
-	*data16 = cpu_to_be16(ctrl->buffer.addr);
-	ctrl->index = 0;
-
-	return ov2680_i2c_write(client, size, (u8 *)&ctrl->buffer);
-}
-
-static int __ov2680_buf_reg_array(struct i2c_client *client,
-				  struct ov2680_write_ctrl *ctrl,
-				  const struct ov2680_reg *next)
-{
-	int size;
-
-	size = 1;
-	ctrl->buffer.data[ctrl->index] = (u8)next->val;
-
-	/* When first item is added, we need to store its starting address */
-	if (ctrl->index == 0)
-		ctrl->buffer.addr = next->reg;
-
-	ctrl->index += size;
-
-	/*
-	 * Buffer cannot guarantee free space for u32? Better flush it to avoid
-	 * possible lack of memory for next item.
-	 */
-	if (ctrl->index + sizeof(u16) >= OV2680_MAX_WRITE_BUF_SIZE)
-		return __ov2680_flush_reg_array(client, ctrl);
-
-	return 0;
-}
-
-static int __ov2680_write_reg_is_consecutive(struct i2c_client *client,
-	struct ov2680_write_ctrl *ctrl,
-	const struct ov2680_reg *next)
-{
-	if (ctrl->index == 0)
-		return 1;
-
-	return ctrl->buffer.addr + ctrl->index == next->reg;
-}
-
 static int ov2680_write_reg_array(struct i2c_client *client,
 				  const struct ov2680_reg *reglist)
 {
 	const struct ov2680_reg *next = reglist;
-	struct ov2680_write_ctrl ctrl;
-	int err;
+	int ret;
 
-	ctrl.index = 0;
 	for (; next->reg != 0; next++) {
-		/*
-		    * If next address is not consecutive, data needs to be
-		    * flushed before proceed.
-		    */
-		dev_dbg(&client->dev,  "%s: reg=0x%02x set to 0x%02x\n",
-			__func__, next->reg, next->val);
-		if (!__ov2680_write_reg_is_consecutive(client, &ctrl,
-							next)) {
-			err = __ov2680_flush_reg_array(client, &ctrl);
-			if (err)
-				return err;
-		}
-		err = __ov2680_buf_reg_array(client, &ctrl, next);
-		if (err) {
-			dev_err(&client->dev,
-				"%s: write error, aborted\n", __func__);
-			return err;
-		}
+		ret = ov2680_write_reg(client, 1, next->reg, next->val);
+		if (ret)
+			return ret;
 	}
 
-	return __ov2680_flush_reg_array(client, &ctrl);
+	return 0;
 }
 
 static int ov2680_g_focal(struct v4l2_subdev *sd, s32 *val)