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