kernel_5.10 no rt

hc

2023-12-11 6778948f9de86c3cfaf36725a7c87dcff9ba247f

 kernel/drivers/phy/rockchip/phy-rockchip-csi2-dphy.c
..
..
@@ -25,6 +25,17 @@
25
25
 #include "phy-rockchip-csi2-dphy-common.h"
26
26
 #include "phy-rockchip-samsung-dcphy.h"
27
27
 
28
+static struct rkmodule_csi_dphy_param rk3588_dcphy_param = {
29
+	.vendor = PHY_VENDOR_SAMSUNG,
30
+	.lp_vol_ref = 3,
31
+	.lp_hys_sw = {3, 0, 0, 0},
32
+	.lp_escclk_pol_sel = {1, 0, 0, 0},
33
+	.skew_data_cal_clk = {0, 3, 3, 3},
34
+	.clk_hs_term_sel = 2,
35
+	.data_hs_term_sel = {2, 2, 2, 2},
36
+	.reserved = {0},
37
+};
38
+
28
39
 struct sensor_async_subdev {
29
40
 	struct v4l2_async_subdev asd;
30
41
 	struct v4l2_mbus_config mbus;
..
..
@@ -42,7 +53,10 @@
42
53
 {
43
54
 	struct media_pad *local, *remote;
44
55
 	struct media_entity *sensor_me;
56
+	struct csi2_dphy *dphy = to_csi2_dphy(sd);
45
57
 
58
+	if (dphy->num_sensors == 0)
59
+		return NULL;
46
60
 	local = &sd->entity.pads[CSI2_DPHY_RX_PAD_SINK];
47
61
 	remote = media_entity_remote_pad(local);
48
62
 	if (!remote) {
..
..
@@ -72,7 +86,7 @@
72
86
 	struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
73
87
 	struct v4l2_ctrl *link_freq;
74
88
 	struct v4l2_querymenu qm = { .id = V4L2_CID_LINK_FREQ, };
75
-	int ret;
89
+	int ret = 0;
76
90
 
77
91
 	if (!sensor_sd)
78
92
 		return -ENODEV;
..
..
@@ -101,14 +115,214 @@
101
115
 	return 0;
102
116
 }
103
117
 
118
+static int rockchip_csi2_dphy_attach_hw(struct csi2_dphy *dphy, int csi_idx, int index)
119
+{
120
+	struct csi2_dphy_hw *dphy_hw;
121
+	struct samsung_mipi_dcphy *dcphy_hw;
122
+	struct v4l2_subdev *sensor_sd = get_remote_sensor(&dphy->sd);
123
+	struct csi2_sensor *sensor = NULL;
124
+	int lanes = 2;
125
+
126
+	if (sensor_sd) {
127
+		sensor = sd_to_sensor(dphy, sensor_sd);
128
+		lanes = sensor->lanes;
129
+	}
130
+
131
+	if (dphy->drv_data->chip_id == CHIP_ID_RK3568 ||
132
+	    dphy->drv_data->chip_id == CHIP_ID_RV1106) {
133
+		dphy_hw = dphy->dphy_hw_group[0];
134
+		mutex_lock(&dphy_hw->mutex);
135
+		dphy_hw->dphy_dev[dphy_hw->dphy_dev_num] = dphy;
136
+		dphy_hw->dphy_dev_num++;
137
+		switch (dphy->phy_index) {
138
+		case 0:
139
+			dphy->lane_mode = PHY_FULL_MODE;
140
+			dphy_hw->lane_mode = LANE_MODE_FULL;
141
+			break;
142
+		case 1:
143
+			dphy->lane_mode = PHY_SPLIT_01;
144
+			dphy_hw->lane_mode = LANE_MODE_SPLIT;
145
+			break;
146
+		case 2:
147
+			dphy->lane_mode = PHY_SPLIT_23;
148
+			dphy_hw->lane_mode = LANE_MODE_SPLIT;
149
+			break;
150
+		default:
151
+			dphy->lane_mode = PHY_FULL_MODE;
152
+			dphy_hw->lane_mode = LANE_MODE_FULL;
153
+			break;
154
+		}
155
+		dphy->dphy_hw = dphy_hw;
156
+		dphy->phy_hw[index] = (void *)dphy_hw;
157
+		dphy->csi_info.dphy_vendor[index] = PHY_VENDOR_INNO;
158
+		mutex_unlock(&dphy_hw->mutex);
159
+	} else if (dphy->drv_data->chip_id == CHIP_ID_RK3588) {
160
+		if (csi_idx < 2) {
161
+			dcphy_hw = dphy->samsung_phy_group[csi_idx];
162
+			mutex_lock(&dcphy_hw->mutex);
163
+			dcphy_hw->dphy_dev_num++;
164
+			mutex_unlock(&dcphy_hw->mutex);
165
+			dphy->samsung_phy = dcphy_hw;
166
+			dphy->phy_hw[index] = (void *)dcphy_hw;
167
+			dphy->dphy_param = rk3588_dcphy_param;
168
+			dphy->csi_info.dphy_vendor[index] = PHY_VENDOR_SAMSUNG;
169
+		} else {
170
+			dphy_hw = dphy->dphy_hw_group[(csi_idx - 2) / 2];
171
+			mutex_lock(&dphy_hw->mutex);
172
+			if (csi_idx == 2 || csi_idx == 4) {
173
+				if (lanes == 4) {
174
+					dphy->lane_mode = PHY_FULL_MODE;
175
+					dphy_hw->lane_mode = LANE_MODE_FULL;
176
+					if (csi_idx == 2)
177
+						dphy->phy_index = 0;
178
+					else
179
+						dphy->phy_index = 3;
180
+				} else {
181
+					dphy->lane_mode = PHY_SPLIT_01;
182
+					dphy_hw->lane_mode = LANE_MODE_SPLIT;
183
+					if (csi_idx == 2)
184
+						dphy->phy_index = 1;
185
+					else
186
+						dphy->phy_index = 4;
187
+				}
188
+			} else if (csi_idx == 3 || csi_idx == 5) {
189
+				if (lanes == 4) {
190
+					dev_info(dphy->dev, "%s csi host%d only support PHY_SPLIT_23\n",
191
+						 __func__, csi_idx);
192
+					mutex_unlock(&dphy_hw->mutex);
193
+					return -EINVAL;
194
+				}
195
+				dphy->lane_mode = PHY_SPLIT_23;
196
+				dphy_hw->lane_mode = LANE_MODE_SPLIT;
197
+				if (csi_idx == 3)
198
+					dphy->phy_index = 2;
199
+				else
200
+					dphy->phy_index = 5;
201
+			}
202
+			dphy_hw->dphy_dev_num++;
203
+			dphy->dphy_hw = dphy_hw;
204
+			dphy->phy_hw[index] = (void *)dphy_hw;
205
+			dphy->csi_info.dphy_vendor[index] = PHY_VENDOR_INNO;
206
+			mutex_unlock(&dphy_hw->mutex);
207
+		}
208
+	} else {
209
+		dphy_hw = dphy->dphy_hw_group[csi_idx / 2];
210
+		mutex_lock(&dphy_hw->mutex);
211
+		if (csi_idx == 0 || csi_idx == 2) {
212
+			if (lanes == 4) {
213
+				dphy->lane_mode = PHY_FULL_MODE;
214
+				dphy_hw->lane_mode = LANE_MODE_FULL;
215
+				if (csi_idx == 0)
216
+					dphy->phy_index = 0;
217
+				else
218
+					dphy->phy_index = 3;
219
+			} else {
220
+				dphy->lane_mode = PHY_SPLIT_01;
221
+				dphy_hw->lane_mode = LANE_MODE_SPLIT;
222
+				if (csi_idx == 0)
223
+					dphy->phy_index = 1;
224
+				else
225
+					dphy->phy_index = 4;
226
+			}
227
+		} else if (csi_idx == 1 || csi_idx == 3) {
228
+			if (lanes == 4) {
229
+				dev_info(dphy->dev, "%s csi host%d only support PHY_SPLIT_23\n",
230
+					 __func__, csi_idx);
231
+				mutex_unlock(&dphy_hw->mutex);
232
+				return -EINVAL;
233
+			}
234
+			dphy->lane_mode = PHY_SPLIT_23;
235
+			dphy_hw->lane_mode = LANE_MODE_SPLIT;
236
+			if (csi_idx == 1)
237
+				dphy->phy_index = 2;
238
+			else
239
+				dphy->phy_index = 5;
240
+		} else {
241
+			dev_info(dphy->dev, "%s error csi host%d\n",
242
+				 __func__, csi_idx);
243
+			mutex_unlock(&dphy_hw->mutex);
244
+			return -EINVAL;
245
+		}
246
+		dphy_hw->dphy_dev[dphy_hw->dphy_dev_num] = dphy;
247
+		dphy_hw->dphy_dev_num++;
248
+		dphy->phy_hw[index] = (void *)dphy_hw;
249
+		dphy->csi_info.dphy_vendor[index] = PHY_VENDOR_INNO;
250
+		mutex_unlock(&dphy_hw->mutex);
251
+	}
252
+
253
+	return 0;
254
+}
255
+
256
+static int rockchip_csi2_dphy_detach_hw(struct csi2_dphy *dphy, int csi_idx, int index)
257
+{
258
+	struct csi2_dphy_hw *dphy_hw = NULL;
259
+	struct samsung_mipi_dcphy *dcphy_hw = NULL;
260
+	struct csi2_dphy *csi2_dphy = NULL;
261
+	int i = 0;
262
+
263
+	if (dphy->drv_data->chip_id == CHIP_ID_RK3568 ||
264
+	    dphy->drv_data->chip_id == CHIP_ID_RV1106) {
265
+		dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
266
+		if (!dphy_hw) {
267
+			dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
268
+				__func__, csi_idx);
269
+			return -EINVAL;
270
+		}
271
+		mutex_lock(&dphy_hw->mutex);
272
+		for (i = 0; i < dphy_hw->dphy_dev_num; i++) {
273
+			csi2_dphy = dphy_hw->dphy_dev[i];
274
+			if (csi2_dphy &&
275
+			    csi2_dphy->phy_index == dphy->phy_index) {
276
+				dphy_hw->dphy_dev[i] = NULL;
277
+				dphy_hw->dphy_dev_num--;
278
+				break;
279
+			}
280
+		}
281
+		mutex_unlock(&dphy_hw->mutex);
282
+	} else if (dphy->drv_data->chip_id == CHIP_ID_RK3588) {
283
+		if (csi_idx < 2) {
284
+			dcphy_hw = (struct samsung_mipi_dcphy *)dphy->phy_hw[index];
285
+			if (!dcphy_hw) {
286
+				dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
287
+					__func__, csi_idx);
288
+				return -EINVAL;
289
+			}
290
+			mutex_lock(&dcphy_hw->mutex);
291
+			dcphy_hw->dphy_dev_num--;
292
+			mutex_unlock(&dcphy_hw->mutex);
293
+		} else {
294
+			dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
295
+			if (!dphy_hw) {
296
+				dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
297
+					__func__, csi_idx);
298
+				return -EINVAL;
299
+			}
300
+			mutex_lock(&dphy_hw->mutex);
301
+			dphy_hw->dphy_dev_num--;
302
+			mutex_unlock(&dphy_hw->mutex);
303
+		}
304
+	} else {
305
+		dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
306
+		if (!dphy_hw) {
307
+			dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
308
+				__func__, csi_idx);
309
+			return -EINVAL;
310
+		}
311
+		mutex_lock(&dphy_hw->mutex);
312
+		dphy_hw->dphy_dev_num--;
313
+		mutex_unlock(&dphy_hw->mutex);
314
+	}
315
+
316
+	return 0;
317
+}
318
+
104
319
 static int csi2_dphy_update_sensor_mbus(struct v4l2_subdev *sd)
105
320
 {
106
321
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
107
322
 	struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
108
323
 	struct csi2_sensor *sensor;
109
324
 	struct v4l2_mbus_config mbus;
110
-	struct rkmodule_bus_config bus_config;
111
-	int ret;
325
+	int ret = 0;
112
326
 
113
327
 	if (!sensor_sd)
114
328
 		return -ENODEV;
..
..
@@ -137,86 +351,83 @@
137
351
 	default:
138
352
 		return -EINVAL;
139
353
 	}
140
-	if (dphy->drv_data->vendor == PHY_VENDOR_INNO) {
141
-		ret = v4l2_subdev_call(sensor_sd, core, ioctl,
142
-				       RKMODULE_GET_BUS_CONFIG, &bus_config);
143
-		if (!ret) {
144
-			dev_info(dphy->dev, "phy_mode %d,lane %d\n",
145
-				bus_config.bus.phy_mode, bus_config.bus.lanes);
146
-			if (bus_config.bus.phy_mode == PHY_FULL_MODE) {
147
-				if (dphy->dphy_hw->drv_data->chip_id == CHIP_ID_RK3588 &&
148
-				    dphy->phy_index % 3 == 2) {
149
-					dev_err(dphy->dev, "%s dphy%d only use for PHY_SPLIT_23\n",
150
-						__func__, dphy->phy_index);
151
-					ret = -EINVAL;
152
-				}
153
-				dphy->lane_mode = LANE_MODE_FULL;
154
-			} else if (bus_config.bus.phy_mode == PHY_SPLIT_01) {
155
-				if (dphy->dphy_hw->drv_data->chip_id == CHIP_ID_RK3588_DCPHY) {
156
-					dev_err(dphy->dev, "%s The chip not support split mode\n",
157
-						__func__);
158
-					ret = -EINVAL;
159
-				} else if (dphy->phy_index % 3 == 2) {
160
-					dev_err(dphy->dev, "%s dphy%d only use for PHY_SPLIT_23\n",
161
-						__func__, dphy->phy_index);
162
-					ret = -EINVAL;
163
-				} else {
164
-					dphy->lane_mode = LANE_MODE_SPLIT;
165
-				}
166
-			} else if (bus_config.bus.phy_mode == PHY_SPLIT_23) {
167
-				if (dphy->dphy_hw->drv_data->chip_id == CHIP_ID_RK3588_DCPHY) {
168
-					dev_err(dphy->dev, "%s The chip not support split mode\n",
169
-						__func__);
170
-					ret = -EINVAL;
171
-				} else if (dphy->phy_index % 3 != 2) {
172
-					dev_err(dphy->dev, "%s dphy%d not support PHY_SPLIT_23\n",
173
-						__func__, dphy->phy_index);
174
-					ret = -EINVAL;
175
-				} else {
176
-					dphy->lane_mode = LANE_MODE_SPLIT;
354
+
355
+	return 0;
356
+}
357
+
358
+static int csi2_dphy_update_config(struct v4l2_subdev *sd)
359
+{
360
+	struct csi2_dphy *dphy = to_csi2_dphy(sd);
361
+	struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
362
+	struct rkmodule_csi_dphy_param dphy_param;
363
+	struct rkmodule_bus_config bus_config;
364
+	int csi_idx = 0;
365
+	int ret = 0;
366
+	int i = 0;
367
+
368
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
369
+		if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
370
+		    dphy->drv_data->chip_id != CHIP_ID_RV1106) {
371
+			csi_idx = dphy->csi_info.csi_idx[i];
372
+			rockchip_csi2_dphy_attach_hw(dphy, csi_idx, i);
373
+		}
374
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_INNO) {
375
+			ret = v4l2_subdev_call(sensor_sd, core, ioctl,
376
+					       RKMODULE_GET_BUS_CONFIG, &bus_config);
377
+			if (!ret) {
378
+				dev_info(dphy->dev, "phy_mode %d,lane %d\n",
379
+					bus_config.bus.phy_mode, bus_config.bus.lanes);
380
+				if (bus_config.bus.phy_mode == PHY_FULL_MODE) {
381
+					if (dphy->phy_index % 3 == 2) {
382
+						dev_err(dphy->dev, "%s dphy%d only use for PHY_SPLIT_23\n",
383
+							__func__, dphy->phy_index);
384
+						return -EINVAL;
385
+					}
386
+					dphy->lane_mode = PHY_FULL_MODE;
387
+					dphy->dphy_hw->lane_mode = LANE_MODE_FULL;
388
+				} else if (bus_config.bus.phy_mode == PHY_SPLIT_01) {
389
+					if (dphy->phy_index % 3 == 2) {
390
+						dev_err(dphy->dev, "%s dphy%d only use for PHY_SPLIT_23\n",
391
+							__func__, dphy->phy_index);
392
+						return -EINVAL;
393
+					}
394
+					dphy->lane_mode = PHY_SPLIT_01;
395
+					dphy->dphy_hw->lane_mode = LANE_MODE_SPLIT;
396
+				} else if (bus_config.bus.phy_mode == PHY_SPLIT_23) {
397
+					if (dphy->phy_index % 3 != 2) {
398
+						dev_err(dphy->dev, "%s dphy%d not support PHY_SPLIT_23\n",
399
+							__func__, dphy->phy_index);
400
+						return -EINVAL;
401
+					}
402
+					dphy->lane_mode = PHY_SPLIT_23;
403
+					dphy->dphy_hw->lane_mode = LANE_MODE_SPLIT;
177
404
 				}
178
405
 			}
179
-			if (!ret)
180
-				dphy->dphy_hw->lane_mode = dphy->lane_mode;
181
-		} else {
182
-			ret = 0;
183
406
 		}
184
407
 	}
185
-	if (dphy->drv_data->vendor == PHY_VENDOR_SAMSUNG) {
186
-		ret = v4l2_subdev_call(sensor_sd, core, ioctl,
187
-				       RKMODULE_GET_CSI_DPHY_PARAM,
188
-				       &dphy->dphy_param);
189
-		if (ret) {
190
-			dev_dbg(dphy->dev, "%s fail to get dphy param, used default value\n",
191
-				__func__);
192
-			ret = 0;
193
-		}
194
-	}
195
-	return ret;
408
+	ret = v4l2_subdev_call(sensor_sd, core, ioctl,
409
+			       RKMODULE_GET_CSI_DPHY_PARAM,
410
+			       &dphy_param);
411
+	if (!ret)
412
+		dphy->dphy_param = dphy_param;
413
+	return 0;
196
414
 }
197
415
 
198
416
 static int csi2_dphy_s_stream_start(struct v4l2_subdev *sd)
199
417
 {
200
418
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
201
-	struct csi2_dphy_hw *hw = dphy->dphy_hw;
202
-	struct samsung_mipi_dcphy *samsung_phy = dphy->samsung_phy;
203
-	int  ret = 0;
419
+	int i = 0;
204
420
 
205
-	if (dphy->is_streaming)
206
-		return 0;
207
-
208
-	ret = csi2_dphy_get_sensor_data_rate(sd);
209
-	if (ret < 0)
210
-		return ret;
211
-
212
-	csi2_dphy_update_sensor_mbus(sd);
213
-
214
-	if (dphy->drv_data->vendor == PHY_VENDOR_SAMSUNG) {
215
-		if (samsung_phy && samsung_phy->stream_on)
216
-			samsung_phy->stream_on(dphy, sd);
217
-	} else {
218
-		if (hw->stream_on)
219
-			hw->stream_on(dphy, sd);
421
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
422
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
423
+			dphy->samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
424
+			if (dphy->samsung_phy && dphy->samsung_phy->stream_on)
425
+				dphy->samsung_phy->stream_on(dphy, sd);
426
+		} else {
427
+			dphy->dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
428
+			if (dphy->dphy_hw && dphy->dphy_hw->stream_on)
429
+				dphy->dphy_hw->stream_on(dphy, sd);
430
+		}
220
431
 	}
221
432
 
222
433
 	dphy->is_streaming = true;
..
..
@@ -227,18 +438,21 @@
227
438
 static int csi2_dphy_s_stream_stop(struct v4l2_subdev *sd)
228
439
 {
229
440
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
230
-	struct csi2_dphy_hw *hw = dphy->dphy_hw;
231
-	struct samsung_mipi_dcphy *samsung_phy = dphy->samsung_phy;
441
+	int i = 0;
232
442
 
233
-	if (!dphy->is_streaming)
234
-		return 0;
235
-
236
-	if (dphy->drv_data->vendor == PHY_VENDOR_SAMSUNG) {
237
-		if (samsung_phy && samsung_phy->stream_off)
238
-			samsung_phy->stream_off(dphy, sd);
239
-	} else {
240
-		if (hw->stream_off)
241
-			hw->stream_off(dphy, sd);
443
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
444
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
445
+			dphy->samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
446
+			if (dphy->samsung_phy && dphy->samsung_phy->stream_off)
447
+				dphy->samsung_phy->stream_off(dphy, sd);
448
+		} else {
449
+			dphy->dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
450
+			if (dphy->dphy_hw && dphy->dphy_hw->stream_off)
451
+				dphy->dphy_hw->stream_off(dphy, sd);
452
+		}
453
+		if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
454
+		    dphy->drv_data->chip_id != CHIP_ID_RV1106)
455
+			rockchip_csi2_dphy_detach_hw(dphy, dphy->csi_info.csi_idx[i], i);
242
456
 	}
243
457
 
244
458
 	dphy->is_streaming = false;
..
..
@@ -249,20 +463,94 @@
249
463
 	return 0;
250
464
 }
251
465
 
466
+static int csi2_dphy_enable_clk(struct csi2_dphy *dphy)
467
+{
468
+	struct csi2_dphy_hw *hw = NULL;
469
+	struct samsung_mipi_dcphy *samsung_phy = NULL;
470
+	int ret;
471
+	int i = 0;
472
+
473
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
474
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
475
+			samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
476
+			if (samsung_phy)
477
+				clk_prepare_enable(samsung_phy->pclk);
478
+		} else {
479
+			hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
480
+			if (hw) {
481
+				ret = clk_bulk_prepare_enable(hw->num_clks, hw->clks_bulk);
482
+				if (ret) {
483
+					dev_err(hw->dev, "failed to enable clks\n");
484
+					return ret;
485
+				}
486
+			}
487
+		}
488
+	}
489
+	return 0;
490
+}
491
+
492
+static void csi2_dphy_disable_clk(struct csi2_dphy *dphy)
493
+{
494
+	struct csi2_dphy_hw *hw = NULL;
495
+	struct samsung_mipi_dcphy *samsung_phy = NULL;
496
+	int i = 0;
497
+
498
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
499
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
500
+			samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
501
+			if (samsung_phy)
502
+				clk_disable_unprepare(samsung_phy->pclk);
503
+		} else {
504
+			hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
505
+			if (hw)
506
+				clk_bulk_disable_unprepare(hw->num_clks, hw->clks_bulk);
507
+		}
508
+	}
509
+}
510
+
252
511
 static int csi2_dphy_s_stream(struct v4l2_subdev *sd, int on)
253
512
 {
254
513
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
255
514
 	int ret = 0;
256
515
 
257
516
 	mutex_lock(&dphy->mutex);
258
-	if (on)
517
+	if (on) {
518
+		if (dphy->is_streaming) {
519
+			mutex_unlock(&dphy->mutex);
520
+			return 0;
521
+		}
522
+
523
+		ret = csi2_dphy_get_sensor_data_rate(sd);
524
+		if (ret < 0) {
525
+			mutex_unlock(&dphy->mutex);
526
+			return ret;
527
+		}
528
+
529
+		csi2_dphy_update_sensor_mbus(sd);
530
+		ret = csi2_dphy_update_config(sd);
531
+		if (ret < 0) {
532
+			mutex_unlock(&dphy->mutex);
533
+			return ret;
534
+		}
535
+
536
+		ret = csi2_dphy_enable_clk(dphy);
537
+		if (ret) {
538
+			mutex_unlock(&dphy->mutex);
539
+			return ret;
540
+		}
259
541
 		ret = csi2_dphy_s_stream_start(sd);
260
-	else
542
+	} else {
543
+		if (!dphy->is_streaming) {
544
+			mutex_unlock(&dphy->mutex);
545
+			return 0;
546
+		}
261
547
 		ret = csi2_dphy_s_stream_stop(sd);
548
+		csi2_dphy_disable_clk(dphy);
549
+	}
262
550
 	mutex_unlock(&dphy->mutex);
263
551
 
264
-	dev_info(dphy->dev, "%s stream on:%d, dphy%d\n",
265
-		 __func__, on, dphy->phy_index);
552
+	dev_info(dphy->dev, "%s stream on:%d, dphy%d, ret %d\n",
553
+		 __func__, on, dphy->phy_index, ret);
266
554
 
267
555
 	return ret;
268
556
 }
..
..
@@ -312,15 +600,10 @@
312
600
 	struct media_entity *me = dev_get_drvdata(dev);
313
601
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me);
314
602
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
315
-	struct csi2_dphy_hw *hw = dphy->dphy_hw;
316
-	struct samsung_mipi_dcphy *samsung_phy = dphy->samsung_phy;
317
603
 
318
-	if (dphy->drv_data->vendor == PHY_VENDOR_SAMSUNG) {
319
-		if (samsung_phy)
320
-			clk_disable_unprepare(samsung_phy->pclk);
321
-	} else {
322
-		if (hw)
323
-			clk_bulk_disable_unprepare(hw->num_clks, hw->clks_bulk);
604
+	if (dphy->is_streaming) {
605
+		csi2_dphy_s_stream(sd, 0);
606
+		dphy->is_streaming = false;
324
607
 	}
325
608
 
326
609
 	return 0;
..
..
@@ -328,26 +611,6 @@
328
611
 
329
612
 static __maybe_unused int csi2_dphy_runtime_resume(struct device *dev)
330
613
 {
331
-	struct media_entity *me = dev_get_drvdata(dev);
332
-	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me);
333
-	struct csi2_dphy *dphy = to_csi2_dphy(sd);
334
-	struct csi2_dphy_hw *hw = dphy->dphy_hw;
335
-	struct samsung_mipi_dcphy *samsung_phy = dphy->samsung_phy;
336
-	int ret;
337
-
338
-	if (dphy->drv_data->vendor == PHY_VENDOR_SAMSUNG) {
339
-		if (samsung_phy)
340
-			clk_prepare_enable(samsung_phy->pclk);
341
-	} else {
342
-		if (hw) {
343
-			ret = clk_bulk_prepare_enable(hw->num_clks, hw->clks_bulk);
344
-			if (ret) {
345
-				dev_err(hw->dev, "failed to enable clks\n");
346
-				return ret;
347
-			}
348
-		}
349
-	}
350
-
351
614
 	return 0;
352
615
 }
353
616
 
..
..
@@ -384,8 +647,55 @@
384
647
 	return v4l2_subdev_call(sensor, pad, get_selection, NULL, sel);
385
648
 }
386
649
 
650
+static long rkcif_csi2_dphy_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
651
+{
652
+	struct csi2_dphy *dphy = to_csi2_dphy(sd);
653
+	long ret = 0;
654
+
655
+	switch (cmd) {
656
+	case RKCIF_CMD_SET_CSI_IDX:
657
+		if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
658
+		    dphy->drv_data->chip_id != CHIP_ID_RV1106)
659
+			dphy->csi_info = *((struct rkcif_csi_info *)arg);
660
+		break;
661
+	default:
662
+		ret = -ENOIOCTLCMD;
663
+		break;
664
+	}
665
+
666
+	return ret;
667
+}
668
+
669
+#ifdef CONFIG_COMPAT
670
+static long rkcif_csi2_dphy_compat_ioctl32(struct v4l2_subdev *sd,
671
+				      unsigned int cmd, unsigned long arg)
672
+{
673
+	void __user *up = compat_ptr(arg);
674
+	struct rkcif_csi_info csi_info = {0};
675
+	long ret;
676
+
677
+	switch (cmd) {
678
+	case RKCIF_CMD_SET_CSI_IDX:
679
+		if (copy_from_user(&csi_info, up, sizeof(struct rkcif_csi_info)))
680
+			return -EFAULT;
681
+
682
+		ret = rkcif_csi2_dphy_ioctl(sd, cmd, &csi_info);
683
+		break;
684
+	default:
685
+		ret = -ENOIOCTLCMD;
686
+		break;
687
+	}
688
+
689
+	return ret;
690
+}
691
+#endif
692
+
387
693
 static const struct v4l2_subdev_core_ops csi2_dphy_core_ops = {
388
694
 	.s_power = csi2_dphy_s_power,
695
+	.ioctl = rkcif_csi2_dphy_ioctl,
696
+#ifdef CONFIG_COMPAT
697
+	.compat_ioctl32 = rkcif_csi2_dphy_compat_ioctl32,
698
+#endif
389
699
 };
390
700
 
391
701
 static const struct v4l2_subdev_video_ops csi2_dphy_video_ops = {
..
..
@@ -491,8 +801,9 @@
491
801
 		return -EINVAL;
492
802
 	}
493
803
 
494
-	if (vep->bus_type == V4L2_MBUS_CSI2_DPHY) {
495
-		config->type = V4L2_MBUS_CSI2_DPHY;
804
+	if (vep->bus_type == V4L2_MBUS_CSI2_DPHY ||
805
+	    vep->bus_type == V4L2_MBUS_CSI2_CPHY) {
806
+		config->type = vep->bus_type;
496
807
 		config->flags = vep->bus.mipi_csi2.flags;
497
808
 		s_asd->lanes = vep->bus.mipi_csi2.num_data_lanes;
498
809
 	} else if (vep->bus_type == V4L2_MBUS_CCP2) {
..
..
@@ -559,162 +870,32 @@
559
870
 	return v4l2_async_register_subdev(&dphy->sd);
560
871
 }
561
872
 
562
-static int rockchip_csi2_dphy_attach_samsung_phy(struct csi2_dphy *dphy)
563
-{
564
-	struct device *dev = dphy->dev;
565
-	struct phy *dcphy;
566
-	struct samsung_mipi_dcphy *dphy_hw;
567
-	int ret = 0;
568
-
569
-	dcphy = devm_phy_optional_get(dev, "dcphy");
570
-	if (IS_ERR(dcphy)) {
571
-		ret = PTR_ERR(dcphy);
572
-		dev_err(dphy->dev, "failed to get mipi dcphy: %d\n", ret);
573
-		return ret;
574
-	}
575
-
576
-	dphy_hw = phy_get_drvdata(dcphy);
577
-	dphy_hw->dphy_dev[dphy_hw->dphy_dev_num] = dphy;
578
-	dphy_hw->dphy_dev_num++;
579
-	dphy->samsung_phy = dphy_hw;
580
-
581
-	return 0;
582
-}
583
-
584
-static int rockchip_csi2_dphy_detach_samsung_phy(struct csi2_dphy *dphy)
585
-{
586
-	struct samsung_mipi_dcphy *dphy_hw = dphy->samsung_phy;
587
-	struct csi2_dphy *csi2_dphy = NULL;
588
-	int i;
589
-
590
-	for (i = 0; i < dphy_hw->dphy_dev_num; i++) {
591
-		csi2_dphy = dphy_hw->dphy_dev[i];
592
-		if (csi2_dphy &&
593
-		    csi2_dphy->phy_index == dphy->phy_index) {
594
-			dphy_hw->dphy_dev[i] = NULL;
595
-			dphy_hw->dphy_dev_num--;
596
-			break;
597
-		}
598
-	}
599
-
600
-	return 0;
601
-}
602
-
603
-static int rockchip_csi2_dphy_attach_hw(struct csi2_dphy *dphy)
604
-{
605
-	struct platform_device *plat_dev;
606
-	struct device *dev = dphy->dev;
607
-	struct csi2_dphy_hw *dphy_hw;
608
-	struct device_node *np;
609
-	enum csi2_dphy_lane_mode target_mode;
610
-	int i;
611
-
612
-	if (dphy->phy_index % 3 == 0)
613
-		target_mode = LANE_MODE_FULL;
614
-	else
615
-		target_mode = LANE_MODE_SPLIT;
616
-
617
-	np = of_parse_phandle(dev->of_node, "rockchip,hw", 0);
618
-	if (!np || !of_device_is_available(np)) {
619
-		dev_err(dphy->dev,
620
-			"failed to get dphy%d hw node\n", dphy->phy_index);
621
-		return -ENODEV;
622
-	}
623
-
624
-	plat_dev = of_find_device_by_node(np);
625
-	of_node_put(np);
626
-	if (!plat_dev) {
627
-		dev_err(dphy->dev,
628
-			"failed to get dphy%d hw from node\n",
629
-			dphy->phy_index);
630
-		return -ENODEV;
631
-	}
632
-
633
-	dphy_hw = platform_get_drvdata(plat_dev);
634
-	if (!dphy_hw) {
635
-		dev_err(dphy->dev,
636
-			"failed attach dphy%d hw\n",
637
-			dphy->phy_index);
638
-		return -EINVAL;
639
-	}
640
-
641
-	if (dphy_hw->lane_mode == LANE_MODE_UNDEF) {
642
-		dphy_hw->lane_mode = target_mode;
643
-	} else {
644
-		struct csi2_dphy *phy = dphy_hw->dphy_dev[0];
645
-
646
-		for (i = 0; i < dphy_hw->dphy_dev_num; i++) {
647
-			if (dphy_hw->dphy_dev[i]->lane_mode == dphy_hw->lane_mode) {
648
-				phy = dphy_hw->dphy_dev[i];
649
-				break;
650
-			}
651
-		}
652
-
653
-		if (target_mode != dphy_hw->lane_mode) {
654
-			dev_err(dphy->dev,
655
-				"Err:csi2 dphy hw has been set as %s mode by phy%d, target mode is:%s\n",
656
-				dphy_hw->lane_mode == LANE_MODE_FULL ? "full" : "split",
657
-				phy->phy_index,
658
-				target_mode == LANE_MODE_FULL ? "full" : "split");
659
-			return -ENODEV;
660
-		}
661
-	}
662
-
663
-	dphy_hw->dphy_dev[dphy_hw->dphy_dev_num] = dphy;
664
-	dphy_hw->dphy_dev_num++;
665
-	dphy->dphy_hw = dphy_hw;
666
-
667
-	return 0;
668
-}
669
-
670
-static int rockchip_csi2_dphy_detach_hw(struct csi2_dphy *dphy)
671
-{
672
-	struct csi2_dphy_hw *dphy_hw = dphy->dphy_hw;
673
-	struct csi2_dphy *csi2_dphy = NULL;
674
-	int i;
675
-
676
-	for (i = 0; i < dphy_hw->dphy_dev_num; i++) {
677
-		csi2_dphy = dphy_hw->dphy_dev[i];
678
-		if (csi2_dphy &&
679
-		    csi2_dphy->phy_index == dphy->phy_index) {
680
-			dphy_hw->dphy_dev[i] = NULL;
681
-			dphy_hw->dphy_dev_num--;
682
-			break;
683
-		}
684
-	}
685
-
686
-	return 0;
687
-}
688
-
689
873
 static struct dphy_drv_data rk3568_dphy_drv_data = {
690
874
 	.dev_name = "csi2dphy",
691
-	.vendor = PHY_VENDOR_INNO,
875
+	.chip_id = CHIP_ID_RK3568,
876
+	.num_inno_phy = 1,
877
+	.num_samsung_phy = 0,
692
878
 };
693
879
 
694
-static struct dphy_drv_data rk3588_dcphy_drv_data = {
695
-	.dev_name = "csi2dcphy",
696
-	.vendor = PHY_VENDOR_SAMSUNG,
697
-};
698
-
699
-static struct rkmodule_csi_dphy_param rk3588_dcphy_param = {
700
-	.vendor = PHY_VENDOR_SAMSUNG,
701
-	.lp_vol_ref = 3,
702
-	.lp_hys_sw = {3, 0, 0, 0},
703
-	.lp_escclk_pol_sel = {1, 0, 0, 0},
704
-	.skew_data_cal_clk = {0, 3, 3, 3},
705
-	.clk_hs_term_sel = 2,
706
-	.data_hs_term_sel = {2, 2, 2, 2},
707
-	.reserved = {0},
880
+static struct dphy_drv_data rk3588_dphy_drv_data = {
881
+	.dev_name = "csi2dphy",
882
+	.chip_id = CHIP_ID_RK3588,
883
+	.num_inno_phy = 2,
884
+	.num_samsung_phy = 2,
708
885
 };
709
886
 
710
887
 static struct dphy_drv_data rv1106_dphy_drv_data = {
711
888
 	.dev_name = "csi2dphy",
712
-	.vendor = PHY_VENDOR_INNO,
889
+	.chip_id = CHIP_ID_RV1106,
890
+	.num_inno_phy = 1,
891
+	.num_samsung_phy = 0,
713
892
 };
714
893
 
715
894
 static struct dphy_drv_data rk3562_dphy_drv_data = {
716
895
 	.dev_name = "csi2dphy",
717
-	.vendor = PHY_VENDOR_INNO,
896
+	.chip_id = CHIP_ID_RK3562,
897
+	.num_inno_phy = 2,
898
+	.num_samsung_phy = 0,
718
899
 };
719
900
 
720
901
 static const struct of_device_id rockchip_csi2_dphy_match_id[] = {
..
..
@@ -723,8 +904,8 @@
723
904
 		.data = &rk3568_dphy_drv_data,
724
905
 	},
725
906
 	{
726
-		.compatible = "rockchip,rk3588-csi2-dcphy",
727
-		.data = &rk3588_dcphy_drv_data,
907
+		.compatible = "rockchip,rk3588-csi2-dphy",
908
+		.data = &rk3588_dphy_drv_data,
728
909
 	},
729
910
 	{
730
911
 		.compatible = "rockchip,rv1106-csi2-dphy",
..
..
@@ -737,6 +918,79 @@
737
918
 	{}
738
919
 };
739
920
 MODULE_DEVICE_TABLE(of, rockchip_csi2_dphy_match_id);
921
+
922
+static int rockchip_csi2_dphy_get_samsung_phy_hw(struct csi2_dphy *dphy)
923
+{
924
+	struct phy *dcphy;
925
+	struct device *dev = dphy->dev;
926
+	struct samsung_mipi_dcphy *dcphy_hw;
927
+	char phy_name[32];
928
+	int i = 0;
929
+	int ret = 0;
930
+
931
+	for (i = 0; i < dphy->drv_data->num_samsung_phy; i++) {
932
+		sprintf(phy_name, "dcphy%d", i);
933
+		dcphy = devm_phy_optional_get(dev, phy_name);
934
+		if (IS_ERR(dcphy)) {
935
+			ret = PTR_ERR(dcphy);
936
+			dev_err(dphy->dev, "failed to get mipi dcphy: %d\n", ret);
937
+			return ret;
938
+		}
939
+		dcphy_hw = phy_get_drvdata(dcphy);
940
+		dphy->samsung_phy_group[i] = dcphy_hw;
941
+	}
942
+	return 0;
943
+}
944
+
945
+static int rockchip_csi2_dphy_get_inno_phy_hw(struct csi2_dphy *dphy)
946
+{
947
+	struct platform_device *plat_dev;
948
+	struct device *dev = dphy->dev;
949
+	struct csi2_dphy_hw *dphy_hw;
950
+	struct device_node *np;
951
+	int i = 0;
952
+
953
+	for (i = 0; i < dphy->drv_data->num_inno_phy; i++) {
954
+		np = of_parse_phandle(dev->of_node, "rockchip,hw", i);
955
+		if (!np || !of_device_is_available(np)) {
956
+			dev_err(dphy->dev,
957
+				"failed to get dphy%d hw node\n", dphy->phy_index);
958
+			return -ENODEV;
959
+		}
960
+		plat_dev = of_find_device_by_node(np);
961
+		of_node_put(np);
962
+		if (!plat_dev) {
963
+			dev_err(dphy->dev,
964
+				"failed to get dphy%d hw from node\n",
965
+				dphy->phy_index);
966
+			return -ENODEV;
967
+		}
968
+		dphy_hw = platform_get_drvdata(plat_dev);
969
+		if (!dphy_hw) {
970
+			dev_err(dphy->dev,
971
+				"failed attach dphy%d hw\n",
972
+				dphy->phy_index);
973
+			return -EINVAL;
974
+		}
975
+		dphy->dphy_hw_group[i] = dphy_hw;
976
+	}
977
+	return 0;
978
+}
979
+
980
+static int rockchip_csi2_dphy_get_hw(struct csi2_dphy *dphy)
981
+{
982
+	int ret = 0;
983
+
984
+	if (dphy->drv_data->chip_id == CHIP_ID_RK3588) {
985
+		ret = rockchip_csi2_dphy_get_samsung_phy_hw(dphy);
986
+		if (ret)
987
+			return ret;
988
+		ret = rockchip_csi2_dphy_get_inno_phy_hw(dphy);
989
+	} else {
990
+		ret = rockchip_csi2_dphy_get_inno_phy_hw(dphy);
991
+	}
992
+	return ret;
993
+}
740
994
 
741
995
 static int rockchip_csi2_dphy_probe(struct platform_device *pdev)
742
996
 {
..
..
@@ -757,22 +1011,22 @@
757
1011
 		return -EINVAL;
758
1012
 	drv_data = of_id->data;
759
1013
 	csi2dphy->drv_data = drv_data;
1014
+
760
1015
 	csi2dphy->phy_index = of_alias_get_id(dev->of_node, drv_data->dev_name);
761
1016
 	if (csi2dphy->phy_index < 0 || csi2dphy->phy_index >= PHY_MAX)
762
1017
 		csi2dphy->phy_index = 0;
763
-	if (csi2dphy->drv_data->vendor == PHY_VENDOR_SAMSUNG) {
764
-		ret = rockchip_csi2_dphy_attach_samsung_phy(csi2dphy);
765
-		csi2dphy->dphy_param = rk3588_dcphy_param;
766
-	} else {
767
-		ret = rockchip_csi2_dphy_attach_hw(csi2dphy);
768
-	}
769
-	if (ret) {
770
-		dev_err(dev,
771
-			"csi2 dphy hw can't be attached, register dphy%d failed!\n",
772
-			csi2dphy->phy_index);
773
-		return -ENODEV;
774
-	}
775
1018
 
1019
+	ret = rockchip_csi2_dphy_get_hw(csi2dphy);
1020
+	if (ret)
1021
+		return -EINVAL;
1022
+	if (csi2dphy->drv_data->chip_id == CHIP_ID_RK3568 ||
1023
+	    csi2dphy->drv_data->chip_id == CHIP_ID_RV1106) {
1024
+		csi2dphy->csi_info.csi_num = 1;
1025
+		csi2dphy->csi_info.dphy_vendor[0] = PHY_VENDOR_INNO;
1026
+		rockchip_csi2_dphy_attach_hw(csi2dphy, 0, 0);
1027
+	} else {
1028
+		csi2dphy->csi_info.csi_num = 0;
1029
+	}
776
1030
 	sd = &csi2dphy->sd;
777
1031
 	mutex_init(&csi2dphy->mutex);
778
1032
 	v4l2_subdev_init(sd, &csi2_dphy_subdev_ops);
..
..
@@ -795,12 +1049,7 @@
795
1049
 
796
1050
 detach_hw:
797
1051
 	mutex_destroy(&csi2dphy->mutex);
798
-	if (csi2dphy->drv_data->vendor == PHY_VENDOR_SAMSUNG)
799
-		rockchip_csi2_dphy_detach_samsung_phy(csi2dphy);
800
-	else
801
-		rockchip_csi2_dphy_detach_hw(csi2dphy);
802
-
803
-	return 0;
1052
+	return -EINVAL;
804
1053
 }
805
1054
 
806
1055
 static int rockchip_csi2_dphy_remove(struct platform_device *pdev)
..
..
@@ -808,7 +1057,10 @@
808
1057
 	struct media_entity *me = platform_get_drvdata(pdev);
809
1058
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me);
810
1059
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
1060
+	int i = 0;
811
1061
 
1062
+	for (i = 0; i < dphy->csi_info.csi_num; i++)
1063
+		rockchip_csi2_dphy_detach_hw(dphy, dphy->csi_info.csi_idx[i], i);
812
1064
 	media_entity_cleanup(&sd->entity);
813
1065
 
814
1066
 	pm_runtime_disable(&pdev->dev);