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
v4l2_subdev_to_csi2rx(struct v4l2_subdev * subdev)89 struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev)
90 {
91 return container_of(subdev, struct csi2rx_priv, subdev);
92 }
93
csi2rx_reset(struct csi2rx_priv * csi2rx)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
csi2rx_start(struct csi2rx_priv * csi2rx)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
csi2rx_stop(struct csi2rx_priv * csi2rx)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
csi2rx_s_stream(struct v4l2_subdev * subdev,int enable)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
csi2rx_async_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * s_subdev,struct v4l2_async_subdev * asd)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
csi2rx_get_resources(struct csi2rx_priv * csi2rx,struct platform_device * pdev)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
csi2rx_parse_dt(struct csi2rx_priv * csi2rx)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 v4l2_async_notifier_cleanup(&csi2rx->notifier);
412 return PTR_ERR(asd);
413 }
414
415 csi2rx->notifier.ops = &csi2rx_notifier_ops;
416
417 ret = v4l2_async_subdev_notifier_register(&csi2rx->subdev,
418 &csi2rx->notifier);
419 if (ret)
420 v4l2_async_notifier_cleanup(&csi2rx->notifier);
421
422 return ret;
423 }
424
csi2rx_probe(struct platform_device * pdev)425 static int csi2rx_probe(struct platform_device *pdev)
426 {
427 struct csi2rx_priv *csi2rx;
428 unsigned int i;
429 int ret;
430
431 csi2rx = kzalloc(sizeof(*csi2rx), GFP_KERNEL);
432 if (!csi2rx)
433 return -ENOMEM;
434 platform_set_drvdata(pdev, csi2rx);
435 csi2rx->dev = &pdev->dev;
436 mutex_init(&csi2rx->lock);
437
438 ret = csi2rx_get_resources(csi2rx, pdev);
439 if (ret)
440 goto err_free_priv;
441
442 ret = csi2rx_parse_dt(csi2rx);
443 if (ret)
444 goto err_free_priv;
445
446 csi2rx->subdev.owner = THIS_MODULE;
447 csi2rx->subdev.dev = &pdev->dev;
448 v4l2_subdev_init(&csi2rx->subdev, &csi2rx_subdev_ops);
449 v4l2_set_subdevdata(&csi2rx->subdev, &pdev->dev);
450 snprintf(csi2rx->subdev.name, V4L2_SUBDEV_NAME_SIZE, "%s.%s",
451 KBUILD_MODNAME, dev_name(&pdev->dev));
452
453 /* Create our media pads */
454 csi2rx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
455 csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
456 for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++)
457 csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE;
458
459 ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX,
460 csi2rx->pads);
461 if (ret)
462 goto err_cleanup;
463
464 ret = v4l2_async_register_subdev(&csi2rx->subdev);
465 if (ret < 0)
466 goto err_cleanup;
467
468 dev_info(&pdev->dev,
469 "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n",
470 csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams,
471 csi2rx->has_internal_dphy ? "internal" : "no");
472
473 return 0;
474
475 err_cleanup:
476 v4l2_async_notifier_unregister(&csi2rx->notifier);
477 v4l2_async_notifier_cleanup(&csi2rx->notifier);
478 err_free_priv:
479 kfree(csi2rx);
480 return ret;
481 }
482
csi2rx_remove(struct platform_device * pdev)483 static int csi2rx_remove(struct platform_device *pdev)
484 {
485 struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev);
486
487 v4l2_async_notifier_unregister(&csi2rx->notifier);
488 v4l2_async_notifier_cleanup(&csi2rx->notifier);
489 v4l2_async_unregister_subdev(&csi2rx->subdev);
490 kfree(csi2rx);
491
492 return 0;
493 }
494
495 static const struct of_device_id csi2rx_of_table[] = {
496 { .compatible = "cdns,csi2rx" },
497 { },
498 };
499 MODULE_DEVICE_TABLE(of, csi2rx_of_table);
500
501 static struct platform_driver csi2rx_driver = {
502 .probe = csi2rx_probe,
503 .remove = csi2rx_remove,
504
505 .driver = {
506 .name = "cdns-csi2rx",
507 .of_match_table = csi2rx_of_table,
508 },
509 };
510 module_platform_driver(csi2rx_driver);
511 MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>");
512 MODULE_DESCRIPTION("Cadence CSI2-RX controller");
513 MODULE_LICENSE("GPL");
514