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