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