1 files changed, 41 insertions, 14 deletions

diff --git a/drivers/media/video/mt9m001.c b/drivers/media/video/mt9m001.c
index 554d2295484e..0c524376b67e 100644
--- a/drivers/media/video/mt9m001.c
+++ b/drivers/media/video/mt9m001.c
@@ -117,24 +117,51 @@ static int reg_clear(struct soc_camera_device *icd, const u8 reg,
 
 static int mt9m001_init(struct soc_camera_device *icd)
 {
+        struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
+        struct soc_camera_link *icl = mt9m001->client->dev.platform_data;
         int ret;
 
-        /* Disable chip, synchronous option update */
         dev_dbg(icd->vdev->parent, "%s\n", __func__);
 
-        ret = reg_write(icd, MT9M001_RESET, 1);
-        if (ret >= 0)
-                ret = reg_write(icd, MT9M001_RESET, 0);
-        if (ret >= 0)
+        if (icl->power) {
+                ret = icl->power(&mt9m001->client->dev, 1);
+                if (ret < 0) {
+                        dev_err(icd->vdev->parent,
+                                "Platform failed to power-on the camera.\n");
+                        return ret;
+                }
+        }
+
+        /* The camera could have been already on, we reset it additionally */
+        if (icl->reset)
+                ret = icl->reset(&mt9m001->client->dev);
+        else
+                ret = -ENODEV;
+
+        if (ret < 0) {
+                /* Either no platform reset, or platform reset failed */
+                ret = reg_write(icd, MT9M001_RESET, 1);
+                if (!ret)
+                        ret = reg_write(icd, MT9M001_RESET, 0);
+        }
+        /* Disable chip, synchronous option update */
+        if (!ret)
                 ret = reg_write(icd, MT9M001_OUTPUT_CONTROL, 0);
 
-        return ret >= 0 ? 0 : -EIO;
+        return ret;
 }
 
 static int mt9m001_release(struct soc_camera_device *icd)
 {
+        struct mt9m001 *mt9m001 = container_of(icd, struct mt9m001, icd);
+        struct soc_camera_link *icl = mt9m001->client->dev.platform_data;
+
         /* Disable the chip */
         reg_write(icd, MT9M001_OUTPUT_CONTROL, 0);
+
+        if (icl->power)
+                icl->power(&mt9m001->client->dev, 0);
+
         return 0;
 }
 
@@ -267,24 +294,24 @@ static int mt9m001_set_fmt_cap(struct soc_camera_device *icd,
 
         /* Blanking and start values - default... */
         ret = reg_write(icd, MT9M001_HORIZONTAL_BLANKING, hblank);
-        if (ret >= 0)
+        if (!ret)
                 ret = reg_write(icd, MT9M001_VERTICAL_BLANKING, vblank);
 
         /* The caller provides a supported format, as verified per
          * call to icd->try_fmt_cap() */
-        if (ret >= 0)
+        if (!ret)
                 ret = reg_write(icd, MT9M001_COLUMN_START, rect->left);
-        if (ret >= 0)
+        if (!ret)
                 ret = reg_write(icd, MT9M001_ROW_START, rect->top);
-        if (ret >= 0)
+        if (!ret)
                 ret = reg_write(icd, MT9M001_WINDOW_WIDTH, rect->width - 1);
-        if (ret >= 0)
+        if (!ret)
                 ret = reg_write(icd, MT9M001_WINDOW_HEIGHT,
                                 rect->height + icd->y_skip_top - 1);
-        if (ret >= 0 && mt9m001->autoexposure) {
+        if (!ret && mt9m001->autoexposure) {
                 ret = reg_write(icd, MT9M001_SHUTTER_WIDTH,
                                 rect->height + icd->y_skip_top + vblank);
-                if (ret >= 0) {
+                if (!ret) {
                         const struct v4l2_queryctrl *qctrl =
                                 soc_camera_find_qctrl(icd->ops,
                                                       V4L2_CID_EXPOSURE);
@@ -295,7 +322,7 @@ static int mt9m001_set_fmt_cap(struct soc_camera_device *icd,
                 }
         }
 
-        return ret < 0 ? ret : 0;
+        return ret;
 }
 
 static int mt9m001_try_fmt_cap(struct soc_camera_device *icd,