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/mipi-csi2.h> 21 #include <media/v4l2-device.h> 22 #include <media/v4l2-event.h> 23 #include <media/v4l2-subdev.h> 24 25 #include "camss-csid.h" 26 #include "camss-csid-gen1.h" 27 #include "camss.h" 28 29 /* offset of CSID registers in VFE region for VFE 480 */ 30 #define VFE_480_CSID_OFFSET 0x1200 31 #define VFE_480_LITE_CSID_OFFSET 0x200 32 33 #define CSID_HW_VERSION 0x0 34 #define HW_VERSION_STEPPING 0 35 #define HW_VERSION_REVISION 16 36 #define HW_VERSION_GENERATION 28 37 38 #define MSM_CSID_NAME "msm_csid" 39 40 const char * const csid_testgen_modes[] = { 41 "Disabled", 42 "Incrementing", 43 "Alternating 0x55/0xAA", 44 "All Zeros 0x00", 45 "All Ones 0xFF", 46 "Pseudo-random Data", 47 "User Specified", 48 "Complex pattern", 49 "Color box", 50 "Color bars", 51 NULL 52 }; 53 54 static const struct csid_format_info formats_4_1[] = { 55 { 56 MEDIA_BUS_FMT_UYVY8_1X16, 57 MIPI_CSI2_DT_YUV422_8B, 58 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 59 8, 60 2, 61 }, 62 { 63 MEDIA_BUS_FMT_VYUY8_1X16, 64 MIPI_CSI2_DT_YUV422_8B, 65 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 66 8, 67 2, 68 }, 69 { 70 MEDIA_BUS_FMT_YUYV8_1X16, 71 MIPI_CSI2_DT_YUV422_8B, 72 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 73 8, 74 2, 75 }, 76 { 77 MEDIA_BUS_FMT_YVYU8_1X16, 78 MIPI_CSI2_DT_YUV422_8B, 79 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 80 8, 81 2, 82 }, 83 { 84 MEDIA_BUS_FMT_SBGGR8_1X8, 85 MIPI_CSI2_DT_RAW8, 86 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 87 8, 88 1, 89 }, 90 { 91 MEDIA_BUS_FMT_SGBRG8_1X8, 92 MIPI_CSI2_DT_RAW8, 93 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 94 8, 95 1, 96 }, 97 { 98 MEDIA_BUS_FMT_SGRBG8_1X8, 99 MIPI_CSI2_DT_RAW8, 100 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 101 8, 102 1, 103 }, 104 { 105 MEDIA_BUS_FMT_SRGGB8_1X8, 106 MIPI_CSI2_DT_RAW8, 107 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 108 8, 109 1, 110 }, 111 { 112 MEDIA_BUS_FMT_SBGGR10_1X10, 113 MIPI_CSI2_DT_RAW10, 114 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 115 10, 116 1, 117 }, 118 { 119 MEDIA_BUS_FMT_SGBRG10_1X10, 120 MIPI_CSI2_DT_RAW10, 121 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 122 10, 123 1, 124 }, 125 { 126 MEDIA_BUS_FMT_SGRBG10_1X10, 127 MIPI_CSI2_DT_RAW10, 128 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 129 10, 130 1, 131 }, 132 { 133 MEDIA_BUS_FMT_SRGGB10_1X10, 134 MIPI_CSI2_DT_RAW10, 135 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 136 10, 137 1, 138 }, 139 { 140 MEDIA_BUS_FMT_SBGGR12_1X12, 141 MIPI_CSI2_DT_RAW12, 142 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 143 12, 144 1, 145 }, 146 { 147 MEDIA_BUS_FMT_SGBRG12_1X12, 148 MIPI_CSI2_DT_RAW12, 149 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 150 12, 151 1, 152 }, 153 { 154 MEDIA_BUS_FMT_SGRBG12_1X12, 155 MIPI_CSI2_DT_RAW12, 156 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 157 12, 158 1, 159 }, 160 { 161 MEDIA_BUS_FMT_SRGGB12_1X12, 162 MIPI_CSI2_DT_RAW12, 163 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 164 12, 165 1, 166 }, 167 { 168 MEDIA_BUS_FMT_Y10_1X10, 169 MIPI_CSI2_DT_RAW10, 170 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 171 10, 172 1, 173 }, 174 }; 175 176 static const struct csid_format_info formats_4_7[] = { 177 { 178 MEDIA_BUS_FMT_UYVY8_1X16, 179 MIPI_CSI2_DT_YUV422_8B, 180 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 181 8, 182 2, 183 }, 184 { 185 MEDIA_BUS_FMT_VYUY8_1X16, 186 MIPI_CSI2_DT_YUV422_8B, 187 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 188 8, 189 2, 190 }, 191 { 192 MEDIA_BUS_FMT_YUYV8_1X16, 193 MIPI_CSI2_DT_YUV422_8B, 194 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 195 8, 196 2, 197 }, 198 { 199 MEDIA_BUS_FMT_YVYU8_1X16, 200 MIPI_CSI2_DT_YUV422_8B, 201 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 202 8, 203 2, 204 }, 205 { 206 MEDIA_BUS_FMT_SBGGR8_1X8, 207 MIPI_CSI2_DT_RAW8, 208 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 209 8, 210 1, 211 }, 212 { 213 MEDIA_BUS_FMT_SGBRG8_1X8, 214 MIPI_CSI2_DT_RAW8, 215 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 216 8, 217 1, 218 }, 219 { 220 MEDIA_BUS_FMT_SGRBG8_1X8, 221 MIPI_CSI2_DT_RAW8, 222 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 223 8, 224 1, 225 }, 226 { 227 MEDIA_BUS_FMT_SRGGB8_1X8, 228 MIPI_CSI2_DT_RAW8, 229 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 230 8, 231 1, 232 }, 233 { 234 MEDIA_BUS_FMT_SBGGR10_1X10, 235 MIPI_CSI2_DT_RAW10, 236 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 237 10, 238 1, 239 }, 240 { 241 MEDIA_BUS_FMT_SGBRG10_1X10, 242 MIPI_CSI2_DT_RAW10, 243 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 244 10, 245 1, 246 }, 247 { 248 MEDIA_BUS_FMT_SGRBG10_1X10, 249 MIPI_CSI2_DT_RAW10, 250 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 251 10, 252 1, 253 }, 254 { 255 MEDIA_BUS_FMT_SRGGB10_1X10, 256 MIPI_CSI2_DT_RAW10, 257 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 258 10, 259 1, 260 }, 261 { 262 MEDIA_BUS_FMT_SBGGR12_1X12, 263 MIPI_CSI2_DT_RAW12, 264 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 265 12, 266 1, 267 }, 268 { 269 MEDIA_BUS_FMT_SGBRG12_1X12, 270 MIPI_CSI2_DT_RAW12, 271 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 272 12, 273 1, 274 }, 275 { 276 MEDIA_BUS_FMT_SGRBG12_1X12, 277 MIPI_CSI2_DT_RAW12, 278 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 279 12, 280 1, 281 }, 282 { 283 MEDIA_BUS_FMT_SRGGB12_1X12, 284 MIPI_CSI2_DT_RAW12, 285 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 286 12, 287 1, 288 }, 289 { 290 MEDIA_BUS_FMT_SBGGR14_1X14, 291 MIPI_CSI2_DT_RAW14, 292 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 293 14, 294 1, 295 }, 296 { 297 MEDIA_BUS_FMT_SGBRG14_1X14, 298 MIPI_CSI2_DT_RAW14, 299 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 300 14, 301 1, 302 }, 303 { 304 MEDIA_BUS_FMT_SGRBG14_1X14, 305 MIPI_CSI2_DT_RAW14, 306 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 307 14, 308 1, 309 }, 310 { 311 MEDIA_BUS_FMT_SRGGB14_1X14, 312 MIPI_CSI2_DT_RAW14, 313 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 314 14, 315 1, 316 }, 317 { 318 MEDIA_BUS_FMT_Y10_1X10, 319 MIPI_CSI2_DT_RAW10, 320 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 321 10, 322 1, 323 }, 324 }; 325 326 static const struct csid_format_info formats_gen2[] = { 327 { 328 MEDIA_BUS_FMT_UYVY8_1X16, 329 MIPI_CSI2_DT_YUV422_8B, 330 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 331 8, 332 2, 333 }, 334 { 335 MEDIA_BUS_FMT_VYUY8_1X16, 336 MIPI_CSI2_DT_YUV422_8B, 337 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 338 8, 339 2, 340 }, 341 { 342 MEDIA_BUS_FMT_YUYV8_1X16, 343 MIPI_CSI2_DT_YUV422_8B, 344 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 345 8, 346 2, 347 }, 348 { 349 MEDIA_BUS_FMT_YVYU8_1X16, 350 MIPI_CSI2_DT_YUV422_8B, 351 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 352 8, 353 2, 354 }, 355 { 356 MEDIA_BUS_FMT_SBGGR8_1X8, 357 MIPI_CSI2_DT_RAW8, 358 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 359 8, 360 1, 361 }, 362 { 363 MEDIA_BUS_FMT_SGBRG8_1X8, 364 MIPI_CSI2_DT_RAW8, 365 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 366 8, 367 1, 368 }, 369 { 370 MEDIA_BUS_FMT_SGRBG8_1X8, 371 MIPI_CSI2_DT_RAW8, 372 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 373 8, 374 1, 375 }, 376 { 377 MEDIA_BUS_FMT_SRGGB8_1X8, 378 MIPI_CSI2_DT_RAW8, 379 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 380 8, 381 1, 382 }, 383 { 384 MEDIA_BUS_FMT_SBGGR10_1X10, 385 MIPI_CSI2_DT_RAW10, 386 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 387 10, 388 1, 389 }, 390 { 391 MEDIA_BUS_FMT_SGBRG10_1X10, 392 MIPI_CSI2_DT_RAW10, 393 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 394 10, 395 1, 396 }, 397 { 398 MEDIA_BUS_FMT_SGRBG10_1X10, 399 MIPI_CSI2_DT_RAW10, 400 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 401 10, 402 1, 403 }, 404 { 405 MEDIA_BUS_FMT_SRGGB10_1X10, 406 MIPI_CSI2_DT_RAW10, 407 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 408 10, 409 1, 410 }, 411 { 412 MEDIA_BUS_FMT_Y8_1X8, 413 MIPI_CSI2_DT_RAW8, 414 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 415 8, 416 1, 417 }, 418 { 419 MEDIA_BUS_FMT_Y10_1X10, 420 MIPI_CSI2_DT_RAW10, 421 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 422 10, 423 1, 424 }, 425 { 426 MEDIA_BUS_FMT_SBGGR12_1X12, 427 MIPI_CSI2_DT_RAW12, 428 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 429 12, 430 1, 431 }, 432 { 433 MEDIA_BUS_FMT_SGBRG12_1X12, 434 MIPI_CSI2_DT_RAW12, 435 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 436 12, 437 1, 438 }, 439 { 440 MEDIA_BUS_FMT_SGRBG12_1X12, 441 MIPI_CSI2_DT_RAW12, 442 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 443 12, 444 1, 445 }, 446 { 447 MEDIA_BUS_FMT_SRGGB12_1X12, 448 MIPI_CSI2_DT_RAW12, 449 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 450 12, 451 1, 452 }, 453 { 454 MEDIA_BUS_FMT_SBGGR14_1X14, 455 MIPI_CSI2_DT_RAW14, 456 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 457 14, 458 1, 459 }, 460 { 461 MEDIA_BUS_FMT_SGBRG14_1X14, 462 MIPI_CSI2_DT_RAW14, 463 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 464 14, 465 1, 466 }, 467 { 468 MEDIA_BUS_FMT_SGRBG14_1X14, 469 MIPI_CSI2_DT_RAW14, 470 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 471 14, 472 1, 473 }, 474 { 475 MEDIA_BUS_FMT_SRGGB14_1X14, 476 MIPI_CSI2_DT_RAW14, 477 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 478 14, 479 1, 480 }, 481 }; 482 483 const struct csid_formats csid_formats_4_1 = { 484 .nformats = ARRAY_SIZE(formats_4_1), 485 .formats = formats_4_1 486 }; 487 488 const struct csid_formats csid_formats_4_7 = { 489 .nformats = ARRAY_SIZE(formats_4_7), 490 .formats = formats_4_7 491 }; 492 493 const struct csid_formats csid_formats_gen2 = { 494 .nformats = ARRAY_SIZE(formats_gen2), 495 .formats = formats_gen2 496 }; 497 498 u32 csid_find_code(u32 *codes, unsigned int ncodes, 499 unsigned int match_format_idx, u32 match_code) 500 { 501 int i; 502 503 if (!match_code && (match_format_idx >= ncodes)) 504 return 0; 505 506 for (i = 0; i < ncodes; i++) 507 if (match_code) { 508 if (codes[i] == match_code) 509 return match_code; 510 } else { 511 if (i == match_format_idx) 512 return codes[i]; 513 } 514 515 return codes[0]; 516 } 517 518 const struct csid_format_info *csid_get_fmt_entry(const struct csid_format_info *formats, 519 unsigned int nformats, 520 u32 code) 521 { 522 unsigned int i; 523 524 for (i = 0; i < nformats; i++) 525 if (code == formats[i].code) 526 return &formats[i]; 527 528 WARN(1, "Unknown format\n"); 529 530 return &formats[0]; 531 } 532 533 /* 534 * csid_set_clock_rates - Calculate and set clock rates on CSID module 535 * @csiphy: CSID device 536 */ 537 static int csid_set_clock_rates(struct csid_device *csid) 538 { 539 struct device *dev = csid->camss->dev; 540 const struct csid_format_info *fmt; 541 s64 link_freq; 542 int i, j; 543 int ret; 544 545 fmt = csid_get_fmt_entry(csid->res->formats->formats, csid->res->formats->nformats, 546 csid->fmt[MSM_CSIPHY_PAD_SINK].code); 547 link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp, 548 csid->phy.lane_cnt); 549 if (link_freq < 0) 550 link_freq = 0; 551 552 for (i = 0; i < csid->nclocks; i++) { 553 struct camss_clock *clock = &csid->clock[i]; 554 555 if (!strcmp(clock->name, "csi0") || 556 !strcmp(clock->name, "csi1") || 557 !strcmp(clock->name, "csi2") || 558 !strcmp(clock->name, "csi3")) { 559 u64 min_rate = link_freq / 4; 560 long rate; 561 562 camss_add_clock_margin(&min_rate); 563 564 for (j = 0; j < clock->nfreqs; j++) 565 if (min_rate < clock->freq[j]) 566 break; 567 568 if (j == clock->nfreqs) { 569 dev_err(dev, 570 "Pixel clock is too high for CSID\n"); 571 return -EINVAL; 572 } 573 574 /* if sensor pixel clock is not available */ 575 /* set highest possible CSID clock rate */ 576 if (min_rate == 0) 577 j = clock->nfreqs - 1; 578 579 rate = clk_round_rate(clock->clk, clock->freq[j]); 580 if (rate < 0) { 581 dev_err(dev, "clk round rate failed: %ld\n", 582 rate); 583 return -EINVAL; 584 } 585 586 ret = clk_set_rate(clock->clk, rate); 587 if (ret < 0) { 588 dev_err(dev, "clk set rate failed: %d\n", ret); 589 return ret; 590 } 591 } else if (clock->nfreqs) { 592 clk_set_rate(clock->clk, clock->freq[0]); 593 } 594 } 595 596 return 0; 597 } 598 599 /* 600 * csid_hw_version - CSID hardware version query 601 * @csid: CSID device 602 * 603 * Return HW version or error 604 */ 605 u32 csid_hw_version(struct csid_device *csid) 606 { 607 u32 hw_version; 608 u32 hw_gen; 609 u32 hw_rev; 610 u32 hw_step; 611 612 hw_version = readl_relaxed(csid->base + CSID_HW_VERSION); 613 hw_gen = (hw_version >> HW_VERSION_GENERATION) & 0xF; 614 hw_rev = (hw_version >> HW_VERSION_REVISION) & 0xFFF; 615 hw_step = (hw_version >> HW_VERSION_STEPPING) & 0xFFFF; 616 dev_info(csid->camss->dev, "CSID:%d HW Version = %u.%u.%u\n", 617 csid->id, hw_gen, hw_rev, hw_step); 618 619 return hw_version; 620 } 621 622 /* 623 * csid_src_pad_code - Pick an output/src format based on the input/sink format 624 * @csid: CSID device 625 * @sink_code: The sink format of the input 626 * @match_format_idx: Request preferred index, as defined by subdevice csid 627 * format. Set @match_code to 0 if used. 628 * @match_code: Request preferred code, set @match_format_idx to 0 if used 629 * 630 * Return 0 on failure or src format code otherwise 631 */ 632 u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code, 633 unsigned int match_format_idx, u32 match_code) 634 { 635 if (csid->camss->res->version == CAMSS_8x16) { 636 if (match_format_idx > 0) 637 return 0; 638 639 return sink_code; 640 } 641 642 switch (sink_code) { 643 case MEDIA_BUS_FMT_SBGGR10_1X10: 644 { 645 u32 src_code[] = { 646 MEDIA_BUS_FMT_SBGGR10_1X10, 647 MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 648 }; 649 650 return csid_find_code(src_code, ARRAY_SIZE(src_code), 651 match_format_idx, match_code); 652 } 653 case MEDIA_BUS_FMT_Y10_1X10: 654 { 655 u32 src_code[] = { 656 MEDIA_BUS_FMT_Y10_1X10, 657 MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 658 }; 659 660 return csid_find_code(src_code, ARRAY_SIZE(src_code), 661 match_format_idx, match_code); 662 } 663 default: 664 if (match_format_idx > 0) 665 return 0; 666 667 return sink_code; 668 } 669 } 670 671 /* 672 * csid_set_power - Power on/off CSID module 673 * @sd: CSID V4L2 subdevice 674 * @on: Requested power state 675 * 676 * Return 0 on success or a negative error code otherwise 677 */ 678 static int csid_set_power(struct v4l2_subdev *sd, int on) 679 { 680 struct csid_device *csid = v4l2_get_subdevdata(sd); 681 struct camss *camss = csid->camss; 682 struct device *dev = camss->dev; 683 int ret = 0; 684 685 if (on) { 686 /* 687 * From SDM845 onwards, the VFE needs to be powered on before 688 * switching on the CSID. Do so unconditionally, as there is no 689 * drawback in following the same powering order on older SoCs. 690 */ 691 ret = csid->res->parent_dev_ops->get(camss, csid->id); 692 if (ret < 0) 693 return ret; 694 695 ret = pm_runtime_resume_and_get(dev); 696 if (ret < 0) 697 return ret; 698 699 ret = regulator_bulk_enable(csid->num_supplies, 700 csid->supplies); 701 if (ret < 0) { 702 pm_runtime_put_sync(dev); 703 return ret; 704 } 705 706 ret = csid_set_clock_rates(csid); 707 if (ret < 0) { 708 regulator_bulk_disable(csid->num_supplies, 709 csid->supplies); 710 pm_runtime_put_sync(dev); 711 return ret; 712 } 713 714 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev); 715 if (ret < 0) { 716 regulator_bulk_disable(csid->num_supplies, 717 csid->supplies); 718 pm_runtime_put_sync(dev); 719 return ret; 720 } 721 722 csid->phy.need_vc_update = true; 723 724 enable_irq(csid->irq); 725 726 ret = csid->res->hw_ops->reset(csid); 727 if (ret < 0) { 728 disable_irq(csid->irq); 729 camss_disable_clocks(csid->nclocks, csid->clock); 730 regulator_bulk_disable(csid->num_supplies, 731 csid->supplies); 732 pm_runtime_put_sync(dev); 733 return ret; 734 } 735 736 csid->res->hw_ops->hw_version(csid); 737 } else { 738 disable_irq(csid->irq); 739 camss_disable_clocks(csid->nclocks, csid->clock); 740 regulator_bulk_disable(csid->num_supplies, 741 csid->supplies); 742 pm_runtime_put_sync(dev); 743 csid->res->parent_dev_ops->put(camss, csid->id); 744 } 745 746 return ret; 747 } 748 749 /* 750 * csid_set_stream - Enable/disable streaming on CSID module 751 * @sd: CSID V4L2 subdevice 752 * @enable: Requested streaming state 753 * 754 * Main configuration of CSID module is also done here. 755 * 756 * Return 0 on success or a negative error code otherwise 757 */ 758 static int csid_set_stream(struct v4l2_subdev *sd, int enable) 759 { 760 struct csid_device *csid = v4l2_get_subdevdata(sd); 761 int ret; 762 763 if (enable) { 764 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) { 765 ret = v4l2_ctrl_handler_setup(&csid->ctrls); 766 if (ret < 0) { 767 dev_err(csid->camss->dev, 768 "could not sync v4l2 controls: %d\n", ret); 769 return ret; 770 } 771 } 772 773 if (!csid->testgen.enabled && 774 !media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK])) 775 return -ENOLINK; 776 } 777 778 if (csid->phy.need_vc_update) { 779 csid->res->hw_ops->configure_stream(csid, enable); 780 csid->phy.need_vc_update = false; 781 } 782 783 return 0; 784 } 785 786 /* 787 * __csid_get_format - Get pointer to format structure 788 * @csid: CSID device 789 * @sd_state: V4L2 subdev state 790 * @pad: pad from which format is requested 791 * @which: TRY or ACTIVE format 792 * 793 * Return pointer to TRY or ACTIVE format structure 794 */ 795 static struct v4l2_mbus_framefmt * 796 __csid_get_format(struct csid_device *csid, 797 struct v4l2_subdev_state *sd_state, 798 unsigned int pad, 799 enum v4l2_subdev_format_whence which) 800 { 801 if (which == V4L2_SUBDEV_FORMAT_TRY) 802 return v4l2_subdev_state_get_format(sd_state, pad); 803 804 return &csid->fmt[pad]; 805 } 806 807 /* 808 * csid_try_format - Handle try format by pad subdev method 809 * @csid: CSID device 810 * @sd_state: V4L2 subdev state 811 * @pad: pad on which format is requested 812 * @fmt: pointer to v4l2 format structure 813 * @which: wanted subdev format 814 */ 815 static void csid_try_format(struct csid_device *csid, 816 struct v4l2_subdev_state *sd_state, 817 unsigned int pad, 818 struct v4l2_mbus_framefmt *fmt, 819 enum v4l2_subdev_format_whence which) 820 { 821 unsigned int i; 822 823 switch (pad) { 824 case MSM_CSID_PAD_SINK: 825 /* Set format on sink pad */ 826 827 for (i = 0; i < csid->res->formats->nformats; i++) 828 if (fmt->code == csid->res->formats->formats[i].code) 829 break; 830 831 /* If not found, use UYVY as default */ 832 if (i >= csid->res->formats->nformats) 833 fmt->code = MEDIA_BUS_FMT_UYVY8_1X16; 834 835 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 836 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 837 838 fmt->field = V4L2_FIELD_NONE; 839 fmt->colorspace = V4L2_COLORSPACE_SRGB; 840 841 break; 842 843 case MSM_CSID_PAD_SRC: 844 if (csid->testgen.nmodes == CSID_PAYLOAD_MODE_DISABLED || 845 csid->testgen_mode->cur.val == 0) { 846 /* Test generator is disabled, */ 847 /* keep pad formats in sync */ 848 u32 code = fmt->code; 849 850 *fmt = *__csid_get_format(csid, sd_state, 851 MSM_CSID_PAD_SINK, which); 852 fmt->code = csid->res->hw_ops->src_pad_code(csid, fmt->code, 0, code); 853 } else { 854 /* Test generator is enabled, set format on source */ 855 /* pad to allow test generator usage */ 856 857 for (i = 0; i < csid->res->formats->nformats; i++) 858 if (csid->res->formats->formats[i].code == fmt->code) 859 break; 860 861 /* If not found, use UYVY as default */ 862 if (i >= csid->res->formats->nformats) 863 fmt->code = MEDIA_BUS_FMT_UYVY8_1X16; 864 865 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 866 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 867 868 fmt->field = V4L2_FIELD_NONE; 869 } 870 break; 871 } 872 873 fmt->colorspace = V4L2_COLORSPACE_SRGB; 874 } 875 876 /* 877 * csid_enum_mbus_code - Handle pixel format enumeration 878 * @sd: CSID V4L2 subdevice 879 * @sd_state: V4L2 subdev state 880 * @code: pointer to v4l2_subdev_mbus_code_enum structure 881 * return -EINVAL or zero on success 882 */ 883 static int csid_enum_mbus_code(struct v4l2_subdev *sd, 884 struct v4l2_subdev_state *sd_state, 885 struct v4l2_subdev_mbus_code_enum *code) 886 { 887 struct csid_device *csid = v4l2_get_subdevdata(sd); 888 889 if (code->pad == MSM_CSID_PAD_SINK) { 890 if (code->index >= csid->res->formats->nformats) 891 return -EINVAL; 892 893 code->code = csid->res->formats->formats[code->index].code; 894 } else { 895 if (csid->testgen.nmodes == CSID_PAYLOAD_MODE_DISABLED || 896 csid->testgen_mode->cur.val == 0) { 897 struct v4l2_mbus_framefmt *sink_fmt; 898 899 sink_fmt = __csid_get_format(csid, sd_state, 900 MSM_CSID_PAD_SINK, 901 code->which); 902 903 code->code = csid->res->hw_ops->src_pad_code(csid, sink_fmt->code, 904 code->index, 0); 905 if (!code->code) 906 return -EINVAL; 907 } else { 908 if (code->index >= csid->res->formats->nformats) 909 return -EINVAL; 910 911 code->code = csid->res->formats->formats[code->index].code; 912 } 913 } 914 915 return 0; 916 } 917 918 /* 919 * csid_enum_frame_size - Handle frame size enumeration 920 * @sd: CSID V4L2 subdevice 921 * @sd_state: V4L2 subdev state 922 * @fse: pointer to v4l2_subdev_frame_size_enum structure 923 * return -EINVAL or zero on success 924 */ 925 static int csid_enum_frame_size(struct v4l2_subdev *sd, 926 struct v4l2_subdev_state *sd_state, 927 struct v4l2_subdev_frame_size_enum *fse) 928 { 929 struct csid_device *csid = v4l2_get_subdevdata(sd); 930 struct v4l2_mbus_framefmt format; 931 932 if (fse->index != 0) 933 return -EINVAL; 934 935 format.code = fse->code; 936 format.width = 1; 937 format.height = 1; 938 csid_try_format(csid, sd_state, fse->pad, &format, fse->which); 939 fse->min_width = format.width; 940 fse->min_height = format.height; 941 942 if (format.code != fse->code) 943 return -EINVAL; 944 945 format.code = fse->code; 946 format.width = -1; 947 format.height = -1; 948 csid_try_format(csid, sd_state, fse->pad, &format, fse->which); 949 fse->max_width = format.width; 950 fse->max_height = format.height; 951 952 return 0; 953 } 954 955 /* 956 * csid_get_format - Handle get format by pads subdev method 957 * @sd: CSID V4L2 subdevice 958 * @sd_state: V4L2 subdev state 959 * @fmt: pointer to v4l2 subdev format structure 960 * 961 * Return -EINVAL or zero on success 962 */ 963 static int csid_get_format(struct v4l2_subdev *sd, 964 struct v4l2_subdev_state *sd_state, 965 struct v4l2_subdev_format *fmt) 966 { 967 struct csid_device *csid = v4l2_get_subdevdata(sd); 968 struct v4l2_mbus_framefmt *format; 969 970 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which); 971 if (format == NULL) 972 return -EINVAL; 973 974 fmt->format = *format; 975 976 return 0; 977 } 978 979 /* 980 * csid_set_format - Handle set format by pads subdev method 981 * @sd: CSID V4L2 subdevice 982 * @sd_state: V4L2 subdev state 983 * @fmt: pointer to v4l2 subdev format structure 984 * 985 * Return -EINVAL or zero on success 986 */ 987 static int csid_set_format(struct v4l2_subdev *sd, 988 struct v4l2_subdev_state *sd_state, 989 struct v4l2_subdev_format *fmt) 990 { 991 struct csid_device *csid = v4l2_get_subdevdata(sd); 992 struct v4l2_mbus_framefmt *format; 993 int i; 994 995 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which); 996 if (format == NULL) 997 return -EINVAL; 998 999 csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which); 1000 *format = fmt->format; 1001 1002 /* Propagate the format from sink to source pads */ 1003 if (fmt->pad == MSM_CSID_PAD_SINK) { 1004 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i) { 1005 format = __csid_get_format(csid, sd_state, i, fmt->which); 1006 1007 *format = fmt->format; 1008 csid_try_format(csid, sd_state, i, format, fmt->which); 1009 } 1010 } 1011 1012 return 0; 1013 } 1014 1015 /* 1016 * csid_init_formats - Initialize formats on all pads 1017 * @sd: CSID V4L2 subdevice 1018 * @fh: V4L2 subdev file handle 1019 * 1020 * Initialize all pad formats with default values. 1021 * 1022 * Return 0 on success or a negative error code otherwise 1023 */ 1024 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) 1025 { 1026 struct v4l2_subdev_format format = { 1027 .pad = MSM_CSID_PAD_SINK, 1028 .which = fh ? V4L2_SUBDEV_FORMAT_TRY : 1029 V4L2_SUBDEV_FORMAT_ACTIVE, 1030 .format = { 1031 .code = MEDIA_BUS_FMT_UYVY8_1X16, 1032 .width = 1920, 1033 .height = 1080 1034 } 1035 }; 1036 1037 return csid_set_format(sd, fh ? fh->state : NULL, &format); 1038 } 1039 1040 /* 1041 * csid_set_test_pattern - Set test generator's pattern mode 1042 * @csid: CSID device 1043 * @value: desired test pattern mode 1044 * 1045 * Return 0 on success or a negative error code otherwise 1046 */ 1047 static int csid_set_test_pattern(struct csid_device *csid, s32 value) 1048 { 1049 struct csid_testgen_config *tg = &csid->testgen; 1050 1051 /* If CSID is linked to CSIPHY, do not allow to enable test generator */ 1052 if (value && media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK])) 1053 return -EBUSY; 1054 1055 tg->enabled = !!value; 1056 1057 return csid->res->hw_ops->configure_testgen_pattern(csid, value); 1058 } 1059 1060 /* 1061 * csid_s_ctrl - Handle set control subdev method 1062 * @ctrl: pointer to v4l2 control structure 1063 * 1064 * Return 0 on success or a negative error code otherwise 1065 */ 1066 static int csid_s_ctrl(struct v4l2_ctrl *ctrl) 1067 { 1068 struct csid_device *csid = container_of(ctrl->handler, 1069 struct csid_device, ctrls); 1070 int ret = -EINVAL; 1071 1072 switch (ctrl->id) { 1073 case V4L2_CID_TEST_PATTERN: 1074 ret = csid_set_test_pattern(csid, ctrl->val); 1075 break; 1076 } 1077 1078 return ret; 1079 } 1080 1081 static const struct v4l2_ctrl_ops csid_ctrl_ops = { 1082 .s_ctrl = csid_s_ctrl, 1083 }; 1084 1085 /* 1086 * msm_csid_subdev_init - Initialize CSID device structure and resources 1087 * @csid: CSID device 1088 * @res: CSID module resources table 1089 * @id: CSID module id 1090 * 1091 * Return 0 on success or a negative error code otherwise 1092 */ 1093 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid, 1094 const struct camss_subdev_resources *res, u8 id) 1095 { 1096 struct device *dev = camss->dev; 1097 struct platform_device *pdev = to_platform_device(dev); 1098 int i, j; 1099 int ret; 1100 1101 csid->camss = camss; 1102 csid->id = id; 1103 csid->res = &res->csid; 1104 1105 if (dev_WARN_ONCE(dev, !csid->res->parent_dev_ops, 1106 "Error: CSID depends on VFE/IFE device ops!\n")) { 1107 return -EINVAL; 1108 } 1109 1110 csid->res->hw_ops->subdev_init(csid); 1111 1112 /* Memory */ 1113 1114 if (camss->res->version == CAMSS_8250) { 1115 /* for titan 480, CSID registers are inside the VFE region, 1116 * between the VFE "top" and "bus" registers. this requires 1117 * VFE to be initialized before CSID 1118 */ 1119 if (id >= 2) /* VFE/CSID lite */ 1120 csid->base = csid->res->parent_dev_ops->get_base_address(camss, id) 1121 + VFE_480_LITE_CSID_OFFSET; 1122 else 1123 csid->base = csid->res->parent_dev_ops->get_base_address(camss, id) 1124 + VFE_480_CSID_OFFSET; 1125 } else { 1126 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]); 1127 if (IS_ERR(csid->base)) 1128 return PTR_ERR(csid->base); 1129 } 1130 1131 /* Interrupt */ 1132 1133 ret = platform_get_irq_byname(pdev, res->interrupt[0]); 1134 if (ret < 0) 1135 return ret; 1136 1137 csid->irq = ret; 1138 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d", 1139 dev_name(dev), MSM_CSID_NAME, csid->id); 1140 ret = devm_request_irq(dev, csid->irq, csid->res->hw_ops->isr, 1141 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN, 1142 csid->irq_name, csid); 1143 if (ret < 0) { 1144 dev_err(dev, "request_irq failed: %d\n", ret); 1145 return ret; 1146 } 1147 1148 /* Clocks */ 1149 1150 csid->nclocks = 0; 1151 while (res->clock[csid->nclocks]) 1152 csid->nclocks++; 1153 1154 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock), 1155 GFP_KERNEL); 1156 if (!csid->clock) 1157 return -ENOMEM; 1158 1159 for (i = 0; i < csid->nclocks; i++) { 1160 struct camss_clock *clock = &csid->clock[i]; 1161 1162 clock->clk = devm_clk_get(dev, res->clock[i]); 1163 if (IS_ERR(clock->clk)) 1164 return PTR_ERR(clock->clk); 1165 1166 clock->name = res->clock[i]; 1167 1168 clock->nfreqs = 0; 1169 while (res->clock_rate[i][clock->nfreqs]) 1170 clock->nfreqs++; 1171 1172 if (!clock->nfreqs) { 1173 clock->freq = NULL; 1174 continue; 1175 } 1176 1177 clock->freq = devm_kcalloc(dev, 1178 clock->nfreqs, 1179 sizeof(*clock->freq), 1180 GFP_KERNEL); 1181 if (!clock->freq) 1182 return -ENOMEM; 1183 1184 for (j = 0; j < clock->nfreqs; j++) 1185 clock->freq[j] = res->clock_rate[i][j]; 1186 } 1187 1188 /* Regulator */ 1189 for (i = 0; i < ARRAY_SIZE(res->regulators); i++) { 1190 if (res->regulators[i]) 1191 csid->num_supplies++; 1192 } 1193 1194 if (csid->num_supplies) { 1195 csid->supplies = devm_kmalloc_array(camss->dev, 1196 csid->num_supplies, 1197 sizeof(*csid->supplies), 1198 GFP_KERNEL); 1199 if (!csid->supplies) 1200 return -ENOMEM; 1201 } 1202 1203 for (i = 0; i < csid->num_supplies; i++) 1204 csid->supplies[i].supply = res->regulators[i]; 1205 1206 ret = devm_regulator_bulk_get(camss->dev, csid->num_supplies, 1207 csid->supplies); 1208 if (ret) 1209 return ret; 1210 1211 init_completion(&csid->reset_complete); 1212 1213 return 0; 1214 } 1215 1216 /* 1217 * msm_csid_get_csid_id - Get CSID HW module id 1218 * @entity: Pointer to CSID media entity structure 1219 * @id: Return CSID HW module id here 1220 */ 1221 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id) 1222 { 1223 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 1224 struct csid_device *csid = v4l2_get_subdevdata(sd); 1225 1226 *id = csid->id; 1227 } 1228 1229 /* 1230 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter 1231 * @lane_cfg - CSI2 lane configuration 1232 * 1233 * Return lane assign 1234 */ 1235 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg) 1236 { 1237 u32 lane_assign = 0; 1238 int i; 1239 1240 for (i = 0; i < lane_cfg->num_data; i++) 1241 lane_assign |= lane_cfg->data[i].pos << (i * 4); 1242 1243 return lane_assign; 1244 } 1245 1246 /* 1247 * csid_link_setup - Setup CSID connections 1248 * @entity: Pointer to media entity structure 1249 * @local: Pointer to local pad 1250 * @remote: Pointer to remote pad 1251 * @flags: Link flags 1252 * 1253 * Return 0 on success 1254 */ 1255 static int csid_link_setup(struct media_entity *entity, 1256 const struct media_pad *local, 1257 const struct media_pad *remote, u32 flags) 1258 { 1259 if (flags & MEDIA_LNK_FL_ENABLED) 1260 if (media_pad_remote_pad_first(local)) 1261 return -EBUSY; 1262 1263 if ((local->flags & MEDIA_PAD_FL_SINK) && 1264 (flags & MEDIA_LNK_FL_ENABLED)) { 1265 struct v4l2_subdev *sd; 1266 struct csid_device *csid; 1267 struct csiphy_device *csiphy; 1268 struct csiphy_lanes_cfg *lane_cfg; 1269 1270 sd = media_entity_to_v4l2_subdev(entity); 1271 csid = v4l2_get_subdevdata(sd); 1272 1273 /* If test generator is enabled */ 1274 /* do not allow a link from CSIPHY to CSID */ 1275 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED && 1276 csid->testgen_mode->cur.val != 0) 1277 return -EBUSY; 1278 1279 sd = media_entity_to_v4l2_subdev(remote->entity); 1280 csiphy = v4l2_get_subdevdata(sd); 1281 1282 /* If a sensor is not linked to CSIPHY */ 1283 /* do no allow a link from CSIPHY to CSID */ 1284 if (!csiphy->cfg.csi2) 1285 return -EPERM; 1286 1287 csid->phy.csiphy_id = csiphy->id; 1288 1289 lane_cfg = &csiphy->cfg.csi2->lane_cfg; 1290 csid->phy.lane_cnt = lane_cfg->num_data; 1291 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg); 1292 } 1293 /* Decide which virtual channels to enable based on which source pads are enabled */ 1294 if (local->flags & MEDIA_PAD_FL_SOURCE) { 1295 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 1296 struct csid_device *csid = v4l2_get_subdevdata(sd); 1297 struct device *dev = csid->camss->dev; 1298 1299 if (flags & MEDIA_LNK_FL_ENABLED) 1300 csid->phy.en_vc |= BIT(local->index - 1); 1301 else 1302 csid->phy.en_vc &= ~BIT(local->index - 1); 1303 1304 csid->phy.need_vc_update = true; 1305 1306 dev_dbg(dev, "%s: Enabled CSID virtual channels mask 0x%x\n", 1307 __func__, csid->phy.en_vc); 1308 } 1309 1310 return 0; 1311 } 1312 1313 static const struct v4l2_subdev_core_ops csid_core_ops = { 1314 .s_power = csid_set_power, 1315 .subscribe_event = v4l2_ctrl_subdev_subscribe_event, 1316 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 1317 }; 1318 1319 static const struct v4l2_subdev_video_ops csid_video_ops = { 1320 .s_stream = csid_set_stream, 1321 }; 1322 1323 static const struct v4l2_subdev_pad_ops csid_pad_ops = { 1324 .enum_mbus_code = csid_enum_mbus_code, 1325 .enum_frame_size = csid_enum_frame_size, 1326 .get_fmt = csid_get_format, 1327 .set_fmt = csid_set_format, 1328 }; 1329 1330 static const struct v4l2_subdev_ops csid_v4l2_ops = { 1331 .core = &csid_core_ops, 1332 .video = &csid_video_ops, 1333 .pad = &csid_pad_ops, 1334 }; 1335 1336 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = { 1337 .open = csid_init_formats, 1338 }; 1339 1340 static const struct media_entity_operations csid_media_ops = { 1341 .link_setup = csid_link_setup, 1342 .link_validate = v4l2_subdev_link_validate, 1343 }; 1344 1345 /* 1346 * msm_csid_register_entity - Register subdev node for CSID module 1347 * @csid: CSID device 1348 * @v4l2_dev: V4L2 device 1349 * 1350 * Return 0 on success or a negative error code otherwise 1351 */ 1352 int msm_csid_register_entity(struct csid_device *csid, 1353 struct v4l2_device *v4l2_dev) 1354 { 1355 struct v4l2_subdev *sd = &csid->subdev; 1356 struct media_pad *pads = csid->pads; 1357 struct device *dev = csid->camss->dev; 1358 int i; 1359 int ret; 1360 1361 v4l2_subdev_init(sd, &csid_v4l2_ops); 1362 sd->internal_ops = &csid_v4l2_internal_ops; 1363 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | 1364 V4L2_SUBDEV_FL_HAS_EVENTS; 1365 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", 1366 MSM_CSID_NAME, csid->id); 1367 v4l2_set_subdevdata(sd, csid); 1368 1369 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) { 1370 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1); 1371 if (ret < 0) { 1372 dev_err(dev, "Failed to init ctrl handler: %d\n", ret); 1373 return ret; 1374 } 1375 1376 csid->testgen_mode = 1377 v4l2_ctrl_new_std_menu_items(&csid->ctrls, 1378 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN, 1379 csid->testgen.nmodes, 0, 0, 1380 csid->testgen.modes); 1381 1382 if (csid->ctrls.error) { 1383 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error); 1384 ret = csid->ctrls.error; 1385 goto free_ctrl; 1386 } 1387 1388 csid->subdev.ctrl_handler = &csid->ctrls; 1389 } 1390 1391 ret = csid_init_formats(sd, NULL); 1392 if (ret < 0) { 1393 dev_err(dev, "Failed to init format: %d\n", ret); 1394 goto free_ctrl; 1395 } 1396 1397 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 1398 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i) 1399 pads[i].flags = MEDIA_PAD_FL_SOURCE; 1400 1401 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; 1402 sd->entity.ops = &csid_media_ops; 1403 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads); 1404 if (ret < 0) { 1405 dev_err(dev, "Failed to init media entity: %d\n", ret); 1406 goto free_ctrl; 1407 } 1408 1409 ret = v4l2_device_register_subdev(v4l2_dev, sd); 1410 if (ret < 0) { 1411 dev_err(dev, "Failed to register subdev: %d\n", ret); 1412 goto media_cleanup; 1413 } 1414 1415 return 0; 1416 1417 media_cleanup: 1418 media_entity_cleanup(&sd->entity); 1419 free_ctrl: 1420 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) 1421 v4l2_ctrl_handler_free(&csid->ctrls); 1422 1423 return ret; 1424 } 1425 1426 /* 1427 * msm_csid_unregister_entity - Unregister CSID module subdev node 1428 * @csid: CSID device 1429 */ 1430 void msm_csid_unregister_entity(struct csid_device *csid) 1431 { 1432 v4l2_device_unregister_subdev(&csid->subdev); 1433 media_entity_cleanup(&csid->subdev.entity); 1434 if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) 1435 v4l2_ctrl_handler_free(&csid->ctrls); 1436 } 1437 1438 inline bool csid_is_lite(struct csid_device *csid) 1439 { 1440 return csid->camss->res->csid_res[csid->id].csid.is_lite; 1441 } 1442