[PATCH v2 2/4] media: ov5640: check chip id

[Hugues Fruchet <hugues.fruchet@st.com>]

Verify that chip identifier is correct before starting streaming

Signed-off-by: Hugues Fruchet <hugues.fruchet@st.com>
---
 drivers/media/i2c/ov5640.c | 30 +++++++++++++++++++++++++++++-
 1 file changed, 29 insertions(+), 1 deletion(-)

diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
index 61071f5..a576d11 100644
--- a/drivers/media/i2c/ov5640.c
+++ b/drivers/media/i2c/ov5640.c
@@ -34,7 +34,8 @@
 
 #define OV5640_DEFAULT_SLAVE_ID 0x3c
 
-#define OV5640_REG_CHIP_ID		0x300a
+#define OV5640_REG_CHIP_ID_HIGH		0x300a
+#define OV5640_REG_CHIP_ID_LOW		0x300b
 #define OV5640_REG_PAD_OUTPUT00		0x3019
 #define OV5640_REG_SC_PLL_CTRL0		0x3034
 #define OV5640_REG_SC_PLL_CTRL1		0x3035
@@ -926,6 +927,29 @@ static int ov5640_load_regs(struct ov5640_dev *sensor,
 	return ret;
 }
 
+static int ov5640_check_chip_id(struct ov5640_dev *sensor)
+{
+	struct i2c_client *client = sensor->i2c_client;
+	int ret;
+	u8 chip_id_h, chip_id_l;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_CHIP_ID_HIGH, &chip_id_h);
+	if (ret)
+		return ret;
+
+	ret = ov5640_read_reg(sensor, OV5640_REG_CHIP_ID_LOW, &chip_id_l);
+	if (ret)
+		return ret;
+
+	if (!(chip_id_h == 0x56 && chip_id_l == 0x40)) {
+		dev_err(&client->dev, "%s: wrong chip identifier, expected 0x5640, got 0x%x%x\n",
+			__func__, chip_id_h, chip_id_l);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 /* read exposure, in number of line periods */
 static int ov5640_get_exposure(struct ov5640_dev *sensor)
 {
@@ -1562,6 +1586,10 @@ static int ov5640_set_power(struct ov5640_dev *sensor, bool on)
 		ov5640_reset(sensor);
 		ov5640_power(sensor, true);
 
+		ret = ov5640_check_chip_id(sensor);
+		if (ret)
+			goto power_off;
+
 		ret = ov5640_init_slave_id(sensor);
 		if (ret)
 			goto power_off;
-- 
1.9.1