1 files changed, 28 insertions, 3 deletions

diff --git a/drivers/media/i2c/soc_camera/mt9v022.c b/drivers/media/i2c/soc_camera/mt9v022.c
index a5210e4d67af..5bc5a9286fd5 100644
--- a/drivers/media/i2c/soc_camera/mt9v022.c
+++ b/drivers/media/i2c/soc_camera/mt9v022.c
@@ -50,6 +50,7 @@ MODULE_PARM_DESC(sensor_type, "Sensor type: \"colour\" or \"monochrome\"");
 #define MT9V022_PIXEL_OPERATION_MODE	0x0f
 #define MT9V022_LED_OUT_CONTROL		0x1b
 #define MT9V022_ADC_MODE_CONTROL	0x1c
+#define MT9V022_REG32			0x20
 #define MT9V022_ANALOG_GAIN		0x35
 #define MT9V022_BLACK_LEVEL_CALIB_CTRL	0x47
 #define MT9V022_PIXCLK_FV_LV		0x74
@@ -78,7 +79,8 @@ MODULE_PARM_DESC(sensor_type, "Sensor type: \"colour\" or \"monochrome\"");
 #define MT9V022_VERTICAL_BLANKING_MAX	3000
 #define MT9V022_VERTICAL_BLANKING_DEF	45
 
-#define is_mt9v024(id) (id == 0x1324)
+#define is_mt9v022_rev3(id)	(id == 0x1313)
+#define is_mt9v024(id)		(id == 0x1324)
 
 /* MT9V022 has only one fixed colorspace per pixelcode */
 struct mt9v022_datafmt {
@@ -152,6 +154,7 @@ struct mt9v022 {
 	int num_fmts;
 	int model;	/* V4L2_IDENT_MT9V022* codes from v4l2-chip-ident.h */
 	u16 chip_control;
+	u16 chip_version;
 	unsigned short y_skip_top;	/* Lines to skip at the top */
 };
 
@@ -234,12 +237,32 @@ static int mt9v022_s_stream(struct v4l2_subdev *sd, int enable)
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct mt9v022 *mt9v022 = to_mt9v022(client);
 
-	if (enable)
+	if (enable) {
 		/* Switch to master "normal" mode */
 		mt9v022->chip_control &= ~0x10;
-	else
+		if (is_mt9v022_rev3(mt9v022->chip_version) ||
+		    is_mt9v024(mt9v022->chip_version)) {
+			/*
+			 * Unset snapshot mode specific settings: clear bit 9
+			 * and bit 2 in reg. 0x20 when in normal mode.
+			 */
+			if (reg_clear(client, MT9V022_REG32, 0x204))
+				return -EIO;
+		}
+	} else {
 		/* Switch to snapshot mode */
 		mt9v022->chip_control |= 0x10;
+		if (is_mt9v022_rev3(mt9v022->chip_version) ||
+		    is_mt9v024(mt9v022->chip_version)) {
+			/*
+			 * Required settings for snapshot mode: set bit 9
+			 * (RST enable) and bit 2 (CR enable) in reg. 0x20
+			 * See TechNote TN0960 or TN-09-225.
+			 */
+			if (reg_set(client, MT9V022_REG32, 0x204))
+				return -EIO;
+		}
+	}
 
 	if (reg_write(client, MT9V022_CHIP_CONTROL, mt9v022->chip_control) < 0)
 		return -EIO;
@@ -646,6 +669,8 @@ static int mt9v022_video_probe(struct i2c_client *client)
 		goto ei2c;
 	}
 
+	mt9v022->chip_version = data;
+
 	mt9v022->reg = is_mt9v024(data) ? &mt9v024_register :
 			&mt9v022_register;