From 60c0ca59d25615cd54e5bec38381bafc85a40ac9 Mon Sep 17 00:00:00 2001
From: Zefa Chen <zefa.chen@rock-chips.com>
Date: Thu, 1 Sep 2022 09:32:50 +0800
Subject: [PATCH] media: i2c: sc230ai support get real fps

Signed-off-by: Zefa Chen <zefa.chen@rock-chips.com>
Change-Id: I1302b46ab2ff7d62f5bbd7b5f5eb6687be865775
---
 drivers/media/i2c/sc230ai.c | 20 +++++++++++++++++---
 1 file changed, 17 insertions(+), 3 deletions(-)

diff --git a/drivers/media/i2c/sc230ai.c b/drivers/media/i2c/sc230ai.c
index c09e298e15f0..df7126b3fe9d 100644
--- a/drivers/media/i2c/sc230ai.c
+++ b/drivers/media/i2c/sc230ai.c
@@ -168,6 +168,7 @@ struct sc230ai {
 	struct v4l2_ctrl	*link_freq;
 	struct v4l2_ctrl	*test_pattern;
 	struct mutex		mutex;
+	struct v4l2_fract	cur_fps;
 	bool			streaming;
 	bool			power_on;
 	const struct sc230ai_mode *cur_mode;
@@ -794,6 +795,7 @@ static int sc230ai_set_fmt(struct v4l2_subdev *sd,
 		pixel_rate = (u32)link_freq_menu_items[mode->mipi_freq_idx] /
 			     mode->bpp * 2 * SC230AI_LANES;
 		__v4l2_ctrl_s_ctrl_int64(sc230ai->pixel_rate, pixel_rate);
+		sc230ai->cur_fps = mode->max_fps;
 	}
 
 	mutex_unlock(&sc230ai->mutex);
@@ -886,9 +888,10 @@ static int sc230ai_g_frame_interval(struct v4l2_subdev *sd,
 	struct sc230ai *sc230ai = to_sc230ai(sd);
 	const struct sc230ai_mode *mode = sc230ai->cur_mode;
 
-	mutex_lock(&sc230ai->mutex);
-	fi->interval = mode->max_fps;
-	mutex_unlock(&sc230ai->mutex);
+	if (sc230ai->streaming)
+		fi->interval = sc230ai->cur_fps;
+	else
+		fi->interval = mode->max_fps;
 
 	return 0;
 }
@@ -963,6 +966,7 @@ static long sc230ai_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
 			__v4l2_ctrl_modify_range(sc230ai->hblank, w, w, 1, w);
 			__v4l2_ctrl_modify_range(sc230ai->vblank, h,
 						 SC230AI_VTS_MAX - sc230ai->cur_mode->height, 1, h);
+			sc230ai->cur_fps = sc230ai->cur_mode->max_fps;
 		}
 		break;
 	case PREISP_CMD_SET_HDRAE_EXP:
@@ -1377,6 +1381,14 @@ static const struct v4l2_subdev_ops sc230ai_subdev_ops = {
 	.pad	= &sc230ai_pad_ops,
 };
 
+static void sc230ai_modify_fps_info(struct sc230ai *sc230ai)
+{
+	const struct sc230ai_mode *mode = sc230ai->cur_mode;
+
+	sc230ai->cur_fps.denominator = mode->max_fps.denominator * sc230ai->cur_vts /
+				       mode->vts_def;
+}
+
 static int sc230ai_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct sc230ai *sc230ai = container_of(ctrl->handler,
@@ -1455,6 +1467,8 @@ static int sc230ai_set_ctrl(struct v4l2_ctrl *ctrl)
 					 (ctrl->val + sc230ai->cur_mode->height)
 					 & 0xff);
 		sc230ai->cur_vts = ctrl->val + sc230ai->cur_mode->height;
+		if (sc230ai->cur_vts != sc230ai->cur_mode->vts_def)
+			sc230ai_modify_fps_info(sc230ai);
 		break;
 	case V4L2_CID_TEST_PATTERN:
 		ret = sc230ai_enable_test_pattern(sc230ai, ctrl->val);