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