openwrt/target/linux/bcm27xx/patches-5.15/950-0542-media-bcm2835-unicam-Parse-pad-numbers-correctly.patch

From 38c605af2dce528db71bd349ad9ecd41389a52b1 Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dave.stevenson@raspberrypi.com>
Date: Wed, 23 Sep 2020 15:16:18 +0100
Subject: [PATCH] media/bcm2835-unicam: Parse pad numbers correctly

The driver was making big assumptions about the source device
using pad 0 and 1, which doesn't follow for more complex
devices where Unicam's source device may be a sink device for
something else.

Read the pad numbers through media controller, and reference
them appropriately.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
---
 .../media/platform/bcm2835/bcm2835-unicam.c   | 89 ++++++++++++-------
 1 file changed, 58 insertions(+), 31 deletions(-)

--- a/drivers/media/platform/bcm2835/bcm2835-unicam.c
+++ b/drivers/media/platform/bcm2835/bcm2835-unicam.c
@@ -383,6 +383,8 @@ struct unicam_node {
 	int open;
 	bool streaming;
 	unsigned int pad_id;
+	/* Source pad id on the sensor for this node */
+	unsigned int src_pad_id;
 	/* Pointer pointing to current v4l2_buffer */
 	struct unicam_buffer *cur_frm;
 	/* Pointer pointing to next v4l2_buffer */
@@ -591,7 +593,7 @@ static int __subdev_get_format(struct un
 {
 	struct v4l2_subdev_format sd_fmt = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-		.pad = pad_id
+		.pad = dev->node[pad_id].src_pad_id,
 	};
 	int ret;
 
@@ -613,7 +615,7 @@ static int __subdev_set_format(struct un
 {
 	struct v4l2_subdev_format sd_fmt = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-		.pad = pad_id
+		.pad = dev->node[pad_id].src_pad_id,
 	};
 	int ret;
 
@@ -1981,7 +1983,7 @@ static int unicam_enum_framesizes(struct
 
 	fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 	fse.index = fsize->index;
-	fse.pad = node->pad_id;
+	fse.pad = node->src_pad_id;
 
 	ret = v4l2_subdev_call(dev->sensor, pad, enum_frame_size, NULL, &fse);
 	if (ret)
@@ -2006,6 +2008,7 @@ static int unicam_enum_frameintervals(st
 	const struct unicam_fmt *fmt;
 	struct v4l2_subdev_frame_interval_enum fie = {
 		.index = fival->index,
+		.pad = node->src_pad_id,
 		.width = fival->width,
 		.height = fival->height,
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
@@ -2097,8 +2100,13 @@ static int unicam_enum_dv_timings(struct
 {
 	struct unicam_node *node = video_drvdata(file);
 	struct unicam_device *dev = node->dev;
+	int ret;
+
+	timings->pad = node->src_pad_id;
+	ret = v4l2_subdev_call(dev->sensor, pad, enum_dv_timings, timings);
+	timings->pad = node->pad_id;
 
-	return v4l2_subdev_call(dev->sensor, pad, enum_dv_timings, timings);
+	return ret;
 }
 
 static int unicam_dv_timings_cap(struct file *file, void *priv,
@@ -2106,8 +2114,13 @@ static int unicam_dv_timings_cap(struct
 {
 	struct unicam_node *node = video_drvdata(file);
 	struct unicam_device *dev = node->dev;
+	int ret;
+
+	cap->pad = node->src_pad_id;
+	ret = v4l2_subdev_call(dev->sensor, pad, dv_timings_cap, cap);
+	cap->pad = node->pad_id;
 
-	return v4l2_subdev_call(dev->sensor, pad, dv_timings_cap, cap);
+	return ret;
 }
 
 static int unicam_subscribe_event(struct v4l2_fh *fh,
@@ -2378,14 +2391,12 @@ static int register_node(struct unicam_d
 			 */
 			fmt = get_first_supported_format(unicam);
 
-			if (!fmt)
-				/* No compatible formats */
-				return -EINVAL;
-
-			mbus_fmt.code = fmt->code;
-			ret = __subdev_set_format(unicam, &mbus_fmt, pad_id);
-			if (ret)
-				return -EINVAL;
+			if (fmt) {
+				mbus_fmt.code = fmt->code;
+				ret = __subdev_set_format(unicam, &mbus_fmt, pad_id);
+				if (ret)
+					return -EINVAL;
+			}
 		}
 		if (mbus_fmt.field != V4L2_FIELD_NONE) {
 			/* Interlaced not supported - disable it now. */
@@ -2395,7 +2406,8 @@ static int register_node(struct unicam_d
 				return -EINVAL;
 		}
 
-		node->v_fmt.fmt.pix.pixelformat = fmt->fourcc ? fmt->fourcc
+		if (fmt)
+			node->v_fmt.fmt.pix.pixelformat = fmt->fourcc ? fmt->fourcc
 						: fmt->repacked_fourcc;
 	} else {
 		/* Fix this node format as embedded data. */
@@ -2408,7 +2420,8 @@ static int register_node(struct unicam_d
 	node->fmt = fmt;
 
 	/* Read current subdev format */
-	unicam_reset_format(node);
+	if (fmt)
+		unicam_reset_format(node);
 
 	if (v4l2_subdev_has_op(unicam->sensor, video, s_std)) {
 		v4l2_std_id tvnorms;
@@ -2497,6 +2510,7 @@ static int register_node(struct unicam_d
 		unicam_err(unicam, "Unable to allocate dummy buffer.\n");
 		return -ENOMEM;
 	}
+
 	if (pad_id == METADATA_PAD ||
 	    !v4l2_subdev_has_op(unicam->sensor, video, s_std)) {
 		v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_STD);
@@ -2555,7 +2569,8 @@ static int register_node(struct unicam_d
 	node->registered = true;
 
 	if (pad_id != METADATA_PAD || unicam->sensor_embedded_data) {
-		ret = media_create_pad_link(&unicam->sensor->entity, pad_id,
+		ret = media_create_pad_link(&unicam->sensor->entity,
+					    node->src_pad_id,
 					    &node->video_dev.entity, 0,
 					    MEDIA_LNK_FL_ENABLED |
 					    MEDIA_LNK_FL_IMMUTABLE);
@@ -2587,8 +2602,10 @@ static void unregister_nodes(struct unic
 	}
 }
 
-static int unicam_probe_complete(struct unicam_device *unicam)
+static int unicam_async_complete(struct v4l2_async_notifier *notifier)
 {
+	struct unicam_device *unicam = to_unicam_device(notifier->v4l2_dev);
+	unsigned int i, source_pads = 0;
 	int ret;
 
 	unicam->v4l2_dev.notify = unicam_notify;
@@ -2597,7 +2614,20 @@ static int unicam_probe_complete(struct
 	if (!unicam->sensor_state)
 		return -ENOMEM;
 
-	unicam->sensor_embedded_data = (unicam->sensor->entity.num_pads >= 2);
+	for (i = 0; i < unicam->sensor->entity.num_pads; i++) {
+		if (unicam->sensor->entity.pads[i].flags & MEDIA_PAD_FL_SOURCE) {
+			if (source_pads < MAX_NODES) {
+				unicam->node[source_pads].src_pad_id = i;
+				unicam_err(unicam, "source pad %u is index %u\n",
+					   source_pads, i);
+			}
+			source_pads++;
+		}
+	}
+	if (!source_pads) {
+		unicam_err(unicam, "No source pads on sensor.\n");
+		goto unregister;
+	}
 
 	ret = register_node(unicam, &unicam->node[IMAGE_PAD],
 			    V4L2_BUF_TYPE_VIDEO_CAPTURE, IMAGE_PAD);
@@ -2606,11 +2636,15 @@ static int unicam_probe_complete(struct
 		goto unregister;
 	}
 
-	ret = register_node(unicam, &unicam->node[METADATA_PAD],
-			    V4L2_BUF_TYPE_META_CAPTURE, METADATA_PAD);
-	if (ret) {
-		unicam_err(unicam, "Unable to register metadata video device.\n");
-		goto unregister;
+	if (source_pads >= 2) {
+		unicam->sensor_embedded_data = true;
+
+		ret = register_node(unicam, &unicam->node[METADATA_PAD],
+				    V4L2_BUF_TYPE_META_CAPTURE, METADATA_PAD);
+		if (ret) {
+			unicam_err(unicam, "Unable to register metadata video device.\n");
+			goto unregister;
+		}
 	}
 
 	ret = v4l2_device_register_ro_subdev_nodes(&unicam->v4l2_dev);
@@ -2633,13 +2667,6 @@ unregister:
 	return ret;
 }
 
-static int unicam_async_complete(struct v4l2_async_notifier *notifier)
-{
-	struct unicam_device *unicam = to_unicam_device(notifier->v4l2_dev);
-
-	return unicam_probe_complete(unicam);
-}
-
 static const struct v4l2_async_notifier_operations unicam_async_ops = {
 	.bound = unicam_async_bound,
 	.complete = unicam_async_complete,
@@ -2748,7 +2775,7 @@ static int of_unicam_connect_subdevs(str
 	dev->notifier.ops = &unicam_async_ops;
 
 	dev->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
-	dev->asd.match.fwnode = of_fwnode_handle(sensor_node);
+	dev->asd.match.fwnode = fwnode_graph_get_remote_endpoint(of_fwnode_handle(ep_node));
 	ret = v4l2_async_notifier_add_subdev(&dev->notifier, &dev->asd);
 	if (ret) {
 		unicam_err(dev, "Error adding subdevice: %d\n", ret);