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