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