1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * Driver for Cadence MIPI-CSI2 RX Controller v1.3
4 *
5 * Copyright (C) 2017 Cadence Design Systems Inc.
6 */
7
8 #include <linux/clk.h>
9 #include <linux/delay.h>
10 #include <linux/io.h>
11 #include <linux/iopoll.h>
12 #include <linux/module.h>
13 #include <linux/of.h>
14 #include <linux/of_graph.h>
15 #include <linux/phy/phy.h>
16 #include <linux/platform_device.h>
17 #include <linux/reset.h>
18 #include <linux/slab.h>
19
20 #include <media/v4l2-ctrls.h>
21 #include <media/v4l2-device.h>
22 #include <media/v4l2-fwnode.h>
23 #include <media/v4l2-subdev.h>
24
25 #define CSI2RX_DEVICE_CFG_REG 0x000
26
27 #define CSI2RX_SOFT_RESET_REG 0x004
28 #define CSI2RX_SOFT_RESET_PROTOCOL BIT(1)
29 #define CSI2RX_SOFT_RESET_FRONT BIT(0)
30
31 #define CSI2RX_STATIC_CFG_REG 0x008
32 #define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane) ((plane) << (16 + (llane) * 4))
33 #define CSI2RX_STATIC_CFG_LANES_MASK GENMASK(11, 8)
34
35 #define CSI2RX_DPHY_LANE_CTRL_REG 0x40
36 #define CSI2RX_DPHY_CL_RST BIT(16)
37 #define CSI2RX_DPHY_DL_RST(i) BIT((i) + 12)
38 #define CSI2RX_DPHY_CL_EN BIT(4)
39 #define CSI2RX_DPHY_DL_EN(i) BIT(i)
40
41 #define CSI2RX_STREAM_BASE(n) (((n) + 1) * 0x100)
42
43 #define CSI2RX_STREAM_CTRL_REG(n) (CSI2RX_STREAM_BASE(n) + 0x000)
44 #define CSI2RX_STREAM_CTRL_SOFT_RST BIT(4)
45 #define CSI2RX_STREAM_CTRL_STOP BIT(1)
46 #define CSI2RX_STREAM_CTRL_START BIT(0)
47
48 #define CSI2RX_STREAM_STATUS_REG(n) (CSI2RX_STREAM_BASE(n) + 0x004)
49 #define CSI2RX_STREAM_STATUS_RDY BIT(31)
50
51 #define CSI2RX_STREAM_DATA_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x008)
52 #define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n) BIT((n) + 16)
53
54 #define CSI2RX_STREAM_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x00c)
55 #define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF (1 << 8)
56
57 #define CSI2RX_LANES_MAX 4
58 #define CSI2RX_STREAMS_MAX 4
59
60 enum csi2rx_pads {
61 CSI2RX_PAD_SINK,
62 CSI2RX_PAD_SOURCE_STREAM0,
63 CSI2RX_PAD_SOURCE_STREAM1,
64 CSI2RX_PAD_SOURCE_STREAM2,
65 CSI2RX_PAD_SOURCE_STREAM3,
66 CSI2RX_PAD_MAX,
67 };
68
69 struct csi2rx_fmt {
70 u32 code;
71 u8 bpp;
72 };
73
74 struct csi2rx_priv {
75 struct device *dev;
76 unsigned int count;
77
78 /*
79 * Used to prevent race conditions between multiple,
80 * concurrent calls to start and stop.
81 */
82 struct mutex lock;
83
84 void __iomem *base;
85 struct clk *sys_clk;
86 struct clk *p_clk;
87 struct clk *pixel_clk[CSI2RX_STREAMS_MAX];
88 struct reset_control *sys_rst;
89 struct reset_control *p_rst;
90 struct reset_control *pixel_rst[CSI2RX_STREAMS_MAX];
91 struct phy *dphy;
92
93 u8 lanes[CSI2RX_LANES_MAX];
94 u8 num_lanes;
95 u8 max_lanes;
96 u8 max_streams;
97 bool has_internal_dphy;
98
99 struct v4l2_subdev subdev;
100 struct v4l2_async_notifier notifier;
101 struct media_pad pads[CSI2RX_PAD_MAX];
102
103 /* Remote source */
104 struct v4l2_subdev *source_subdev;
105 int source_pad;
106 };
107
108 static const struct csi2rx_fmt formats[] = {
109 { .code = MEDIA_BUS_FMT_YUYV8_1X16, .bpp = 16, },
110 { .code = MEDIA_BUS_FMT_UYVY8_1X16, .bpp = 16, },
111 { .code = MEDIA_BUS_FMT_YVYU8_1X16, .bpp = 16, },
112 { .code = MEDIA_BUS_FMT_VYUY8_1X16, .bpp = 16, },
113 { .code = MEDIA_BUS_FMT_SBGGR8_1X8, .bpp = 8, },
114 { .code = MEDIA_BUS_FMT_SGBRG8_1X8, .bpp = 8, },
115 { .code = MEDIA_BUS_FMT_SGRBG8_1X8, .bpp = 8, },
116 { .code = MEDIA_BUS_FMT_SRGGB8_1X8, .bpp = 8, },
117 { .code = MEDIA_BUS_FMT_SBGGR10_1X10, .bpp = 10, },
118 { .code = MEDIA_BUS_FMT_SGBRG10_1X10, .bpp = 10, },
119 { .code = MEDIA_BUS_FMT_SGRBG10_1X10, .bpp = 10, },
120 { .code = MEDIA_BUS_FMT_SRGGB10_1X10, .bpp = 10, },
121 };
122
csi2rx_get_fmt_by_code(u32 code)123 static const struct csi2rx_fmt *csi2rx_get_fmt_by_code(u32 code)
124 {
125 unsigned int i;
126
127 for (i = 0; i < ARRAY_SIZE(formats); i++)
128 if (formats[i].code == code)
129 return &formats[i];
130
131 return NULL;
132 }
133
134 static inline
v4l2_subdev_to_csi2rx(struct v4l2_subdev * subdev)135 struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev)
136 {
137 return container_of(subdev, struct csi2rx_priv, subdev);
138 }
139
csi2rx_reset(struct csi2rx_priv * csi2rx)140 static void csi2rx_reset(struct csi2rx_priv *csi2rx)
141 {
142 unsigned int i;
143
144 /* Reset module */
145 writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT,
146 csi2rx->base + CSI2RX_SOFT_RESET_REG);
147 /* Reset individual streams. */
148 for (i = 0; i < csi2rx->max_streams; i++) {
149 writel(CSI2RX_STREAM_CTRL_SOFT_RST,
150 csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
151 }
152
153 usleep_range(10, 20);
154
155 /* Clear resets */
156 writel(0, csi2rx->base + CSI2RX_SOFT_RESET_REG);
157 for (i = 0; i < csi2rx->max_streams; i++)
158 writel(0, csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
159 }
160
csi2rx_configure_ext_dphy(struct csi2rx_priv * csi2rx)161 static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
162 {
163 union phy_configure_opts opts = { };
164 struct phy_configure_opts_mipi_dphy *cfg = &opts.mipi_dphy;
165 struct v4l2_subdev_format sd_fmt = {
166 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
167 .pad = CSI2RX_PAD_SINK,
168 };
169 const struct csi2rx_fmt *fmt;
170 s64 link_freq;
171 int ret;
172
173 ret = v4l2_subdev_call_state_active(&csi2rx->subdev, pad, get_fmt,
174 &sd_fmt);
175 if (ret < 0)
176 return ret;
177
178 fmt = csi2rx_get_fmt_by_code(sd_fmt.format.code);
179
180 link_freq = v4l2_get_link_freq(csi2rx->source_subdev->ctrl_handler,
181 fmt->bpp, 2 * csi2rx->num_lanes);
182 if (link_freq < 0)
183 return link_freq;
184
185 ret = phy_mipi_dphy_get_default_config_for_hsclk(link_freq,
186 csi2rx->num_lanes, cfg);
187 if (ret)
188 return ret;
189
190 ret = phy_power_on(csi2rx->dphy);
191 if (ret)
192 return ret;
193
194 ret = phy_configure(csi2rx->dphy, &opts);
195 if (ret) {
196 phy_power_off(csi2rx->dphy);
197 return ret;
198 }
199
200 return 0;
201 }
202
csi2rx_start(struct csi2rx_priv * csi2rx)203 static int csi2rx_start(struct csi2rx_priv *csi2rx)
204 {
205 unsigned int i;
206 unsigned long lanes_used = 0;
207 u32 reg;
208 int ret;
209
210 ret = clk_prepare_enable(csi2rx->p_clk);
211 if (ret)
212 return ret;
213
214 reset_control_deassert(csi2rx->p_rst);
215 csi2rx_reset(csi2rx);
216
217 reg = csi2rx->num_lanes << 8;
218 for (i = 0; i < csi2rx->num_lanes; i++) {
219 reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csi2rx->lanes[i]);
220 set_bit(csi2rx->lanes[i], &lanes_used);
221 }
222
223 /*
224 * Even the unused lanes need to be mapped. In order to avoid
225 * to map twice to the same physical lane, keep the lanes used
226 * in the previous loop, and only map unused physical lanes to
227 * the rest of our logical lanes.
228 */
229 for (i = csi2rx->num_lanes; i < csi2rx->max_lanes; i++) {
230 unsigned int idx = find_first_zero_bit(&lanes_used,
231 csi2rx->max_lanes);
232 set_bit(idx, &lanes_used);
233 reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, i + 1);
234 }
235
236 writel(reg, csi2rx->base + CSI2RX_STATIC_CFG_REG);
237
238 ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true);
239 if (ret)
240 goto err_disable_pclk;
241
242 /* Enable DPHY clk and data lanes. */
243 if (csi2rx->dphy) {
244 reg = CSI2RX_DPHY_CL_EN | CSI2RX_DPHY_CL_RST;
245 for (i = 0; i < csi2rx->num_lanes; i++) {
246 reg |= CSI2RX_DPHY_DL_EN(csi2rx->lanes[i] - 1);
247 reg |= CSI2RX_DPHY_DL_RST(csi2rx->lanes[i] - 1);
248 }
249
250 writel(reg, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
251 }
252
253 /*
254 * Create a static mapping between the CSI virtual channels
255 * and the output stream.
256 *
257 * This should be enhanced, but v4l2 lacks the support for
258 * changing that mapping dynamically.
259 *
260 * We also cannot enable and disable independent streams here,
261 * hence the reference counting.
262 */
263 for (i = 0; i < csi2rx->max_streams; i++) {
264 ret = clk_prepare_enable(csi2rx->pixel_clk[i]);
265 if (ret)
266 goto err_disable_pixclk;
267
268 reset_control_deassert(csi2rx->pixel_rst[i]);
269
270 writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF,
271 csi2rx->base + CSI2RX_STREAM_CFG_REG(i));
272
273 /*
274 * Enable one virtual channel. When multiple virtual channels
275 * are supported this will have to be changed.
276 */
277 writel(CSI2RX_STREAM_DATA_CFG_VC_SELECT(0),
278 csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i));
279
280 writel(CSI2RX_STREAM_CTRL_START,
281 csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
282 }
283
284 ret = clk_prepare_enable(csi2rx->sys_clk);
285 if (ret)
286 goto err_disable_pixclk;
287
288 reset_control_deassert(csi2rx->sys_rst);
289
290 if (csi2rx->dphy) {
291 ret = csi2rx_configure_ext_dphy(csi2rx);
292 if (ret) {
293 dev_err(csi2rx->dev,
294 "Failed to configure external DPHY: %d\n", ret);
295 goto err_disable_sysclk;
296 }
297 }
298
299 clk_disable_unprepare(csi2rx->p_clk);
300
301 return 0;
302
303 err_disable_sysclk:
304 clk_disable_unprepare(csi2rx->sys_clk);
305 err_disable_pixclk:
306 for (; i > 0; i--) {
307 reset_control_assert(csi2rx->pixel_rst[i - 1]);
308 clk_disable_unprepare(csi2rx->pixel_clk[i - 1]);
309 }
310
311 err_disable_pclk:
312 clk_disable_unprepare(csi2rx->p_clk);
313
314 return ret;
315 }
316
csi2rx_stop(struct csi2rx_priv * csi2rx)317 static void csi2rx_stop(struct csi2rx_priv *csi2rx)
318 {
319 unsigned int i;
320 u32 val;
321 int ret;
322
323 clk_prepare_enable(csi2rx->p_clk);
324 reset_control_assert(csi2rx->sys_rst);
325 clk_disable_unprepare(csi2rx->sys_clk);
326
327 for (i = 0; i < csi2rx->max_streams; i++) {
328 writel(CSI2RX_STREAM_CTRL_STOP,
329 csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
330
331 ret = readl_relaxed_poll_timeout(csi2rx->base +
332 CSI2RX_STREAM_STATUS_REG(i),
333 val,
334 !(val & CSI2RX_STREAM_STATUS_RDY),
335 10, 10000);
336 if (ret)
337 dev_warn(csi2rx->dev,
338 "Failed to stop streaming on pad%u\n", i);
339
340 reset_control_assert(csi2rx->pixel_rst[i]);
341 clk_disable_unprepare(csi2rx->pixel_clk[i]);
342 }
343
344 reset_control_assert(csi2rx->p_rst);
345 clk_disable_unprepare(csi2rx->p_clk);
346
347 if (v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, false))
348 dev_warn(csi2rx->dev, "Couldn't disable our subdev\n");
349
350 if (csi2rx->dphy) {
351 writel(0, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
352
353 if (phy_power_off(csi2rx->dphy))
354 dev_warn(csi2rx->dev, "Couldn't power off DPHY\n");
355 }
356 }
357
csi2rx_s_stream(struct v4l2_subdev * subdev,int enable)358 static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable)
359 {
360 struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
361 int ret = 0;
362
363 mutex_lock(&csi2rx->lock);
364
365 if (enable) {
366 /*
367 * If we're not the first users, there's no need to
368 * enable the whole controller.
369 */
370 if (!csi2rx->count) {
371 ret = csi2rx_start(csi2rx);
372 if (ret)
373 goto out;
374 }
375
376 csi2rx->count++;
377 } else {
378 csi2rx->count--;
379
380 /*
381 * Let the last user turn off the lights.
382 */
383 if (!csi2rx->count)
384 csi2rx_stop(csi2rx);
385 }
386
387 out:
388 mutex_unlock(&csi2rx->lock);
389 return ret;
390 }
391
csi2rx_set_fmt(struct v4l2_subdev * subdev,struct v4l2_subdev_state * state,struct v4l2_subdev_format * format)392 static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
393 struct v4l2_subdev_state *state,
394 struct v4l2_subdev_format *format)
395 {
396 struct v4l2_mbus_framefmt *fmt;
397 unsigned int i;
398
399 /* No transcoding, source and sink formats must match. */
400 if (format->pad != CSI2RX_PAD_SINK)
401 return v4l2_subdev_get_fmt(subdev, state, format);
402
403 if (!csi2rx_get_fmt_by_code(format->format.code))
404 format->format.code = formats[0].code;
405
406 format->format.field = V4L2_FIELD_NONE;
407
408 /* Set sink format */
409 fmt = v4l2_subdev_state_get_format(state, format->pad);
410 *fmt = format->format;
411
412 /* Propagate to source formats */
413 for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
414 fmt = v4l2_subdev_state_get_format(state, i);
415 *fmt = format->format;
416 }
417
418 return 0;
419 }
420
csi2rx_init_state(struct v4l2_subdev * subdev,struct v4l2_subdev_state * state)421 static int csi2rx_init_state(struct v4l2_subdev *subdev,
422 struct v4l2_subdev_state *state)
423 {
424 struct v4l2_subdev_format format = {
425 .pad = CSI2RX_PAD_SINK,
426 .format = {
427 .width = 640,
428 .height = 480,
429 .code = MEDIA_BUS_FMT_UYVY8_1X16,
430 .field = V4L2_FIELD_NONE,
431 .colorspace = V4L2_COLORSPACE_SRGB,
432 .ycbcr_enc = V4L2_YCBCR_ENC_601,
433 .quantization = V4L2_QUANTIZATION_LIM_RANGE,
434 .xfer_func = V4L2_XFER_FUNC_SRGB,
435 },
436 };
437
438 return csi2rx_set_fmt(subdev, state, &format);
439 }
440
441 static const struct v4l2_subdev_pad_ops csi2rx_pad_ops = {
442 .get_fmt = v4l2_subdev_get_fmt,
443 .set_fmt = csi2rx_set_fmt,
444 };
445
446 static const struct v4l2_subdev_video_ops csi2rx_video_ops = {
447 .s_stream = csi2rx_s_stream,
448 };
449
450 static const struct v4l2_subdev_ops csi2rx_subdev_ops = {
451 .video = &csi2rx_video_ops,
452 .pad = &csi2rx_pad_ops,
453 };
454
455 static const struct v4l2_subdev_internal_ops csi2rx_internal_ops = {
456 .init_state = csi2rx_init_state,
457 };
458
459 static const struct media_entity_operations csi2rx_media_ops = {
460 .link_validate = v4l2_subdev_link_validate,
461 };
462
csi2rx_async_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * s_subdev,struct v4l2_async_connection * asd)463 static int csi2rx_async_bound(struct v4l2_async_notifier *notifier,
464 struct v4l2_subdev *s_subdev,
465 struct v4l2_async_connection *asd)
466 {
467 struct v4l2_subdev *subdev = notifier->sd;
468 struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
469
470 csi2rx->source_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
471 s_subdev->fwnode,
472 MEDIA_PAD_FL_SOURCE);
473 if (csi2rx->source_pad < 0) {
474 dev_err(csi2rx->dev, "Couldn't find output pad for subdev %s\n",
475 s_subdev->name);
476 return csi2rx->source_pad;
477 }
478
479 csi2rx->source_subdev = s_subdev;
480
481 dev_dbg(csi2rx->dev, "Bound %s pad: %d\n", s_subdev->name,
482 csi2rx->source_pad);
483
484 return media_create_pad_link(&csi2rx->source_subdev->entity,
485 csi2rx->source_pad,
486 &csi2rx->subdev.entity, 0,
487 MEDIA_LNK_FL_ENABLED |
488 MEDIA_LNK_FL_IMMUTABLE);
489 }
490
491 static const struct v4l2_async_notifier_operations csi2rx_notifier_ops = {
492 .bound = csi2rx_async_bound,
493 };
494
csi2rx_get_resources(struct csi2rx_priv * csi2rx,struct platform_device * pdev)495 static int csi2rx_get_resources(struct csi2rx_priv *csi2rx,
496 struct platform_device *pdev)
497 {
498 unsigned char i;
499 u32 dev_cfg;
500 int ret;
501
502 csi2rx->base = devm_platform_ioremap_resource(pdev, 0);
503 if (IS_ERR(csi2rx->base))
504 return PTR_ERR(csi2rx->base);
505
506 csi2rx->sys_clk = devm_clk_get(&pdev->dev, "sys_clk");
507 if (IS_ERR(csi2rx->sys_clk)) {
508 dev_err(&pdev->dev, "Couldn't get sys clock\n");
509 return PTR_ERR(csi2rx->sys_clk);
510 }
511
512 csi2rx->p_clk = devm_clk_get(&pdev->dev, "p_clk");
513 if (IS_ERR(csi2rx->p_clk)) {
514 dev_err(&pdev->dev, "Couldn't get P clock\n");
515 return PTR_ERR(csi2rx->p_clk);
516 }
517
518 csi2rx->sys_rst = devm_reset_control_get_optional_exclusive(&pdev->dev,
519 "sys");
520 if (IS_ERR(csi2rx->sys_rst))
521 return PTR_ERR(csi2rx->sys_rst);
522
523 csi2rx->p_rst = devm_reset_control_get_optional_exclusive(&pdev->dev,
524 "reg_bank");
525 if (IS_ERR(csi2rx->p_rst))
526 return PTR_ERR(csi2rx->p_rst);
527
528 csi2rx->dphy = devm_phy_optional_get(&pdev->dev, "dphy");
529 if (IS_ERR(csi2rx->dphy)) {
530 dev_err(&pdev->dev, "Couldn't get external D-PHY\n");
531 return PTR_ERR(csi2rx->dphy);
532 }
533
534 ret = clk_prepare_enable(csi2rx->p_clk);
535 if (ret) {
536 dev_err(&pdev->dev, "Couldn't prepare and enable P clock\n");
537 return ret;
538 }
539
540 dev_cfg = readl(csi2rx->base + CSI2RX_DEVICE_CFG_REG);
541 clk_disable_unprepare(csi2rx->p_clk);
542
543 csi2rx->max_lanes = dev_cfg & 7;
544 if (csi2rx->max_lanes > CSI2RX_LANES_MAX) {
545 dev_err(&pdev->dev, "Invalid number of lanes: %u\n",
546 csi2rx->max_lanes);
547 return -EINVAL;
548 }
549
550 csi2rx->max_streams = (dev_cfg >> 4) & 7;
551 if (csi2rx->max_streams > CSI2RX_STREAMS_MAX) {
552 dev_err(&pdev->dev, "Invalid number of streams: %u\n",
553 csi2rx->max_streams);
554 return -EINVAL;
555 }
556
557 csi2rx->has_internal_dphy = dev_cfg & BIT(3) ? true : false;
558
559 /*
560 * FIXME: Once we'll have internal D-PHY support, the check
561 * will need to be removed.
562 */
563 if (!csi2rx->dphy && csi2rx->has_internal_dphy) {
564 dev_err(&pdev->dev, "Internal D-PHY not supported yet\n");
565 return -EINVAL;
566 }
567
568 for (i = 0; i < csi2rx->max_streams; i++) {
569 char name[16];
570
571 snprintf(name, sizeof(name), "pixel_if%u_clk", i);
572 csi2rx->pixel_clk[i] = devm_clk_get(&pdev->dev, name);
573 if (IS_ERR(csi2rx->pixel_clk[i])) {
574 dev_err(&pdev->dev, "Couldn't get clock %s\n", name);
575 return PTR_ERR(csi2rx->pixel_clk[i]);
576 }
577
578 snprintf(name, sizeof(name), "pixel_if%u", i);
579 csi2rx->pixel_rst[i] =
580 devm_reset_control_get_optional_exclusive(&pdev->dev,
581 name);
582 if (IS_ERR(csi2rx->pixel_rst[i]))
583 return PTR_ERR(csi2rx->pixel_rst[i]);
584 }
585
586 return 0;
587 }
588
csi2rx_parse_dt(struct csi2rx_priv * csi2rx)589 static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx)
590 {
591 struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 };
592 struct v4l2_async_connection *asd;
593 struct fwnode_handle *fwh;
594 struct device_node *ep;
595 int ret;
596
597 ep = of_graph_get_endpoint_by_regs(csi2rx->dev->of_node, 0, 0);
598 if (!ep)
599 return -EINVAL;
600
601 fwh = of_fwnode_handle(ep);
602 ret = v4l2_fwnode_endpoint_parse(fwh, &v4l2_ep);
603 if (ret) {
604 dev_err(csi2rx->dev, "Could not parse v4l2 endpoint\n");
605 of_node_put(ep);
606 return ret;
607 }
608
609 if (v4l2_ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
610 dev_err(csi2rx->dev, "Unsupported media bus type: 0x%x\n",
611 v4l2_ep.bus_type);
612 of_node_put(ep);
613 return -EINVAL;
614 }
615
616 memcpy(csi2rx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
617 sizeof(csi2rx->lanes));
618 csi2rx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
619 if (csi2rx->num_lanes > csi2rx->max_lanes) {
620 dev_err(csi2rx->dev, "Unsupported number of data-lanes: %d\n",
621 csi2rx->num_lanes);
622 of_node_put(ep);
623 return -EINVAL;
624 }
625
626 v4l2_async_subdev_nf_init(&csi2rx->notifier, &csi2rx->subdev);
627
628 asd = v4l2_async_nf_add_fwnode_remote(&csi2rx->notifier, fwh,
629 struct v4l2_async_connection);
630 of_node_put(ep);
631 if (IS_ERR(asd)) {
632 v4l2_async_nf_cleanup(&csi2rx->notifier);
633 return PTR_ERR(asd);
634 }
635
636 csi2rx->notifier.ops = &csi2rx_notifier_ops;
637
638 ret = v4l2_async_nf_register(&csi2rx->notifier);
639 if (ret)
640 v4l2_async_nf_cleanup(&csi2rx->notifier);
641
642 return ret;
643 }
644
csi2rx_probe(struct platform_device * pdev)645 static int csi2rx_probe(struct platform_device *pdev)
646 {
647 struct csi2rx_priv *csi2rx;
648 unsigned int i;
649 int ret;
650
651 csi2rx = kzalloc(sizeof(*csi2rx), GFP_KERNEL);
652 if (!csi2rx)
653 return -ENOMEM;
654 platform_set_drvdata(pdev, csi2rx);
655 csi2rx->dev = &pdev->dev;
656 mutex_init(&csi2rx->lock);
657
658 ret = csi2rx_get_resources(csi2rx, pdev);
659 if (ret)
660 goto err_free_priv;
661
662 ret = csi2rx_parse_dt(csi2rx);
663 if (ret)
664 goto err_free_priv;
665
666 csi2rx->subdev.owner = THIS_MODULE;
667 csi2rx->subdev.dev = &pdev->dev;
668 v4l2_subdev_init(&csi2rx->subdev, &csi2rx_subdev_ops);
669 csi2rx->subdev.internal_ops = &csi2rx_internal_ops;
670 v4l2_set_subdevdata(&csi2rx->subdev, &pdev->dev);
671 snprintf(csi2rx->subdev.name, sizeof(csi2rx->subdev.name),
672 "%s.%s", KBUILD_MODNAME, dev_name(&pdev->dev));
673
674 /* Create our media pads */
675 csi2rx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
676 csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
677 for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++)
678 csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE;
679 csi2rx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
680 csi2rx->subdev.entity.ops = &csi2rx_media_ops;
681
682 ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX,
683 csi2rx->pads);
684 if (ret)
685 goto err_cleanup;
686
687 ret = v4l2_subdev_init_finalize(&csi2rx->subdev);
688 if (ret)
689 goto err_cleanup;
690
691 ret = v4l2_async_register_subdev(&csi2rx->subdev);
692 if (ret < 0)
693 goto err_free_state;
694
695 dev_info(&pdev->dev,
696 "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n",
697 csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams,
698 csi2rx->dphy ? "external" :
699 csi2rx->has_internal_dphy ? "internal" : "no");
700
701 return 0;
702
703 err_free_state:
704 v4l2_subdev_cleanup(&csi2rx->subdev);
705 err_cleanup:
706 v4l2_async_nf_unregister(&csi2rx->notifier);
707 v4l2_async_nf_cleanup(&csi2rx->notifier);
708 media_entity_cleanup(&csi2rx->subdev.entity);
709 err_free_priv:
710 kfree(csi2rx);
711 return ret;
712 }
713
csi2rx_remove(struct platform_device * pdev)714 static void csi2rx_remove(struct platform_device *pdev)
715 {
716 struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev);
717
718 v4l2_async_nf_unregister(&csi2rx->notifier);
719 v4l2_async_nf_cleanup(&csi2rx->notifier);
720 v4l2_async_unregister_subdev(&csi2rx->subdev);
721 v4l2_subdev_cleanup(&csi2rx->subdev);
722 media_entity_cleanup(&csi2rx->subdev.entity);
723 kfree(csi2rx);
724 }
725
726 static const struct of_device_id csi2rx_of_table[] = {
727 { .compatible = "starfive,jh7110-csi2rx" },
728 { .compatible = "cdns,csi2rx" },
729 { },
730 };
731 MODULE_DEVICE_TABLE(of, csi2rx_of_table);
732
733 static struct platform_driver csi2rx_driver = {
734 .probe = csi2rx_probe,
735 .remove_new = csi2rx_remove,
736
737 .driver = {
738 .name = "cdns-csi2rx",
739 .of_match_table = csi2rx_of_table,
740 },
741 };
742 module_platform_driver(csi2rx_driver);
743 MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>");
744 MODULE_DESCRIPTION("Cadence CSI2-RX controller");
745 MODULE_LICENSE("GPL");
746