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_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
682err_parse:
683 fwnode_handle_put(ep);
684 return ret;
685}
686
687static 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
770clean_notifier:
771 v4l2_async_notifier_unregister(&csi2->notifier);
772 v4l2_async_notifier_cleanup(&csi2->notifier);
773 clk_disable_unprepare(csi2->dphy_clk);
774pllref_off:
775 clk_disable_unprepare(csi2->pllref_clk);
776rmmutex:
777 mutex_destroy(&csi2->lock);
778 return ret;
779}
780
781static 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
797static const struct of_device_id csi2_dt_ids[] = {
798 { .compatible = "fsl,imx6-mipi-csi2", },
799 { }
800};
801MODULE_DEVICE_TABLE(of, csi2_dt_ids);
802
803static 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
812module_platform_driver(csi2_driver);
813
814MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver");
815MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
816MODULE_LICENSE("GPL");
817