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 
37 struct 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 
sd_to_dev(struct v4l2_subdev * sdev)94 static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev)
95 {
96 	return container_of(sdev, struct csi2_dev, sd);
97 }
98 
notifier_to_dev(struct v4l2_async_notifier * n)99 static 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 
csi2_enable(struct csi2_dev * csi2,bool enable)132 static void csi2_enable(struct csi2_dev *csi2, bool enable)
133 {
134 	if (enable) {
135 		writel(0x1, csi2->base + CSI2_PHY_SHUTDOWNZ);
136 		writel(0x1, csi2->base + CSI2_DPHY_RSTZ);
137 		writel(0x1, csi2->base + CSI2_RESETN);
138 	} else {
139 		writel(0x0, csi2->base + CSI2_PHY_SHUTDOWNZ);
140 		writel(0x0, csi2->base + CSI2_DPHY_RSTZ);
141 		writel(0x0, csi2->base + CSI2_RESETN);
142 	}
143 }
144 
csi2_set_lanes(struct csi2_dev * csi2,unsigned int lanes)145 static void csi2_set_lanes(struct csi2_dev *csi2, unsigned int lanes)
146 {
147 	writel(lanes - 1, csi2->base + CSI2_N_LANES);
148 }
149 
dw_mipi_csi2_phy_write(struct csi2_dev * csi2,u32 test_code,u32 test_data)150 static 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, csi2->base + CSI2_PHY_TST_CTRL0);
155 	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL1);
156 	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
157 
158 	/* Raise test interface strobe signal */
159 	writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
160 
161 	/* Configure address write on falling edge and lower strobe signal */
162 	writel(PHY_TESTEN | test_code, csi2->base + CSI2_PHY_TST_CTRL1);
163 	writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
164 
165 	/* Configure data write on rising edge and raise strobe signal */
166 	writel(test_data, csi2->base + CSI2_PHY_TST_CTRL1);
167 	writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
168 
169 	/* Clear strobe signal */
170 	writel(0x0, 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  */
178 static 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 
max_mbps_to_hsfreqrange_sel(u32 max_mbps)192 static 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 
csi2_dphy_init(struct csi2_dev * csi2)203 static 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(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(mbps_per_lane);
218 	if (sel < 0)
219 		return sel;
220 
221 	dw_mipi_csi2_phy_write(csi2, 0x44, 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  */
csi2_dphy_wait_ulp(struct csi2_dev * csi2)230 static 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. */
csi2_dphy_wait_stopstate(struct csi2_dev * csi2,unsigned int lanes)255 static 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. */
csi2_dphy_wait_clock_lane(struct csi2_dev * csi2)271 static 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 */
csi2ipu_gasket_init(struct csi2_dev * csi2)288 static 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(reg, csi2->base + CSI2IPU_GASKET);
302 }
303 
csi2_get_active_lanes(struct csi2_dev * csi2,unsigned int * lanes)304 static 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 
csi2_start(struct csi2_dev * csi2)341 static int csi2_start(struct csi2_dev *csi2)
342 {
343 	unsigned int lanes;
344 	int ret;
345 
346 	ret = clk_prepare_enable(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);
359 	if (ret)
360 		goto err_disable_clk;
361 
362 	/* Step 4 */
363 	csi2_set_lanes(csi2, lanes);
364 	csi2_enable(csi2, 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 
386 err_stop_upstream:
387 	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
388 err_stop_lp11:
389 	v4l2_subdev_call(csi2->src_sd, video, post_streamoff);
390 err_assert_reset:
391 	csi2_enable(csi2, false);
392 err_disable_clk:
393 	clk_disable_unprepare(csi2->pix_clk);
394 	return ret;
395 }
396 
csi2_stop(struct csi2_dev * csi2)397 static 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, false);
404 	clk_disable_unprepare(csi2->pix_clk);
405 }
406 
407 /*
408  * V4L2 subdev operations.
409  */
410 
csi2_s_stream(struct v4l2_subdev * sd,int enable)411 static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
412 {
413 	struct csi2_dev *csi2 = sd_to_dev(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 
447 update_count:
448 	csi2->stream_count += enable ? 1 : -1;
449 	if (csi2->stream_count < 0)
450 		csi2->stream_count = 0;
451 out:
452 	mutex_unlock(&csi2->lock);
453 	return ret;
454 }
455 
csi2_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)456 static 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(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 
494 out:
495 	mutex_unlock(&csi2->lock);
496 	return ret;
497 }
498 
499 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)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_get_try_format(&csi2->sd, sd_state, pad);
505 	else
506 		return &csi2->format_mbus;
507 }
508 
csi2_get_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat)509 static 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(sd);
514 	struct v4l2_mbus_framefmt *fmt;
515 
516 	mutex_lock(&csi2->lock);
517 
518 	fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
519 
520 	sdformat->format = *fmt;
521 
522 	mutex_unlock(&csi2->lock);
523 
524 	return 0;
525 }
526 
csi2_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * sdformat)527 static 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(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, sdformat->pad, sdformat->which);
550 
551 	*fmt = sdformat->format;
552 out:
553 	mutex_unlock(&csi2->lock);
554 	return ret;
555 }
556 
csi2_registered(struct v4l2_subdev * sd)557 static int csi2_registered(struct v4l2_subdev *sd)
558 {
559 	struct csi2_dev *csi2 = sd_to_dev(sd);
560 
561 	/* set a default mbus format  */
562 	return imx_media_init_mbus_fmt(&csi2->format_mbus,
563 				      IMX_MEDIA_DEF_PIX_WIDTH,
564 				      IMX_MEDIA_DEF_PIX_HEIGHT, 0,
565 				      V4L2_FIELD_NONE, NULL);
566 }
567 
568 /* --------------- CORE OPS --------------- */
569 
csi2_log_status(struct v4l2_subdev * sd)570 static int csi2_log_status(struct v4l2_subdev *sd)
571 {
572 	struct csi2_dev *csi2 = sd_to_dev(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 
607 static const struct v4l2_subdev_core_ops csi2_core_ops = {
608 	.log_status = csi2_log_status,
609 };
610 
611 static 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 
617 static const struct v4l2_subdev_video_ops csi2_video_ops = {
618 	.s_stream = csi2_s_stream,
619 };
620 
621 static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
622 	.init_cfg = imx_media_init_cfg,
623 	.get_fmt = csi2_get_fmt,
624 	.set_fmt = csi2_set_fmt,
625 };
626 
627 static const struct v4l2_subdev_ops csi2_subdev_ops = {
628 	.core = &csi2_core_ops,
629 	.video = &csi2_video_ops,
630 	.pad = &csi2_pad_ops,
631 };
632 
633 static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
634 	.registered = csi2_registered,
635 };
636 
csi2_notify_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd,struct v4l2_async_connection * asd)637 static 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(notifier);
642 	struct media_pad *sink = &csi2->sd.entity.pads[CSI2_SINK_PAD];
643 	int pad;
644 
645 	pad = media_entity_get_fwnode_pad(&sd->entity, 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(sd, sink, 0);
658 }
659 
csi2_notify_unbind(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd,struct v4l2_async_connection * asd)660 static 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(notifier);
665 
666 	csi2->remote = NULL;
667 }
668 
669 static const struct v4l2_async_notifier_operations csi2_notify_ops = {
670 	.bound = csi2_notify_bound,
671 	.unbind = csi2_notify_unbind,
672 };
673 
csi2_async_register(struct csi2_dev * csi2)674 static 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(&csi2->notifier, &csi2->sd);
684 
685 	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi2->dev), 0, 0,
686 					     FWNODE_GRAPH_ENDPOINT_NEXT);
687 	if (!ep)
688 		return -ENOTCONN;
689 
690 	ret = v4l2_fwnode_endpoint_parse(ep, &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(ep);
702 
703 	if (IS_ERR(asd))
704 		return PTR_ERR(asd);
705 
706 	csi2->notifier.ops = &csi2_notify_ops;
707 
708 	ret = v4l2_async_nf_register(&csi2->notifier);
709 	if (ret)
710 		return ret;
711 
712 	return v4l2_async_register_subdev(&csi2->sd);
713 
714 err_parse:
715 	fwnode_handle_put(ep);
716 	return ret;
717 }
718 
csi2_probe(struct platform_device * pdev)719 static 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(&pdev->dev, sizeof(*csi2), GFP_KERNEL);
726 	if (!csi2)
727 		return -ENOMEM;
728 
729 	csi2->dev = &pdev->dev;
730 
731 	v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
732 	v4l2_set_subdevdata(&csi2->sd, &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(&csi2->sd.entity, CSI2_NUM_PADS,
748 				     csi2->pad);
749 	if (ret)
750 		return ret;
751 
752 	csi2->pllref_clk = devm_clk_get(&pdev->dev, "ref");
753 	if (IS_ERR(csi2->pllref_clk)) {
754 		v4l2_err(&csi2->sd, "failed to get pll reference clock\n");
755 		return PTR_ERR(csi2->pllref_clk);
756 	}
757 
758 	csi2->dphy_clk = devm_clk_get(&pdev->dev, "dphy");
759 	if (IS_ERR(csi2->dphy_clk)) {
760 		v4l2_err(&csi2->sd, "failed to get dphy clock\n");
761 		return PTR_ERR(csi2->dphy_clk);
762 	}
763 
764 	csi2->pix_clk = devm_clk_get(&pdev->dev, "pix");
765 	if (IS_ERR(csi2->pix_clk)) {
766 		v4l2_err(&csi2->sd, "failed to get pixel clock\n");
767 		return PTR_ERR(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(&pdev->dev, res->start, PAGE_SIZE);
777 	if (!csi2->base)
778 		return -ENOMEM;
779 
780 	mutex_init(&csi2->lock);
781 
782 	ret = clk_prepare_enable(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(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, &csi2->sd);
795 
796 	ret = csi2_async_register(csi2);
797 	if (ret)
798 		goto clean_notifier;
799 
800 	return 0;
801 
802 clean_notifier:
803 	v4l2_async_nf_unregister(&csi2->notifier);
804 	v4l2_async_nf_cleanup(&csi2->notifier);
805 	clk_disable_unprepare(csi2->dphy_clk);
806 pllref_off:
807 	clk_disable_unprepare(csi2->pllref_clk);
808 rmmutex:
809 	mutex_destroy(&csi2->lock);
810 	return ret;
811 }
812 
csi2_remove(struct platform_device * pdev)813 static 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(sd);
817 
818 	v4l2_async_nf_unregister(&csi2->notifier);
819 	v4l2_async_nf_cleanup(&csi2->notifier);
820 	v4l2_async_unregister_subdev(sd);
821 	clk_disable_unprepare(csi2->dphy_clk);
822 	clk_disable_unprepare(csi2->pllref_clk);
823 	mutex_destroy(&csi2->lock);
824 	media_entity_cleanup(&sd->entity);
825 }
826 
827 static const struct of_device_id csi2_dt_ids[] = {
828 	{ .compatible = "fsl,imx6-mipi-csi2", },
829 	{ /* sentinel */ }
830 };
831 MODULE_DEVICE_TABLE(of, csi2_dt_ids);
832 
833 static 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_new = csi2_remove,
840 };
841 
842 module_platform_driver(csi2_driver);
843 
844 MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver");
845 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
846 MODULE_LICENSE("GPL");
847