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
csid_find_code(u32 * code,unsigned int n_code,unsigned int index,u32 req_code)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
csid_src_pad_code(struct csid_device * csid,u32 sink_code,unsigned int index,u32 src_req_code)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
csid_get_fmt_entry(const struct csid_format * formats,unsigned int nformat,u32 code)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 */
csid_isr(int irq,void * dev)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 */
csid_set_clock_rates(struct csid_device * csid)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 */
csid_reset(struct csid_device * csid)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 */
csid_set_power(struct v4l2_subdev * sd,int on)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 pm_runtime_put_sync(dev);
567 return ret;
568 }
569
570 ret = regulator_enable(csid->vdda);
571 if (ret < 0) {
572 pm_runtime_put_sync(dev);
573 return ret;
574 }
575
576 ret = csid_set_clock_rates(csid);
577 if (ret < 0) {
578 regulator_disable(csid->vdda);
579 pm_runtime_put_sync(dev);
580 return ret;
581 }
582
583 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
584 if (ret < 0) {
585 regulator_disable(csid->vdda);
586 pm_runtime_put_sync(dev);
587 return ret;
588 }
589
590 enable_irq(csid->irq);
591
592 ret = csid_reset(csid);
593 if (ret < 0) {
594 disable_irq(csid->irq);
595 camss_disable_clocks(csid->nclocks, csid->clock);
596 regulator_disable(csid->vdda);
597 pm_runtime_put_sync(dev);
598 return ret;
599 }
600
601 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
602 dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version);
603 } else {
604 disable_irq(csid->irq);
605 camss_disable_clocks(csid->nclocks, csid->clock);
606 ret = regulator_disable(csid->vdda);
607 pm_runtime_put_sync(dev);
608 }
609
610 return ret;
611 }
612
613 /*
614 * csid_set_stream - Enable/disable streaming on CSID module
615 * @sd: CSID V4L2 subdevice
616 * @enable: Requested streaming state
617 *
618 * Main configuration of CSID module is also done here.
619 *
620 * Return 0 on success or a negative error code otherwise
621 */
csid_set_stream(struct v4l2_subdev * sd,int enable)622 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
623 {
624 struct csid_device *csid = v4l2_get_subdevdata(sd);
625 struct csid_testgen_config *tg = &csid->testgen;
626 enum camss_version ver = csid->camss->version;
627 u32 val;
628
629 if (enable) {
630 u8 vc = 0; /* Virtual Channel 0 */
631 u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
632 u8 dt, dt_shift, df;
633 int ret;
634
635 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
636 if (ret < 0) {
637 dev_err(csid->camss->dev,
638 "could not sync v4l2 controls: %d\n", ret);
639 return ret;
640 }
641
642 if (!tg->enabled &&
643 !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
644 return -ENOLINK;
645
646 if (tg->enabled) {
647 /* Config Test Generator */
648 struct v4l2_mbus_framefmt *f =
649 &csid->fmt[MSM_CSID_PAD_SRC];
650 const struct csid_format *format = csid_get_fmt_entry(
651 csid->formats, csid->nformats, f->code);
652 u32 num_bytes_per_line =
653 f->width * format->bpp * format->spp / 8;
654 u32 num_lines = f->height;
655
656 /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
657 /* 1:0 VC */
658 val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
659 ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
660 writel_relaxed(val, csid->base +
661 CAMSS_CSID_TG_VC_CFG(ver));
662
663 /* 28:16 bytes per lines, 12:0 num of lines */
664 val = ((num_bytes_per_line & 0x1fff) << 16) |
665 (num_lines & 0x1fff);
666 writel_relaxed(val, csid->base +
667 CAMSS_CSID_TG_DT_n_CGG_0(ver, 0));
668
669 dt = format->data_type;
670
671 /* 5:0 data type */
672 val = dt;
673 writel_relaxed(val, csid->base +
674 CAMSS_CSID_TG_DT_n_CGG_1(ver, 0));
675
676 /* 2:0 output test pattern */
677 val = tg->payload_mode;
678 writel_relaxed(val, csid->base +
679 CAMSS_CSID_TG_DT_n_CGG_2(ver, 0));
680
681 df = format->decode_format;
682 } else {
683 struct v4l2_mbus_framefmt *f =
684 &csid->fmt[MSM_CSID_PAD_SINK];
685 const struct csid_format *format = csid_get_fmt_entry(
686 csid->formats, csid->nformats, f->code);
687 struct csid_phy_config *phy = &csid->phy;
688
689 val = phy->lane_cnt - 1;
690 val |= phy->lane_assign << 4;
691
692 writel_relaxed(val,
693 csid->base + CAMSS_CSID_CORE_CTRL_0);
694
695 val = phy->csiphy_id << 17;
696 val |= 0x9;
697
698 writel_relaxed(val,
699 csid->base + CAMSS_CSID_CORE_CTRL_1);
700
701 dt = format->data_type;
702 df = format->decode_format;
703 }
704
705 /* Config LUT */
706
707 dt_shift = (cid % 4) * 8;
708
709 val = readl_relaxed(csid->base +
710 CAMSS_CSID_CID_LUT_VC_n(ver, vc));
711 val &= ~(0xff << dt_shift);
712 val |= dt << dt_shift;
713 writel_relaxed(val, csid->base +
714 CAMSS_CSID_CID_LUT_VC_n(ver, vc));
715
716 val = CAMSS_CSID_CID_n_CFG_ISPIF_EN;
717 val |= CAMSS_CSID_CID_n_CFG_RDI_EN;
718 val |= df << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT;
719 val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP;
720
721 if (csid->camss->version == CAMSS_8x96) {
722 u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code;
723 u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code;
724
725 if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 &&
726 src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) ||
727 (sink_code == MEDIA_BUS_FMT_Y10_1X10 &&
728 src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) {
729 val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING;
730 val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16;
731 val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB;
732 }
733 }
734
735 writel_relaxed(val, csid->base +
736 CAMSS_CSID_CID_n_CFG(ver, cid));
737
738 if (tg->enabled) {
739 val = CAMSS_CSID_TG_CTRL_ENABLE;
740 writel_relaxed(val, csid->base +
741 CAMSS_CSID_TG_CTRL(ver));
742 }
743 } else {
744 if (tg->enabled) {
745 val = CAMSS_CSID_TG_CTRL_DISABLE;
746 writel_relaxed(val, csid->base +
747 CAMSS_CSID_TG_CTRL(ver));
748 }
749 }
750
751 return 0;
752 }
753
754 /*
755 * __csid_get_format - Get pointer to format structure
756 * @csid: CSID device
757 * @cfg: V4L2 subdev pad configuration
758 * @pad: pad from which format is requested
759 * @which: TRY or ACTIVE format
760 *
761 * Return pointer to TRY or ACTIVE format structure
762 */
763 static struct v4l2_mbus_framefmt *
__csid_get_format(struct csid_device * csid,struct v4l2_subdev_pad_config * cfg,unsigned int pad,enum v4l2_subdev_format_whence which)764 __csid_get_format(struct csid_device *csid,
765 struct v4l2_subdev_pad_config *cfg,
766 unsigned int pad,
767 enum v4l2_subdev_format_whence which)
768 {
769 if (which == V4L2_SUBDEV_FORMAT_TRY)
770 return v4l2_subdev_get_try_format(&csid->subdev, cfg, pad);
771
772 return &csid->fmt[pad];
773 }
774
775 /*
776 * csid_try_format - Handle try format by pad subdev method
777 * @csid: CSID device
778 * @cfg: V4L2 subdev pad configuration
779 * @pad: pad on which format is requested
780 * @fmt: pointer to v4l2 format structure
781 * @which: wanted subdev format
782 */
csid_try_format(struct csid_device * csid,struct v4l2_subdev_pad_config * cfg,unsigned int pad,struct v4l2_mbus_framefmt * fmt,enum v4l2_subdev_format_whence which)783 static void csid_try_format(struct csid_device *csid,
784 struct v4l2_subdev_pad_config *cfg,
785 unsigned int pad,
786 struct v4l2_mbus_framefmt *fmt,
787 enum v4l2_subdev_format_whence which)
788 {
789 unsigned int i;
790
791 switch (pad) {
792 case MSM_CSID_PAD_SINK:
793 /* Set format on sink pad */
794
795 for (i = 0; i < csid->nformats; i++)
796 if (fmt->code == csid->formats[i].code)
797 break;
798
799 /* If not found, use UYVY as default */
800 if (i >= csid->nformats)
801 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
802
803 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
804 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
805
806 fmt->field = V4L2_FIELD_NONE;
807 fmt->colorspace = V4L2_COLORSPACE_SRGB;
808
809 break;
810
811 case MSM_CSID_PAD_SRC:
812 if (csid->testgen_mode->cur.val == 0) {
813 /* Test generator is disabled, */
814 /* keep pad formats in sync */
815 u32 code = fmt->code;
816
817 *fmt = *__csid_get_format(csid, cfg,
818 MSM_CSID_PAD_SINK, which);
819 fmt->code = csid_src_pad_code(csid, fmt->code, 0, code);
820 } else {
821 /* Test generator is enabled, set format on source */
822 /* pad to allow test generator usage */
823
824 for (i = 0; i < csid->nformats; i++)
825 if (csid->formats[i].code == fmt->code)
826 break;
827
828 /* If not found, use UYVY as default */
829 if (i >= csid->nformats)
830 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
831
832 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
833 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
834
835 fmt->field = V4L2_FIELD_NONE;
836 }
837 break;
838 }
839
840 fmt->colorspace = V4L2_COLORSPACE_SRGB;
841 }
842
843 /*
844 * csid_enum_mbus_code - Handle pixel format enumeration
845 * @sd: CSID V4L2 subdevice
846 * @cfg: V4L2 subdev pad configuration
847 * @code: pointer to v4l2_subdev_mbus_code_enum structure
848 * return -EINVAL or zero on success
849 */
csid_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_mbus_code_enum * code)850 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
851 struct v4l2_subdev_pad_config *cfg,
852 struct v4l2_subdev_mbus_code_enum *code)
853 {
854 struct csid_device *csid = v4l2_get_subdevdata(sd);
855
856 if (code->pad == MSM_CSID_PAD_SINK) {
857 if (code->index >= csid->nformats)
858 return -EINVAL;
859
860 code->code = csid->formats[code->index].code;
861 } else {
862 if (csid->testgen_mode->cur.val == 0) {
863 struct v4l2_mbus_framefmt *sink_fmt;
864
865 sink_fmt = __csid_get_format(csid, cfg,
866 MSM_CSID_PAD_SINK,
867 code->which);
868
869 code->code = csid_src_pad_code(csid, sink_fmt->code,
870 code->index, 0);
871 if (!code->code)
872 return -EINVAL;
873 } else {
874 if (code->index >= csid->nformats)
875 return -EINVAL;
876
877 code->code = csid->formats[code->index].code;
878 }
879 }
880
881 return 0;
882 }
883
884 /*
885 * csid_enum_frame_size - Handle frame size enumeration
886 * @sd: CSID V4L2 subdevice
887 * @cfg: V4L2 subdev pad configuration
888 * @fse: pointer to v4l2_subdev_frame_size_enum structure
889 * return -EINVAL or zero on success
890 */
csid_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_frame_size_enum * fse)891 static int csid_enum_frame_size(struct v4l2_subdev *sd,
892 struct v4l2_subdev_pad_config *cfg,
893 struct v4l2_subdev_frame_size_enum *fse)
894 {
895 struct csid_device *csid = v4l2_get_subdevdata(sd);
896 struct v4l2_mbus_framefmt format;
897
898 if (fse->index != 0)
899 return -EINVAL;
900
901 format.code = fse->code;
902 format.width = 1;
903 format.height = 1;
904 csid_try_format(csid, cfg, fse->pad, &format, fse->which);
905 fse->min_width = format.width;
906 fse->min_height = format.height;
907
908 if (format.code != fse->code)
909 return -EINVAL;
910
911 format.code = fse->code;
912 format.width = -1;
913 format.height = -1;
914 csid_try_format(csid, cfg, fse->pad, &format, fse->which);
915 fse->max_width = format.width;
916 fse->max_height = format.height;
917
918 return 0;
919 }
920
921 /*
922 * csid_get_format - Handle get format by pads subdev method
923 * @sd: CSID V4L2 subdevice
924 * @cfg: V4L2 subdev pad configuration
925 * @fmt: pointer to v4l2 subdev format structure
926 *
927 * Return -EINVAL or zero on success
928 */
csid_get_format(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)929 static int csid_get_format(struct v4l2_subdev *sd,
930 struct v4l2_subdev_pad_config *cfg,
931 struct v4l2_subdev_format *fmt)
932 {
933 struct csid_device *csid = v4l2_get_subdevdata(sd);
934 struct v4l2_mbus_framefmt *format;
935
936 format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
937 if (format == NULL)
938 return -EINVAL;
939
940 fmt->format = *format;
941
942 return 0;
943 }
944
945 /*
946 * csid_set_format - Handle set format by pads subdev method
947 * @sd: CSID V4L2 subdevice
948 * @cfg: V4L2 subdev pad configuration
949 * @fmt: pointer to v4l2 subdev format structure
950 *
951 * Return -EINVAL or zero on success
952 */
csid_set_format(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)953 static int csid_set_format(struct v4l2_subdev *sd,
954 struct v4l2_subdev_pad_config *cfg,
955 struct v4l2_subdev_format *fmt)
956 {
957 struct csid_device *csid = v4l2_get_subdevdata(sd);
958 struct v4l2_mbus_framefmt *format;
959
960 format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
961 if (format == NULL)
962 return -EINVAL;
963
964 csid_try_format(csid, cfg, fmt->pad, &fmt->format, fmt->which);
965 *format = fmt->format;
966
967 /* Propagate the format from sink to source */
968 if (fmt->pad == MSM_CSID_PAD_SINK) {
969 format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SRC,
970 fmt->which);
971
972 *format = fmt->format;
973 csid_try_format(csid, cfg, MSM_CSID_PAD_SRC, format,
974 fmt->which);
975 }
976
977 return 0;
978 }
979
980 /*
981 * csid_init_formats - Initialize formats on all pads
982 * @sd: CSID V4L2 subdevice
983 * @fh: V4L2 subdev file handle
984 *
985 * Initialize all pad formats with default values.
986 *
987 * Return 0 on success or a negative error code otherwise
988 */
csid_init_formats(struct v4l2_subdev * sd,struct v4l2_subdev_fh * fh)989 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
990 {
991 struct v4l2_subdev_format format = {
992 .pad = MSM_CSID_PAD_SINK,
993 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
994 V4L2_SUBDEV_FORMAT_ACTIVE,
995 .format = {
996 .code = MEDIA_BUS_FMT_UYVY8_2X8,
997 .width = 1920,
998 .height = 1080
999 }
1000 };
1001
1002 return csid_set_format(sd, fh ? fh->pad : NULL, &format);
1003 }
1004
1005 static const char * const csid_test_pattern_menu[] = {
1006 "Disabled",
1007 "Incrementing",
1008 "Alternating 0x55/0xAA",
1009 "All Zeros 0x00",
1010 "All Ones 0xFF",
1011 "Pseudo-random Data",
1012 };
1013
1014 /*
1015 * csid_set_test_pattern - Set test generator's pattern mode
1016 * @csid: CSID device
1017 * @value: desired test pattern mode
1018 *
1019 * Return 0 on success or a negative error code otherwise
1020 */
csid_set_test_pattern(struct csid_device * csid,s32 value)1021 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
1022 {
1023 struct csid_testgen_config *tg = &csid->testgen;
1024
1025 /* If CSID is linked to CSIPHY, do not allow to enable test generator */
1026 if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
1027 return -EBUSY;
1028
1029 tg->enabled = !!value;
1030
1031 switch (value) {
1032 case 1:
1033 tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING;
1034 break;
1035 case 2:
1036 tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA;
1037 break;
1038 case 3:
1039 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES;
1040 break;
1041 case 4:
1042 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES;
1043 break;
1044 case 5:
1045 tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM;
1046 break;
1047 }
1048
1049 return 0;
1050 }
1051
1052 /*
1053 * csid_s_ctrl - Handle set control subdev method
1054 * @ctrl: pointer to v4l2 control structure
1055 *
1056 * Return 0 on success or a negative error code otherwise
1057 */
csid_s_ctrl(struct v4l2_ctrl * ctrl)1058 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
1059 {
1060 struct csid_device *csid = container_of(ctrl->handler,
1061 struct csid_device, ctrls);
1062 int ret = -EINVAL;
1063
1064 switch (ctrl->id) {
1065 case V4L2_CID_TEST_PATTERN:
1066 ret = csid_set_test_pattern(csid, ctrl->val);
1067 break;
1068 }
1069
1070 return ret;
1071 }
1072
1073 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
1074 .s_ctrl = csid_s_ctrl,
1075 };
1076
1077 /*
1078 * msm_csid_subdev_init - Initialize CSID device structure and resources
1079 * @csid: CSID device
1080 * @res: CSID module resources table
1081 * @id: CSID module id
1082 *
1083 * Return 0 on success or a negative error code otherwise
1084 */
msm_csid_subdev_init(struct camss * camss,struct csid_device * csid,const struct resources * res,u8 id)1085 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
1086 const struct resources *res, u8 id)
1087 {
1088 struct device *dev = camss->dev;
1089 struct platform_device *pdev = to_platform_device(dev);
1090 struct resource *r;
1091 int i, j;
1092 int ret;
1093
1094 csid->camss = camss;
1095 csid->id = id;
1096
1097 if (camss->version == CAMSS_8x16) {
1098 csid->formats = csid_formats_8x16;
1099 csid->nformats =
1100 ARRAY_SIZE(csid_formats_8x16);
1101 } else if (camss->version == CAMSS_8x96) {
1102 csid->formats = csid_formats_8x96;
1103 csid->nformats =
1104 ARRAY_SIZE(csid_formats_8x96);
1105 } else {
1106 return -EINVAL;
1107 }
1108
1109 /* Memory */
1110
1111 r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
1112 csid->base = devm_ioremap_resource(dev, r);
1113 if (IS_ERR(csid->base)) {
1114 dev_err(dev, "could not map memory\n");
1115 return PTR_ERR(csid->base);
1116 }
1117
1118 /* Interrupt */
1119
1120 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
1121 res->interrupt[0]);
1122 if (!r) {
1123 dev_err(dev, "missing IRQ\n");
1124 return -EINVAL;
1125 }
1126
1127 csid->irq = r->start;
1128 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
1129 dev_name(dev), MSM_CSID_NAME, csid->id);
1130 ret = devm_request_irq(dev, csid->irq, csid_isr,
1131 IRQF_TRIGGER_RISING, csid->irq_name, csid);
1132 if (ret < 0) {
1133 dev_err(dev, "request_irq failed: %d\n", ret);
1134 return ret;
1135 }
1136
1137 disable_irq(csid->irq);
1138
1139 /* Clocks */
1140
1141 csid->nclocks = 0;
1142 while (res->clock[csid->nclocks])
1143 csid->nclocks++;
1144
1145 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),
1146 GFP_KERNEL);
1147 if (!csid->clock)
1148 return -ENOMEM;
1149
1150 for (i = 0; i < csid->nclocks; i++) {
1151 struct camss_clock *clock = &csid->clock[i];
1152
1153 clock->clk = devm_clk_get(dev, res->clock[i]);
1154 if (IS_ERR(clock->clk))
1155 return PTR_ERR(clock->clk);
1156
1157 clock->name = res->clock[i];
1158
1159 clock->nfreqs = 0;
1160 while (res->clock_rate[i][clock->nfreqs])
1161 clock->nfreqs++;
1162
1163 if (!clock->nfreqs) {
1164 clock->freq = NULL;
1165 continue;
1166 }
1167
1168 clock->freq = devm_kcalloc(dev,
1169 clock->nfreqs,
1170 sizeof(*clock->freq),
1171 GFP_KERNEL);
1172 if (!clock->freq)
1173 return -ENOMEM;
1174
1175 for (j = 0; j < clock->nfreqs; j++)
1176 clock->freq[j] = res->clock_rate[i][j];
1177 }
1178
1179 /* Regulator */
1180
1181 csid->vdda = devm_regulator_get(dev, res->regulator[0]);
1182 if (IS_ERR(csid->vdda)) {
1183 dev_err(dev, "could not get regulator\n");
1184 return PTR_ERR(csid->vdda);
1185 }
1186
1187 init_completion(&csid->reset_complete);
1188
1189 return 0;
1190 }
1191
1192 /*
1193 * msm_csid_get_csid_id - Get CSID HW module id
1194 * @entity: Pointer to CSID media entity structure
1195 * @id: Return CSID HW module id here
1196 */
msm_csid_get_csid_id(struct media_entity * entity,u8 * id)1197 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
1198 {
1199 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
1200 struct csid_device *csid = v4l2_get_subdevdata(sd);
1201
1202 *id = csid->id;
1203 }
1204
1205 /*
1206 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
1207 * @lane_cfg - CSI2 lane configuration
1208 *
1209 * Return lane assign
1210 */
csid_get_lane_assign(struct csiphy_lanes_cfg * lane_cfg)1211 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
1212 {
1213 u32 lane_assign = 0;
1214 int i;
1215
1216 for (i = 0; i < lane_cfg->num_data; i++)
1217 lane_assign |= lane_cfg->data[i].pos << (i * 4);
1218
1219 return lane_assign;
1220 }
1221
1222 /*
1223 * csid_link_setup - Setup CSID connections
1224 * @entity: Pointer to media entity structure
1225 * @local: Pointer to local pad
1226 * @remote: Pointer to remote pad
1227 * @flags: Link flags
1228 *
1229 * Return 0 on success
1230 */
csid_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)1231 static int csid_link_setup(struct media_entity *entity,
1232 const struct media_pad *local,
1233 const struct media_pad *remote, u32 flags)
1234 {
1235 if (flags & MEDIA_LNK_FL_ENABLED)
1236 if (media_entity_remote_pad(local))
1237 return -EBUSY;
1238
1239 if ((local->flags & MEDIA_PAD_FL_SINK) &&
1240 (flags & MEDIA_LNK_FL_ENABLED)) {
1241 struct v4l2_subdev *sd;
1242 struct csid_device *csid;
1243 struct csiphy_device *csiphy;
1244 struct csiphy_lanes_cfg *lane_cfg;
1245 struct v4l2_subdev_format format = { 0 };
1246
1247 sd = media_entity_to_v4l2_subdev(entity);
1248 csid = v4l2_get_subdevdata(sd);
1249
1250 /* If test generator is enabled */
1251 /* do not allow a link from CSIPHY to CSID */
1252 if (csid->testgen_mode->cur.val != 0)
1253 return -EBUSY;
1254
1255 sd = media_entity_to_v4l2_subdev(remote->entity);
1256 csiphy = v4l2_get_subdevdata(sd);
1257
1258 /* If a sensor is not linked to CSIPHY */
1259 /* do no allow a link from CSIPHY to CSID */
1260 if (!csiphy->cfg.csi2)
1261 return -EPERM;
1262
1263 csid->phy.csiphy_id = csiphy->id;
1264
1265 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
1266 csid->phy.lane_cnt = lane_cfg->num_data;
1267 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
1268
1269 /* Reset format on source pad to sink pad format */
1270 format.pad = MSM_CSID_PAD_SRC;
1271 format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
1272 csid_set_format(&csid->subdev, NULL, &format);
1273 }
1274
1275 return 0;
1276 }
1277
1278 static const struct v4l2_subdev_core_ops csid_core_ops = {
1279 .s_power = csid_set_power,
1280 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1281 .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1282 };
1283
1284 static const struct v4l2_subdev_video_ops csid_video_ops = {
1285 .s_stream = csid_set_stream,
1286 };
1287
1288 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
1289 .enum_mbus_code = csid_enum_mbus_code,
1290 .enum_frame_size = csid_enum_frame_size,
1291 .get_fmt = csid_get_format,
1292 .set_fmt = csid_set_format,
1293 };
1294
1295 static const struct v4l2_subdev_ops csid_v4l2_ops = {
1296 .core = &csid_core_ops,
1297 .video = &csid_video_ops,
1298 .pad = &csid_pad_ops,
1299 };
1300
1301 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
1302 .open = csid_init_formats,
1303 };
1304
1305 static const struct media_entity_operations csid_media_ops = {
1306 .link_setup = csid_link_setup,
1307 .link_validate = v4l2_subdev_link_validate,
1308 };
1309
1310 /*
1311 * msm_csid_register_entity - Register subdev node for CSID module
1312 * @csid: CSID device
1313 * @v4l2_dev: V4L2 device
1314 *
1315 * Return 0 on success or a negative error code otherwise
1316 */
msm_csid_register_entity(struct csid_device * csid,struct v4l2_device * v4l2_dev)1317 int msm_csid_register_entity(struct csid_device *csid,
1318 struct v4l2_device *v4l2_dev)
1319 {
1320 struct v4l2_subdev *sd = &csid->subdev;
1321 struct media_pad *pads = csid->pads;
1322 struct device *dev = csid->camss->dev;
1323 int ret;
1324
1325 v4l2_subdev_init(sd, &csid_v4l2_ops);
1326 sd->internal_ops = &csid_v4l2_internal_ops;
1327 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1328 V4L2_SUBDEV_FL_HAS_EVENTS;
1329 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
1330 MSM_CSID_NAME, csid->id);
1331 v4l2_set_subdevdata(sd, csid);
1332
1333 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
1334 if (ret < 0) {
1335 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
1336 return ret;
1337 }
1338
1339 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
1340 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
1341 ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0,
1342 csid_test_pattern_menu);
1343
1344 if (csid->ctrls.error) {
1345 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
1346 ret = csid->ctrls.error;
1347 goto free_ctrl;
1348 }
1349
1350 csid->subdev.ctrl_handler = &csid->ctrls;
1351
1352 ret = csid_init_formats(sd, NULL);
1353 if (ret < 0) {
1354 dev_err(dev, "Failed to init format: %d\n", ret);
1355 goto free_ctrl;
1356 }
1357
1358 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1359 pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
1360
1361 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
1362 sd->entity.ops = &csid_media_ops;
1363 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
1364 if (ret < 0) {
1365 dev_err(dev, "Failed to init media entity: %d\n", ret);
1366 goto free_ctrl;
1367 }
1368
1369 ret = v4l2_device_register_subdev(v4l2_dev, sd);
1370 if (ret < 0) {
1371 dev_err(dev, "Failed to register subdev: %d\n", ret);
1372 goto media_cleanup;
1373 }
1374
1375 return 0;
1376
1377 media_cleanup:
1378 media_entity_cleanup(&sd->entity);
1379 free_ctrl:
1380 v4l2_ctrl_handler_free(&csid->ctrls);
1381
1382 return ret;
1383 }
1384
1385 /*
1386 * msm_csid_unregister_entity - Unregister CSID module subdev node
1387 * @csid: CSID device
1388 */
msm_csid_unregister_entity(struct csid_device * csid)1389 void msm_csid_unregister_entity(struct csid_device *csid)
1390 {
1391 v4l2_device_unregister_subdev(&csid->subdev);
1392 media_entity_cleanup(&csid->subdev.entity);
1393 v4l2_ctrl_handler_free(&csid->ctrls);
1394 }
1395
