1 files changed, 16 insertions, 9 deletions

diff --git a/drivers/staging/media/imx/imx6-mipi-csi2.c b/drivers/staging/media/imx/imx6-mipi-csi2.c
index 477d191..ceeeb30 100644
--- a/drivers/staging/media/imx/imx6-mipi-csi2.c
+++ b/drivers/staging/media/imx/imx6-mipi-csi2.c
@@ -447,6 +447,16 @@ out:
 	return ret;
 }
 
+static struct v4l2_mbus_framefmt *
+__csi2_get_fmt(struct csi2_dev *csi2, struct v4l2_subdev_pad_config *cfg,
+	       unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csi2->sd, cfg, pad);
+	else
+		return &csi2->format_mbus;
+}
+
 static int csi2_get_fmt(struct v4l2_subdev *sd,
 			struct v4l2_subdev_pad_config *cfg,
 			struct v4l2_subdev_format *sdformat)
@@ -456,11 +466,7 @@ static int csi2_get_fmt(struct v4l2_subdev *sd,
 
 	mutex_lock(&csi2->lock);
 
-	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY)
-		fmt = v4l2_subdev_get_try_format(&csi2->sd, cfg,
-						 sdformat->pad);
-	else
-		fmt = &csi2->format_mbus;
+	fmt = __csi2_get_fmt(csi2, cfg, sdformat->pad, sdformat->which);
 
 	sdformat->format = *fmt;
 
@@ -474,6 +480,7 @@ static int csi2_set_fmt(struct v4l2_subdev *sd,
 			struct v4l2_subdev_format *sdformat)
 {
 	struct csi2_dev *csi2 = sd_to_dev(sd);
+	struct v4l2_mbus_framefmt *fmt;
 	int ret = 0;
 
 	if (sdformat->pad >= CSI2_NUM_PADS)
@@ -490,10 +497,9 @@ static int csi2_set_fmt(struct v4l2_subdev *sd,
 	if (sdformat->pad != CSI2_SINK_PAD)
 		sdformat->format = csi2->format_mbus;
 
-	if (sdformat->which == V4L2_SUBDEV_FORMAT_TRY)
-		cfg->try_fmt = sdformat->format;
-	else
-		csi2->format_mbus = sdformat->format;
+	fmt = __csi2_get_fmt(csi2, cfg, sdformat->pad, sdformat->which);
+
+	*fmt = sdformat->format;
 out:
 	mutex_unlock(&csi2->lock);
 	return ret;
@@ -531,6 +537,7 @@ static const struct v4l2_subdev_video_ops csi2_video_ops = {
 };
 
 static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
+	.init_cfg = imx_media_init_cfg,
 	.get_fmt = csi2_get_fmt,
 	.set_fmt = csi2_set_fmt,
 };