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

source code of linux/drivers/staging/media/imx/imx6-mipi-csi2.c