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 csid->phy.need_vc_update = true; 200 201 enable_irq(csid->irq); 202 203 ret = csid->ops->reset(csid); 204 if (ret < 0) { 205 disable_irq(csid->irq); 206 camss_disable_clocks(csid->nclocks, csid->clock); 207 regulator_bulk_disable(csid->num_supplies, 208 csid->supplies); 209 pm_runtime_put_sync(dev); 210 return ret; 211 } 212 213 csid->ops->hw_version(csid); 214 } else { 215 disable_irq(csid->irq); 216 camss_disable_clocks(csid->nclocks, csid->clock); 217 regulator_bulk_disable(csid->num_supplies, 218 csid->supplies); 219 pm_runtime_put_sync(dev); 220 if (version == CAMSS_8250 || version == CAMSS_845) 221 vfe_put(vfe); 222 } 223 224 return ret; 225 } 226 227 /* 228 * csid_set_stream - Enable/disable streaming on CSID module 229 * @sd: CSID V4L2 subdevice 230 * @enable: Requested streaming state 231 * 232 * Main configuration of CSID module is also done here. 233 * 234 * Return 0 on success or a negative error code otherwise 235 */ 236 static int csid_set_stream(struct v4l2_subdev *sd, int enable) 237 { 238 struct csid_device *csid = v4l2_get_subdevdata(sd); 239 int ret; 240 241 if (enable) { 242 ret = v4l2_ctrl_handler_setup(&csid->ctrls); 243 if (ret < 0) { 244 dev_err(csid->camss->dev, 245 "could not sync v4l2 controls: %d\n", ret); 246 return ret; 247 } 248 249 if (!csid->testgen.enabled && 250 !media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK])) 251 return -ENOLINK; 252 } 253 254 if (csid->phy.need_vc_update) { 255 csid->ops->configure_stream(csid, enable); 256 csid->phy.need_vc_update = false; 257 } 258 259 return 0; 260 } 261 262 /* 263 * __csid_get_format - Get pointer to format structure 264 * @csid: CSID device 265 * @cfg: V4L2 subdev pad configuration 266 * @pad: pad from which format is requested 267 * @which: TRY or ACTIVE format 268 * 269 * Return pointer to TRY or ACTIVE format structure 270 */ 271 static struct v4l2_mbus_framefmt * 272 __csid_get_format(struct csid_device *csid, 273 struct v4l2_subdev_state *sd_state, 274 unsigned int pad, 275 enum v4l2_subdev_format_whence which) 276 { 277 if (which == V4L2_SUBDEV_FORMAT_TRY) 278 return v4l2_subdev_get_try_format(&csid->subdev, sd_state, 279 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 * @cfg: V4L2 subdev pad configuration 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_2X8; 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_2X8; 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 * @cfg: V4L2 subdev pad configuration 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 * @cfg: V4L2 subdev pad configuration 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 * @cfg: V4L2 subdev pad configuration 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 * @cfg: V4L2 subdev pad configuration 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_2X8, 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 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 579 if (camss->version == CAMSS_8x16) { 580 csid->ops = &csid_ops_4_1; 581 } else if (camss->version == CAMSS_8x96 || 582 camss->version == CAMSS_660) { 583 csid->ops = &csid_ops_4_7; 584 } else if (camss->version == CAMSS_845 || 585 camss->version == CAMSS_8250) { 586 csid->ops = &csid_ops_gen2; 587 } else { 588 return -EINVAL; 589 } 590 csid->ops->subdev_init(csid); 591 592 /* Memory */ 593 594 if (camss->version == CAMSS_8250) { 595 /* for titan 480, CSID registers are inside the VFE region, 596 * between the VFE "top" and "bus" registers. this requires 597 * VFE to be initialized before CSID 598 */ 599 if (id >= 2) /* VFE/CSID lite */ 600 csid->base = camss->vfe[id].base + VFE_480_LITE_CSID_OFFSET; 601 else 602 csid->base = camss->vfe[id].base + VFE_480_CSID_OFFSET; 603 } else { 604 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]); 605 if (IS_ERR(csid->base)) 606 return PTR_ERR(csid->base); 607 } 608 609 /* Interrupt */ 610 611 ret = platform_get_irq_byname(pdev, res->interrupt[0]); 612 if (ret < 0) 613 return ret; 614 615 csid->irq = ret; 616 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d", 617 dev_name(dev), MSM_CSID_NAME, csid->id); 618 ret = devm_request_irq(dev, csid->irq, csid->ops->isr, 619 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN, 620 csid->irq_name, csid); 621 if (ret < 0) { 622 dev_err(dev, "request_irq failed: %d\n", ret); 623 return ret; 624 } 625 626 /* Clocks */ 627 628 csid->nclocks = 0; 629 while (res->clock[csid->nclocks]) 630 csid->nclocks++; 631 632 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock), 633 GFP_KERNEL); 634 if (!csid->clock) 635 return -ENOMEM; 636 637 for (i = 0; i < csid->nclocks; i++) { 638 struct camss_clock *clock = &csid->clock[i]; 639 640 clock->clk = devm_clk_get(dev, res->clock[i]); 641 if (IS_ERR(clock->clk)) 642 return PTR_ERR(clock->clk); 643 644 clock->name = res->clock[i]; 645 646 clock->nfreqs = 0; 647 while (res->clock_rate[i][clock->nfreqs]) 648 clock->nfreqs++; 649 650 if (!clock->nfreqs) { 651 clock->freq = NULL; 652 continue; 653 } 654 655 clock->freq = devm_kcalloc(dev, 656 clock->nfreqs, 657 sizeof(*clock->freq), 658 GFP_KERNEL); 659 if (!clock->freq) 660 return -ENOMEM; 661 662 for (j = 0; j < clock->nfreqs; j++) 663 clock->freq[j] = res->clock_rate[i][j]; 664 } 665 666 /* Regulator */ 667 for (i = 0; i < ARRAY_SIZE(res->regulators); i++) { 668 if (res->regulators[i]) 669 csid->num_supplies++; 670 } 671 672 if (csid->num_supplies) { 673 csid->supplies = devm_kmalloc_array(camss->dev, 674 csid->num_supplies, 675 sizeof(*csid->supplies), 676 GFP_KERNEL); 677 if (!csid->supplies) 678 return -ENOMEM; 679 } 680 681 for (i = 0; i < csid->num_supplies; i++) 682 csid->supplies[i].supply = res->regulators[i]; 683 684 ret = devm_regulator_bulk_get(camss->dev, csid->num_supplies, 685 csid->supplies); 686 if (ret) 687 return ret; 688 689 init_completion(&csid->reset_complete); 690 691 return 0; 692 } 693 694 /* 695 * msm_csid_get_csid_id - Get CSID HW module id 696 * @entity: Pointer to CSID media entity structure 697 * @id: Return CSID HW module id here 698 */ 699 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id) 700 { 701 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 702 struct csid_device *csid = v4l2_get_subdevdata(sd); 703 704 *id = csid->id; 705 } 706 707 /* 708 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter 709 * @lane_cfg - CSI2 lane configuration 710 * 711 * Return lane assign 712 */ 713 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg) 714 { 715 u32 lane_assign = 0; 716 int i; 717 718 for (i = 0; i < lane_cfg->num_data; i++) 719 lane_assign |= lane_cfg->data[i].pos << (i * 4); 720 721 return lane_assign; 722 } 723 724 /* 725 * csid_link_setup - Setup CSID connections 726 * @entity: Pointer to media entity structure 727 * @local: Pointer to local pad 728 * @remote: Pointer to remote pad 729 * @flags: Link flags 730 * 731 * Return 0 on success 732 */ 733 static int csid_link_setup(struct media_entity *entity, 734 const struct media_pad *local, 735 const struct media_pad *remote, u32 flags) 736 { 737 if (flags & MEDIA_LNK_FL_ENABLED) 738 if (media_pad_remote_pad_first(local)) 739 return -EBUSY; 740 741 if ((local->flags & MEDIA_PAD_FL_SINK) && 742 (flags & MEDIA_LNK_FL_ENABLED)) { 743 struct v4l2_subdev *sd; 744 struct csid_device *csid; 745 struct csiphy_device *csiphy; 746 struct csiphy_lanes_cfg *lane_cfg; 747 748 sd = media_entity_to_v4l2_subdev(entity); 749 csid = v4l2_get_subdevdata(sd); 750 751 /* If test generator is enabled */ 752 /* do not allow a link from CSIPHY to CSID */ 753 if (csid->testgen_mode->cur.val != 0) 754 return -EBUSY; 755 756 sd = media_entity_to_v4l2_subdev(remote->entity); 757 csiphy = v4l2_get_subdevdata(sd); 758 759 /* If a sensor is not linked to CSIPHY */ 760 /* do no allow a link from CSIPHY to CSID */ 761 if (!csiphy->cfg.csi2) 762 return -EPERM; 763 764 csid->phy.csiphy_id = csiphy->id; 765 766 lane_cfg = &csiphy->cfg.csi2->lane_cfg; 767 csid->phy.lane_cnt = lane_cfg->num_data; 768 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg); 769 } 770 /* Decide which virtual channels to enable based on which source pads are enabled */ 771 if (local->flags & MEDIA_PAD_FL_SOURCE) { 772 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 773 struct csid_device *csid = v4l2_get_subdevdata(sd); 774 struct device *dev = csid->camss->dev; 775 776 if (flags & MEDIA_LNK_FL_ENABLED) 777 csid->phy.en_vc |= BIT(local->index - 1); 778 else 779 csid->phy.en_vc &= ~BIT(local->index - 1); 780 781 csid->phy.need_vc_update = true; 782 783 dev_dbg(dev, "%s: Enabled CSID virtual channels mask 0x%x\n", 784 __func__, csid->phy.en_vc); 785 } 786 787 return 0; 788 } 789 790 static const struct v4l2_subdev_core_ops csid_core_ops = { 791 .s_power = csid_set_power, 792 .subscribe_event = v4l2_ctrl_subdev_subscribe_event, 793 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 794 }; 795 796 static const struct v4l2_subdev_video_ops csid_video_ops = { 797 .s_stream = csid_set_stream, 798 }; 799 800 static const struct v4l2_subdev_pad_ops csid_pad_ops = { 801 .enum_mbus_code = csid_enum_mbus_code, 802 .enum_frame_size = csid_enum_frame_size, 803 .get_fmt = csid_get_format, 804 .set_fmt = csid_set_format, 805 }; 806 807 static const struct v4l2_subdev_ops csid_v4l2_ops = { 808 .core = &csid_core_ops, 809 .video = &csid_video_ops, 810 .pad = &csid_pad_ops, 811 }; 812 813 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = { 814 .open = csid_init_formats, 815 }; 816 817 static const struct media_entity_operations csid_media_ops = { 818 .link_setup = csid_link_setup, 819 .link_validate = v4l2_subdev_link_validate, 820 }; 821 822 /* 823 * msm_csid_register_entity - Register subdev node for CSID module 824 * @csid: CSID device 825 * @v4l2_dev: V4L2 device 826 * 827 * Return 0 on success or a negative error code otherwise 828 */ 829 int msm_csid_register_entity(struct csid_device *csid, 830 struct v4l2_device *v4l2_dev) 831 { 832 struct v4l2_subdev *sd = &csid->subdev; 833 struct media_pad *pads = csid->pads; 834 struct device *dev = csid->camss->dev; 835 int i; 836 int ret; 837 838 v4l2_subdev_init(sd, &csid_v4l2_ops); 839 sd->internal_ops = &csid_v4l2_internal_ops; 840 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | 841 V4L2_SUBDEV_FL_HAS_EVENTS; 842 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", 843 MSM_CSID_NAME, csid->id); 844 v4l2_set_subdevdata(sd, csid); 845 846 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1); 847 if (ret < 0) { 848 dev_err(dev, "Failed to init ctrl handler: %d\n", ret); 849 return ret; 850 } 851 852 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls, 853 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN, 854 csid->testgen.nmodes, 0, 0, 855 csid->testgen.modes); 856 857 if (csid->ctrls.error) { 858 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error); 859 ret = csid->ctrls.error; 860 goto free_ctrl; 861 } 862 863 csid->subdev.ctrl_handler = &csid->ctrls; 864 865 ret = csid_init_formats(sd, NULL); 866 if (ret < 0) { 867 dev_err(dev, "Failed to init format: %d\n", ret); 868 goto free_ctrl; 869 } 870 871 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 872 for (i = MSM_CSID_PAD_FIRST_SRC; i < MSM_CSID_PADS_NUM; ++i) 873 pads[i].flags = MEDIA_PAD_FL_SOURCE; 874 875 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; 876 sd->entity.ops = &csid_media_ops; 877 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads); 878 if (ret < 0) { 879 dev_err(dev, "Failed to init media entity: %d\n", ret); 880 goto free_ctrl; 881 } 882 883 ret = v4l2_device_register_subdev(v4l2_dev, sd); 884 if (ret < 0) { 885 dev_err(dev, "Failed to register subdev: %d\n", ret); 886 goto media_cleanup; 887 } 888 889 return 0; 890 891 media_cleanup: 892 media_entity_cleanup(&sd->entity); 893 free_ctrl: 894 v4l2_ctrl_handler_free(&csid->ctrls); 895 896 return ret; 897 } 898 899 /* 900 * msm_csid_unregister_entity - Unregister CSID module subdev node 901 * @csid: CSID device 902 */ 903 void msm_csid_unregister_entity(struct csid_device *csid) 904 { 905 v4l2_device_unregister_subdev(&csid->subdev); 906 media_entity_cleanup(&csid->subdev.entity); 907 v4l2_ctrl_handler_free(&csid->ctrls); 908 } 909