[PATCH v2 2/3] media: i2c: ov772x: Add support for BT656 mode

[Lad Prabhakar <prabhakar.mahadev-lad.rj@xxxxxxxxxxxxxx>]

Add support to read the bus-type and enable BT656 mode if needed.

The driver defaults to parallel mode if bus-type is not specified in DT.

Signed-off-by: Lad Prabhakar <prabhakar.mahadev-lad.rj@xxxxxxxxxxxxxx>
Reviewed-by: Biju Das <biju.das.jz@xxxxxxxxxxxxxx>
---
 drivers/media/i2c/ov772x.c | 40 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/drivers/media/i2c/ov772x.c b/drivers/media/i2c/ov772x.c
index 2cc6a678069a..2de9248e3689 100644
--- a/drivers/media/i2c/ov772x.c
+++ b/drivers/media/i2c/ov772x.c
@@ -31,6 +31,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
 #include <media/v4l2-image-sizes.h>
 #include <media/v4l2-subdev.h>
 
@@ -434,6 +435,7 @@ struct ov772x_priv {
 #ifdef CONFIG_MEDIA_CONTROLLER
 	struct media_pad pad;
 #endif
+	struct v4l2_fwnode_endpoint ep;
 };
 
 /*
@@ -574,6 +576,7 @@ static int ov772x_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct ov772x_priv *priv = to_ov772x(sd);
+	unsigned int val;
 	int ret = 0;
 
 	mutex_lock(&priv->lock);
@@ -581,6 +584,22 @@ static int ov772x_s_stream(struct v4l2_subdev *sd, int enable)
 	if (priv->streaming == enable)
 		goto done;
 
+	if (priv->ep.bus_type == V4L2_MBUS_BT656 && enable) {
+		ret = regmap_read(priv->regmap, COM7, &val);
+		if (ret)
+			goto done;
+		val |= ITU656_ON_OFF;
+		ret = regmap_write(priv->regmap, COM7, val);
+	} else if (priv->ep.bus_type == V4L2_MBUS_BT656 && !enable) {
+		ret = regmap_read(priv->regmap, COM7, &val);
+		if (ret)
+			goto done;
+		val &= ~ITU656_ON_OFF;
+		ret = regmap_write(priv->regmap, COM7, val);
+	}
+	if (ret)
+		goto done;
+
 	ret = regmap_update_bits(priv->regmap, COM2, SOFT_SLEEP_MODE,
 				 enable ? 0 : SOFT_SLEEP_MODE);
 	if (ret)
@@ -1354,6 +1373,7 @@ static const struct v4l2_subdev_ops ov772x_subdev_ops = {
 
 static int ov772x_probe(struct i2c_client *client)
 {
+	struct fwnode_handle *endpoint;
 	struct ov772x_priv	*priv;
 	int			ret;
 	static const struct regmap_config ov772x_regmap_config = {
@@ -1415,6 +1435,26 @@ static int ov772x_probe(struct i2c_client *client)
 		goto error_clk_put;
 	}
 
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
+						  NULL);
+	if (!endpoint) {
+		dev_err(&client->dev, "endpoint node not found\n");
+		ret = -EINVAL;
+		goto error_clk_put;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &priv->ep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(&client->dev, "Could not parse endpoint\n");
+		goto error_clk_put;
+	}
+
+	/* fallback to parallel mode */
+	if (priv->ep.bus_type != V4L2_MBUS_PARALLEL &&
+	    priv->ep.bus_type != V4L2_MBUS_BT656)
+		priv->ep.bus_type = V4L2_MBUS_PARALLEL;
+
 	ret = ov772x_video_probe(priv);
 	if (ret < 0)
 		goto error_gpio_put;
-- 
2.17.1