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 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) {
243 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
244 if (ret < 0) {
245 dev_err(csid->camss->dev,
246 "could not sync v4l2 controls: %d\n", ret);
247 return ret;
248 }
249 }
250
251 if (!csid->testgen.enabled &&
252 !media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]))
253 return -ENOLINK;
254 }
255
256 if (csid->phy.need_vc_update) {
257 csid->ops->configure_stream(csid, enable);
258 csid->phy.need_vc_update = false;
259 }
260
261 return 0;
262 }
263
264 /*
265 * __csid_get_format - Get pointer to format structure
266 * @csid: CSID device
267 * @cfg: V4L2 subdev pad configuration
268 * @pad: pad from which format is requested
269 * @which: TRY or ACTIVE format
270 *
271 * Return pointer to TRY or ACTIVE format structure
272 */
273 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)274 __csid_get_format(struct csid_device *csid,
275 struct v4l2_subdev_state *sd_state,
276 unsigned int pad,
277 enum v4l2_subdev_format_whence which)
278 {
279 if (which == V4L2_SUBDEV_FORMAT_TRY)
280 return v4l2_subdev_get_try_format(&csid->subdev, sd_state,
281 pad);
282
283 return &csid->fmt[pad];
284 }
285
286 /*
287 * csid_try_format - Handle try format by pad subdev method
288 * @csid: CSID device
289 * @cfg: V4L2 subdev pad configuration
290 * @pad: pad on which format is requested
291 * @fmt: pointer to v4l2 format structure
292 * @which: wanted subdev format
293 */
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)294 static void csid_try_format(struct csid_device *csid,
295 struct v4l2_subdev_state *sd_state,
296 unsigned int pad,
297 struct v4l2_mbus_framefmt *fmt,
298 enum v4l2_subdev_format_whence which)
299 {
300 unsigned int i;
301
302 switch (pad) {
303 case MSM_CSID_PAD_SINK:
304 /* Set format on sink pad */
305
306 for (i = 0; i < csid->nformats; i++)
307 if (fmt->code == csid->formats[i].code)
308 break;
309
310 /* If not found, use UYVY as default */
311 if (i >= csid->nformats)
312 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
313
314 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
315 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
316
317 fmt->field = V4L2_FIELD_NONE;
318 fmt->colorspace = V4L2_COLORSPACE_SRGB;
319
320 break;
321
322 case MSM_CSID_PAD_SRC:
323 if (csid->testgen.nmodes == CSID_PAYLOAD_MODE_DISABLED ||
324 csid->testgen_mode->cur.val == 0) {
325 /* Test generator is disabled, */
326 /* keep pad formats in sync */
327 u32 code = fmt->code;
328
329 *fmt = *__csid_get_format(csid, sd_state,
330 MSM_CSID_PAD_SINK, which);
331 fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
332 } else {
333 /* Test generator is enabled, set format on source */
334 /* pad to allow test generator usage */
335
336 for (i = 0; i < csid->nformats; i++)
337 if (csid->formats[i].code == fmt->code)
338 break;
339
340 /* If not found, use UYVY as default */
341 if (i >= csid->nformats)
342 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
343
344 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
345 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
346
347 fmt->field = V4L2_FIELD_NONE;
348 }
349 break;
350 }
351
352 fmt->colorspace = V4L2_COLORSPACE_SRGB;
353 }
354
355 /*
356 * csid_enum_mbus_code - Handle pixel format enumeration
357 * @sd: CSID V4L2 subdevice
358 * @cfg: V4L2 subdev pad configuration
359 * @code: pointer to v4l2_subdev_mbus_code_enum structure
360 * return -EINVAL or zero on success
361 */
csid_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)362 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
363 struct v4l2_subdev_state *sd_state,
364 struct v4l2_subdev_mbus_code_enum *code)
365 {
366 struct csid_device *csid = v4l2_get_subdevdata(sd);
367
368 if (code->pad == MSM_CSID_PAD_SINK) {
369 if (code->index >= csid->nformats)
370 return -EINVAL;
371
372 code->code = csid->formats[code->index].code;
373 } else {
374 if (csid->testgen.nmodes == CSID_PAYLOAD_MODE_DISABLED ||
375 csid->testgen_mode->cur.val == 0) {
376 struct v4l2_mbus_framefmt *sink_fmt;
377
378 sink_fmt = __csid_get_format(csid, sd_state,
379 MSM_CSID_PAD_SINK,
380 code->which);
381
382 code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
383 code->index, 0);
384 if (!code->code)
385 return -EINVAL;
386 } else {
387 if (code->index >= csid->nformats)
388 return -EINVAL;
389
390 code->code = csid->formats[code->index].code;
391 }
392 }
393
394 return 0;
395 }
396
397 /*
398 * csid_enum_frame_size - Handle frame size enumeration
399 * @sd: CSID V4L2 subdevice
400 * @cfg: V4L2 subdev pad configuration
401 * @fse: pointer to v4l2_subdev_frame_size_enum structure
402 * return -EINVAL or zero on success
403 */
csid_enum_frame_size(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)404 static int csid_enum_frame_size(struct v4l2_subdev *sd,
405 struct v4l2_subdev_state *sd_state,
406 struct v4l2_subdev_frame_size_enum *fse)
407 {
408 struct csid_device *csid = v4l2_get_subdevdata(sd);
409 struct v4l2_mbus_framefmt format;
410
411 if (fse->index != 0)
412 return -EINVAL;
413
414 format.code = fse->code;
415 format.width = 1;
416 format.height = 1;
417 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
418 fse->min_width = format.width;
419 fse->min_height = format.height;
420
421 if (format.code != fse->code)
422 return -EINVAL;
423
424 format.code = fse->code;
425 format.width = -1;
426 format.height = -1;
427 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
428 fse->max_width = format.width;
429 fse->max_height = format.height;
430
431 return 0;
432 }
433
434 /*
435 * csid_get_format - Handle get format by pads subdev method
436 * @sd: CSID V4L2 subdevice
437 * @cfg: V4L2 subdev pad configuration
438 * @fmt: pointer to v4l2 subdev format structure
439 *
440 * Return -EINVAL or zero on success
441 */
csid_get_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)442 static int csid_get_format(struct v4l2_subdev *sd,
443 struct v4l2_subdev_state *sd_state,
444 struct v4l2_subdev_format *fmt)
445 {
446 struct csid_device *csid = v4l2_get_subdevdata(sd);
447 struct v4l2_mbus_framefmt *format;
448
449 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
450 if (format == NULL)
451 return -EINVAL;
452
453 fmt->format = *format;
454
455 return 0;
456 }
457
458 /*
459 * csid_set_format - Handle set format by pads subdev method
460 * @sd: CSID V4L2 subdevice
461 * @cfg: V4L2 subdev pad configuration
462 * @fmt: pointer to v4l2 subdev format structure
463 *
464 * Return -EINVAL or zero on success
465 */
csid_set_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)466 static int csid_set_format(struct v4l2_subdev *sd,
467 struct v4l2_subdev_state *sd_state,
468 struct v4l2_subdev_format *fmt)
469 {
470 struct csid_device *csid = v4l2_get_subdevdata(sd);
471 struct v4l2_mbus_framefmt *format;
472 int i;
473
474 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
475 if (format == NULL)
476 return -EINVAL;
477
478 csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which);
479 *format = fmt->format;
480
481 /* Propagate the format from sink to source pads */
482 if (fmt->pad == MSM_CSID_PAD_SINK) {
483 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i) {
484 format = __csid_get_format(csid, sd_state, i, fmt->which);
485
486 *format = fmt->format;
487 csid_try_format(csid, sd_state, i, format, fmt->which);
488 }
489 }
490
491 return 0;
492 }
493
494 /*
495 * csid_init_formats - Initialize formats on all pads
496 * @sd: CSID V4L2 subdevice
497 * @fh: V4L2 subdev file handle
498 *
499 * Initialize all pad formats with default values.
500 *
501 * Return 0 on success or a negative error code otherwise
502 */
csid_init_formats(struct v4l2_subdev * sd,struct v4l2_subdev_fh * fh)503 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
504 {
505 struct v4l2_subdev_format format = {
506 .pad = MSM_CSID_PAD_SINK,
507 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
508 V4L2_SUBDEV_FORMAT_ACTIVE,
509 .format = {
510 .code = MEDIA_BUS_FMT_UYVY8_2X8,
511 .width = 1920,
512 .height = 1080
513 }
514 };
515
516 return csid_set_format(sd, fh ? fh->state : NULL, &format);
517 }
518
519 /*
520 * csid_set_test_pattern - Set test generator's pattern mode
521 * @csid: CSID device
522 * @value: desired test pattern mode
523 *
524 * Return 0 on success or a negative error code otherwise
525 */
csid_set_test_pattern(struct csid_device * csid,s32 value)526 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
527 {
528 struct csid_testgen_config *tg = &csid->testgen;
529
530 /* If CSID is linked to CSIPHY, do not allow to enable test generator */
531 if (value && media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]))
532 return -EBUSY;
533
534 tg->enabled = !!value;
535
536 return csid->ops->configure_testgen_pattern(csid, value);
537 }
538
539 /*
540 * csid_s_ctrl - Handle set control subdev method
541 * @ctrl: pointer to v4l2 control structure
542 *
543 * Return 0 on success or a negative error code otherwise
544 */
csid_s_ctrl(struct v4l2_ctrl * ctrl)545 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
546 {
547 struct csid_device *csid = container_of(ctrl->handler,
548 struct csid_device, ctrls);
549 int ret = -EINVAL;
550
551 switch (ctrl->id) {
552 case V4L2_CID_TEST_PATTERN:
553 ret = csid_set_test_pattern(csid, ctrl->val);
554 break;
555 }
556
557 return ret;
558 }
559
560 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
561 .s_ctrl = csid_s_ctrl,
562 };
563
564 /*
565 * msm_csid_subdev_init - Initialize CSID device structure and resources
566 * @csid: CSID device
567 * @res: CSID module resources table
568 * @id: CSID module id
569 *
570 * Return 0 on success or a negative error code otherwise
571 */
msm_csid_subdev_init(struct camss * camss,struct csid_device * csid,const struct resources * res,u8 id)572 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
573 const struct resources *res, u8 id)
574 {
575 struct device *dev = camss->dev;
576 struct platform_device *pdev = to_platform_device(dev);
577 int i, j;
578 int ret;
579
580 csid->camss = camss;
581 csid->id = id;
582
583 if (camss->version == CAMSS_8x16) {
584 csid->ops = &csid_ops_4_1;
585 } else if (camss->version == CAMSS_8x96 ||
586 camss->version == CAMSS_660) {
587 csid->ops = &csid_ops_4_7;
588 } else if (camss->version == CAMSS_845 ||
589 camss->version == CAMSS_8250) {
590 csid->ops = &csid_ops_gen2;
591 } else {
592 return -EINVAL;
593 }
594 csid->ops->subdev_init(csid);
595
596 /* Memory */
597
598 if (camss->version == CAMSS_8250) {
599 /* for titan 480, CSID registers are inside the VFE region,
600 * between the VFE "top" and "bus" registers. this requires
601 * VFE to be initialized before CSID
602 */
603 if (id >= 2) /* VFE/CSID lite */
604 csid->base = camss->vfe[id].base + VFE_480_LITE_CSID_OFFSET;
605 else
606 csid->base = camss->vfe[id].base + VFE_480_CSID_OFFSET;
607 } else {
608 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
609 if (IS_ERR(csid->base))
610 return PTR_ERR(csid->base);
611 }
612
613 /* Interrupt */
614
615 ret = platform_get_irq_byname(pdev, res->interrupt[0]);
616 if (ret < 0)
617 return ret;
618
619 csid->irq = ret;
620 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
621 dev_name(dev), MSM_CSID_NAME, csid->id);
622 ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
623 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
624 csid->irq_name, csid);
625 if (ret < 0) {
626 dev_err(dev, "request_irq failed: %d\n", ret);
627 return ret;
628 }
629
630 /* Clocks */
631
632 csid->nclocks = 0;
633 while (res->clock[csid->nclocks])
634 csid->nclocks++;
635
636 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),
637 GFP_KERNEL);
638 if (!csid->clock)
639 return -ENOMEM;
640
641 for (i = 0; i < csid->nclocks; i++) {
642 struct camss_clock *clock = &csid->clock[i];
643
644 clock->clk = devm_clk_get(dev, res->clock[i]);
645 if (IS_ERR(clock->clk))
646 return PTR_ERR(clock->clk);
647
648 clock->name = res->clock[i];
649
650 clock->nfreqs = 0;
651 while (res->clock_rate[i][clock->nfreqs])
652 clock->nfreqs++;
653
654 if (!clock->nfreqs) {
655 clock->freq = NULL;
656 continue;
657 }
658
659 clock->freq = devm_kcalloc(dev,
660 clock->nfreqs,
661 sizeof(*clock->freq),
662 GFP_KERNEL);
663 if (!clock->freq)
664 return -ENOMEM;
665
666 for (j = 0; j < clock->nfreqs; j++)
667 clock->freq[j] = res->clock_rate[i][j];
668 }
669
670 /* Regulator */
671 for (i = 0; i < ARRAY_SIZE(res->regulators); i++) {
672 if (res->regulators[i])
673 csid->num_supplies++;
674 }
675
676 if (csid->num_supplies) {
677 csid->supplies = devm_kmalloc_array(camss->dev,
678 csid->num_supplies,
679 sizeof(*csid->supplies),
680 GFP_KERNEL);
681 if (!csid->supplies)
682 return -ENOMEM;
683 }
684
685 for (i = 0; i < csid->num_supplies; i++)
686 csid->supplies[i].supply = res->regulators[i];
687
688 ret = devm_regulator_bulk_get(camss->dev, csid->num_supplies,
689 csid->supplies);
690 if (ret)
691 return ret;
692
693 init_completion(&csid->reset_complete);
694
695 return 0;
696 }
697
698 /*
699 * msm_csid_get_csid_id - Get CSID HW module id
700 * @entity: Pointer to CSID media entity structure
701 * @id: Return CSID HW module id here
702 */
msm_csid_get_csid_id(struct media_entity * entity,u8 * id)703 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
704 {
705 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
706 struct csid_device *csid = v4l2_get_subdevdata(sd);
707
708 *id = csid->id;
709 }
710
711 /*
712 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
713 * @lane_cfg - CSI2 lane configuration
714 *
715 * Return lane assign
716 */
csid_get_lane_assign(struct csiphy_lanes_cfg * lane_cfg)717 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
718 {
719 u32 lane_assign = 0;
720 int i;
721
722 for (i = 0; i < lane_cfg->num_data; i++)
723 lane_assign |= lane_cfg->data[i].pos << (i * 4);
724
725 return lane_assign;
726 }
727
728 /*
729 * csid_link_setup - Setup CSID connections
730 * @entity: Pointer to media entity structure
731 * @local: Pointer to local pad
732 * @remote: Pointer to remote pad
733 * @flags: Link flags
734 *
735 * Return 0 on success
736 */
csid_link_setup(struct media_entity * entity,const struct media_pad * local,const struct media_pad * remote,u32 flags)737 static int csid_link_setup(struct media_entity *entity,
738 const struct media_pad *local,
739 const struct media_pad *remote, u32 flags)
740 {
741 if (flags & MEDIA_LNK_FL_ENABLED)
742 if (media_pad_remote_pad_first(local))
743 return -EBUSY;
744
745 if ((local->flags & MEDIA_PAD_FL_SINK) &&
746 (flags & MEDIA_LNK_FL_ENABLED)) {
747 struct v4l2_subdev *sd;
748 struct csid_device *csid;
749 struct csiphy_device *csiphy;
750 struct csiphy_lanes_cfg *lane_cfg;
751
752 sd = media_entity_to_v4l2_subdev(entity);
753 csid = v4l2_get_subdevdata(sd);
754
755 /* If test generator is enabled */
756 /* do not allow a link from CSIPHY to CSID */
757 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED &&
758 csid->testgen_mode->cur.val != 0)
759 return -EBUSY;
760
761 sd = media_entity_to_v4l2_subdev(remote->entity);
762 csiphy = v4l2_get_subdevdata(sd);
763
764 /* If a sensor is not linked to CSIPHY */
765 /* do no allow a link from CSIPHY to CSID */
766 if (!csiphy->cfg.csi2)
767 return -EPERM;
768
769 csid->phy.csiphy_id = csiphy->id;
770
771 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
772 csid->phy.lane_cnt = lane_cfg->num_data;
773 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
774 }
775 /* Decide which virtual channels to enable based on which source pads are enabled */
776 if (local->flags & MEDIA_PAD_FL_SOURCE) {
777 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
778 struct csid_device *csid = v4l2_get_subdevdata(sd);
779 struct device *dev = csid->camss->dev;
780
781 if (flags & MEDIA_LNK_FL_ENABLED)
782 csid->phy.en_vc |= BIT(local->index - 1);
783 else
784 csid->phy.en_vc &= ~BIT(local->index - 1);
785
786 csid->phy.need_vc_update = true;
787
788 dev_dbg(dev, "%s: Enabled CSID virtual channels mask 0x%x\n",
789 __func__, csid->phy.en_vc);
790 }
791
792 return 0;
793 }
794
795 static const struct v4l2_subdev_core_ops csid_core_ops = {
796 .s_power = csid_set_power,
797 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
798 .unsubscribe_event = v4l2_event_subdev_unsubscribe,
799 };
800
801 static const struct v4l2_subdev_video_ops csid_video_ops = {
802 .s_stream = csid_set_stream,
803 };
804
805 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
806 .enum_mbus_code = csid_enum_mbus_code,
807 .enum_frame_size = csid_enum_frame_size,
808 .get_fmt = csid_get_format,
809 .set_fmt = csid_set_format,
810 };
811
812 static const struct v4l2_subdev_ops csid_v4l2_ops = {
813 .core = &csid_core_ops,
814 .video = &csid_video_ops,
815 .pad = &csid_pad_ops,
816 };
817
818 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
819 .open = csid_init_formats,
820 };
821
822 static const struct media_entity_operations csid_media_ops = {
823 .link_setup = csid_link_setup,
824 .link_validate = v4l2_subdev_link_validate,
825 };
826
827 /*
828 * msm_csid_register_entity - Register subdev node for CSID module
829 * @csid: CSID device
830 * @v4l2_dev: V4L2 device
831 *
832 * Return 0 on success or a negative error code otherwise
833 */
msm_csid_register_entity(struct csid_device * csid,struct v4l2_device * v4l2_dev)834 int msm_csid_register_entity(struct csid_device *csid,
835 struct v4l2_device *v4l2_dev)
836 {
837 struct v4l2_subdev *sd = &csid->subdev;
838 struct media_pad *pads = csid->pads;
839 struct device *dev = csid->camss->dev;
840 int i;
841 int ret;
842
843 v4l2_subdev_init(sd, &csid_v4l2_ops);
844 sd->internal_ops = &csid_v4l2_internal_ops;
845 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
846 V4L2_SUBDEV_FL_HAS_EVENTS;
847 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
848 MSM_CSID_NAME, csid->id);
849 v4l2_set_subdevdata(sd, csid);
850
851 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) {
852 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
853 if (ret < 0) {
854 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
855 return ret;
856 }
857
858 csid->testgen_mode =
859 v4l2_ctrl_new_std_menu_items(&csid->ctrls,
860 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
861 csid->testgen.nmodes, 0, 0,
862 csid->testgen.modes);
863
864 if (csid->ctrls.error) {
865 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
866 ret = csid->ctrls.error;
867 goto free_ctrl;
868 }
869
870 csid->subdev.ctrl_handler = &csid->ctrls;
871 }
872
873 ret = csid_init_formats(sd, NULL);
874 if (ret < 0) {
875 dev_err(dev, "Failed to init format: %d\n", ret);
876 goto free_ctrl;
877 }
878
879 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
880 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i)
881 pads[i].flags = MEDIA_PAD_FL_SOURCE;
882
883 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
884 sd->entity.ops = &csid_media_ops;
885 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
886 if (ret < 0) {
887 dev_err(dev, "Failed to init media entity: %d\n", ret);
888 goto free_ctrl;
889 }
890
891 ret = v4l2_device_register_subdev(v4l2_dev, sd);
892 if (ret < 0) {
893 dev_err(dev, "Failed to register subdev: %d\n", ret);
894 goto media_cleanup;
895 }
896
897 return 0;
898
899 media_cleanup:
900 media_entity_cleanup(&sd->entity);
901 free_ctrl:
902 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED)
903 v4l2_ctrl_handler_free(&csid->ctrls);
904
905 return ret;
906 }
907
908 /*
909 * msm_csid_unregister_entity - Unregister CSID module subdev node
910 * @csid: CSID device
911 */
msm_csid_unregister_entity(struct csid_device * csid)912 void msm_csid_unregister_entity(struct csid_device *csid)
913 {
914 v4l2_device_unregister_subdev(&csid->subdev);
915 media_entity_cleanup(&csid->subdev.entity);
916 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED)
917 v4l2_ctrl_handler_free(&csid->ctrls);
918 }
919