media: ov7670: hook s_power onto v4l2 core

The commit 71862f63f351 ("media: ov7670: Add the ov7670_s_power function")
added a power control routing. However, it was not good enough to use as
a s_power() callback: it merely flipped on the power GPIOs without
restoring the register settings.

Fix this now and register an actual power callback.

Signed-off-by: Lubomir Rintel <lkundrak@v3.sk>
Signed-off-by: Sakari Ailus <sakari.ailus@linux.intel.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab+samsung@kernel.org>

diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index 6ab88626c760ec9d5fe1d28fd64dec2407f9a8a7..a5b591dde10c9fab7b85b61c71b5c9efb8e141ca 100644 (file)
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -243,6 +243,7 @@ struct ov7670_info {
        struct ov7670_format_struct *fmt;  /* Current format */
        struct ov7670_win_size *wsize;
        struct clk *clk;
+       int on;
        struct gpio_desc *resetb_gpio;
        struct gpio_desc *pwdn_gpio;
        unsigned int mbus_config;       /* Media bus configuration flags */
@@ -1617,19 +1618,54 @@ static int ov7670_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_regis
 }
 #endif
 
-static int ov7670_s_power(struct v4l2_subdev *sd, int on)
+static void ov7670_power_on(struct v4l2_subdev *sd)
 {
        struct ov7670_info *info = to_state(sd);
 
+       if (info->on)
+               return;
+
        if (info->pwdn_gpio)
-               gpiod_set_value(info->pwdn_gpio, !on);
-       if (on && info->resetb_gpio) {
+               gpiod_set_value(info->pwdn_gpio, 0);
+       if (info->resetb_gpio) {
                gpiod_set_value(info->resetb_gpio, 1);
                usleep_range(500, 1000);
                gpiod_set_value(info->resetb_gpio, 0);
                usleep_range(3000, 5000);
        }
 
+       info->on = true;
+}
+
+static void ov7670_power_off(struct v4l2_subdev *sd)
+{
+       struct ov7670_info *info = to_state(sd);
+
+       if (!info->on)
+               return;
+
+       if (info->pwdn_gpio)
+               gpiod_set_value(info->pwdn_gpio, 1);
+
+       info->on = false;
+}
+
+static int ov7670_s_power(struct v4l2_subdev *sd, int on)
+{
+       struct ov7670_info *info = to_state(sd);
+
+       if (info->on == on)
+               return 0;
+
+       if (on) {
+               ov7670_power_on (sd);
+               ov7670_apply_fmt(sd);
+               ov7675_apply_framerate(sd);
+               v4l2_ctrl_handler_setup(&info->hdl);
+       } else {
+               ov7670_power_off (sd);
+       }
+
        return 0;
 }
 
@@ -1662,6 +1698,7 @@ static int ov7670_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 static const struct v4l2_subdev_core_ops ov7670_core_ops = {
        .reset = ov7670_reset,
        .init = ov7670_init,
+       .s_power = ov7670_s_power,
        .log_status = v4l2_ctrl_subdev_log_status,
        .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
        .unsubscribe_event = v4l2_event_subdev_unsubscribe,
@@ -1826,6 +1863,7 @@ static int ov7670_probe(struct i2c_client *client,
                else
                        return ret;
        }
+
        if (info->clk) {
                ret = clk_prepare_enable(info->clk);
                if (ret)
@@ -1842,7 +1880,7 @@ static int ov7670_probe(struct i2c_client *client,
        if (ret)
                goto clk_disable;
 
-       ov7670_s_power(sd, 1);
+       ov7670_power_on(sd);
 
        /* Make sure it's an ov7670 */
        ret = ov7670_detect(sd);
@@ -1930,7 +1968,7 @@ entity_cleanup:
 hdl_free:
        v4l2_ctrl_handler_free(&info->hdl);
 power_off:
-       ov7670_s_power(sd, 0);
+       ov7670_power_off(sd);
 clk_disable:
        clk_disable_unprepare(info->clk);
        return ret;
@@ -1946,7 +1984,7 @@ static int ov7670_remove(struct i2c_client *client)
        v4l2_ctrl_handler_free(&info->hdl);
        clk_disable_unprepare(info->clk);
        media_entity_cleanup(&info->sd.entity);
-       ov7670_s_power(sd, 0);
+       ov7670_power_off(sd);
        return 0;
 }