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