@@ -1271,6 +1271,9 @@ static int cio2_subdev_set_fmt(struct v4
struct v4l2_subdev_format *fmt)
{
struct cio2_queue *q = container_of(sd, struct cio2_queue, subdev);
+ struct v4l2_mbus_framefmt *mbus;
+ u32 mbus_code = fmt->format.code;
+ unsigned int i;
/*
* Only allow setting sink pad format;
@@ -1279,18 +1282,26 @@ static int cio2_subdev_set_fmt(struct v4
if (fmt->pad == CIO2_PAD_SOURCE)
return cio2_subdev_get_fmt(sd, cfg, fmt);
- mutex_lock(&q->subdev_lock);
+ if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+ mbus = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+ else
+ mbus = &q->subdev_fmt;
+
+ fmt->format.code = formats[0].mbus_code;
- if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
- *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) = fmt->format;
- } else {
- /* It's the sink, allow changing frame size */
- q->subdev_fmt.width = fmt->format.width;
- q->subdev_fmt.height = fmt->format.height;
- q->subdev_fmt.code = fmt->format.code;
- fmt->format = q->subdev_fmt;
+ for (i = 0; i < ARRAY_SIZE(formats); i++) {
+ if (formats[i].mbus_code == fmt->format.code) {
+ fmt->format.code = mbus_code;
+ break;
+ }
}
+ fmt->format.width = min_t(u32, fmt->format.width, CIO2_IMAGE_MAX_WIDTH);
+ fmt->format.height = min_t(u32, fmt->format.height,
+ CIO2_IMAGE_MAX_LENGTH);
+
+ mutex_lock(&q->subdev_lock);
+ *mbus = fmt->format;
mutex_unlock(&q->subdev_lock);
return 0;