[PATCH 2/3] media: sun6i-csi: implement V4L2_CAP_IO_MC

[adam@xxxxxxxxxxx]

From: Adam Pigg <adam@xxxxxxxxxxx>

modify sun6i_csi_capture_enum_fmt to check the mbus_codes from the
pixel format array for the supplied format

Signed-off-by: Adam Pigg <adam@xxxxxxxxxxx>
---
 .../sunxi/sun6i-csi/sun6i_csi_capture.c       | 36 +++++++++++++++++--
 1 file changed, 33 insertions(+), 3 deletions(-)

diff --git a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi_capture.c b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi_capture.c
index 69578075421c..54beba4d2b2f 100644
--- a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi_capture.c
+++ b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi_capture.c
@@ -666,13 +666,43 @@ static int sun6i_csi_capture_enum_fmt(struct file *file, void *private,
 				      struct v4l2_fmtdesc *fmtdesc)
 {
 	u32 index = fmtdesc->index;
+	unsigned int i;
 
 	if (index >= ARRAY_SIZE(sun6i_csi_capture_formats))
 		return -EINVAL;
 
-	fmtdesc->pixelformat = sun6i_csi_capture_formats[index].pixelformat;
+	for (i = 0; i < ARRAY_SIZE(sun6i_csi_capture_formats); i++) {
+		const struct sun6i_csi_capture_format *format =
+			&sun6i_csi_capture_formats[i];
 
-	return 0;
+		/*
+		 * If a media bus code is specified, only consider formats that
+		 * match it.
+		 */
+		if (fmtdesc->mbus_code) {
+			unsigned int j;
+
+			if (!format->mbus_codes)
+				continue;
+
+			for (j = 0; format->mbus_codes[j]; j++) {
+				if (fmtdesc->mbus_code == format->mbus_codes[j])
+					break;
+			}
+
+			if (!format->mbus_codes[j])
+				continue;
+		}
+
+		if (index == 0) {
+			fmtdesc->pixelformat = format->pixelformat;
+			return 0;
+		}
+
+		index--;
+	}
+
+	return -EINVAL;
 }
 
 static int sun6i_csi_capture_g_fmt(struct file *file, void *private,
@@ -978,7 +1008,7 @@ int sun6i_csi_capture_setup(struct sun6i_csi_device *csi_dev)
 
 	strscpy(video_dev->name, SUN6I_CSI_CAPTURE_NAME,
 		sizeof(video_dev->name));
-	video_dev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
+	video_dev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
 	video_dev->vfl_dir = VFL_DIR_RX;
 	video_dev->release = video_device_release_empty;
 	video_dev->fops = &sun6i_csi_capture_fops;
-- 
2.39.0