From 4721f56a81e13353f6900a7346bebf330d3d560b Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dave.stevenson@raspberrypi.com>
Date: Thu, 31 Mar 2022 16:45:36 +0100
Subject: [PATCH] media: i2c: imx258: Add support for long exposure
 modes

The sensor has a register CIT_LSHIFT which extends the exposure
and frame times by the specified power of 2 for longer
exposure times.

Add support for this by configuring this register via V4L2_CID_VBLANK
and extending the V4L2_CID_EXPOSURE range accordingly.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
---
 drivers/media/i2c/imx258.c | 38 ++++++++++++++++++++++++++++++++------
 1 file changed, 32 insertions(+), 6 deletions(-)

--- a/drivers/media/i2c/imx258.c
+++ b/drivers/media/i2c/imx258.c
@@ -69,6 +69,10 @@
 #define IMX258_HDR_RATIO_STEP		1
 #define IMX258_HDR_RATIO_DEFAULT	0x0
 
+/* Long exposure multiplier */
+#define IMX258_LONG_EXP_SHIFT_MAX	7
+#define IMX258_LONG_EXP_SHIFT_REG	0x3002
+
 /* Test Pattern Control */
 #define IMX258_REG_TEST_PATTERN		0x0600
 
@@ -843,6 +847,8 @@ struct imx258 {
 	struct v4l2_ctrl *exposure;
 	struct v4l2_ctrl *hflip;
 	struct v4l2_ctrl *vflip;
+	/* Current long exposure factor in use. Set through V4L2_CID_VBLANK */
+	unsigned int long_exp_shift;
 
 	/* Current mode */
 	const struct imx258_mode *cur_mode;
@@ -1020,6 +1026,26 @@ static void imx258_adjust_exposure_range
 				 exposure_def);
 }
 
+static int imx258_set_frame_length(struct imx258 *imx258, unsigned int val)
+{
+	int ret;
+
+	imx258->long_exp_shift = 0;
+
+	while (val > IMX258_VTS_MAX) {
+		imx258->long_exp_shift++;
+		val >>= 1;
+	}
+
+	ret = imx258_write_reg(imx258, IMX258_REG_VTS,
+			       IMX258_REG_VALUE_16BIT, val);
+	if (ret)
+		return ret;
+
+	return imx258_write_reg(imx258, IMX258_LONG_EXP_SHIFT_REG,
+				IMX258_REG_VALUE_08BIT, imx258->long_exp_shift);
+}
+
 static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct imx258 *imx258 =
@@ -1050,7 +1076,7 @@ static int imx258_set_ctrl(struct v4l2_c
 	case V4L2_CID_EXPOSURE:
 		ret = imx258_write_reg(imx258, IMX258_REG_EXPOSURE,
 				IMX258_REG_VALUE_16BIT,
-				ctrl->val);
+				ctrl->val >> imx258->long_exp_shift);
 		break;
 	case V4L2_CID_DIGITAL_GAIN:
 		ret = imx258_update_digital_gain(imx258, IMX258_REG_VALUE_16BIT,
@@ -1078,9 +1104,8 @@ static int imx258_set_ctrl(struct v4l2_c
 		}
 		break;
 	case V4L2_CID_VBLANK:
-		ret = imx258_write_reg(imx258, IMX258_REG_VTS,
-				       IMX258_REG_VALUE_16BIT,
-				       imx258->cur_mode->height + ctrl->val);
+		ret = imx258_set_frame_length(imx258,
+					      imx258->cur_mode->height + ctrl->val);
 		break;
 	case V4L2_CID_VFLIP:
 	case V4L2_CID_HFLIP:
@@ -1218,8 +1243,9 @@ static int imx258_set_pad_format(struct
 			     imx258->cur_mode->height;
 		__v4l2_ctrl_modify_range(
 			imx258->vblank, vblank_min,
-			IMX258_VTS_MAX - imx258->cur_mode->height, 1,
-			vblank_def);
+			((1 << IMX258_LONG_EXP_SHIFT_MAX) * IMX258_VTS_MAX) -
+						imx258->cur_mode->height,
+			1, vblank_def);
 		__v4l2_ctrl_s_ctrl(imx258->vblank, vblank_def);
 		h_blank =
 			imx258->link_freq_configs[mode->link_freq_index].pixels_per_line
