forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2024-01-03 2f7c68cb55ecb7331f2381deb497c27155f32faf 
 kernel/drivers/phy/rockchip/phy-rockchip-csi2-dphy-hw.c
....@@ -54,6 +54,7 @@
5454 #define CSI2_DPHY_DUAL_CAL_EN		(0x80)
5555 #define CSI2_DPHY_CLK_INV		(0X84)
5656 
57
+#define CSI2_DPHY_CLK_CONTINUE_MODE	(0x128)
5758 #define CSI2_DPHY_CLK_WR_THS_SETTLE	(0x160)
5859 #define CSI2_DPHY_CLK_CALIB_EN		(0x168)
5960 #define CSI2_DPHY_LANE0_WR_THS_SETTLE	(0x1e0)
....@@ -64,6 +65,7 @@
6465 #define CSI2_DPHY_LANE2_CALIB_EN	(0x2e8)
6566 #define CSI2_DPHY_LANE3_WR_THS_SETTLE	(0x360)
6667 #define CSI2_DPHY_LANE3_CALIB_EN	(0x368)
68
+#define CSI2_DPHY_CLK1_CONTINUE_MODE	(0x3a8)
6769 #define CSI2_DPHY_CLK1_WR_THS_SETTLE	(0x3e0)
6870 #define CSI2_DPHY_CLK1_CALIB_EN		(0x3e8)
6971 
....@@ -213,6 +215,8 @@
213215 	CSI2PHY_PATH1_MODEL,
214216 	CSI2PHY_PATH1_LVDS_MODEL,
215217 	CSI2PHY_CLK_INV,
218
+	CSI2PHY_CLK_CONTINUE_MODE,
219
+	CSI2PHY_CLK1_CONTINUE_MODE,
216220 };
217221 
218222 #define HIWORD_UPDATE(val, mask, shift) \
....@@ -232,9 +236,14 @@
232236 static inline void write_sys_grf_reg(struct csi2_dphy_hw *hw,
233237 				     int index, u8 value)
234238 {
235
-	const struct grf_reg *reg = &hw->grf_regs[index];
236
-	unsigned int val = HIWORD_UPDATE(value, reg->mask, reg->shift);
239
+	const struct grf_reg *reg = NULL;
240
+	unsigned int val = 0;
237241 
242
+	if (index >= hw->drv_data->num_grf_regs)
243
+		return;
244
+
245
+	reg = &hw->grf_regs[index];
246
+	val = HIWORD_UPDATE(value, reg->mask, reg->shift);
238247 	if (reg->mask)
239248 		regmap_write(hw->regmap_sys_grf, reg->offset, val);
240249 }
....@@ -242,18 +251,27 @@
242251 static inline void write_grf_reg(struct csi2_dphy_hw *hw,
243252 				     int index, u8 value)
244253 {
245
-	const struct grf_reg *reg = &hw->grf_regs[index];
246
-	unsigned int val = HIWORD_UPDATE(value, reg->mask, reg->shift);
254
+	const struct grf_reg *reg = NULL;
255
+	unsigned int val = 0;
247256 
257
+	if (index >= hw->drv_data->num_grf_regs)
258
+		return;
259
+
260
+	reg = &hw->grf_regs[index];
261
+	val = HIWORD_UPDATE(value, reg->mask, reg->shift);
248262 	if (reg->mask)
249263 		regmap_write(hw->regmap_grf, reg->offset, val);
250264 }
251265 
252266 static inline u32 read_grf_reg(struct csi2_dphy_hw *hw, int index)
253267 {
254
-	const struct grf_reg *reg = &hw->grf_regs[index];
268
+	const struct grf_reg *reg = NULL;
255269 	unsigned int val = 0;
256270 
271
+	if (index >= hw->drv_data->num_grf_regs)
272
+		return -EINVAL;
273
+
274
+	reg = &hw->grf_regs[index];
257275 	if (reg->mask) {
258276 		regmap_read(hw->regmap_grf, reg->offset, &val);
259277 		val = (val >> reg->shift) & reg->mask;
....@@ -265,8 +283,12 @@
265283 static inline void write_csi2_dphy_reg(struct csi2_dphy_hw *hw,
266284 					    int index, u32 value)
267285 {
268
-	const struct csi2dphy_reg *reg = &hw->csi2dphy_regs[index];
286
+	const struct csi2dphy_reg *reg = NULL;
269287 
288
+	if (index >= hw->drv_data->num_csi2dphy_regs)
289
+		return;
290
+
291
+	reg = &hw->csi2dphy_regs[index];
270292 	if ((index == CSI2PHY_REG_CTRL_LANE_ENABLE) ||
271293 	    (index == CSI2PHY_CLK_LANE_ENABLE) ||
272294 	    (index != CSI2PHY_REG_CTRL_LANE_ENABLE &&
....@@ -277,9 +299,13 @@
277299 static inline void write_csi2_dphy_reg_mask(struct csi2_dphy_hw *hw,
278300 					    int index, u32 value, u32 mask)
279301 {
280
-	const struct csi2dphy_reg *reg = &hw->csi2dphy_regs[index];
302
+	const struct csi2dphy_reg *reg = NULL;
281303 	u32 read_val = 0;
282304 
305
+	if (index >= hw->drv_data->num_csi2dphy_regs)
306
+		return;
307
+
308
+	reg = &hw->csi2dphy_regs[index];
283309 	read_val = readl(hw->hw_base_addr + reg->offset);
284310 	read_val &= ~mask;
285311 	read_val |= value;
....@@ -289,8 +315,12 @@
289315 static inline void read_csi2_dphy_reg(struct csi2_dphy_hw *hw,
290316 					   int index, u32 *value)
291317 {
292
-	const struct csi2dphy_reg *reg = &hw->csi2dphy_regs[index];
318
+	const struct csi2dphy_reg *reg = NULL;
293319 
320
+	if (index >= hw->drv_data->num_csi2dphy_regs)
321
+		return;
322
+
323
+	reg = &hw->csi2dphy_regs[index];
294324 	if ((index == CSI2PHY_REG_CTRL_LANE_ENABLE) ||
295325 	    (index == CSI2PHY_CLK_LANE_ENABLE) ||
296326 	    (index != CSI2PHY_REG_CTRL_LANE_ENABLE &&
....@@ -397,6 +427,8 @@
397427 	[CSI2PHY_CLK1_THS_SETTLE] = CSI2PHY_REG(CSI2_DPHY_CLK1_WR_THS_SETTLE),
398428 	[CSI2PHY_CLK1_CALIB_ENABLE] = CSI2PHY_REG(CSI2_DPHY_CLK1_CALIB_EN),
399429 	[CSI2PHY_CLK1_LANE_ENABLE] = CSI2PHY_REG(CSI2_DPHY_CLK1_LANE_EN),
430
+	[CSI2PHY_CLK_CONTINUE_MODE] = CSI2PHY_REG(CSI2_DPHY_CLK_CONTINUE_MODE),
431
+	[CSI2PHY_CLK1_CONTINUE_MODE] = CSI2PHY_REG(CSI2_DPHY_CLK1_CONTINUE_MODE),
400432 };
401433 
402434 static const struct grf_reg rv1106_grf_dphy_regs[] = {
....@@ -709,19 +741,30 @@
709741 		val |= (GENMASK(sensor->lanes - 1, 0) <<
710742 			CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT) |
711743 			(0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
744
+		if (sensor->mbus.flags & V4L2_MBUS_CSI2_CONTINUOUS_CLOCK)
745
+			write_csi2_dphy_reg(hw, CSI2PHY_CLK_CONTINUE_MODE, 0x30);
712746 	} else {
713747 		if (!(pre_val & (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT)))
714748 			val |= (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
715749 
716
-		if (dphy->phy_index % 3 == DPHY1)
750
+		if (dphy->phy_index % 3 == DPHY1) {
717751 			val |= (GENMASK(sensor->lanes - 1, 0) <<
718752 				CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT);
753
+			if (sensor->mbus.flags &
754
+			    V4L2_MBUS_CSI2_CONTINUOUS_CLOCK)
755
+				write_csi2_dphy_reg(
756
+					hw, CSI2PHY_CLK_CONTINUE_MODE, 0x30);
757
+		}
719758 
720759 		if (dphy->phy_index % 3 == DPHY2) {
721760 			val |= (GENMASK(sensor->lanes - 1, 0) <<
722761 				CSI2_DPHY_CTRL_DATALANE_SPLIT_LANE2_3_OFFSET_BIT);
723762 			if (hw->drv_data->chip_id >= CHIP_ID_RK3588)
724763 				write_csi2_dphy_reg(hw, CSI2PHY_CLK1_LANE_ENABLE, BIT(6));
764
+			if (sensor->mbus.flags &
765
+			    V4L2_MBUS_CSI2_CONTINUOUS_CLOCK)
766
+				write_csi2_dphy_reg(
767
+					hw, CSI2PHY_CLK1_CONTINUE_MODE, 0x30);
725768 		}
726769 	}
727770 	val |= pre_val;
....@@ -859,10 +902,87 @@
859902 
860903 	write_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, 0x01);
861904 	csi2_dphy_hw_do_reset(hw);
862
-	usleep_range(500, 1000);
863905 
864906 	mutex_unlock(&hw->mutex);
865907 
908
+	return 0;
909
+}
910
+
911
+static int csi2_dphy_hw_quick_stream_on(struct csi2_dphy *dphy,
912
+					struct v4l2_subdev *sd)
913
+{
914
+	struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
915
+	struct csi2_sensor *sensor;
916
+	struct csi2_dphy_hw *hw = dphy->dphy_hw;
917
+	u32 val = 0, pre_val = 0;
918
+
919
+	if (!sensor_sd)
920
+		return -ENODEV;
921
+	sensor = sd_to_sensor(dphy, sensor_sd);
922
+	if (!sensor)
923
+		return -ENODEV;
924
+
925
+	read_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, &pre_val);
926
+	if (hw->lane_mode == LANE_MODE_FULL) {
927
+		val |= (GENMASK(sensor->lanes - 1, 0) <<
928
+			CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT) |
929
+			(0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
930
+	} else {
931
+		if (!(pre_val & (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT)))
932
+			val |= (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
933
+
934
+		if (dphy->phy_index % 3 == DPHY1)
935
+			val |= (GENMASK(sensor->lanes - 1, 0) <<
936
+				CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT);
937
+
938
+		if (dphy->phy_index % 3 == DPHY2) {
939
+			val |= (GENMASK(sensor->lanes - 1, 0) <<
940
+				CSI2_DPHY_CTRL_DATALANE_SPLIT_LANE2_3_OFFSET_BIT);
941
+			if (hw->drv_data->chip_id >= CHIP_ID_RK3588)
942
+				write_csi2_dphy_reg(hw, CSI2PHY_CLK1_LANE_ENABLE, BIT(6));
943
+		}
944
+	}
945
+	pre_val |= val;
946
+	write_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, pre_val);
947
+	return 0;
948
+}
949
+
950
+static int csi2_dphy_hw_quick_stream_off(struct csi2_dphy *dphy,
951
+					 struct v4l2_subdev *sd)
952
+{
953
+	struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
954
+	struct csi2_sensor *sensor;
955
+	struct csi2_dphy_hw *hw = dphy->dphy_hw;
956
+	u32 val = 0, pre_val = 0;
957
+
958
+	if (!sensor_sd)
959
+		return -ENODEV;
960
+	sensor = sd_to_sensor(dphy, sensor_sd);
961
+	if (!sensor)
962
+		return -ENODEV;
963
+
964
+	read_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, &pre_val);
965
+	if (hw->lane_mode == LANE_MODE_FULL) {
966
+		val |= (GENMASK(sensor->lanes - 1, 0) <<
967
+			CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT) |
968
+			(0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
969
+	} else {
970
+		if (!(pre_val & (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT)))
971
+			val |= (0x1 << CSI2_DPHY_CTRL_CLKLANE_ENABLE_OFFSET_BIT);
972
+
973
+		if (dphy->phy_index % 3 == DPHY1)
974
+			val |= (GENMASK(sensor->lanes - 1, 0) <<
975
+				CSI2_DPHY_CTRL_DATALANE_ENABLE_OFFSET_BIT);
976
+
977
+		if (dphy->phy_index % 3 == DPHY2) {
978
+			val |= (GENMASK(sensor->lanes - 1, 0) <<
979
+				CSI2_DPHY_CTRL_DATALANE_SPLIT_LANE2_3_OFFSET_BIT);
980
+			if (hw->drv_data->chip_id >= CHIP_ID_RK3588)
981
+				write_csi2_dphy_reg(hw, CSI2PHY_CLK1_LANE_ENABLE, BIT(6));
982
+		}
983
+	}
984
+	pre_val &= ~val;
985
+	write_csi2_dphy_reg(hw, CSI2PHY_REG_CTRL_LANE_ENABLE, pre_val);
866986 	return 0;
867987 }
868988 
....@@ -913,7 +1033,9 @@
9131033 	.hsfreq_ranges = rk3568_csi2_dphy_hw_hsfreq_ranges,
9141034 	.num_hsfreq_ranges = ARRAY_SIZE(rk3568_csi2_dphy_hw_hsfreq_ranges),
9151035 	.csi2dphy_regs = rk3568_csi2dphy_regs,
1036
+	.num_csi2dphy_regs = ARRAY_SIZE(rk3568_csi2dphy_regs),
9161037 	.grf_regs = rk3568_grf_dphy_regs,
1038
+	.num_grf_regs = ARRAY_SIZE(rk3568_grf_dphy_regs),
9171039 	.individual_init = rk3568_csi2_dphy_hw_individual_init,
9181040 	.chip_id = CHIP_ID_RK3568,
9191041 	.stream_on = csi2_dphy_hw_stream_on,
....@@ -924,7 +1046,9 @@
9241046 	.hsfreq_ranges = rk3568_csi2_dphy_hw_hsfreq_ranges,
9251047 	.num_hsfreq_ranges = ARRAY_SIZE(rk3568_csi2_dphy_hw_hsfreq_ranges),
9261048 	.csi2dphy_regs = rk3588_csi2dphy_regs,
1049
+	.num_csi2dphy_regs = ARRAY_SIZE(rk3588_csi2dphy_regs),
9271050 	.grf_regs = rk3588_grf_dphy_regs,
1051
+	.num_grf_regs = ARRAY_SIZE(rk3588_grf_dphy_regs),
9281052 	.individual_init = rk3588_csi2_dphy_hw_individual_init,
9291053 	.chip_id = CHIP_ID_RK3588,
9301054 	.stream_on = csi2_dphy_hw_stream_on,
....@@ -935,7 +1059,9 @@
9351059 	.hsfreq_ranges = rk3568_csi2_dphy_hw_hsfreq_ranges,
9361060 	.num_hsfreq_ranges = ARRAY_SIZE(rk3568_csi2_dphy_hw_hsfreq_ranges),
9371061 	.csi2dphy_regs = rv1106_csi2dphy_regs,
1062
+	.num_csi2dphy_regs = ARRAY_SIZE(rv1106_csi2dphy_regs),
9381063 	.grf_regs = rv1106_grf_dphy_regs,
1064
+	.num_grf_regs = ARRAY_SIZE(rv1106_grf_dphy_regs),
9391065 	.individual_init = rv1106_csi2_dphy_hw_individual_init,
9401066 	.chip_id = CHIP_ID_RV1106,
9411067 	.stream_on = csi2_dphy_hw_stream_on,
....@@ -946,7 +1072,9 @@
9461072 	.hsfreq_ranges = rk3568_csi2_dphy_hw_hsfreq_ranges,
9471073 	.num_hsfreq_ranges = ARRAY_SIZE(rk3568_csi2_dphy_hw_hsfreq_ranges),
9481074 	.csi2dphy_regs = rk3562_csi2dphy_regs,
1075
+	.num_csi2dphy_regs = ARRAY_SIZE(rk3562_csi2dphy_regs),
9491076 	.grf_regs = rk3562_grf_dphy_regs,
1077
+	.num_grf_regs = ARRAY_SIZE(rk3562_grf_dphy_regs),
9501078 	.individual_init = rk3562_csi2_dphy_hw_individual_init,
9511079 	.chip_id = CHIP_ID_RK3562,
9521080 	.stream_on = csi2_dphy_hw_stream_on,
....@@ -1024,7 +1152,6 @@
10241152 	dphy_hw->drv_data = drv_data;
10251153 	dphy_hw->lane_mode = LANE_MODE_UNDEF;
10261154 	dphy_hw->grf_regs = drv_data->grf_regs;
1027
-	dphy_hw->txrx_regs = drv_data->txrx_regs;
10281155 	dphy_hw->csi2dphy_regs = drv_data->csi2dphy_regs;
10291156 
10301157 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
....@@ -1041,6 +1168,8 @@
10411168 	}
10421169 	dphy_hw->stream_on = drv_data->stream_on;
10431170 	dphy_hw->stream_off = drv_data->stream_off;
1171
+	dphy_hw->quick_stream_on = csi2_dphy_hw_quick_stream_on;
1172
+	dphy_hw->quick_stream_off = csi2_dphy_hw_quick_stream_off;
10441173 
10451174 	if (drv_data->chip_id == CHIP_ID_RV1106) {
10461175 		dphy_hw->ttl_mode_enable = csi2_dphy_hw_ttl_mode_enable;