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 * @sd_state: V4L2 subdev state 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_state_get_format(sd_state, 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 * @sd_state: V4L2 subdev state 288 * @pad: pad on which format is requested 289 * @fmt: pointer to v4l2 format structure 290 * @which: wanted subdev format 291 */ 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_1X16; 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_1X16; 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 * @sd_state: V4L2 subdev state 356 * @code: pointer to v4l2_subdev_mbus_code_enum structure 357 * return -EINVAL or zero on success 358 */ 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 * @sd_state: V4L2 subdev state 397 * @fse: pointer to v4l2_subdev_frame_size_enum structure 398 * return -EINVAL or zero on success 399 */ 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 * @sd_state: V4L2 subdev state 434 * @fmt: pointer to v4l2 subdev format structure 435 * 436 * Return -EINVAL or zero on success 437 */ 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 * @sd_state: V4L2 subdev state 458 * @fmt: pointer to v4l2 subdev format structure 459 * 460 * Return -EINVAL or zero on success 461 */ 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 */ 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_1X16, 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 */ 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 */ 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 */ 568 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid, 569 const struct camss_subdev_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 csid->ops = res->ops; 579 580 csid->ops->subdev_init(csid); 581 582 /* Memory */ 583 584 if (camss->res->version == CAMSS_8250) { 585 /* for titan 480, CSID registers are inside the VFE region, 586 * between the VFE "top" and "bus" registers. this requires 587 * VFE to be initialized before CSID 588 */ 589 if (id >= 2) /* VFE/CSID lite */ 590 csid->base = camss->vfe[id].base + VFE_480_LITE_CSID_OFFSET; 591 else 592 csid->base = camss->vfe[id].base + VFE_480_CSID_OFFSET; 593 } else { 594 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]); 595 if (IS_ERR(csid->base)) 596 return PTR_ERR(csid->base); 597 } 598 599 /* Interrupt */ 600 601 ret = platform_get_irq_byname(pdev, res->interrupt[0]); 602 if (ret < 0) 603 return ret; 604 605 csid->irq = ret; 606 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d", 607 dev_name(dev), MSM_CSID_NAME, csid->id); 608 ret = devm_request_irq(dev, csid->irq, csid->ops->isr, 609 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN, 610 csid->irq_name, csid); 611 if (ret < 0) { 612 dev_err(dev, "request_irq failed: %d\n", ret); 613 return ret; 614 } 615 616 /* Clocks */ 617 618 csid->nclocks = 0; 619 while (res->clock[csid->nclocks]) 620 csid->nclocks++; 621 622 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock), 623 GFP_KERNEL); 624 if (!csid->clock) 625 return -ENOMEM; 626 627 for (i = 0; i < csid->nclocks; i++) { 628 struct camss_clock *clock = &csid->clock[i]; 629 630 clock->clk = devm_clk_get(dev, res->clock[i]); 631 if (IS_ERR(clock->clk)) 632 return PTR_ERR(clock->clk); 633 634 clock->name = res->clock[i]; 635 636 clock->nfreqs = 0; 637 while (res->clock_rate[i][clock->nfreqs]) 638 clock->nfreqs++; 639 640 if (!clock->nfreqs) { 641 clock->freq = NULL; 642 continue; 643 } 644 645 clock->freq = devm_kcalloc(dev, 646 clock->nfreqs, 647 sizeof(*clock->freq), 648 GFP_KERNEL); 649 if (!clock->freq) 650 return -ENOMEM; 651 652 for (j = 0; j < clock->nfreqs; j++) 653 clock->freq[j] = res->clock_rate[i][j]; 654 } 655 656 /* Regulator */ 657 for (i = 0; i < ARRAY_SIZE(res->regulators); i++) { 658 if (res->regulators[i]) 659 csid->num_supplies++; 660 } 661 662 if (csid->num_supplies) { 663 csid->supplies = devm_kmalloc_array(camss->dev, 664 csid->num_supplies, 665 sizeof(*csid->supplies), 666 GFP_KERNEL); 667 if (!csid->supplies) 668 return -ENOMEM; 669 } 670 671 for (i = 0; i < csid->num_supplies; i++) 672 csid->supplies[i].supply = res->regulators[i]; 673 674 ret = devm_regulator_bulk_get(camss->dev, csid->num_supplies, 675 csid->supplies); 676 if (ret) 677 return ret; 678 679 init_completion(&csid->reset_complete); 680 681 return 0; 682 } 683 684 /* 685 * msm_csid_get_csid_id - Get CSID HW module id 686 * @entity: Pointer to CSID media entity structure 687 * @id: Return CSID HW module id here 688 */ 689 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id) 690 { 691 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 692 struct csid_device *csid = v4l2_get_subdevdata(sd); 693 694 *id = csid->id; 695 } 696 697 /* 698 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter 699 * @lane_cfg - CSI2 lane configuration 700 * 701 * Return lane assign 702 */ 703 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg) 704 { 705 u32 lane_assign = 0; 706 int i; 707 708 for (i = 0; i < lane_cfg->num_data; i++) 709 lane_assign |= lane_cfg->data[i].pos << (i * 4); 710 711 return lane_assign; 712 } 713 714 /* 715 * csid_link_setup - Setup CSID connections 716 * @entity: Pointer to media entity structure 717 * @local: Pointer to local pad 718 * @remote: Pointer to remote pad 719 * @flags: Link flags 720 * 721 * Return 0 on success 722 */ 723 static int csid_link_setup(struct media_entity *entity, 724 const struct media_pad *local, 725 const struct media_pad *remote, u32 flags) 726 { 727 if (flags & MEDIA_LNK_FL_ENABLED) 728 if (media_pad_remote_pad_first(local)) 729 return -EBUSY; 730 731 if ((local->flags & MEDIA_PAD_FL_SINK) && 732 (flags & MEDIA_LNK_FL_ENABLED)) { 733 struct v4l2_subdev *sd; 734 struct csid_device *csid; 735 struct csiphy_device *csiphy; 736 struct csiphy_lanes_cfg *lane_cfg; 737 738 sd = media_entity_to_v4l2_subdev(entity); 739 csid = v4l2_get_subdevdata(sd); 740 741 /* If test generator is enabled */ 742 /* do not allow a link from CSIPHY to CSID */ 743 if (csid->testgen_mode->cur.val != 0) 744 return -EBUSY; 745 746 sd = media_entity_to_v4l2_subdev(remote->entity); 747 csiphy = v4l2_get_subdevdata(sd); 748 749 /* If a sensor is not linked to CSIPHY */ 750 /* do no allow a link from CSIPHY to CSID */ 751 if (!csiphy->cfg.csi2) 752 return -EPERM; 753 754 csid->phy.csiphy_id = csiphy->id; 755 756 lane_cfg = &csiphy->cfg.csi2->lane_cfg; 757 csid->phy.lane_cnt = lane_cfg->num_data; 758 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg); 759 } 760 /* Decide which virtual channels to enable based on which source pads are enabled */ 761 if (local->flags & MEDIA_PAD_FL_SOURCE) { 762 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 763 struct csid_device *csid = v4l2_get_subdevdata(sd); 764 struct device *dev = csid->camss->dev; 765 766 if (flags & MEDIA_LNK_FL_ENABLED) 767 csid->phy.en_vc |= BIT(local->index - 1); 768 else 769 csid->phy.en_vc &= ~BIT(local->index - 1); 770 771 csid->phy.need_vc_update = true; 772 773 dev_dbg(dev, "%s: Enabled CSID virtual channels mask 0x%x\n", 774 __func__, csid->phy.en_vc); 775 } 776 777 return 0; 778 } 779 780 static const struct v4l2_subdev_core_ops csid_core_ops = { 781 .s_power = csid_set_power, 782 .subscribe_event = v4l2_ctrl_subdev_subscribe_event, 783 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 784 }; 785 786 static const struct v4l2_subdev_video_ops csid_video_ops = { 787 .s_stream = csid_set_stream, 788 }; 789 790 static const struct v4l2_subdev_pad_ops csid_pad_ops = { 791 .enum_mbus_code = csid_enum_mbus_code, 792 .enum_frame_size = csid_enum_frame_size, 793 .get_fmt = csid_get_format, 794 .set_fmt = csid_set_format, 795 }; 796 797 static const struct v4l2_subdev_ops csid_v4l2_ops = { 798 .core = &csid_core_ops, 799 .video = &csid_video_ops, 800 .pad = &csid_pad_ops, 801 }; 802 803 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = { 804 .open = csid_init_formats, 805 }; 806 807 static const struct media_entity_operations csid_media_ops = { 808 .link_setup = csid_link_setup, 809 .link_validate = v4l2_subdev_link_validate, 810 }; 811 812 /* 813 * msm_csid_register_entity - Register subdev node for CSID module 814 * @csid: CSID device 815 * @v4l2_dev: V4L2 device 816 * 817 * Return 0 on success or a negative error code otherwise 818 */ 819 int msm_csid_register_entity(struct csid_device *csid, 820 struct v4l2_device *v4l2_dev) 821 { 822 struct v4l2_subdev *sd = &csid->subdev; 823 struct media_pad *pads = csid->pads; 824 struct device *dev = csid->camss->dev; 825 int i; 826 int ret; 827 828 v4l2_subdev_init(sd, &csid_v4l2_ops); 829 sd->internal_ops = &csid_v4l2_internal_ops; 830 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | 831 V4L2_SUBDEV_FL_HAS_EVENTS; 832 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", 833 MSM_CSID_NAME, csid->id); 834 v4l2_set_subdevdata(sd, csid); 835 836 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1); 837 if (ret < 0) { 838 dev_err(dev, "Failed to init ctrl handler: %d\n", ret); 839 return ret; 840 } 841 842 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls, 843 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN, 844 csid->testgen.nmodes, 0, 0, 845 csid->testgen.modes); 846 847 if (csid->ctrls.error) { 848 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error); 849 ret = csid->ctrls.error; 850 goto free_ctrl; 851 } 852 853 csid->subdev.ctrl_handler = &csid->ctrls; 854 855 ret = csid_init_formats(sd, NULL); 856 if (ret < 0) { 857 dev_err(dev, "Failed to init format: %d\n", ret); 858 goto free_ctrl; 859 } 860 861 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 862 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i) 863 pads[i].flags = MEDIA_PAD_FL_SOURCE; 864 865 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; 866 sd->entity.ops = &csid_media_ops; 867 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads); 868 if (ret < 0) { 869 dev_err(dev, "Failed to init media entity: %d\n", ret); 870 goto free_ctrl; 871 } 872 873 ret = v4l2_device_register_subdev(v4l2_dev, sd); 874 if (ret < 0) { 875 dev_err(dev, "Failed to register subdev: %d\n", ret); 876 goto media_cleanup; 877 } 878 879 return 0; 880 881 media_cleanup: 882 media_entity_cleanup(&sd->entity); 883 free_ctrl: 884 v4l2_ctrl_handler_free(&csid->ctrls); 885 886 return ret; 887 } 888 889 /* 890 * msm_csid_unregister_entity - Unregister CSID module subdev node 891 * @csid: CSID device 892 */ 893 void msm_csid_unregister_entity(struct csid_device *csid) 894 { 895 v4l2_device_unregister_subdev(&csid->subdev); 896 media_entity_cleanup(&csid->subdev.entity); 897 v4l2_ctrl_handler_free(&csid->ctrls); 898 } 899 900 inline bool csid_is_lite(struct csid_device *csid) 901 { 902 return csid->camss->res->csid_res[csid->id].is_lite; 903 } 904