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/module.h>
12 #include <linux/of.h>
13 #include <linux/of_graph.h>
14 #include <linux/phy/phy.h>
15 #include <linux/platform_device.h>
16 #include <linux/slab.h>
17 
18 #include <media/v4l2-ctrls.h>
19 #include <media/v4l2-device.h>
20 #include <media/v4l2-fwnode.h>
21 #include <media/v4l2-subdev.h>
22 
23 #define CSI2RX_DEVICE_CFG_REG			0x000
24 
25 #define CSI2RX_SOFT_RESET_REG			0x004
26 #define CSI2RX_SOFT_RESET_PROTOCOL			BIT(1)
27 #define CSI2RX_SOFT_RESET_FRONT				BIT(0)
28 
29 #define CSI2RX_STATIC_CFG_REG			0x008
30 #define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane)	((plane) << (16 + (llane) * 4))
31 #define CSI2RX_STATIC_CFG_LANES_MASK			GENMASK(11, 8)
32 
33 #define CSI2RX_STREAM_BASE(n)		(((n) + 1) * 0x100)
34 
35 #define CSI2RX_STREAM_CTRL_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x000)
36 #define CSI2RX_STREAM_CTRL_START			BIT(0)
37 
38 #define CSI2RX_STREAM_DATA_CFG_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x008)
39 #define CSI2RX_STREAM_DATA_CFG_EN_VC_SELECT		BIT(31)
40 #define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n)		BIT((n) + 16)
41 
42 #define CSI2RX_STREAM_CFG_REG(n)		(CSI2RX_STREAM_BASE(n) + 0x00c)
43 #define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF		(1 << 8)
44 
45 #define CSI2RX_LANES_MAX	4
46 #define CSI2RX_STREAMS_MAX	4
47 
48 enum csi2rx_pads {
49 	CSI2RX_PAD_SINK,
50 	CSI2RX_PAD_SOURCE_STREAM0,
51 	CSI2RX_PAD_SOURCE_STREAM1,
52 	CSI2RX_PAD_SOURCE_STREAM2,
53 	CSI2RX_PAD_SOURCE_STREAM3,
54 	CSI2RX_PAD_MAX,
55 };
56 
57 struct csi2rx_priv {
58 	struct device			*dev;
59 	unsigned int			count;
60 
61 	/*
62 	 * Used to prevent race conditions between multiple,
63 	 * concurrent calls to start and stop.
64 	 */
65 	struct mutex			lock;
66 
67 	void __iomem			*base;
68 	struct clk			*sys_clk;
69 	struct clk			*p_clk;
70 	struct clk			*pixel_clk[CSI2RX_STREAMS_MAX];
71 	struct phy			*dphy;
72 
73 	u8				lanes[CSI2RX_LANES_MAX];
74 	u8				num_lanes;
75 	u8				max_lanes;
76 	u8				max_streams;
77 	bool				has_internal_dphy;
78 
79 	struct v4l2_subdev		subdev;
80 	struct v4l2_async_notifier	notifier;
81 	struct media_pad		pads[CSI2RX_PAD_MAX];
82 
83 	/* Remote source */
84 	struct v4l2_async_subdev	asd;
85 	struct v4l2_subdev		*source_subdev;
86 	int				source_pad;
87 };
88 
89 static inline
90 struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev)
91 {
92 	return container_of(subdev, struct csi2rx_priv, subdev);
93 }
94 
95 static void csi2rx_reset(struct csi2rx_priv *csi2rx)
96 {
97 	writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT,
98 	       csi2rx->base + CSI2RX_SOFT_RESET_REG);
99 
100 	udelay(10);
101 
102 	writel(0, csi2rx->base + CSI2RX_SOFT_RESET_REG);
103 }
104 
105 static int csi2rx_start(struct csi2rx_priv *csi2rx)
106 {
107 	unsigned int i;
108 	unsigned long lanes_used = 0;
109 	u32 reg;
110 	int ret;
111 
112 	ret = clk_prepare_enable(csi2rx->p_clk);
113 	if (ret)
114 		return ret;
115 
116 	csi2rx_reset(csi2rx);
117 
118 	reg = csi2rx->num_lanes << 8;
119 	for (i = 0; i < csi2rx->num_lanes; i++) {
120 		reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csi2rx->lanes[i]);
121 		set_bit(csi2rx->lanes[i], &lanes_used);
122 	}
123 
124 	/*
125 	 * Even the unused lanes need to be mapped. In order to avoid
126 	 * to map twice to the same physical lane, keep the lanes used
127 	 * in the previous loop, and only map unused physical lanes to
128 	 * the rest of our logical lanes.
129 	 */
130 	for (i = csi2rx->num_lanes; i < csi2rx->max_lanes; i++) {
131 		unsigned int idx = find_first_zero_bit(&lanes_used,
132 						       sizeof(lanes_used));
133 		set_bit(idx, &lanes_used);
134 		reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, i + 1);
135 	}
136 
137 	writel(reg, csi2rx->base + CSI2RX_STATIC_CFG_REG);
138 
139 	ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true);
140 	if (ret)
141 		goto err_disable_pclk;
142 
143 	/*
144 	 * Create a static mapping between the CSI virtual channels
145 	 * and the output stream.
146 	 *
147 	 * This should be enhanced, but v4l2 lacks the support for
148 	 * changing that mapping dynamically.
149 	 *
150 	 * We also cannot enable and disable independent streams here,
151 	 * hence the reference counting.
152 	 */
153 	for (i = 0; i < csi2rx->max_streams; i++) {
154 		ret = clk_prepare_enable(csi2rx->pixel_clk[i]);
155 		if (ret)
156 			goto err_disable_pixclk;
157 
158 		writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF,
159 		       csi2rx->base + CSI2RX_STREAM_CFG_REG(i));
160 
161 		writel(CSI2RX_STREAM_DATA_CFG_EN_VC_SELECT |
162 		       CSI2RX_STREAM_DATA_CFG_VC_SELECT(i),
163 		       csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i));
164 
165 		writel(CSI2RX_STREAM_CTRL_START,
166 		       csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
167 	}
168 
169 	ret = clk_prepare_enable(csi2rx->sys_clk);
170 	if (ret)
171 		goto err_disable_pixclk;
172 
173 	clk_disable_unprepare(csi2rx->p_clk);
174 
175 	return 0;
176 
177 err_disable_pixclk:
178 	for (; i > 0; i--)
179 		clk_disable_unprepare(csi2rx->pixel_clk[i - 1]);
180 
181 err_disable_pclk:
182 	clk_disable_unprepare(csi2rx->p_clk);
183 
184 	return ret;
185 }
186 
187 static void csi2rx_stop(struct csi2rx_priv *csi2rx)
188 {
189 	unsigned int i;
190 
191 	clk_prepare_enable(csi2rx->p_clk);
192 	clk_disable_unprepare(csi2rx->sys_clk);
193 
194 	for (i = 0; i < csi2rx->max_streams; i++) {
195 		writel(0, csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
196 
197 		clk_disable_unprepare(csi2rx->pixel_clk[i]);
198 	}
199 
200 	clk_disable_unprepare(csi2rx->p_clk);
201 
202 	if (v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, false))
203 		dev_warn(csi2rx->dev, "Couldn't disable our subdev\n");
204 }
205 
206 static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable)
207 {
208 	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
209 	int ret = 0;
210 
211 	mutex_lock(&csi2rx->lock);
212 
213 	if (enable) {
214 		/*
215 		 * If we're not the first users, there's no need to
216 		 * enable the whole controller.
217 		 */
218 		if (!csi2rx->count) {
219 			ret = csi2rx_start(csi2rx);
220 			if (ret)
221 				goto out;
222 		}
223 
224 		csi2rx->count++;
225 	} else {
226 		csi2rx->count--;
227 
228 		/*
229 		 * Let the last user turn off the lights.
230 		 */
231 		if (!csi2rx->count)
232 			csi2rx_stop(csi2rx);
233 	}
234 
235 out:
236 	mutex_unlock(&csi2rx->lock);
237 	return ret;
238 }
239 
240 static const struct v4l2_subdev_video_ops csi2rx_video_ops = {
241 	.s_stream	= csi2rx_s_stream,
242 };
243 
244 static const struct v4l2_subdev_ops csi2rx_subdev_ops = {
245 	.video		= &csi2rx_video_ops,
246 };
247 
248 static int csi2rx_async_bound(struct v4l2_async_notifier *notifier,
249 			      struct v4l2_subdev *s_subdev,
250 			      struct v4l2_async_subdev *asd)
251 {
252 	struct v4l2_subdev *subdev = notifier->sd;
253 	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
254 
255 	csi2rx->source_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
256 							 s_subdev->fwnode,
257 							 MEDIA_PAD_FL_SOURCE);
258 	if (csi2rx->source_pad < 0) {
259 		dev_err(csi2rx->dev, "Couldn't find output pad for subdev %s\n",
260 			s_subdev->name);
261 		return csi2rx->source_pad;
262 	}
263 
264 	csi2rx->source_subdev = s_subdev;
265 
266 	dev_dbg(csi2rx->dev, "Bound %s pad: %d\n", s_subdev->name,
267 		csi2rx->source_pad);
268 
269 	return media_create_pad_link(&csi2rx->source_subdev->entity,
270 				     csi2rx->source_pad,
271 				     &csi2rx->subdev.entity, 0,
272 				     MEDIA_LNK_FL_ENABLED |
273 				     MEDIA_LNK_FL_IMMUTABLE);
274 }
275 
276 static const struct v4l2_async_notifier_operations csi2rx_notifier_ops = {
277 	.bound		= csi2rx_async_bound,
278 };
279 
280 static int csi2rx_get_resources(struct csi2rx_priv *csi2rx,
281 				struct platform_device *pdev)
282 {
283 	struct resource *res;
284 	unsigned char i;
285 	u32 dev_cfg;
286 
287 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
288 	csi2rx->base = devm_ioremap_resource(&pdev->dev, res);
289 	if (IS_ERR(csi2rx->base))
290 		return PTR_ERR(csi2rx->base);
291 
292 	csi2rx->sys_clk = devm_clk_get(&pdev->dev, "sys_clk");
293 	if (IS_ERR(csi2rx->sys_clk)) {
294 		dev_err(&pdev->dev, "Couldn't get sys clock\n");
295 		return PTR_ERR(csi2rx->sys_clk);
296 	}
297 
298 	csi2rx->p_clk = devm_clk_get(&pdev->dev, "p_clk");
299 	if (IS_ERR(csi2rx->p_clk)) {
300 		dev_err(&pdev->dev, "Couldn't get P clock\n");
301 		return PTR_ERR(csi2rx->p_clk);
302 	}
303 
304 	csi2rx->dphy = devm_phy_optional_get(&pdev->dev, "dphy");
305 	if (IS_ERR(csi2rx->dphy)) {
306 		dev_err(&pdev->dev, "Couldn't get external D-PHY\n");
307 		return PTR_ERR(csi2rx->dphy);
308 	}
309 
310 	/*
311 	 * FIXME: Once we'll have external D-PHY support, the check
312 	 * will need to be removed.
313 	 */
314 	if (csi2rx->dphy) {
315 		dev_err(&pdev->dev, "External D-PHY not supported yet\n");
316 		return -EINVAL;
317 	}
318 
319 	clk_prepare_enable(csi2rx->p_clk);
320 	dev_cfg = readl(csi2rx->base + CSI2RX_DEVICE_CFG_REG);
321 	clk_disable_unprepare(csi2rx->p_clk);
322 
323 	csi2rx->max_lanes = dev_cfg & 7;
324 	if (csi2rx->max_lanes > CSI2RX_LANES_MAX) {
325 		dev_err(&pdev->dev, "Invalid number of lanes: %u\n",
326 			csi2rx->max_lanes);
327 		return -EINVAL;
328 	}
329 
330 	csi2rx->max_streams = (dev_cfg >> 4) & 7;
331 	if (csi2rx->max_streams > CSI2RX_STREAMS_MAX) {
332 		dev_err(&pdev->dev, "Invalid number of streams: %u\n",
333 			csi2rx->max_streams);
334 		return -EINVAL;
335 	}
336 
337 	csi2rx->has_internal_dphy = dev_cfg & BIT(3) ? true : false;
338 
339 	/*
340 	 * FIXME: Once we'll have internal D-PHY support, the check
341 	 * will need to be removed.
342 	 */
343 	if (csi2rx->has_internal_dphy) {
344 		dev_err(&pdev->dev, "Internal D-PHY not supported yet\n");
345 		return -EINVAL;
346 	}
347 
348 	for (i = 0; i < csi2rx->max_streams; i++) {
349 		char clk_name[16];
350 
351 		snprintf(clk_name, sizeof(clk_name), "pixel_if%u_clk", i);
352 		csi2rx->pixel_clk[i] = devm_clk_get(&pdev->dev, clk_name);
353 		if (IS_ERR(csi2rx->pixel_clk[i])) {
354 			dev_err(&pdev->dev, "Couldn't get clock %s\n", clk_name);
355 			return PTR_ERR(csi2rx->pixel_clk[i]);
356 		}
357 	}
358 
359 	return 0;
360 }
361 
362 static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx)
363 {
364 	struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 };
365 	struct fwnode_handle *fwh;
366 	struct device_node *ep;
367 	int ret;
368 
369 	ep = of_graph_get_endpoint_by_regs(csi2rx->dev->of_node, 0, 0);
370 	if (!ep)
371 		return -EINVAL;
372 
373 	fwh = of_fwnode_handle(ep);
374 	ret = v4l2_fwnode_endpoint_parse(fwh, &v4l2_ep);
375 	if (ret) {
376 		dev_err(csi2rx->dev, "Could not parse v4l2 endpoint\n");
377 		of_node_put(ep);
378 		return ret;
379 	}
380 
381 	if (v4l2_ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
382 		dev_err(csi2rx->dev, "Unsupported media bus type: 0x%x\n",
383 			v4l2_ep.bus_type);
384 		of_node_put(ep);
385 		return -EINVAL;
386 	}
387 
388 	memcpy(csi2rx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
389 	       sizeof(csi2rx->lanes));
390 	csi2rx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
391 	if (csi2rx->num_lanes > csi2rx->max_lanes) {
392 		dev_err(csi2rx->dev, "Unsupported number of data-lanes: %d\n",
393 			csi2rx->num_lanes);
394 		of_node_put(ep);
395 		return -EINVAL;
396 	}
397 
398 	csi2rx->asd.match.fwnode = fwnode_graph_get_remote_port_parent(fwh);
399 	csi2rx->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
400 	of_node_put(ep);
401 
402 	v4l2_async_notifier_init(&csi2rx->notifier);
403 
404 	ret = v4l2_async_notifier_add_subdev(&csi2rx->notifier, &csi2rx->asd);
405 	if (ret) {
406 		fwnode_handle_put(csi2rx->asd.match.fwnode);
407 		return ret;
408 	}
409 
410 	csi2rx->notifier.ops = &csi2rx_notifier_ops;
411 
412 	ret = v4l2_async_subdev_notifier_register(&csi2rx->subdev,
413 						  &csi2rx->notifier);
414 	if (ret)
415 		v4l2_async_notifier_cleanup(&csi2rx->notifier);
416 
417 	return ret;
418 }
419 
420 static int csi2rx_probe(struct platform_device *pdev)
421 {
422 	struct csi2rx_priv *csi2rx;
423 	unsigned int i;
424 	int ret;
425 
426 	csi2rx = kzalloc(sizeof(*csi2rx), GFP_KERNEL);
427 	if (!csi2rx)
428 		return -ENOMEM;
429 	platform_set_drvdata(pdev, csi2rx);
430 	csi2rx->dev = &pdev->dev;
431 	mutex_init(&csi2rx->lock);
432 
433 	ret = csi2rx_get_resources(csi2rx, pdev);
434 	if (ret)
435 		goto err_free_priv;
436 
437 	ret = csi2rx_parse_dt(csi2rx);
438 	if (ret)
439 		goto err_free_priv;
440 
441 	csi2rx->subdev.owner = THIS_MODULE;
442 	csi2rx->subdev.dev = &pdev->dev;
443 	v4l2_subdev_init(&csi2rx->subdev, &csi2rx_subdev_ops);
444 	v4l2_set_subdevdata(&csi2rx->subdev, &pdev->dev);
445 	snprintf(csi2rx->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.%s",
446 		 KBUILD_MODNAME, dev_name(&pdev->dev));
447 
448 	/* Create our media pads */
449 	csi2rx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
450 	csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
451 	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++)
452 		csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE;
453 
454 	ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX,
455 				     csi2rx->pads);
456 	if (ret)
457 		goto err_cleanup;
458 
459 	ret = v4l2_async_register_subdev(&csi2rx->subdev);
460 	if (ret < 0)
461 		goto err_cleanup;
462 
463 	dev_info(&pdev->dev,
464 		 "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n",
465 		 csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams,
466 		 csi2rx->has_internal_dphy ? "internal" : "no");
467 
468 	return 0;
469 
470 err_cleanup:
471 	v4l2_async_notifier_cleanup(&csi2rx->notifier);
472 err_free_priv:
473 	kfree(csi2rx);
474 	return ret;
475 }
476 
477 static int csi2rx_remove(struct platform_device *pdev)
478 {
479 	struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev);
480 
481 	v4l2_async_unregister_subdev(&csi2rx->subdev);
482 	kfree(csi2rx);
483 
484 	return 0;
485 }
486 
487 static const struct of_device_id csi2rx_of_table[] = {
488 	{ .compatible = "cdns,csi2rx" },
489 	{ },
490 };
491 MODULE_DEVICE_TABLE(of, csi2rx_of_table);
492 
493 static struct platform_driver csi2rx_driver = {
494 	.probe	= csi2rx_probe,
495 	.remove	= csi2rx_remove,
496 
497 	.driver	= {
498 		.name		= "cdns-csi2rx",
499 		.of_match_table	= csi2rx_of_table,
500 	},
501 };
502 module_platform_driver(csi2rx_driver);
503 MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>");
504 MODULE_DESCRIPTION("Cadence CSI2-RX controller");
505 MODULE_LICENSE("GPL");
506