--- /dev/null
+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)
projects / openwrt / openwrt.git / blobdiff