ov5640_mipi: add i2c address change feature

Signed-off-by: Troy Kisky <troy.kisky@boundarydevices.com>
Acked-by: Max Krummenacher <max.krummenacher@toradex.com>
(cherry picked from commit 5798f58479291edf32f5a587832acc0176dcf3c1)
(cherry picked from commit 73924ed00eda7cfe2c055b279617bd4ab60b432c)

1 files changed, 47 insertions, 5 deletions

diff --git a/drivers/media/platform/mxc/capture/ov5640_mipi.c b/drivers/media/platform/mxc/capture/ov5640_mipi.c
index 0e46b26ffae0..b31239be08be 100644
--- a/drivers/media/platform/mxc/capture/ov5640_mipi.c
+++ b/drivers/media/platform/mxc/capture/ov5640_mipi.c
@@ -693,8 +693,36 @@ static void ov5640_standby(s32 enable)
 	msleep(100);
 }
 
+static s32 update_device_addr(struct sensor_data *sensor)
+{
+	int ret;
+	u8 buf[4];
+	unsigned reg = 0x3100;
+	unsigned default_addr = 0x3c;
+	struct i2c_msg msg;
+
+	if (sensor->i2c_client->addr == default_addr)
+		return 0;
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xff;
+	buf[2] = sensor->i2c_client->addr << 1;
+	msg.addr = default_addr;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = buf;
+
+
+	ret = i2c_transfer(sensor->i2c_client->adapter, &msg, 1);
+	if (ret < 0)
+		pr_err("%s: ov5642 ret=%d\n", __func__, ret);
+	return ret;
+}
+
 static void ov5640_reset(void)
 {
+	mxc_camera_common_lock();
+
 	/* camera reset */
 	gpio_set_value(rst_gpio, 1);
 
@@ -709,7 +737,9 @@ static void ov5640_reset(void)
 	msleep(1);
 
 	gpio_set_value(rst_gpio, 1);
-	msleep(5);
+	msleep(20);
+	update_device_addr(&ov5640_data);
+	mxc_camera_common_unlock();
 
 	gpio_set_value(pwn_gpio, 1);
 }
@@ -778,16 +808,28 @@ static int ov5640_power_on(struct device *dev)
 
 static s32 ov5640_write_reg(u16 reg, u8 val)
 {
+	int ret;
 	u8 au8Buf[3] = {0};
 
 	au8Buf[0] = reg >> 8;
 	au8Buf[1] = reg & 0xff;
 	au8Buf[2] = val;
 
-	if (i2c_master_send(ov5640_data.i2c_client, au8Buf, 3) < 0) {
-		pr_err("%s:write reg error:reg=%x,val=%x\n",
-			__func__, reg, val);
-		return -1;
+	if ((reg == 0x3008) && (val & 0x80)) {
+		mxc_camera_common_lock();
+
+		ret = i2c_master_send(ov5640_data.i2c_client, au8Buf, 3);
+		update_device_addr(&ov5640_data);
+
+		mxc_camera_common_unlock();
+	} else {
+		ret = i2c_master_send(ov5640_data.i2c_client, au8Buf, 3);
+	}
+
+	if (ret < 0) {
+		pr_err("%s:write reg error:reg=%x,val=%x ret=%d\n",
+			__func__, reg, val, ret);
+		return ret;
 	}
 	pr_debug("reg=%x,val=%x\n", reg, val);
 	return 0;