1
2
3
4
5
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
23
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
32
33
34#define CSI2_DEFAULT_MAX_MBPS 849
35
36struct 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;
44 void __iomem *base;
45
46 struct v4l2_subdev *remote;
47 unsigned int remote_pad;
48 unsigned short data_lanes;
49
50
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
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
87
88
89
90#define CSI2IPU_GASKET 0xf00
91#define CSI2IPU_YUV422_YUYV BIT(2)
92
93static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev)
94{
95 return container_of(sdev, struct csi2_dev, sd);
96}
97
98static 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
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131static 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
144static void csi2_set_lanes(struct csi2_dev *csi2, unsigned int lanes)
145{
146 writel(lanes - 1, csi2->base + CSI2_N_LANES);
147}
148
149static void dw_mipi_csi2_phy_write(struct csi2_dev *csi2,
150 u32 test_code, u32 test_data)
151{
152
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
158 writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
159
160
161 writel(PHY_TESTEN | test_code, csi2->base + CSI2_PHY_TST_CTRL1);
162 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
163
164
165 writel(test_data, csi2->base + CSI2_PHY_TST_CTRL1);
166 writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
167
168
169 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
170}
171
172
173
174
175
176
177static 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
191static 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
202static 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
227
228
229static int __maybe_unused csi2_dphy_wait_ulp(struct csi2_dev *csi2)
230{
231 u32 reg;
232 int ret;
233
234
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
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
254static 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
270static 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
287static 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
303static 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
359static 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
369 csi2ipu_gasket_init(csi2);
370
371
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
381 csi2_set_lanes(csi2, lanes);
382 csi2_enable(csi2, true);
383
384
385 csi2_dphy_wait_stopstate(csi2, lanes);
386
387
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
394 ret = csi2_dphy_wait_clock_lane(csi2);
395 if (ret)
396 goto err_stop_upstream;
397
398 return 0;
399
400err_stop_upstream:
401 v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
402err_assert_reset:
403 csi2_enable(csi2, false);
404err_disable_clk:
405 clk_disable_unprepare(csi2->pix_clk);
406 return ret;
407}
408
409static void csi2_stop(struct csi2_dev *csi2)
410{
411
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
420
421
422static 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
445
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
458update_count:
459 csi2->stream_count += enable ? 1 : -1;
460 if (csi2->stream_count < 0)
461 csi2->stream_count = 0;
462out:
463 mutex_unlock(&csi2->lock);
464 return ret;
465}
466
467static 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
505out:
506 mutex_unlock(&csi2->lock);
507 return ret;
508}
509
510static struct v4l2_mbus_framefmt *
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
520static 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
538static 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
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;
563out:
564 mutex_unlock(&csi2->lock);
565 return ret;
566}
567
568static int csi2_registered(struct v4l2_subdev *sd)
569{
570 struct csi2_dev *csi2 = sd_to_dev(sd);
571
572
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
579static 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
585static const struct v4l2_subdev_video_ops csi2_video_ops = {
586 .s_stream = csi2_s_stream,
587};
588
589static 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
595static const struct v4l2_subdev_ops csi2_subdev_ops = {
596 .video = &csi2_video_ops,
597 .pad = &csi2_pad_ops,
598};
599
600static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
601 .registered = csi2_registered,
602};
603
604static 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
627static 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
636static const struct v4l2_async_notifier_operations csi2_notify_ops = {
637 .bound = csi2_notify_bound,
638 .unbind = csi2_notify_unbind,
639};
640
641static 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_nf_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_nf_add_fwnode_remote(&csi2->notifier, ep,
667 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_nf_register(&csi2->sd, &csi2->notifier);
676 if (ret)
677 return ret;
678
679 return v4l2_async_register_subdev(&csi2->sd);
680
681err_parse:
682 fwnode_handle_put(ep);
683 return ret;
684}
685
686static int csi2_probe(struct platform_device *pdev)
687{
688 struct csi2_dev *csi2;
689 struct resource *res;
690 int i, ret;
691
692 csi2 = devm_kzalloc(&pdev->dev, sizeof(*csi2), GFP_KERNEL);
693 if (!csi2)
694 return -ENOMEM;
695
696 csi2->dev = &pdev->dev;
697
698 v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
699 v4l2_set_subdevdata(&csi2->sd, &pdev->dev);
700 csi2->sd.internal_ops = &csi2_internal_ops;
701 csi2->sd.entity.ops = &csi2_entity_ops;
702 csi2->sd.dev = &pdev->dev;
703 csi2->sd.owner = THIS_MODULE;
704 csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
705 strscpy(csi2->sd.name, DEVICE_NAME, sizeof(csi2->sd.name));
706 csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
707 csi2->sd.grp_id = IMX_MEDIA_GRP_ID_CSI2;
708
709 for (i = 0; i < CSI2_NUM_PADS; i++) {
710 csi2->pad[i].flags = (i == CSI2_SINK_PAD) ?
711 MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
712 }
713
714 ret = media_entity_pads_init(&csi2->sd.entity, CSI2_NUM_PADS,
715 csi2->pad);
716 if (ret)
717 return ret;
718
719 csi2->pllref_clk = devm_clk_get(&pdev->dev, "ref");
720 if (IS_ERR(csi2->pllref_clk)) {
721 v4l2_err(&csi2->sd, "failed to get pll reference clock\n");
722 return PTR_ERR(csi2->pllref_clk);
723 }
724
725 csi2->dphy_clk = devm_clk_get(&pdev->dev, "dphy");
726 if (IS_ERR(csi2->dphy_clk)) {
727 v4l2_err(&csi2->sd, "failed to get dphy clock\n");
728 return PTR_ERR(csi2->dphy_clk);
729 }
730
731 csi2->pix_clk = devm_clk_get(&pdev->dev, "pix");
732 if (IS_ERR(csi2->pix_clk)) {
733 v4l2_err(&csi2->sd, "failed to get pixel clock\n");
734 return PTR_ERR(csi2->pix_clk);
735 }
736
737 res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
738 if (!res) {
739 v4l2_err(&csi2->sd, "failed to get platform resources\n");
740 return -ENODEV;
741 }
742
743 csi2->base = devm_ioremap(&pdev->dev, res->start, PAGE_SIZE);
744 if (!csi2->base)
745 return -ENOMEM;
746
747 mutex_init(&csi2->lock);
748
749 ret = clk_prepare_enable(csi2->pllref_clk);
750 if (ret) {
751 v4l2_err(&csi2->sd, "failed to enable pllref_clk\n");
752 goto rmmutex;
753 }
754
755 ret = clk_prepare_enable(csi2->dphy_clk);
756 if (ret) {
757 v4l2_err(&csi2->sd, "failed to enable dphy_clk\n");
758 goto pllref_off;
759 }
760
761 platform_set_drvdata(pdev, &csi2->sd);
762
763 ret = csi2_async_register(csi2);
764 if (ret)
765 goto clean_notifier;
766
767 return 0;
768
769clean_notifier:
770 v4l2_async_nf_unregister(&csi2->notifier);
771 v4l2_async_nf_cleanup(&csi2->notifier);
772 clk_disable_unprepare(csi2->dphy_clk);
773pllref_off:
774 clk_disable_unprepare(csi2->pllref_clk);
775rmmutex:
776 mutex_destroy(&csi2->lock);
777 return ret;
778}
779
780static int csi2_remove(struct platform_device *pdev)
781{
782 struct v4l2_subdev *sd = platform_get_drvdata(pdev);
783 struct csi2_dev *csi2 = sd_to_dev(sd);
784
785 v4l2_async_nf_unregister(&csi2->notifier);
786 v4l2_async_nf_cleanup(&csi2->notifier);
787 v4l2_async_unregister_subdev(sd);
788 clk_disable_unprepare(csi2->dphy_clk);
789 clk_disable_unprepare(csi2->pllref_clk);
790 mutex_destroy(&csi2->lock);
791 media_entity_cleanup(&sd->entity);
792
793 return 0;
794}
795
796static const struct of_device_id csi2_dt_ids[] = {
797 { .compatible = "fsl,imx6-mipi-csi2", },
798 { }
799};
800MODULE_DEVICE_TABLE(of, csi2_dt_ids);
801
802static struct platform_driver csi2_driver = {
803 .driver = {
804 .name = DEVICE_NAME,
805 .of_match_table = csi2_dt_ids,
806 },
807 .probe = csi2_probe,
808 .remove = csi2_remove,
809};
810
811module_platform_driver(csi2_driver);
812
813MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver");
814MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
815MODULE_LICENSE("GPL");
816