From fbb943e35b519549eac8ee17bf20d651388a27dd Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dave.stevenson@raspberrypi.com>
Date: Wed, 29 Apr 2020 21:39:58 +0100
Subject: [PATCH] media: i2c: ov5647: Add support for V4L2_CID_VBLANK

Adds vblank control to allow for frame rate control.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
---
 drivers/media/i2c/ov5647.c | 65 ++++++++++++++++++++++++++++++++------
 1 file changed, 55 insertions(+), 10 deletions(-)

--- a/drivers/media/i2c/ov5647.c
+++ b/drivers/media/i2c/ov5647.c
@@ -61,6 +61,8 @@
 #define OV5647_REG_AEC_AGC		0x3503
 #define OV5647_REG_GAIN_HI		0x350A
 #define OV5647_REG_GAIN_LO		0x350B
+#define OV5647_REG_VTS_HI		0x380e
+#define OV5647_REG_VTS_LO		0x380f
 #define OV5647_REG_FRAME_OFF_NUMBER	0x4202
 #define OV5647_REG_MIPI_CTRL00		0x4800
 #define OV5647_REG_MIPI_CTRL14		0x4814
@@ -79,6 +81,9 @@
 #define OV5647_PIXEL_ARRAY_WIDTH	2592U
 #define OV5647_PIXEL_ARRAY_HEIGHT	1944U
 
+#define OV5647_VBLANK_MIN		4
+#define OV5647_VTS_MAX			32767
+
 struct regval_list {
 	u16 addr;
 	u8 data;
@@ -92,6 +97,8 @@ struct ov5647_mode {
 	u64 pixel_rate;
 	/* HTS as defined in the register set (0x380C/0x380D) */
 	int hts;
+	/* Default VTS value for this mode */
+	int vts_def;
 
 	struct regval_list		*reg_list;
 	unsigned int			num_regs;
@@ -109,6 +116,7 @@ struct ov5647 {
 	struct v4l2_ctrl_handler	ctrls;
 	struct v4l2_ctrl		*pixel_rate;
 	struct v4l2_ctrl		*hblank;
+	struct v4l2_ctrl		*vblank;
 	bool				write_mode_regs;
 };
 
@@ -161,8 +169,6 @@ static struct regval_list ov5647_640x480
 	{0x3b07, 0x0c},
 	{0x380c, 0x07},
 	{0x380d, 0x68},
-	{0x380e, 0x03},
-	{0x380f, 0xd8},
 	{0x3814, 0x31},
 	{0x3815, 0x31},
 	{0x3708, 0x64},
@@ -251,8 +257,6 @@ static struct regval_list ov5647_2592x19
 	{0x3b07, 0x0c},
 	{0x380c, 0x0b},
 	{0x380d, 0x1c},
-	{0x380e, 0x07},
-	{0x380f, 0xb0},
 	{0x3814, 0x11},
 	{0x3815, 0x11},
 	{0x3708, 0x64},
@@ -342,8 +346,6 @@ static struct regval_list ov5647_1080p30
 	{0x3b07, 0x0c},
 	{0x380c, 0x09},
 	{0x380d, 0x70},
-	{0x380e, 0x04},
-	{0x380f, 0x50},
 	{0x3814, 0x11},
 	{0x3815, 0x11},
 	{0x3708, 0x64},
@@ -485,8 +487,6 @@ static struct regval_list ov5647_2x2binn
 	{0x3503, 0x03},
 	{0x3820, 0x41},
 	{0x3821, 0x07},
-	{0x380E, 0x05},
-	{0x380F, 0x9B},
 	{0x350A, 0x00},
 	{0x350B, 0x10},
 	{0x3500, 0x00},
@@ -520,8 +520,6 @@ static struct regval_list ov5647_640x480
 	{0x3b07, 0x0c},
 	{0x380c, 0x07},
 	{0x380d, 0x3c},
-	{0x380e, 0x01},
-	{0x380f, 0xf8},
 	{0x3814, 0x35},
 	{0x3815, 0x35},
 	{0x3708, 0x64},
@@ -609,6 +607,7 @@ static struct ov5647_mode supported_mode
 		},
 		.pixel_rate = 77291670,
 		.hts = 1896,
+		.vts_def = 0x3d8,
 		ov5647_640x480_8bit,
 		ARRAY_SIZE(ov5647_640x480_8bit)
 	},
@@ -634,6 +633,7 @@ static struct ov5647_mode supported_mode
 		},
 		.pixel_rate = 87500000,
 		.hts = 2844,
+		.vts_def = 0x7b0,
 		ov5647_2592x1944_10bit,
 		ARRAY_SIZE(ov5647_2592x1944_10bit)
 	},
@@ -657,6 +657,7 @@ static struct ov5647_mode supported_mode
 		},
 		.pixel_rate = 81666700,
 		.hts = 2416,
+		.vts_def = 0x450,
 		ov5647_1080p30_10bit,
 		ARRAY_SIZE(ov5647_1080p30_10bit)
 	},
@@ -679,6 +680,7 @@ static struct ov5647_mode supported_mode
 		},
 		.pixel_rate = 81666700,
 		.hts = 1896,
+		.vts_def = 0x59b,
 		ov5647_2x2binned_10bit,
 		ARRAY_SIZE(ov5647_2x2binned_10bit)
 	},
@@ -702,6 +704,7 @@ static struct ov5647_mode supported_mode
 		},
 		.pixel_rate = 55000000,
 		.hts = 1852,
+		.vts_def = 0x1f8,
 		ov5647_640x480_10bit,
 		ARRAY_SIZE(ov5647_640x480_10bit)
 	},
@@ -710,6 +713,29 @@ static struct ov5647_mode supported_mode
 /* Use 2x2 binned 10-bit mode as default. */
 #define OV5647_DEFAULT_MODE (&supported_modes_10bit[2])
 
+static int ov5647_write16(struct v4l2_subdev *sd, u16 reg, u16 val)
+{
+	int ret;
+	unsigned char data[4] = { reg >> 8, reg & 0xff, val >> 8, val & 0xff};
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	ret = i2c_master_send(client, data, 4);
+	/*
+	 * Writing the wrong number of bytes also needs to be flagged as an
+	 * error. Success needs to produce a 0 return code.
+	 */
+	if (ret == 4) {
+		ret = 0;
+	} else {
+		dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
+			__func__, reg);
+		if (ret >= 0)
+			ret = -EINVAL;
+	}
+
+	return ret;
+}
+
 static int ov5647_write(struct v4l2_subdev *sd, u16 reg, u8 val)
 {
 	int ret;
@@ -1189,6 +1215,14 @@ static int ov5647_set_fmt(struct v4l2_su
 		hblank = mode->hts - mode->format.width;
 		__v4l2_ctrl_modify_range(state->hblank, hblank, hblank, 1,
 					 hblank);
+
+		__v4l2_ctrl_modify_range(state->vblank,
+					 OV5647_VBLANK_MIN,
+					 OV5647_VTS_MAX - mode->format.height,
+					 1,
+					 mode->vts_def - mode->format.height);
+		__v4l2_ctrl_s_ctrl(state->vblank,
+				   mode->vts_def - mode->format.height);
 	}
 
 	mutex_unlock(&state->lock);
@@ -1411,6 +1445,10 @@ static int ov5647_s_ctrl(struct v4l2_ctr
 	case V4L2_CID_HBLANK:
 		/* Read-only, but we adjust it based on mode. */
 		break;
+	case V4L2_CID_VBLANK:
+		ret = ov5647_write16(sd, OV5647_REG_VTS_HI,
+				     state->mode->format.height + ctrl->val);
+		break;
 	default:
 		dev_info(&client->dev,
 			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
@@ -1519,6 +1557,13 @@ static int ov5647_probe(struct i2c_clien
 					   hblank);
 	sensor->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
+	sensor->vblank = v4l2_ctrl_new_std(&sensor->ctrls, &ov5647_ctrl_ops,
+					   V4L2_CID_VBLANK, OV5647_VBLANK_MIN,
+					   OV5647_VTS_MAX -
+						sensor->mode->format.height, 1,
+					   sensor->mode->vts_def -
+						sensor->mode->format.height);
+
 	if (sensor->ctrls.error) {
 		ret = sensor->ctrls.error;
 		dev_err(&client->dev, "%s control init failed (%d)\n",