mipi: reduce code, pair up mipi_csi2_pixelclk_enable/disable

9 files changed, 119 insertions, 355 deletions

diff --git a/drivers/media/platform/mxc/capture/ipu_bg_overlay_sdc.c b/drivers/media/platform/mxc/capture/ipu_bg_overlay_sdc.c
index 8680f45981ea..7b4e3d33ae8c 100644
--- a/drivers/media/platform/mxc/capture/ipu_bg_overlay_sdc.c
+++ b/drivers/media/platform/mxc/capture/ipu_bg_overlay_sdc.c
@@ -129,11 +129,6 @@ static int csi_enc_setup(cam_data *cam)
 	ipu_channel_params_t params;
 	u32 pixel_fmt;
 	int err = 0, sensor_protocol = 0;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	if (!cam) {
 		printk(KERN_ERR "cam private is NULL\n");
@@ -161,36 +156,9 @@ static int csi_enc_setup(cam_data *cam)
 		printk(KERN_ERR "sensor protocol unsupported\n");
 		return -EINVAL;
 	}
-
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id) {
-				params.csi_mem.mipi_en = true;
-				params.csi_mem.mipi_vc =
-				mipi_csi2_get_virtual_channel(mipi_csi2_info);
-				params.csi_mem.mipi_id =
-				mipi_csi2_get_datatype(mipi_csi2_info);
-
-				mipi_csi2_pixelclk_enable(mipi_csi2_info);
-			} else {
-				params.csi_mem.mipi_en = false;
-				params.csi_mem.mipi_vc = 0;
-				params.csi_mem.mipi_id = 0;
-			}
-		} else {
-			params.csi_mem.mipi_en = false;
-			params.csi_mem.mipi_vc = 0;
-			params.csi_mem.mipi_id = 0;
-		}
-	}
-#endif
+	err = cam_mipi_csi2_enable(cam, &params.csi_mem.mipi);
+	if (err)
+		return err;
 
 	if (cam->vf_bufs_vaddr[0]) {
 		dma_free_coherent(0, cam->vf_bufs_size[0],
@@ -379,12 +347,8 @@ static int bg_overlay_start(void *private)
 static int bg_overlay_stop(void *private)
 {
 	int err = 0;
+	int err2 = 0;
 	cam_data *cam = (cam_data *) private;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	if (cam->overlay_active == false)
 		return 0;
@@ -395,21 +359,7 @@ static int bg_overlay_stop(void *private)
 
 	csi_buffer_num = 0;
 
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id)
-				mipi_csi2_pixelclk_disable(mipi_csi2_info);
-		}
-	}
-#endif
-
+	err2 = cam_mipi_csi2_disable(cam);
 	flush_work(&cam->csi_work_struct);
 	cancel_work_sync(&cam->csi_work_struct);
 
@@ -441,7 +391,7 @@ static int bg_overlay_stop(void *private)
 	}
 
 	cam->overlay_active = false;
-	return err;
+	return err ? err : err2;
 }
 
 /*!
diff --git a/drivers/media/platform/mxc/capture/ipu_csi_enc.c b/drivers/media/platform/mxc/capture/ipu_csi_enc.c
index ad9371567b50..eb18af86aec1 100644
--- a/drivers/media/platform/mxc/capture/ipu_csi_enc.c
+++ b/drivers/media/platform/mxc/capture/ipu_csi_enc.c
@@ -69,11 +69,6 @@ static int csi_enc_setup(cam_data *cam)
 	u32 pixel_fmt;
 	int err = 0, sensor_protocol = 0;
 	dma_addr_t dummy = cam->dummy_frame.buffer.m.offset;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	CAMERA_TRACE("In csi_enc_setup\n");
 	if (!cam) {
@@ -129,36 +124,9 @@ static int csi_enc_setup(cam_data *cam)
 		printk(KERN_ERR "format not supported\n");
 		return -EINVAL;
 	}
-
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id) {
-				params.csi_mem.mipi_en = true;
-				params.csi_mem.mipi_vc =
-				mipi_csi2_get_virtual_channel(mipi_csi2_info);
-				params.csi_mem.mipi_id =
-				mipi_csi2_get_datatype(mipi_csi2_info);
-
-				mipi_csi2_pixelclk_enable(mipi_csi2_info);
-			} else {
-				params.csi_mem.mipi_en = false;
-				params.csi_mem.mipi_vc = 0;
-				params.csi_mem.mipi_id = 0;
-			}
-		} else {
-			params.csi_mem.mipi_en = false;
-			params.csi_mem.mipi_vc = 0;
-			params.csi_mem.mipi_id = 0;
-		}
-	}
-#endif
+	err = cam_mipi_csi2_enable(cam, &params.csi_mem.mipi);
+	if (err)
+		return err;
 
 	err = ipu_channel_request(cam->ipu, CSI_MEM, &params, &cam->ipu_chan);
 	if (err) {
@@ -276,11 +244,7 @@ static int csi_enc_disabling_tasks(void *private)
 {
 	cam_data *cam = (cam_data *) private;
 	int err = 0;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
+	int err2 = 0;
 
 	err = ipu_channel_disable(cam->ipu_chan, true);
 
@@ -292,23 +256,8 @@ static int csi_enc_disabling_tasks(void *private)
 				  cam->dummy_frame.paddress);
 		cam->dummy_frame.vaddress = 0;
 	}
-
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id)
-				mipi_csi2_pixelclk_disable(mipi_csi2_info);
-		}
-	}
-#endif
-
-	return err;
+	err2 = cam_mipi_csi2_disable(cam);
+	return err ? err : err2;
 }
 
 /*!
diff --git a/drivers/media/platform/mxc/capture/ipu_fg_overlay_sdc.c b/drivers/media/platform/mxc/capture/ipu_fg_overlay_sdc.c
index 597ecb69b2c7..54e59be281d4 100644
--- a/drivers/media/platform/mxc/capture/ipu_fg_overlay_sdc.c
+++ b/drivers/media/platform/mxc/capture/ipu_fg_overlay_sdc.c
@@ -134,11 +134,6 @@ static int csi_enc_setup(cam_data *cam)
 {
 	ipu_channel_params_t params;
 	int err = 0, sensor_protocol = 0;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	CAMERA_TRACE("In csi_enc_setup\n");
 	if (!cam) {
@@ -167,36 +162,9 @@ static int csi_enc_setup(cam_data *cam)
 		printk(KERN_ERR "sensor protocol unsupported\n");
 		return -EINVAL;
 	}
-
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id) {
-				params.csi_mem.mipi_en = true;
-				params.csi_mem.mipi_vc =
-				mipi_csi2_get_virtual_channel(mipi_csi2_info);
-				params.csi_mem.mipi_id =
-				mipi_csi2_get_datatype(mipi_csi2_info);
-
-				mipi_csi2_pixelclk_enable(mipi_csi2_info);
-			} else {
-				params.csi_mem.mipi_en = false;
-				params.csi_mem.mipi_vc = 0;
-				params.csi_mem.mipi_id = 0;
-			}
-		} else {
-			params.csi_mem.mipi_en = false;
-			params.csi_mem.mipi_vc = 0;
-			params.csi_mem.mipi_id = 0;
-		}
-	}
-#endif
+	err = cam_mipi_csi2_enable(cam, &params.csi_mem.mipi);
+	if (err)
+		return err;
 
 	if (cam->vf_bufs_vaddr[0]) {
 		dma_free_coherent(0, cam->vf_bufs_size[0],
@@ -448,15 +416,10 @@ static int foreground_stop(void *private)
 {
 	cam_data *cam = (cam_data *) private;
 	int err = 0, i = 0;
+	int err2 = 0;
 	struct fb_info *fbi = NULL;
 	struct fb_var_screeninfo fbvar;
 
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
-
 	if (cam->overlay_active == false)
 		return 0;
 
@@ -491,21 +454,7 @@ static int foreground_stop(void *private)
 	fbvar.nonstd = cam->fb_origin_std;
 	fbvar.activate |= FB_ACTIVATE_FORCE;
 	fb_set_var(fbi, &fbvar);
-
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id)
-				mipi_csi2_pixelclk_disable(mipi_csi2_info);
-		}
-	}
-#endif
+	err2 = cam_mipi_csi2_disable(cam);
 
 	flush_work(&cam->csi_work_struct);
 	cancel_work_sync(&cam->csi_work_struct);
@@ -526,7 +475,7 @@ static int foreground_stop(void *private)
 	}
 
 	cam->overlay_active = false;
-	return err;
+	return err ? err : err2;
 }
 
 /*!
diff --git a/drivers/media/platform/mxc/capture/ipu_prp_enc.c b/drivers/media/platform/mxc/capture/ipu_prp_enc.c
index 5b4673ac92cc..18fe28cc7ae9 100644
--- a/drivers/media/platform/mxc/capture/ipu_prp_enc.c
+++ b/drivers/media/platform/mxc/capture/ipu_prp_enc.c
@@ -71,11 +71,6 @@ static int prp_enc_setup(cam_data *cam)
 	ipu_channel_params_t enc;
 	int err = 0;
 	dma_addr_t dummy = cam->dummy_frame.buffer.m.offset;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	CAMERA_TRACE("In prp_enc_setup\n");
 	if (!cam) {
@@ -133,36 +128,9 @@ static int prp_enc_setup(cam_data *cam)
 		printk(KERN_ERR "format not supported\n");
 		return -EINVAL;
 	}
-
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id) {
-				enc.csi_prp_enc_mem.mipi_en = true;
-				enc.csi_prp_enc_mem.mipi_vc =
-				mipi_csi2_get_virtual_channel(mipi_csi2_info);
-				enc.csi_prp_enc_mem.mipi_id =
-				mipi_csi2_get_datatype(mipi_csi2_info);
-
-				mipi_csi2_pixelclk_enable(mipi_csi2_info);
-			} else {
-				enc.csi_prp_enc_mem.mipi_en = false;
-				enc.csi_prp_enc_mem.mipi_vc = 0;
-				enc.csi_prp_enc_mem.mipi_id = 0;
-			}
-		} else {
-			enc.csi_prp_enc_mem.mipi_en = false;
-			enc.csi_prp_enc_mem.mipi_vc = 0;
-			enc.csi_prp_enc_mem.mipi_id = 0;
-		}
-	}
-#endif
+	err = cam_mipi_csi2_enable(cam, &enc.csi_prp_enc_mem.mipi);
+	if (err)
+		return err;
 
 	err = ipu_channel_request(cam->ipu, CSI_PRP_ENC_MEM, &enc, &cam->ipu_chan);
 	if (err) {
@@ -428,11 +396,7 @@ static int prp_enc_disabling_tasks(void *private)
 	cam_data *cam = (cam_data *) private;
 	int err = 0;
 	int err2 = 0;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
+	int err3 = 0;
 
 	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
 		ipu_free_irq(cam->ipu, IPU_IRQ_PRP_ENC_ROT_OUT_EOF, cam);
@@ -451,23 +415,8 @@ static int prp_enc_disabling_tasks(void *private)
 				  cam->dummy_frame.paddress);
 		cam->dummy_frame.vaddress = 0;
 	}
-
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id)
-				mipi_csi2_pixelclk_disable(mipi_csi2_info);
-		}
-	}
-#endif
-
-	return err ? err : err2;
+	err3 = cam_mipi_csi2_disable(cam);
+	return err ? err : (err2 ? err2 : err3);
 }
 
 /*!
diff --git a/drivers/media/platform/mxc/capture/ipu_prp_vf_sdc.c b/drivers/media/platform/mxc/capture/ipu_prp_vf_sdc.c
index e9385ffd0ec8..e2d837a30684 100644
--- a/drivers/media/platform/mxc/capture/ipu_prp_vf_sdc.c
+++ b/drivers/media/platform/mxc/capture/ipu_prp_vf_sdc.c
@@ -78,11 +78,6 @@ static int prpvf_start(void *private)
 	u32 size = 2, temp = 0;
 	int err = 0, i = 0;
 	short *tmp, color;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	if (!cam) {
 		printk(KERN_ERR "private is NULL\n");
@@ -169,35 +164,9 @@ static int prpvf_start(void *private)
 	vf.csi_prp_vf_mem.out_pixel_fmt = vf_out_format;
 	size = cam->win.w.width * cam->win.w.height * size;
 
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id) {
-				vf.csi_prp_vf_mem.mipi_en = true;
-				vf.csi_prp_vf_mem.mipi_vc =
-				mipi_csi2_get_virtual_channel(mipi_csi2_info);
-				vf.csi_prp_vf_mem.mipi_id =
-				mipi_csi2_get_datatype(mipi_csi2_info);
-
-				mipi_csi2_pixelclk_enable(mipi_csi2_info);
-			} else {
-				vf.csi_prp_vf_mem.mipi_en = false;
-				vf.csi_prp_vf_mem.mipi_vc = 0;
-				vf.csi_prp_vf_mem.mipi_id = 0;
-			}
-		} else {
-			vf.csi_prp_vf_mem.mipi_en = false;
-			vf.csi_prp_vf_mem.mipi_vc = 0;
-			vf.csi_prp_vf_mem.mipi_id = 0;
-		}
-	}
-#endif
+	err = cam_mipi_csi2_enable(cam, &vf.csi_prp_vf_mem.mipi);
+	if (err)
+		return err;
 
 	err = ipu_channel_request(cam->ipu, CSI_PRP_VF_MEM, &vf, &cam->ipu_chan);
 	if (err) {
@@ -395,11 +364,6 @@ static int prpvf_stop(void *private)
 	int err = 0, i = 0;
 	struct fb_info *fbi = NULL;
 	struct fb_var_screeninfo fbvar;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	if (cam->overlay_active == false)
 		return 0;
@@ -439,21 +403,7 @@ static int prpvf_stop(void *private)
 	fbvar.nonstd = cam->fb_origin_std;
 	fbvar.activate |= FB_ACTIVATE_FORCE;
 	fb_set_var(fbi, &fbvar);
-
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id)
-				mipi_csi2_pixelclk_disable(mipi_csi2_info);
-		}
-	}
-#endif
+	err2 = cam_mipi_csi2_disable(cam);
 
 	if (cam->vf_bufs_vaddr[0]) {
 		dma_free_coherent(0, cam->vf_bufs_size[0],
@@ -471,7 +421,7 @@ static int prpvf_stop(void *private)
 	}
 
 	cam->overlay_active = false;
-	return err;
+	return err ? err : err2;
 }
 
 /*!
diff --git a/drivers/media/platform/mxc/capture/ipu_prp_vf_sdc_bg.c b/drivers/media/platform/mxc/capture/ipu_prp_vf_sdc_bg.c
index fc027b0f0753..ff9d55262342 100644
--- a/drivers/media/platform/mxc/capture/ipu_prp_vf_sdc_bg.c
+++ b/drivers/media/platform/mxc/capture/ipu_prp_vf_sdc_bg.c
@@ -98,11 +98,6 @@ static int prpvf_start(void *private)
 	u32 offset;
 	u32 bpp, size = 3;
 	int err = 0;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	if (!cam) {
 		printk(KERN_ERR "private is NULL\n");
@@ -154,35 +149,9 @@ static int prpvf_start(void *private)
 	vf.csi_prp_vf_mem.out_pixel_fmt = format;
 	size = cam->win.w.width * cam->win.w.height * size;
 
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id) {
-				vf.csi_prp_vf_mem.mipi_en = true;
-				vf.csi_prp_vf_mem.mipi_vc =
-				mipi_csi2_get_virtual_channel(mipi_csi2_info);
-				vf.csi_prp_vf_mem.mipi_id =
-				mipi_csi2_get_datatype(mipi_csi2_info);
-
-				mipi_csi2_pixelclk_enable(mipi_csi2_info);
-			} else {
-				vf.csi_prp_vf_mem.mipi_en = false;
-				vf.csi_prp_vf_mem.mipi_vc = 0;
-				vf.csi_prp_vf_mem.mipi_id = 0;
-			}
-		} else {
-			vf.csi_prp_vf_mem.mipi_en = false;
-			vf.csi_prp_vf_mem.mipi_vc = 0;
-			vf.csi_prp_vf_mem.mipi_id = 0;
-		}
-	}
-#endif
+	err = cam_mipi_csi2_enable(cam, &vf.csi_prp_vf_mem.mipi);
+	if (err)
+		return err;
 
 	err = ipu_channel_request(cam->ipu, CSI_PRP_VF_MEM, &vf, &cam->ipu_chan);
 	if (err) {
@@ -356,12 +325,8 @@ out_4:
  */
 static int prpvf_stop(void *private)
 {
+	int err = 0;
 	cam_data *cam = (cam_data *) private;
-#ifdef CONFIG_MXC_MIPI_CSI2
-	void *mipi_csi2_info;
-	int ipu_id;
-	int csi_id;
-#endif
 
 	if (cam->overlay_active == false)
 		return 0;
@@ -373,20 +338,7 @@ static int prpvf_stop(void *private)
 	ipu_channel_free(&cam->ipu_chan);
 	ipu_channel_free(&cam->ipu_chan_rot);
 
-#ifdef CONFIG_MXC_MIPI_CSI2
-	mipi_csi2_info = mipi_csi2_get_info();
-
-	if (mipi_csi2_info) {
-		if (mipi_csi2_get_status(mipi_csi2_info)) {
-			ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
-			csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
-
-			if (cam->ipu == ipu_get_soc(ipu_id)
-				&& cam->csi == csi_id)
-				mipi_csi2_pixelclk_disable(mipi_csi2_info);
-		}
-	}
-#endif
+	err = cam_mipi_csi2_disable(cam);
 
 	if (cam->vf_bufs_vaddr[0]) {
 		dma_free_coherent(0, cam->vf_bufs_size[0],
@@ -418,7 +370,7 @@ static int prpvf_stop(void *private)
 	buffer_num = 0;
 	buffer_ready = 0;
 	cam->overlay_active = false;
-	return 0;
+	return err;
 }
 
 /*!
diff --git a/drivers/media/platform/mxc/capture/mxc_v4l2_capture.h b/drivers/media/platform/mxc/capture/mxc_v4l2_capture.h
index e9a1566d18e7..0abecbc788e1 100644
--- a/drivers/media/platform/mxc/capture/mxc_v4l2_capture.h
+++ b/drivers/media/platform/mxc/capture/mxc_v4l2_capture.h
@@ -35,6 +35,7 @@
 #include <linux/pxp_dma.h>
 #include <linux/ipu-v3.h>
 #include <linux/platform_data/dma-imx.h>
+#include <linux/mipi_csi2.h>
 
 #include <media/v4l2-dev.h>
 #include <media/v4l2-int-device.h>
@@ -203,6 +204,7 @@ typedef struct _cam_data {
 	bool overlay_on;
 	bool capture_on;
 	bool ipu_enable_csi_called;
+	bool mipi_pixelclk_enabled;
 	struct ipu_chan *ipu_chan;
 	struct ipu_chan *ipu_chan_rot;
 	int overlay_pid;
@@ -282,5 +284,69 @@ static inline int cam_ipu_disable_csi(cam_data *cam)
 	return ipu_disable_csi(cam->ipu, cam->csi);
 }
 
+static inline int cam_mipi_csi2_enable(cam_data *cam, struct mipi_fields *mf)
+{
+#ifdef CONFIG_MXC_MIPI_CSI2
+	void *mipi_csi2_info;
+	int ipu_id;
+	int csi_id;
+
+	mipi_csi2_info = mipi_csi2_get_info();
+
+	if (!mipi_csi2_info) {
+//		printk(KERN_ERR "%s() in %s: Fail to get mipi_csi2_info!\n",
+//		       __func__, __FILE__);
+//		return -EPERM;
+		return 0;
+	}
+	if (mipi_csi2_get_status(mipi_csi2_info)) {
+		ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
+		csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
+
+		if (cam->ipu == ipu_get_soc(ipu_id)
+				&& cam->csi == csi_id) {
+			mf->en = true;
+			mf->vc = mipi_csi2_get_virtual_channel(mipi_csi2_info);
+			mf->id = mipi_csi2_get_datatype(mipi_csi2_info);
+			if (!mipi_csi2_pixelclk_enable(mipi_csi2_info))
+				cam->mipi_pixelclk_enabled = 1;
+			return 0;
+		}
+	}
+	mf->en = false;
+	mf->vc = 0;
+	mf->id = 0;
+#endif
+	return 0;
+}
+
+static inline int cam_mipi_csi2_disable(cam_data *cam)
+{
+#ifdef CONFIG_MXC_MIPI_CSI2
+	void *mipi_csi2_info;
+	int ipu_id;
+	int csi_id;
+
+	if (!cam->mipi_pixelclk_enabled)
+		return 0;
+	cam->mipi_pixelclk_enabled = 0;
 
+	mipi_csi2_info = mipi_csi2_get_info();
+
+	if (!mipi_csi2_info) {
+//		printk(KERN_ERR "%s() in %s: Fail to get mipi_csi2_info!\n",
+//		       __func__, __FILE__);
+//		return -EPERM;
+		return 0;
+	}
+	if (mipi_csi2_get_status(mipi_csi2_info)) {
+		ipu_id = mipi_csi2_get_bind_ipu(mipi_csi2_info);
+		csi_id = mipi_csi2_get_bind_csi(mipi_csi2_info);
+
+		if ((cam->ipu == ipu_get_soc(ipu_id)) && (cam->csi == csi_id))
+			mipi_csi2_pixelclk_disable(mipi_csi2_info);
+	}
+#endif
+	return 0;
+}
 #endif				/* __MXC_V4L2_CAPTURE_H__ */
diff --git a/drivers/mxc/ipu3/ipu_common.c b/drivers/mxc/ipu3/ipu_common.c
index 74bd861dd7af..a2a2edba866f 100644
--- a/drivers/mxc/ipu3/ipu_common.c
+++ b/drivers/mxc/ipu3/ipu_common.c
@@ -644,13 +644,13 @@ int32_t ipu_init_channel(struct ipu_soc *ipu, ipu_channel_t channel, ipu_channel
 		ipu->csi_channel[params->csi_mem.csi] = channel;
 
 		/*SMFC setting*/
-		if (params->csi_mem.mipi_en) {
+		if (params->csi_mem.mipi.en) {
 			ipu_conf |= (1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
 				params->csi_mem.csi));
-			_ipu_smfc_init(ipu, channel, params->csi_mem.mipi_vc,
+			_ipu_smfc_init(ipu, channel, params->csi_mem.mipi.vc,
 				params->csi_mem.csi);
-			_ipu_csi_set_mipi_di(ipu, params->csi_mem.mipi_vc,
-				params->csi_mem.mipi_id, params->csi_mem.csi);
+			_ipu_csi_set_mipi_di(ipu, params->csi_mem.mipi.vc,
+				params->csi_mem.mipi.id, params->csi_mem.csi);
 		} else {
 			ipu_conf &= ~(1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
 				params->csi_mem.csi));
@@ -675,12 +675,12 @@ int32_t ipu_init_channel(struct ipu_soc *ipu, ipu_channel_t channel, ipu_channel
 		ipu->ic_use_count++;
 		ipu->csi_channel[params->csi_prp_enc_mem.csi] = channel;
 
-		if (params->csi_prp_enc_mem.mipi_en) {
+		if (params->csi_prp_enc_mem.mipi.en) {
 			ipu_conf |= (1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
 				params->csi_prp_enc_mem.csi));
 			_ipu_csi_set_mipi_di(ipu,
-				params->csi_prp_enc_mem.mipi_vc,
-				params->csi_prp_enc_mem.mipi_id,
+				params->csi_prp_enc_mem.mipi.vc,
+				params->csi_prp_enc_mem.mipi.id,
 				params->csi_prp_enc_mem.csi);
 		} else
 			ipu_conf &= ~(1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
@@ -716,12 +716,12 @@ int32_t ipu_init_channel(struct ipu_soc *ipu, ipu_channel_t channel, ipu_channel
 		ipu->ic_use_count++;
 		ipu->csi_channel[params->csi_prp_vf_mem.csi] = channel;
 
-		if (params->csi_prp_vf_mem.mipi_en) {
+		if (params->csi_prp_vf_mem.mipi.en) {
 			ipu_conf |= (1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
 				params->csi_prp_vf_mem.csi));
 			_ipu_csi_set_mipi_di(ipu,
-				params->csi_prp_vf_mem.mipi_vc,
-				params->csi_prp_vf_mem.mipi_id,
+				params->csi_prp_vf_mem.mipi.vc,
+				params->csi_prp_vf_mem.mipi.id,
 				params->csi_prp_vf_mem.csi);
 		} else
 			ipu_conf &= ~(1 << (IPU_CONF_CSI0_DATA_SOURCE_OFFSET +
diff --git a/include/linux/ipu-v3.h b/include/linux/ipu-v3.h
index 359256982845..1145c79030da 100644
--- a/include/linux/ipu-v3.h
+++ b/include/linux/ipu-v3.h
@@ -127,15 +127,18 @@ typedef enum {
 	XY
 } display_addressing_t;
 
+struct mipi_fields {
+	uint32_t id;
+	uint32_t vc;
+	bool en;
+};
 /*!
  * Union of initialization parameters for a logical channel.
  */
 typedef union {
 	struct {
 		uint32_t csi;
-		uint32_t mipi_id;
-		uint32_t mipi_vc;
-		bool mipi_en;
+		struct mipi_fields mipi;
 		bool interlaced;
 	} csi_mem;
 	struct {
@@ -148,9 +151,7 @@ typedef union {
 		uint32_t outh_resize_ratio;
 		uint32_t outv_resize_ratio;
 		uint32_t csi;
-		uint32_t mipi_id;
-		uint32_t mipi_vc;
-		bool mipi_en;
+		struct mipi_fields mipi;
 	} csi_prp_enc_mem;
 	struct {
 		uint32_t in_width;
@@ -189,9 +190,7 @@ typedef union {
 		ipu_motion_sel motion_sel;
 		enum v4l2_field field_fmt;
 		uint32_t csi;
-		uint32_t mipi_id;
-		uint32_t mipi_vc;
-		bool mipi_en;
+		struct mipi_fields mipi;
 	} csi_prp_vf_mem;
 	struct {
 		uint32_t in_width;