1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * camss-csid.c
4 *
5 * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
6 *
7 * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
8 * Copyright (C) 2015-2018 Linaro Ltd.
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/mipi-csi2.h>
21 #include <media/v4l2-device.h>
22 #include <media/v4l2-event.h>
23 #include <media/v4l2-subdev.h>
24
25 #include "camss-csid.h"
26 #include "camss-csid-gen1.h"
27 #include "camss.h"
28
29 /* offset of CSID registers in VFE region for VFE 480 */
30 #define VFE_480_CSID_OFFSET 0x1200
31 #define VFE_480_LITE_CSID_OFFSET 0x200
32
33 #define CSID_HW_VERSION 0x0
34 #define HW_VERSION_STEPPING 0
35 #define HW_VERSION_REVISION 16
36 #define HW_VERSION_GENERATION 28
37
38 #define MSM_CSID_NAME "msm_csid"
39
40 const char * const csid_testgen_modes[] = {
41 "Disabled",
42 "Incrementing",
43 "Alternating 0x55/0xAA",
44 "All Zeros 0x00",
45 "All Ones 0xFF",
46 "Pseudo-random Data",
47 "User Specified",
48 "Complex pattern",
49 "Color box",
50 "Color bars",
51 NULL
52 };
53
54 static const struct csid_format_info formats_4_1[] = {
55 {
56 MEDIA_BUS_FMT_UYVY8_1X16,
57 MIPI_CSI2_DT_YUV422_8B,
58 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
59 8,
60 2,
61 },
62 {
63 MEDIA_BUS_FMT_VYUY8_1X16,
64 MIPI_CSI2_DT_YUV422_8B,
65 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
66 8,
67 2,
68 },
69 {
70 MEDIA_BUS_FMT_YUYV8_1X16,
71 MIPI_CSI2_DT_YUV422_8B,
72 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
73 8,
74 2,
75 },
76 {
77 MEDIA_BUS_FMT_YVYU8_1X16,
78 MIPI_CSI2_DT_YUV422_8B,
79 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
80 8,
81 2,
82 },
83 {
84 MEDIA_BUS_FMT_SBGGR8_1X8,
85 MIPI_CSI2_DT_RAW8,
86 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
87 8,
88 1,
89 },
90 {
91 MEDIA_BUS_FMT_SGBRG8_1X8,
92 MIPI_CSI2_DT_RAW8,
93 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
94 8,
95 1,
96 },
97 {
98 MEDIA_BUS_FMT_SGRBG8_1X8,
99 MIPI_CSI2_DT_RAW8,
100 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
101 8,
102 1,
103 },
104 {
105 MEDIA_BUS_FMT_SRGGB8_1X8,
106 MIPI_CSI2_DT_RAW8,
107 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
108 8,
109 1,
110 },
111 {
112 MEDIA_BUS_FMT_SBGGR10_1X10,
113 MIPI_CSI2_DT_RAW10,
114 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
115 10,
116 1,
117 },
118 {
119 MEDIA_BUS_FMT_SGBRG10_1X10,
120 MIPI_CSI2_DT_RAW10,
121 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
122 10,
123 1,
124 },
125 {
126 MEDIA_BUS_FMT_SGRBG10_1X10,
127 MIPI_CSI2_DT_RAW10,
128 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
129 10,
130 1,
131 },
132 {
133 MEDIA_BUS_FMT_SRGGB10_1X10,
134 MIPI_CSI2_DT_RAW10,
135 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
136 10,
137 1,
138 },
139 {
140 MEDIA_BUS_FMT_SBGGR12_1X12,
141 MIPI_CSI2_DT_RAW12,
142 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
143 12,
144 1,
145 },
146 {
147 MEDIA_BUS_FMT_SGBRG12_1X12,
148 MIPI_CSI2_DT_RAW12,
149 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
150 12,
151 1,
152 },
153 {
154 MEDIA_BUS_FMT_SGRBG12_1X12,
155 MIPI_CSI2_DT_RAW12,
156 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
157 12,
158 1,
159 },
160 {
161 MEDIA_BUS_FMT_SRGGB12_1X12,
162 MIPI_CSI2_DT_RAW12,
163 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
164 12,
165 1,
166 },
167 {
168 MEDIA_BUS_FMT_Y10_1X10,
169 MIPI_CSI2_DT_RAW10,
170 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
171 10,
172 1,
173 },
174 };
175
176 static const struct csid_format_info formats_4_7[] = {
177 {
178 MEDIA_BUS_FMT_UYVY8_1X16,
179 MIPI_CSI2_DT_YUV422_8B,
180 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
181 8,
182 2,
183 },
184 {
185 MEDIA_BUS_FMT_VYUY8_1X16,
186 MIPI_CSI2_DT_YUV422_8B,
187 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
188 8,
189 2,
190 },
191 {
192 MEDIA_BUS_FMT_YUYV8_1X16,
193 MIPI_CSI2_DT_YUV422_8B,
194 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
195 8,
196 2,
197 },
198 {
199 MEDIA_BUS_FMT_YVYU8_1X16,
200 MIPI_CSI2_DT_YUV422_8B,
201 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
202 8,
203 2,
204 },
205 {
206 MEDIA_BUS_FMT_SBGGR8_1X8,
207 MIPI_CSI2_DT_RAW8,
208 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
209 8,
210 1,
211 },
212 {
213 MEDIA_BUS_FMT_SGBRG8_1X8,
214 MIPI_CSI2_DT_RAW8,
215 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
216 8,
217 1,
218 },
219 {
220 MEDIA_BUS_FMT_SGRBG8_1X8,
221 MIPI_CSI2_DT_RAW8,
222 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
223 8,
224 1,
225 },
226 {
227 MEDIA_BUS_FMT_SRGGB8_1X8,
228 MIPI_CSI2_DT_RAW8,
229 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
230 8,
231 1,
232 },
233 {
234 MEDIA_BUS_FMT_SBGGR10_1X10,
235 MIPI_CSI2_DT_RAW10,
236 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
237 10,
238 1,
239 },
240 {
241 MEDIA_BUS_FMT_SGBRG10_1X10,
242 MIPI_CSI2_DT_RAW10,
243 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
244 10,
245 1,
246 },
247 {
248 MEDIA_BUS_FMT_SGRBG10_1X10,
249 MIPI_CSI2_DT_RAW10,
250 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
251 10,
252 1,
253 },
254 {
255 MEDIA_BUS_FMT_SRGGB10_1X10,
256 MIPI_CSI2_DT_RAW10,
257 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
258 10,
259 1,
260 },
261 {
262 MEDIA_BUS_FMT_SBGGR12_1X12,
263 MIPI_CSI2_DT_RAW12,
264 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
265 12,
266 1,
267 },
268 {
269 MEDIA_BUS_FMT_SGBRG12_1X12,
270 MIPI_CSI2_DT_RAW12,
271 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
272 12,
273 1,
274 },
275 {
276 MEDIA_BUS_FMT_SGRBG12_1X12,
277 MIPI_CSI2_DT_RAW12,
278 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
279 12,
280 1,
281 },
282 {
283 MEDIA_BUS_FMT_SRGGB12_1X12,
284 MIPI_CSI2_DT_RAW12,
285 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
286 12,
287 1,
288 },
289 {
290 MEDIA_BUS_FMT_SBGGR14_1X14,
291 MIPI_CSI2_DT_RAW14,
292 DECODE_FORMAT_UNCOMPRESSED_14_BIT,
293 14,
294 1,
295 },
296 {
297 MEDIA_BUS_FMT_SGBRG14_1X14,
298 MIPI_CSI2_DT_RAW14,
299 DECODE_FORMAT_UNCOMPRESSED_14_BIT,
300 14,
301 1,
302 },
303 {
304 MEDIA_BUS_FMT_SGRBG14_1X14,
305 MIPI_CSI2_DT_RAW14,
306 DECODE_FORMAT_UNCOMPRESSED_14_BIT,
307 14,
308 1,
309 },
310 {
311 MEDIA_BUS_FMT_SRGGB14_1X14,
312 MIPI_CSI2_DT_RAW14,
313 DECODE_FORMAT_UNCOMPRESSED_14_BIT,
314 14,
315 1,
316 },
317 {
318 MEDIA_BUS_FMT_Y10_1X10,
319 MIPI_CSI2_DT_RAW10,
320 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
321 10,
322 1,
323 },
324 };
325
326 static const struct csid_format_info formats_gen2[] = {
327 {
328 MEDIA_BUS_FMT_UYVY8_1X16,
329 MIPI_CSI2_DT_YUV422_8B,
330 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
331 8,
332 2,
333 },
334 {
335 MEDIA_BUS_FMT_VYUY8_1X16,
336 MIPI_CSI2_DT_YUV422_8B,
337 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
338 8,
339 2,
340 },
341 {
342 MEDIA_BUS_FMT_YUYV8_1X16,
343 MIPI_CSI2_DT_YUV422_8B,
344 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
345 8,
346 2,
347 },
348 {
349 MEDIA_BUS_FMT_YVYU8_1X16,
350 MIPI_CSI2_DT_YUV422_8B,
351 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
352 8,
353 2,
354 },
355 {
356 MEDIA_BUS_FMT_SBGGR8_1X8,
357 MIPI_CSI2_DT_RAW8,
358 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
359 8,
360 1,
361 },
362 {
363 MEDIA_BUS_FMT_SGBRG8_1X8,
364 MIPI_CSI2_DT_RAW8,
365 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
366 8,
367 1,
368 },
369 {
370 MEDIA_BUS_FMT_SGRBG8_1X8,
371 MIPI_CSI2_DT_RAW8,
372 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
373 8,
374 1,
375 },
376 {
377 MEDIA_BUS_FMT_SRGGB8_1X8,
378 MIPI_CSI2_DT_RAW8,
379 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
380 8,
381 1,
382 },
383 {
384 MEDIA_BUS_FMT_SBGGR10_1X10,
385 MIPI_CSI2_DT_RAW10,
386 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
387 10,
388 1,
389 },
390 {
391 MEDIA_BUS_FMT_SGBRG10_1X10,
392 MIPI_CSI2_DT_RAW10,
393 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
394 10,
395 1,
396 },
397 {
398 MEDIA_BUS_FMT_SGRBG10_1X10,
399 MIPI_CSI2_DT_RAW10,
400 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
401 10,
402 1,
403 },
404 {
405 MEDIA_BUS_FMT_SRGGB10_1X10,
406 MIPI_CSI2_DT_RAW10,
407 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
408 10,
409 1,
410 },
411 {
412 MEDIA_BUS_FMT_Y8_1X8,
413 MIPI_CSI2_DT_RAW8,
414 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
415 8,
416 1,
417 },
418 {
419 MEDIA_BUS_FMT_Y10_1X10,
420 MIPI_CSI2_DT_RAW10,
421 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
422 10,
423 1,
424 },
425 {
426 MEDIA_BUS_FMT_SBGGR12_1X12,
427 MIPI_CSI2_DT_RAW12,
428 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
429 12,
430 1,
431 },
432 {
433 MEDIA_BUS_FMT_SGBRG12_1X12,
434 MIPI_CSI2_DT_RAW12,
435 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
436 12,
437 1,
438 },
439 {
440 MEDIA_BUS_FMT_SGRBG12_1X12,
441 MIPI_CSI2_DT_RAW12,
442 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
443 12,
444 1,
445 },
446 {
447 MEDIA_BUS_FMT_SRGGB12_1X12,
448 MIPI_CSI2_DT_RAW12,
449 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
450 12,
451 1,
452 },
453 {
454 MEDIA_BUS_FMT_SBGGR14_1X14,
455 MIPI_CSI2_DT_RAW14,
456 DECODE_FORMAT_UNCOMPRESSED_14_BIT,
457 14,
458 1,
459 },
460 {
461 MEDIA_BUS_FMT_SGBRG14_1X14,
462 MIPI_CSI2_DT_RAW14,
463 DECODE_FORMAT_UNCOMPRESSED_14_BIT,
464 14,
465 1,
466 },
467 {
468 MEDIA_BUS_FMT_SGRBG14_1X14,
469 MIPI_CSI2_DT_RAW14,
470 DECODE_FORMAT_UNCOMPRESSED_14_BIT,
471 14,
472 1,
473 },
474 {
475 MEDIA_BUS_FMT_SRGGB14_1X14,
476 MIPI_CSI2_DT_RAW14,
477 DECODE_FORMAT_UNCOMPRESSED_14_BIT,
478 14,
479 1,
480 },
481 };
482
483 const struct csid_formats csid_formats_4_1 = {
484 .nformats = ARRAY_SIZE(formats_4_1),
485 .formats = formats_4_1
486 };
487
488 const struct csid_formats csid_formats_4_7 = {
489 .nformats = ARRAY_SIZE(formats_4_7),
490 .formats = formats_4_7
491 };
492
493 const struct csid_formats csid_formats_gen2 = {
494 .nformats = ARRAY_SIZE(formats_gen2),
495 .formats = formats_gen2
496 };
497
csid_find_code(u32 * codes,unsigned int ncodes,unsigned int match_format_idx,u32 match_code)498 u32 csid_find_code(u32 *codes, unsigned int ncodes,
499 unsigned int match_format_idx, u32 match_code)
500 {
501 int i;
502
503 if (!match_code && (match_format_idx >= ncodes))
504 return 0;
505
506 for (i = 0; i < ncodes; i++)
507 if (match_code) {
508 if (codes[i] == match_code)
509 return match_code;
510 } else {
511 if (i == match_format_idx)
512 return codes[i];
513 }
514
515 return codes[0];
516 }
517
csid_get_fmt_entry(const struct csid_format_info * formats,unsigned int nformats,u32 code)518 const struct csid_format_info *csid_get_fmt_entry(const struct csid_format_info *formats,
519 unsigned int nformats,
520 u32 code)
521 {
522 unsigned int i;
523
524 for (i = 0; i < nformats; i++)
525 if (code == formats[i].code)
526 return &formats[i];
527
528 WARN(1, "Unknown format\n");
529
530 return &formats[0];
531 }
532
533 /*
534 * csid_set_clock_rates - Calculate and set clock rates on CSID module
535 * @csiphy: CSID device
536 */
csid_set_clock_rates(struct csid_device * csid)537 static int csid_set_clock_rates(struct csid_device *csid)
538 {
539 struct device *dev = csid->camss->dev;
540 const struct csid_format_info *fmt;
541 s64 link_freq;
542 int i, j;
543 int ret;
544
545 fmt = csid_get_fmt_entry(csid->res->formats->formats, csid->res->formats->nformats,
546 csid->fmt[MSM_CSIPHY_PAD_SINK].code);
547 link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,
548 csid->phy.lane_cnt);
549 if (link_freq < 0)
550 link_freq = 0;
551
552 for (i = 0; i < csid->nclocks; i++) {
553 struct camss_clock *clock = &csid->clock[i];
554
555 if (!strcmp(clock->name, "csi0") ||
556 !strcmp(clock->name, "csi1") ||
557 !strcmp(clock->name, "csi2") ||
558 !strcmp(clock->name, "csi3")) {
559 u64 min_rate = link_freq / 4;
560 long rate;
561
562 camss_add_clock_margin(&min_rate);
563
564 for (j = 0; j < clock->nfreqs; j++)
565 if (min_rate < clock->freq[j])
566 break;
567
568 if (j == clock->nfreqs) {
569 dev_err(dev,
570 "Pixel clock is too high for CSID\n");
571 return -EINVAL;
572 }
573
574 /* if sensor pixel clock is not available */
575 /* set highest possible CSID clock rate */
576 if (min_rate == 0)
577 j = clock->nfreqs - 1;
578
579 rate = clk_round_rate(clock->clk, clock->freq[j]);
580 if (rate < 0) {
581 dev_err(dev, "clk round rate failed: %ld\n",
582 rate);
583 return -EINVAL;
584 }
585
586 ret = clk_set_rate(clock->clk, rate);
587 if (ret < 0) {
588 dev_err(dev, "clk set rate failed: %d\n", ret);
589 return ret;
590 }
591 } else if (clock->nfreqs) {
592 clk_set_rate(clock->clk, clock->freq[0]);
593 }
594 }
595
596 return 0;
597 }
598
599 /*
600 * csid_hw_version - CSID hardware version query
601 * @csid: CSID device
602 *
603 * Return HW version or error
604 */
csid_hw_version(struct csid_device * csid)605 u32 csid_hw_version(struct csid_device *csid)
606 {
607 u32 hw_version;
608 u32 hw_gen;
609 u32 hw_rev;
610 u32 hw_step;
611
612 hw_version = readl_relaxed(csid->base + CSID_HW_VERSION);
613 hw_gen = (hw_version >> HW_VERSION_GENERATION) & 0xF;
614 hw_rev = (hw_version >> HW_VERSION_REVISION) & 0xFFF;
615 hw_step = (hw_version >> HW_VERSION_STEPPING) & 0xFFFF;
616 dev_info(csid->camss->dev, "CSID:%d HW Version = %u.%u.%u\n",
617 csid->id, hw_gen, hw_rev, hw_step);
618
619 return hw_version;
620 }
621
622 /*
623 * csid_src_pad_code - Pick an output/src format based on the input/sink format
624 * @csid: CSID device
625 * @sink_code: The sink format of the input
626 * @match_format_idx: Request preferred index, as defined by subdevice csid
627 * format. Set @match_code to 0 if used.
628 * @match_code: Request preferred code, set @match_format_idx to 0 if used
629 *
630 * Return 0 on failure or src format code otherwise
631 */
csid_src_pad_code(struct csid_device * csid,u32 sink_code,unsigned int match_format_idx,u32 match_code)632 u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
633 unsigned int match_format_idx, u32 match_code)
634 {
635 if (csid->camss->res->version == CAMSS_8x16) {
636 if (match_format_idx > 0)
637 return 0;
638
639 return sink_code;
640 }
641
642 switch (sink_code) {
643 case MEDIA_BUS_FMT_SBGGR10_1X10:
644 {
645 u32 src_code[] = {
646 MEDIA_BUS_FMT_SBGGR10_1X10,
647 MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
648 };
649
650 return csid_find_code(src_code, ARRAY_SIZE(src_code),
651 match_format_idx, match_code);
652 }
653 case MEDIA_BUS_FMT_Y10_1X10:
654 {
655 u32 src_code[] = {
656 MEDIA_BUS_FMT_Y10_1X10,
657 MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
658 };
659
660 return csid_find_code(src_code, ARRAY_SIZE(src_code),
661 match_format_idx, match_code);
662 }
663 default:
664 if (match_format_idx > 0)
665 return 0;
666
667 return sink_code;
668 }
669 }
670
671 /*
672 * csid_set_power - Power on/off CSID module
673 * @sd: CSID V4L2 subdevice
674 * @on: Requested power state
675 *
676 * Return 0 on success or a negative error code otherwise
677 */
csid_set_power(struct v4l2_subdev * sd,int on)678 static int csid_set_power(struct v4l2_subdev *sd, int on)
679 {
680 struct csid_device *csid = v4l2_get_subdevdata(sd);
681 struct camss *camss = csid->camss;
682 struct device *dev = camss->dev;
683 int ret = 0;
684
685 if (on) {
686 /*
687 * From SDM845 onwards, the VFE needs to be powered on before
688 * switching on the CSID. Do so unconditionally, as there is no
689 * drawback in following the same powering order on older SoCs.
690 */
691 ret = csid->res->parent_dev_ops->get(camss, csid->id);
692 if (ret < 0)
693 return ret;
694
695 ret = pm_runtime_resume_and_get(dev);
696 if (ret < 0)
697 return ret;
698
699 ret = regulator_bulk_enable(csid->num_supplies,
700 csid->supplies);
701 if (ret < 0) {
702 pm_runtime_put_sync(dev);
703 return ret;
704 }
705
706 ret = csid_set_clock_rates(csid);
707 if (ret < 0) {
708 regulator_bulk_disable(csid->num_supplies,
709 csid->supplies);
710 pm_runtime_put_sync(dev);
711 return ret;
712 }
713
714 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
715 if (ret < 0) {
716 regulator_bulk_disable(csid->num_supplies,
717 csid->supplies);
718 pm_runtime_put_sync(dev);
719 return ret;
720 }
721
722 csid->phy.need_vc_update = true;
723
724 enable_irq(csid->irq);
725
726 ret = csid->res->hw_ops->reset(csid);
727 if (ret < 0) {
728 disable_irq(csid->irq);
729 camss_disable_clocks(csid->nclocks, csid->clock);
730 regulator_bulk_disable(csid->num_supplies,
731 csid->supplies);
732 pm_runtime_put_sync(dev);
733 return ret;
734 }
735
736 csid->res->hw_ops->hw_version(csid);
737 } else {
738 disable_irq(csid->irq);
739 camss_disable_clocks(csid->nclocks, csid->clock);
740 regulator_bulk_disable(csid->num_supplies,
741 csid->supplies);
742 pm_runtime_put_sync(dev);
743 csid->res->parent_dev_ops->put(camss, csid->id);
744 }
745
746 return ret;
747 }
748
749 /*
750 * csid_set_stream - Enable/disable streaming on CSID module
751 * @sd: CSID V4L2 subdevice
752 * @enable: Requested streaming state
753 *
754 * Main configuration of CSID module is also done here.
755 *
756 * Return 0 on success or a negative error code otherwise
757 */
csid_set_stream(struct v4l2_subdev * sd,int enable)758 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
759 {
760 struct csid_device *csid = v4l2_get_subdevdata(sd);
761 int ret;
762
763 if (enable) {
764 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) {
765 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
766 if (ret < 0) {
767 dev_err(csid->camss->dev,
768 "could not sync v4l2 controls: %d\n", ret);
769 return ret;
770 }
771 }
772
773 if (!csid->testgen.enabled &&
774 !media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]))
775 return -ENOLINK;
776 }
777
778 if (csid->phy.need_vc_update) {
779 csid->res->hw_ops->configure_stream(csid, enable);
780 csid->phy.need_vc_update = false;
781 }
782
783 return 0;
784 }
785
786 /*
787 * __csid_get_format - Get pointer to format structure
788 * @csid: CSID device
789 * @sd_state: V4L2 subdev state
790 * @pad: pad from which format is requested
791 * @which: TRY or ACTIVE format
792 *
793 * Return pointer to TRY or ACTIVE format structure
794 */
795 static struct v4l2_mbus_framefmt *
__csid_get_format(struct csid_device * csid,struct v4l2_subdev_state * sd_state,unsigned int pad,enum v4l2_subdev_format_whence which)796 __csid_get_format(struct csid_device *csid,
797 struct v4l2_subdev_state *sd_state,
798 unsigned int pad,
799 enum v4l2_subdev_format_whence which)
800 {
801 if (which == V4L2_SUBDEV_FORMAT_TRY)
802 return v4l2_subdev_state_get_format(sd_state, pad);
803
804 return &csid->fmt[pad];
805 }
806
807 /*
808 * csid_try_format - Handle try format by pad subdev method
809 * @csid: CSID device
810 * @sd_state: V4L2 subdev state
811 * @pad: pad on which format is requested
812 * @fmt: pointer to v4l2 format structure
813 * @which: wanted subdev format
814 */
csid_try_format(struct csid_device * csid,struct v4l2_subdev_state * sd_state,unsigned int pad,struct v4l2_mbus_framefmt * fmt,enum v4l2_subdev_format_whence which)815 static void csid_try_format(struct csid_device *csid,
816 struct v4l2_subdev_state *sd_state,
817 unsigned int pad,
818 struct v4l2_mbus_framefmt *fmt,
819 enum v4l2_subdev_format_whence which)
820 {
821 unsigned int i;
822
823 switch (pad) {
824 case MSM_CSID_PAD_SINK:
825 /* Set format on sink pad */
826
827 for (i = 0; i < csid->res->formats->nformats; i++)
828 if (fmt->code == csid->res->formats->formats[i].code)
829 break;
830
831 /* If not found, use UYVY as default */
832 if (i >= csid->res->formats->nformats)
833 fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
834
835 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
836 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
837
838 fmt->field = V4L2_FIELD_NONE;
839 fmt->colorspace = V4L2_COLORSPACE_SRGB;
840
841 break;
842
843 case MSM_CSID_PAD_SRC:
844 if (csid->testgen.nmodes == CSID_PAYLOAD_MODE_DISABLED ||
845 csid->testgen_mode->cur.val == 0) {
846 /* Test generator is disabled, */
847 /* keep pad formats in sync */
848 u32 code = fmt->code;
849
850 *fmt = *__csid_get_format(csid, sd_state,
851 MSM_CSID_PAD_SINK, which);
852 fmt->code = csid->res->hw_ops->src_pad_code(csid, fmt->code, 0, code);
853 } else {
854 /* Test generator is enabled, set format on source */
855 /* pad to allow test generator usage */
856
857 for (i = 0; i < csid->res->formats->nformats; i++)
858 if (csid->res->formats->formats[i].code == fmt->code)
859 break;
860
861 /* If not found, use UYVY as default */
862 if (i >= csid->res->formats->nformats)
863 fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
864
865 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
866 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
867
868 fmt->field = V4L2_FIELD_NONE;
869 }
870 break;
871 }
872
873 fmt->colorspace = V4L2_COLORSPACE_SRGB;
874 }
875
876 /*
877 * csid_enum_mbus_code - Handle pixel format enumeration
878 * @sd: CSID V4L2 subdevice
879 * @sd_state: V4L2 subdev state
880 * @code: pointer to v4l2_subdev_mbus_code_enum structure
881 * return -EINVAL or zero on success
882 */
csid_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)883 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
884 struct v4l2_subdev_state *sd_state,
885 struct v4l2_subdev_mbus_code_enum *code)
886 {
887 struct csid_device *csid = v4l2_get_subdevdata(sd);
888
889 if (code->pad == MSM_CSID_PAD_SINK) {
890 if (code->index >= csid->res->formats->nformats)
891 return -EINVAL;
892
893 code->code = csid->res->formats->formats[code->index].code;
894 } else {
895 if (csid->testgen.nmodes == CSID_PAYLOAD_MODE_DISABLED ||
896 csid->testgen_mode->cur.val == 0) {
897 struct v4l2_mbus_framefmt *sink_fmt;
898
899 sink_fmt = __csid_get_format(csid, sd_state,
900 MSM_CSID_PAD_SINK,
901 code->which);
902
903 code->code = csid->res->hw_ops->src_pad_code(csid, sink_fmt->code,
904 code->index, 0);
905 if (!code->code)
906 return -EINVAL;
907 } else {
908 if (code->index >= csid->res->formats->nformats)
909 return -EINVAL;
910
911 code->code = csid->res->formats->formats[code->index].code;
912 }
913 }
914
915 return 0;
916 }
917
918 /*
919 * csid_enum_frame_size - Handle frame size enumeration
920 * @sd: CSID V4L2 subdevice
921 * @sd_state: V4L2 subdev state
922 * @fse: pointer to v4l2_subdev_frame_size_enum structure
923 * return -EINVAL or zero on success
924 */
csid_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)925 static int csid_enum_frame_size(struct v4l2_subdev *sd,
926 struct v4l2_subdev_state *sd_state,
927 struct v4l2_subdev_frame_size_enum *fse)
928 {
929 struct csid_device *csid = v4l2_get_subdevdata(sd);
930 struct v4l2_mbus_framefmt format;
931
932 if (fse->index != 0)
933 return -EINVAL;
934
935 format.code = fse->code;
936 format.width = 1;
937 format.height = 1;
938 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
939 fse->min_width = format.width;
940 fse->min_height = format.height;
941
942 if (format.code != fse->code)
943 return -EINVAL;
944
945 format.code = fse->code;
946 format.width = -1;
947 format.height = -1;
948 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
949 fse->max_width = format.width;
950 fse->max_height = format.height;
951
952 return 0;
953 }
954
955 /*
956 * csid_get_format - Handle get format by pads subdev method
957 * @sd: CSID V4L2 subdevice
958 * @sd_state: V4L2 subdev state
959 * @fmt: pointer to v4l2 subdev format structure
960 *
961 * Return -EINVAL or zero on success
962 */
csid_get_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)963 static int csid_get_format(struct v4l2_subdev *sd,
964 struct v4l2_subdev_state *sd_state,
965 struct v4l2_subdev_format *fmt)
966 {
967 struct csid_device *csid = v4l2_get_subdevdata(sd);
968 struct v4l2_mbus_framefmt *format;
969
970 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
971 if (format == NULL)
972 return -EINVAL;
973
974 fmt->format = *format;
975
976 return 0;
977 }
978
979 /*
980 * csid_set_format - Handle set format by pads subdev method
981 * @sd: CSID V4L2 subdevice
982 * @sd_state: V4L2 subdev state
983 * @fmt: pointer to v4l2 subdev format structure
984 *
985 * Return -EINVAL or zero on success
986 */
csid_set_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)987 static int csid_set_format(struct v4l2_subdev *sd,
988 struct v4l2_subdev_state *sd_state,
989 struct v4l2_subdev_format *fmt)
990 {
991 struct csid_device *csid = v4l2_get_subdevdata(sd);
992 struct v4l2_mbus_framefmt *format;
993 int i;
994
995 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
996 if (format == NULL)
997 return -EINVAL;
998
999 csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which);
1000 *format = fmt->format;
1001
1002 /* Propagate the format from sink to source pads */
1003 if (fmt->pad == MSM_CSID_PAD_SINK) {
1004 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i) {
1005 format = __csid_get_format(csid, sd_state, i, fmt->which);
1006
1007 *format = fmt->format;
1008 csid_try_format(csid, sd_state, i, format, fmt->which);
1009 }
1010 }
1011
1012 return 0;
1013 }
1014
1015 /*
1016 * csid_init_formats - Initialize formats on all pads
1017 * @sd: CSID V4L2 subdevice
1018 * @fh: V4L2 subdev file handle
1019 *
1020 * Initialize all pad formats with default values.
1021 *
1022 * Return 0 on success or a negative error code otherwise
1023 */
csid_init_formats(struct v4l2_subdev * sd,struct v4l2_subdev_fh * fh)1024 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1025 {
1026 struct v4l2_subdev_format format = {
1027 .pad = MSM_CSID_PAD_SINK,
1028 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
1029 V4L2_SUBDEV_FORMAT_ACTIVE,
1030 .format = {
1031 .code = MEDIA_BUS_FMT_UYVY8_1X16,
1032 .width = 1920,
1033 .height = 1080
1034 }
1035 };
1036
1037 return csid_set_format(sd, fh ? fh->state : NULL, &format);
1038 }
1039
1040 /*
1041 * csid_set_test_pattern - Set test generator's pattern mode
1042 * @csid: CSID device
1043 * @value: desired test pattern mode
1044 *
1045 * Return 0 on success or a negative error code otherwise
1046 */
csid_set_test_pattern(struct csid_device * csid,s32 value)1047 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
1048 {
1049 struct csid_testgen_config *tg = &csid->testgen;
1050
1051 /* If CSID is linked to CSIPHY, do not allow to enable test generator */
1052 if (value && media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]))
1053 return -EBUSY;
1054
1055 tg->enabled = !!value;
1056
1057 return csid->res->hw_ops->configure_testgen_pattern(csid, value);
1058 }
1059
1060 /*
1061 * csid_s_ctrl - Handle set control subdev method
1062 * @ctrl: pointer to v4l2 control structure
1063 *
1064 * Return 0 on success or a negative error code otherwise
1065 */
csid_s_ctrl(struct v4l2_ctrl * ctrl)1066 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
1067 {
1068 struct csid_device *csid = container_of(ctrl->handler,
1069 struct csid_device, ctrls);
1070 int ret = -EINVAL;
1071
1072 switch (ctrl->id) {
1073 case V4L2_CID_TEST_PATTERN:
1074 ret = csid_set_test_pattern(csid, ctrl->val);
1075 break;
1076 }
1077
1078 return ret;
1079 }
1080
1081 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
1082 .s_ctrl = csid_s_ctrl,
1083 };
1084
1085 /*
1086 * msm_csid_subdev_init - Initialize CSID device structure and resources
1087 * @csid: CSID device
1088 * @res: CSID module resources table
1089 * @id: CSID module id
1090 *
1091 * Return 0 on success or a negative error code otherwise
1092 */
msm_csid_subdev_init(struct camss * camss,struct csid_device * csid,const struct camss_subdev_resources * res,u8 id)1093 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
1094 const struct camss_subdev_resources *res, u8 id)
1095 {
1096 struct device *dev = camss->dev;
1097 struct platform_device *pdev = to_platform_device(dev);
1098 int i, j;
1099 int ret;
1100
1101 csid->camss = camss;
1102 csid->id = id;
1103 csid->res = &res->csid;
1104
1105 if (dev_WARN_ONCE(dev, !csid->res->parent_dev_ops,
1106 "Error: CSID depends on VFE/IFE device ops!\n")) {
1107 return -EINVAL;
1108 }
1109
1110 csid->res->hw_ops->subdev_init(csid);
1111
1112 /* Memory */
1113
1114 if (camss->res->version == CAMSS_8250) {
1115 /* for titan 480, CSID registers are inside the VFE region,
1116 * between the VFE "top" and "bus" registers. this requires
1117 * VFE to be initialized before CSID
1118 */
1119 if (id >= 2) /* VFE/CSID lite */
1120 csid->base = csid->res->parent_dev_ops->get_base_address(camss, id)
1121 + VFE_480_LITE_CSID_OFFSET;
1122 else
1123 csid->base = csid->res->parent_dev_ops->get_base_address(camss, id)
1124 + VFE_480_CSID_OFFSET;
1125 } else {
1126 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
1127 if (IS_ERR(csid->base))
1128 return PTR_ERR(csid->base);
1129 }
1130
1131 /* Interrupt */
1132
1133 ret = platform_get_irq_byname(pdev, res->interrupt[0]);
1134 if (ret < 0)
1135 return ret;
1136
1137 csid->irq = ret;
1138 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
1139 dev_name(dev), MSM_CSID_NAME, csid->id);
1140 ret = devm_request_irq(dev, csid->irq, csid->res->hw_ops->isr,
1141 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
1142 csid->irq_name, csid);
1143 if (ret < 0) {
1144 dev_err(dev, "request_irq failed: %d\n", ret);
1145 return ret;
1146 }
1147
1148 /* Clocks */
1149
1150 csid->nclocks = 0;
1151 while (res->clock[csid->nclocks])
1152 csid->nclocks++;
1153
1154 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),
1155 GFP_KERNEL);
1156 if (!csid->clock)
1157 return -ENOMEM;
1158
1159 for (i = 0; i < csid->nclocks; i++) {
1160 struct camss_clock *clock = &csid->clock[i];
1161
1162 clock->clk = devm_clk_get(dev, res->clock[i]);
1163 if (IS_ERR(clock->clk))
1164 return PTR_ERR(clock->clk);
1165
1166 clock->name = res->clock[i];
1167
1168 clock->nfreqs = 0;
1169 while (res->clock_rate[i][clock->nfreqs])
1170 clock->nfreqs++;
1171
1172 if (!clock->nfreqs) {
1173 clock->freq = NULL;
1174 continue;
1175 }
1176
1177 clock->freq = devm_kcalloc(dev,
1178 clock->nfreqs,
1179 sizeof(*clock->freq),
1180 GFP_KERNEL);
1181 if (!clock->freq)
1182 return -ENOMEM;
1183
1184 for (j = 0; j < clock->nfreqs; j++)
1185 clock->freq[j] = res->clock_rate[i][j];
1186 }
1187
1188 /* Regulator */
1189 for (i = 0; i < ARRAY_SIZE(res->regulators); i++) {
1190 if (res->regulators[i])
1191 csid->num_supplies++;
1192 }
1193
1194 if (csid->num_supplies) {
1195 csid->supplies = devm_kmalloc_array(camss->dev,
1196 csid->num_supplies,
1197 sizeof(*csid->supplies),
1198 GFP_KERNEL);
1199 if (!csid->supplies)
1200 return -ENOMEM;
1201 }
1202
1203 for (i = 0; i < csid->num_supplies; i++)
1204 csid->supplies[i].supply = res->regulators[i];
1205
1206 ret = devm_regulator_bulk_get(camss->dev, csid->num_supplies,
1207 csid->supplies);
1208 if (ret)
1209 return ret;
1210
1211 init_completion(&csid->reset_complete);
1212
1213 return 0;
1214 }
1215
1216 /*
1217 * msm_csid_get_csid_id - Get CSID HW module id
1218 * @entity: Pointer to CSID media entity structure
1219 * @id: Return CSID HW module id here
1220 */
msm_csid_get_csid_id(struct media_entity * entity,u8 * id)1221 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
1222 {
1223 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
1224 struct csid_device *csid = v4l2_get_subdevdata(sd);
1225
1226 *id = csid->id;
1227 }
1228
1229 /*
1230 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
1231 * @lane_cfg - CSI2 lane configuration
1232 *
1233 * Return lane assign
1234 */
csid_get_lane_assign(struct csiphy_lanes_cfg * lane_cfg)1235 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
1236 {
1237 u32 lane_assign = 0;
1238 int i;
1239
1240 for (i = 0; i < lane_cfg->num_data; i++)
1241 lane_assign |= lane_cfg->data[i].pos << (i * 4);
1242
1243 return lane_assign;
1244 }
1245
1246 /*
1247 * csid_link_setup - Setup CSID connections
1248 * @entity: Pointer to media entity structure
1249 * @local: Pointer to local pad
1250 * @remote: Pointer to remote pad
1251 * @flags: Link flags
1252 *
1253 * Return 0 on success
1254 */
csid_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)1255 static int csid_link_setup(struct media_entity *entity,
1256 const struct media_pad *local,
1257 const struct media_pad *remote, u32 flags)
1258 {
1259 if (flags & MEDIA_LNK_FL_ENABLED)
1260 if (media_pad_remote_pad_first(local))
1261 return -EBUSY;
1262
1263 if ((local->flags & MEDIA_PAD_FL_SINK) &&
1264 (flags & MEDIA_LNK_FL_ENABLED)) {
1265 struct v4l2_subdev *sd;
1266 struct csid_device *csid;
1267 struct csiphy_device *csiphy;
1268 struct csiphy_lanes_cfg *lane_cfg;
1269
1270 sd = media_entity_to_v4l2_subdev(entity);
1271 csid = v4l2_get_subdevdata(sd);
1272
1273 /* If test generator is enabled */
1274 /* do not allow a link from CSIPHY to CSID */
1275 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED &&
1276 csid->testgen_mode->cur.val != 0)
1277 return -EBUSY;
1278
1279 sd = media_entity_to_v4l2_subdev(remote->entity);
1280 csiphy = v4l2_get_subdevdata(sd);
1281
1282 /* If a sensor is not linked to CSIPHY */
1283 /* do no allow a link from CSIPHY to CSID */
1284 if (!csiphy->cfg.csi2)
1285 return -EPERM;
1286
1287 csid->phy.csiphy_id = csiphy->id;
1288
1289 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
1290 csid->phy.lane_cnt = lane_cfg->num_data;
1291 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
1292 }
1293 /* Decide which virtual channels to enable based on which source pads are enabled */
1294 if (local->flags & MEDIA_PAD_FL_SOURCE) {
1295 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
1296 struct csid_device *csid = v4l2_get_subdevdata(sd);
1297 struct device *dev = csid->camss->dev;
1298
1299 if (flags & MEDIA_LNK_FL_ENABLED)
1300 csid->phy.en_vc |= BIT(local->index - 1);
1301 else
1302 csid->phy.en_vc &= ~BIT(local->index - 1);
1303
1304 csid->phy.need_vc_update = true;
1305
1306 dev_dbg(dev, "%s: Enabled CSID virtual channels mask 0x%x\n",
1307 __func__, csid->phy.en_vc);
1308 }
1309
1310 return 0;
1311 }
1312
1313 static const struct v4l2_subdev_core_ops csid_core_ops = {
1314 .s_power = csid_set_power,
1315 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1316 .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1317 };
1318
1319 static const struct v4l2_subdev_video_ops csid_video_ops = {
1320 .s_stream = csid_set_stream,
1321 };
1322
1323 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
1324 .enum_mbus_code = csid_enum_mbus_code,
1325 .enum_frame_size = csid_enum_frame_size,
1326 .get_fmt = csid_get_format,
1327 .set_fmt = csid_set_format,
1328 };
1329
1330 static const struct v4l2_subdev_ops csid_v4l2_ops = {
1331 .core = &csid_core_ops,
1332 .video = &csid_video_ops,
1333 .pad = &csid_pad_ops,
1334 };
1335
1336 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
1337 .open = csid_init_formats,
1338 };
1339
1340 static const struct media_entity_operations csid_media_ops = {
1341 .link_setup = csid_link_setup,
1342 .link_validate = v4l2_subdev_link_validate,
1343 };
1344
1345 /*
1346 * msm_csid_register_entity - Register subdev node for CSID module
1347 * @csid: CSID device
1348 * @v4l2_dev: V4L2 device
1349 *
1350 * Return 0 on success or a negative error code otherwise
1351 */
msm_csid_register_entity(struct csid_device * csid,struct v4l2_device * v4l2_dev)1352 int msm_csid_register_entity(struct csid_device *csid,
1353 struct v4l2_device *v4l2_dev)
1354 {
1355 struct v4l2_subdev *sd = &csid->subdev;
1356 struct media_pad *pads = csid->pads;
1357 struct device *dev = csid->camss->dev;
1358 int i;
1359 int ret;
1360
1361 v4l2_subdev_init(sd, &csid_v4l2_ops);
1362 sd->internal_ops = &csid_v4l2_internal_ops;
1363 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1364 V4L2_SUBDEV_FL_HAS_EVENTS;
1365 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
1366 MSM_CSID_NAME, csid->id);
1367 v4l2_set_subdevdata(sd, csid);
1368
1369 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) {
1370 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
1371 if (ret < 0) {
1372 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
1373 return ret;
1374 }
1375
1376 csid->testgen_mode =
1377 v4l2_ctrl_new_std_menu_items(&csid->ctrls,
1378 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
1379 csid->testgen.nmodes, 0, 0,
1380 csid->testgen.modes);
1381
1382 if (csid->ctrls.error) {
1383 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
1384 ret = csid->ctrls.error;
1385 goto free_ctrl;
1386 }
1387
1388 csid->subdev.ctrl_handler = &csid->ctrls;
1389 }
1390
1391 ret = csid_init_formats(sd, NULL);
1392 if (ret < 0) {
1393 dev_err(dev, "Failed to init format: %d\n", ret);
1394 goto free_ctrl;
1395 }
1396
1397 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1398 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i)
1399 pads[i].flags = MEDIA_PAD_FL_SOURCE;
1400
1401 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
1402 sd->entity.ops = &csid_media_ops;
1403 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
1404 if (ret < 0) {
1405 dev_err(dev, "Failed to init media entity: %d\n", ret);
1406 goto free_ctrl;
1407 }
1408
1409 ret = v4l2_device_register_subdev(v4l2_dev, sd);
1410 if (ret < 0) {
1411 dev_err(dev, "Failed to register subdev: %d\n", ret);
1412 goto media_cleanup;
1413 }
1414
1415 return 0;
1416
1417 media_cleanup:
1418 media_entity_cleanup(&sd->entity);
1419 free_ctrl:
1420 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED)
1421 v4l2_ctrl_handler_free(&csid->ctrls);
1422
1423 return ret;
1424 }
1425
1426 /*
1427 * msm_csid_unregister_entity - Unregister CSID module subdev node
1428 * @csid: CSID device
1429 */
msm_csid_unregister_entity(struct csid_device * csid)1430 void msm_csid_unregister_entity(struct csid_device *csid)
1431 {
1432 v4l2_device_unregister_subdev(&csid->subdev);
1433 media_entity_cleanup(&csid->subdev.entity);
1434 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED)
1435 v4l2_ctrl_handler_free(&csid->ctrls);
1436 }
1437
csid_is_lite(struct csid_device * csid)1438 inline bool csid_is_lite(struct csid_device *csid)
1439 {
1440 return csid->camss->res->csid_res[csid->id].csid.is_lite;
1441 }
1442