1 /*
2 * camss-csid.c
3 *
4 * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
5 *
6 * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
7 * Copyright (C) 2015-2017 Linaro Ltd.
8 *
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License version 2 and
11 * only version 2 as published by the Free Software Foundation.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
17 */
18 #include <linux/clk.h>
19 #include <linux/completion.h>
20 #include <linux/interrupt.h>
21 #include <linux/kernel.h>
22 #include <linux/of.h>
23 #include <linux/platform_device.h>
24 #include <linux/regulator/consumer.h>
25 #include <media/media-entity.h>
26 #include <media/v4l2-device.h>
27 #include <media/v4l2-subdev.h>
28
29 #include "camss-csid.h"
30 #include "camss.h"
31
32 #define MSM_CSID_NAME "msm_csid"
33
34 #define CAMSS_CSID_HW_VERSION 0x0
35 #define CAMSS_CSID_CORE_CTRL_0 0x004
36 #define CAMSS_CSID_CORE_CTRL_1 0x008
37 #define CAMSS_CSID_RST_CMD 0x00c
38 #define CAMSS_CSID_CID_LUT_VC_n(n) (0x010 + 0x4 * (n))
39 #define CAMSS_CSID_CID_n_CFG(n) (0x020 + 0x4 * (n))
40 #define CAMSS_CSID_IRQ_CLEAR_CMD 0x060
41 #define CAMSS_CSID_IRQ_MASK 0x064
42 #define CAMSS_CSID_IRQ_STATUS 0x068
43 #define CAMSS_CSID_TG_CTRL 0x0a0
44 #define CAMSS_CSID_TG_CTRL_DISABLE 0xa06436
45 #define CAMSS_CSID_TG_CTRL_ENABLE 0xa06437
46 #define CAMSS_CSID_TG_VC_CFG 0x0a4
47 #define CAMSS_CSID_TG_VC_CFG_H_BLANKING 0x3ff
48 #define CAMSS_CSID_TG_VC_CFG_V_BLANKING 0x7f
49 #define CAMSS_CSID_TG_DT_n_CGG_0(n) (0x0ac + 0xc * (n))
50 #define CAMSS_CSID_TG_DT_n_CGG_1(n) (0x0b0 + 0xc * (n))
51 #define CAMSS_CSID_TG_DT_n_CGG_2(n) (0x0b4 + 0xc * (n))
52
53 #define DATA_TYPE_EMBEDDED_DATA_8BIT 0x12
54 #define DATA_TYPE_YUV422_8BIT 0x1e
55 #define DATA_TYPE_RAW_6BIT 0x28
56 #define DATA_TYPE_RAW_8BIT 0x2a
57 #define DATA_TYPE_RAW_10BIT 0x2b
58 #define DATA_TYPE_RAW_12BIT 0x2c
59
60 #define DECODE_FORMAT_UNCOMPRESSED_6_BIT 0x0
61 #define DECODE_FORMAT_UNCOMPRESSED_8_BIT 0x1
62 #define DECODE_FORMAT_UNCOMPRESSED_10_BIT 0x2
63 #define DECODE_FORMAT_UNCOMPRESSED_12_BIT 0x3
64
65 #define CSID_RESET_TIMEOUT_MS 500
66
67 struct csid_fmts {
68 u32 code;
69 u8 data_type;
70 u8 decode_format;
71 u8 bpp;
72 u8 spp; /* bus samples per pixel */
73 };
74
75 static const struct csid_fmts csid_input_fmts[] = {
76 {
77 MEDIA_BUS_FMT_UYVY8_2X8,
78 DATA_TYPE_YUV422_8BIT,
79 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
80 8,
81 2,
82 },
83 {
84 MEDIA_BUS_FMT_VYUY8_2X8,
85 DATA_TYPE_YUV422_8BIT,
86 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
87 8,
88 2,
89 },
90 {
91 MEDIA_BUS_FMT_YUYV8_2X8,
92 DATA_TYPE_YUV422_8BIT,
93 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
94 8,
95 2,
96 },
97 {
98 MEDIA_BUS_FMT_YVYU8_2X8,
99 DATA_TYPE_YUV422_8BIT,
100 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
101 8,
102 2,
103 },
104 {
105 MEDIA_BUS_FMT_SBGGR8_1X8,
106 DATA_TYPE_RAW_8BIT,
107 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
108 8,
109 1,
110 },
111 {
112 MEDIA_BUS_FMT_SGBRG8_1X8,
113 DATA_TYPE_RAW_8BIT,
114 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
115 8,
116 1,
117 },
118 {
119 MEDIA_BUS_FMT_SGRBG8_1X8,
120 DATA_TYPE_RAW_8BIT,
121 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
122 8,
123 1,
124 },
125 {
126 MEDIA_BUS_FMT_SRGGB8_1X8,
127 DATA_TYPE_RAW_8BIT,
128 DECODE_FORMAT_UNCOMPRESSED_8_BIT,
129 8,
130 1,
131 },
132 {
133 MEDIA_BUS_FMT_SBGGR10_1X10,
134 DATA_TYPE_RAW_10BIT,
135 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
136 10,
137 1,
138 },
139 {
140 MEDIA_BUS_FMT_SGBRG10_1X10,
141 DATA_TYPE_RAW_10BIT,
142 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
143 10,
144 1,
145 },
146 {
147 MEDIA_BUS_FMT_SGRBG10_1X10,
148 DATA_TYPE_RAW_10BIT,
149 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
150 10,
151 1,
152 },
153 {
154 MEDIA_BUS_FMT_SRGGB10_1X10,
155 DATA_TYPE_RAW_10BIT,
156 DECODE_FORMAT_UNCOMPRESSED_10_BIT,
157 10,
158 1,
159 },
160 {
161 MEDIA_BUS_FMT_SBGGR12_1X12,
162 DATA_TYPE_RAW_12BIT,
163 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
164 12,
165 1,
166 },
167 {
168 MEDIA_BUS_FMT_SGBRG12_1X12,
169 DATA_TYPE_RAW_12BIT,
170 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
171 12,
172 1,
173 },
174 {
175 MEDIA_BUS_FMT_SGRBG12_1X12,
176 DATA_TYPE_RAW_12BIT,
177 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
178 12,
179 1,
180 },
181 {
182 MEDIA_BUS_FMT_SRGGB12_1X12,
183 DATA_TYPE_RAW_12BIT,
184 DECODE_FORMAT_UNCOMPRESSED_12_BIT,
185 12,
186 1,
187 }
188 };
189
csid_get_fmt_entry(u32 code)190 static const struct csid_fmts *csid_get_fmt_entry(u32 code)
191 {
192 unsigned int i;
193
194 for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
195 if (code == csid_input_fmts[i].code)
196 return &csid_input_fmts[i];
197
198 WARN(1, "Unknown format\n");
199
200 return &csid_input_fmts[0];
201 }
202
203 /*
204 * csid_isr - CSID module interrupt handler
205 * @irq: Interrupt line
206 * @dev: CSID device
207 *
208 * Return IRQ_HANDLED on success
209 */
csid_isr(int irq,void * dev)210 static irqreturn_t csid_isr(int irq, void *dev)
211 {
212 struct csid_device *csid = dev;
213 u32 value;
214
215 value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS);
216 writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD);
217
218 if ((value >> 11) & 0x1)
219 complete(&csid->reset_complete);
220
221 return IRQ_HANDLED;
222 }
223
224 /*
225 * csid_set_clock_rates - Calculate and set clock rates on CSID module
226 * @csiphy: CSID device
227 */
csid_set_clock_rates(struct csid_device * csid)228 static int csid_set_clock_rates(struct csid_device *csid)
229 {
230 struct device *dev = to_device_index(csid, csid->id);
231 u32 pixel_clock;
232 int i, j;
233 int ret;
234
235 ret = camss_get_pixel_clock(&csid->subdev.entity, &pixel_clock);
236 if (ret)
237 pixel_clock = 0;
238
239 for (i = 0; i < csid->nclocks; i++) {
240 struct camss_clock *clock = &csid->clock[i];
241
242 if (!strcmp(clock->name, "csi0") ||
243 !strcmp(clock->name, "csi1")) {
244 u8 bpp = csid_get_fmt_entry(
245 csid->fmt[MSM_CSIPHY_PAD_SINK].code)->bpp;
246 u8 num_lanes = csid->phy.lane_cnt;
247 u64 min_rate = pixel_clock * bpp / (2 * num_lanes * 4);
248 long rate;
249
250 camss_add_clock_margin(&min_rate);
251
252 for (j = 0; j < clock->nfreqs; j++)
253 if (min_rate < clock->freq[j])
254 break;
255
256 if (j == clock->nfreqs) {
257 dev_err(dev,
258 "Pixel clock is too high for CSID\n");
259 return -EINVAL;
260 }
261
262 /* if sensor pixel clock is not available */
263 /* set highest possible CSID clock rate */
264 if (min_rate == 0)
265 j = clock->nfreqs - 1;
266
267 rate = clk_round_rate(clock->clk, clock->freq[j]);
268 if (rate < 0) {
269 dev_err(dev, "clk round rate failed: %ld\n",
270 rate);
271 return -EINVAL;
272 }
273
274 ret = clk_set_rate(clock->clk, rate);
275 if (ret < 0) {
276 dev_err(dev, "clk set rate failed: %d\n", ret);
277 return ret;
278 }
279 }
280 }
281
282 return 0;
283 }
284
285 /*
286 * csid_reset - Trigger reset on CSID module and wait to complete
287 * @csid: CSID device
288 *
289 * Return 0 on success or a negative error code otherwise
290 */
csid_reset(struct csid_device * csid)291 static int csid_reset(struct csid_device *csid)
292 {
293 unsigned long time;
294
295 reinit_completion(&csid->reset_complete);
296
297 writel_relaxed(0x7fff, csid->base + CAMSS_CSID_RST_CMD);
298
299 time = wait_for_completion_timeout(&csid->reset_complete,
300 msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
301 if (!time) {
302 dev_err(to_device_index(csid, csid->id),
303 "CSID reset timeout\n");
304 return -EIO;
305 }
306
307 return 0;
308 }
309
310 /*
311 * csid_set_power - Power on/off CSID module
312 * @sd: CSID V4L2 subdevice
313 * @on: Requested power state
314 *
315 * Return 0 on success or a negative error code otherwise
316 */
csid_set_power(struct v4l2_subdev * sd,int on)317 static int csid_set_power(struct v4l2_subdev *sd, int on)
318 {
319 struct csid_device *csid = v4l2_get_subdevdata(sd);
320 struct device *dev = to_device_index(csid, csid->id);
321 int ret;
322
323 if (on) {
324 u32 hw_version;
325
326 ret = regulator_enable(csid->vdda);
327 if (ret < 0)
328 return ret;
329
330 ret = csid_set_clock_rates(csid);
331 if (ret < 0) {
332 regulator_disable(csid->vdda);
333 return ret;
334 }
335
336 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
337 if (ret < 0) {
338 regulator_disable(csid->vdda);
339 return ret;
340 }
341
342 enable_irq(csid->irq);
343
344 ret = csid_reset(csid);
345 if (ret < 0) {
346 disable_irq(csid->irq);
347 camss_disable_clocks(csid->nclocks, csid->clock);
348 regulator_disable(csid->vdda);
349 return ret;
350 }
351
352 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
353 dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version);
354 } else {
355 disable_irq(csid->irq);
356 camss_disable_clocks(csid->nclocks, csid->clock);
357 ret = regulator_disable(csid->vdda);
358 }
359
360 return ret;
361 }
362
363 /*
364 * csid_set_stream - Enable/disable streaming on CSID module
365 * @sd: CSID V4L2 subdevice
366 * @enable: Requested streaming state
367 *
368 * Main configuration of CSID module is also done here.
369 *
370 * Return 0 on success or a negative error code otherwise
371 */
csid_set_stream(struct v4l2_subdev * sd,int enable)372 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
373 {
374 struct csid_device *csid = v4l2_get_subdevdata(sd);
375 struct csid_testgen_config *tg = &csid->testgen;
376 u32 val;
377
378 if (enable) {
379 u8 vc = 0; /* Virtual Channel 0 */
380 u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
381 u8 dt, dt_shift, df;
382 int ret;
383
384 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
385 if (ret < 0) {
386 dev_err(to_device_index(csid, csid->id),
387 "could not sync v4l2 controls: %d\n", ret);
388 return ret;
389 }
390
391 if (!tg->enabled &&
392 !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
393 return -ENOLINK;
394
395 if (tg->enabled) {
396 /* Config Test Generator */
397 struct v4l2_mbus_framefmt *f =
398 &csid->fmt[MSM_CSID_PAD_SRC];
399 u8 bpp = csid_get_fmt_entry(f->code)->bpp;
400 u8 spp = csid_get_fmt_entry(f->code)->spp;
401 u32 num_bytes_per_line = f->width * bpp * spp / 8;
402 u32 num_lines = f->height;
403
404 /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */
405 /* 1:0 VC */
406 val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) |
407 ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13);
408 writel_relaxed(val, csid->base + CAMSS_CSID_TG_VC_CFG);
409
410 /* 28:16 bytes per lines, 12:0 num of lines */
411 val = ((num_bytes_per_line & 0x1fff) << 16) |
412 (num_lines & 0x1fff);
413 writel_relaxed(val, csid->base +
414 CAMSS_CSID_TG_DT_n_CGG_0(0));
415
416 dt = csid_get_fmt_entry(
417 csid->fmt[MSM_CSID_PAD_SRC].code)->data_type;
418
419 /* 5:0 data type */
420 val = dt;
421 writel_relaxed(val, csid->base +
422 CAMSS_CSID_TG_DT_n_CGG_1(0));
423
424 /* 2:0 output test pattern */
425 val = tg->payload_mode;
426 writel_relaxed(val, csid->base +
427 CAMSS_CSID_TG_DT_n_CGG_2(0));
428
429 df = csid_get_fmt_entry(
430 csid->fmt[MSM_CSID_PAD_SRC].code)->decode_format;
431 } else {
432 struct csid_phy_config *phy = &csid->phy;
433
434 val = phy->lane_cnt - 1;
435 val |= phy->lane_assign << 4;
436
437 writel_relaxed(val,
438 csid->base + CAMSS_CSID_CORE_CTRL_0);
439
440 val = phy->csiphy_id << 17;
441 val |= 0x9;
442
443 writel_relaxed(val,
444 csid->base + CAMSS_CSID_CORE_CTRL_1);
445
446 dt = csid_get_fmt_entry(
447 csid->fmt[MSM_CSID_PAD_SINK].code)->data_type;
448 df = csid_get_fmt_entry(
449 csid->fmt[MSM_CSID_PAD_SINK].code)->decode_format;
450 }
451
452 /* Config LUT */
453
454 dt_shift = (cid % 4) * 8;
455
456 val = readl_relaxed(csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
457 val &= ~(0xff << dt_shift);
458 val |= dt << dt_shift;
459 writel_relaxed(val, csid->base + CAMSS_CSID_CID_LUT_VC_n(vc));
460
461 val = (df << 4) | 0x3;
462 writel_relaxed(val, csid->base + CAMSS_CSID_CID_n_CFG(cid));
463
464 if (tg->enabled) {
465 val = CAMSS_CSID_TG_CTRL_ENABLE;
466 writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
467 }
468 } else {
469 if (tg->enabled) {
470 val = CAMSS_CSID_TG_CTRL_DISABLE;
471 writel_relaxed(val, csid->base + CAMSS_CSID_TG_CTRL);
472 }
473 }
474
475 return 0;
476 }
477
478 /*
479 * __csid_get_format - Get pointer to format structure
480 * @csid: CSID device
481 * @cfg: V4L2 subdev pad configuration
482 * @pad: pad from which format is requested
483 * @which: TRY or ACTIVE format
484 *
485 * Return pointer to TRY or ACTIVE format structure
486 */
487 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)488 __csid_get_format(struct csid_device *csid,
489 struct v4l2_subdev_pad_config *cfg,
490 unsigned int pad,
491 enum v4l2_subdev_format_whence which)
492 {
493 if (which == V4L2_SUBDEV_FORMAT_TRY)
494 return v4l2_subdev_get_try_format(&csid->subdev, cfg, pad);
495
496 return &csid->fmt[pad];
497 }
498
499 /*
500 * csid_try_format - Handle try format by pad subdev method
501 * @csid: CSID device
502 * @cfg: V4L2 subdev pad configuration
503 * @pad: pad on which format is requested
504 * @fmt: pointer to v4l2 format structure
505 * @which: wanted subdev format
506 */
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)507 static void csid_try_format(struct csid_device *csid,
508 struct v4l2_subdev_pad_config *cfg,
509 unsigned int pad,
510 struct v4l2_mbus_framefmt *fmt,
511 enum v4l2_subdev_format_whence which)
512 {
513 unsigned int i;
514
515 switch (pad) {
516 case MSM_CSID_PAD_SINK:
517 /* Set format on sink pad */
518
519 for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
520 if (fmt->code == csid_input_fmts[i].code)
521 break;
522
523 /* If not found, use UYVY as default */
524 if (i >= ARRAY_SIZE(csid_input_fmts))
525 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
526
527 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
528 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
529
530 fmt->field = V4L2_FIELD_NONE;
531 fmt->colorspace = V4L2_COLORSPACE_SRGB;
532
533 break;
534
535 case MSM_CSID_PAD_SRC:
536 if (csid->testgen_mode->cur.val == 0) {
537 /* Test generator is disabled, keep pad formats */
538 /* in sync - set and return a format same as sink pad */
539 struct v4l2_mbus_framefmt format;
540
541 format = *__csid_get_format(csid, cfg,
542 MSM_CSID_PAD_SINK, which);
543 *fmt = format;
544 } else {
545 /* Test generator is enabled, set format on source*/
546 /* pad to allow test generator usage */
547
548 for (i = 0; i < ARRAY_SIZE(csid_input_fmts); i++)
549 if (csid_input_fmts[i].code == fmt->code)
550 break;
551
552 /* If not found, use UYVY as default */
553 if (i >= ARRAY_SIZE(csid_input_fmts))
554 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
555
556 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
557 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
558
559 fmt->field = V4L2_FIELD_NONE;
560 }
561 break;
562 }
563
564 fmt->colorspace = V4L2_COLORSPACE_SRGB;
565 }
566
567 /*
568 * csid_enum_mbus_code - Handle pixel format enumeration
569 * @sd: CSID V4L2 subdevice
570 * @cfg: V4L2 subdev pad configuration
571 * @code: pointer to v4l2_subdev_mbus_code_enum structure
572 * return -EINVAL or zero on success
573 */
csid_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_mbus_code_enum * code)574 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
575 struct v4l2_subdev_pad_config *cfg,
576 struct v4l2_subdev_mbus_code_enum *code)
577 {
578 struct csid_device *csid = v4l2_get_subdevdata(sd);
579 struct v4l2_mbus_framefmt *format;
580
581 if (code->pad == MSM_CSID_PAD_SINK) {
582 if (code->index >= ARRAY_SIZE(csid_input_fmts))
583 return -EINVAL;
584
585 code->code = csid_input_fmts[code->index].code;
586 } else {
587 if (csid->testgen_mode->cur.val == 0) {
588 if (code->index > 0)
589 return -EINVAL;
590
591 format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SINK,
592 code->which);
593
594 code->code = format->code;
595 } else {
596 if (code->index >= ARRAY_SIZE(csid_input_fmts))
597 return -EINVAL;
598
599 code->code = csid_input_fmts[code->index].code;
600 }
601 }
602
603 return 0;
604 }
605
606 /*
607 * csid_enum_frame_size - Handle frame size enumeration
608 * @sd: CSID V4L2 subdevice
609 * @cfg: V4L2 subdev pad configuration
610 * @fse: pointer to v4l2_subdev_frame_size_enum structure
611 * return -EINVAL or zero on success
612 */
csid_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_frame_size_enum * fse)613 static int csid_enum_frame_size(struct v4l2_subdev *sd,
614 struct v4l2_subdev_pad_config *cfg,
615 struct v4l2_subdev_frame_size_enum *fse)
616 {
617 struct csid_device *csid = v4l2_get_subdevdata(sd);
618 struct v4l2_mbus_framefmt format;
619
620 if (fse->index != 0)
621 return -EINVAL;
622
623 format.code = fse->code;
624 format.width = 1;
625 format.height = 1;
626 csid_try_format(csid, cfg, fse->pad, &format, fse->which);
627 fse->min_width = format.width;
628 fse->min_height = format.height;
629
630 if (format.code != fse->code)
631 return -EINVAL;
632
633 format.code = fse->code;
634 format.width = -1;
635 format.height = -1;
636 csid_try_format(csid, cfg, fse->pad, &format, fse->which);
637 fse->max_width = format.width;
638 fse->max_height = format.height;
639
640 return 0;
641 }
642
643 /*
644 * csid_get_format - Handle get format by pads subdev method
645 * @sd: CSID V4L2 subdevice
646 * @cfg: V4L2 subdev pad configuration
647 * @fmt: pointer to v4l2 subdev format structure
648 *
649 * Return -EINVAL or zero on success
650 */
csid_get_format(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)651 static int csid_get_format(struct v4l2_subdev *sd,
652 struct v4l2_subdev_pad_config *cfg,
653 struct v4l2_subdev_format *fmt)
654 {
655 struct csid_device *csid = v4l2_get_subdevdata(sd);
656 struct v4l2_mbus_framefmt *format;
657
658 format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
659 if (format == NULL)
660 return -EINVAL;
661
662 fmt->format = *format;
663
664 return 0;
665 }
666
667 /*
668 * csid_set_format - Handle set format by pads subdev method
669 * @sd: CSID V4L2 subdevice
670 * @cfg: V4L2 subdev pad configuration
671 * @fmt: pointer to v4l2 subdev format structure
672 *
673 * Return -EINVAL or zero on success
674 */
csid_set_format(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)675 static int csid_set_format(struct v4l2_subdev *sd,
676 struct v4l2_subdev_pad_config *cfg,
677 struct v4l2_subdev_format *fmt)
678 {
679 struct csid_device *csid = v4l2_get_subdevdata(sd);
680 struct v4l2_mbus_framefmt *format;
681
682 format = __csid_get_format(csid, cfg, fmt->pad, fmt->which);
683 if (format == NULL)
684 return -EINVAL;
685
686 csid_try_format(csid, cfg, fmt->pad, &fmt->format, fmt->which);
687 *format = fmt->format;
688
689 /* Propagate the format from sink to source */
690 if (fmt->pad == MSM_CSID_PAD_SINK) {
691 format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SRC,
692 fmt->which);
693
694 *format = fmt->format;
695 csid_try_format(csid, cfg, MSM_CSID_PAD_SRC, format,
696 fmt->which);
697 }
698
699 return 0;
700 }
701
702 /*
703 * csid_init_formats - Initialize formats on all pads
704 * @sd: CSID V4L2 subdevice
705 * @fh: V4L2 subdev file handle
706 *
707 * Initialize all pad formats with default values.
708 *
709 * Return 0 on success or a negative error code otherwise
710 */
csid_init_formats(struct v4l2_subdev * sd,struct v4l2_subdev_fh * fh)711 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
712 {
713 struct v4l2_subdev_format format = {
714 .pad = MSM_CSID_PAD_SINK,
715 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
716 V4L2_SUBDEV_FORMAT_ACTIVE,
717 .format = {
718 .code = MEDIA_BUS_FMT_UYVY8_2X8,
719 .width = 1920,
720 .height = 1080
721 }
722 };
723
724 return csid_set_format(sd, fh ? fh->pad : NULL, &format);
725 }
726
727 static const char * const csid_test_pattern_menu[] = {
728 "Disabled",
729 "Incrementing",
730 "Alternating 0x55/0xAA",
731 "All Zeros 0x00",
732 "All Ones 0xFF",
733 "Pseudo-random Data",
734 };
735
736 /*
737 * csid_set_test_pattern - Set test generator's pattern mode
738 * @csid: CSID device
739 * @value: desired test pattern mode
740 *
741 * Return 0 on success or a negative error code otherwise
742 */
csid_set_test_pattern(struct csid_device * csid,s32 value)743 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
744 {
745 struct csid_testgen_config *tg = &csid->testgen;
746
747 /* If CSID is linked to CSIPHY, do not allow to enable test generator */
748 if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
749 return -EBUSY;
750
751 tg->enabled = !!value;
752
753 switch (value) {
754 case 1:
755 tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING;
756 break;
757 case 2:
758 tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA;
759 break;
760 case 3:
761 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES;
762 break;
763 case 4:
764 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES;
765 break;
766 case 5:
767 tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM;
768 break;
769 }
770
771 return 0;
772 }
773
774 /*
775 * csid_s_ctrl - Handle set control subdev method
776 * @ctrl: pointer to v4l2 control structure
777 *
778 * Return 0 on success or a negative error code otherwise
779 */
csid_s_ctrl(struct v4l2_ctrl * ctrl)780 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
781 {
782 struct csid_device *csid = container_of(ctrl->handler,
783 struct csid_device, ctrls);
784 int ret = -EINVAL;
785
786 switch (ctrl->id) {
787 case V4L2_CID_TEST_PATTERN:
788 ret = csid_set_test_pattern(csid, ctrl->val);
789 break;
790 }
791
792 return ret;
793 }
794
795 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
796 .s_ctrl = csid_s_ctrl,
797 };
798
799 /*
800 * msm_csid_subdev_init - Initialize CSID device structure and resources
801 * @csid: CSID device
802 * @res: CSID module resources table
803 * @id: CSID module id
804 *
805 * Return 0 on success or a negative error code otherwise
806 */
msm_csid_subdev_init(struct csid_device * csid,const struct resources * res,u8 id)807 int msm_csid_subdev_init(struct csid_device *csid,
808 const struct resources *res, u8 id)
809 {
810 struct device *dev = to_device_index(csid, id);
811 struct platform_device *pdev = to_platform_device(dev);
812 struct resource *r;
813 int i, j;
814 int ret;
815
816 csid->id = id;
817
818 /* Memory */
819
820 r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
821 csid->base = devm_ioremap_resource(dev, r);
822 if (IS_ERR(csid->base)) {
823 dev_err(dev, "could not map memory\n");
824 return PTR_ERR(csid->base);
825 }
826
827 /* Interrupt */
828
829 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
830 res->interrupt[0]);
831 if (!r) {
832 dev_err(dev, "missing IRQ\n");
833 return -EINVAL;
834 }
835
836 csid->irq = r->start;
837 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
838 dev_name(dev), MSM_CSID_NAME, csid->id);
839 ret = devm_request_irq(dev, csid->irq, csid_isr,
840 IRQF_TRIGGER_RISING, csid->irq_name, csid);
841 if (ret < 0) {
842 dev_err(dev, "request_irq failed: %d\n", ret);
843 return ret;
844 }
845
846 disable_irq(csid->irq);
847
848 /* Clocks */
849
850 csid->nclocks = 0;
851 while (res->clock[csid->nclocks])
852 csid->nclocks++;
853
854 csid->clock = devm_kzalloc(dev, csid->nclocks * sizeof(*csid->clock),
855 GFP_KERNEL);
856 if (!csid->clock)
857 return -ENOMEM;
858
859 for (i = 0; i < csid->nclocks; i++) {
860 struct camss_clock *clock = &csid->clock[i];
861
862 clock->clk = devm_clk_get(dev, res->clock[i]);
863 if (IS_ERR(clock->clk))
864 return PTR_ERR(clock->clk);
865
866 clock->name = res->clock[i];
867
868 clock->nfreqs = 0;
869 while (res->clock_rate[i][clock->nfreqs])
870 clock->nfreqs++;
871
872 if (!clock->nfreqs) {
873 clock->freq = NULL;
874 continue;
875 }
876
877 clock->freq = devm_kzalloc(dev, clock->nfreqs *
878 sizeof(*clock->freq), GFP_KERNEL);
879 if (!clock->freq)
880 return -ENOMEM;
881
882 for (j = 0; j < clock->nfreqs; j++)
883 clock->freq[j] = res->clock_rate[i][j];
884 }
885
886 /* Regulator */
887
888 csid->vdda = devm_regulator_get(dev, res->regulator[0]);
889 if (IS_ERR(csid->vdda)) {
890 dev_err(dev, "could not get regulator\n");
891 return PTR_ERR(csid->vdda);
892 }
893
894 init_completion(&csid->reset_complete);
895
896 return 0;
897 }
898
899 /*
900 * msm_csid_get_csid_id - Get CSID HW module id
901 * @entity: Pointer to CSID media entity structure
902 * @id: Return CSID HW module id here
903 */
msm_csid_get_csid_id(struct media_entity * entity,u8 * id)904 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
905 {
906 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
907 struct csid_device *csid = v4l2_get_subdevdata(sd);
908
909 *id = csid->id;
910 }
911
912 /*
913 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
914 * @lane_cfg - CSI2 lane configuration
915 *
916 * Return lane assign
917 */
csid_get_lane_assign(struct csiphy_lanes_cfg * lane_cfg)918 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
919 {
920 u32 lane_assign = 0;
921 int i;
922
923 for (i = 0; i < lane_cfg->num_data; i++)
924 lane_assign |= lane_cfg->data[i].pos << (i * 4);
925
926 return lane_assign;
927 }
928
929 /*
930 * csid_link_setup - Setup CSID connections
931 * @entity: Pointer to media entity structure
932 * @local: Pointer to local pad
933 * @remote: Pointer to remote pad
934 * @flags: Link flags
935 *
936 * Return 0 on success
937 */
csid_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)938 static int csid_link_setup(struct media_entity *entity,
939 const struct media_pad *local,
940 const struct media_pad *remote, u32 flags)
941 {
942 if (flags & MEDIA_LNK_FL_ENABLED)
943 if (media_entity_remote_pad(local))
944 return -EBUSY;
945
946 if ((local->flags & MEDIA_PAD_FL_SINK) &&
947 (flags & MEDIA_LNK_FL_ENABLED)) {
948 struct v4l2_subdev *sd;
949 struct csid_device *csid;
950 struct csiphy_device *csiphy;
951 struct csiphy_lanes_cfg *lane_cfg;
952 struct v4l2_subdev_format format = { 0 };
953
954 sd = media_entity_to_v4l2_subdev(entity);
955 csid = v4l2_get_subdevdata(sd);
956
957 /* If test generator is enabled */
958 /* do not allow a link from CSIPHY to CSID */
959 if (csid->testgen_mode->cur.val != 0)
960 return -EBUSY;
961
962 sd = media_entity_to_v4l2_subdev(remote->entity);
963 csiphy = v4l2_get_subdevdata(sd);
964
965 /* If a sensor is not linked to CSIPHY */
966 /* do no allow a link from CSIPHY to CSID */
967 if (!csiphy->cfg.csi2)
968 return -EPERM;
969
970 csid->phy.csiphy_id = csiphy->id;
971
972 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
973 csid->phy.lane_cnt = lane_cfg->num_data;
974 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
975
976 /* Reset format on source pad to sink pad format */
977 format.pad = MSM_CSID_PAD_SRC;
978 format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
979 csid_set_format(&csid->subdev, NULL, &format);
980 }
981
982 return 0;
983 }
984
985 static const struct v4l2_subdev_core_ops csid_core_ops = {
986 .s_power = csid_set_power,
987 };
988
989 static const struct v4l2_subdev_video_ops csid_video_ops = {
990 .s_stream = csid_set_stream,
991 };
992
993 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
994 .enum_mbus_code = csid_enum_mbus_code,
995 .enum_frame_size = csid_enum_frame_size,
996 .get_fmt = csid_get_format,
997 .set_fmt = csid_set_format,
998 };
999
1000 static const struct v4l2_subdev_ops csid_v4l2_ops = {
1001 .core = &csid_core_ops,
1002 .video = &csid_video_ops,
1003 .pad = &csid_pad_ops,
1004 };
1005
1006 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
1007 .open = csid_init_formats,
1008 };
1009
1010 static const struct media_entity_operations csid_media_ops = {
1011 .link_setup = csid_link_setup,
1012 .link_validate = v4l2_subdev_link_validate,
1013 };
1014
1015 /*
1016 * msm_csid_register_entity - Register subdev node for CSID module
1017 * @csid: CSID device
1018 * @v4l2_dev: V4L2 device
1019 *
1020 * Return 0 on success or a negative error code otherwise
1021 */
msm_csid_register_entity(struct csid_device * csid,struct v4l2_device * v4l2_dev)1022 int msm_csid_register_entity(struct csid_device *csid,
1023 struct v4l2_device *v4l2_dev)
1024 {
1025 struct v4l2_subdev *sd = &csid->subdev;
1026 struct media_pad *pads = csid->pads;
1027 struct device *dev = to_device_index(csid, csid->id);
1028 int ret;
1029
1030 v4l2_subdev_init(sd, &csid_v4l2_ops);
1031 sd->internal_ops = &csid_v4l2_internal_ops;
1032 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1033 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
1034 MSM_CSID_NAME, csid->id);
1035 v4l2_set_subdevdata(sd, csid);
1036
1037 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
1038 if (ret < 0) {
1039 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
1040 return ret;
1041 }
1042
1043 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
1044 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
1045 ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0,
1046 csid_test_pattern_menu);
1047
1048 if (csid->ctrls.error) {
1049 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
1050 ret = csid->ctrls.error;
1051 goto free_ctrl;
1052 }
1053
1054 csid->subdev.ctrl_handler = &csid->ctrls;
1055
1056 ret = csid_init_formats(sd, NULL);
1057 if (ret < 0) {
1058 dev_err(dev, "Failed to init format: %d\n", ret);
1059 goto free_ctrl;
1060 }
1061
1062 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1063 pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
1064
1065 sd->entity.function = MEDIA_ENT_F_IO_V4L;
1066 sd->entity.ops = &csid_media_ops;
1067 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
1068 if (ret < 0) {
1069 dev_err(dev, "Failed to init media entity: %d\n", ret);
1070 goto free_ctrl;
1071 }
1072
1073 ret = v4l2_device_register_subdev(v4l2_dev, sd);
1074 if (ret < 0) {
1075 dev_err(dev, "Failed to register subdev: %d\n", ret);
1076 goto media_cleanup;
1077 }
1078
1079 return 0;
1080
1081 media_cleanup:
1082 media_entity_cleanup(&sd->entity);
1083 free_ctrl:
1084 v4l2_ctrl_handler_free(&csid->ctrls);
1085
1086 return ret;
1087 }
1088
1089 /*
1090 * msm_csid_unregister_entity - Unregister CSID module subdev node
1091 * @csid: CSID device
1092 */
msm_csid_unregister_entity(struct csid_device * csid)1093 void msm_csid_unregister_entity(struct csid_device *csid)
1094 {
1095 v4l2_device_unregister_subdev(&csid->subdev);
1096 media_entity_cleanup(&csid->subdev.entity);
1097 v4l2_ctrl_handler_free(&csid->ctrls);
1098 }
1099