diff mbox series

[v2,2/3] media: mt9m111: fix device power usage

Message ID 20220916135713.143890-2-m.felsch@pengutronix.de (mailing list archive)
State New, archived
Headers show
Series [v2,1/3] media: mt9m111: add V4L2_CID_LINK_FREQ support | expand

Commit Message

Marco Felsch Sept. 16, 2022, 1:57 p.m. UTC
Currently the driver turn off the power after probe and toggle it during
.stream by using the .s_power callback. This is problematic since other
callbacks like .set_fmt accessing the hardware as well which will fail.
So in the end the default format is the only supported format.

Remove the hardware register access from the callbacks and instead sync
the state once right before the stream gets enabled to fix this for
.set_fmt() and .set_selection(). For the debug register access we need
to turn the device on/off before accessing the registers to fix this and
finally for the ctrls access we need the device to be powered to fix
this.

Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
---
Changelog:

v2:
- squash patch 2 and 3
---
 drivers/media/i2c/mt9m111.c | 40 ++++++++++++++++++++-----------------
 1 file changed, 22 insertions(+), 18 deletions(-)

Comments

Sakari Ailus Sept. 19, 2022, 12:45 p.m. UTC | #1
Hi Marco,

On Fri, Sep 16, 2022 at 03:57:12PM +0200, Marco Felsch wrote:
> @@ -758,10 +751,14 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
>  	if (reg->reg > 0x2ff)
>  		return -EINVAL;
>  
> +	mt9m111_s_power(sd, 1);

It would be nice to have this driver converted to runtime PM. Up to you.
Laurent Pinchart Sept. 19, 2022, 12:55 p.m. UTC | #2
On Mon, Sep 19, 2022 at 12:45:42PM +0000, Sakari Ailus wrote:
> Hi Marco,
> 
> On Fri, Sep 16, 2022 at 03:57:12PM +0200, Marco Felsch wrote:
> > @@ -758,10 +751,14 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
> >  	if (reg->reg > 0x2ff)
> >  		return -EINVAL;
> >  
> > +	mt9m111_s_power(sd, 1);
> 
> It would be nice to have this driver converted to runtime PM. Up to you.

I second that :-)
Marco Felsch Sept. 19, 2022, 1:10 p.m. UTC | #3
On 22-09-19, Laurent Pinchart wrote:
> On Mon, Sep 19, 2022 at 12:45:42PM +0000, Sakari Ailus wrote:
> > Hi Marco,
> > 
> > On Fri, Sep 16, 2022 at 03:57:12PM +0200, Marco Felsch wrote:
> > > @@ -758,10 +751,14 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
> > >  	if (reg->reg > 0x2ff)
> > >  		return -EINVAL;
> > >  
> > > +	mt9m111_s_power(sd, 1);
> > 
> > It would be nice to have this driver converted to runtime PM. Up to you.
> 
> I second that :-)

I would rather keep it this way and add 2nd patch to change. So it would
be easier to backport the patch.

Regards,
  Marco

> 
> -- 
> Regards,
> 
> Laurent Pinchart
>
Sakari Ailus Sept. 19, 2022, 1:13 p.m. UTC | #4
On Mon, Sep 19, 2022 at 03:10:09PM +0200, Marco Felsch wrote:
> On 22-09-19, Laurent Pinchart wrote:
> > On Mon, Sep 19, 2022 at 12:45:42PM +0000, Sakari Ailus wrote:
> > > Hi Marco,
> > > 
> > > On Fri, Sep 16, 2022 at 03:57:12PM +0200, Marco Felsch wrote:
> > > > @@ -758,10 +751,14 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
> > > >  	if (reg->reg > 0x2ff)
> > > >  		return -EINVAL;
> > > >  
> > > > +	mt9m111_s_power(sd, 1);
> > > 
> > > It would be nice to have this driver converted to runtime PM. Up to you.
> > 
> > I second that :-)
> 
> I would rather keep it this way and add 2nd patch to change. So it would
> be easier to backport the patch.

Works for me.
Jacopo Mondi Sept. 20, 2022, 10:27 a.m. UTC | #5
Hi Marco

On Fri, Sep 16, 2022 at 03:57:12PM +0200, Marco Felsch wrote:
> Currently the driver turn off the power after probe and toggle it during
> .stream by using the .s_power callback. This is problematic since other
> callbacks like .set_fmt accessing the hardware as well which will fail.
> So in the end the default format is the only supported format.
>
> Remove the hardware register access from the callbacks and instead sync
> the state once right before the stream gets enabled to fix this for
> .set_fmt() and .set_selection(). For the debug register access we need
> to turn the device on/off before accessing the registers to fix this and
> finally for the ctrls access we need the device to be powered to fix
> this.
>
> Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
> ---
> Changelog:
>
> v2:
> - squash patch 2 and 3
> ---
>  drivers/media/i2c/mt9m111.c | 40 ++++++++++++++++++++-----------------
>  1 file changed, 22 insertions(+), 18 deletions(-)
>
> diff --git a/drivers/media/i2c/mt9m111.c b/drivers/media/i2c/mt9m111.c
> index 52be1c310455..8de93ab99cbc 100644
> --- a/drivers/media/i2c/mt9m111.c
> +++ b/drivers/media/i2c/mt9m111.c
> @@ -455,7 +455,7 @@ static int mt9m111_set_selection(struct v4l2_subdev *sd,
>  	struct mt9m111 *mt9m111 = to_mt9m111(client);
>  	struct v4l2_rect rect = sel->r;
>  	int width, height;
> -	int ret, align = 0;
> +	int align = 0;
>
>  	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
>  	    sel->target != V4L2_SEL_TGT_CROP)
> @@ -481,14 +481,11 @@ static int mt9m111_set_selection(struct v4l2_subdev *sd,
>  	width = min(mt9m111->width, rect.width);
>  	height = min(mt9m111->height, rect.height);
>
> -	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
> -	if (!ret) {
> -		mt9m111->rect = rect;
> -		mt9m111->width = width;
> -		mt9m111->height = height;
> -	}
> +	mt9m111->rect = rect;
> +	mt9m111->width = width;
> +	mt9m111->height = height;
>
> -	return ret;
> +	return 0;
>  }
>
>  static int mt9m111_get_selection(struct v4l2_subdev *sd,
> @@ -632,7 +629,6 @@ static int mt9m111_set_fmt(struct v4l2_subdev *sd,
>  	const struct mt9m111_datafmt *fmt;
>  	struct v4l2_rect *rect = &mt9m111->rect;
>  	bool bayer;
> -	int ret;
>
>  	if (mt9m111->is_streaming)
>  		return -EBUSY;
> @@ -681,16 +677,11 @@ static int mt9m111_set_fmt(struct v4l2_subdev *sd,
>  		return 0;
>  	}
>
> -	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
> -	if (!ret)
> -		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
> -	if (!ret) {
> -		mt9m111->width	= mf->width;
> -		mt9m111->height	= mf->height;
> -		mt9m111->fmt	= fmt;
> -	}
> +	mt9m111->width	= mf->width;
> +	mt9m111->height	= mf->height;
> +	mt9m111->fmt	= fmt;
>
> -	return ret;
> +	return 0;
>  }
>
>  static const struct mt9m111_mode_info *
> @@ -748,6 +739,8 @@ mt9m111_find_mode(struct mt9m111 *mt9m111, unsigned int req_fps,
>  	return mode;
>  }
>
> +static int mt9m111_s_power(struct v4l2_subdev *sd, int on);
> +
>  #ifdef CONFIG_VIDEO_ADV_DEBUG
>  static int mt9m111_g_register(struct v4l2_subdev *sd,
>  			      struct v4l2_dbg_register *reg)
> @@ -758,10 +751,14 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
>  	if (reg->reg > 0x2ff)
>  		return -EINVAL;
>
> +	mt9m111_s_power(sd, 1);
> +
>  	val = mt9m111_reg_read(client, reg->reg);
>  	reg->size = 2;
>  	reg->val = (u64)val;
>
> +	mt9m111_s_power(sd, 0);
> +
>  	if (reg->val > 0xffff)
>  		return -EIO;
>
> @@ -776,9 +773,13 @@ static int mt9m111_s_register(struct v4l2_subdev *sd,
>  	if (reg->reg > 0x2ff)
>  		return -EINVAL;
>
> +	mt9m111_s_power(sd, 1);
> +
>  	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
>  		return -EIO;
>
> +	mt9m111_s_power(sd, 0);
> +

So far so good

>  	return 0;
>  }
>  #endif
> @@ -891,6 +892,9 @@ static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
>  	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
>  					       struct mt9m111, hdl);
>
> +	if (!mt9m111->is_streaming)
> +		return 0;
> +

If you refuse to set controls if the sensor is not streaming (ie
powered) which I think it's right, shouldn't you call
__v4l2_ctrl_handler_setup() at s_stream(1) time to have the controls
applied ?

>  	switch (ctrl->id) {
>  	case V4L2_CID_VFLIP:
>  		return mt9m111_set_flip(mt9m111, ctrl->val,
> --
> 2.30.2
>
Jacopo Mondi Sept. 20, 2022, 10:34 a.m. UTC | #6
Hi Marco

On Tue, Sep 20, 2022 at 12:27:04PM +0200, Jacopo Mondi wrote:
> Hi Marco
>
> On Fri, Sep 16, 2022 at 03:57:12PM +0200, Marco Felsch wrote:
> > Currently the driver turn off the power after probe and toggle it during
> > .stream by using the .s_power callback. This is problematic since other
> > callbacks like .set_fmt accessing the hardware as well which will fail.
> > So in the end the default format is the only supported format.
> >
> > Remove the hardware register access from the callbacks and instead sync
> > the state once right before the stream gets enabled to fix this for
> > .set_fmt() and .set_selection(). For the debug register access we need
> > to turn the device on/off before accessing the registers to fix this and
> > finally for the ctrls access we need the device to be powered to fix
> > this.
> >
> > Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
> > ---
> > Changelog:
> >
> > v2:
> > - squash patch 2 and 3
> > ---
> >  drivers/media/i2c/mt9m111.c | 40 ++++++++++++++++++++-----------------
> >  1 file changed, 22 insertions(+), 18 deletions(-)
> >
> > diff --git a/drivers/media/i2c/mt9m111.c b/drivers/media/i2c/mt9m111.c
> > index 52be1c310455..8de93ab99cbc 100644
> > --- a/drivers/media/i2c/mt9m111.c
> > +++ b/drivers/media/i2c/mt9m111.c
> > @@ -455,7 +455,7 @@ static int mt9m111_set_selection(struct v4l2_subdev *sd,
> >  	struct mt9m111 *mt9m111 = to_mt9m111(client);
> >  	struct v4l2_rect rect = sel->r;
> >  	int width, height;
> > -	int ret, align = 0;
> > +	int align = 0;
> >
> >  	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
> >  	    sel->target != V4L2_SEL_TGT_CROP)
> > @@ -481,14 +481,11 @@ static int mt9m111_set_selection(struct v4l2_subdev *sd,
> >  	width = min(mt9m111->width, rect.width);
> >  	height = min(mt9m111->height, rect.height);
> >
> > -	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
> > -	if (!ret) {
> > -		mt9m111->rect = rect;
> > -		mt9m111->width = width;
> > -		mt9m111->height = height;
> > -	}
> > +	mt9m111->rect = rect;
> > +	mt9m111->width = width;
> > +	mt9m111->height = height;
> >
> > -	return ret;
> > +	return 0;
> >  }
> >
> >  static int mt9m111_get_selection(struct v4l2_subdev *sd,
> > @@ -632,7 +629,6 @@ static int mt9m111_set_fmt(struct v4l2_subdev *sd,
> >  	const struct mt9m111_datafmt *fmt;
> >  	struct v4l2_rect *rect = &mt9m111->rect;
> >  	bool bayer;
> > -	int ret;
> >
> >  	if (mt9m111->is_streaming)
> >  		return -EBUSY;
> > @@ -681,16 +677,11 @@ static int mt9m111_set_fmt(struct v4l2_subdev *sd,
> >  		return 0;
> >  	}
> >
> > -	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
> > -	if (!ret)
> > -		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
> > -	if (!ret) {
> > -		mt9m111->width	= mf->width;
> > -		mt9m111->height	= mf->height;
> > -		mt9m111->fmt	= fmt;
> > -	}
> > +	mt9m111->width	= mf->width;
> > +	mt9m111->height	= mf->height;
> > +	mt9m111->fmt	= fmt;
> >
> > -	return ret;
> > +	return 0;
> >  }
> >
> >  static const struct mt9m111_mode_info *
> > @@ -748,6 +739,8 @@ mt9m111_find_mode(struct mt9m111 *mt9m111, unsigned int req_fps,
> >  	return mode;
> >  }
> >
> > +static int mt9m111_s_power(struct v4l2_subdev *sd, int on);
> > +
> >  #ifdef CONFIG_VIDEO_ADV_DEBUG
> >  static int mt9m111_g_register(struct v4l2_subdev *sd,
> >  			      struct v4l2_dbg_register *reg)
> > @@ -758,10 +751,14 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
> >  	if (reg->reg > 0x2ff)
> >  		return -EINVAL;
> >
> > +	mt9m111_s_power(sd, 1);
> > +
> >  	val = mt9m111_reg_read(client, reg->reg);
> >  	reg->size = 2;
> >  	reg->val = (u64)val;
> >
> > +	mt9m111_s_power(sd, 0);
> > +
> >  	if (reg->val > 0xffff)
> >  		return -EIO;
> >
> > @@ -776,9 +773,13 @@ static int mt9m111_s_register(struct v4l2_subdev *sd,
> >  	if (reg->reg > 0x2ff)
> >  		return -EINVAL;
> >
> > +	mt9m111_s_power(sd, 1);
> > +
> >  	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
> >  		return -EIO;
> >
> > +	mt9m111_s_power(sd, 0);
> > +
>
> So far so good
>
> >  	return 0;
> >  }
> >  #endif
> > @@ -891,6 +892,9 @@ static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
> >  	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
> >  					       struct mt9m111, hdl);
> >
> > +	if (!mt9m111->is_streaming)
> > +		return 0;
> > +
>
> If you refuse to set controls if the sensor is not streaming (ie
> powered) which I think it's right, shouldn't you call
> __v4l2_ctrl_handler_setup() at s_stream(1) time to have the controls
> applied ?
>

Oh scratch that, it's in the s_power(1) call path. It's would be nicer
to have runtime_pm, but you already know that :)

Reviewed-by: Jacopo Mondi <jacopo@jmondi.org>

Thanks
   j

> >  	switch (ctrl->id) {
> >  	case V4L2_CID_VFLIP:
> >  		return mt9m111_set_flip(mt9m111, ctrl->val,
> > --
> > 2.30.2
> >
Laurent Pinchart Sept. 20, 2022, 10:36 a.m. UTC | #7
On Tue, Sep 20, 2022 at 12:34:12PM +0200, Jacopo Mondi wrote:
> On Tue, Sep 20, 2022 at 12:27:04PM +0200, Jacopo Mondi wrote:
> > On Fri, Sep 16, 2022 at 03:57:12PM +0200, Marco Felsch wrote:
> > > Currently the driver turn off the power after probe and toggle it during
> > > .stream by using the .s_power callback. This is problematic since other
> > > callbacks like .set_fmt accessing the hardware as well which will fail.
> > > So in the end the default format is the only supported format.
> > >
> > > Remove the hardware register access from the callbacks and instead sync
> > > the state once right before the stream gets enabled to fix this for
> > > .set_fmt() and .set_selection(). For the debug register access we need
> > > to turn the device on/off before accessing the registers to fix this and
> > > finally for the ctrls access we need the device to be powered to fix
> > > this.
> > >
> > > Signed-off-by: Marco Felsch <m.felsch@pengutronix.de>
> > > ---
> > > Changelog:
> > >
> > > v2:
> > > - squash patch 2 and 3
> > > ---
> > >  drivers/media/i2c/mt9m111.c | 40 ++++++++++++++++++++-----------------
> > >  1 file changed, 22 insertions(+), 18 deletions(-)
> > >
> > > diff --git a/drivers/media/i2c/mt9m111.c b/drivers/media/i2c/mt9m111.c
> > > index 52be1c310455..8de93ab99cbc 100644
> > > --- a/drivers/media/i2c/mt9m111.c
> > > +++ b/drivers/media/i2c/mt9m111.c
> > > @@ -455,7 +455,7 @@ static int mt9m111_set_selection(struct v4l2_subdev *sd,
> > >  	struct mt9m111 *mt9m111 = to_mt9m111(client);
> > >  	struct v4l2_rect rect = sel->r;
> > >  	int width, height;
> > > -	int ret, align = 0;
> > > +	int align = 0;
> > >
> > >  	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
> > >  	    sel->target != V4L2_SEL_TGT_CROP)
> > > @@ -481,14 +481,11 @@ static int mt9m111_set_selection(struct v4l2_subdev *sd,
> > >  	width = min(mt9m111->width, rect.width);
> > >  	height = min(mt9m111->height, rect.height);
> > >
> > > -	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
> > > -	if (!ret) {
> > > -		mt9m111->rect = rect;
> > > -		mt9m111->width = width;
> > > -		mt9m111->height = height;
> > > -	}
> > > +	mt9m111->rect = rect;
> > > +	mt9m111->width = width;
> > > +	mt9m111->height = height;
> > >
> > > -	return ret;
> > > +	return 0;
> > >  }
> > >
> > >  static int mt9m111_get_selection(struct v4l2_subdev *sd,
> > > @@ -632,7 +629,6 @@ static int mt9m111_set_fmt(struct v4l2_subdev *sd,
> > >  	const struct mt9m111_datafmt *fmt;
> > >  	struct v4l2_rect *rect = &mt9m111->rect;
> > >  	bool bayer;
> > > -	int ret;
> > >
> > >  	if (mt9m111->is_streaming)
> > >  		return -EBUSY;
> > > @@ -681,16 +677,11 @@ static int mt9m111_set_fmt(struct v4l2_subdev *sd,
> > >  		return 0;
> > >  	}
> > >
> > > -	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
> > > -	if (!ret)
> > > -		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
> > > -	if (!ret) {
> > > -		mt9m111->width	= mf->width;
> > > -		mt9m111->height	= mf->height;
> > > -		mt9m111->fmt	= fmt;
> > > -	}
> > > +	mt9m111->width	= mf->width;
> > > +	mt9m111->height	= mf->height;
> > > +	mt9m111->fmt	= fmt;
> > >
> > > -	return ret;
> > > +	return 0;
> > >  }
> > >
> > >  static const struct mt9m111_mode_info *
> > > @@ -748,6 +739,8 @@ mt9m111_find_mode(struct mt9m111 *mt9m111, unsigned int req_fps,
> > >  	return mode;
> > >  }
> > >
> > > +static int mt9m111_s_power(struct v4l2_subdev *sd, int on);
> > > +
> > >  #ifdef CONFIG_VIDEO_ADV_DEBUG
> > >  static int mt9m111_g_register(struct v4l2_subdev *sd,
> > >  			      struct v4l2_dbg_register *reg)
> > > @@ -758,10 +751,14 @@ static int mt9m111_g_register(struct v4l2_subdev *sd,
> > >  	if (reg->reg > 0x2ff)
> > >  		return -EINVAL;
> > >
> > > +	mt9m111_s_power(sd, 1);
> > > +
> > >  	val = mt9m111_reg_read(client, reg->reg);
> > >  	reg->size = 2;
> > >  	reg->val = (u64)val;
> > >
> > > +	mt9m111_s_power(sd, 0);
> > > +
> > >  	if (reg->val > 0xffff)
> > >  		return -EIO;
> > >
> > > @@ -776,9 +773,13 @@ static int mt9m111_s_register(struct v4l2_subdev *sd,
> > >  	if (reg->reg > 0x2ff)
> > >  		return -EINVAL;
> > >
> > > +	mt9m111_s_power(sd, 1);
> > > +
> > >  	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
> > >  		return -EIO;
> > >
> > > +	mt9m111_s_power(sd, 0);
> > > +
> >
> > So far so good
> >
> > >  	return 0;
> > >  }
> > >  #endif
> > > @@ -891,6 +892,9 @@ static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
> > >  	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
> > >  					       struct mt9m111, hdl);
> > >
> > > +	if (!mt9m111->is_streaming)
> > > +		return 0;
> > > +
> >
> > If you refuse to set controls if the sensor is not streaming (ie
> > powered) which I think it's right, shouldn't you call
> > __v4l2_ctrl_handler_setup() at s_stream(1) time to have the controls
> > applied ?
> 
> Oh scratch that, it's in the s_power(1) call path. It's would be nicer
> to have runtime_pm, but you already know that :)

Runtime PM shouldn't be hard to implement, I'd really prefer that.

> Reviewed-by: Jacopo Mondi <jacopo@jmondi.org>
> 
> > >  	switch (ctrl->id) {
> > >  	case V4L2_CID_VFLIP:
> > >  		return mt9m111_set_flip(mt9m111, ctrl->val,
diff mbox series

Patch

diff --git a/drivers/media/i2c/mt9m111.c b/drivers/media/i2c/mt9m111.c
index 52be1c310455..8de93ab99cbc 100644
--- a/drivers/media/i2c/mt9m111.c
+++ b/drivers/media/i2c/mt9m111.c
@@ -455,7 +455,7 @@  static int mt9m111_set_selection(struct v4l2_subdev *sd,
 	struct mt9m111 *mt9m111 = to_mt9m111(client);
 	struct v4l2_rect rect = sel->r;
 	int width, height;
-	int ret, align = 0;
+	int align = 0;
 
 	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE ||
 	    sel->target != V4L2_SEL_TGT_CROP)
@@ -481,14 +481,11 @@  static int mt9m111_set_selection(struct v4l2_subdev *sd,
 	width = min(mt9m111->width, rect.width);
 	height = min(mt9m111->height, rect.height);
 
-	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
-	if (!ret) {
-		mt9m111->rect = rect;
-		mt9m111->width = width;
-		mt9m111->height = height;
-	}
+	mt9m111->rect = rect;
+	mt9m111->width = width;
+	mt9m111->height = height;
 
-	return ret;
+	return 0;
 }
 
 static int mt9m111_get_selection(struct v4l2_subdev *sd,
@@ -632,7 +629,6 @@  static int mt9m111_set_fmt(struct v4l2_subdev *sd,
 	const struct mt9m111_datafmt *fmt;
 	struct v4l2_rect *rect = &mt9m111->rect;
 	bool bayer;
-	int ret;
 
 	if (mt9m111->is_streaming)
 		return -EBUSY;
@@ -681,16 +677,11 @@  static int mt9m111_set_fmt(struct v4l2_subdev *sd,
 		return 0;
 	}
 
-	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
-	if (!ret)
-		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
-	if (!ret) {
-		mt9m111->width	= mf->width;
-		mt9m111->height	= mf->height;
-		mt9m111->fmt	= fmt;
-	}
+	mt9m111->width	= mf->width;
+	mt9m111->height	= mf->height;
+	mt9m111->fmt	= fmt;
 
-	return ret;
+	return 0;
 }
 
 static const struct mt9m111_mode_info *
@@ -748,6 +739,8 @@  mt9m111_find_mode(struct mt9m111 *mt9m111, unsigned int req_fps,
 	return mode;
 }
 
+static int mt9m111_s_power(struct v4l2_subdev *sd, int on);
+
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int mt9m111_g_register(struct v4l2_subdev *sd,
 			      struct v4l2_dbg_register *reg)
@@ -758,10 +751,14 @@  static int mt9m111_g_register(struct v4l2_subdev *sd,
 	if (reg->reg > 0x2ff)
 		return -EINVAL;
 
+	mt9m111_s_power(sd, 1);
+
 	val = mt9m111_reg_read(client, reg->reg);
 	reg->size = 2;
 	reg->val = (u64)val;
 
+	mt9m111_s_power(sd, 0);
+
 	if (reg->val > 0xffff)
 		return -EIO;
 
@@ -776,9 +773,13 @@  static int mt9m111_s_register(struct v4l2_subdev *sd,
 	if (reg->reg > 0x2ff)
 		return -EINVAL;
 
+	mt9m111_s_power(sd, 1);
+
 	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
 		return -EIO;
 
+	mt9m111_s_power(sd, 0);
+
 	return 0;
 }
 #endif
@@ -891,6 +892,9 @@  static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
 	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
 					       struct mt9m111, hdl);
 
+	if (!mt9m111->is_streaming)
+		return 0;
+
 	switch (ctrl->id) {
 	case V4L2_CID_VFLIP:
 		return mt9m111_set_flip(mt9m111, ctrl->val,