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