From a189a016325250d0dd40d98ac0616f96f98b834d Mon Sep 17 00:00:00 2001
From: Naushir Patuck <naush@raspberrypi.com>
Date: Fri, 8 May 2020 09:41:17 +0100
Subject: [PATCH] media: i2c: imx477: Add support for adaptive frame
 control

Use V4L2_CID_EXPOSURE_AUTO_PRIORITY to control if the driver should
automatically adjust the sensor frame length based on exposure time,
allowing variable frame rates and longer exposures.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
---
 drivers/media/i2c/imx477.c | 113 +++++++++++++++++++++++++++++--------
 1 file changed, 91 insertions(+), 22 deletions(-)

--- a/drivers/media/i2c/imx477.c
+++ b/drivers/media/i2c/imx477.c
@@ -1082,6 +1082,8 @@ struct imx477 {
 	struct v4l2_ctrl *hflip;
 	struct v4l2_ctrl *vblank;
 	struct v4l2_ctrl *hblank;
+	/* This ctrl allows automatic variable framerate */
+	struct v4l2_ctrl *exposure_auto;
 
 	/* Current mode */
 	const struct imx477_mode *mode;
@@ -1278,6 +1280,72 @@ static int imx477_open(struct v4l2_subde
 	return 0;
 }
 
+static int imx477_set_exposure(struct imx477 *imx477, unsigned int val)
+{
+	int ret;
+
+	ret = imx477_write_reg(imx477, IMX477_REG_EXPOSURE,
+			       IMX477_REG_VALUE_16BIT, val);
+
+	/* Setup the frame length in the case of auto framerate mode. */
+	if (imx477->exposure_auto->val) {
+		unsigned int frame_length, frame_length_max, frame_length_min;
+
+		frame_length_min = imx477->vblank->minimum +
+				   imx477->mode->height;
+		frame_length_max = imx477->vblank->maximum +
+				   imx477->mode->height;
+		frame_length = max(frame_length_min,
+				   val + IMX477_EXPOSURE_OFFSET);
+		frame_length = min(frame_length_max, frame_length);
+		ret += imx477_write_reg(imx477, IMX477_REG_FRAME_LENGTH,
+					IMX477_REG_VALUE_16BIT, frame_length);
+	}
+
+	return ret;
+}
+
+static void imx477_adjust_exposure_range(struct imx477 *imx477,
+					 struct v4l2_ctrl *ctrl)
+{
+	int exposure_max, exposure_def;
+
+	if (ctrl->id == V4L2_CID_VBLANK || !ctrl->val) {
+		/*
+		 * Either VBLANK has been changed or auto framerate
+		 * adjusting has been disabled. Honour the VBLANK limits
+		 * when setting exposure.
+		 */
+		exposure_max = imx477->mode->height + imx477->vblank->val -
+						      IMX477_EXPOSURE_OFFSET;
+
+		if (ctrl->id == V4L2_CID_EXPOSURE_AUTO_PRIORITY) {
+			/*
+			 * Allow VBLANK adjustments since the driver is not
+			 * handling frame length control automatically.
+			 */
+			__v4l2_ctrl_grab(imx477->vblank, false);
+		}
+	} else {
+		/*
+		 * Auto framerate adjusting has been enabled. VBLANK
+		 * ctrl has been disabled and exposure can ramp up
+		 * to the maximum allowable value.
+		 */
+		exposure_max = IMX477_EXPOSURE_MAX;
+		/*
+		 * Do not allow VBLANK adjustments if the driver is
+		 * handling it frame length control automatically.
+		 */
+		__v4l2_ctrl_grab(imx477->vblank, true);
+	}
+
+	exposure_def = min(exposure_max, imx477->exposure->val);
+	__v4l2_ctrl_modify_range(imx477->exposure, imx477->exposure->minimum,
+				 exposure_max, imx477->exposure->step,
+				 exposure_def);
+}
+
 static int imx477_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct imx477 *imx477 =
@@ -1285,17 +1353,13 @@ static int imx477_set_ctrl(struct v4l2_c
 	struct i2c_client *client = v4l2_get_subdevdata(&imx477->sd);
 	int ret = 0;
 
-	if (ctrl->id == V4L2_CID_VBLANK) {
-		int exposure_max, exposure_def;
-
-		/* Update max exposure while meeting expected vblanking */
-		exposure_max = imx477->mode->height + ctrl->val -
-							IMX477_EXPOSURE_OFFSET;
-		exposure_def = min(exposure_max, imx477->exposure->val);
-		__v4l2_ctrl_modify_range(imx477->exposure,
-					 imx477->exposure->minimum,
-					 exposure_max, imx477->exposure->step,
-					 exposure_def);
+	if (ctrl->id == V4L2_CID_VBLANK ||
+	    ctrl->id == V4L2_CID_EXPOSURE_AUTO_PRIORITY) {
+		/*
+		 * These controls may change the limits of usable exposure,
+		 * so check and adjust if necessary.
+		 */
+		imx477_adjust_exposure_range(imx477, ctrl);
 	}
 
 	/*
@@ -1311,8 +1375,14 @@ static int imx477_set_ctrl(struct v4l2_c
 				       IMX477_REG_VALUE_16BIT, ctrl->val);
 		break;
 	case V4L2_CID_EXPOSURE:
-		ret = imx477_write_reg(imx477, IMX477_REG_EXPOSURE,
-				       IMX477_REG_VALUE_16BIT, ctrl->val);
+		ret = imx477_set_exposure(imx477, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO_PRIORITY:
+		/*
+		 * imx477_set_exposure() will recalculate the frame length
+		 * to adjust the framerate to match the exposure.
+		 */
+		ret = imx477_set_exposure(imx477, imx477->exposure->val);
 		break;
 	case V4L2_CID_DIGITAL_GAIN:
 		ret = imx477_write_reg(imx477, IMX477_REG_DIGITAL_GAIN,
@@ -1510,9 +1580,8 @@ unsigned int imx477_get_frame_length(con
 
 static void imx477_set_framing_limits(struct imx477 *imx477)
 {
+	unsigned int frm_length_min, frm_length_default, hblank;
 	const struct imx477_mode *mode = imx477->mode;
-	unsigned int frm_length_min, frm_length_default;
-	unsigned int exposure_max, exposure_def, hblank;
 
 	frm_length_min = imx477_get_frame_length(mode, &mode->timeperframe_min);
 	frm_length_default =
@@ -1522,15 +1591,10 @@ static void imx477_set_framing_limits(st
 	__v4l2_ctrl_modify_range(imx477->vblank, frm_length_min - mode->height,
 				 IMX477_FRAME_LENGTH_MAX - mode->height,
 				 1, frm_length_default - mode->height);
+
+	/* Setting this will adjust the exposure limits as well. */
 	__v4l2_ctrl_s_ctrl(imx477->vblank, frm_length_default - mode->height);
 
-	/* Update max exposure while meeting expected vblanking */
-	exposure_max = IMX477_FRAME_LENGTH_MAX - IMX477_EXPOSURE_OFFSET;
-	exposure_def = frm_length_default - mode->height -
-					    IMX477_EXPOSURE_OFFSET;
-	__v4l2_ctrl_modify_range(imx477->exposure, imx477->exposure->minimum,
-				 exposure_max, imx477->exposure->step,
-				 exposure_def);
 	/*
 	 * Currently PPL is fixed to the mode specified value, so hblank
 	 * depends on mode->width only, and is not changeable in any
@@ -1939,6 +2003,11 @@ static int imx477_init_controls(struct i
 			  IMX477_DGTL_GAIN_MIN, IMX477_DGTL_GAIN_MAX,
 			  IMX477_DGTL_GAIN_STEP, IMX477_DGTL_GAIN_DEFAULT);
 
+	imx477->exposure_auto =
+			v4l2_ctrl_new_std(ctrl_hdlr, &imx477_ctrl_ops,
+					  V4L2_CID_EXPOSURE_AUTO_PRIORITY,
+					  0, 1, 1, 0);
+
 	imx477->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx477_ctrl_ops,
 					  V4L2_CID_HFLIP, 0, 1, 1, 0);
 	if (imx477->hflip)