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