xref: /linux/drivers/media/platform/cadence/cdns-csi2rx.c (revision c5dbf04160005e07e8ca7232a7faa77ab1547ae0)
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 
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
135 struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev)
136 {
137 	return container_of(subdev, struct csi2rx_priv, subdev);
138 }
139 
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 
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 
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 
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 
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 
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_get_pad_format(subdev, 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_get_pad_format(subdev, state, i);
415 		*fmt = format->format;
416 	}
417 
418 	return 0;
419 }
420 
421 static int csi2rx_init_cfg(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 	.init_cfg	= csi2rx_init_cfg,
445 };
446 
447 static const struct v4l2_subdev_video_ops csi2rx_video_ops = {
448 	.s_stream	= csi2rx_s_stream,
449 };
450 
451 static const struct v4l2_subdev_ops csi2rx_subdev_ops = {
452 	.video		= &csi2rx_video_ops,
453 	.pad		= &csi2rx_pad_ops,
454 };
455 
456 static const struct media_entity_operations csi2rx_media_ops = {
457 	.link_validate = v4l2_subdev_link_validate,
458 };
459 
460 static int csi2rx_async_bound(struct v4l2_async_notifier *notifier,
461 			      struct v4l2_subdev *s_subdev,
462 			      struct v4l2_async_connection *asd)
463 {
464 	struct v4l2_subdev *subdev = notifier->sd;
465 	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
466 
467 	csi2rx->source_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
468 							 s_subdev->fwnode,
469 							 MEDIA_PAD_FL_SOURCE);
470 	if (csi2rx->source_pad < 0) {
471 		dev_err(csi2rx->dev, "Couldn't find output pad for subdev %s\n",
472 			s_subdev->name);
473 		return csi2rx->source_pad;
474 	}
475 
476 	csi2rx->source_subdev = s_subdev;
477 
478 	dev_dbg(csi2rx->dev, "Bound %s pad: %d\n", s_subdev->name,
479 		csi2rx->source_pad);
480 
481 	return media_create_pad_link(&csi2rx->source_subdev->entity,
482 				     csi2rx->source_pad,
483 				     &csi2rx->subdev.entity, 0,
484 				     MEDIA_LNK_FL_ENABLED |
485 				     MEDIA_LNK_FL_IMMUTABLE);
486 }
487 
488 static const struct v4l2_async_notifier_operations csi2rx_notifier_ops = {
489 	.bound		= csi2rx_async_bound,
490 };
491 
492 static int csi2rx_get_resources(struct csi2rx_priv *csi2rx,
493 				struct platform_device *pdev)
494 {
495 	unsigned char i;
496 	u32 dev_cfg;
497 	int ret;
498 
499 	csi2rx->base = devm_platform_ioremap_resource(pdev, 0);
500 	if (IS_ERR(csi2rx->base))
501 		return PTR_ERR(csi2rx->base);
502 
503 	csi2rx->sys_clk = devm_clk_get(&pdev->dev, "sys_clk");
504 	if (IS_ERR(csi2rx->sys_clk)) {
505 		dev_err(&pdev->dev, "Couldn't get sys clock\n");
506 		return PTR_ERR(csi2rx->sys_clk);
507 	}
508 
509 	csi2rx->p_clk = devm_clk_get(&pdev->dev, "p_clk");
510 	if (IS_ERR(csi2rx->p_clk)) {
511 		dev_err(&pdev->dev, "Couldn't get P clock\n");
512 		return PTR_ERR(csi2rx->p_clk);
513 	}
514 
515 	csi2rx->sys_rst = devm_reset_control_get_optional_exclusive(&pdev->dev,
516 								    "sys");
517 	if (IS_ERR(csi2rx->sys_rst))
518 		return PTR_ERR(csi2rx->sys_rst);
519 
520 	csi2rx->p_rst = devm_reset_control_get_optional_exclusive(&pdev->dev,
521 								  "reg_bank");
522 	if (IS_ERR(csi2rx->p_rst))
523 		return PTR_ERR(csi2rx->p_rst);
524 
525 	csi2rx->dphy = devm_phy_optional_get(&pdev->dev, "dphy");
526 	if (IS_ERR(csi2rx->dphy)) {
527 		dev_err(&pdev->dev, "Couldn't get external D-PHY\n");
528 		return PTR_ERR(csi2rx->dphy);
529 	}
530 
531 	ret = clk_prepare_enable(csi2rx->p_clk);
532 	if (ret) {
533 		dev_err(&pdev->dev, "Couldn't prepare and enable P clock\n");
534 		return ret;
535 	}
536 
537 	dev_cfg = readl(csi2rx->base + CSI2RX_DEVICE_CFG_REG);
538 	clk_disable_unprepare(csi2rx->p_clk);
539 
540 	csi2rx->max_lanes = dev_cfg & 7;
541 	if (csi2rx->max_lanes > CSI2RX_LANES_MAX) {
542 		dev_err(&pdev->dev, "Invalid number of lanes: %u\n",
543 			csi2rx->max_lanes);
544 		return -EINVAL;
545 	}
546 
547 	csi2rx->max_streams = (dev_cfg >> 4) & 7;
548 	if (csi2rx->max_streams > CSI2RX_STREAMS_MAX) {
549 		dev_err(&pdev->dev, "Invalid number of streams: %u\n",
550 			csi2rx->max_streams);
551 		return -EINVAL;
552 	}
553 
554 	csi2rx->has_internal_dphy = dev_cfg & BIT(3) ? true : false;
555 
556 	/*
557 	 * FIXME: Once we'll have internal D-PHY support, the check
558 	 * will need to be removed.
559 	 */
560 	if (!csi2rx->dphy && csi2rx->has_internal_dphy) {
561 		dev_err(&pdev->dev, "Internal D-PHY not supported yet\n");
562 		return -EINVAL;
563 	}
564 
565 	for (i = 0; i < csi2rx->max_streams; i++) {
566 		char name[16];
567 
568 		snprintf(name, sizeof(name), "pixel_if%u_clk", i);
569 		csi2rx->pixel_clk[i] = devm_clk_get(&pdev->dev, name);
570 		if (IS_ERR(csi2rx->pixel_clk[i])) {
571 			dev_err(&pdev->dev, "Couldn't get clock %s\n", name);
572 			return PTR_ERR(csi2rx->pixel_clk[i]);
573 		}
574 
575 		snprintf(name, sizeof(name), "pixel_if%u", i);
576 		csi2rx->pixel_rst[i] =
577 			devm_reset_control_get_optional_exclusive(&pdev->dev,
578 								  name);
579 		if (IS_ERR(csi2rx->pixel_rst[i]))
580 			return PTR_ERR(csi2rx->pixel_rst[i]);
581 	}
582 
583 	return 0;
584 }
585 
586 static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx)
587 {
588 	struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 };
589 	struct v4l2_async_connection *asd;
590 	struct fwnode_handle *fwh;
591 	struct device_node *ep;
592 	int ret;
593 
594 	ep = of_graph_get_endpoint_by_regs(csi2rx->dev->of_node, 0, 0);
595 	if (!ep)
596 		return -EINVAL;
597 
598 	fwh = of_fwnode_handle(ep);
599 	ret = v4l2_fwnode_endpoint_parse(fwh, &v4l2_ep);
600 	if (ret) {
601 		dev_err(csi2rx->dev, "Could not parse v4l2 endpoint\n");
602 		of_node_put(ep);
603 		return ret;
604 	}
605 
606 	if (v4l2_ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
607 		dev_err(csi2rx->dev, "Unsupported media bus type: 0x%x\n",
608 			v4l2_ep.bus_type);
609 		of_node_put(ep);
610 		return -EINVAL;
611 	}
612 
613 	memcpy(csi2rx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
614 	       sizeof(csi2rx->lanes));
615 	csi2rx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
616 	if (csi2rx->num_lanes > csi2rx->max_lanes) {
617 		dev_err(csi2rx->dev, "Unsupported number of data-lanes: %d\n",
618 			csi2rx->num_lanes);
619 		of_node_put(ep);
620 		return -EINVAL;
621 	}
622 
623 	v4l2_async_subdev_nf_init(&csi2rx->notifier, &csi2rx->subdev);
624 
625 	asd = v4l2_async_nf_add_fwnode_remote(&csi2rx->notifier, fwh,
626 					      struct v4l2_async_connection);
627 	of_node_put(ep);
628 	if (IS_ERR(asd)) {
629 		v4l2_async_nf_cleanup(&csi2rx->notifier);
630 		return PTR_ERR(asd);
631 	}
632 
633 	csi2rx->notifier.ops = &csi2rx_notifier_ops;
634 
635 	ret = v4l2_async_nf_register(&csi2rx->notifier);
636 	if (ret)
637 		v4l2_async_nf_cleanup(&csi2rx->notifier);
638 
639 	return ret;
640 }
641 
642 static int csi2rx_probe(struct platform_device *pdev)
643 {
644 	struct csi2rx_priv *csi2rx;
645 	unsigned int i;
646 	int ret;
647 
648 	csi2rx = kzalloc(sizeof(*csi2rx), GFP_KERNEL);
649 	if (!csi2rx)
650 		return -ENOMEM;
651 	platform_set_drvdata(pdev, csi2rx);
652 	csi2rx->dev = &pdev->dev;
653 	mutex_init(&csi2rx->lock);
654 
655 	ret = csi2rx_get_resources(csi2rx, pdev);
656 	if (ret)
657 		goto err_free_priv;
658 
659 	ret = csi2rx_parse_dt(csi2rx);
660 	if (ret)
661 		goto err_free_priv;
662 
663 	csi2rx->subdev.owner = THIS_MODULE;
664 	csi2rx->subdev.dev = &pdev->dev;
665 	v4l2_subdev_init(&csi2rx->subdev, &csi2rx_subdev_ops);
666 	v4l2_set_subdevdata(&csi2rx->subdev, &pdev->dev);
667 	snprintf(csi2rx->subdev.name, sizeof(csi2rx->subdev.name),
668 		 "%s.%s", KBUILD_MODNAME, dev_name(&pdev->dev));
669 
670 	/* Create our media pads */
671 	csi2rx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
672 	csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
673 	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++)
674 		csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE;
675 	csi2rx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
676 	csi2rx->subdev.entity.ops = &csi2rx_media_ops;
677 
678 	ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX,
679 				     csi2rx->pads);
680 	if (ret)
681 		goto err_cleanup;
682 
683 	ret = v4l2_subdev_init_finalize(&csi2rx->subdev);
684 	if (ret)
685 		goto err_cleanup;
686 
687 	ret = v4l2_async_register_subdev(&csi2rx->subdev);
688 	if (ret < 0)
689 		goto err_free_state;
690 
691 	dev_info(&pdev->dev,
692 		 "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n",
693 		 csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams,
694 		 csi2rx->dphy ? "external" :
695 		 csi2rx->has_internal_dphy ? "internal" : "no");
696 
697 	return 0;
698 
699 err_free_state:
700 	v4l2_subdev_cleanup(&csi2rx->subdev);
701 err_cleanup:
702 	v4l2_async_nf_unregister(&csi2rx->notifier);
703 	v4l2_async_nf_cleanup(&csi2rx->notifier);
704 	media_entity_cleanup(&csi2rx->subdev.entity);
705 err_free_priv:
706 	kfree(csi2rx);
707 	return ret;
708 }
709 
710 static void csi2rx_remove(struct platform_device *pdev)
711 {
712 	struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev);
713 
714 	v4l2_async_nf_unregister(&csi2rx->notifier);
715 	v4l2_async_nf_cleanup(&csi2rx->notifier);
716 	v4l2_async_unregister_subdev(&csi2rx->subdev);
717 	v4l2_subdev_cleanup(&csi2rx->subdev);
718 	media_entity_cleanup(&csi2rx->subdev.entity);
719 	kfree(csi2rx);
720 }
721 
722 static const struct of_device_id csi2rx_of_table[] = {
723 	{ .compatible = "starfive,jh7110-csi2rx" },
724 	{ .compatible = "cdns,csi2rx" },
725 	{ },
726 };
727 MODULE_DEVICE_TABLE(of, csi2rx_of_table);
728 
729 static struct platform_driver csi2rx_driver = {
730 	.probe	= csi2rx_probe,
731 	.remove_new = csi2rx_remove,
732 
733 	.driver	= {
734 		.name		= "cdns-csi2rx",
735 		.of_match_table	= csi2rx_of_table,
736 	},
737 };
738 module_platform_driver(csi2rx_driver);
739 MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>");
740 MODULE_DESCRIPTION("Cadence CSI2-RX controller");
741 MODULE_LICENSE("GPL");
742