MLK-16977-1 media: v4l_cap: implement S_FMT ioctl

gsteamer uses S_FMT ioctl to set the resolution instead of using
S_PARM ioctl. The S_PARM ioctl depends on the capture mode which is
not in the common way.
This patch uses a common method to set the picture resulotion.

Signed-off-by: Robby Cai <robby.cai@nxp.com>

diff --git a/drivers/media/platform/imx8/mxc-mipi-csi2_yav.c b/drivers/media/platform/imx8/mxc-mipi-csi2_yav.c
index 01fc53e..dc78c31 100644 (file)
--- a/drivers/media/platform/imx8/mxc-mipi-csi2_yav.c
+++ b/drivers/media/platform/imx8/mxc-mipi-csi2_yav.c
@@ -395,8 +395,13 @@ static int mipi_csi2_set_fmt(struct v4l2_subdev *sd,
                             struct v4l2_subdev_pad_config *cfg,
                             struct v4l2_subdev_format *fmt)
 {
-       /* TODO */
-       return 0;
+       struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+       struct v4l2_subdev *sensor_sd = csi2dev->sensor_sd;
+
+       if (fmt->pad)
+               return -EINVAL;
+
+       return v4l2_subdev_call(sensor_sd, pad, set_fmt, NULL, fmt);
 }
 
 static int mipi_csis_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)

diff --git a/drivers/media/platform/mxc/capture/ov5640_mipi_v2.c b/drivers/media/platform/mxc/capture/ov5640_mipi_v2.c
index 4ea4f6a..58b3320 100644 (file)
--- a/drivers/media/platform/mxc/capture/ov5640_mipi_v2.c
+++ b/drivers/media/platform/mxc/capture/ov5640_mipi_v2.c
@@ -116,12 +116,26 @@ struct ov5640 {
 
        void (*io_init)(void);
 };
+
+struct ov5640_res {
+       int width;
+       int height;
+};
+
 /*!
  * Maintains the information on the current state of the sesor.
  */
 static struct ov5640 ov5640_data;
 static int pwn_gpio, rst_gpio;
 
+struct ov5640_res ov5640_valid_res[] = {
+       [0] = {640, 480},
+       [1] = {720, 480},
+       [2] = {1280, 720},
+       [3] = {1920, 1080},
+       [4] = {2592, 1944},
+};
+
 static struct reg_value ov5640_init_setting_30fps_VGA[] = {
 
        {0x3103, 0x11, 0, 0}, {0x3008, 0x82, 0, 5}, {0x3008, 0x42, 0, 0},
@@ -402,6 +416,19 @@ static const struct ov5640_datafmt ov5640_colour_fmts[] = {
        {MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG},
 };
 
+static int get_capturemode(int width, int height)
+{
+       int i;
+
+       for (i = 0; i < ARRAY_SIZE(ov5640_valid_res); i++) {
+               if ((ov5640_valid_res[i].width == width) &&
+                    (ov5640_valid_res[i].height == height))
+                       return i;
+       }
+
+       return -1;
+}
+
 static struct ov5640 *to_ov5640(const struct i2c_client *client)
 {
        return container_of(i2c_get_clientdata(client), struct ov5640, subdev);
@@ -1297,6 +1324,7 @@ static int ov5640_set_fmt(struct v4l2_subdev *sd,
        const struct ov5640_datafmt *fmt = ov5640_find_datafmt(mf->code);
        struct i2c_client *client = v4l2_get_subdevdata(sd);
        struct ov5640 *sensor = to_ov5640(client);
+       int capturemode;
 
        if (!fmt) {
                mf->code        = ov5640_colour_fmts[0].code;
@@ -1310,7 +1338,16 @@ static int ov5640_set_fmt(struct v4l2_subdev *sd,
 
        sensor->fmt = fmt;
 
-       return 0;
+       capturemode = get_capturemode(mf->width, mf->height);
+       if (capturemode >= 0) {
+               ov5640_data.streamcap.capturemode = capturemode;
+               ov5640_data.pix.width = mf->width;
+               ov5640_data.pix.height = mf->height;
+               return 0;
+       }
+
+       dev_err(&client->dev, "%s set fail\n", __func__);
+       return -EINVAL;
 }
 
 
@@ -1330,6 +1367,9 @@ static int ov5640_get_fmt(struct v4l2_subdev *sd,
        mf->colorspace  = fmt->colorspace;
        mf->field       = V4L2_FIELD_NONE;
 
+       mf->width       = ov5640_data.pix.width;
+       mf->height      = ov5640_data.pix.height;
+
        return 0;
 }