[PATCH v3 28/31] media: synopsys: csi2: Add customize get_frame_desc() callback

[Frank Li <Frank.Li@nxp.com>]

Add customize get_frame_desc() callback to work with existed isi drivers.

Signed-off-by: Frank Li <Frank.Li@nxp.com>
---
 drivers/media/platform/synopsys/mipi-csi2.c | 73 +++++++++++++++++++++++++++++
 1 file changed, 73 insertions(+)

diff --git a/drivers/media/platform/synopsys/mipi-csi2.c b/drivers/media/platform/synopsys/mipi-csi2.c
index 5ed65e89d4d9a75b245ebc6f28df989dcbd00b7b..695159f463f872ee0775c271b8e1c4e1d289de20 100644
--- a/drivers/media/platform/synopsys/mipi-csi2.c
+++ b/drivers/media/platform/synopsys/mipi-csi2.c
@@ -761,6 +761,78 @@ static int csi2_set_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static int dw_csi2_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				  struct v4l2_mbus_frame_desc *fd)
+{
+	struct dw_mipi_csi2_dev *csi2 = sd_to_dev(sd);
+	struct v4l2_mbus_frame_desc source_fd;
+	struct v4l2_subdev_route *route;
+	struct v4l2_subdev_state *state;
+	int data_type;
+	int ret;
+
+	if (!(csi2->pad[pad].flags & MEDIA_PAD_FL_SOURCE))
+		return -EINVAL;
+
+	memset(fd, 0, sizeof(*fd));
+
+	data_type = media_bus_fmt_to_csi2_dt(csi2->format_mbus.code);
+
+	ret = v4l2_subdev_call(csi2->src_sd, pad, get_frame_desc,
+			       csi2->remote_pad, &source_fd);
+	if (ret < 0) {
+		dev_info(csi2->dev,
+			 "Remote sub-device on pad %d should implement .get_frame_desc! Forcing VC = 0 and DT = %x\n",
+			 pad, data_type);
+		fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
+		fd->num_entries = 1;
+		fd->entry[0].pixelcode = csi2->format_mbus.code;
+		fd->entry[0].bus.csi2.vc = 0;
+		fd->entry[0].bus.csi2.dt = data_type;
+
+		return 0;
+	}
+
+	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	for_each_active_route(&state->routing, route) {
+		struct v4l2_mbus_frame_desc_entry *entry = NULL;
+		unsigned int i;
+
+		if (route->source_pad != pad)
+			continue;
+
+		for (i = 0; i < source_fd.num_entries; ++i) {
+			if (source_fd.entry[i].stream == route->sink_stream) {
+				entry = &source_fd.entry[i];
+				break;
+			}
+		}
+
+		if (!entry) {
+			dev_err(csi2->dev,
+				"Failed to find stream from source frames desc\n");
+			ret = -EPIPE;
+			goto out_unlock;
+		}
+
+		fd->entry[fd->num_entries].stream = route->source_stream;
+		fd->entry[fd->num_entries].flags = entry->flags;
+		fd->entry[fd->num_entries].length = entry->length;
+		fd->entry[fd->num_entries].pixelcode = entry->pixelcode;
+		fd->entry[fd->num_entries].bus.csi2.vc = entry->bus.csi2.vc;
+		fd->entry[fd->num_entries].bus.csi2.dt = entry->bus.csi2.dt;
+
+		fd->num_entries++;
+	}
+
+out_unlock:
+	v4l2_subdev_unlock_state(state);
+	return ret;
+}
+
 /* --------------- CORE OPS --------------- */
 
 static int csi2_log_status(struct v4l2_subdev *sd)
@@ -799,6 +871,7 @@ static const struct media_entity_operations csi2_entity_ops = {
 static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
 	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = csi2_set_fmt,
+	.get_frame_desc = dw_csi2_get_frame_desc,
 	.enable_streams = dw_csi2_enable_streams,
 	.disable_streams = dw_csi2_disable_streams,
 };

-- 
2.34.1