forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2023-12-11 d2ccde1c8e90d38cee87a1b0309ad2827f3fd30d 
 kernel/drivers/media/i2c/sc530ai.c
....@@ -8,6 +8,7 @@
88  * V0.0X01.0X01 fix set vflip/hflip failed bug.
99  */
1010 
11
+//#define DEBUG
1112 #include <linux/clk.h>
1213 #include <linux/device.h>
1314 #include <linux/delay.h>
....@@ -59,7 +60,7 @@
5960 
6061 #define SC530AI_XVCLK_FREQ		27000000
6162 
62
-#define SC530AI_CHIP_ID			0x9e39
63
+#define SC530AI_CHIP_ID			0x8e39
6364 #define SC530AI_REG_CHIP_ID		0x3107
6465 
6566 #define SC530AI_REG_CTRL_MODE		0x0100
....@@ -77,10 +78,10 @@
7778 #define SC530AI_REG_DIG_FINE_GAIN	0x3e07
7879 #define SC530AI_REG_ANA_GAIN		0x3e09
7980 
80
-#define SC530AI_GAIN_MIN		0x800
81
-#define SC530AI_GAIN_MAX		0xa3300
81
+#define SC530AI_GAIN_MIN		0x20
82
+#define SC530AI_GAIN_MAX		(32 * 326)
8283 #define SC530AI_GAIN_STEP		1
83
-#define SC530AI_GAIN_DEFAULT		0x800
84
+#define SC530AI_GAIN_DEFAULT		0x20
8485 
8586 #define SC530AI_REG_VTS_H		0x320e
8687 #define SC530AI_REG_VTS_L		0x320f
....@@ -139,14 +140,6 @@
139140 
140141 #define sc530ai_NUM_SUPPLIES ARRAY_SIZE(sc530ai_supply_names)
141142 
142
-enum sc530ai_max_pad {
143
-	PAD0, /* link to isp */
144
-	PAD1, /* link to csi wr0 | hdr x2:L x3:M */
145
-	PAD2, /* link to csi wr1 | hdr      x3:L */
146
-	PAD3, /* link to csi wr2 | hdr x2:M x3:S */
147
-	PAD_MAX,
148
-};
149
-
150143 struct regval {
151144 	u16 addr;
152145 	u8 val;
....@@ -177,7 +170,8 @@
177170 	struct pinctrl		*pinctrl;
178171 	struct pinctrl_state	*pins_default;
179172 	struct pinctrl_state	*pins_sleep;
180
-
173
+	struct v4l2_fract	cur_fps;
174
+	u32			cur_vts;
181175 	struct v4l2_subdev	subdev;
182176 	struct media_pad	pad;
183177 	struct v4l2_ctrl_handler ctrl_handler;
....@@ -191,7 +185,9 @@
191185 	struct mutex		mutex;
192186 	bool			streaming;
193187 	bool			power_on;
188
+	const struct sc530ai_mode *support_modes;
194189 	const struct sc530ai_mode *cur_mode;
190
+	u32			support_modes_num;
195191 	unsigned int		lane_num;
196192 	u32			module_index;
197193 	const char		*module_facing;
....@@ -208,7 +204,7 @@
208204  * max_framerate 30fps
209205  * mipi_datarate per lane 1008Mbps, 4lane
210206  */
211
-static const struct regval sc530ai_linear_10_30fps_2880x1620_regs[] = {
207
+static const struct regval sc530ai_linear_10_30fps_2880x1620_4lane_regs[] = {
212208 	{0x0103, 0x01},
213209 	{0x0100, 0x00},
214210 	{0x36e9, 0x80},
....@@ -317,6 +313,7 @@
317313 	{0x3e02, 0xa0},
318314 	{0x440e, 0x02},
319315 	{0x4509, 0x20},
316
+	{0x4800, 0x04},
320317 	{0x4837, 0x28},
321318 	{0x5010, 0x10},
322319 	{0x5799, 0x06},
....@@ -356,7 +353,7 @@
356353 	{REG_NULL, 0x00},
357354 };
358355 
359
-static const struct regval sc530ai_hdr_10_30fps_2880x1620_regs[] = {
356
+static const struct regval sc530ai_hdr_10_30fps_2880x1620_4lane_regs[] = {
360357 	{0x0103, 0x01},
361358 	{0x0100, 0x00},
362359 	{0x36e9, 0x80},
....@@ -471,6 +468,7 @@
471468 	{0x3e24, 0xc8},
472469 	{0x440e, 0x02},
473470 	{0x4509, 0x20},
471
+	{0x4800, 0x04},
474472 	{0x4816, 0x11},
475473 	{0x5010, 0x10},
476474 	{0x5799, 0x06},
....@@ -626,6 +624,7 @@
626624 	{0x3e02, 0xa0},
627625 	{0x440e, 0x02},
628626 	{0x4509, 0x20},
627
+	{0x4800, 0x04},
629628 	{0x4837, 0x14},
630629 	{0x5010, 0x10},
631630 	{0x5799, 0x06},
....@@ -664,7 +663,7 @@
664663 	{REG_NULL, 0x00},
665664 };
666665 
667
-static const struct sc530ai_mode supported_modes[] = {
666
+static const struct sc530ai_mode supported_modes_4lane[] = {
668667 	{
669668 		.width = 2880,
670669 		.height = 1620,
....@@ -676,7 +675,7 @@
676675 		.hts_def = 0xb40,
677676 		.vts_def = 0x0672,
678677 		.bus_fmt = MEDIA_BUS_FMT_SBGGR10_1X10,
679
-		.reg_list = sc530ai_linear_10_30fps_2880x1620_regs,
678
+		.reg_list = sc530ai_linear_10_30fps_2880x1620_4lane_regs,
680679 		.mipi_freq_idx = 0,
681680 		.bpp = 10,
682681 		.hdr_mode = NO_HDR,
....@@ -693,7 +692,7 @@
693692 		.hts_def = 0xb40,
694693 		.vts_def = 0x0ce4,
695694 		.bus_fmt = MEDIA_BUS_FMT_SBGGR10_1X10,
696
-		.reg_list = sc530ai_hdr_10_30fps_2880x1620_regs,
695
+		.reg_list = sc530ai_hdr_10_30fps_2880x1620_4lane_regs,
697696 		.mipi_freq_idx = 1,
698697 		.bpp = 10,
699698 		.hdr_mode = HDR_X2,
....@@ -702,7 +701,10 @@
702701 		.vc[PAD2] = V4L2_MBUS_CSI2_CHANNEL_1,
703702 		.vc[PAD3] = V4L2_MBUS_CSI2_CHANNEL_1,//M->csi wr2
704703 	},
705
-	{
704
+};
705
+
706
+static const struct sc530ai_mode supported_modes_2lane[] = {
707
+{
706708 		.width = 2880,
707709 		.height = 1620,
708710 		.max_fps = {
....@@ -812,7 +814,7 @@
812814 }
813815 
814816 static const struct sc530ai_mode *
815
-sc530ai_find_best_fit(struct v4l2_subdev_format *fmt)
817
+sc530ai_find_best_fit(struct sc530ai *sc530ai, struct v4l2_subdev_format *fmt)
816818 {
817819 	struct v4l2_mbus_framefmt *framefmt = &fmt->format;
818820 	int dist;
....@@ -820,15 +822,15 @@
820822 	int cur_best_fit_dist = -1;
821823 	unsigned int i;
822824 
823
-	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
824
-		dist = sc530ai_get_reso_dist(&supported_modes[i], framefmt);
825
+	for (i = 0; i < sc530ai->support_modes_num; i++) {
826
+		dist = sc530ai_get_reso_dist(&sc530ai->support_modes[i], framefmt);
825827 		if (cur_best_fit_dist == -1 || dist < cur_best_fit_dist) {
826828 			cur_best_fit_dist = dist;
827829 			cur_best_fit = i;
828830 		}
829831 	}
830832 
831
-	return &supported_modes[cur_best_fit];
833
+	return &sc530ai->support_modes[cur_best_fit];
832834 }
833835 
834836 static int sc530ai_set_fmt(struct v4l2_subdev *sd,
....@@ -842,7 +844,7 @@
842844 
843845 	mutex_lock(&sc530ai->mutex);
844846 
845
-	mode = sc530ai_find_best_fit(fmt);
847
+	mode = sc530ai_find_best_fit(sc530ai, fmt);
846848 	fmt->format.code = mode->bus_fmt;
847849 	fmt->format.width = mode->width;
848850 	fmt->format.height = mode->height;
....@@ -869,6 +871,8 @@
869871 		pixel_rate = (u32)link_freq_items[mode->mipi_freq_idx] /
870872 			     mode->bpp * 2 * sc530ai->lane_num;
871873 		__v4l2_ctrl_s_ctrl_int64(sc530ai->pixel_rate, pixel_rate);
874
+		sc530ai->cur_vts = mode->vts_def;
875
+		sc530ai->cur_fps = mode->max_fps;
872876 	}
873877 
874878 	mutex_unlock(&sc530ai->mutex);
....@@ -924,16 +928,18 @@
924928 				    struct v4l2_subdev_pad_config *cfg,
925929 				    struct v4l2_subdev_frame_size_enum *fse)
926930 {
927
-	if (fse->index >= ARRAY_SIZE(supported_modes))
931
+	struct sc530ai *sc530ai = to_sc530ai(sd);
932
+
933
+	if (fse->index >= sc530ai->support_modes_num)
928934 		return -EINVAL;
929935 
930
-	if (fse->code != supported_modes[0].bus_fmt)
936
+	if (fse->code != sc530ai->support_modes[fse->index].bus_fmt)
931937 		return -EINVAL;
932938 
933
-	fse->min_width  = supported_modes[fse->index].width;
934
-	fse->max_width  = supported_modes[fse->index].width;
935
-	fse->max_height = supported_modes[fse->index].height;
936
-	fse->min_height = supported_modes[fse->index].height;
939
+	fse->min_width  = sc530ai->support_modes[fse->index].width;
940
+	fse->max_width  = sc530ai->support_modes[fse->index].width;
941
+	fse->max_height = sc530ai->support_modes[fse->index].height;
942
+	fse->min_height = sc530ai->support_modes[fse->index].height;
937943 
938944 	return 0;
939945 }
....@@ -944,14 +950,15 @@
944950 	struct sc530ai *sc530ai = to_sc530ai(sd);
945951 	const struct sc530ai_mode *mode = sc530ai->cur_mode;
946952 
947
-	mutex_lock(&sc530ai->mutex);
948
-	fi->interval = mode->max_fps;
949
-	mutex_unlock(&sc530ai->mutex);
953
+	if (sc530ai->streaming)
954
+		fi->interval = sc530ai->cur_fps;
955
+	else
956
+		fi->interval = mode->max_fps;
950957 
951958 	return 0;
952959 }
953960 
954
-static int sc530ai_g_mbus_config(struct v4l2_subdev *sd,
961
+static int sc530ai_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
955962 				 struct v4l2_mbus_config *config)
956963 {
957964 	struct sc530ai *sc530ai = to_sc530ai(sd);
....@@ -965,7 +972,7 @@
965972 	if (mode->hdr_mode == HDR_X3)
966973 		val |= V4L2_MBUS_CSI2_CHANNEL_2;
967974 
968
-	config->type = V4L2_MBUS_CSI2;
975
+	config->type = V4L2_MBUS_CSI2_DPHY;
969976 	config->flags = val;
970977 
971978 	return 0;
....@@ -984,42 +991,50 @@
984991 static void sc530ai_get_gain_reg(u32 total_gain, u32 *again, u32 *dgain,
985992 				u32 *dgain_fine)
986993 {
987
-	if (total_gain < 0x1000) { /* 1 - 2x gain */
994
+	u32 gain_factor = 0;
995
+
996
+	if (total_gain < SC530AI_GAIN_MIN)
997
+		total_gain = SC530AI_GAIN_MIN;
998
+	else if (total_gain > SC530AI_GAIN_MAX)
999
+		total_gain = SC530AI_GAIN_MAX;
1000
+
1001
+	gain_factor = total_gain * 1000 / 32;
1002
+	if (gain_factor < 2000) { /* 1 - 2x gain */
9881003 		*again = 0x00;
9891004 		*dgain = 0x00;
990
-		*dgain_fine = total_gain >> 4;
991
-	} else if (total_gain < 0x1466) { /* 2x - 2.55x gain */
1005
+		*dgain_fine = gain_factor * 128 / 1000;
1006
+	} else if (gain_factor < 2550) { /* 2x - 2.55x gain */
9921007 		*again = 0x01;
9931008 		*dgain = 0x00;
994
-		*dgain_fine = total_gain >> 5;
995
-	} else if (total_gain < 0x28cc) { /* 2.55x - 5.1x gain */
1009
+		*dgain_fine = gain_factor * 128 / 2000;
1010
+	} else if (gain_factor < 2550 * 2) { /* 2.55x - 5.1x gain */
9961011 		*again = 0x40;
9971012 		*dgain = 0x00;
998
-		*dgain_fine = total_gain * 0x80 / 0x1466;
999
-	} else if (total_gain < 0x5198) { /* 5.1x - 10.2x gain */
1013
+		*dgain_fine = gain_factor * 128 / 2550;
1014
+	} else if (gain_factor < 2550 * 4) { /* 5.1x - 10.2x gain */
10001015 		*again = 0x48;
10011016 		*dgain = 0x00;
1002
-		*dgain_fine = (total_gain * 0x80 / 0x1466) >> 1;
1003
-	} else if (total_gain < 0xa330) { /* 10.2x - 20.4x gain */
1017
+		*dgain_fine = gain_factor * 128 / 5110;
1018
+	} else if (gain_factor < 2550 * 8) { /* 10.2x - 20.4x gain */
10041019 		*again = 0x49;
10051020 		*dgain = 0x00;
1006
-		*dgain_fine = (total_gain * 0x80 / 0x1466) >> 2;
1007
-	} else if (total_gain < 0x14660) { /* 20.4x - 40.8x gain */
1021
+		*dgain_fine = gain_factor * 128 / 10200;
1022
+	} else if (gain_factor < 2550 * 16) { /* 20.4x - 40.8x gain */
10081023 		*again = 0x4B;
10091024 		*dgain = 0x00;
1010
-		*dgain_fine = (total_gain * 0x80 / 0x1466) >> 3;
1011
-	} else if (total_gain < 0x28cc0) { /* 40.8x - 81.6x gain */
1025
+		*dgain_fine = gain_factor * 128 / 20400;
1026
+	} else if (gain_factor < 2550 * 32) { /* 40.8x - 81.6x gain */
10121027 		*again = 0x4f;
10131028 		*dgain = 0x00;
1014
-		*dgain_fine = (total_gain * 0x80 / 0x1466) >> 4;
1015
-	} else if (total_gain < 0x51980) { /* 81.6x - 163.2x gain */
1029
+		*dgain_fine = gain_factor * 128 / 40800;
1030
+	} else if (gain_factor < 2550 * 64) { /* 81.6x - 163.2x gain */
10161031 		*again = 0x5f;
10171032 		*dgain = 0x00;
1018
-		*dgain_fine = (total_gain * 0x80 / 0x1466) >> 5;
1019
-	} else if (total_gain < 0xa3300) { /* 163.2x - 326.4x gain */
1033
+		*dgain_fine = gain_factor * 128 / 40800 / 2;
1034
+	} else if (gain_factor < 2550 * 128) { /* 163.2x - 326.4x gain */
10201035 		*again = 0x5f;
10211036 		*dgain = 0x01;
1022
-		*dgain_fine = (total_gain * 0x80 / 0x1466) >> 6;
1037
+		*dgain_fine = gain_factor * 128 / 40800 / 4;
10231038 	}
10241039 }
10251040 
....@@ -1114,11 +1129,23 @@
11141129 	return ret;
11151130 }
11161131 
1132
+static int sc530ai_get_channel_info(struct sc530ai *sc530ai, struct rkmodule_channel_info *ch_info)
1133
+{
1134
+	if (ch_info->index < PAD0 || ch_info->index >= PAD_MAX)
1135
+		return -EINVAL;
1136
+	ch_info->vc = sc530ai->cur_mode->vc[ch_info->index];
1137
+	ch_info->width = sc530ai->cur_mode->width;
1138
+	ch_info->height = sc530ai->cur_mode->height;
1139
+	ch_info->bus_fmt = sc530ai->cur_mode->bus_fmt;
1140
+	return 0;
1141
+}
1142
+
11171143 static long sc530ai_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
11181144 {
11191145 	struct sc530ai *sc530ai = to_sc530ai(sd);
11201146 	struct rkmodule_hdr_cfg *hdr;
11211147 	const struct sc530ai_mode *mode;
1148
+	struct rkmodule_channel_info *ch_info;
11221149 
11231150 	long ret = 0;
11241151 	u32 i, h = 0, w;
....@@ -1136,15 +1163,17 @@
11361163 		break;
11371164 	case RKMODULE_SET_HDR_CFG:
11381165 		hdr = (struct rkmodule_hdr_cfg *)arg;
1139
-		w = sc530ai->cur_mode->mipi_freq_idx;
1140
-		for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
1141
-			if (w == supported_modes[i].mipi_freq_idx &&
1142
-				supported_modes[i].hdr_mode == hdr->hdr_mode) {
1143
-				sc530ai->cur_mode = &supported_modes[i];
1166
+		w = sc530ai->cur_mode->width;
1167
+		h = sc530ai->cur_mode->height;
1168
+		for (i = 0; i < sc530ai->support_modes_num; i++) {
1169
+			if (w == sc530ai->support_modes[i].width &&
1170
+				h == sc530ai->support_modes[i].height &&
1171
+				sc530ai->support_modes[i].hdr_mode == hdr->hdr_mode) {
1172
+				sc530ai->cur_mode = &sc530ai->support_modes[i];
11441173 				break;
11451174 			}
11461175 		}
1147
-		if (i == ARRAY_SIZE(supported_modes)) {
1176
+		if (i == sc530ai->support_modes_num) {
11481177 			dev_err(&sc530ai->client->dev,
11491178 				"not find hdr mode:%d %dx%d config\n",
11501179 				hdr->hdr_mode, w, h);
....@@ -1169,7 +1198,8 @@
11691198 
11701199 			__v4l2_ctrl_s_ctrl_int64(sc530ai->pixel_rate,
11711200 						 pixel_rate);
1172
-
1201
+			sc530ai->cur_vts = mode->vts_def;
1202
+			sc530ai->cur_fps = mode->max_fps;
11731203 			dev_info(&sc530ai->client->dev, "sensor mode: %d\n",
11741204 				 sc530ai->cur_mode->hdr_mode);
11751205 		}
....@@ -1191,6 +1221,10 @@
11911221 						SC530AI_REG_VALUE_08BIT,
11921222 						SC530AI_MODE_SW_STANDBY);
11931223 		break;
1224
+	case RKMODULE_GET_CHANNEL_INFO:
1225
+		ch_info = (struct rkmodule_channel_info *)arg;
1226
+		ret = sc530ai_get_channel_info(sc530ai, ch_info);
1227
+		break;
11941228 	default:
11951229 		ret = -ENOIOCTLCMD;
11961230 		break;
....@@ -1207,6 +1241,7 @@
12071241 	struct rkmodule_inf *inf;
12081242 	struct rkmodule_hdr_cfg *hdr;
12091243 	struct preisp_hdrae_exp_s *hdrae;
1244
+	struct rkmodule_channel_info *ch_info;
12101245 	long ret = 0;
12111246 	u32 stream = 0;
12121247 
....@@ -1276,6 +1311,21 @@
12761311 			return -EFAULT;
12771312 
12781313 		ret = sc530ai_ioctl(sd, cmd, &stream);
1314
+		break;
1315
+	case RKMODULE_GET_CHANNEL_INFO:
1316
+		ch_info = kzalloc(sizeof(*ch_info), GFP_KERNEL);
1317
+		if (!ch_info) {
1318
+			ret = -ENOMEM;
1319
+			return ret;
1320
+		}
1321
+
1322
+		ret = sc530ai_ioctl(sd, cmd, ch_info);
1323
+		if (!ret) {
1324
+			ret = copy_to_user(up, ch_info, sizeof(*ch_info));
1325
+			if (ret)
1326
+				ret = -EFAULT;
1327
+		}
1328
+		kfree(ch_info);
12791329 		break;
12801330 	default:
12811331 		ret = -ENOIOCTLCMD;
....@@ -1457,7 +1507,7 @@
14571507 	regulator_bulk_disable(sc530ai_NUM_SUPPLIES, sc530ai->supplies);
14581508 }
14591509 
1460
-static int sc530ai_runtime_resume(struct device *dev)
1510
+static int __maybe_unused sc530ai_runtime_resume(struct device *dev)
14611511 {
14621512 	struct i2c_client *client = to_i2c_client(dev);
14631513 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
....@@ -1466,7 +1516,7 @@
14661516 	return __sc530ai_power_on(sc530ai);
14671517 }
14681518 
1469
-static int sc530ai_runtime_suspend(struct device *dev)
1519
+static int __maybe_unused sc530ai_runtime_suspend(struct device *dev)
14701520 {
14711521 	struct i2c_client *client = to_i2c_client(dev);
14721522 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
....@@ -1483,7 +1533,7 @@
14831533 	struct sc530ai *sc530ai = to_sc530ai(sd);
14841534 	struct v4l2_mbus_framefmt *try_fmt =
14851535 			v4l2_subdev_get_try_format(sd, fh->pad, 0);
1486
-	const struct sc530ai_mode *def_mode = &supported_modes[0];
1536
+	const struct sc530ai_mode *def_mode = &sc530ai->support_modes[0];
14871537 
14881538 	mutex_lock(&sc530ai->mutex);
14891539 	/* Initialize try_fmt */
....@@ -1529,14 +1579,16 @@
15291579 				       struct v4l2_subdev_pad_config *cfg,
15301580 				       struct v4l2_subdev_frame_interval_enum *fie)
15311581 {
1532
-	if (fie->index >= ARRAY_SIZE(supported_modes))
1582
+	struct sc530ai *sc530ai = to_sc530ai(sd);
1583
+
1584
+	if (fie->index >= sc530ai->support_modes_num)
15331585 		return -EINVAL;
15341586 
1535
-	fie->code = supported_modes[fie->index].bus_fmt;
1536
-	fie->width = supported_modes[fie->index].width;
1537
-	fie->height = supported_modes[fie->index].height;
1538
-	fie->interval = supported_modes[fie->index].max_fps;
1539
-	fie->reserved[0] = supported_modes[fie->index].hdr_mode;
1587
+	fie->code = sc530ai->support_modes[fie->index].bus_fmt;
1588
+	fie->width = sc530ai->support_modes[fie->index].width;
1589
+	fie->height = sc530ai->support_modes[fie->index].height;
1590
+	fie->interval = sc530ai->support_modes[fie->index].max_fps;
1591
+	fie->reserved[0] = sc530ai->support_modes[fie->index].hdr_mode;
15401592 	return 0;
15411593 }
15421594 
....@@ -1562,7 +1614,6 @@
15621614 static const struct v4l2_subdev_video_ops sc530ai_video_ops = {
15631615 	.s_stream = sc530ai_s_stream,
15641616 	.g_frame_interval = sc530ai_g_frame_interval,
1565
-	.g_mbus_config = sc530ai_g_mbus_config,
15661617 };
15671618 
15681619 static const struct v4l2_subdev_pad_ops sc530ai_pad_ops = {
....@@ -1572,6 +1623,7 @@
15721623 	.get_fmt = sc530ai_get_fmt,
15731624 	.set_fmt = sc530ai_set_fmt,
15741625 	.get_selection = sc530ai_get_selection,
1626
+	.get_mbus_config = sc530ai_g_mbus_config,
15751627 };
15761628 
15771629 static const struct v4l2_subdev_ops sc530ai_subdev_ops = {
....@@ -1579,6 +1631,14 @@
15791631 	.video	= &sc530ai_video_ops,
15801632 	.pad	= &sc530ai_pad_ops,
15811633 };
1634
+
1635
+static void sc530ai_modify_fps_info(struct sc530ai *sc5330ai)
1636
+{
1637
+	const struct sc530ai_mode *mode = sc5330ai->cur_mode;
1638
+
1639
+	sc5330ai->cur_fps.denominator = mode->max_fps.denominator * mode->vts_def /
1640
+					sc5330ai->cur_vts;
1641
+}
15821642 
15831643 static int sc530ai_set_ctrl(struct v4l2_ctrl *ctrl)
15841644 {
....@@ -1608,7 +1668,7 @@
16081668 	switch (ctrl->id) {
16091669 	case V4L2_CID_EXPOSURE:
16101670 		if (sc530ai->cur_mode->hdr_mode != NO_HDR)
1611
-			return ret;
1671
+			goto ctrl_end;
16121672 		val = ctrl->val << 1;
16131673 		ret = sc530ai_write_reg(sc530ai->client,
16141674 					SC530AI_REG_EXPOSURE_H,
....@@ -1627,7 +1687,7 @@
16271687 		break;
16281688 	case V4L2_CID_ANALOGUE_GAIN:
16291689 		if (sc530ai->cur_mode->hdr_mode != NO_HDR)
1630
-			return ret;
1690
+			goto ctrl_end;
16311691 
16321692 		sc530ai_get_gain_reg(ctrl->val, &again, &dgain, &dgain_fine);
16331693 		ret = sc530ai_write_reg(sc530ai->client,
....@@ -1642,7 +1702,7 @@
16421702 					 SC530AI_REG_ANA_GAIN,
16431703 					SC530AI_REG_VALUE_08BIT,
16441704 					 again);
1645
-
1705
+		dev_dbg(&client->dev, "set gain 0x%x\n", ctrl->val);
16461706 		break;
16471707 	case V4L2_CID_VBLANK:
16481708 		vts = ctrl->val + sc530ai->cur_mode->height;
....@@ -1654,6 +1714,11 @@
16541714 					 SC530AI_REG_VTS_L,
16551715 					 SC530AI_REG_VALUE_08BIT,
16561716 					 vts & 0xff);
1717
+		if (!ret)
1718
+			sc530ai->cur_vts = vts;
1719
+		if (sc530ai->cur_vts != sc530ai->cur_mode->vts_def)
1720
+			sc530ai_modify_fps_info(sc530ai);
1721
+		dev_dbg(&client->dev, "set vblank 0x%x\n", ctrl->val);
16571722 		break;
16581723 	case V4L2_CID_HFLIP:
16591724 		ret = sc530ai_read_reg(sc530ai->client, SC530AI_FLIP_MIRROR_REG,
....@@ -1691,6 +1756,7 @@
16911756 		break;
16921757 	}
16931758 
1759
+ctrl_end:
16941760 	pm_runtime_put(&client->dev);
16951761 
16961762 	return ret;
....@@ -1715,22 +1781,23 @@
17151781 	fwnode = of_fwnode_handle(endpoint);
17161782 	rval = fwnode_property_read_u32_array(fwnode, "data-lanes", NULL, 0);
17171783 	if (rval <= 0) {
1718
-		dev_warn(dev, " Get mipi lane num failed!\n");
1719
-		return -1;
1784
+		dev_err(dev, " Get mipi lane num failed!\n");
1785
+		return -EINVAL;
17201786 	}
17211787 
17221788 	sc530ai->lane_num = rval;
1789
+	dev_info(dev, "lane_num = %d\n", sc530ai->lane_num);
17231790 
17241791 	if (sc530ai->lane_num == 2) {
1725
-		sc530ai->cur_mode = &supported_modes[2];
1726
-		dev_info(dev, "lane_num(%d)\n", sc530ai->lane_num);
1727
-	} else if (sc530ai->lane_num == 2) {
1728
-		sc530ai->cur_mode = &supported_modes[0];
1729
-		dev_info(dev, "lane_num(%d)\n", sc530ai->lane_num);
1730
-	} else {
1731
-		dev_err(dev, "unsupported lane_num(%d)\n", sc530ai->lane_num);
1732
-		return -1;
1792
+		sc530ai->support_modes = supported_modes_2lane;
1793
+		sc530ai->support_modes_num = ARRAY_SIZE(supported_modes_2lane);
1794
+	} else if (sc530ai->lane_num == 4) {
1795
+		sc530ai->support_modes = supported_modes_4lane;
1796
+		sc530ai->support_modes_num = ARRAY_SIZE(supported_modes_4lane);
17331797 	}
1798
+
1799
+	sc530ai->cur_mode = &sc530ai->support_modes[0];
1800
+
17341801 	return 0;
17351802 }
17361803 
....@@ -1809,6 +1876,8 @@
18091876 	}
18101877 	sc530ai->subdev.ctrl_handler = handler;
18111878 	sc530ai->has_init_exp = false;
1879
+	sc530ai->cur_vts = mode->vts_def;
1880
+	sc530ai->cur_fps = mode->max_fps;
18121881 
18131882 	return 0;
18141883 
....@@ -1857,7 +1926,7 @@
18571926 	struct v4l2_subdev *sd;
18581927 	char facing[2];
18591928 	int ret;
1860
-	u32 i, hdr_mode = 0;
1929
+	u32 hdr_mode = 0;
18611930 
18621931 	dev_info(dev, "driver version: %02x.%02x.%02x",
18631932 		 DRIVER_VERSION >> 16,
....@@ -1883,14 +1952,10 @@
18831952 	}
18841953 
18851954 	sc530ai->client = client;
1886
-	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
1887
-		if (hdr_mode == supported_modes[i].hdr_mode) {
1888
-			sc530ai->cur_mode = &supported_modes[i];
1889
-			break;
1890
-		}
1891
-	}
1892
-	if (i == ARRAY_SIZE(supported_modes))
1893
-		sc530ai->cur_mode = &supported_modes[0];
1955
+
1956
+	ret = sc530ai_parse_of(sc530ai);
1957
+	if (ret)
1958
+		return -EINVAL;
18941959 
18951960 	sc530ai->xvclk = devm_clk_get(dev, "xvclk");
18961961 	if (IS_ERR(sc530ai->xvclk)) {
....@@ -1928,10 +1993,6 @@
19281993 		dev_err(dev, "Failed to get power regulators\n");
19291994 		return ret;
19301995 	}
1931
-
1932
-	ret = sc530ai_parse_of(sc530ai);
1933
-	if (ret != 0)
1934
-		return -EINVAL;
19351996 
19361997 	mutex_init(&sc530ai->mutex);
19371998