@@ -80,6 +80,8 @@
#define MT9P031_PIXEL_CLOCK_SHIFT(n) ((n) << 8)
#define MT9P031_PIXEL_CLOCK_DIVIDE(n) ((n) << 0)
#define MT9P031_FRAME_RESTART 0x0b
+#define MT9P031_FRAME_RESTART_SET (1 << 0)
+#define MT9P031_FRAME_PAUSE_RESTART_SET (1 << 1)
#define MT9P031_SHUTTER_DELAY 0x0c
#define MT9P031_RST 0x0d
#define MT9P031_RST_ENABLE 1
@@ -483,9 +485,25 @@ static int mt9p031_set_params(struct mt9p031 *mt9p031)
static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
{
struct mt9p031 *mt9p031 = to_mt9p031(subdev);
+ struct i2c_client *client = v4l2_get_subdevdata(subdev);
+ int val;
int ret;
if (!enable) {
+ val = mt9p031_read(client, MT9P031_FRAME_RESTART);
+
+ /* enable pause restart */
+ val |= MT9P031_FRAME_PAUSE_RESTART_SET;
+ ret = mt9p031_write(client, MT9P031_FRAME_RESTART, val);
+ if (ret < 0)
+ return ret;
+
+ /* enable restart + keep pause restart set */
+ val |= MT9P031_FRAME_RESTART_SET;
+ ret = mt9p031_write(client, MT9P031_FRAME_RESTART, val);
+ if (ret < 0)
+ return ret;
+
/* Stop sensor readout */
ret = mt9p031_set_output_control(mt9p031,
MT9P031_OUTPUT_CONTROL_CEN, 0);
@@ -505,6 +523,13 @@ static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
if (ret < 0)
return ret;
+ val = mt9p031_read(client, MT9P031_FRAME_RESTART);
+ /* disable reset + pause restart */
+ val &= ~MT9P031_FRAME_PAUSE_RESTART_SET;
+ ret = mt9p031_write(client, MT9P031_FRAME_RESTART, val);
+ if (ret < 0)
+ return ret;
+
return mt9p031_pll_enable(mt9p031);
}