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