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_state *sd_state, 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, sd_state, 254 pad); 255 256 return &csid->fmt[pad]; 257 } 258 259 /* 260 * csid_try_format - Handle try format by pad subdev method 261 * @csid: CSID device 262 * @cfg: V4L2 subdev pad configuration 263 * @pad: pad on which format is requested 264 * @fmt: pointer to v4l2 format structure 265 * @which: wanted subdev format 266 */ 267 static void csid_try_format(struct csid_device *csid, 268 struct v4l2_subdev_state *sd_state, 269 unsigned int pad, 270 struct v4l2_mbus_framefmt *fmt, 271 enum v4l2_subdev_format_whence which) 272 { 273 unsigned int i; 274 275 switch (pad) { 276 case MSM_CSID_PAD_SINK: 277 /* Set format on sink pad */ 278 279 for (i = 0; i < csid->nformats; i++) 280 if (fmt->code == csid->formats[i].code) 281 break; 282 283 /* If not found, use UYVY as default */ 284 if (i >= csid->nformats) 285 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8; 286 287 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 288 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 289 290 fmt->field = V4L2_FIELD_NONE; 291 fmt->colorspace = V4L2_COLORSPACE_SRGB; 292 293 break; 294 295 case MSM_CSID_PAD_SRC: 296 if (csid->testgen_mode->cur.val == 0) { 297 /* Test generator is disabled, */ 298 /* keep pad formats in sync */ 299 u32 code = fmt->code; 300 301 *fmt = *__csid_get_format(csid, sd_state, 302 MSM_CSID_PAD_SINK, which); 303 fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code); 304 } else { 305 /* Test generator is enabled, set format on source */ 306 /* pad to allow test generator usage */ 307 308 for (i = 0; i < csid->nformats; i++) 309 if (csid->formats[i].code == fmt->code) 310 break; 311 312 /* If not found, use UYVY as default */ 313 if (i >= csid->nformats) 314 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8; 315 316 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 317 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 318 319 fmt->field = V4L2_FIELD_NONE; 320 } 321 break; 322 } 323 324 fmt->colorspace = V4L2_COLORSPACE_SRGB; 325 } 326 327 /* 328 * csid_enum_mbus_code - Handle pixel format enumeration 329 * @sd: CSID V4L2 subdevice 330 * @cfg: V4L2 subdev pad configuration 331 * @code: pointer to v4l2_subdev_mbus_code_enum structure 332 * return -EINVAL or zero on success 333 */ 334 static int csid_enum_mbus_code(struct v4l2_subdev *sd, 335 struct v4l2_subdev_state *sd_state, 336 struct v4l2_subdev_mbus_code_enum *code) 337 { 338 struct csid_device *csid = v4l2_get_subdevdata(sd); 339 340 if (code->pad == MSM_CSID_PAD_SINK) { 341 if (code->index >= csid->nformats) 342 return -EINVAL; 343 344 code->code = csid->formats[code->index].code; 345 } else { 346 if (csid->testgen_mode->cur.val == 0) { 347 struct v4l2_mbus_framefmt *sink_fmt; 348 349 sink_fmt = __csid_get_format(csid, sd_state, 350 MSM_CSID_PAD_SINK, 351 code->which); 352 353 code->code = csid->ops->src_pad_code(csid, sink_fmt->code, 354 code->index, 0); 355 if (!code->code) 356 return -EINVAL; 357 } else { 358 if (code->index >= csid->nformats) 359 return -EINVAL; 360 361 code->code = csid->formats[code->index].code; 362 } 363 } 364 365 return 0; 366 } 367 368 /* 369 * csid_enum_frame_size - Handle frame size enumeration 370 * @sd: CSID V4L2 subdevice 371 * @cfg: V4L2 subdev pad configuration 372 * @fse: pointer to v4l2_subdev_frame_size_enum structure 373 * return -EINVAL or zero on success 374 */ 375 static int csid_enum_frame_size(struct v4l2_subdev *sd, 376 struct v4l2_subdev_state *sd_state, 377 struct v4l2_subdev_frame_size_enum *fse) 378 { 379 struct csid_device *csid = v4l2_get_subdevdata(sd); 380 struct v4l2_mbus_framefmt format; 381 382 if (fse->index != 0) 383 return -EINVAL; 384 385 format.code = fse->code; 386 format.width = 1; 387 format.height = 1; 388 csid_try_format(csid, sd_state, fse->pad, &format, fse->which); 389 fse->min_width = format.width; 390 fse->min_height = format.height; 391 392 if (format.code != fse->code) 393 return -EINVAL; 394 395 format.code = fse->code; 396 format.width = -1; 397 format.height = -1; 398 csid_try_format(csid, sd_state, fse->pad, &format, fse->which); 399 fse->max_width = format.width; 400 fse->max_height = format.height; 401 402 return 0; 403 } 404 405 /* 406 * csid_get_format - Handle get format by pads subdev method 407 * @sd: CSID V4L2 subdevice 408 * @cfg: V4L2 subdev pad configuration 409 * @fmt: pointer to v4l2 subdev format structure 410 * 411 * Return -EINVAL or zero on success 412 */ 413 static int csid_get_format(struct v4l2_subdev *sd, 414 struct v4l2_subdev_state *sd_state, 415 struct v4l2_subdev_format *fmt) 416 { 417 struct csid_device *csid = v4l2_get_subdevdata(sd); 418 struct v4l2_mbus_framefmt *format; 419 420 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which); 421 if (format == NULL) 422 return -EINVAL; 423 424 fmt->format = *format; 425 426 return 0; 427 } 428 429 /* 430 * csid_set_format - Handle set format by pads subdev method 431 * @sd: CSID V4L2 subdevice 432 * @cfg: V4L2 subdev pad configuration 433 * @fmt: pointer to v4l2 subdev format structure 434 * 435 * Return -EINVAL or zero on success 436 */ 437 static int csid_set_format(struct v4l2_subdev *sd, 438 struct v4l2_subdev_state *sd_state, 439 struct v4l2_subdev_format *fmt) 440 { 441 struct csid_device *csid = v4l2_get_subdevdata(sd); 442 struct v4l2_mbus_framefmt *format; 443 444 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which); 445 if (format == NULL) 446 return -EINVAL; 447 448 csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which); 449 *format = fmt->format; 450 451 /* Propagate the format from sink to source */ 452 if (fmt->pad == MSM_CSID_PAD_SINK) { 453 format = __csid_get_format(csid, sd_state, MSM_CSID_PAD_SRC, 454 fmt->which); 455 456 *format = fmt->format; 457 csid_try_format(csid, sd_state, MSM_CSID_PAD_SRC, format, 458 fmt->which); 459 } 460 461 return 0; 462 } 463 464 /* 465 * csid_init_formats - Initialize formats on all pads 466 * @sd: CSID V4L2 subdevice 467 * @fh: V4L2 subdev file handle 468 * 469 * Initialize all pad formats with default values. 470 * 471 * Return 0 on success or a negative error code otherwise 472 */ 473 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) 474 { 475 struct v4l2_subdev_format format = { 476 .pad = MSM_CSID_PAD_SINK, 477 .which = fh ? V4L2_SUBDEV_FORMAT_TRY : 478 V4L2_SUBDEV_FORMAT_ACTIVE, 479 .format = { 480 .code = MEDIA_BUS_FMT_UYVY8_2X8, 481 .width = 1920, 482 .height = 1080 483 } 484 }; 485 486 return csid_set_format(sd, fh ? fh->state : NULL, &format); 487 } 488 489 /* 490 * csid_set_test_pattern - Set test generator's pattern mode 491 * @csid: CSID device 492 * @value: desired test pattern mode 493 * 494 * Return 0 on success or a negative error code otherwise 495 */ 496 static int csid_set_test_pattern(struct csid_device *csid, s32 value) 497 { 498 struct csid_testgen_config *tg = &csid->testgen; 499 500 /* If CSID is linked to CSIPHY, do not allow to enable test generator */ 501 if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) 502 return -EBUSY; 503 504 tg->enabled = !!value; 505 506 return csid->ops->configure_testgen_pattern(csid, value); 507 } 508 509 /* 510 * csid_s_ctrl - Handle set control subdev method 511 * @ctrl: pointer to v4l2 control structure 512 * 513 * Return 0 on success or a negative error code otherwise 514 */ 515 static int csid_s_ctrl(struct v4l2_ctrl *ctrl) 516 { 517 struct csid_device *csid = container_of(ctrl->handler, 518 struct csid_device, ctrls); 519 int ret = -EINVAL; 520 521 switch (ctrl->id) { 522 case V4L2_CID_TEST_PATTERN: 523 ret = csid_set_test_pattern(csid, ctrl->val); 524 break; 525 } 526 527 return ret; 528 } 529 530 static const struct v4l2_ctrl_ops csid_ctrl_ops = { 531 .s_ctrl = csid_s_ctrl, 532 }; 533 534 /* 535 * msm_csid_subdev_init - Initialize CSID device structure and resources 536 * @csid: CSID device 537 * @res: CSID module resources table 538 * @id: CSID module id 539 * 540 * Return 0 on success or a negative error code otherwise 541 */ 542 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid, 543 const struct resources *res, u8 id) 544 { 545 struct device *dev = camss->dev; 546 struct platform_device *pdev = to_platform_device(dev); 547 struct resource *r; 548 int i, j; 549 int ret; 550 551 csid->camss = camss; 552 csid->id = id; 553 554 if (camss->version == CAMSS_8x16) { 555 csid->ops = &csid_ops_4_1; 556 } else if (camss->version == CAMSS_8x96 || 557 camss->version == CAMSS_660) { 558 csid->ops = &csid_ops_4_7; 559 } else if (camss->version == CAMSS_845) { 560 csid->ops = &csid_ops_170; 561 } else { 562 return -EINVAL; 563 } 564 csid->ops->subdev_init(csid); 565 566 /* Memory */ 567 568 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]); 569 if (IS_ERR(csid->base)) 570 return PTR_ERR(csid->base); 571 572 /* Interrupt */ 573 574 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, 575 res->interrupt[0]); 576 if (!r) { 577 dev_err(dev, "missing IRQ\n"); 578 return -EINVAL; 579 } 580 581 csid->irq = r->start; 582 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d", 583 dev_name(dev), MSM_CSID_NAME, csid->id); 584 ret = devm_request_irq(dev, csid->irq, csid->ops->isr, 585 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN, 586 csid->irq_name, csid); 587 if (ret < 0) { 588 dev_err(dev, "request_irq failed: %d\n", ret); 589 return ret; 590 } 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