[PATCH] media: i2c: ov5645: Add virtual_channel module parameter

[Lad Prabhakar]

OV5645 can operate in virtual channel 0-3 in CSI2 interfaces, this patch
adds support for module parameter virtual_channel to select the required
channel. By default OV5645 operates in virtual channel 0.

Signed-off-by: Lad Prabhakar <prabhakar.mahadev-lad.rj@xxxxxxxxxxxxxx>
---
 drivers/media/i2c/ov5645.c | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/drivers/media/i2c/ov5645.c b/drivers/media/i2c/ov5645.c
index a6c17d15d754..0a0671164623 100644
--- a/drivers/media/i2c/ov5645.c
+++ b/drivers/media/i2c/ov5645.c
@@ -54,6 +54,7 @@
 #define OV5645_TIMING_TC_REG21		0x3821
 #define		OV5645_SENSOR_MIRROR		BIT(1)
 #define OV5645_MIPI_CTRL00		0x4800
+#define OV5645_REG_DEBUG_MODE		0x4814
 #define OV5645_PRE_ISP_TEST_SETTING_1	0x503d
 #define		OV5645_TEST_PATTERN_MASK	0x3
 #define		OV5645_SET_TEST_PATTERN(x)	((x) & OV5645_TEST_PATTERN_MASK)
@@ -61,6 +62,11 @@
 #define OV5645_SDE_SAT_U		0x5583
 #define OV5645_SDE_SAT_V		0x5584
 
+static u8 virtual_channel;
+module_param(virtual_channel, byte, 0644);
+MODULE_PARM_DESC(virtual_channel,
+		 "MIPI CSI-2 virtual channel (0..3), default 0");
+
 /* regulator supplies */
 static const char * const ov5645_supply_name[] = {
 	"vdddo", /* Digital I/O (1.8V) supply */
@@ -983,12 +989,34 @@ static int ov5645_get_selection(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static int ov5645_set_virtual_channel(struct ov5645 *ov5645)
+{
+	u8 temp, channel = virtual_channel;
+	int ret;
+
+	if (channel > 3)
+		return -EINVAL;
+
+	ret = ov5645_read_reg(ov5645, OV5645_REG_DEBUG_MODE, &temp);
+	if (ret)
+		return ret;
+
+	temp &= ~(3 << 6);
+	temp |= (channel << 6);
+
+	return ov5645_write_reg(ov5645, OV5645_REG_DEBUG_MODE, temp);
+}
+
 static int ov5645_s_stream(struct v4l2_subdev *subdev, int enable)
 {
 	struct ov5645 *ov5645 = to_ov5645(subdev);
 	int ret;
 
 	if (enable) {
+		ret = ov5645_set_virtual_channel(ov5645);
+		if (ret < 0)
+			return ret;
+
 		ret = ov5645_set_register_array(ov5645,
 					ov5645->current_mode->data,
 					ov5645->current_mode->data_size);
-- 
2.20.1