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