The try_fmt was not actually trying out the provided format but merely returning the current format basically like get_fmt. In addition set_fmt should first invoked try_fmt to validate the given format before applying it to the hardware.
To fix all of these the whole get/try/set ioctl functions had to be reworked. When calculating the bytesperline/stride and sizeimage format member we don't need to locally store the current value of bytesperpixel as it can easily get derived dynamically. Signed-off-by: Benoit Parrot <[email protected]> --- drivers/media/platform/am437x/am437x-vpfe.c | 334 +++++++++----------- drivers/media/platform/am437x/am437x-vpfe.h | 2 - 2 files changed, 155 insertions(+), 181 deletions(-) diff --git a/drivers/media/platform/am437x/am437x-vpfe.c b/drivers/media/platform/am437x/am437x-vpfe.c index 4fb9c8ed7e92..9759ed398943 100644 --- a/drivers/media/platform/am437x/am437x-vpfe.c +++ b/drivers/media/platform/am437x/am437x-vpfe.c @@ -143,9 +143,11 @@ static struct vpfe_fmt formats[] = { }, }; -static int -__vpfe_get_format(struct vpfe_device *vpfe, - struct v4l2_format *format, unsigned int *bpp); +static int __subdev_get_format(struct vpfe_device *vpfe, + struct v4l2_mbus_framefmt *fmt); +static int vpfe_calc_format_size(struct vpfe_device *vpfe, + const struct vpfe_fmt *fmt, + struct v4l2_format *f); static const struct vpfe_fmt *find_format_by_code(struct vpfe_device *vpfe, unsigned int code) @@ -177,62 +179,16 @@ static const struct vpfe_fmt *find_format_by_pix(struct vpfe_device *vpfe, return NULL; } -static void -mbus_to_pix(struct vpfe_device *vpfe, - const struct v4l2_mbus_framefmt *mbus, - struct v4l2_pix_format *pix, unsigned int *bpp) +static unsigned int __get_bytesperpixel(struct vpfe_device *vpfe, + const struct vpfe_fmt *fmt) { struct vpfe_subdev_info *sdinfo = vpfe->current_subdev; unsigned int bus_width = sdinfo->vpfe_param.bus_width; - const struct vpfe_fmt *fmt; - - fmt = find_format_by_code(vpfe, mbus->code); - if (WARN_ON(fmt == NULL)) { - pr_err("Invalid mbus code set\n"); - *bpp = 1; - return; - } + u32 bpp; - memset(pix, 0, sizeof(*pix)); - v4l2_fill_pix_format(pix, mbus); - pix->pixelformat = fmt->fourcc; - *bpp = (bus_width == 10) ? fmt->l.bpp : fmt->s.bpp; + bpp = (bus_width == 10) ? fmt->l.bpp : fmt->s.bpp; - /* pitch should be 32 bytes aligned */ - pix->bytesperline = ALIGN(pix->width * *bpp, 32); - pix->sizeimage = pix->bytesperline * pix->height; -} - -static void pix_to_mbus(struct vpfe_device *vpfe, - struct v4l2_pix_format *pix_fmt, - struct v4l2_mbus_framefmt *mbus_fmt) -{ - const struct vpfe_fmt *fmt; - - fmt = find_format_by_pix(vpfe, pix_fmt->pixelformat); - if (!fmt) { - /* default to first entry */ - vpfe_dbg(3, vpfe, "Invalid pixel code: %x, default used instead\n", - pix_fmt->pixelformat); - fmt = vpfe->active_fmt[0]; - } - - memset(mbus_fmt, 0, sizeof(*mbus_fmt)); - v4l2_fill_mbus_format(mbus_fmt, pix_fmt, fmt->code); -} - -static int -cmp_v4l2_format(const struct v4l2_format *lhs, const struct v4l2_format *rhs) -{ - return lhs->type == rhs->type && - lhs->fmt.pix.width == rhs->fmt.pix.width && - lhs->fmt.pix.height == rhs->fmt.pix.height && - lhs->fmt.pix.pixelformat == rhs->fmt.pix.pixelformat && - lhs->fmt.pix.field == rhs->fmt.pix.field && - lhs->fmt.pix.colorspace == rhs->fmt.pix.colorspace && - lhs->fmt.pix.ycbcr_enc == rhs->fmt.pix.ycbcr_enc && - lhs->fmt.pix.quantization == rhs->fmt.pix.quantization && - lhs->fmt.pix.xfer_func == rhs->fmt.pix.xfer_func; + return bpp; } static inline u32 vpfe_reg_read(struct vpfe_ccdc *ccdc, u32 offset) @@ -971,6 +927,7 @@ static int vpfe_get_ccdc_image_format(struct vpfe_device *vpfe, static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe) { enum ccdc_frmfmt frm_fmt = CCDC_FRMFMT_INTERLACED; + u32 bpp; int ret = 0; vpfe_dbg(2, vpfe, "%s:\n", __func__); @@ -985,7 +942,8 @@ static int vpfe_config_ccdc_image_format(struct vpfe_device *vpfe) } /* configure the image window */ - vpfe_ccdc_set_image_window(&vpfe->ccdc, &vpfe->crop, vpfe->bpp); + bpp = __get_bytesperpixel(vpfe, vpfe->fmt); + vpfe_ccdc_set_image_window(&vpfe->ccdc, &vpfe->crop, bpp); switch (vpfe->v_fmt.fmt.pix.field) { case V4L2_FIELD_INTERLACED: @@ -1030,7 +988,7 @@ static int vpfe_config_image_format(struct vpfe_device *vpfe, v4l2_std_id std_id) { const struct vpfe_fmt *fmt; - struct v4l2_pix_format *pix = &vpfe->v_fmt.fmt.pix; + struct v4l2_mbus_framefmt mbus_fmt; int i, ret; for (i = 0; i < ARRAY_SIZE(vpfe_standards); i++) { @@ -1052,34 +1010,29 @@ static int vpfe_config_image_format(struct vpfe_device *vpfe, return -EINVAL; } - vpfe->crop.top = vpfe->crop.left = 0; - vpfe->crop.width = vpfe->std_info.active_pixels; - vpfe->crop.height = vpfe->std_info.active_lines; - pix->width = vpfe->crop.width; - pix->height = vpfe->crop.height; - pix->pixelformat = V4L2_PIX_FMT_YUYV; - - /* first field and frame format based on standard frame format */ - if (vpfe->std_info.frame_format) - pix->field = V4L2_FIELD_INTERLACED; - else - pix->field = V4L2_FIELD_NONE; - - ret = __vpfe_get_format(vpfe, &vpfe->v_fmt, &vpfe->bpp); + ret = __subdev_get_format(vpfe, &mbus_fmt); if (ret) return ret; - fmt = find_format_by_pix(vpfe, pix->pixelformat); + fmt = find_format_by_code(vpfe, mbus_fmt.code); if (!fmt) { - vpfe_dbg(3, vpfe, "Invalid pixel code: %4.4s\n", - (char *)&pix->pixelformat); + vpfe_dbg(3, vpfe, "mbus code format (0x%08x) not found.\n", + mbus_fmt.code); return -EINVAL; } + + /* Save current subdev format */ + v4l2_fill_pix_format(&vpfe->v_fmt.fmt.pix, &mbus_fmt); + vpfe->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + vpfe->v_fmt.fmt.pix.pixelformat = fmt->fourcc; + vpfe_calc_format_size(vpfe, fmt, &vpfe->v_fmt); vpfe->fmt = fmt; /* Update the crop window based on found values */ - vpfe->crop.width = pix->width; - vpfe->crop.height = pix->height; + vpfe->crop.top = 0; + vpfe->crop.left = 0; + vpfe->crop.width = mbus_fmt.width; + vpfe->crop.height = mbus_fmt.height; return vpfe_config_ccdc_image_format(vpfe); } @@ -1359,83 +1312,74 @@ static int vpfe_querycap(struct file *file, void *priv, } /* get the format set at output pad of the adjacent subdev */ -static int __vpfe_get_format(struct vpfe_device *vpfe, - struct v4l2_format *format, unsigned int *bpp) +static int __subdev_get_format(struct vpfe_device *vpfe, + struct v4l2_mbus_framefmt *fmt) { - struct v4l2_mbus_framefmt mbus_fmt; - struct vpfe_subdev_info *sdinfo; - struct v4l2_subdev_format fmt; + struct v4l2_subdev *sd = vpfe->current_subdev->sd; + struct v4l2_subdev_format sd_fmt; + struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; int ret; - sdinfo = vpfe->current_subdev; - if (!sdinfo->sd) - return -EINVAL; + sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; + sd_fmt.pad = 0; - fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; - fmt.pad = 0; - - ret = v4l2_subdev_call(sdinfo->sd, pad, get_fmt, NULL, &fmt); - if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) + ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt); + if (ret) return ret; - if (!ret) { - v4l2_fill_pix_format(&format->fmt.pix, &fmt.format); - mbus_to_pix(vpfe, &fmt.format, &format->fmt.pix, bpp); - } else { - ret = v4l2_device_call_until_err(&vpfe->v4l2_dev, - sdinfo->grp_id, - pad, get_fmt, - NULL, &fmt); - if (ret && ret != -ENOIOCTLCMD && ret != -ENODEV) - return ret; - v4l2_fill_pix_format(&format->fmt.pix, &mbus_fmt); - mbus_to_pix(vpfe, &mbus_fmt, &format->fmt.pix, bpp); - } + *fmt = *mbus_fmt; - format->type = vpfe->v_fmt.type; - - vpfe_dbg(1, vpfe, - "%s: size %dx%d (%4.4s) bytesperline = %d, size = %d, bpp = %d\n", - __func__, format->fmt.pix.width, format->fmt.pix.height, - (char *)&format->fmt.pix.pixelformat, - format->fmt.pix.bytesperline, format->fmt.pix.sizeimage, *bpp); + vpfe_dbg(1, vpfe, "%s: %dx%d code:%04X\n", __func__, + fmt->width, fmt->height, fmt->code); return 0; } /* set the format at output pad of the adjacent subdev */ -static int __vpfe_set_format(struct vpfe_device *vpfe, - struct v4l2_format *format, unsigned int *bpp) +static int __subdev_set_format(struct vpfe_device *vpfe, + struct v4l2_mbus_framefmt *fmt) { - struct vpfe_subdev_info *sdinfo; - struct v4l2_subdev_format fmt; + struct v4l2_subdev *sd = vpfe->current_subdev->sd; + struct v4l2_subdev_format sd_fmt; + struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; int ret; - vpfe_dbg(2, vpfe, "%s:\n", __func__); + sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; + sd_fmt.pad = 0; + *mbus_fmt = *fmt; - sdinfo = vpfe->current_subdev; - if (!sdinfo->sd) - return -EINVAL; + ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &sd_fmt); + if (ret) + return ret; - fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; - fmt.pad = 0; + vpfe_dbg(1, vpfe, "%s %dx%d code:%04X\n", __func__, + fmt->width, fmt->height, fmt->code); - pix_to_mbus(vpfe, &format->fmt.pix, &fmt.format); + return 0; +} - ret = v4l2_subdev_call(sdinfo->sd, pad, set_fmt, NULL, &fmt); - if (ret) - return ret; +static int vpfe_calc_format_size(struct vpfe_device *vpfe, + const struct vpfe_fmt *fmt, + struct v4l2_format *f) +{ + u32 bpp; - v4l2_fill_pix_format(&format->fmt.pix, &fmt.format); - mbus_to_pix(vpfe, &fmt.format, &format->fmt.pix, bpp); + if (!fmt) { + vpfe_dbg(3, vpfe, "No vpfe_fmt provided!\n"); + return -EINVAL; + } - format->type = vpfe->v_fmt.type; + bpp = __get_bytesperpixel(vpfe, fmt); - vpfe_dbg(1, vpfe, - "%s: size %dx%d (%4.4s) bytesperline = %d, size = %d, bpp = %d\n", - __func__, format->fmt.pix.width, format->fmt.pix.height, - (char *)&format->fmt.pix.pixelformat, - format->fmt.pix.bytesperline, format->fmt.pix.sizeimage, *bpp); + /* pitch should be 32 bytes aligned */ + f->fmt.pix.bytesperline = ALIGN(f->fmt.pix.width * bpp, 32); + f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * + f->fmt.pix.height; + + vpfe_dbg(3, vpfe, "%s: fourcc: %4.4s size: %dx%d bpl:%d img_size:%d\n", + __func__, (char *)&f->fmt.pix.pixelformat, + f->fmt.pix.width, f->fmt.pix.height, + f->fmt.pix.bytesperline, f->fmt.pix.sizeimage); return 0; } @@ -1479,14 +1423,62 @@ static int vpfe_enum_fmt(struct file *file, void *priv, } static int vpfe_try_fmt(struct file *file, void *priv, - struct v4l2_format *fmt) + struct v4l2_format *f) { struct vpfe_device *vpfe = video_drvdata(file); - unsigned int bpp; + struct v4l2_subdev *sd = vpfe->current_subdev->sd; + const struct vpfe_fmt *fmt; + struct v4l2_subdev_frame_size_enum fse; + int ret, found; - vpfe_dbg(2, vpfe, "%s:\n", __func__); + fmt = find_format_by_pix(vpfe, f->fmt.pix.pixelformat); + if (!fmt) { + /* default to first entry */ + vpfe_dbg(3, vpfe, "Invalid pixel code: %x, default used instead\n", + f->fmt.pix.pixelformat); + fmt = vpfe->active_fmt[0]; + f->fmt.pix.pixelformat = fmt->fourcc; + } + + f->fmt.pix.field = vpfe->v_fmt.fmt.pix.field; + + /* check for/find a valid width/height */ + ret = 0; + found = false; + fse.pad = 0; + fse.code = fmt->code; + fse.which = V4L2_SUBDEV_FORMAT_ACTIVE; + for (fse.index = 0; ; fse.index++) { + ret = v4l2_subdev_call(sd, pad, enum_frame_size, + NULL, &fse); + if (ret) + break; + + if (f->fmt.pix.width == fse.max_width && + f->fmt.pix.height == fse.max_height) { + found = true; + break; + } else if (f->fmt.pix.width >= fse.min_width && + f->fmt.pix.width <= fse.max_width && + f->fmt.pix.height >= fse.min_height && + f->fmt.pix.height <= fse.max_height) { + found = true; + break; + } + } + + if (!found) { + /* use existing values as default */ + f->fmt.pix.width = vpfe->v_fmt.fmt.pix.width; + f->fmt.pix.height = vpfe->v_fmt.fmt.pix.height; + } - return __vpfe_get_format(vpfe, fmt, &bpp); + /* + * Use current colorspace for now, it will get + * updated properly during s_fmt + */ + f->fmt.pix.colorspace = vpfe->v_fmt.fmt.pix.colorspace; + return vpfe_calc_format_size(vpfe, fmt, f); } static int vpfe_s_fmt(struct file *file, void *priv, @@ -1494,43 +1486,40 @@ static int vpfe_s_fmt(struct file *file, void *priv, { struct vpfe_device *vpfe = video_drvdata(file); const struct vpfe_fmt *fmt; - struct v4l2_format format; - unsigned int bpp; + struct v4l2_mbus_framefmt mbus_fmt; int ret; - vpfe_dbg(2, vpfe, "%s:\n", __func__); - /* If streaming is started, return error */ if (vb2_is_busy(&vpfe->buffer_queue)) { vpfe_err(vpfe, "%s device busy\n", __func__); return -EBUSY; } - ret = __vpfe_get_format(vpfe, &format, &bpp); - if (ret) + ret = vpfe_try_fmt(file, priv, f); + if (ret < 0) return ret; - if (!cmp_v4l2_format(f, &format)) { - /* Sensor format is different from the requested format - * so we need to change it - */ - ret = __vpfe_set_format(vpfe, f, &bpp); - if (ret) - return ret; - } else /* Just make sure all of the fields are consistent */ - *f = format; - fmt = find_format_by_pix(vpfe, f->fmt.pix.pixelformat); - if (!fmt) { - vpfe_dbg(3, vpfe, "Invalid pixel code: %4.4s, This should not happen!!\n", - (char *)&f->fmt.pix.pixelformat); + + v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, fmt->code); + + ret = __subdev_set_format(vpfe, &mbus_fmt); + if (ret) + return ret; + + /* Just double check nothing has gone wrong */ + if (mbus_fmt.code != fmt->code) { + vpfe_dbg(3, vpfe, + "%s subdev changed format on us, this should not happen\n", + __func__); return -EINVAL; } - /* First detach any IRQ if currently attached */ - vpfe_detach_irq(vpfe); - vpfe->v_fmt = *f; - vpfe->bpp = bpp; + v4l2_fill_pix_format(&vpfe->v_fmt.fmt.pix, &mbus_fmt); + vpfe->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + vpfe->v_fmt.fmt.pix.pixelformat = fmt->fourcc; + vpfe_calc_format_size(vpfe, fmt, &vpfe->v_fmt); + *f = vpfe->v_fmt; vpfe->fmt = fmt; /* Update the crop window based on found values */ @@ -1546,9 +1535,7 @@ static int vpfe_enum_size(struct file *file, void *priv, { struct vpfe_device *vpfe = video_drvdata(file); struct v4l2_subdev_frame_size_enum fse; - struct vpfe_subdev_info *sdinfo; - struct v4l2_mbus_framefmt mbus; - struct v4l2_pix_format pix; + struct v4l2_subdev *sd = vpfe->current_subdev->sd; const struct vpfe_fmt *fmt; int ret; @@ -1557,34 +1544,21 @@ static int vpfe_enum_size(struct file *file, void *priv, /* check for valid format */ fmt = find_format_by_pix(vpfe, fsize->pixel_format); if (!fmt) { - vpfe_dbg(3, vpfe, "Invalid pixel code: %x, default used instead\n", - fsize->pixel_format); + vpfe_dbg(3, vpfe, "Invalid pixel code: %x\n", + fsize->pixel_format); return -EINVAL; } memset(fsize->reserved, 0x0, sizeof(fsize->reserved)); - sdinfo = vpfe->current_subdev; - if (!sdinfo->sd) - return -EINVAL; - - memset(&pix, 0x0, sizeof(pix)); - /* Construct pix from parameter and use default for the rest */ - pix.pixelformat = fsize->pixel_format; - pix.width = 640; - pix.height = 480; - pix.colorspace = V4L2_COLORSPACE_SRGB; - pix.field = V4L2_FIELD_NONE; - pix_to_mbus(vpfe, &pix, &mbus); - memset(&fse, 0x0, sizeof(fse)); fse.index = fsize->index; fse.pad = 0; - fse.code = mbus.code; + fse.code = fmt->code; fse.which = V4L2_SUBDEV_FORMAT_ACTIVE; - ret = v4l2_subdev_call(sdinfo->sd, pad, enum_frame_size, NULL, &fse); + ret = v4l2_subdev_call(sd, pad, enum_frame_size, NULL, &fse); if (ret) - return -EINVAL; + return ret; vpfe_dbg(1, vpfe, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n", __func__, fse.index, fse.code, fse.min_width, fse.max_width, @@ -2091,6 +2065,7 @@ vpfe_s_selection(struct file *file, void *fh, struct v4l2_selection *s) struct vpfe_device *vpfe = video_drvdata(file); struct v4l2_rect cr = vpfe->crop; struct v4l2_rect r = s->r; + u32 bpp; /* If streaming is started, return error */ if (vb2_is_busy(&vpfe->buffer_queue)) { @@ -2116,7 +2091,8 @@ vpfe_s_selection(struct file *file, void *fh, struct v4l2_selection *s) s->r = vpfe->crop = r; - vpfe_ccdc_set_image_window(&vpfe->ccdc, &r, vpfe->bpp); + bpp = __get_bytesperpixel(vpfe, vpfe->fmt); + vpfe_ccdc_set_image_window(&vpfe->ccdc, &r, bpp); vpfe->v_fmt.fmt.pix.width = r.width; vpfe->v_fmt.fmt.pix.height = r.height; vpfe->v_fmt.fmt.pix.bytesperline = diff --git a/drivers/media/platform/am437x/am437x-vpfe.h b/drivers/media/platform/am437x/am437x-vpfe.h index 64a25bf720e4..0d10d2b4d7a2 100644 --- a/drivers/media/platform/am437x/am437x-vpfe.h +++ b/drivers/media/platform/am437x/am437x-vpfe.h @@ -282,8 +282,6 @@ struct vpfe_device { /* Used to store pixel format */ const struct vpfe_fmt *fmt; struct v4l2_format v_fmt; - /* Used to store current bytes per pixel based on current format */ - unsigned int bpp; struct vpfe_fmt *active_fmt[VPFE_MAX_ACTIVE_FMT]; unsigned int num_active_fmt; -- 2.17.1
