AX88772C_eeprom and ax8872c build together

hc

2024-05-16 8d2a02b24d66aa359e83eebc1ed3c0f85367a1cb

 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,252 @@
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[dcphy_hw->dphy_dev_num] = dphy;
164
+			dcphy_hw->dphy_dev_num++;
165
+			mutex_unlock(&dcphy_hw->mutex);
166
+			dphy->samsung_phy = dcphy_hw;
167
+			dphy->phy_hw[index] = (void *)dcphy_hw;
168
+			dphy->dphy_param = rk3588_dcphy_param;
169
+			dphy->csi_info.dphy_vendor[index] = PHY_VENDOR_SAMSUNG;
170
+		} else {
171
+			dphy_hw = dphy->dphy_hw_group[(csi_idx - 2) / 2];
172
+			mutex_lock(&dphy_hw->mutex);
173
+			if (csi_idx == 2 || csi_idx == 4) {
174
+				if (lanes == 4) {
175
+					dphy->lane_mode = PHY_FULL_MODE;
176
+					dphy_hw->lane_mode = LANE_MODE_FULL;
177
+					if (csi_idx == 2)
178
+						dphy->phy_index = 0;
179
+					else
180
+						dphy->phy_index = 3;
181
+				} else {
182
+					dphy->lane_mode = PHY_SPLIT_01;
183
+					dphy_hw->lane_mode = LANE_MODE_SPLIT;
184
+					if (csi_idx == 2)
185
+						dphy->phy_index = 1;
186
+					else
187
+						dphy->phy_index = 4;
188
+				}
189
+			} else if (csi_idx == 3 || csi_idx == 5) {
190
+				if (lanes == 4) {
191
+					dev_info(dphy->dev, "%s csi host%d only support PHY_SPLIT_23\n",
192
+						 __func__, csi_idx);
193
+					mutex_unlock(&dphy_hw->mutex);
194
+					return -EINVAL;
195
+				}
196
+				dphy->lane_mode = PHY_SPLIT_23;
197
+				dphy_hw->lane_mode = LANE_MODE_SPLIT;
198
+				if (csi_idx == 3)
199
+					dphy->phy_index = 2;
200
+				else
201
+					dphy->phy_index = 5;
202
+			}
203
+			dphy_hw->dphy_dev[dphy_hw->dphy_dev_num] = dphy;
204
+			dphy_hw->dphy_dev_num++;
205
+			dphy->dphy_hw = dphy_hw;
206
+			dphy->phy_hw[index] = (void *)dphy_hw;
207
+			dphy->csi_info.dphy_vendor[index] = PHY_VENDOR_INNO;
208
+			mutex_unlock(&dphy_hw->mutex);
209
+		}
210
+	} else {
211
+		dphy_hw = dphy->dphy_hw_group[csi_idx / 2];
212
+		mutex_lock(&dphy_hw->mutex);
213
+		if (csi_idx == 0 || csi_idx == 2) {
214
+			if (lanes == 4) {
215
+				dphy->lane_mode = PHY_FULL_MODE;
216
+				dphy_hw->lane_mode = LANE_MODE_FULL;
217
+				if (csi_idx == 0)
218
+					dphy->phy_index = 0;
219
+				else
220
+					dphy->phy_index = 3;
221
+			} else {
222
+				dphy->lane_mode = PHY_SPLIT_01;
223
+				dphy_hw->lane_mode = LANE_MODE_SPLIT;
224
+				if (csi_idx == 0)
225
+					dphy->phy_index = 1;
226
+				else
227
+					dphy->phy_index = 4;
228
+			}
229
+		} else if (csi_idx == 1 || csi_idx == 3) {
230
+			if (lanes == 4) {
231
+				dev_info(dphy->dev, "%s csi host%d only support PHY_SPLIT_23\n",
232
+					 __func__, csi_idx);
233
+				mutex_unlock(&dphy_hw->mutex);
234
+				return -EINVAL;
235
+			}
236
+			dphy->lane_mode = PHY_SPLIT_23;
237
+			dphy_hw->lane_mode = LANE_MODE_SPLIT;
238
+			if (csi_idx == 1)
239
+				dphy->phy_index = 2;
240
+			else
241
+				dphy->phy_index = 5;
242
+		} else {
243
+			dev_info(dphy->dev, "%s error csi host%d\n",
244
+				 __func__, csi_idx);
245
+			mutex_unlock(&dphy_hw->mutex);
246
+			return -EINVAL;
247
+		}
248
+		dphy_hw->dphy_dev[dphy_hw->dphy_dev_num] = dphy;
249
+		dphy_hw->dphy_dev_num++;
250
+		dphy->phy_hw[index] = (void *)dphy_hw;
251
+		dphy->csi_info.dphy_vendor[index] = PHY_VENDOR_INNO;
252
+		mutex_unlock(&dphy_hw->mutex);
253
+	}
254
+
255
+	return 0;
256
+}
257
+
258
+static void rockchip_csi2_samsung_phy_remove_dphy_dev(struct csi2_dphy *dphy,
259
+						   struct samsung_mipi_dcphy *dcphy_hw)
260
+{
261
+	int i = 0;
262
+	bool is_find_dev = false;
263
+	struct csi2_dphy *csi2_dphy = NULL;
264
+
265
+	for (i = 0; i < dcphy_hw->dphy_dev_num; i++) {
266
+		csi2_dphy = dcphy_hw->dphy_dev[i];
267
+		if (csi2_dphy &&
268
+		    csi2_dphy->phy_index == dphy->phy_index)
269
+			is_find_dev = true;
270
+		if (is_find_dev) {
271
+			if (i < dcphy_hw->dphy_dev_num - 1)
272
+				dcphy_hw->dphy_dev[i] = dcphy_hw->dphy_dev[i + 1];
273
+			else
274
+				dcphy_hw->dphy_dev[i] = NULL;
275
+		}
276
+	}
277
+	if (is_find_dev)
278
+		dcphy_hw->dphy_dev_num--;
279
+}
280
+
281
+static void rockchip_csi2_inno_phy_remove_dphy_dev(struct csi2_dphy *dphy,
282
+						   struct csi2_dphy_hw *dphy_hw)
283
+{
284
+	int i = 0;
285
+	bool is_find_dev = false;
286
+	struct csi2_dphy *csi2_dphy = NULL;
287
+
288
+	for (i = 0; i < dphy_hw->dphy_dev_num; i++) {
289
+		csi2_dphy = dphy_hw->dphy_dev[i];
290
+		if (csi2_dphy &&
291
+		    csi2_dphy->phy_index == dphy->phy_index)
292
+			is_find_dev = true;
293
+		if (is_find_dev) {
294
+			if (i < dphy_hw->dphy_dev_num - 1)
295
+				dphy_hw->dphy_dev[i] = dphy_hw->dphy_dev[i + 1];
296
+			else
297
+				dphy_hw->dphy_dev[i] = NULL;
298
+		}
299
+	}
300
+	if (is_find_dev)
301
+		dphy_hw->dphy_dev_num--;
302
+}
303
+
304
+static int rockchip_csi2_dphy_detach_hw(struct csi2_dphy *dphy, int csi_idx, int index)
305
+{
306
+	struct csi2_dphy_hw *dphy_hw = NULL;
307
+	struct samsung_mipi_dcphy *dcphy_hw = NULL;
308
+
309
+	if (dphy->drv_data->chip_id == CHIP_ID_RK3568 ||
310
+	    dphy->drv_data->chip_id == CHIP_ID_RV1106) {
311
+		dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
312
+		if (!dphy_hw) {
313
+			dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
314
+				__func__, csi_idx);
315
+			return -EINVAL;
316
+		}
317
+		mutex_lock(&dphy_hw->mutex);
318
+		rockchip_csi2_inno_phy_remove_dphy_dev(dphy, dphy_hw);
319
+		mutex_unlock(&dphy_hw->mutex);
320
+	} else if (dphy->drv_data->chip_id == CHIP_ID_RK3588) {
321
+		if (csi_idx < 2) {
322
+			dcphy_hw = (struct samsung_mipi_dcphy *)dphy->phy_hw[index];
323
+			if (!dcphy_hw) {
324
+				dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
325
+					__func__, csi_idx);
326
+				return -EINVAL;
327
+			}
328
+			mutex_lock(&dcphy_hw->mutex);
329
+			rockchip_csi2_samsung_phy_remove_dphy_dev(dphy, dcphy_hw);
330
+			mutex_unlock(&dcphy_hw->mutex);
331
+		} else {
332
+			dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
333
+			if (!dphy_hw) {
334
+				dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
335
+					__func__, csi_idx);
336
+				return -EINVAL;
337
+			}
338
+			mutex_lock(&dphy_hw->mutex);
339
+			rockchip_csi2_inno_phy_remove_dphy_dev(dphy, dphy_hw);
340
+			mutex_unlock(&dphy_hw->mutex);
341
+		}
342
+	} else {
343
+		dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
344
+		if (!dphy_hw) {
345
+			dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
346
+				__func__, csi_idx);
347
+			return -EINVAL;
348
+		}
349
+		mutex_lock(&dphy_hw->mutex);
350
+		rockchip_csi2_inno_phy_remove_dphy_dev(dphy, dphy_hw);
351
+		mutex_unlock(&dphy_hw->mutex);
352
+	}
353
+
354
+	return 0;
355
+}
356
+
104
357
 static int csi2_dphy_update_sensor_mbus(struct v4l2_subdev *sd)
105
358
 {
106
359
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
107
360
 	struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
108
361
 	struct csi2_sensor *sensor;
109
362
 	struct v4l2_mbus_config mbus;
110
-	struct rkmodule_bus_config bus_config;
111
-	int ret;
363
+	int ret = 0;
112
364
 
113
365
 	if (!sensor_sd)
114
366
 		return -ENODEV;
..
..
@@ -137,86 +389,83 @@
137
389
 	default:
138
390
 		return -EINVAL;
139
391
 	}
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;
392
+
393
+	return 0;
394
+}
395
+
396
+static int csi2_dphy_update_config(struct v4l2_subdev *sd)
397
+{
398
+	struct csi2_dphy *dphy = to_csi2_dphy(sd);
399
+	struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
400
+	struct rkmodule_csi_dphy_param dphy_param;
401
+	struct rkmodule_bus_config bus_config;
402
+	int csi_idx = 0;
403
+	int ret = 0;
404
+	int i = 0;
405
+
406
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
407
+		if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
408
+		    dphy->drv_data->chip_id != CHIP_ID_RV1106) {
409
+			csi_idx = dphy->csi_info.csi_idx[i];
410
+			rockchip_csi2_dphy_attach_hw(dphy, csi_idx, i);
411
+		}
412
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_INNO) {
413
+			ret = v4l2_subdev_call(sensor_sd, core, ioctl,
414
+					       RKMODULE_GET_BUS_CONFIG, &bus_config);
415
+			if (!ret) {
416
+				dev_info(dphy->dev, "phy_mode %d,lane %d\n",
417
+					bus_config.bus.phy_mode, bus_config.bus.lanes);
418
+				if (bus_config.bus.phy_mode == PHY_FULL_MODE) {
419
+					if (dphy->phy_index % 3 == 2) {
420
+						dev_err(dphy->dev, "%s dphy%d only use for PHY_SPLIT_23\n",
421
+							__func__, dphy->phy_index);
422
+						return -EINVAL;
423
+					}
424
+					dphy->lane_mode = PHY_FULL_MODE;
425
+					dphy->dphy_hw->lane_mode = LANE_MODE_FULL;
426
+				} else if (bus_config.bus.phy_mode == PHY_SPLIT_01) {
427
+					if (dphy->phy_index % 3 == 2) {
428
+						dev_err(dphy->dev, "%s dphy%d only use for PHY_SPLIT_23\n",
429
+							__func__, dphy->phy_index);
430
+						return -EINVAL;
431
+					}
432
+					dphy->lane_mode = PHY_SPLIT_01;
433
+					dphy->dphy_hw->lane_mode = LANE_MODE_SPLIT;
434
+				} else if (bus_config.bus.phy_mode == PHY_SPLIT_23) {
435
+					if (dphy->phy_index % 3 != 2) {
436
+						dev_err(dphy->dev, "%s dphy%d not support PHY_SPLIT_23\n",
437
+							__func__, dphy->phy_index);
438
+						return -EINVAL;
439
+					}
440
+					dphy->lane_mode = PHY_SPLIT_23;
441
+					dphy->dphy_hw->lane_mode = LANE_MODE_SPLIT;
177
442
 				}
178
443
 			}
179
-			if (!ret)
180
-				dphy->dphy_hw->lane_mode = dphy->lane_mode;
181
-		} else {
182
-			ret = 0;
183
444
 		}
184
445
 	}
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;
446
+	ret = v4l2_subdev_call(sensor_sd, core, ioctl,
447
+			       RKMODULE_GET_CSI_DPHY_PARAM,
448
+			       &dphy_param);
449
+	if (!ret)
450
+		dphy->dphy_param = dphy_param;
451
+	return 0;
196
452
 }
197
453
 
198
454
 static int csi2_dphy_s_stream_start(struct v4l2_subdev *sd)
199
455
 {
200
456
 	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;
457
+	int i = 0;
204
458
 
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);
459
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
460
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
461
+			dphy->samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
462
+			if (dphy->samsung_phy && dphy->samsung_phy->stream_on)
463
+				dphy->samsung_phy->stream_on(dphy, sd);
464
+		} else {
465
+			dphy->dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
466
+			if (dphy->dphy_hw && dphy->dphy_hw->stream_on)
467
+				dphy->dphy_hw->stream_on(dphy, sd);
468
+		}
220
469
 	}
221
470
 
222
471
 	dphy->is_streaming = true;
..
..
@@ -227,18 +476,21 @@
227
476
 static int csi2_dphy_s_stream_stop(struct v4l2_subdev *sd)
228
477
 {
229
478
 	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;
479
+	int i = 0;
232
480
 
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);
481
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
482
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
483
+			dphy->samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
484
+			if (dphy->samsung_phy && dphy->samsung_phy->stream_off)
485
+				dphy->samsung_phy->stream_off(dphy, sd);
486
+		} else {
487
+			dphy->dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
488
+			if (dphy->dphy_hw && dphy->dphy_hw->stream_off)
489
+				dphy->dphy_hw->stream_off(dphy, sd);
490
+		}
491
+		if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
492
+		    dphy->drv_data->chip_id != CHIP_ID_RV1106)
493
+			rockchip_csi2_dphy_detach_hw(dphy, dphy->csi_info.csi_idx[i], i);
242
494
 	}
243
495
 
244
496
 	dphy->is_streaming = false;
..
..
@@ -249,20 +501,94 @@
249
501
 	return 0;
250
502
 }
251
503
 
504
+static int csi2_dphy_enable_clk(struct csi2_dphy *dphy)
505
+{
506
+	struct csi2_dphy_hw *hw = NULL;
507
+	struct samsung_mipi_dcphy *samsung_phy = NULL;
508
+	int ret;
509
+	int i = 0;
510
+
511
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
512
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
513
+			samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
514
+			if (samsung_phy)
515
+				clk_prepare_enable(samsung_phy->pclk);
516
+		} else {
517
+			hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
518
+			if (hw) {
519
+				ret = clk_bulk_prepare_enable(hw->num_clks, hw->clks_bulk);
520
+				if (ret) {
521
+					dev_err(hw->dev, "failed to enable clks\n");
522
+					return ret;
523
+				}
524
+			}
525
+		}
526
+	}
527
+	return 0;
528
+}
529
+
530
+static void csi2_dphy_disable_clk(struct csi2_dphy *dphy)
531
+{
532
+	struct csi2_dphy_hw *hw = NULL;
533
+	struct samsung_mipi_dcphy *samsung_phy = NULL;
534
+	int i = 0;
535
+
536
+	for (i = 0; i < dphy->csi_info.csi_num; i++) {
537
+		if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
538
+			samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
539
+			if (samsung_phy)
540
+				clk_disable_unprepare(samsung_phy->pclk);
541
+		} else {
542
+			hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
543
+			if (hw)
544
+				clk_bulk_disable_unprepare(hw->num_clks, hw->clks_bulk);
545
+		}
546
+	}
547
+}
548
+
252
549
 static int csi2_dphy_s_stream(struct v4l2_subdev *sd, int on)
253
550
 {
254
551
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
255
552
 	int ret = 0;
256
553
 
257
554
 	mutex_lock(&dphy->mutex);
258
-	if (on)
555
+	if (on) {
556
+		if (dphy->is_streaming) {
557
+			mutex_unlock(&dphy->mutex);
558
+			return 0;
559
+		}
560
+
561
+		ret = csi2_dphy_get_sensor_data_rate(sd);
562
+		if (ret < 0) {
563
+			mutex_unlock(&dphy->mutex);
564
+			return ret;
565
+		}
566
+
567
+		csi2_dphy_update_sensor_mbus(sd);
568
+		ret = csi2_dphy_update_config(sd);
569
+		if (ret < 0) {
570
+			mutex_unlock(&dphy->mutex);
571
+			return ret;
572
+		}
573
+
574
+		ret = csi2_dphy_enable_clk(dphy);
575
+		if (ret) {
576
+			mutex_unlock(&dphy->mutex);
577
+			return ret;
578
+		}
259
579
 		ret = csi2_dphy_s_stream_start(sd);
260
-	else
580
+	} else {
581
+		if (!dphy->is_streaming) {
582
+			mutex_unlock(&dphy->mutex);
583
+			return 0;
584
+		}
261
585
 		ret = csi2_dphy_s_stream_stop(sd);
586
+		csi2_dphy_disable_clk(dphy);
587
+	}
262
588
 	mutex_unlock(&dphy->mutex);
263
589
 
264
-	dev_info(dphy->dev, "%s stream on:%d, dphy%d\n",
265
-		 __func__, on, dphy->phy_index);
590
+	dev_info(dphy->dev, "%s stream on:%d, dphy%d, ret %d\n",
591
+		 __func__, on, dphy->phy_index, ret);
266
592
 
267
593
 	return ret;
268
594
 }
..
..
@@ -312,15 +638,10 @@
312
638
 	struct media_entity *me = dev_get_drvdata(dev);
313
639
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me);
314
640
 	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
641
 
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);
642
+	if (dphy->is_streaming) {
643
+		csi2_dphy_s_stream(sd, 0);
644
+		dphy->is_streaming = false;
324
645
 	}
325
646
 
326
647
 	return 0;
..
..
@@ -328,26 +649,6 @@
328
649
 
329
650
 static __maybe_unused int csi2_dphy_runtime_resume(struct device *dev)
330
651
 {
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
652
 	return 0;
352
653
 }
353
654
 
..
..
@@ -384,8 +685,75 @@
384
685
 	return v4l2_subdev_call(sensor, pad, get_selection, NULL, sel);
385
686
 }
386
687
 
688
+static long rkcif_csi2_dphy_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
689
+{
690
+	struct csi2_dphy *dphy = to_csi2_dphy(sd);
691
+	long ret = 0;
692
+	int i = 0;
693
+	int on = 0;
694
+
695
+	switch (cmd) {
696
+	case RKCIF_CMD_SET_CSI_IDX:
697
+		if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
698
+		    dphy->drv_data->chip_id != CHIP_ID_RV1106)
699
+			dphy->csi_info = *((struct rkcif_csi_info *)arg);
700
+		break;
701
+	case RKMODULE_SET_QUICK_STREAM:
702
+		for (i = 0; i < dphy->csi_info.csi_num; i++) {
703
+			if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_INNO) {
704
+				dphy->dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
705
+				if (!dphy->dphy_hw ||
706
+				    !dphy->dphy_hw->quick_stream_off ||
707
+				    !dphy->dphy_hw->quick_stream_on) {
708
+					ret = -EINVAL;
709
+					break;
710
+				}
711
+				on = *(int *)arg;
712
+				if (on)
713
+					dphy->dphy_hw->quick_stream_on(dphy, sd);
714
+				else
715
+					dphy->dphy_hw->quick_stream_off(dphy, sd);
716
+			}
717
+		}
718
+		break;
719
+	default:
720
+		ret = -ENOIOCTLCMD;
721
+		break;
722
+	}
723
+
724
+	return ret;
725
+}
726
+
727
+#ifdef CONFIG_COMPAT
728
+static long rkcif_csi2_dphy_compat_ioctl32(struct v4l2_subdev *sd,
729
+				      unsigned int cmd, unsigned long arg)
730
+{
731
+	void __user *up = compat_ptr(arg);
732
+	struct rkcif_csi_info csi_info = {0};
733
+	long ret;
734
+
735
+	switch (cmd) {
736
+	case RKCIF_CMD_SET_CSI_IDX:
737
+		if (copy_from_user(&csi_info, up, sizeof(struct rkcif_csi_info)))
738
+			return -EFAULT;
739
+
740
+		ret = rkcif_csi2_dphy_ioctl(sd, cmd, &csi_info);
741
+		break;
742
+	default:
743
+		ret = -ENOIOCTLCMD;
744
+		break;
745
+	}
746
+
747
+	return ret;
748
+}
749
+#endif
750
+
387
751
 static const struct v4l2_subdev_core_ops csi2_dphy_core_ops = {
388
752
 	.s_power = csi2_dphy_s_power,
753
+	.ioctl = rkcif_csi2_dphy_ioctl,
754
+#ifdef CONFIG_COMPAT
755
+	.compat_ioctl32 = rkcif_csi2_dphy_compat_ioctl32,
756
+#endif
389
757
 };
390
758
 
391
759
 static const struct v4l2_subdev_video_ops csi2_dphy_video_ops = {
..
..
@@ -491,8 +859,9 @@
491
859
 		return -EINVAL;
492
860
 	}
493
861
 
494
-	if (vep->bus_type == V4L2_MBUS_CSI2_DPHY) {
495
-		config->type = V4L2_MBUS_CSI2_DPHY;
862
+	if (vep->bus_type == V4L2_MBUS_CSI2_DPHY ||
863
+	    vep->bus_type == V4L2_MBUS_CSI2_CPHY) {
864
+		config->type = vep->bus_type;
496
865
 		config->flags = vep->bus.mipi_csi2.flags;
497
866
 		s_asd->lanes = vep->bus.mipi_csi2.num_data_lanes;
498
867
 	} else if (vep->bus_type == V4L2_MBUS_CCP2) {
..
..
@@ -559,162 +928,32 @@
559
928
 	return v4l2_async_register_subdev(&dphy->sd);
560
929
 }
561
930
 
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
931
 static struct dphy_drv_data rk3568_dphy_drv_data = {
690
932
 	.dev_name = "csi2dphy",
691
-	.vendor = PHY_VENDOR_INNO,
933
+	.chip_id = CHIP_ID_RK3568,
934
+	.num_inno_phy = 1,
935
+	.num_samsung_phy = 0,
692
936
 };
693
937
 
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},
938
+static struct dphy_drv_data rk3588_dphy_drv_data = {
939
+	.dev_name = "csi2dphy",
940
+	.chip_id = CHIP_ID_RK3588,
941
+	.num_inno_phy = 2,
942
+	.num_samsung_phy = 2,
708
943
 };
709
944
 
710
945
 static struct dphy_drv_data rv1106_dphy_drv_data = {
711
946
 	.dev_name = "csi2dphy",
712
-	.vendor = PHY_VENDOR_INNO,
947
+	.chip_id = CHIP_ID_RV1106,
948
+	.num_inno_phy = 1,
949
+	.num_samsung_phy = 0,
713
950
 };
714
951
 
715
952
 static struct dphy_drv_data rk3562_dphy_drv_data = {
716
953
 	.dev_name = "csi2dphy",
717
-	.vendor = PHY_VENDOR_INNO,
954
+	.chip_id = CHIP_ID_RK3562,
955
+	.num_inno_phy = 2,
956
+	.num_samsung_phy = 0,
718
957
 };
719
958
 
720
959
 static const struct of_device_id rockchip_csi2_dphy_match_id[] = {
..
..
@@ -723,8 +962,8 @@
723
962
 		.data = &rk3568_dphy_drv_data,
724
963
 	},
725
964
 	{
726
-		.compatible = "rockchip,rk3588-csi2-dcphy",
727
-		.data = &rk3588_dcphy_drv_data,
965
+		.compatible = "rockchip,rk3588-csi2-dphy",
966
+		.data = &rk3588_dphy_drv_data,
728
967
 	},
729
968
 	{
730
969
 		.compatible = "rockchip,rv1106-csi2-dphy",
..
..
@@ -737,6 +976,79 @@
737
976
 	{}
738
977
 };
739
978
 MODULE_DEVICE_TABLE(of, rockchip_csi2_dphy_match_id);
979
+
980
+static int rockchip_csi2_dphy_get_samsung_phy_hw(struct csi2_dphy *dphy)
981
+{
982
+	struct phy *dcphy;
983
+	struct device *dev = dphy->dev;
984
+	struct samsung_mipi_dcphy *dcphy_hw;
985
+	char phy_name[32];
986
+	int i = 0;
987
+	int ret = 0;
988
+
989
+	for (i = 0; i < dphy->drv_data->num_samsung_phy; i++) {
990
+		sprintf(phy_name, "dcphy%d", i);
991
+		dcphy = devm_phy_optional_get(dev, phy_name);
992
+		if (IS_ERR(dcphy)) {
993
+			ret = PTR_ERR(dcphy);
994
+			dev_err(dphy->dev, "failed to get mipi dcphy: %d\n", ret);
995
+			return ret;
996
+		}
997
+		dcphy_hw = phy_get_drvdata(dcphy);
998
+		dphy->samsung_phy_group[i] = dcphy_hw;
999
+	}
1000
+	return 0;
1001
+}
1002
+
1003
+static int rockchip_csi2_dphy_get_inno_phy_hw(struct csi2_dphy *dphy)
1004
+{
1005
+	struct platform_device *plat_dev;
1006
+	struct device *dev = dphy->dev;
1007
+	struct csi2_dphy_hw *dphy_hw;
1008
+	struct device_node *np;
1009
+	int i = 0;
1010
+
1011
+	for (i = 0; i < dphy->drv_data->num_inno_phy; i++) {
1012
+		np = of_parse_phandle(dev->of_node, "rockchip,hw", i);
1013
+		if (!np || !of_device_is_available(np)) {
1014
+			dev_err(dphy->dev,
1015
+				"failed to get dphy%d hw node\n", dphy->phy_index);
1016
+			return -ENODEV;
1017
+		}
1018
+		plat_dev = of_find_device_by_node(np);
1019
+		of_node_put(np);
1020
+		if (!plat_dev) {
1021
+			dev_err(dphy->dev,
1022
+				"failed to get dphy%d hw from node\n",
1023
+				dphy->phy_index);
1024
+			return -ENODEV;
1025
+		}
1026
+		dphy_hw = platform_get_drvdata(plat_dev);
1027
+		if (!dphy_hw) {
1028
+			dev_err(dphy->dev,
1029
+				"failed attach dphy%d hw\n",
1030
+				dphy->phy_index);
1031
+			return -EINVAL;
1032
+		}
1033
+		dphy->dphy_hw_group[i] = dphy_hw;
1034
+	}
1035
+	return 0;
1036
+}
1037
+
1038
+static int rockchip_csi2_dphy_get_hw(struct csi2_dphy *dphy)
1039
+{
1040
+	int ret = 0;
1041
+
1042
+	if (dphy->drv_data->chip_id == CHIP_ID_RK3588) {
1043
+		ret = rockchip_csi2_dphy_get_samsung_phy_hw(dphy);
1044
+		if (ret)
1045
+			return ret;
1046
+		ret = rockchip_csi2_dphy_get_inno_phy_hw(dphy);
1047
+	} else {
1048
+		ret = rockchip_csi2_dphy_get_inno_phy_hw(dphy);
1049
+	}
1050
+	return ret;
1051
+}
740
1052
 
741
1053
 static int rockchip_csi2_dphy_probe(struct platform_device *pdev)
742
1054
 {
..
..
@@ -757,22 +1069,22 @@
757
1069
 		return -EINVAL;
758
1070
 	drv_data = of_id->data;
759
1071
 	csi2dphy->drv_data = drv_data;
1072
+
760
1073
 	csi2dphy->phy_index = of_alias_get_id(dev->of_node, drv_data->dev_name);
761
1074
 	if (csi2dphy->phy_index < 0 || csi2dphy->phy_index >= PHY_MAX)
762
1075
 		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
1076
 
1077
+	ret = rockchip_csi2_dphy_get_hw(csi2dphy);
1078
+	if (ret)
1079
+		return -EINVAL;
1080
+	if (csi2dphy->drv_data->chip_id == CHIP_ID_RK3568 ||
1081
+	    csi2dphy->drv_data->chip_id == CHIP_ID_RV1106) {
1082
+		csi2dphy->csi_info.csi_num = 1;
1083
+		csi2dphy->csi_info.dphy_vendor[0] = PHY_VENDOR_INNO;
1084
+		rockchip_csi2_dphy_attach_hw(csi2dphy, 0, 0);
1085
+	} else {
1086
+		csi2dphy->csi_info.csi_num = 0;
1087
+	}
776
1088
 	sd = &csi2dphy->sd;
777
1089
 	mutex_init(&csi2dphy->mutex);
778
1090
 	v4l2_subdev_init(sd, &csi2_dphy_subdev_ops);
..
..
@@ -795,12 +1107,7 @@
795
1107
 
796
1108
 detach_hw:
797
1109
 	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;
1110
+	return -EINVAL;
804
1111
 }
805
1112
 
806
1113
 static int rockchip_csi2_dphy_remove(struct platform_device *pdev)
..
..
@@ -808,7 +1115,10 @@
808
1115
 	struct media_entity *me = platform_get_drvdata(pdev);
809
1116
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me);
810
1117
 	struct csi2_dphy *dphy = to_csi2_dphy(sd);
1118
+	int i = 0;
811
1119
 
1120
+	for (i = 0; i < dphy->csi_info.csi_num; i++)
1121
+		rockchip_csi2_dphy_detach_hw(dphy, dphy->csi_info.csi_idx[i], i);
812
1122
 	media_entity_cleanup(&sd->entity);
813
1123
 
814
1124
 	pm_runtime_disable(&pdev->dev);