diff --git a/drivers/media/platform/msm/camera_v2/isp/msm_isp.h b/drivers/media/platform/msm/camera_v2/isp/msm_isp.h
index 64e219841af..6059e099bc8 100644
--- a/drivers/media/platform/msm/camera_v2/isp/msm_isp.h
+++ b/drivers/media/platform/msm/camera_v2/isp/msm_isp.h
@@ -183,6 +183,7 @@ struct msm_vfe_ops {
 
 struct msm_vfe_hardware_info {
 	int num_iommu_ctx;
+	int vfe_clk_idx;
 	struct msm_vfe_ops vfe_ops;
 	struct msm_vfe_axi_hardware_info *axi_hw_info;
 	struct msm_vfe_stats_hardware_info *stats_hw_info;
@@ -265,6 +266,7 @@ struct msm_vfe_src_info {
 	uint8_t pix_stream_count;
 	uint8_t raw_stream_count;
 	enum msm_vfe_inputmux input_mux;
+	uint32_t pixel_clock;
 };
 
 enum msm_wm_ub_cfg_type {
diff --git a/drivers/media/platform/msm/camera_v2/isp/msm_isp32.c b/drivers/media/platform/msm/camera_v2/isp/msm_isp32.c
index 1d9dded99de..c1548952f07 100644
--- a/drivers/media/platform/msm/camera_v2/isp/msm_isp32.c
+++ b/drivers/media/platform/msm/camera_v2/isp/msm_isp32.c
@@ -75,6 +75,7 @@ static struct msm_bus_scale_pdata msm_vfe32_bus_client_pdata = {
 		.name = "msm_camera_vfe",
 };
 
+#define VFE32_CLK_IDX 0
 static struct msm_cam_clk_info msm_vfe32_clk_info[] = {
 	{"vfe_clk", 266667000},
 	{"vfe_pclk", -1},
@@ -1021,6 +1022,7 @@ static struct v4l2_subdev_internal_ops msm_vfe32_internal_ops = {
 
 struct msm_vfe_hardware_info vfe32_hw_info = {
 	.num_iommu_ctx = 2,
+	.vfe_clk_idx = VFE32_CLK_IDX,
 	.vfe_ops = {
 		.irq_ops = {
 			.read_irq_status = msm_vfe32_read_irq_status,
diff --git a/drivers/media/platform/msm/camera_v2/isp/msm_isp40.c b/drivers/media/platform/msm/camera_v2/isp/msm_isp40.c
index 2b49e2b4223..91f6254cb1e 100644
--- a/drivers/media/platform/msm/camera_v2/isp/msm_isp40.c
+++ b/drivers/media/platform/msm/camera_v2/isp/msm_isp40.c
@@ -114,6 +114,7 @@ static struct msm_bus_scale_pdata msm_vfe40_bus_client_pdata = {
 	.name = "msm_camera_vfe",
 };
 
+#define VFE40_CLK_IDX 1
 static struct msm_cam_clk_info msm_vfe40_clk_info[] = {
 	{"camss_top_ahb_clk", -1},
 	{"vfe_clk_src", 266670000},
@@ -1290,6 +1291,7 @@ static struct v4l2_subdev_internal_ops msm_vfe40_internal_ops = {
 
 struct msm_vfe_hardware_info vfe40_hw_info = {
 	.num_iommu_ctx = 1,
+	.vfe_clk_idx = VFE40_CLK_IDX,
 	.vfe_ops = {
 		.irq_ops = {
 			.read_irq_status = msm_vfe40_read_irq_status,
diff --git a/drivers/media/platform/msm/camera_v2/isp/msm_isp_util.c b/drivers/media/platform/msm/camera_v2/isp/msm_isp_util.c
index 3035d93a563..9e4a0a1508c 100644
--- a/drivers/media/platform/msm/camera_v2/isp/msm_isp_util.c
+++ b/drivers/media/platform/msm/camera_v2/isp/msm_isp_util.c
@@ -68,28 +68,65 @@ int msm_isp_unsubscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
 	return rc;
 }
 
-int msm_isp_cfg_pix(struct vfe_device *vfe_dev,
-	struct msm_vfe_pix_cfg *pix_cfg)
+static int msm_isp_set_clk_rate(struct vfe_device *vfe_dev, uint32_t rate)
 {
 	int rc = 0;
-	/*TD Validate config info
-	 * should check if all streams are off */
+	int clk_idx = vfe_dev->hw_info->vfe_clk_idx;
+	long round_rate =
+		clk_round_rate(vfe_dev->vfe_clk[clk_idx], rate);
+	if (round_rate < 0) {
+		pr_err("%s: Invalid vfe clock rate\n", __func__);
+		return round_rate;
+	}
 
-	vfe_dev->axi_data.src_info[VFE_PIX_0].input_mux = pix_cfg->input_mux;
+	rc = clk_set_rate(vfe_dev->vfe_clk[clk_idx], round_rate);
+	if (rc < 0) {
+		pr_err("%s: Vfe set rate error\n", __func__);
+		return rc;
+	}
+	return 0;
+}
 
-	vfe_dev->hw_info->vfe_ops.core_ops.cfg_camif(vfe_dev, pix_cfg);
+int msm_isp_cfg_pix(struct vfe_device *vfe_dev,
+	struct msm_vfe_input_cfg *input_cfg)
+{
+	int rc = 0;
+	if (vfe_dev->axi_data.src_info[VFE_PIX_0].active) {
+		pr_err("%s: pixel path is active\n", __func__);
+		return -EINVAL;
+	}
+
+	vfe_dev->axi_data.src_info[VFE_PIX_0].pixel_clock =
+		input_cfg->input_pix_clk;
+	vfe_dev->axi_data.src_info[VFE_PIX_0].input_mux =
+		input_cfg->d.pix_cfg.input_mux;
+
+	rc = msm_isp_set_clk_rate(vfe_dev,
+		vfe_dev->axi_data.src_info[VFE_PIX_0].pixel_clock);
+	if (rc < 0) {
+		pr_err("%s: clock set rate failed\n", __func__);
+		return rc;
+	}
+
+	vfe_dev->hw_info->vfe_ops.core_ops.cfg_camif(
+		vfe_dev, &input_cfg->d.pix_cfg);
 	return rc;
 }
 
 int msm_isp_cfg_rdi(struct vfe_device *vfe_dev,
-	struct msm_vfe_rdi_cfg *rdi_cfg, enum msm_vfe_input_src input_src)
+	struct msm_vfe_input_cfg *input_cfg)
 {
 	int rc = 0;
-	/*TD Validate config info
-	 * should check if all streams are off */
+	if (vfe_dev->axi_data.src_info[input_cfg->input_src].active) {
+		pr_err("%s: RAW%d path is active\n", __func__,
+			   input_cfg->input_src - VFE_RAW_0);
+		return -EINVAL;
+	}
 
-	vfe_dev->hw_info->vfe_ops.core_ops.
-		cfg_rdi_reg(vfe_dev, rdi_cfg, input_src);
+	vfe_dev->axi_data.src_info[input_cfg->input_src].pixel_clock =
+		input_cfg->input_pix_clk;
+	vfe_dev->hw_info->vfe_ops.core_ops.cfg_rdi_reg(
+		vfe_dev, &input_cfg->d.rdi_cfg, input_cfg->input_src);
 	return rc;
 }
 
@@ -100,16 +137,16 @@ int msm_isp_cfg_input(struct vfe_device *vfe_dev, void *arg)
 
 	switch (input_cfg->input_src) {
 	case VFE_PIX_0:
-		msm_isp_cfg_pix(vfe_dev, &input_cfg->d.pix_cfg);
+		rc = msm_isp_cfg_pix(vfe_dev, input_cfg);
 		break;
 	case VFE_RAW_0:
 	case VFE_RAW_1:
 	case VFE_RAW_2:
-		msm_isp_cfg_rdi(vfe_dev, &input_cfg->d.rdi_cfg,
-						input_cfg->input_src);
-		break;
-	case VFE_SRC_MAX:
+		rc = msm_isp_cfg_rdi(vfe_dev, input_cfg);
 		break;
+	default:
+		pr_err("%s: Invalid input source\n", __func__);
+		rc = -EINVAL;
 	}
 	return rc;
 }
diff --git a/include/media/msmb_isp.h b/include/media/msmb_isp.h
index ea9ca79d5b0..1342ccef809 100644
--- a/include/media/msmb_isp.h
+++ b/include/media/msmb_isp.h
@@ -109,6 +109,7 @@ struct msm_vfe_input_cfg {
 		struct msm_vfe_rdi_cfg rdi_cfg;
 	} d;
 	enum msm_vfe_input_src input_src;
+	uint32_t input_pix_clk;
 };
 
 struct msm_vfe_axi_plane_cfg {