From 12dbdce91f1c512d0df454915960b40a4f8e778a Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dave.stevenson@raspberrypi.com>
Date: Tue, 7 Jan 2025 10:23:34 +0000
Subject: [PATCH] media: rp1-cfe: Fix up link validation for CFE CFG input

After commit 5fd3e2412ade ("media: v4l2-subdev: Support hybrid links
in v4l2_subdev_link_validate()") link_validate is called on V4L2
OUTPUT devices such as the CFE cfg buffers input.
The CFE link_validate function was assuming it was always the
sink of a link, which goes wrong on that port and does an invalid
dereference.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
---
 .../media/platform/raspberrypi/rp1_cfe/cfe.c  | 157 +++++++++++-------
 1 file changed, 101 insertions(+), 56 deletions(-)

--- a/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
+++ b/drivers/media/platform/raspberrypi/rp1_cfe/cfe.c
@@ -1658,36 +1658,14 @@ static const struct v4l2_file_operations
 	.mmap = vb2_fop_mmap,
 };
 
-static int cfe_video_link_validate(struct media_link *link)
+static int cfe_video_link_validate(struct cfe_node *node,
+				   struct v4l2_subdev *remote_sd,
+				   struct v4l2_mbus_framefmt *remote_fmt)
 {
-	struct video_device *vd = container_of(link->sink->entity,
-					       struct video_device, entity);
-	struct cfe_node *node = container_of(vd, struct cfe_node, video_dev);
 	struct cfe_device *cfe = node->cfe;
-	struct v4l2_mbus_framefmt *source_fmt;
-	struct v4l2_subdev_state *state;
-	struct v4l2_subdev *source_sd;
-	int ret = 0;
-
-	cfe_dbg("%s: [%s] link \"%s\":%u -> \"%s\":%u\n", __func__,
-		node_desc[node->id].name,
-		link->source->entity->name, link->source->index,
-		link->sink->entity->name, link->sink->index);
 
-	if (!media_entity_remote_source_pad_unique(link->sink->entity)) {
-		cfe_err("video node %s pad not connected\n", vd->name);
-		return -ENOTCONN;
-	}
-
-	source_sd = media_entity_to_v4l2_subdev(link->source->entity);
-
-	state = v4l2_subdev_lock_and_get_active_state(source_sd);
-
-	source_fmt = v4l2_subdev_state_get_format(state,
-						link->source->index);
-	if (!source_fmt) {
-		ret = -EINVAL;
-		goto out;
+	if (!remote_fmt) {
+		return -EINVAL;
 	}
 
 	if (is_image_output_node(node)) {
@@ -1695,18 +1673,17 @@ static int cfe_video_link_validate(struc
 		const struct cfe_fmt *fmt = NULL;
 		unsigned int i;
 
-		if (source_fmt->width != pix_fmt->width ||
-		    source_fmt->height != pix_fmt->height) {
+		if (remote_fmt->width != pix_fmt->width ||
+		    remote_fmt->height != pix_fmt->height) {
 			cfe_err("Wrong width or height %ux%u (remote pad set to %ux%u)\n",
 				pix_fmt->width, pix_fmt->height,
-				source_fmt->width,
-				source_fmt->height);
-			ret = -EINVAL;
-			goto out;
+				remote_fmt->width,
+				remote_fmt->height);
+			return -EINVAL;
 		}
 
 		for (i = 0; i < ARRAY_SIZE(formats); i++) {
-			if (formats[i].code == source_fmt->code &&
+			if (formats[i].code == remote_fmt->code &&
 			    formats[i].fourcc == pix_fmt->pixelformat) {
 				fmt = &formats[i];
 				break;
@@ -1714,45 +1691,110 @@ static int cfe_video_link_validate(struc
 		}
 		if (!fmt) {
 			cfe_err("Format mismatch!\n");
-			ret = -EINVAL;
-			goto out;
+			return -EINVAL;
 		}
 	} else if (is_csi2_node(node) && is_meta_output_node(node)) {
 		struct v4l2_meta_format *meta_fmt = &node->meta_fmt.fmt.meta;
 		const struct cfe_fmt *fmt;
-		u32 source_size;
+		u32 remote_size;
 
-		fmt = find_format_by_code(source_fmt->code);
+		fmt = find_format_by_code(remote_fmt->code);
 		if (!fmt || fmt->fourcc != meta_fmt->dataformat) {
 			cfe_err("Metadata format mismatch!\n");
-			ret = -EINVAL;
-			goto out;
+			return -EINVAL;
 		}
 
-		source_size = DIV_ROUND_UP(source_fmt->width * source_fmt->height * fmt->depth, 8);
+		remote_size = DIV_ROUND_UP(remote_fmt->width * remote_fmt->height * fmt->depth, 8);
 
-		if (source_fmt->code != MEDIA_BUS_FMT_SENSOR_DATA) {
+		if (remote_fmt->code != MEDIA_BUS_FMT_SENSOR_DATA) {
 			cfe_err("Bad metadata mbus format\n");
-			ret = -EINVAL;
-			goto out;
+			return -EINVAL;
 		}
 
-		if (source_size > meta_fmt->buffersize) {
+		if (remote_size > meta_fmt->buffersize) {
 			cfe_err("Metadata buffer too small: %u < %u\n",
-				meta_fmt->buffersize, source_size);
-			ret = -EINVAL;
-			goto out;
+				meta_fmt->buffersize, remote_size);
+			return -EINVAL;
 		}
 	}
 
-out:
+	return 0;
+}
+
+static int cfe_video_link_validate_output(struct media_link *link)
+{
+	struct video_device *vd = container_of(link->source->entity,
+					       struct video_device, entity);
+	struct cfe_node *node = container_of(vd, struct cfe_node, video_dev);
+	struct cfe_device *cfe = node->cfe;
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_subdev_state *state;
+	struct v4l2_subdev *sink_sd;
+	int ret;
+
+	cfe_dbg("%s: [%s] link \"%s\":%u -> \"%s\":%u\n", __func__,
+		node_desc[node->id].name,
+		link->source->entity->name, link->source->index,
+		link->sink->entity->name, link->sink->index);
+
+	if (!media_entity_remote_source_pad_unique(link->source->entity)) {
+		cfe_err("video node %s pad not connected\n", vd->name);
+		return -ENOTCONN;
+	}
+
+	sink_sd = media_entity_to_v4l2_subdev(link->sink->entity);
+
+	state = v4l2_subdev_lock_and_get_active_state(sink_sd);
+
+	sink_fmt = v4l2_subdev_state_get_format(state, link->sink->index);
+
+	ret = cfe_video_link_validate(node, sink_sd, sink_fmt);
+
 	v4l2_subdev_unlock_state(state);
 
 	return ret;
 }
 
-static const struct media_entity_operations cfe_media_entity_ops = {
-	.link_validate = cfe_video_link_validate,
+static int cfe_video_link_validate_capture(struct media_link *link)
+{
+	struct video_device *vd = container_of(link->sink->entity,
+					       struct video_device, entity);
+	struct cfe_node *node = container_of(vd, struct cfe_node, video_dev);
+	struct cfe_device *cfe = node->cfe;
+	struct v4l2_mbus_framefmt *source_fmt;
+	struct v4l2_subdev_state *state;
+	struct v4l2_subdev *source_sd;
+	int ret;
+
+	cfe_dbg("%s: [%s] link \"%s\":%u -> \"%s\":%u\n", __func__,
+		node_desc[node->id].name,
+		link->source->entity->name, link->source->index,
+		link->sink->entity->name, link->sink->index);
+
+	if (!media_entity_remote_source_pad_unique(link->sink->entity)) {
+		cfe_err("video node %s pad not connected\n", vd->name);
+		return -ENOTCONN;
+	}
+
+	source_sd = media_entity_to_v4l2_subdev(link->source->entity);
+
+	state = v4l2_subdev_lock_and_get_active_state(source_sd);
+
+	source_fmt = v4l2_subdev_state_get_format(state, link->source->index);
+
+	ret = cfe_video_link_validate(node, source_sd, source_fmt);
+
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static const struct media_entity_operations cfe_media_entity_ops_output = {
+	.link_validate = cfe_video_link_validate_output,
+};
+
+static const struct media_entity_operations cfe_media_entity_ops_capture = {
+	.link_validate = cfe_video_link_validate_capture,
 };
 
 static int cfe_video_link_notify(struct media_link *link, u32 flags,
@@ -1917,12 +1959,15 @@ static int cfe_register_node(struct cfe_
 	vdev->release = cfe_node_release;
 	vdev->fops = &cfe_fops;
 	vdev->ioctl_ops = &cfe_ioctl_ops;
-	vdev->entity.ops = &cfe_media_entity_ops;
 	vdev->v4l2_dev = &cfe->v4l2_dev;
-	vdev->vfl_dir = (node_supports_image_output(node) ||
-			 node_supports_meta_output(node)) ?
-				VFL_DIR_RX :
-				VFL_DIR_TX;
+	if ((node_supports_image_output(node) ||
+	     node_supports_meta_output(node))) {
+		vdev->entity.ops = &cfe_media_entity_ops_capture;
+		vdev->vfl_dir = VFL_DIR_RX;
+	} else {
+		vdev->entity.ops = &cfe_media_entity_ops_output;
+		vdev->vfl_dir = VFL_DIR_TX;
+	}
 	vdev->queue = q;
 	vdev->lock = &node->lock;
 	vdev->device_caps = node_desc[id].caps;
