From 2c51b8e533a8b43bde18072c9dbbd0fc5084bbe7 Mon Sep 17 00:00:00 2001 From: Dave Stevenson 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 --- .../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)