1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Rockchip MIPI CSI2 DPHY driver
4 *
5 * Copyright (C) 2021 Rockchip Electronics Co., Ltd.
6 */
7
8 #include <linux/clk.h>
9 #include <linux/delay.h>
10 #include <linux/io.h>
11 #include <linux/module.h>
12 #include <linux/of.h>
13 #include <linux/of_graph.h>
14 #include <linux/of_platform.h>
15 #include <linux/platform_device.h>
16 #include <linux/pm_runtime.h>
17 #include <linux/regmap.h>
18 #include <linux/mfd/syscon.h>
19 #include <media/media-entity.h>
20 #include <media/v4l2-ctrls.h>
21 #include <media/v4l2-fwnode.h>
22 #include <media/v4l2-subdev.h>
23 #include <media/v4l2-device.h>
24 #include <linux/phy/phy.h>
25 #include "phy-rockchip-csi2-dphy-common.h"
26 #include "phy-rockchip-samsung-dcphy.h"
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
39 struct sensor_async_subdev {
40 struct v4l2_async_subdev asd;
41 struct v4l2_mbus_config mbus;
42 int lanes;
43 };
44
45 static LIST_HEAD(csi2dphy_device_list);
46
to_csi2_dphy(struct v4l2_subdev * subdev)47 static inline struct csi2_dphy *to_csi2_dphy(struct v4l2_subdev *subdev)
48 {
49 return container_of(subdev, struct csi2_dphy, sd);
50 }
51
get_remote_sensor(struct v4l2_subdev * sd)52 static struct v4l2_subdev *get_remote_sensor(struct v4l2_subdev *sd)
53 {
54 struct media_pad *local, *remote;
55 struct media_entity *sensor_me;
56 struct csi2_dphy *dphy = to_csi2_dphy(sd);
57
58 if (dphy->num_sensors == 0)
59 return NULL;
60 local = &sd->entity.pads[CSI2_DPHY_RX_PAD_SINK];
61 remote = media_entity_remote_pad(local);
62 if (!remote) {
63 v4l2_warn(sd, "No link between dphy and sensor\n");
64 return NULL;
65 }
66
67 sensor_me = media_entity_remote_pad(local)->entity;
68 return media_entity_to_v4l2_subdev(sensor_me);
69 }
70
sd_to_sensor(struct csi2_dphy * dphy,struct v4l2_subdev * sd)71 static struct csi2_sensor *sd_to_sensor(struct csi2_dphy *dphy,
72 struct v4l2_subdev *sd)
73 {
74 int i;
75
76 for (i = 0; i < dphy->num_sensors; ++i)
77 if (dphy->sensors[i].sd == sd)
78 return &dphy->sensors[i];
79
80 return NULL;
81 }
82
csi2_dphy_get_sensor_data_rate(struct v4l2_subdev * sd)83 static int csi2_dphy_get_sensor_data_rate(struct v4l2_subdev *sd)
84 {
85 struct csi2_dphy *dphy = to_csi2_dphy(sd);
86 struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
87 struct v4l2_ctrl *link_freq;
88 struct v4l2_querymenu qm = { .id = V4L2_CID_LINK_FREQ, };
89 int ret = 0;
90
91 if (!sensor_sd)
92 return -ENODEV;
93
94 link_freq = v4l2_ctrl_find(sensor_sd->ctrl_handler, V4L2_CID_LINK_FREQ);
95 if (!link_freq) {
96 v4l2_warn(sd, "No pixel rate control in subdev\n");
97 return -EPIPE;
98 }
99
100 qm.index = v4l2_ctrl_g_ctrl(link_freq);
101 ret = v4l2_querymenu(sensor_sd->ctrl_handler, &qm);
102 if (ret < 0) {
103 v4l2_err(sd, "Failed to get menu item\n");
104 return ret;
105 }
106
107 if (!qm.value) {
108 v4l2_err(sd, "Invalid link_freq\n");
109 return -EINVAL;
110 }
111 dphy->data_rate_mbps = qm.value * 2;
112 do_div(dphy->data_rate_mbps, 1000 * 1000);
113 v4l2_info(sd, "dphy%d, data_rate_mbps %lld\n",
114 dphy->phy_index, dphy->data_rate_mbps);
115 return 0;
116 }
117
rockchip_csi2_dphy_attach_hw(struct csi2_dphy * dphy,int csi_idx,int index)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->phy_hw[index] = (void *)dphy_hw;
248 dphy->csi_info.dphy_vendor[index] = PHY_VENDOR_INNO;
249 mutex_unlock(&dphy_hw->mutex);
250 }
251
252 return 0;
253 }
254
rockchip_csi2_dphy_detach_hw(struct csi2_dphy * dphy,int csi_idx,int index)255 static int rockchip_csi2_dphy_detach_hw(struct csi2_dphy *dphy, int csi_idx, int index)
256 {
257 struct csi2_dphy_hw *dphy_hw = NULL;
258 struct samsung_mipi_dcphy *dcphy_hw = NULL;
259 struct csi2_dphy *csi2_dphy = NULL;
260 int i = 0;
261
262 if (dphy->drv_data->chip_id == CHIP_ID_RK3568 ||
263 dphy->drv_data->chip_id == CHIP_ID_RV1106) {
264 dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
265 if (!dphy_hw) {
266 dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
267 __func__, csi_idx);
268 return -EINVAL;
269 }
270 mutex_lock(&dphy_hw->mutex);
271 for (i = 0; i < dphy_hw->dphy_dev_num; i++) {
272 csi2_dphy = dphy_hw->dphy_dev[i];
273 if (csi2_dphy &&
274 csi2_dphy->phy_index == dphy->phy_index) {
275 dphy_hw->dphy_dev[i] = NULL;
276 dphy_hw->dphy_dev_num--;
277 break;
278 }
279 }
280 mutex_unlock(&dphy_hw->mutex);
281 } else if (dphy->drv_data->chip_id == CHIP_ID_RK3588) {
282 if (csi_idx < 2) {
283 dcphy_hw = (struct samsung_mipi_dcphy *)dphy->phy_hw[index];
284 if (!dcphy_hw) {
285 dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
286 __func__, csi_idx);
287 return -EINVAL;
288 }
289 mutex_lock(&dcphy_hw->mutex);
290 dcphy_hw->dphy_dev_num--;
291 mutex_unlock(&dcphy_hw->mutex);
292 } else {
293 dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
294 if (!dphy_hw) {
295 dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
296 __func__, csi_idx);
297 return -EINVAL;
298 }
299 mutex_lock(&dphy_hw->mutex);
300 dphy_hw->dphy_dev_num--;
301 mutex_unlock(&dphy_hw->mutex);
302 }
303 } else {
304 dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[index];
305 if (!dphy_hw) {
306 dev_err(dphy->dev, "%s csi_idx %d detach hw failed\n",
307 __func__, csi_idx);
308 return -EINVAL;
309 }
310 mutex_lock(&dphy_hw->mutex);
311 dphy_hw->dphy_dev_num--;
312 mutex_unlock(&dphy_hw->mutex);
313 }
314
315 return 0;
316 }
317
csi2_dphy_update_sensor_mbus(struct v4l2_subdev * sd)318 static int csi2_dphy_update_sensor_mbus(struct v4l2_subdev *sd)
319 {
320 struct csi2_dphy *dphy = to_csi2_dphy(sd);
321 struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
322 struct csi2_sensor *sensor;
323 struct v4l2_mbus_config mbus;
324 int ret = 0;
325
326 if (!sensor_sd)
327 return -ENODEV;
328 sensor = sd_to_sensor(dphy, sensor_sd);
329 if (!sensor)
330 return -ENODEV;
331
332 ret = v4l2_subdev_call(sensor_sd, pad, get_mbus_config, 0, &mbus);
333 if (ret)
334 return ret;
335
336 sensor->mbus = mbus;
337 switch (mbus.flags & V4L2_MBUS_CSI2_LANES) {
338 case V4L2_MBUS_CSI2_1_LANE:
339 sensor->lanes = 1;
340 break;
341 case V4L2_MBUS_CSI2_2_LANE:
342 sensor->lanes = 2;
343 break;
344 case V4L2_MBUS_CSI2_3_LANE:
345 sensor->lanes = 3;
346 break;
347 case V4L2_MBUS_CSI2_4_LANE:
348 sensor->lanes = 4;
349 break;
350 default:
351 return -EINVAL;
352 }
353
354 return 0;
355 }
356
csi2_dphy_update_config(struct v4l2_subdev * sd)357 static int csi2_dphy_update_config(struct v4l2_subdev *sd)
358 {
359 struct csi2_dphy *dphy = to_csi2_dphy(sd);
360 struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
361 struct rkmodule_csi_dphy_param dphy_param;
362 struct rkmodule_bus_config bus_config;
363 int csi_idx = 0;
364 int ret = 0;
365 int i = 0;
366
367 for (i = 0; i < dphy->csi_info.csi_num; i++) {
368 if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
369 dphy->drv_data->chip_id != CHIP_ID_RV1106) {
370 csi_idx = dphy->csi_info.csi_idx[i];
371 rockchip_csi2_dphy_attach_hw(dphy, csi_idx, i);
372 }
373 if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_INNO) {
374 ret = v4l2_subdev_call(sensor_sd, core, ioctl,
375 RKMODULE_GET_BUS_CONFIG, &bus_config);
376 if (!ret) {
377 dev_info(dphy->dev, "phy_mode %d,lane %d\n",
378 bus_config.bus.phy_mode, bus_config.bus.lanes);
379 if (bus_config.bus.phy_mode == PHY_FULL_MODE) {
380 if (dphy->phy_index % 3 == 2) {
381 dev_err(dphy->dev, "%s dphy%d only use for PHY_SPLIT_23\n",
382 __func__, dphy->phy_index);
383 return -EINVAL;
384 }
385 dphy->lane_mode = PHY_FULL_MODE;
386 dphy->dphy_hw->lane_mode = LANE_MODE_FULL;
387 } else if (bus_config.bus.phy_mode == PHY_SPLIT_01) {
388 if (dphy->phy_index % 3 == 2) {
389 dev_err(dphy->dev, "%s dphy%d only use for PHY_SPLIT_23\n",
390 __func__, dphy->phy_index);
391 return -EINVAL;
392 }
393 dphy->lane_mode = PHY_SPLIT_01;
394 dphy->dphy_hw->lane_mode = LANE_MODE_SPLIT;
395 } else if (bus_config.bus.phy_mode == PHY_SPLIT_23) {
396 if (dphy->phy_index % 3 != 2) {
397 dev_err(dphy->dev, "%s dphy%d not support PHY_SPLIT_23\n",
398 __func__, dphy->phy_index);
399 return -EINVAL;
400 }
401 dphy->lane_mode = PHY_SPLIT_23;
402 dphy->dphy_hw->lane_mode = LANE_MODE_SPLIT;
403 }
404 }
405 }
406 }
407 ret = v4l2_subdev_call(sensor_sd, core, ioctl,
408 RKMODULE_GET_CSI_DPHY_PARAM,
409 &dphy_param);
410 if (!ret)
411 dphy->dphy_param = dphy_param;
412 return 0;
413 }
414
csi2_dphy_s_stream_start(struct v4l2_subdev * sd)415 static int csi2_dphy_s_stream_start(struct v4l2_subdev *sd)
416 {
417 struct csi2_dphy *dphy = to_csi2_dphy(sd);
418 int i = 0;
419
420 for (i = 0; i < dphy->csi_info.csi_num; i++) {
421 if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
422 dphy->samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
423 if (dphy->samsung_phy && dphy->samsung_phy->stream_on)
424 dphy->samsung_phy->stream_on(dphy, sd);
425 } else {
426 dphy->dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
427 if (dphy->dphy_hw && dphy->dphy_hw->stream_on)
428 dphy->dphy_hw->stream_on(dphy, sd);
429 }
430 }
431
432 dphy->is_streaming = true;
433
434 return 0;
435 }
436
csi2_dphy_s_stream_stop(struct v4l2_subdev * sd)437 static int csi2_dphy_s_stream_stop(struct v4l2_subdev *sd)
438 {
439 struct csi2_dphy *dphy = to_csi2_dphy(sd);
440 int i = 0;
441
442 for (i = 0; i < dphy->csi_info.csi_num; i++) {
443 if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
444 dphy->samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
445 if (dphy->samsung_phy && dphy->samsung_phy->stream_off)
446 dphy->samsung_phy->stream_off(dphy, sd);
447 } else {
448 dphy->dphy_hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
449 if (dphy->dphy_hw && dphy->dphy_hw->stream_off)
450 dphy->dphy_hw->stream_off(dphy, sd);
451 }
452 if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
453 dphy->drv_data->chip_id != CHIP_ID_RV1106)
454 rockchip_csi2_dphy_detach_hw(dphy, dphy->csi_info.csi_idx[i], i);
455 }
456
457 dphy->is_streaming = false;
458
459 dev_info(dphy->dev, "%s stream stop, dphy%d\n",
460 __func__, dphy->phy_index);
461
462 return 0;
463 }
464
csi2_dphy_enable_clk(struct csi2_dphy * dphy)465 static int csi2_dphy_enable_clk(struct csi2_dphy *dphy)
466 {
467 struct csi2_dphy_hw *hw = NULL;
468 struct samsung_mipi_dcphy *samsung_phy = NULL;
469 int ret;
470 int i = 0;
471
472 for (i = 0; i < dphy->csi_info.csi_num; i++) {
473 if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
474 samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
475 if (samsung_phy)
476 clk_prepare_enable(samsung_phy->pclk);
477 } else {
478 hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
479 if (hw) {
480 ret = clk_bulk_prepare_enable(hw->num_clks, hw->clks_bulk);
481 if (ret) {
482 dev_err(hw->dev, "failed to enable clks\n");
483 return ret;
484 }
485 }
486 }
487 }
488 return 0;
489 }
490
csi2_dphy_disable_clk(struct csi2_dphy * dphy)491 static void csi2_dphy_disable_clk(struct csi2_dphy *dphy)
492 {
493 struct csi2_dphy_hw *hw = NULL;
494 struct samsung_mipi_dcphy *samsung_phy = NULL;
495 int i = 0;
496
497 for (i = 0; i < dphy->csi_info.csi_num; i++) {
498 if (dphy->csi_info.dphy_vendor[i] == PHY_VENDOR_SAMSUNG) {
499 samsung_phy = (struct samsung_mipi_dcphy *)dphy->phy_hw[i];
500 if (samsung_phy)
501 clk_disable_unprepare(samsung_phy->pclk);
502 } else {
503 hw = (struct csi2_dphy_hw *)dphy->phy_hw[i];
504 if (hw)
505 clk_bulk_disable_unprepare(hw->num_clks, hw->clks_bulk);
506 }
507 }
508 }
509
csi2_dphy_s_stream(struct v4l2_subdev * sd,int on)510 static int csi2_dphy_s_stream(struct v4l2_subdev *sd, int on)
511 {
512 struct csi2_dphy *dphy = to_csi2_dphy(sd);
513 int ret = 0;
514
515 mutex_lock(&dphy->mutex);
516 if (on) {
517 if (dphy->is_streaming) {
518 mutex_unlock(&dphy->mutex);
519 return 0;
520 }
521
522 ret = csi2_dphy_get_sensor_data_rate(sd);
523 if (ret < 0) {
524 mutex_unlock(&dphy->mutex);
525 return ret;
526 }
527
528 csi2_dphy_update_sensor_mbus(sd);
529 ret = csi2_dphy_update_config(sd);
530 if (ret < 0) {
531 mutex_unlock(&dphy->mutex);
532 return ret;
533 }
534
535 ret = csi2_dphy_enable_clk(dphy);
536 if (ret) {
537 mutex_unlock(&dphy->mutex);
538 return ret;
539 }
540 ret = csi2_dphy_s_stream_start(sd);
541 } else {
542 if (!dphy->is_streaming) {
543 mutex_unlock(&dphy->mutex);
544 return 0;
545 }
546 ret = csi2_dphy_s_stream_stop(sd);
547 csi2_dphy_disable_clk(dphy);
548 }
549 mutex_unlock(&dphy->mutex);
550
551 dev_info(dphy->dev, "%s stream on:%d, dphy%d, ret %d\n",
552 __func__, on, dphy->phy_index, ret);
553
554 return ret;
555 }
556
csi2_dphy_g_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_frame_interval * fi)557 static int csi2_dphy_g_frame_interval(struct v4l2_subdev *sd,
558 struct v4l2_subdev_frame_interval *fi)
559 {
560 struct v4l2_subdev *sensor = get_remote_sensor(sd);
561
562 if (sensor)
563 return v4l2_subdev_call(sensor, video, g_frame_interval, fi);
564
565 return -EINVAL;
566 }
567
csi2_dphy_g_mbus_config(struct v4l2_subdev * sd,unsigned int pad_id,struct v4l2_mbus_config * config)568 static int csi2_dphy_g_mbus_config(struct v4l2_subdev *sd,
569 unsigned int pad_id,
570 struct v4l2_mbus_config *config)
571 {
572 struct csi2_dphy *dphy = to_csi2_dphy(sd);
573 struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
574 struct csi2_sensor *sensor;
575
576 if (!sensor_sd)
577 return -ENODEV;
578 sensor = sd_to_sensor(dphy, sensor_sd);
579 if (!sensor)
580 return -ENODEV;
581 csi2_dphy_update_sensor_mbus(sd);
582 *config = sensor->mbus;
583
584 return 0;
585 }
586
csi2_dphy_s_power(struct v4l2_subdev * sd,int on)587 static int csi2_dphy_s_power(struct v4l2_subdev *sd, int on)
588 {
589 struct csi2_dphy *dphy = to_csi2_dphy(sd);
590
591 if (on)
592 return pm_runtime_get_sync(dphy->dev);
593 else
594 return pm_runtime_put(dphy->dev);
595 }
596
csi2_dphy_runtime_suspend(struct device * dev)597 static __maybe_unused int csi2_dphy_runtime_suspend(struct device *dev)
598 {
599 struct media_entity *me = dev_get_drvdata(dev);
600 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me);
601 struct csi2_dphy *dphy = to_csi2_dphy(sd);
602
603 if (dphy->is_streaming) {
604 csi2_dphy_s_stream(sd, 0);
605 dphy->is_streaming = false;
606 }
607
608 return 0;
609 }
610
csi2_dphy_runtime_resume(struct device * dev)611 static __maybe_unused int csi2_dphy_runtime_resume(struct device *dev)
612 {
613 return 0;
614 }
615
616 /* dphy accepts all fmt/size from sensor */
csi2_dphy_get_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)617 static int csi2_dphy_get_set_fmt(struct v4l2_subdev *sd,
618 struct v4l2_subdev_pad_config *cfg,
619 struct v4l2_subdev_format *fmt)
620 {
621 struct csi2_dphy *dphy = to_csi2_dphy(sd);
622 struct v4l2_subdev *sensor_sd = get_remote_sensor(sd);
623 struct csi2_sensor *sensor;
624 int ret;
625 /*
626 * Do not allow format changes and just relay whatever
627 * set currently in the sensor.
628 */
629 if (!sensor_sd)
630 return -ENODEV;
631 sensor = sd_to_sensor(dphy, sensor_sd);
632 if (!sensor)
633 return -ENODEV;
634 ret = v4l2_subdev_call(sensor_sd, pad, get_fmt, NULL, fmt);
635 if (!ret && fmt->pad == 0 && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
636 sensor->format = fmt->format;
637 return ret;
638 }
639
csi2_dphy_get_selection(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_selection * sel)640 static int csi2_dphy_get_selection(struct v4l2_subdev *sd,
641 struct v4l2_subdev_pad_config *cfg,
642 struct v4l2_subdev_selection *sel)
643 {
644 struct v4l2_subdev *sensor = get_remote_sensor(sd);
645
646 return v4l2_subdev_call(sensor, pad, get_selection, NULL, sel);
647 }
648
rkcif_csi2_dphy_ioctl(struct v4l2_subdev * sd,unsigned int cmd,void * arg)649 static long rkcif_csi2_dphy_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
650 {
651 struct csi2_dphy *dphy = to_csi2_dphy(sd);
652 long ret = 0;
653
654 switch (cmd) {
655 case RKCIF_CMD_SET_CSI_IDX:
656 if (dphy->drv_data->chip_id != CHIP_ID_RK3568 &&
657 dphy->drv_data->chip_id != CHIP_ID_RV1106)
658 dphy->csi_info = *((struct rkcif_csi_info *)arg);
659 break;
660 default:
661 ret = -ENOIOCTLCMD;
662 break;
663 }
664
665 return ret;
666 }
667
668 #ifdef CONFIG_COMPAT
rkcif_csi2_dphy_compat_ioctl32(struct v4l2_subdev * sd,unsigned int cmd,unsigned long arg)669 static long rkcif_csi2_dphy_compat_ioctl32(struct v4l2_subdev *sd,
670 unsigned int cmd, unsigned long arg)
671 {
672 void __user *up = compat_ptr(arg);
673 struct rkcif_csi_info csi_info = {0};
674 long ret;
675
676 switch (cmd) {
677 case RKCIF_CMD_SET_CSI_IDX:
678 if (copy_from_user(&csi_info, up, sizeof(struct rkcif_csi_info)))
679 return -EFAULT;
680
681 ret = rkcif_csi2_dphy_ioctl(sd, cmd, &csi_info);
682 break;
683 default:
684 ret = -ENOIOCTLCMD;
685 break;
686 }
687
688 return ret;
689 }
690 #endif
691
692 static const struct v4l2_subdev_core_ops csi2_dphy_core_ops = {
693 .s_power = csi2_dphy_s_power,
694 .ioctl = rkcif_csi2_dphy_ioctl,
695 #ifdef CONFIG_COMPAT
696 .compat_ioctl32 = rkcif_csi2_dphy_compat_ioctl32,
697 #endif
698 };
699
700 static const struct v4l2_subdev_video_ops csi2_dphy_video_ops = {
701 .g_frame_interval = csi2_dphy_g_frame_interval,
702 .s_stream = csi2_dphy_s_stream,
703 };
704
705 static const struct v4l2_subdev_pad_ops csi2_dphy_subdev_pad_ops = {
706 .set_fmt = csi2_dphy_get_set_fmt,
707 .get_fmt = csi2_dphy_get_set_fmt,
708 .get_selection = csi2_dphy_get_selection,
709 .get_mbus_config = csi2_dphy_g_mbus_config,
710 };
711
712 static const struct v4l2_subdev_ops csi2_dphy_subdev_ops = {
713 .core = &csi2_dphy_core_ops,
714 .video = &csi2_dphy_video_ops,
715 .pad = &csi2_dphy_subdev_pad_ops,
716 };
717
718 /* The .bound() notifier callback when a match is found */
719 static int
rockchip_csi2_dphy_notifier_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)720 rockchip_csi2_dphy_notifier_bound(struct v4l2_async_notifier *notifier,
721 struct v4l2_subdev *sd,
722 struct v4l2_async_subdev *asd)
723 {
724 struct csi2_dphy *dphy = container_of(notifier,
725 struct csi2_dphy,
726 notifier);
727 struct sensor_async_subdev *s_asd = container_of(asd,
728 struct sensor_async_subdev, asd);
729 struct csi2_sensor *sensor;
730 unsigned int pad, ret;
731
732 if (dphy->num_sensors == ARRAY_SIZE(dphy->sensors))
733 return -EBUSY;
734
735 sensor = &dphy->sensors[dphy->num_sensors++];
736 sensor->lanes = s_asd->lanes;
737 sensor->mbus = s_asd->mbus;
738 sensor->sd = sd;
739
740 dev_info(dphy->dev, "dphy%d matches %s:bus type %d\n",
741 dphy->phy_index, sd->name, s_asd->mbus.type);
742
743 for (pad = 0; pad < sensor->sd->entity.num_pads; pad++)
744 if (sensor->sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE)
745 break;
746
747 if (pad == sensor->sd->entity.num_pads) {
748 dev_err(dphy->dev,
749 "failed to find src pad for %s\n",
750 sensor->sd->name);
751
752 return -ENXIO;
753 }
754
755 ret = media_create_pad_link(
756 &sensor->sd->entity, pad,
757 &dphy->sd.entity, CSI2_DPHY_RX_PAD_SINK,
758 dphy->num_sensors != 1 ? 0 : MEDIA_LNK_FL_ENABLED);
759 if (ret) {
760 dev_err(dphy->dev,
761 "failed to create link for %s\n",
762 sensor->sd->name);
763 return ret;
764 }
765
766 return 0;
767 }
768
769 /* The .unbind callback */
770 static void
rockchip_csi2_dphy_notifier_unbind(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)771 rockchip_csi2_dphy_notifier_unbind(struct v4l2_async_notifier *notifier,
772 struct v4l2_subdev *sd,
773 struct v4l2_async_subdev *asd)
774 {
775 struct csi2_dphy *dphy = container_of(notifier,
776 struct csi2_dphy,
777 notifier);
778 struct csi2_sensor *sensor = sd_to_sensor(dphy, sd);
779
780 if (sensor)
781 sensor->sd = NULL;
782 }
783
784 static const struct
785 v4l2_async_notifier_operations rockchip_csi2_dphy_async_ops = {
786 .bound = rockchip_csi2_dphy_notifier_bound,
787 .unbind = rockchip_csi2_dphy_notifier_unbind,
788 };
789
rockchip_csi2_dphy_fwnode_parse(struct device * dev,struct v4l2_fwnode_endpoint * vep,struct v4l2_async_subdev * asd)790 static int rockchip_csi2_dphy_fwnode_parse(struct device *dev,
791 struct v4l2_fwnode_endpoint *vep,
792 struct v4l2_async_subdev *asd)
793 {
794 struct sensor_async_subdev *s_asd =
795 container_of(asd, struct sensor_async_subdev, asd);
796 struct v4l2_mbus_config *config = &s_asd->mbus;
797
798 if (vep->base.port != 0) {
799 dev_err(dev, "The PHY has only port 0\n");
800 return -EINVAL;
801 }
802
803 if (vep->bus_type == V4L2_MBUS_CSI2_DPHY ||
804 vep->bus_type == V4L2_MBUS_CSI2_CPHY) {
805 config->type = vep->bus_type;
806 config->flags = vep->bus.mipi_csi2.flags;
807 s_asd->lanes = vep->bus.mipi_csi2.num_data_lanes;
808 } else if (vep->bus_type == V4L2_MBUS_CCP2) {
809 config->type = V4L2_MBUS_CCP2;
810 s_asd->lanes = vep->bus.mipi_csi1.data_lane;
811 } else {
812 dev_err(dev, "Only CSI2 type is currently supported\n");
813 return -EINVAL;
814 }
815
816 switch (s_asd->lanes) {
817 case 1:
818 config->flags |= V4L2_MBUS_CSI2_1_LANE;
819 break;
820 case 2:
821 config->flags |= V4L2_MBUS_CSI2_2_LANE;
822 break;
823 case 3:
824 config->flags |= V4L2_MBUS_CSI2_3_LANE;
825 break;
826 case 4:
827 config->flags |= V4L2_MBUS_CSI2_4_LANE;
828 break;
829 default:
830 return -EINVAL;
831 }
832
833 return 0;
834 }
835
rockchip_csi2dphy_media_init(struct csi2_dphy * dphy)836 static int rockchip_csi2dphy_media_init(struct csi2_dphy *dphy)
837 {
838 int ret;
839
840 dphy->pads[CSI2_DPHY_RX_PAD_SOURCE].flags =
841 MEDIA_PAD_FL_SOURCE | MEDIA_PAD_FL_MUST_CONNECT;
842 dphy->pads[CSI2_DPHY_RX_PAD_SINK].flags =
843 MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
844 dphy->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
845 ret = media_entity_pads_init(&dphy->sd.entity,
846 CSI2_DPHY_RX_PADS_NUM, dphy->pads);
847 if (ret < 0)
848 return ret;
849
850 v4l2_async_notifier_init(&dphy->notifier);
851
852 ret = v4l2_async_notifier_parse_fwnode_endpoints_by_port(
853 dphy->dev, &dphy->notifier,
854 sizeof(struct sensor_async_subdev), 0,
855 rockchip_csi2_dphy_fwnode_parse);
856 if (ret < 0)
857 return ret;
858
859 dphy->sd.subdev_notifier = &dphy->notifier;
860 dphy->notifier.ops = &rockchip_csi2_dphy_async_ops;
861 ret = v4l2_async_subdev_notifier_register(&dphy->sd, &dphy->notifier);
862 if (ret) {
863 dev_err(dphy->dev,
864 "failed to register async notifier : %d\n", ret);
865 v4l2_async_notifier_cleanup(&dphy->notifier);
866 return ret;
867 }
868
869 return v4l2_async_register_subdev(&dphy->sd);
870 }
871
872 static struct dphy_drv_data rk3568_dphy_drv_data = {
873 .dev_name = "csi2dphy",
874 .chip_id = CHIP_ID_RK3568,
875 .num_inno_phy = 1,
876 .num_samsung_phy = 0,
877 };
878
879 static struct dphy_drv_data rk3588_dphy_drv_data = {
880 .dev_name = "csi2dphy",
881 .chip_id = CHIP_ID_RK3588,
882 .num_inno_phy = 2,
883 .num_samsung_phy = 2,
884 };
885
886 static struct dphy_drv_data rv1106_dphy_drv_data = {
887 .dev_name = "csi2dphy",
888 .chip_id = CHIP_ID_RV1106,
889 .num_inno_phy = 1,
890 .num_samsung_phy = 0,
891 };
892
893 static struct dphy_drv_data rk3562_dphy_drv_data = {
894 .dev_name = "csi2dphy",
895 .chip_id = CHIP_ID_RK3562,
896 .num_inno_phy = 2,
897 .num_samsung_phy = 0,
898 };
899
900 static const struct of_device_id rockchip_csi2_dphy_match_id[] = {
901 {
902 .compatible = "rockchip,rk3568-csi2-dphy",
903 .data = &rk3568_dphy_drv_data,
904 },
905 {
906 .compatible = "rockchip,rk3588-csi2-dphy",
907 .data = &rk3588_dphy_drv_data,
908 },
909 {
910 .compatible = "rockchip,rv1106-csi2-dphy",
911 .data = &rv1106_dphy_drv_data,
912 },
913 {
914 .compatible = "rockchip,rk3562-csi2-dphy",
915 .data = &rk3562_dphy_drv_data,
916 },
917 {}
918 };
919 MODULE_DEVICE_TABLE(of, rockchip_csi2_dphy_match_id);
920
rockchip_csi2_dphy_get_samsung_phy_hw(struct csi2_dphy * dphy)921 static int rockchip_csi2_dphy_get_samsung_phy_hw(struct csi2_dphy *dphy)
922 {
923 struct phy *dcphy;
924 struct device *dev = dphy->dev;
925 struct samsung_mipi_dcphy *dcphy_hw;
926 char phy_name[32];
927 int i = 0;
928 int ret = 0;
929
930 for (i = 0; i < dphy->drv_data->num_samsung_phy; i++) {
931 sprintf(phy_name, "dcphy%d", i);
932 dcphy = devm_phy_optional_get(dev, phy_name);
933 if (IS_ERR(dcphy)) {
934 ret = PTR_ERR(dcphy);
935 dev_err(dphy->dev, "failed to get mipi dcphy: %d\n", ret);
936 return ret;
937 }
938 dcphy_hw = phy_get_drvdata(dcphy);
939 dphy->samsung_phy_group[i] = dcphy_hw;
940 }
941 return 0;
942 }
943
rockchip_csi2_dphy_get_inno_phy_hw(struct csi2_dphy * dphy)944 static int rockchip_csi2_dphy_get_inno_phy_hw(struct csi2_dphy *dphy)
945 {
946 struct platform_device *plat_dev;
947 struct device *dev = dphy->dev;
948 struct csi2_dphy_hw *dphy_hw;
949 struct device_node *np;
950 int i = 0;
951
952 for (i = 0; i < dphy->drv_data->num_inno_phy; i++) {
953 np = of_parse_phandle(dev->of_node, "rockchip,hw", i);
954 if (!np || !of_device_is_available(np)) {
955 dev_err(dphy->dev,
956 "failed to get dphy%d hw node\n", dphy->phy_index);
957 return -ENODEV;
958 }
959 plat_dev = of_find_device_by_node(np);
960 of_node_put(np);
961 if (!plat_dev) {
962 dev_err(dphy->dev,
963 "failed to get dphy%d hw from node\n",
964 dphy->phy_index);
965 return -ENODEV;
966 }
967 dphy_hw = platform_get_drvdata(plat_dev);
968 if (!dphy_hw) {
969 dev_err(dphy->dev,
970 "failed attach dphy%d hw\n",
971 dphy->phy_index);
972 return -EINVAL;
973 }
974 dphy->dphy_hw_group[i] = dphy_hw;
975 }
976 return 0;
977 }
978
rockchip_csi2_dphy_get_hw(struct csi2_dphy * dphy)979 static int rockchip_csi2_dphy_get_hw(struct csi2_dphy *dphy)
980 {
981 int ret = 0;
982
983 if (dphy->drv_data->chip_id == CHIP_ID_RK3588) {
984 ret = rockchip_csi2_dphy_get_samsung_phy_hw(dphy);
985 if (ret)
986 return ret;
987 ret = rockchip_csi2_dphy_get_inno_phy_hw(dphy);
988 } else {
989 ret = rockchip_csi2_dphy_get_inno_phy_hw(dphy);
990 }
991 return ret;
992 }
993
rockchip_csi2_dphy_probe(struct platform_device * pdev)994 static int rockchip_csi2_dphy_probe(struct platform_device *pdev)
995 {
996 struct device *dev = &pdev->dev;
997 const struct of_device_id *of_id;
998 struct csi2_dphy *csi2dphy;
999 struct v4l2_subdev *sd;
1000 const struct dphy_drv_data *drv_data;
1001 int ret;
1002
1003 csi2dphy = devm_kzalloc(dev, sizeof(*csi2dphy), GFP_KERNEL);
1004 if (!csi2dphy)
1005 return -ENOMEM;
1006 csi2dphy->dev = dev;
1007
1008 of_id = of_match_device(rockchip_csi2_dphy_match_id, dev);
1009 if (!of_id)
1010 return -EINVAL;
1011 drv_data = of_id->data;
1012 csi2dphy->drv_data = drv_data;
1013
1014 csi2dphy->phy_index = of_alias_get_id(dev->of_node, drv_data->dev_name);
1015 if (csi2dphy->phy_index < 0 || csi2dphy->phy_index >= PHY_MAX)
1016 csi2dphy->phy_index = 0;
1017
1018 ret = rockchip_csi2_dphy_get_hw(csi2dphy);
1019 if (ret)
1020 return -EINVAL;
1021 if (csi2dphy->drv_data->chip_id == CHIP_ID_RK3568 ||
1022 csi2dphy->drv_data->chip_id == CHIP_ID_RV1106) {
1023 csi2dphy->csi_info.csi_num = 1;
1024 csi2dphy->csi_info.dphy_vendor[0] = PHY_VENDOR_INNO;
1025 rockchip_csi2_dphy_attach_hw(csi2dphy, 0, 0);
1026 } else {
1027 csi2dphy->csi_info.csi_num = 0;
1028 }
1029 sd = &csi2dphy->sd;
1030 mutex_init(&csi2dphy->mutex);
1031 v4l2_subdev_init(sd, &csi2_dphy_subdev_ops);
1032 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1033 snprintf(sd->name, sizeof(sd->name),
1034 "rockchip-csi2-dphy%d", csi2dphy->phy_index);
1035 sd->dev = dev;
1036
1037 platform_set_drvdata(pdev, &sd->entity);
1038
1039 ret = rockchip_csi2dphy_media_init(csi2dphy);
1040 if (ret < 0)
1041 goto detach_hw;
1042
1043 pm_runtime_enable(&pdev->dev);
1044
1045 dev_info(dev, "csi2 dphy%d probe successfully!\n", csi2dphy->phy_index);
1046
1047 return 0;
1048
1049 detach_hw:
1050 mutex_destroy(&csi2dphy->mutex);
1051 return -EINVAL;
1052 }
1053
rockchip_csi2_dphy_remove(struct platform_device * pdev)1054 static int rockchip_csi2_dphy_remove(struct platform_device *pdev)
1055 {
1056 struct media_entity *me = platform_get_drvdata(pdev);
1057 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(me);
1058 struct csi2_dphy *dphy = to_csi2_dphy(sd);
1059 int i = 0;
1060
1061 for (i = 0; i < dphy->csi_info.csi_num; i++)
1062 rockchip_csi2_dphy_detach_hw(dphy, dphy->csi_info.csi_idx[i], i);
1063 media_entity_cleanup(&sd->entity);
1064
1065 pm_runtime_disable(&pdev->dev);
1066 mutex_destroy(&dphy->mutex);
1067 return 0;
1068 }
1069
1070 static const struct dev_pm_ops rockchip_csi2_dphy_pm_ops = {
1071 SET_RUNTIME_PM_OPS(csi2_dphy_runtime_suspend,
1072 csi2_dphy_runtime_resume, NULL)
1073 };
1074
1075 struct platform_driver rockchip_csi2_dphy_driver = {
1076 .probe = rockchip_csi2_dphy_probe,
1077 .remove = rockchip_csi2_dphy_remove,
1078 .driver = {
1079 .name = "rockchip-csi2-dphy",
1080 .pm = &rockchip_csi2_dphy_pm_ops,
1081 .of_match_table = rockchip_csi2_dphy_match_id,
1082 },
1083 };
1084
rockchip_csi2_dphy_init(void)1085 int rockchip_csi2_dphy_init(void)
1086 {
1087 return platform_driver_register(&rockchip_csi2_dphy_driver);
1088 }
1089
1090 #if defined(CONFIG_VIDEO_ROCKCHIP_THUNDER_BOOT_ISP) && !defined(CONFIG_INITCALL_ASYNC)
1091 subsys_initcall(rockchip_csi2_dphy_init);
1092 #else
1093 #if !defined(CONFIG_VIDEO_REVERSE_IMAGE)
1094 module_platform_driver(rockchip_csi2_dphy_driver);
1095 #endif
1096 #endif
1097
1098 MODULE_AUTHOR("Rockchip Camera/ISP team");
1099 MODULE_DESCRIPTION("Rockchip MIPI CSI2 DPHY driver");
1100 MODULE_LICENSE("GPL v2");
1101