1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * MIPI CSI-2 Receiver Subdev for Freescale i.MX6 SOC.
4 *
5 * Copyright (c) 2012-2017 Mentor Graphics Inc.
6 */
7 #include <linux/clk.h>
8 #include <linux/interrupt.h>
9 #include <linux/io.h>
10 #include <linux/iopoll.h>
11 #include <linux/irq.h>
12 #include <linux/module.h>
13 #include <linux/of_graph.h>
14 #include <linux/platform_device.h>
15 #include <media/v4l2-device.h>
16 #include <media/v4l2-fwnode.h>
17 #include <media/v4l2-mc.h>
18 #include <media/v4l2-subdev.h>
19 #include "imx-media.h"
20
21 /*
22 * there must be 5 pads: 1 input pad from sensor, and
23 * the 4 virtual channel output pads
24 */
25 #define CSI2_SINK_PAD 0
26 #define CSI2_NUM_SINK_PADS 1
27 #define CSI2_NUM_SRC_PADS 4
28 #define CSI2_NUM_PADS 5
29
30 /*
31 * The default maximum bit-rate per lane in Mbps, if the
32 * source subdev does not provide V4L2_CID_LINK_FREQ.
33 */
34 #define CSI2_DEFAULT_MAX_MBPS 849
35
36 struct csi2_dev {
37 struct device *dev;
38 struct v4l2_subdev sd;
39 struct v4l2_async_notifier notifier;
40 struct media_pad pad[CSI2_NUM_PADS];
41 struct clk *dphy_clk;
42 struct clk *pllref_clk;
43 struct clk *pix_clk; /* what is this? */
44 void __iomem *base;
45
46 struct v4l2_subdev *remote;
47 unsigned int remote_pad;
48 unsigned short data_lanes;
49
50 /* lock to protect all members below */
51 struct mutex lock;
52
53 struct v4l2_mbus_framefmt format_mbus;
54
55 int stream_count;
56 struct v4l2_subdev *src_sd;
57 bool sink_linked[CSI2_NUM_SRC_PADS];
58 };
59
60 #define DEVICE_NAME "imx6-mipi-csi2"
61
62 /* Register offsets */
63 #define CSI2_VERSION 0x000
64 #define CSI2_N_LANES 0x004
65 #define CSI2_PHY_SHUTDOWNZ 0x008
66 #define CSI2_DPHY_RSTZ 0x00c
67 #define CSI2_RESETN 0x010
68 #define CSI2_PHY_STATE 0x014
69 #define PHY_STOPSTATEDATA_BIT 4
70 #define PHY_STOPSTATEDATA(n) BIT(PHY_STOPSTATEDATA_BIT + (n))
71 #define PHY_RXCLKACTIVEHS BIT(8)
72 #define PHY_RXULPSCLKNOT BIT(9)
73 #define PHY_STOPSTATECLK BIT(10)
74 #define CSI2_DATA_IDS_1 0x018
75 #define CSI2_DATA_IDS_2 0x01c
76 #define CSI2_ERR1 0x020
77 #define CSI2_ERR2 0x024
78 #define CSI2_MSK1 0x028
79 #define CSI2_MSK2 0x02c
80 #define CSI2_PHY_TST_CTRL0 0x030
81 #define PHY_TESTCLR BIT(0)
82 #define PHY_TESTCLK BIT(1)
83 #define CSI2_PHY_TST_CTRL1 0x034
84 #define PHY_TESTEN BIT(16)
85 /*
86 * i.MX CSI2IPU Gasket registers follow. The CSI2IPU gasket is
87 * not part of the MIPI CSI-2 core, but its registers fall in the
88 * same register map range.
89 */
90 #define CSI2IPU_GASKET 0xf00
91 #define CSI2IPU_YUV422_YUYV BIT(2)
92
sd_to_dev(struct v4l2_subdev * sdev)93 static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev)
94 {
95 return container_of(sdev, struct csi2_dev, sd);
96 }
97
notifier_to_dev(struct v4l2_async_notifier * n)98 static inline struct csi2_dev *notifier_to_dev(struct v4l2_async_notifier *n)
99 {
100 return container_of(n, struct csi2_dev, notifier);
101 }
102
103 /*
104 * The required sequence of MIPI CSI-2 startup as specified in the i.MX6
105 * reference manual is as follows:
106 *
107 * 1. Deassert presetn signal (global reset).
108 * It's not clear what this "global reset" signal is (maybe APB
109 * global reset), but in any case this step would be probably
110 * be carried out during driver load in csi2_probe().
111 *
112 * 2. Configure MIPI Camera Sensor to put all Tx lanes in LP-11 state.
113 * This must be carried out by the MIPI sensor's s_power(ON) subdev
114 * op.
115 *
116 * 3. D-PHY initialization.
117 * 4. CSI2 Controller programming (Set N_LANES, deassert PHY_SHUTDOWNZ,
118 * deassert PHY_RSTZ, deassert CSI2_RESETN).
119 * 5. Read the PHY status register (PHY_STATE) to confirm that all data and
120 * clock lanes of the D-PHY are in LP-11 state.
121 * 6. Configure the MIPI Camera Sensor to start transmitting a clock on the
122 * D-PHY clock lane.
123 * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE)
124 * to confirm that the D-PHY is receiving a clock on the D-PHY clock lane.
125 *
126 * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step
127 * 6 is accomplished by calling the source subdev's s_stream(ON) between
128 * steps 5 and 7.
129 */
130
csi2_enable(struct csi2_dev * csi2,bool enable)131 static void csi2_enable(struct csi2_dev *csi2, bool enable)
132 {
133 if (enable) {
134 writel(0x1, csi2->base + CSI2_PHY_SHUTDOWNZ);
135 writel(0x1, csi2->base + CSI2_DPHY_RSTZ);
136 writel(0x1, csi2->base + CSI2_RESETN);
137 } else {
138 writel(0x0, csi2->base + CSI2_PHY_SHUTDOWNZ);
139 writel(0x0, csi2->base + CSI2_DPHY_RSTZ);
140 writel(0x0, csi2->base + CSI2_RESETN);
141 }
142 }
143
csi2_set_lanes(struct csi2_dev * csi2,unsigned int lanes)144 static void csi2_set_lanes(struct csi2_dev *csi2, unsigned int lanes)
145 {
146 writel(lanes - 1, csi2->base + CSI2_N_LANES);
147 }
148
dw_mipi_csi2_phy_write(struct csi2_dev * csi2,u32 test_code,u32 test_data)149 static void dw_mipi_csi2_phy_write(struct csi2_dev *csi2,
150 u32 test_code, u32 test_data)
151 {
152 /* Clear PHY test interface */
153 writel(PHY_TESTCLR, csi2->base + CSI2_PHY_TST_CTRL0);
154 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL1);
155 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
156
157 /* Raise test interface strobe signal */
158 writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
159
160 /* Configure address write on falling edge and lower strobe signal */
161 writel(PHY_TESTEN | test_code, csi2->base + CSI2_PHY_TST_CTRL1);
162 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
163
164 /* Configure data write on rising edge and raise strobe signal */
165 writel(test_data, csi2->base + CSI2_PHY_TST_CTRL1);
166 writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
167
168 /* Clear strobe signal */
169 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
170 }
171
172 /*
173 * This table is based on the table documented at
174 * https://community.nxp.com/docs/DOC-94312. It assumes
175 * a 27MHz D-PHY pll reference clock.
176 */
177 static const struct {
178 u32 max_mbps;
179 u32 hsfreqrange_sel;
180 } hsfreq_map[] = {
181 { 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02},
182 {140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24},
183 {200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46},
184 {270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a},
185 {400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e},
186 {600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12},
187 {800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54},
188 {1000, 0x74},
189 };
190
max_mbps_to_hsfreqrange_sel(u32 max_mbps)191 static int max_mbps_to_hsfreqrange_sel(u32 max_mbps)
192 {
193 int i;
194
195 for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++)
196 if (hsfreq_map[i].max_mbps > max_mbps)
197 return hsfreq_map[i].hsfreqrange_sel;
198
199 return -EINVAL;
200 }
201
csi2_dphy_init(struct csi2_dev * csi2)202 static int csi2_dphy_init(struct csi2_dev *csi2)
203 {
204 struct v4l2_ctrl *ctrl;
205 u32 mbps_per_lane;
206 int sel;
207
208 ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler,
209 V4L2_CID_LINK_FREQ);
210 if (!ctrl)
211 mbps_per_lane = CSI2_DEFAULT_MAX_MBPS;
212 else
213 mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val],
214 USEC_PER_SEC);
215
216 sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane);
217 if (sel < 0)
218 return sel;
219
220 dw_mipi_csi2_phy_write(csi2, 0x44, sel);
221
222 return 0;
223 }
224
225 /*
226 * Waits for ultra-low-power state on D-PHY clock lane. This is currently
227 * unused and may not be needed at all, but keep around just in case.
228 */
csi2_dphy_wait_ulp(struct csi2_dev * csi2)229 static int __maybe_unused csi2_dphy_wait_ulp(struct csi2_dev *csi2)
230 {
231 u32 reg;
232 int ret;
233
234 /* wait for ULP on clock lane */
235 ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
236 !(reg & PHY_RXULPSCLKNOT), 0, 500000);
237 if (ret) {
238 v4l2_err(&csi2->sd, "ULP timeout, phy_state = 0x%08x\n", reg);
239 return ret;
240 }
241
242 /* wait until no errors on bus */
243 ret = readl_poll_timeout(csi2->base + CSI2_ERR1, reg,
244 reg == 0x0, 0, 500000);
245 if (ret) {
246 v4l2_err(&csi2->sd, "stable bus timeout, err1 = 0x%08x\n", reg);
247 return ret;
248 }
249
250 return 0;
251 }
252
253 /* Waits for low-power LP-11 state on data and clock lanes. */
csi2_dphy_wait_stopstate(struct csi2_dev * csi2,unsigned int lanes)254 static void csi2_dphy_wait_stopstate(struct csi2_dev *csi2, unsigned int lanes)
255 {
256 u32 mask, reg;
257 int ret;
258
259 mask = PHY_STOPSTATECLK | (((1 << lanes) - 1) << PHY_STOPSTATEDATA_BIT);
260
261 ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
262 (reg & mask) == mask, 0, 500000);
263 if (ret) {
264 v4l2_warn(&csi2->sd, "LP-11 wait timeout, likely a sensor driver bug, expect capture failures.\n");
265 v4l2_warn(&csi2->sd, "phy_state = 0x%08x\n", reg);
266 }
267 }
268
269 /* Wait for active clock on the clock lane. */
csi2_dphy_wait_clock_lane(struct csi2_dev * csi2)270 static int csi2_dphy_wait_clock_lane(struct csi2_dev *csi2)
271 {
272 u32 reg;
273 int ret;
274
275 ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
276 (reg & PHY_RXCLKACTIVEHS), 0, 500000);
277 if (ret) {
278 v4l2_err(&csi2->sd, "clock lane timeout, phy_state = 0x%08x\n",
279 reg);
280 return ret;
281 }
282
283 return 0;
284 }
285
286 /* Setup the i.MX CSI2IPU Gasket */
csi2ipu_gasket_init(struct csi2_dev * csi2)287 static void csi2ipu_gasket_init(struct csi2_dev *csi2)
288 {
289 u32 reg = 0;
290
291 switch (csi2->format_mbus.code) {
292 case MEDIA_BUS_FMT_YUYV8_2X8:
293 case MEDIA_BUS_FMT_YUYV8_1X16:
294 reg = CSI2IPU_YUV422_YUYV;
295 break;
296 default:
297 break;
298 }
299
300 writel(reg, csi2->base + CSI2IPU_GASKET);
301 }
302
csi2_get_active_lanes(struct csi2_dev * csi2,unsigned int * lanes)303 static int csi2_get_active_lanes(struct csi2_dev *csi2, unsigned int *lanes)
304 {
305 struct v4l2_mbus_config mbus_config = { 0 };
306 unsigned int num_lanes = UINT_MAX;
307 int ret;
308
309 *lanes = csi2->data_lanes;
310
311 ret = v4l2_subdev_call(csi2->remote, pad, get_mbus_config,
312 csi2->remote_pad, &mbus_config);
313 if (ret == -ENOIOCTLCMD) {
314 dev_dbg(csi2->dev, "No remote mbus configuration available\n");
315 return 0;
316 }
317
318 if (ret) {
319 dev_err(csi2->dev, "Failed to get remote mbus configuration\n");
320 return ret;
321 }
322
323 if (mbus_config.type != V4L2_MBUS_CSI2_DPHY) {
324 dev_err(csi2->dev, "Unsupported media bus type %u\n",
325 mbus_config.type);
326 return -EINVAL;
327 }
328
329 switch (mbus_config.flags & V4L2_MBUS_CSI2_LANES) {
330 case V4L2_MBUS_CSI2_1_LANE:
331 num_lanes = 1;
332 break;
333 case V4L2_MBUS_CSI2_2_LANE:
334 num_lanes = 2;
335 break;
336 case V4L2_MBUS_CSI2_3_LANE:
337 num_lanes = 3;
338 break;
339 case V4L2_MBUS_CSI2_4_LANE:
340 num_lanes = 4;
341 break;
342 default:
343 num_lanes = csi2->data_lanes;
344 break;
345 }
346
347 if (num_lanes > csi2->data_lanes) {
348 dev_err(csi2->dev,
349 "Unsupported mbus config: too many data lanes %u\n",
350 num_lanes);
351 return -EINVAL;
352 }
353
354 *lanes = num_lanes;
355
356 return 0;
357 }
358
csi2_start(struct csi2_dev * csi2)359 static int csi2_start(struct csi2_dev *csi2)
360 {
361 unsigned int lanes;
362 int ret;
363
364 ret = clk_prepare_enable(csi2->pix_clk);
365 if (ret)
366 return ret;
367
368 /* setup the gasket */
369 csi2ipu_gasket_init(csi2);
370
371 /* Step 3 */
372 ret = csi2_dphy_init(csi2);
373 if (ret)
374 goto err_disable_clk;
375
376 ret = csi2_get_active_lanes(csi2, &lanes);
377 if (ret)
378 goto err_disable_clk;
379
380 /* Step 4 */
381 csi2_set_lanes(csi2, lanes);
382 csi2_enable(csi2, true);
383
384 /* Step 5 */
385 csi2_dphy_wait_stopstate(csi2, lanes);
386
387 /* Step 6 */
388 ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1);
389 ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
390 if (ret)
391 goto err_assert_reset;
392
393 /* Step 7 */
394 ret = csi2_dphy_wait_clock_lane(csi2);
395 if (ret)
396 goto err_stop_upstream;
397
398 return 0;
399
400 err_stop_upstream:
401 v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
402 err_assert_reset:
403 csi2_enable(csi2, false);
404 err_disable_clk:
405 clk_disable_unprepare(csi2->pix_clk);
406 return ret;
407 }
408
csi2_stop(struct csi2_dev * csi2)409 static void csi2_stop(struct csi2_dev *csi2)
410 {
411 /* stop upstream */
412 v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
413
414 csi2_enable(csi2, false);
415 clk_disable_unprepare(csi2->pix_clk);
416 }
417
418 /*
419 * V4L2 subdev operations.
420 */
421
csi2_s_stream(struct v4l2_subdev * sd,int enable)422 static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
423 {
424 struct csi2_dev *csi2 = sd_to_dev(sd);
425 int i, ret = 0;
426
427 mutex_lock(&csi2->lock);
428
429 if (!csi2->src_sd) {
430 ret = -EPIPE;
431 goto out;
432 }
433
434 for (i = 0; i < CSI2_NUM_SRC_PADS; i++) {
435 if (csi2->sink_linked[i])
436 break;
437 }
438 if (i >= CSI2_NUM_SRC_PADS) {
439 ret = -EPIPE;
440 goto out;
441 }
442
443 /*
444 * enable/disable streaming only if stream_count is
445 * going from 0 to 1 / 1 to 0.
446 */
447 if (csi2->stream_count != !enable)
448 goto update_count;
449
450 dev_dbg(csi2->dev, "stream %s\n", enable ? "ON" : "OFF");
451 if (enable)
452 ret = csi2_start(csi2);
453 else
454 csi2_stop(csi2);
455 if (ret)
456 goto out;
457
458 update_count:
459 csi2->stream_count += enable ? 1 : -1;
460 if (csi2->stream_count < 0)
461 csi2->stream_count = 0;
462 out:
463 mutex_unlock(&csi2->lock);
464 return ret;
465 }
466
csi2_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)467 static int csi2_link_setup(struct media_entity *entity,
468 const struct media_pad *local,
469 const struct media_pad *remote, u32 flags)
470 {
471 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
472 struct csi2_dev *csi2 = sd_to_dev(sd);
473 struct v4l2_subdev *remote_sd;
474 int ret = 0;
475
476 dev_dbg(csi2->dev, "link setup %s -> %s", remote->entity->name,
477 local->entity->name);
478
479 remote_sd = media_entity_to_v4l2_subdev(remote->entity);
480
481 mutex_lock(&csi2->lock);
482
483 if (local->flags & MEDIA_PAD_FL_SOURCE) {
484 if (flags & MEDIA_LNK_FL_ENABLED) {
485 if (csi2->sink_linked[local->index - 1]) {
486 ret = -EBUSY;
487 goto out;
488 }
489 csi2->sink_linked[local->index - 1] = true;
490 } else {
491 csi2->sink_linked[local->index - 1] = false;
492 }
493 } else {
494 if (flags & MEDIA_LNK_FL_ENABLED) {
495 if (csi2->src_sd) {
496 ret = -EBUSY;
497 goto out;
498 }
499 csi2->src_sd = remote_sd;
500 } else {
501 csi2->src_sd = NULL;
502 }
503 }
504
505 out:
506 mutex_unlock(&csi2->lock);
507 return ret;
508 }
509
510 static struct v4l2_mbus_framefmt *
__csi2_get_fmt(struct csi2_dev * csi2,struct v4l2_subdev_state * sd_state,unsigned int pad,enum v4l2_subdev_format_whence which)511 __csi2_get_fmt(struct csi2_dev *csi2, struct v4l2_subdev_state *sd_state,
512 unsigned int pad, enum v4l2_subdev_format_whence which)
513 {
514 if (which == V4L2_SUBDEV_FORMAT_TRY)
515 return v4l2_subdev_get_try_format(&csi2->sd, sd_state, pad);
516 else
517 return &csi2->format_mbus;
518 }
519
csi2_get_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat)520 static int csi2_get_fmt(struct v4l2_subdev *sd,
521 struct v4l2_subdev_state *sd_state,
522 struct v4l2_subdev_format *sdformat)
523 {
524 struct csi2_dev *csi2 = sd_to_dev(sd);
525 struct v4l2_mbus_framefmt *fmt;
526
527 mutex_lock(&csi2->lock);
528
529 fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
530
531 sdformat->format = *fmt;
532
533 mutex_unlock(&csi2->lock);
534
535 return 0;
536 }
537
csi2_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat)538 static int csi2_set_fmt(struct v4l2_subdev *sd,
539 struct v4l2_subdev_state *sd_state,
540 struct v4l2_subdev_format *sdformat)
541 {
542 struct csi2_dev *csi2 = sd_to_dev(sd);
543 struct v4l2_mbus_framefmt *fmt;
544 int ret = 0;
545
546 if (sdformat->pad >= CSI2_NUM_PADS)
547 return -EINVAL;
548
549 mutex_lock(&csi2->lock);
550
551 if (csi2->stream_count > 0) {
552 ret = -EBUSY;
553 goto out;
554 }
555
556 /* Output pads mirror active input pad, no limits on input pads */
557 if (sdformat->pad != CSI2_SINK_PAD)
558 sdformat->format = csi2->format_mbus;
559
560 fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
561
562 *fmt = sdformat->format;
563 out:
564 mutex_unlock(&csi2->lock);
565 return ret;
566 }
567
csi2_registered(struct v4l2_subdev * sd)568 static int csi2_registered(struct v4l2_subdev *sd)
569 {
570 struct csi2_dev *csi2 = sd_to_dev(sd);
571
572 /* set a default mbus format */
573 return imx_media_init_mbus_fmt(&csi2->format_mbus,
574 IMX_MEDIA_DEF_PIX_WIDTH,
575 IMX_MEDIA_DEF_PIX_HEIGHT, 0,
576 V4L2_FIELD_NONE, NULL);
577 }
578
579 static const struct media_entity_operations csi2_entity_ops = {
580 .link_setup = csi2_link_setup,
581 .link_validate = v4l2_subdev_link_validate,
582 .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
583 };
584
585 static const struct v4l2_subdev_video_ops csi2_video_ops = {
586 .s_stream = csi2_s_stream,
587 };
588
589 static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
590 .init_cfg = imx_media_init_cfg,
591 .get_fmt = csi2_get_fmt,
592 .set_fmt = csi2_set_fmt,
593 };
594
595 static const struct v4l2_subdev_ops csi2_subdev_ops = {
596 .video = &csi2_video_ops,
597 .pad = &csi2_pad_ops,
598 };
599
600 static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
601 .registered = csi2_registered,
602 };
603
csi2_notify_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)604 static int csi2_notify_bound(struct v4l2_async_notifier *notifier,
605 struct v4l2_subdev *sd,
606 struct v4l2_async_subdev *asd)
607 {
608 struct csi2_dev *csi2 = notifier_to_dev(notifier);
609 struct media_pad *sink = &csi2->sd.entity.pads[CSI2_SINK_PAD];
610 int pad;
611
612 pad = media_entity_get_fwnode_pad(&sd->entity, asd->match.fwnode,
613 MEDIA_PAD_FL_SOURCE);
614 if (pad < 0) {
615 dev_err(csi2->dev, "Failed to find pad for %s\n", sd->name);
616 return pad;
617 }
618
619 csi2->remote = sd;
620 csi2->remote_pad = pad;
621
622 dev_dbg(csi2->dev, "Bound %s pad: %d\n", sd->name, pad);
623
624 return v4l2_create_fwnode_links_to_pad(sd, sink, 0);
625 }
626
csi2_notify_unbind(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)627 static void csi2_notify_unbind(struct v4l2_async_notifier *notifier,
628 struct v4l2_subdev *sd,
629 struct v4l2_async_subdev *asd)
630 {
631 struct csi2_dev *csi2 = notifier_to_dev(notifier);
632
633 csi2->remote = NULL;
634 }
635
636 static const struct v4l2_async_notifier_operations csi2_notify_ops = {
637 .bound = csi2_notify_bound,
638 .unbind = csi2_notify_unbind,
639 };
640
csi2_async_register(struct csi2_dev * csi2)641 static int csi2_async_register(struct csi2_dev *csi2)
642 {
643 struct v4l2_fwnode_endpoint vep = {
644 .bus_type = V4L2_MBUS_CSI2_DPHY,
645 };
646 struct v4l2_async_subdev *asd;
647 struct fwnode_handle *ep;
648 int ret;
649
650 v4l2_async_notifier_init(&csi2->notifier);
651
652 ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi2->dev), 0, 0,
653 FWNODE_GRAPH_ENDPOINT_NEXT);
654 if (!ep)
655 return -ENOTCONN;
656
657 ret = v4l2_fwnode_endpoint_parse(ep, &vep);
658 if (ret)
659 goto err_parse;
660
661 csi2->data_lanes = vep.bus.mipi_csi2.num_data_lanes;
662
663 dev_dbg(csi2->dev, "data lanes: %d\n", vep.bus.mipi_csi2.num_data_lanes);
664 dev_dbg(csi2->dev, "flags: 0x%08x\n", vep.bus.mipi_csi2.flags);
665
666 asd = v4l2_async_notifier_add_fwnode_remote_subdev(
667 &csi2->notifier, ep, struct v4l2_async_subdev);
668 fwnode_handle_put(ep);
669
670 if (IS_ERR(asd))
671 return PTR_ERR(asd);
672
673 csi2->notifier.ops = &csi2_notify_ops;
674
675 ret = v4l2_async_subdev_notifier_register(&csi2->sd,
676 &csi2->notifier);
677 if (ret)
678 return ret;
679
680 return v4l2_async_register_subdev(&csi2->sd);
681
682 err_parse:
683 fwnode_handle_put(ep);
684 return ret;
685 }
686
csi2_probe(struct platform_device * pdev)687 static int csi2_probe(struct platform_device *pdev)
688 {
689 struct csi2_dev *csi2;
690 struct resource *res;
691 int i, ret;
692
693 csi2 = devm_kzalloc(&pdev->dev, sizeof(*csi2), GFP_KERNEL);
694 if (!csi2)
695 return -ENOMEM;
696
697 csi2->dev = &pdev->dev;
698
699 v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
700 v4l2_set_subdevdata(&csi2->sd, &pdev->dev);
701 csi2->sd.internal_ops = &csi2_internal_ops;
702 csi2->sd.entity.ops = &csi2_entity_ops;
703 csi2->sd.dev = &pdev->dev;
704 csi2->sd.owner = THIS_MODULE;
705 csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
706 strscpy(csi2->sd.name, DEVICE_NAME, sizeof(csi2->sd.name));
707 csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
708 csi2->sd.grp_id = IMX_MEDIA_GRP_ID_CSI2;
709
710 for (i = 0; i < CSI2_NUM_PADS; i++) {
711 csi2->pad[i].flags = (i == CSI2_SINK_PAD) ?
712 MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
713 }
714
715 ret = media_entity_pads_init(&csi2->sd.entity, CSI2_NUM_PADS,
716 csi2->pad);
717 if (ret)
718 return ret;
719
720 csi2->pllref_clk = devm_clk_get(&pdev->dev, "ref");
721 if (IS_ERR(csi2->pllref_clk)) {
722 v4l2_err(&csi2->sd, "failed to get pll reference clock\n");
723 return PTR_ERR(csi2->pllref_clk);
724 }
725
726 csi2->dphy_clk = devm_clk_get(&pdev->dev, "dphy");
727 if (IS_ERR(csi2->dphy_clk)) {
728 v4l2_err(&csi2->sd, "failed to get dphy clock\n");
729 return PTR_ERR(csi2->dphy_clk);
730 }
731
732 csi2->pix_clk = devm_clk_get(&pdev->dev, "pix");
733 if (IS_ERR(csi2->pix_clk)) {
734 v4l2_err(&csi2->sd, "failed to get pixel clock\n");
735 return PTR_ERR(csi2->pix_clk);
736 }
737
738 res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
739 if (!res) {
740 v4l2_err(&csi2->sd, "failed to get platform resources\n");
741 return -ENODEV;
742 }
743
744 csi2->base = devm_ioremap(&pdev->dev, res->start, PAGE_SIZE);
745 if (!csi2->base)
746 return -ENOMEM;
747
748 mutex_init(&csi2->lock);
749
750 ret = clk_prepare_enable(csi2->pllref_clk);
751 if (ret) {
752 v4l2_err(&csi2->sd, "failed to enable pllref_clk\n");
753 goto rmmutex;
754 }
755
756 ret = clk_prepare_enable(csi2->dphy_clk);
757 if (ret) {
758 v4l2_err(&csi2->sd, "failed to enable dphy_clk\n");
759 goto pllref_off;
760 }
761
762 platform_set_drvdata(pdev, &csi2->sd);
763
764 ret = csi2_async_register(csi2);
765 if (ret)
766 goto clean_notifier;
767
768 return 0;
769
770 clean_notifier:
771 v4l2_async_notifier_unregister(&csi2->notifier);
772 v4l2_async_notifier_cleanup(&csi2->notifier);
773 clk_disable_unprepare(csi2->dphy_clk);
774 pllref_off:
775 clk_disable_unprepare(csi2->pllref_clk);
776 rmmutex:
777 mutex_destroy(&csi2->lock);
778 return ret;
779 }
780
csi2_remove(struct platform_device * pdev)781 static int csi2_remove(struct platform_device *pdev)
782 {
783 struct v4l2_subdev *sd = platform_get_drvdata(pdev);
784 struct csi2_dev *csi2 = sd_to_dev(sd);
785
786 v4l2_async_notifier_unregister(&csi2->notifier);
787 v4l2_async_notifier_cleanup(&csi2->notifier);
788 v4l2_async_unregister_subdev(sd);
789 clk_disable_unprepare(csi2->dphy_clk);
790 clk_disable_unprepare(csi2->pllref_clk);
791 mutex_destroy(&csi2->lock);
792 media_entity_cleanup(&sd->entity);
793
794 return 0;
795 }
796
797 static const struct of_device_id csi2_dt_ids[] = {
798 { .compatible = "fsl,imx6-mipi-csi2", },
799 { /* sentinel */ }
800 };
801 MODULE_DEVICE_TABLE(of, csi2_dt_ids);
802
803 static struct platform_driver csi2_driver = {
804 .driver = {
805 .name = DEVICE_NAME,
806 .of_match_table = csi2_dt_ids,
807 },
808 .probe = csi2_probe,
809 .remove = csi2_remove,
810 };
811
812 module_platform_driver(csi2_driver);
813
814 MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver");
815 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
816 MODULE_LICENSE("GPL");
817
