openwrt/target/linux/brcm2708/patches-4.19/950-0729-media-bcm2835-unicam-Rework-to-not-cache-the-list-of.patch

From 2c51b8e533a8b43bde18072c9dbbd0fc5084bbe7 Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dave.stevenson@raspberrypi.org>
Date: Wed, 2 Oct 2019 17:40:38 +0100
Subject: [PATCH] media: bcm2835-unicam: Rework to not cache the list
 of active fmts

Some sensors will change Bayer order based on H & V flips,
therefore collecting the list of formats at async_bound has
problems.

Enumerate the formats from the sensor every time.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.org>
---
 .../media/platform/bcm2835/bcm2835-unicam.c   | 246 ++++++++++--------
 1 file changed, 136 insertions(+), 110 deletions(-)

--- a/drivers/media/platform/bcm2835/bcm2835-unicam.c
+++ b/drivers/media/platform/bcm2835/bcm2835-unicam.c
@@ -363,8 +363,6 @@ struct unicam_device {
 	/* Used to store current mbus frame format */
 	struct v4l2_mbus_framefmt m_fmt;
 
-	struct unicam_fmt active_fmts[MAX_POSSIBLE_PIX_FMTS];
-	int num_active_fmt;
 	unsigned int virtual_channel;
 	enum v4l2_mbus_type bus_type;
 	/*
@@ -455,48 +453,30 @@ static int find_mbus_depth_by_code(u32 c
 	return 0;
 }
 
-static const struct unicam_fmt *find_format_by_code(struct unicam_device *dev,
-						    u32 code)
+static const struct unicam_fmt *find_format_by_code(u32 code)
 {
-	const struct unicam_fmt *fmt;
 	unsigned int k;
 
-	for (k = 0; k < dev->num_active_fmt; k++) {
-		fmt = &dev->active_fmts[k];
-		if (fmt->code == code)
-			return fmt;
+	for (k = 0; k < ARRAY_SIZE(formats); k++) {
+		if (formats[k].code == code)
+			return &formats[k];
 	}
 
 	return NULL;
 }
 
-static const struct unicam_fmt *find_format_by_pix(struct unicam_device *dev,
-						   u32 pixelformat)
+static const struct unicam_fmt *find_format_by_pix(u32 pixelformat)
 {
-	const struct unicam_fmt *fmt;
 	unsigned int k;
 
-	for (k = 0; k < dev->num_active_fmt; k++) {
-		fmt = &dev->active_fmts[k];
-		if (fmt->fourcc == pixelformat)
-			return fmt;
+	for (k = 0; k < ARRAY_SIZE(formats); k++) {
+		if (formats[k].fourcc == pixelformat)
+			return &formats[k];
 	}
 
 	return NULL;
 }
 
-static void dump_active_formats(struct unicam_device *dev)
-{
-	int i;
-
-	for (i = 0; i < dev->num_active_fmt; i++) {
-		unicam_dbg(3, dev, "active_fmt[%d] (%p) is code %04x, fourcc " V4L2_FOURCC_CONV ", depth %d\n",
-			   i, &dev->active_fmts[i], dev->active_fmts[i].code,
-			   V4L2_FOURCC_CONV_ARGS(dev->active_fmts[i].fourcc),
-			   dev->active_fmts[i].depth);
-	}
-}
-
 static inline unsigned int bytes_per_line(u32 width,
 					  const struct unicam_fmt *fmt)
 {
@@ -726,14 +706,40 @@ static int unicam_enum_fmt_vid_cap(struc
 				   struct v4l2_fmtdesc *f)
 {
 	struct unicam_device *dev = video_drvdata(file);
+	struct v4l2_subdev_mbus_code_enum mbus_code;
 	const struct unicam_fmt *fmt = NULL;
+	int index = 0;
+	int ret = 0;
+	int i;
 
-	if (f->index >= dev->num_active_fmt)
-		return -EINVAL;
+	/* Loop whilst the sensor driver says it has more formats, but add a
+	 * failsafe against a dodgy driver at 128 (more than any sensor will
+	 * ever sensibly advertise)
+	 */
+	for (i = 0; !ret && i < 128 ; i++) {
+		memset(&mbus_code, 0, sizeof(mbus_code));
+		mbus_code.index = i;
 
-	fmt = &dev->active_fmts[f->index];
+		ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code,
+				       NULL, &mbus_code);
+		if (ret < 0) {
+			unicam_dbg(2, dev,
+				   "subdev->enum_mbus_code idx %d returned %d - index invalid\n",
+				   i, ret);
+			return -EINVAL;
+		}
 
-	f->pixelformat = fmt->fourcc;
+		fmt = find_format_by_code(mbus_code.code);
+		if (fmt) {
+			if (fmt->fourcc) {
+				if (index == f->index) {
+					f->pixelformat = fmt->fourcc;
+					break;
+				}
+				index++;
+			}
+		}
+	}
 
 	return 0;
 }
@@ -748,6 +754,39 @@ static int unicam_g_fmt_vid_cap(struct f
 	return 0;
 }
 
+static
+const struct unicam_fmt *get_first_supported_format(struct unicam_device *dev)
+{
+	struct v4l2_subdev_mbus_code_enum mbus_code;
+	const struct unicam_fmt *fmt = NULL;
+	int ret;
+	int j;
+
+	for (j = 0; ret != -EINVAL && ret != -ENOIOCTLCMD; ++j) {
+		memset(&mbus_code, 0, sizeof(mbus_code));
+		mbus_code.index = j;
+		ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL,
+				       &mbus_code);
+		if (ret < 0) {
+			unicam_dbg(2, dev,
+				   "subdev->enum_mbus_code idx %d returned %d - continue\n",
+				   j, ret);
+			continue;
+		}
+
+		unicam_dbg(2, dev, "subdev %s: code: %04x idx: %d\n",
+			   dev->sensor->name, mbus_code.code, j);
+
+		fmt = find_format_by_code(mbus_code.code);
+		unicam_dbg(2, dev, "fmt %04x returned as %p, V4L2 FOURCC %04x, csi_dt %02X\n",
+			   mbus_code.code, fmt, fmt ? fmt->fourcc : 0,
+			   fmt ? fmt->csi_dt : 0);
+		if (fmt)
+			return fmt;
+	}
+
+	return NULL;
+}
 static int unicam_try_fmt_vid_cap(struct file *file, void *priv,
 				  struct v4l2_format *f)
 {
@@ -759,13 +798,15 @@ static int unicam_try_fmt_vid_cap(struct
 	struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
 	int ret;
 
-	fmt = find_format_by_pix(dev, f->fmt.pix.pixelformat);
+	fmt = find_format_by_pix(f->fmt.pix.pixelformat);
 	if (!fmt) {
-		unicam_dbg(3, dev, "Fourcc format (0x%08x) not found. Use default of %08X\n",
-			   f->fmt.pix.pixelformat, dev->active_fmts[0].fourcc);
+		/* Pixel format not supported by unicam. Choose the first
+		 * supported format, and let the sensor choose something else.
+		 */
+		unicam_dbg(3, dev, "Fourcc format (0x%08x) not found. Use first format.\n",
+			   f->fmt.pix.pixelformat);
 
-		/* Just get the first one enumerated */
-		fmt = &dev->active_fmts[0];
+		fmt = &formats[0];
 		f->fmt.pix.pixelformat = fmt->fourcc;
 	}
 
@@ -785,6 +826,40 @@ static int unicam_try_fmt_vid_cap(struct
 		unicam_info(dev, "Sensor trying to send interlaced video - results may be unpredictable\n");
 
 	v4l2_fill_pix_format(&f->fmt.pix, &sd_fmt.format);
+	if (mbus_fmt->code != fmt->code) {
+		/* Sensor has returned an alternate format */
+		fmt = find_format_by_code(mbus_fmt->code);
+		if (!fmt) {
+			/* The alternate format is one unicam can't support.
+			 * Find the first format that is supported by both, and
+			 * then set that.
+			 */
+			fmt = get_first_supported_format(dev);
+			mbus_fmt->code = fmt->code;
+
+			ret = v4l2_subdev_call(dev->sensor, pad, set_fmt,
+					       dev->sensor_config, &sd_fmt);
+			if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV)
+				return ret;
+
+			if (mbus_fmt->field != V4L2_FIELD_NONE)
+				unicam_info(dev, "Sensor trying to send interlaced video - results may be unpredictable\n");
+
+			v4l2_fill_pix_format(&f->fmt.pix, &sd_fmt.format);
+
+			if (mbus_fmt->code != fmt->code) {
+				/* We've set a format that the sensor reports
+				 * as being supported, but it refuses to set it.
+				 * Not much else we can do.
+				 * Assume that the sensor driver may accept the
+				 * format when it is set (rather than tried).
+				 */
+				unicam_err(dev, "Sensor won't accept default format, and Unicam can't support sensor default\n");
+			}
+		}
+
+		f->fmt.pix.pixelformat = fmt->fourcc;
+	}
 
 	return unicam_calc_format_size_bpl(dev, fmt, f);
 }
@@ -805,10 +880,18 @@ static int unicam_s_fmt_vid_cap(struct f
 	if (ret < 0)
 		return ret;
 
-	fmt = find_format_by_pix(dev, f->fmt.pix.pixelformat);
+	fmt = find_format_by_pix(f->fmt.pix.pixelformat);
 	if (!fmt) {
-		/* Unknown pixel format - adopt a default */
-		fmt = &dev->active_fmts[0];
+		/* Unknown pixel format - adopt a default.
+		 * This shouldn't happen as try_fmt should have resolved any
+		 * issues first.
+		 */
+		fmt = get_first_supported_format(dev);
+		if (!fmt)
+			/* It shouldn't be possible to get here with no
+			 * supported formats
+			 */
+			return -EINVAL;
 		f->fmt.pix.pixelformat = fmt->fourcc;
 		return -EINVAL;
 	}
@@ -944,6 +1027,7 @@ static void unicam_set_packing_config(st
 			unpack = UNICAM_PUM_NONE;
 			break;
 		}
+
 		switch (v4l2_depth) {
 		case 8:
 			pack = UNICAM_PPM_PACK8;
@@ -1439,7 +1523,7 @@ static int unicam_enum_framesizes(struct
 	int ret;
 
 	/* check for valid format */
-	fmt = find_format_by_pix(dev, fsize->pixel_format);
+	fmt = find_format_by_pix(fsize->pixel_format);
 	if (!fmt) {
 		unicam_dbg(3, dev, "Invalid pixel code: %x\n",
 			   fsize->pixel_format);
@@ -1478,7 +1562,7 @@ static int unicam_enum_frameintervals(st
 	};
 	int ret;
 
-	fmt = find_format_by_pix(dev, fival->pixel_format);
+	fmt = find_format_by_pix(fival->pixel_format);
 	if (!fmt)
 		return -EINVAL;
 
@@ -1742,27 +1826,6 @@ static const struct v4l2_ioctl_ops unica
 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
 };
 
-/*
- * Adds an entry to the active_fmts array
- * Returns non-zero if attempting to write off the end of the array.
- */
-static int unicam_add_active_format(struct unicam_device *unicam,
-				    const struct unicam_fmt *fmt)
-{
-	//Ensure we don't run off the end of the array.
-	if (unicam->num_active_fmt >= MAX_POSSIBLE_PIX_FMTS)
-		return 1;
-
-	unicam->active_fmts[unicam->num_active_fmt] = *fmt;
-	unicam_dbg(2, unicam,
-		   "matched fourcc: " V4L2_FOURCC_CONV ": code: %04x idx: %d\n",
-		   V4L2_FOURCC_CONV_ARGS(fmt->fourcc),
-		   fmt->code, unicam->num_active_fmt);
-	unicam->num_active_fmt++;
-
-	return 0;
-}
-
 static int
 unicam_async_bound(struct v4l2_async_notifier *notifier,
 		   struct v4l2_subdev *subdev,
@@ -1770,9 +1833,6 @@ unicam_async_bound(struct v4l2_async_not
 {
 	struct unicam_device *unicam = container_of(notifier->v4l2_dev,
 					       struct unicam_device, v4l2_dev);
-	struct v4l2_subdev_mbus_code_enum mbus_code;
-	int ret = 0;
-	int j;
 
 	if (unicam->sensor) {
 		unicam_info(unicam, "Rejecting subdev %s (Already set!!)",
@@ -1783,47 +1843,6 @@ unicam_async_bound(struct v4l2_async_not
 	unicam->sensor = subdev;
 	unicam_dbg(1, unicam, "Using sensor %s for capture\n", subdev->name);
 
-	/* Enumerate sub device formats and enable all matching local formats */
-	unicam->num_active_fmt = 0;
-	unicam_dbg(2, unicam, "Get supported formats...\n");
-	for (j = 0; ret != -EINVAL && ret != -ENOIOCTLCMD; ++j) {
-		const struct unicam_fmt *fmt = NULL;
-		int k;
-
-		memset(&mbus_code, 0, sizeof(mbus_code));
-		mbus_code.index = j;
-		ret = v4l2_subdev_call(subdev, pad, enum_mbus_code,
-				       NULL, &mbus_code);
-		if (ret < 0) {
-			unicam_dbg(2, unicam,
-				   "subdev->enum_mbus_code idx %d returned %d - continue\n",
-				   j, ret);
-			continue;
-		}
-
-		unicam_dbg(2, unicam, "subdev %s: code: %04x idx: %d\n",
-			   subdev->name, mbus_code.code, j);
-
-		for (k = 0; k < ARRAY_SIZE(formats); k++) {
-			if (mbus_code.code == formats[k].code) {
-				fmt = &formats[k];
-				break;
-			}
-		}
-		unicam_dbg(2, unicam, "fmt %04x returned as %p, V4L2 FOURCC %04x, csi_dt %02X\n",
-			   mbus_code.code, fmt, fmt ? fmt->fourcc : 0,
-			   fmt ? fmt->csi_dt : 0);
-		if (fmt) {
-			if (unicam_add_active_format(unicam, fmt)) {
-				unicam_dbg(1, unicam, "Active fmt list truncated\n");
-				break;
-			}
-		}
-	}
-	unicam_dbg(2, unicam,
-		   "Done all formats\n");
-	dump_active_formats(unicam);
-
 	return 0;
 }
 
@@ -1849,10 +1868,17 @@ static int unicam_probe_complete(struct
 		return ret;
 	}
 
-	fmt = find_format_by_code(unicam, mbus_fmt.code);
+	fmt = find_format_by_code(mbus_fmt.code);
 	if (!fmt) {
-		/* Default image format not valid. Choose first active fmt. */
-		fmt = &unicam->active_fmts[0];
+		/* Find the first format that the sensor and unicam both
+		 * support
+		 */
+		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);
 		if (ret)