1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * camss-csid.c
4 *
5 * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
6 *
7 * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
8 * Copyright (C) 2015-2018 Linaro Ltd.
9 */
10 #include <linux/clk.h>
11 #include <linux/completion.h>
12 #include <linux/interrupt.h>
13 #include <linux/io.h>
14 #include <linux/kernel.h>
15 #include <linux/of.h>
16 #include <linux/platform_device.h>
17 #include <linux/pm_runtime.h>
18 #include <linux/regulator/consumer.h>
19 #include <media/media-entity.h>
20 #include <media/v4l2-device.h>
21 #include <media/v4l2-event.h>
22 #include <media/v4l2-subdev.h>
23
24 #include "camss-csid.h"
25 #include "camss-csid-gen1.h"
26 #include "camss.h"
27
28 /* offset of CSID registers in VFE region for VFE 480 */
29 #define VFE_480_CSID_OFFSET 0x1200
30 #define VFE_480_LITE_CSID_OFFSET 0x200
31
32 #define MSM_CSID_NAME "msm_csid"
33
34 const char * const csid_testgen_modes[] = {
35 "Disabled",
36 "Incrementing",
37 "Alternating 0x55/0xAA",
38 "All Zeros 0x00",
39 "All Ones 0xFF",
40 "Pseudo-random Data",
41 "User Specified",
42 "Complex pattern",
43 "Color box",
44 "Color bars",
45 NULL
46 };
47
csid_find_code(u32 * codes,unsigned int ncodes,unsigned int match_format_idx,u32 match_code)48 u32 csid_find_code(u32 *codes, unsigned int ncodes,
49 unsigned int match_format_idx, u32 match_code)
50 {
51 int i;
52
53 if (!match_code && (match_format_idx >= ncodes))
54 return 0;
55
56 for (i = 0; i < ncodes; i++)
57 if (match_code) {
58 if (codes[i] == match_code)
59 return match_code;
60 } else {
61 if (i == match_format_idx)
62 return codes[i];
63 }
64
65 return codes[0];
66 }
67
csid_get_fmt_entry(const struct csid_format * formats,unsigned int nformats,u32 code)68 const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
69 unsigned int nformats,
70 u32 code)
71 {
72 unsigned int i;
73
74 for (i = 0; i < nformats; i++)
75 if (code == formats[i].code)
76 return &formats[i];
77
78 WARN(1, "Unknown format\n");
79
80 return &formats[0];
81 }
82
83 /*
84 * csid_set_clock_rates - Calculate and set clock rates on CSID module
85 * @csiphy: CSID device
86 */
csid_set_clock_rates(struct csid_device * csid)87 static int csid_set_clock_rates(struct csid_device *csid)
88 {
89 struct device *dev = csid->camss->dev;
90 const struct csid_format *fmt;
91 s64 link_freq;
92 int i, j;
93 int ret;
94
95 fmt = csid_get_fmt_entry(csid->formats, csid->nformats,
96 csid->fmt[MSM_CSIPHY_PAD_SINK].code);
97 link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,
98 csid->phy.lane_cnt);
99 if (link_freq < 0)
100 link_freq = 0;
101
102 for (i = 0; i < csid->nclocks; i++) {
103 struct camss_clock *clock = &csid->clock[i];
104
105 if (!strcmp(clock->name, "csi0") ||
106 !strcmp(clock->name, "csi1") ||
107 !strcmp(clock->name, "csi2") ||
108 !strcmp(clock->name, "csi3")) {
109 u64 min_rate = link_freq / 4;
110 long rate;
111
112 camss_add_clock_margin(&min_rate);
113
114 for (j = 0; j < clock->nfreqs; j++)
115 if (min_rate < clock->freq[j])
116 break;
117
118 if (j == clock->nfreqs) {
119 dev_err(dev,
120 "Pixel clock is too high for CSID\n");
121 return -EINVAL;
122 }
123
124 /* if sensor pixel clock is not available */
125 /* set highest possible CSID clock rate */
126 if (min_rate == 0)
127 j = clock->nfreqs - 1;
128
129 rate = clk_round_rate(clock->clk, clock->freq[j]);
130 if (rate < 0) {
131 dev_err(dev, "clk round rate failed: %ld\n",
132 rate);
133 return -EINVAL;
134 }
135
136 ret = clk_set_rate(clock->clk, rate);
137 if (ret < 0) {
138 dev_err(dev, "clk set rate failed: %d\n", ret);
139 return ret;
140 }
141 } else if (clock->nfreqs) {
142 clk_set_rate(clock->clk, clock->freq[0]);
143 }
144 }
145
146 return 0;
147 }
148
149 /*
150 * csid_set_power - Power on/off CSID module
151 * @sd: CSID V4L2 subdevice
152 * @on: Requested power state
153 *
154 * Return 0 on success or a negative error code otherwise
155 */
csid_set_power(struct v4l2_subdev * sd,int on)156 static int csid_set_power(struct v4l2_subdev *sd, int on)
157 {
158 struct csid_device *csid = v4l2_get_subdevdata(sd);
159 struct camss *camss = csid->camss;
160 struct device *dev = camss->dev;
161 struct vfe_device *vfe = &camss->vfe[csid->id];
162 u32 version = camss->version;
163 int ret = 0;
164
165 if (on) {
166 if (version == CAMSS_8250 || version == CAMSS_845) {
167 ret = vfe_get(vfe);
168 if (ret < 0)
169 return ret;
170 }
171
172 ret = pm_runtime_resume_and_get(dev);
173 if (ret < 0)
174 return ret;
175
176 ret = regulator_bulk_enable(csid->num_supplies,
177 csid->supplies);
178 if (ret < 0) {
179 pm_runtime_put_sync(dev);
180 return ret;
181 }
182
183 ret = csid_set_clock_rates(csid);
184 if (ret < 0) {
185 regulator_bulk_disable(csid->num_supplies,
186 csid->supplies);
187 pm_runtime_put_sync(dev);
188 return ret;
189 }
190
191 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
192 if (ret < 0) {
193 regulator_bulk_disable(csid->num_supplies,
194 csid->supplies);
195 pm_runtime_put_sync(dev);
196 return ret;
197 }
198
199 csid->phy.need_vc_update = true;
200
201 enable_irq(csid->irq);
202
203 ret = csid->ops->reset(csid);
204 if (ret < 0) {
205 disable_irq(csid->irq);
206 camss_disable_clocks(csid->nclocks, csid->clock);
207 regulator_bulk_disable(csid->num_supplies,
208 csid->supplies);
209 pm_runtime_put_sync(dev);
210 return ret;
211 }
212
213 csid->ops->hw_version(csid);
214 } else {
215 disable_irq(csid->irq);
216 camss_disable_clocks(csid->nclocks, csid->clock);
217 regulator_bulk_disable(csid->num_supplies,
218 csid->supplies);
219 pm_runtime_put_sync(dev);
220 if (version == CAMSS_8250 || version == CAMSS_845)
221 vfe_put(vfe);
222 }
223
224 return ret;
225 }
226
227 /*
228 * csid_set_stream - Enable/disable streaming on CSID module
229 * @sd: CSID V4L2 subdevice
230 * @enable: Requested streaming state
231 *
232 * Main configuration of CSID module is also done here.
233 *
234 * Return 0 on success or a negative error code otherwise
235 */
csid_set_stream(struct v4l2_subdev * sd,int enable)236 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
237 {
238 struct csid_device *csid = v4l2_get_subdevdata(sd);
239 int ret;
240
241 if (enable) {
242 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
243 if (ret < 0) {
244 dev_err(csid->camss->dev,
245 "could not sync v4l2 controls: %d\n", ret);
246 return ret;
247 }
248
249 if (!csid->testgen.enabled &&
250 !media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]))
251 return -ENOLINK;
252 }
253
254 if (csid->phy.need_vc_update) {
255 csid->ops->configure_stream(csid, enable);
256 csid->phy.need_vc_update = false;
257 }
258
259 return 0;
260 }
261
262 /*
263 * __csid_get_format - Get pointer to format structure
264 * @csid: CSID device
265 * @cfg: V4L2 subdev pad configuration
266 * @pad: pad from which format is requested
267 * @which: TRY or ACTIVE format
268 *
269 * Return pointer to TRY or ACTIVE format structure
270 */
271 static struct v4l2_mbus_framefmt *
__csid_get_format(struct csid_device * csid,struct v4l2_subdev_state * sd_state,unsigned int pad,enum v4l2_subdev_format_whence which)272 __csid_get_format(struct csid_device *csid,
273 struct v4l2_subdev_state *sd_state,
274 unsigned int pad,
275 enum v4l2_subdev_format_whence which)
276 {
277 if (which == V4L2_SUBDEV_FORMAT_TRY)
278 return v4l2_subdev_get_try_format(&csid->subdev, sd_state,
279 pad);
280
281 return &csid->fmt[pad];
282 }
283
284 /*
285 * csid_try_format - Handle try format by pad subdev method
286 * @csid: CSID device
287 * @cfg: V4L2 subdev pad configuration
288 * @pad: pad on which format is requested
289 * @fmt: pointer to v4l2 format structure
290 * @which: wanted subdev format
291 */
csid_try_format(struct csid_device * csid,struct v4l2_subdev_state * sd_state,unsigned int pad,struct v4l2_mbus_framefmt * fmt,enum v4l2_subdev_format_whence which)292 static void csid_try_format(struct csid_device *csid,
293 struct v4l2_subdev_state *sd_state,
294 unsigned int pad,
295 struct v4l2_mbus_framefmt *fmt,
296 enum v4l2_subdev_format_whence which)
297 {
298 unsigned int i;
299
300 switch (pad) {
301 case MSM_CSID_PAD_SINK:
302 /* Set format on sink pad */
303
304 for (i = 0; i < csid->nformats; i++)
305 if (fmt->code == csid->formats[i].code)
306 break;
307
308 /* If not found, use UYVY as default */
309 if (i >= csid->nformats)
310 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
311
312 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
313 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
314
315 fmt->field = V4L2_FIELD_NONE;
316 fmt->colorspace = V4L2_COLORSPACE_SRGB;
317
318 break;
319
320 case MSM_CSID_PAD_SRC:
321 if (csid->testgen_mode->cur.val == 0) {
322 /* Test generator is disabled, */
323 /* keep pad formats in sync */
324 u32 code = fmt->code;
325
326 *fmt = *__csid_get_format(csid, sd_state,
327 MSM_CSID_PAD_SINK, which);
328 fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
329 } else {
330 /* Test generator is enabled, set format on source */
331 /* pad to allow test generator usage */
332
333 for (i = 0; i < csid->nformats; i++)
334 if (csid->formats[i].code == fmt->code)
335 break;
336
337 /* If not found, use UYVY as default */
338 if (i >= csid->nformats)
339 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
340
341 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
342 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
343
344 fmt->field = V4L2_FIELD_NONE;
345 }
346 break;
347 }
348
349 fmt->colorspace = V4L2_COLORSPACE_SRGB;
350 }
351
352 /*
353 * csid_enum_mbus_code - Handle pixel format enumeration
354 * @sd: CSID V4L2 subdevice
355 * @cfg: V4L2 subdev pad configuration
356 * @code: pointer to v4l2_subdev_mbus_code_enum structure
357 * return -EINVAL or zero on success
358 */
csid_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)359 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
360 struct v4l2_subdev_state *sd_state,
361 struct v4l2_subdev_mbus_code_enum *code)
362 {
363 struct csid_device *csid = v4l2_get_subdevdata(sd);
364
365 if (code->pad == MSM_CSID_PAD_SINK) {
366 if (code->index >= csid->nformats)
367 return -EINVAL;
368
369 code->code = csid->formats[code->index].code;
370 } else {
371 if (csid->testgen_mode->cur.val == 0) {
372 struct v4l2_mbus_framefmt *sink_fmt;
373
374 sink_fmt = __csid_get_format(csid, sd_state,
375 MSM_CSID_PAD_SINK,
376 code->which);
377
378 code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
379 code->index, 0);
380 if (!code->code)
381 return -EINVAL;
382 } else {
383 if (code->index >= csid->nformats)
384 return -EINVAL;
385
386 code->code = csid->formats[code->index].code;
387 }
388 }
389
390 return 0;
391 }
392
393 /*
394 * csid_enum_frame_size - Handle frame size enumeration
395 * @sd: CSID V4L2 subdevice
396 * @cfg: V4L2 subdev pad configuration
397 * @fse: pointer to v4l2_subdev_frame_size_enum structure
398 * return -EINVAL or zero on success
399 */
csid_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)400 static int csid_enum_frame_size(struct v4l2_subdev *sd,
401 struct v4l2_subdev_state *sd_state,
402 struct v4l2_subdev_frame_size_enum *fse)
403 {
404 struct csid_device *csid = v4l2_get_subdevdata(sd);
405 struct v4l2_mbus_framefmt format;
406
407 if (fse->index != 0)
408 return -EINVAL;
409
410 format.code = fse->code;
411 format.width = 1;
412 format.height = 1;
413 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
414 fse->min_width = format.width;
415 fse->min_height = format.height;
416
417 if (format.code != fse->code)
418 return -EINVAL;
419
420 format.code = fse->code;
421 format.width = -1;
422 format.height = -1;
423 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
424 fse->max_width = format.width;
425 fse->max_height = format.height;
426
427 return 0;
428 }
429
430 /*
431 * csid_get_format - Handle get format by pads subdev method
432 * @sd: CSID V4L2 subdevice
433 * @cfg: V4L2 subdev pad configuration
434 * @fmt: pointer to v4l2 subdev format structure
435 *
436 * Return -EINVAL or zero on success
437 */
csid_get_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)438 static int csid_get_format(struct v4l2_subdev *sd,
439 struct v4l2_subdev_state *sd_state,
440 struct v4l2_subdev_format *fmt)
441 {
442 struct csid_device *csid = v4l2_get_subdevdata(sd);
443 struct v4l2_mbus_framefmt *format;
444
445 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
446 if (format == NULL)
447 return -EINVAL;
448
449 fmt->format = *format;
450
451 return 0;
452 }
453
454 /*
455 * csid_set_format - Handle set format by pads subdev method
456 * @sd: CSID V4L2 subdevice
457 * @cfg: V4L2 subdev pad configuration
458 * @fmt: pointer to v4l2 subdev format structure
459 *
460 * Return -EINVAL or zero on success
461 */
csid_set_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)462 static int csid_set_format(struct v4l2_subdev *sd,
463 struct v4l2_subdev_state *sd_state,
464 struct v4l2_subdev_format *fmt)
465 {
466 struct csid_device *csid = v4l2_get_subdevdata(sd);
467 struct v4l2_mbus_framefmt *format;
468 int i;
469
470 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
471 if (format == NULL)
472 return -EINVAL;
473
474 csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which);
475 *format = fmt->format;
476
477 /* Propagate the format from sink to source pads */
478 if (fmt->pad == MSM_CSID_PAD_SINK) {
479 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i) {
480 format = __csid_get_format(csid, sd_state, i, fmt->which);
481
482 *format = fmt->format;
483 csid_try_format(csid, sd_state, i, format, fmt->which);
484 }
485 }
486
487 return 0;
488 }
489
490 /*
491 * csid_init_formats - Initialize formats on all pads
492 * @sd: CSID V4L2 subdevice
493 * @fh: V4L2 subdev file handle
494 *
495 * Initialize all pad formats with default values.
496 *
497 * Return 0 on success or a negative error code otherwise
498 */
csid_init_formats(struct v4l2_subdev * sd,struct v4l2_subdev_fh * fh)499 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
500 {
501 struct v4l2_subdev_format format = {
502 .pad = MSM_CSID_PAD_SINK,
503 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
504 V4L2_SUBDEV_FORMAT_ACTIVE,
505 .format = {
506 .code = MEDIA_BUS_FMT_UYVY8_2X8,
507 .width = 1920,
508 .height = 1080
509 }
510 };
511
512 return csid_set_format(sd, fh ? fh->state : NULL, &format);
513 }
514
515 /*
516 * csid_set_test_pattern - Set test generator's pattern mode
517 * @csid: CSID device
518 * @value: desired test pattern mode
519 *
520 * Return 0 on success or a negative error code otherwise
521 */
csid_set_test_pattern(struct csid_device * csid,s32 value)522 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
523 {
524 struct csid_testgen_config *tg = &csid->testgen;
525
526 /* If CSID is linked to CSIPHY, do not allow to enable test generator */
527 if (value && media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]))
528 return -EBUSY;
529
530 tg->enabled = !!value;
531
532 return csid->ops->configure_testgen_pattern(csid, value);
533 }
534
535 /*
536 * csid_s_ctrl - Handle set control subdev method
537 * @ctrl: pointer to v4l2 control structure
538 *
539 * Return 0 on success or a negative error code otherwise
540 */
csid_s_ctrl(struct v4l2_ctrl * ctrl)541 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
542 {
543 struct csid_device *csid = container_of(ctrl->handler,
544 struct csid_device, ctrls);
545 int ret = -EINVAL;
546
547 switch (ctrl->id) {
548 case V4L2_CID_TEST_PATTERN:
549 ret = csid_set_test_pattern(csid, ctrl->val);
550 break;
551 }
552
553 return ret;
554 }
555
556 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
557 .s_ctrl = csid_s_ctrl,
558 };
559
560 /*
561 * msm_csid_subdev_init - Initialize CSID device structure and resources
562 * @csid: CSID device
563 * @res: CSID module resources table
564 * @id: CSID module id
565 *
566 * Return 0 on success or a negative error code otherwise
567 */
msm_csid_subdev_init(struct camss * camss,struct csid_device * csid,const struct resources * res,u8 id)568 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
569 const struct resources *res, u8 id)
570 {
571 struct device *dev = camss->dev;
572 struct platform_device *pdev = to_platform_device(dev);
573 int i, j;
574 int ret;
575
576 csid->camss = camss;
577 csid->id = id;
578
579 if (camss->version == CAMSS_8x16) {
580 csid->ops = &csid_ops_4_1;
581 } else if (camss->version == CAMSS_8x96 ||
582 camss->version == CAMSS_660) {
583 csid->ops = &csid_ops_4_7;
584 } else if (camss->version == CAMSS_845 ||
585 camss->version == CAMSS_8250) {
586 csid->ops = &csid_ops_gen2;
587 } else {
588 return -EINVAL;
589 }
590 csid->ops->subdev_init(csid);
591
592 /* Memory */
593
594 if (camss->version == CAMSS_8250) {
595 /* for titan 480, CSID registers are inside the VFE region,
596 * between the VFE "top" and "bus" registers. this requires
597 * VFE to be initialized before CSID
598 */
599 if (id >= 2) /* VFE/CSID lite */
600 csid->base = camss->vfe[id].base + VFE_480_LITE_CSID_OFFSET;
601 else
602 csid->base = camss->vfe[id].base + VFE_480_CSID_OFFSET;
603 } else {
604 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
605 if (IS_ERR(csid->base))
606 return PTR_ERR(csid->base);
607 }
608
609 /* Interrupt */
610
611 ret = platform_get_irq_byname(pdev, res->interrupt[0]);
612 if (ret < 0)
613 return ret;
614
615 csid->irq = ret;
616 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
617 dev_name(dev), MSM_CSID_NAME, csid->id);
618 ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
619 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
620 csid->irq_name, csid);
621 if (ret < 0) {
622 dev_err(dev, "request_irq failed: %d\n", ret);
623 return ret;
624 }
625
626 /* Clocks */
627
628 csid->nclocks = 0;
629 while (res->clock[csid->nclocks])
630 csid->nclocks++;
631
632 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),
633 GFP_KERNEL);
634 if (!csid->clock)
635 return -ENOMEM;
636
637 for (i = 0; i < csid->nclocks; i++) {
638 struct camss_clock *clock = &csid->clock[i];
639
640 clock->clk = devm_clk_get(dev, res->clock[i]);
641 if (IS_ERR(clock->clk))
642 return PTR_ERR(clock->clk);
643
644 clock->name = res->clock[i];
645
646 clock->nfreqs = 0;
647 while (res->clock_rate[i][clock->nfreqs])
648 clock->nfreqs++;
649
650 if (!clock->nfreqs) {
651 clock->freq = NULL;
652 continue;
653 }
654
655 clock->freq = devm_kcalloc(dev,
656 clock->nfreqs,
657 sizeof(*clock->freq),
658 GFP_KERNEL);
659 if (!clock->freq)
660 return -ENOMEM;
661
662 for (j = 0; j < clock->nfreqs; j++)
663 clock->freq[j] = res->clock_rate[i][j];
664 }
665
666 /* Regulator */
667 for (i = 0; i < ARRAY_SIZE(res->regulators); i++) {
668 if (res->regulators[i])
669 csid->num_supplies++;
670 }
671
672 if (csid->num_supplies) {
673 csid->supplies = devm_kmalloc_array(camss->dev,
674 csid->num_supplies,
675 sizeof(*csid->supplies),
676 GFP_KERNEL);
677 if (!csid->supplies)
678 return -ENOMEM;
679 }
680
681 for (i = 0; i < csid->num_supplies; i++)
682 csid->supplies[i].supply = res->regulators[i];
683
684 ret = devm_regulator_bulk_get(camss->dev, csid->num_supplies,
685 csid->supplies);
686 if (ret)
687 return ret;
688
689 init_completion(&csid->reset_complete);
690
691 return 0;
692 }
693
694 /*
695 * msm_csid_get_csid_id - Get CSID HW module id
696 * @entity: Pointer to CSID media entity structure
697 * @id: Return CSID HW module id here
698 */
msm_csid_get_csid_id(struct media_entity * entity,u8 * id)699 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
700 {
701 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
702 struct csid_device *csid = v4l2_get_subdevdata(sd);
703
704 *id = csid->id;
705 }
706
707 /*
708 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
709 * @lane_cfg - CSI2 lane configuration
710 *
711 * Return lane assign
712 */
csid_get_lane_assign(struct csiphy_lanes_cfg * lane_cfg)713 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
714 {
715 u32 lane_assign = 0;
716 int i;
717
718 for (i = 0; i < lane_cfg->num_data; i++)
719 lane_assign |= lane_cfg->data[i].pos << (i * 4);
720
721 return lane_assign;
722 }
723
724 /*
725 * csid_link_setup - Setup CSID connections
726 * @entity: Pointer to media entity structure
727 * @local: Pointer to local pad
728 * @remote: Pointer to remote pad
729 * @flags: Link flags
730 *
731 * Return 0 on success
732 */
csid_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)733 static int csid_link_setup(struct media_entity *entity,
734 const struct media_pad *local,
735 const struct media_pad *remote, u32 flags)
736 {
737 if (flags & MEDIA_LNK_FL_ENABLED)
738 if (media_pad_remote_pad_first(local))
739 return -EBUSY;
740
741 if ((local->flags & MEDIA_PAD_FL_SINK) &&
742 (flags & MEDIA_LNK_FL_ENABLED)) {
743 struct v4l2_subdev *sd;
744 struct csid_device *csid;
745 struct csiphy_device *csiphy;
746 struct csiphy_lanes_cfg *lane_cfg;
747
748 sd = media_entity_to_v4l2_subdev(entity);
749 csid = v4l2_get_subdevdata(sd);
750
751 /* If test generator is enabled */
752 /* do not allow a link from CSIPHY to CSID */
753 if (csid->testgen_mode->cur.val != 0)
754 return -EBUSY;
755
756 sd = media_entity_to_v4l2_subdev(remote->entity);
757 csiphy = v4l2_get_subdevdata(sd);
758
759 /* If a sensor is not linked to CSIPHY */
760 /* do no allow a link from CSIPHY to CSID */
761 if (!csiphy->cfg.csi2)
762 return -EPERM;
763
764 csid->phy.csiphy_id = csiphy->id;
765
766 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
767 csid->phy.lane_cnt = lane_cfg->num_data;
768 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
769 }
770 /* Decide which virtual channels to enable based on which source pads are enabled */
771 if (local->flags & MEDIA_PAD_FL_SOURCE) {
772 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
773 struct csid_device *csid = v4l2_get_subdevdata(sd);
774 struct device *dev = csid->camss->dev;
775
776 if (flags & MEDIA_LNK_FL_ENABLED)
777 csid->phy.en_vc |= BIT(local->index - 1);
778 else
779 csid->phy.en_vc &= ~BIT(local->index - 1);
780
781 csid->phy.need_vc_update = true;
782
783 dev_dbg(dev, "%s: Enabled CSID virtual channels mask 0x%x\n",
784 __func__, csid->phy.en_vc);
785 }
786
787 return 0;
788 }
789
790 static const struct v4l2_subdev_core_ops csid_core_ops = {
791 .s_power = csid_set_power,
792 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
793 .unsubscribe_event = v4l2_event_subdev_unsubscribe,
794 };
795
796 static const struct v4l2_subdev_video_ops csid_video_ops = {
797 .s_stream = csid_set_stream,
798 };
799
800 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
801 .enum_mbus_code = csid_enum_mbus_code,
802 .enum_frame_size = csid_enum_frame_size,
803 .get_fmt = csid_get_format,
804 .set_fmt = csid_set_format,
805 };
806
807 static const struct v4l2_subdev_ops csid_v4l2_ops = {
808 .core = &csid_core_ops,
809 .video = &csid_video_ops,
810 .pad = &csid_pad_ops,
811 };
812
813 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
814 .open = csid_init_formats,
815 };
816
817 static const struct media_entity_operations csid_media_ops = {
818 .link_setup = csid_link_setup,
819 .link_validate = v4l2_subdev_link_validate,
820 };
821
822 /*
823 * msm_csid_register_entity - Register subdev node for CSID module
824 * @csid: CSID device
825 * @v4l2_dev: V4L2 device
826 *
827 * Return 0 on success or a negative error code otherwise
828 */
msm_csid_register_entity(struct csid_device * csid,struct v4l2_device * v4l2_dev)829 int msm_csid_register_entity(struct csid_device *csid,
830 struct v4l2_device *v4l2_dev)
831 {
832 struct v4l2_subdev *sd = &csid->subdev;
833 struct media_pad *pads = csid->pads;
834 struct device *dev = csid->camss->dev;
835 int i;
836 int ret;
837
838 v4l2_subdev_init(sd, &csid_v4l2_ops);
839 sd->internal_ops = &csid_v4l2_internal_ops;
840 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
841 V4L2_SUBDEV_FL_HAS_EVENTS;
842 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
843 MSM_CSID_NAME, csid->id);
844 v4l2_set_subdevdata(sd, csid);
845
846 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
847 if (ret < 0) {
848 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
849 return ret;
850 }
851
852 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
853 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
854 csid->testgen.nmodes, 0, 0,
855 csid->testgen.modes);
856
857 if (csid->ctrls.error) {
858 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
859 ret = csid->ctrls.error;
860 goto free_ctrl;
861 }
862
863 csid->subdev.ctrl_handler = &csid->ctrls;
864
865 ret = csid_init_formats(sd, NULL);
866 if (ret < 0) {
867 dev_err(dev, "Failed to init format: %d\n", ret);
868 goto free_ctrl;
869 }
870
871 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
872 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i)
873 pads[i].flags = MEDIA_PAD_FL_SOURCE;
874
875 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
876 sd->entity.ops = &csid_media_ops;
877 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
878 if (ret < 0) {
879 dev_err(dev, "Failed to init media entity: %d\n", ret);
880 goto free_ctrl;
881 }
882
883 ret = v4l2_device_register_subdev(v4l2_dev, sd);
884 if (ret < 0) {
885 dev_err(dev, "Failed to register subdev: %d\n", ret);
886 goto media_cleanup;
887 }
888
889 return 0;
890
891 media_cleanup:
892 media_entity_cleanup(&sd->entity);
893 free_ctrl:
894 v4l2_ctrl_handler_free(&csid->ctrls);
895
896 return ret;
897 }
898
899 /*
900 * msm_csid_unregister_entity - Unregister CSID module subdev node
901 * @csid: CSID device
902 */
msm_csid_unregister_entity(struct csid_device * csid)903 void msm_csid_unregister_entity(struct csid_device *csid)
904 {
905 v4l2_device_unregister_subdev(&csid->subdev);
906 media_entity_cleanup(&csid->subdev.entity);
907 v4l2_ctrl_handler_free(&csid->ctrls);
908 }
909
