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