1
2
3
4
5
6
7
8
9
10#include <linux/clk.h>
11#include <linux/completion.h>
12#include <linux/interrupt.h>
13#include <linux/io.h>
14#include <linux/kernel.h>
15#include <linux/of.h>
16#include <linux/platform_device.h>
17#include <linux/pm_runtime.h>
18#include <linux/regulator/consumer.h>
19#include <media/media-entity.h>
20#include <media/v4l2-device.h>
21#include <media/v4l2-event.h>
22#include <media/v4l2-subdev.h>
23
24#include "camss-csid.h"
25#include "camss-csid-gen1.h"
26#include "camss.h"
27
28#define MSM_CSID_NAME "msm_csid"
29
30const char * const csid_testgen_modes[] = {
31 "Disabled",
32 "Incrementing",
33 "Alternating 0x55/0xAA",
34 "All Zeros 0x00",
35 "All Ones 0xFF",
36 "Pseudo-random Data",
37 "User Specified",
38 "Complex pattern",
39 "Color box",
40 "Color bars",
41 NULL
42};
43
44u32 csid_find_code(u32 *codes, unsigned int ncodes,
45 unsigned int match_format_idx, u32 match_code)
46{
47 int i;
48
49 if (!match_code && (match_format_idx >= ncodes))
50 return 0;
51
52 for (i = 0; i < ncodes; i++)
53 if (match_code) {
54 if (codes[i] == match_code)
55 return match_code;
56 } else {
57 if (i == match_format_idx)
58 return codes[i];
59 }
60
61 return codes[0];
62}
63
64const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
65 unsigned int nformats,
66 u32 code)
67{
68 unsigned int i;
69
70 for (i = 0; i < nformats; i++)
71 if (code == formats[i].code)
72 return &formats[i];
73
74 WARN(1, "Unknown format\n");
75
76 return &formats[0];
77}
78
79
80
81
82
83static int csid_set_clock_rates(struct csid_device *csid)
84{
85 struct device *dev = csid->camss->dev;
86 const struct csid_format *fmt;
87 s64 link_freq;
88 int i, j;
89 int ret;
90
91 fmt = csid_get_fmt_entry(csid->formats, csid->nformats,
92 csid->fmt[MSM_CSIPHY_PAD_SINK].code);
93 link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,
94 csid->phy.lane_cnt);
95 if (link_freq < 0)
96 link_freq = 0;
97
98 for (i = 0; i < csid->nclocks; i++) {
99 struct camss_clock *clock = &csid->clock[i];
100
101 if (!strcmp(clock->name, "csi0") ||
102 !strcmp(clock->name, "csi1") ||
103 !strcmp(clock->name, "csi2") ||
104 !strcmp(clock->name, "csi3")) {
105 u64 min_rate = link_freq / 4;
106 long rate;
107
108 camss_add_clock_margin(&min_rate);
109
110 for (j = 0; j < clock->nfreqs; j++)
111 if (min_rate < clock->freq[j])
112 break;
113
114 if (j == clock->nfreqs) {
115 dev_err(dev,
116 "Pixel clock is too high for CSID\n");
117 return -EINVAL;
118 }
119
120
121
122 if (min_rate == 0)
123 j = clock->nfreqs - 1;
124
125 rate = clk_round_rate(clock->clk, clock->freq[j]);
126 if (rate < 0) {
127 dev_err(dev, "clk round rate failed: %ld\n",
128 rate);
129 return -EINVAL;
130 }
131
132 ret = clk_set_rate(clock->clk, rate);
133 if (ret < 0) {
134 dev_err(dev, "clk set rate failed: %d\n", ret);
135 return ret;
136 }
137 } else if (clock->nfreqs) {
138 clk_set_rate(clock->clk, clock->freq[0]);
139 }
140 }
141
142 return 0;
143}
144
145
146
147
148
149
150
151
152static int csid_set_power(struct v4l2_subdev *sd, int on)
153{
154 struct csid_device *csid = v4l2_get_subdevdata(sd);
155 struct device *dev = csid->camss->dev;
156 int ret;
157
158 if (on) {
159 ret = pm_runtime_resume_and_get(dev);
160 if (ret < 0)
161 return ret;
162
163 ret = regulator_enable(csid->vdda);
164 if (ret < 0) {
165 pm_runtime_put_sync(dev);
166 return ret;
167 }
168
169 ret = csid_set_clock_rates(csid);
170 if (ret < 0) {
171 regulator_disable(csid->vdda);
172 pm_runtime_put_sync(dev);
173 return ret;
174 }
175
176 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
177 if (ret < 0) {
178 regulator_disable(csid->vdda);
179 pm_runtime_put_sync(dev);
180 return ret;
181 }
182
183 enable_irq(csid->irq);
184
185 ret = csid->ops->reset(csid);
186 if (ret < 0) {
187 disable_irq(csid->irq);
188 camss_disable_clocks(csid->nclocks, csid->clock);
189 regulator_disable(csid->vdda);
190 pm_runtime_put_sync(dev);
191 return ret;
192 }
193
194 csid->ops->hw_version(csid);
195 } else {
196 disable_irq(csid->irq);
197 camss_disable_clocks(csid->nclocks, csid->clock);
198 ret = regulator_disable(csid->vdda);
199 pm_runtime_put_sync(dev);
200 }
201
202 return ret;
203}
204
205
206
207
208
209
210
211
212
213
214static int csid_set_stream(struct v4l2_subdev *sd, int enable)
215{
216 struct csid_device *csid = v4l2_get_subdevdata(sd);
217 int ret;
218
219 if (enable) {
220 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
221 if (ret < 0) {
222 dev_err(csid->camss->dev,
223 "could not sync v4l2 controls: %d\n", ret);
224 return ret;
225 }
226
227 if (!csid->testgen.enabled &&
228 !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
229 return -ENOLINK;
230 }
231
232 csid->ops->configure_stream(csid, enable);
233
234 return 0;
235}
236
237
238
239
240
241
242
243
244
245
246static struct v4l2_mbus_framefmt *
247__csid_get_format(struct csid_device *csid,
248 struct v4l2_subdev_state *sd_state,
249 unsigned int pad,
250 enum v4l2_subdev_format_whence which)
251{
252 if (which == V4L2_SUBDEV_FORMAT_TRY)
253 return v4l2_subdev_get_try_format(&csid->subdev, sd_state,
254 pad);
255
256 return &csid->fmt[pad];
257}
258
259
260
261
262
263
264
265
266
267static void csid_try_format(struct csid_device *csid,
268 struct v4l2_subdev_state *sd_state,
269 unsigned int pad,
270 struct v4l2_mbus_framefmt *fmt,
271 enum v4l2_subdev_format_whence which)
272{
273 unsigned int i;
274
275 switch (pad) {
276 case MSM_CSID_PAD_SINK:
277
278
279 for (i = 0; i < csid->nformats; i++)
280 if (fmt->code == csid->formats[i].code)
281 break;
282
283
284 if (i >= csid->nformats)
285 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
286
287 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
288 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
289
290 fmt->field = V4L2_FIELD_NONE;
291 fmt->colorspace = V4L2_COLORSPACE_SRGB;
292
293 break;
294
295 case MSM_CSID_PAD_SRC:
296 if (csid->testgen_mode->cur.val == 0) {
297
298
299 u32 code = fmt->code;
300
301 *fmt = *__csid_get_format(csid, sd_state,
302 MSM_CSID_PAD_SINK, which);
303 fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
304 } else {
305
306
307
308 for (i = 0; i < csid->nformats; i++)
309 if (csid->formats[i].code == fmt->code)
310 break;
311
312
313 if (i >= csid->nformats)
314 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
315
316 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
317 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
318
319 fmt->field = V4L2_FIELD_NONE;
320 }
321 break;
322 }
323
324 fmt->colorspace = V4L2_COLORSPACE_SRGB;
325}
326
327
328
329
330
331
332
333
334static int csid_enum_mbus_code(struct v4l2_subdev *sd,
335 struct v4l2_subdev_state *sd_state,
336 struct v4l2_subdev_mbus_code_enum *code)
337{
338 struct csid_device *csid = v4l2_get_subdevdata(sd);
339
340 if (code->pad == MSM_CSID_PAD_SINK) {
341 if (code->index >= csid->nformats)
342 return -EINVAL;
343
344 code->code = csid->formats[code->index].code;
345 } else {
346 if (csid->testgen_mode->cur.val == 0) {
347 struct v4l2_mbus_framefmt *sink_fmt;
348
349 sink_fmt = __csid_get_format(csid, sd_state,
350 MSM_CSID_PAD_SINK,
351 code->which);
352
353 code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
354 code->index, 0);
355 if (!code->code)
356 return -EINVAL;
357 } else {
358 if (code->index >= csid->nformats)
359 return -EINVAL;
360
361 code->code = csid->formats[code->index].code;
362 }
363 }
364
365 return 0;
366}
367
368
369
370
371
372
373
374
375static int csid_enum_frame_size(struct v4l2_subdev *sd,
376 struct v4l2_subdev_state *sd_state,
377 struct v4l2_subdev_frame_size_enum *fse)
378{
379 struct csid_device *csid = v4l2_get_subdevdata(sd);
380 struct v4l2_mbus_framefmt format;
381
382 if (fse->index != 0)
383 return -EINVAL;
384
385 format.code = fse->code;
386 format.width = 1;
387 format.height = 1;
388 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
389 fse->min_width = format.width;
390 fse->min_height = format.height;
391
392 if (format.code != fse->code)
393 return -EINVAL;
394
395 format.code = fse->code;
396 format.width = -1;
397 format.height = -1;
398 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
399 fse->max_width = format.width;
400 fse->max_height = format.height;
401
402 return 0;
403}
404
405
406
407
408
409
410
411
412
413static int csid_get_format(struct v4l2_subdev *sd,
414 struct v4l2_subdev_state *sd_state,
415 struct v4l2_subdev_format *fmt)
416{
417 struct csid_device *csid = v4l2_get_subdevdata(sd);
418 struct v4l2_mbus_framefmt *format;
419
420 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
421 if (format == NULL)
422 return -EINVAL;
423
424 fmt->format = *format;
425
426 return 0;
427}
428
429
430
431
432
433
434
435
436
437static int csid_set_format(struct v4l2_subdev *sd,
438 struct v4l2_subdev_state *sd_state,
439 struct v4l2_subdev_format *fmt)
440{
441 struct csid_device *csid = v4l2_get_subdevdata(sd);
442 struct v4l2_mbus_framefmt *format;
443
444 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
445 if (format == NULL)
446 return -EINVAL;
447
448 csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which);
449 *format = fmt->format;
450
451
452 if (fmt->pad == MSM_CSID_PAD_SINK) {
453 format = __csid_get_format(csid, sd_state, MSM_CSID_PAD_SRC,
454 fmt->which);
455
456 *format = fmt->format;
457 csid_try_format(csid, sd_state, MSM_CSID_PAD_SRC, format,
458 fmt->which);
459 }
460
461 return 0;
462}
463
464
465
466
467
468
469
470
471
472
473static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
474{
475 struct v4l2_subdev_format format = {
476 .pad = MSM_CSID_PAD_SINK,
477 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
478 V4L2_SUBDEV_FORMAT_ACTIVE,
479 .format = {
480 .code = MEDIA_BUS_FMT_UYVY8_2X8,
481 .width = 1920,
482 .height = 1080
483 }
484 };
485
486 return csid_set_format(sd, fh ? fh->state : NULL, &format);
487}
488
489
490
491
492
493
494
495
496static int csid_set_test_pattern(struct csid_device *csid, s32 value)
497{
498 struct csid_testgen_config *tg = &csid->testgen;
499
500
501 if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
502 return -EBUSY;
503
504 tg->enabled = !!value;
505
506 return csid->ops->configure_testgen_pattern(csid, value);
507}
508
509
510
511
512
513
514
515static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
516{
517 struct csid_device *csid = container_of(ctrl->handler,
518 struct csid_device, ctrls);
519 int ret = -EINVAL;
520
521 switch (ctrl->id) {
522 case V4L2_CID_TEST_PATTERN:
523 ret = csid_set_test_pattern(csid, ctrl->val);
524 break;
525 }
526
527 return ret;
528}
529
530static const struct v4l2_ctrl_ops csid_ctrl_ops = {
531 .s_ctrl = csid_s_ctrl,
532};
533
534
535
536
537
538
539
540
541
542int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
543 const struct resources *res, u8 id)
544{
545 struct device *dev = camss->dev;
546 struct platform_device *pdev = to_platform_device(dev);
547 struct resource *r;
548 int i, j;
549 int ret;
550
551 csid->camss = camss;
552 csid->id = id;
553
554 if (camss->version == CAMSS_8x16) {
555 csid->ops = &csid_ops_4_1;
556 } else if (camss->version == CAMSS_8x96 ||
557 camss->version == CAMSS_660) {
558 csid->ops = &csid_ops_4_7;
559 } else if (camss->version == CAMSS_845) {
560 csid->ops = &csid_ops_170;
561 } else {
562 return -EINVAL;
563 }
564 csid->ops->subdev_init(csid);
565
566
567
568 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
569 if (IS_ERR(csid->base))
570 return PTR_ERR(csid->base);
571
572
573
574 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
575 res->interrupt[0]);
576 if (!r) {
577 dev_err(dev, "missing IRQ\n");
578 return -EINVAL;
579 }
580
581 csid->irq = r->start;
582 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
583 dev_name(dev), MSM_CSID_NAME, csid->id);
584 ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
585 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
586 csid->irq_name, csid);
587 if (ret < 0) {
588 dev_err(dev, "request_irq failed: %d\n", ret);
589 return ret;
590 }
591
592
593
594 csid->nclocks = 0;
595 while (res->clock[csid->nclocks])
596 csid->nclocks++;
597
598 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),
599 GFP_KERNEL);
600 if (!csid->clock)
601 return -ENOMEM;
602
603 for (i = 0; i < csid->nclocks; i++) {
604 struct camss_clock *clock = &csid->clock[i];
605
606 clock->clk = devm_clk_get(dev, res->clock[i]);
607 if (IS_ERR(clock->clk))
608 return PTR_ERR(clock->clk);
609
610 clock->name = res->clock[i];
611
612 clock->nfreqs = 0;
613 while (res->clock_rate[i][clock->nfreqs])
614 clock->nfreqs++;
615
616 if (!clock->nfreqs) {
617 clock->freq = NULL;
618 continue;
619 }
620
621 clock->freq = devm_kcalloc(dev,
622 clock->nfreqs,
623 sizeof(*clock->freq),
624 GFP_KERNEL);
625 if (!clock->freq)
626 return -ENOMEM;
627
628 for (j = 0; j < clock->nfreqs; j++)
629 clock->freq[j] = res->clock_rate[i][j];
630 }
631
632
633
634 csid->vdda = devm_regulator_get(dev, res->regulator[0]);
635 if (IS_ERR(csid->vdda)) {
636 dev_err(dev, "could not get regulator\n");
637 return PTR_ERR(csid->vdda);
638 }
639
640 init_completion(&csid->reset_complete);
641
642 return 0;
643}
644
645
646
647
648
649
650void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
651{
652 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
653 struct csid_device *csid = v4l2_get_subdevdata(sd);
654
655 *id = csid->id;
656}
657
658
659
660
661
662
663
664static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
665{
666 u32 lane_assign = 0;
667 int i;
668
669 for (i = 0; i < lane_cfg->num_data; i++)
670 lane_assign |= lane_cfg->data[i].pos << (i * 4);
671
672 return lane_assign;
673}
674
675
676
677
678
679
680
681
682
683
684static int csid_link_setup(struct media_entity *entity,
685 const struct media_pad *local,
686 const struct media_pad *remote, u32 flags)
687{
688 if (flags & MEDIA_LNK_FL_ENABLED)
689 if (media_entity_remote_pad(local))
690 return -EBUSY;
691
692 if ((local->flags & MEDIA_PAD_FL_SINK) &&
693 (flags & MEDIA_LNK_FL_ENABLED)) {
694 struct v4l2_subdev *sd;
695 struct csid_device *csid;
696 struct csiphy_device *csiphy;
697 struct csiphy_lanes_cfg *lane_cfg;
698 struct v4l2_subdev_format format = { 0 };
699
700 sd = media_entity_to_v4l2_subdev(entity);
701 csid = v4l2_get_subdevdata(sd);
702
703
704
705 if (csid->testgen_mode->cur.val != 0)
706 return -EBUSY;
707
708 sd = media_entity_to_v4l2_subdev(remote->entity);
709 csiphy = v4l2_get_subdevdata(sd);
710
711
712
713 if (!csiphy->cfg.csi2)
714 return -EPERM;
715
716 csid->phy.csiphy_id = csiphy->id;
717
718 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
719 csid->phy.lane_cnt = lane_cfg->num_data;
720 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
721
722
723 format.pad = MSM_CSID_PAD_SRC;
724 format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
725 csid_set_format(&csid->subdev, NULL, &format);
726 }
727
728 return 0;
729}
730
731static const struct v4l2_subdev_core_ops csid_core_ops = {
732 .s_power = csid_set_power,
733 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
734 .unsubscribe_event = v4l2_event_subdev_unsubscribe,
735};
736
737static const struct v4l2_subdev_video_ops csid_video_ops = {
738 .s_stream = csid_set_stream,
739};
740
741static const struct v4l2_subdev_pad_ops csid_pad_ops = {
742 .enum_mbus_code = csid_enum_mbus_code,
743 .enum_frame_size = csid_enum_frame_size,
744 .get_fmt = csid_get_format,
745 .set_fmt = csid_set_format,
746};
747
748static const struct v4l2_subdev_ops csid_v4l2_ops = {
749 .core = &csid_core_ops,
750 .video = &csid_video_ops,
751 .pad = &csid_pad_ops,
752};
753
754static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
755 .open = csid_init_formats,
756};
757
758static const struct media_entity_operations csid_media_ops = {
759 .link_setup = csid_link_setup,
760 .link_validate = v4l2_subdev_link_validate,
761};
762
763
764
765
766
767
768
769
770int msm_csid_register_entity(struct csid_device *csid,
771 struct v4l2_device *v4l2_dev)
772{
773 struct v4l2_subdev *sd = &csid->subdev;
774 struct media_pad *pads = csid->pads;
775 struct device *dev = csid->camss->dev;
776 int ret;
777
778 v4l2_subdev_init(sd, &csid_v4l2_ops);
779 sd->internal_ops = &csid_v4l2_internal_ops;
780 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
781 V4L2_SUBDEV_FL_HAS_EVENTS;
782 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
783 MSM_CSID_NAME, csid->id);
784 v4l2_set_subdevdata(sd, csid);
785
786 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
787 if (ret < 0) {
788 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
789 return ret;
790 }
791
792 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
793 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
794 csid->testgen.nmodes, 0, 0,
795 csid->testgen.modes);
796
797 if (csid->ctrls.error) {
798 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
799 ret = csid->ctrls.error;
800 goto free_ctrl;
801 }
802
803 csid->subdev.ctrl_handler = &csid->ctrls;
804
805 ret = csid_init_formats(sd, NULL);
806 if (ret < 0) {
807 dev_err(dev, "Failed to init format: %d\n", ret);
808 goto free_ctrl;
809 }
810
811 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
812 pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
813
814 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
815 sd->entity.ops = &csid_media_ops;
816 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
817 if (ret < 0) {
818 dev_err(dev, "Failed to init media entity: %d\n", ret);
819 goto free_ctrl;
820 }
821
822 ret = v4l2_device_register_subdev(v4l2_dev, sd);
823 if (ret < 0) {
824 dev_err(dev, "Failed to register subdev: %d\n", ret);
825 goto media_cleanup;
826 }
827
828 return 0;
829
830media_cleanup:
831 media_entity_cleanup(&sd->entity);
832free_ctrl:
833 v4l2_ctrl_handler_free(&csid->ctrls);
834
835 return ret;
836}
837
838
839
840
841
842void msm_csid_unregister_entity(struct csid_device *csid)
843{
844 v4l2_device_unregister_subdev(&csid->subdev);
845 media_entity_cleanup(&csid->subdev.entity);
846 v4l2_ctrl_handler_free(&csid->ctrls);
847}
848