diff --git a/drivers/staging/media/imx/imx6-mipi-csi2.c b/drivers/staging/media/imx/imx6-mipi-csi2.c
index f29e28df36ed..06345de9f060 100644
--- a/drivers/staging/media/imx/imx6-mipi-csi2.c
+++ b/drivers/staging/media/imx/imx6-mipi-csi2.c
@@ -47,6 +47,7 @@ struct csi2_dev {
 
 	struct v4l2_mbus_framefmt format_mbus;
 
+	int                     power_count;
 	int                     stream_count;
 	struct v4l2_subdev      *src_sd;
 	bool                    sink_linked[CSI2_NUM_SRC_PADS];
@@ -113,9 +114,10 @@ static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev)
  * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE)
  *    to confirm that the D-PHY is receiving a clock on the D-PHY clock lane.
  *
- * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step
- * 6 is accomplished by calling the source subdev's s_stream(ON) between
- * steps 5 and 7.
+ * Steps 3 through 5 are carried out by csi2_s_power(ON) here.
+ *
+ * Steps 6 and 7 are carried out by stream(ON) here. Step 6 is accomplished
+ * by calling the source subdev's s_stream(ON).
  */
 
 static void csi2_enable(struct csi2_dev *csi2, bool enable)
@@ -295,7 +297,7 @@ static void csi2ipu_gasket_init(struct csi2_dev *csi2)
 	writel(reg, csi2->base + CSI2IPU_GASKET);
 }
 
-static int csi2_start(struct csi2_dev *csi2)
+static int csi2_power_on(struct csi2_dev *csi2)
 {
 	int ret;
 
@@ -320,41 +322,87 @@ static int csi2_start(struct csi2_dev *csi2)
 	if (ret)
 		goto err_assert_reset;
 
+	return 0;
+
+err_assert_reset:
+	csi2_enable(csi2, false);
+err_disable_clk:
+	clk_disable_unprepare(csi2->pix_clk);
+	return ret;
+}
+
+static void csi2_power_off(struct csi2_dev *csi2)
+{
+	csi2_enable(csi2, false);
+	clk_disable_unprepare(csi2->pix_clk);
+}
+
+static int csi2_stream_on(struct csi2_dev *csi2)
+{
+	int ret;
+
 	/* Step 6 */
 	ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1);
 	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
 	if (ret)
-		goto err_assert_reset;
+		return ret;
 
 	/* Step 7 */
 	ret = csi2_dphy_wait_clock_lane(csi2);
 	if (ret)
-		goto err_stop_upstream;
-
-	return 0;
+		v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
 
-err_stop_upstream:
-	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
-err_assert_reset:
-	csi2_enable(csi2, false);
-err_disable_clk:
-	clk_disable_unprepare(csi2->pix_clk);
 	return ret;
 }
 
-static void csi2_stop(struct csi2_dev *csi2)
+static void csi2_stream_off(struct csi2_dev *csi2)
 {
 	/* stop upstream */
 	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
-
-	csi2_enable(csi2, false);
-	clk_disable_unprepare(csi2->pix_clk);
 }
 
 /*
  * V4L2 subdev operations.
  */
 
+static int csi2_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&csi2->lock);
+
+	if (!csi2->src_sd) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * power on/off only if power_count is going from
+	 * 0 to 1 / 1 to 0.
+	 */
+	if (csi2->power_count != !on)
+		goto update_count;
+
+	dev_dbg(csi2->dev, "power %s\n", on ? "ON" : "OFF");
+
+	if (on)
+		ret = csi2_power_on(csi2);
+	else
+		csi2_power_off(csi2);
+	if (ret)
+		goto out;
+
+	/* Update the power count. */
+update_count:
+	csi2->power_count += on ? 1 : -1;
+	if (csi2->power_count < 0)
+		csi2->power_count = 0;
+out:
+	mutex_unlock(&csi2->lock);
+	return ret;
+}
+
 static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct csi2_dev *csi2 = sd_to_dev(sd);
@@ -385,9 +433,9 @@ static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
 
 	dev_dbg(csi2->dev, "stream %s\n", enable ? "ON" : "OFF");
 	if (enable)
-		ret = csi2_start(csi2);
+		ret = csi2_stream_on(csi2);
 	else
-		csi2_stop(csi2);
+		csi2_stream_off(csi2);
 	if (ret)
 		goto out;
 
@@ -528,6 +576,10 @@ static const struct media_entity_operations csi2_entity_ops = {
 	.link_validate = v4l2_subdev_link_validate,
 };
 
+static const struct v4l2_subdev_core_ops csi2_core_ops = {
+	.s_power = csi2_s_power,
+};
+
 static const struct v4l2_subdev_video_ops csi2_video_ops = {
 	.s_stream = csi2_s_stream,
 };
@@ -539,6 +591,7 @@ static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
 };
 
 static const struct v4l2_subdev_ops csi2_subdev_ops = {
+	.core = &csi2_core_ops,
 	.video = &csi2_video_ops,
 	.pad = &csi2_pad_ops,
 };
