diff --git a/drivers/media/platform/mtk-mipicsi/mtk_mipicsi.c b/drivers/media/platform/mtk-mipicsi/mtk_mipicsi.c
index ad54cb9..683c775 100644
--- a/drivers/media/platform/mtk-mipicsi/mtk_mipicsi.c
+++ b/drivers/media/platform/mtk-mipicsi/mtk_mipicsi.c
@@ -573,11 +573,9 @@
 static int mtk_mipicsi_pm_suspend(struct device *dev)
 {
 	struct mtk_mipicsi_dev *mipicsi = dev_get_drvdata(dev);
-	struct v4l2_subdev *sd = mipicsi->mipicsi_sd.subdev;
 	int ret = 0;
 
 	mipicsi_clk_enable(mipicsi, false);
-	v4l2_subdev_call(sd, video, s_stream, 0);
 
 	if (mipicsi->larb_pdev != NULL)
 		mtk_smi_larb_put(mipicsi->larb_pdev);
@@ -596,7 +594,6 @@
 static int mtk_mipicsi_pm_resume(struct device *dev)
 {
 	struct mtk_mipicsi_dev *mipicsi = dev_get_drvdata(dev);
-	struct v4l2_subdev *sd = mipicsi->mipicsi_sd.subdev;
 	int ret = 0;
 
 	if (mipicsi->larb_pdev != NULL) {
@@ -610,7 +607,6 @@
 
 	mipicsi_clk_enable(mipicsi, true);
 	mtk_mipicsi_reg_init(mipicsi);
-	v4l2_subdev_call(sd, video, s_stream, 1);
 
 	return ret;
 }
@@ -774,6 +770,9 @@
 		unsigned int count)
 {
 	struct mtk_mipicsi_dev *mipicsi = vb2_get_drv_priv(vq);
+	struct v4l2_subdev *sd = mipicsi->mipicsi_sd.subdev;
+
+	v4l2_subdev_call(sd, video, s_stream, 1);
 
 	mtk_mipicsi_cmos_vf_enable(mipicsi, mipicsi->camsv_num, true);
 
@@ -785,12 +784,15 @@
 static void mtk_mipicsi_vb2_stop_streaming(struct vb2_queue *vq)
 {
 	struct mtk_mipicsi_dev *mipicsi = vb2_get_drv_priv(vq);
+	struct v4l2_subdev *sd = mipicsi->mipicsi_sd.subdev;
 	struct mtk_mipicsi_buf *buf = NULL;
 	struct mtk_mipicsi_buf *tmp = NULL;
 	unsigned int index = 0;
 
 	mtk_mipicsi_cmos_vf_enable(mipicsi, mipicsi->camsv_num, false);
 
+	v4l2_subdev_call(sd, video, s_stream, 0);
+
 	spin_lock(&mipicsi->queue_lock);
 	while (list_empty(&(mipicsi->fb_list)) == 0) {
 		list_for_each_entry_safe(buf, tmp, &(mipicsi->fb_list), queue) {
@@ -922,10 +924,12 @@
 	struct v4l2_subdev *sd = mipicsi->mipicsi_sd.subdev;
 	struct device *dev = &mipicsi->pdev->dev;
 	struct v4l2_pix_format *pix = &f->fmt.pix;
+	struct mtk_mipicsi_channel *ch = mipicsi->channel;
 	struct v4l2_subdev_format format = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 	};
 	const struct mtk_format *current_fmt;
+	unsigned int i;
 	int ret;
 
 	ret = mtk_mipicsi_try_fmt(mipicsi, f, &current_fmt);
@@ -942,6 +946,14 @@
 	mipicsi->fmt = *f;
 	mipicsi->current_fmt = current_fmt;
 
+	for (i = 0; i < mipicsi->camsv_num; i++) {
+		u32 b = mipicsi->fmt.fmt.pix.width * 2;
+		u32 h = mipicsi->fmt.fmt.pix.height;
+
+		mtk_mipicsi_seninf_mux_init(ch[i].seninf_mux, i);
+		mtk_mipicsi_camsv_init(ch[i].camsv, b, h);
+	}
+
 	dev_info(dev, "width/height/sizeimage %u/%u/%u", pix->width,
 							 pix->height,
 							 pix->sizeimage);