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 return PTR_ERR(csid->base); 573 574 /* Interrupt */ 575 576 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, 577 res->interrupt[0]); 578 if (!r) { 579 dev_err(dev, "missing IRQ\n"); 580 return -EINVAL; 581 } 582 583 csid->irq = r->start; 584 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d", 585 dev_name(dev), MSM_CSID_NAME, csid->id); 586 ret = devm_request_irq(dev, csid->irq, csid->ops->isr, 587 IRQF_TRIGGER_RISING, csid->irq_name, csid); 588 if (ret < 0) { 589 dev_err(dev, "request_irq failed: %d\n", ret); 590 return ret; 591 } 592 593 disable_irq(csid->irq); 594 595 /* Clocks */ 596 597 csid->nclocks = 0; 598 while (res->clock[csid->nclocks]) 599 csid->nclocks++; 600 601 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock), 602 GFP_KERNEL); 603 if (!csid->clock) 604 return -ENOMEM; 605 606 for (i = 0; i < csid->nclocks; i++) { 607 struct camss_clock *clock = &csid->clock[i]; 608 609 clock->clk = devm_clk_get(dev, res->clock[i]); 610 if (IS_ERR(clock->clk)) 611 return PTR_ERR(clock->clk); 612 613 clock->name = res->clock[i]; 614 615 clock->nfreqs = 0; 616 while (res->clock_rate[i][clock->nfreqs]) 617 clock->nfreqs++; 618 619 if (!clock->nfreqs) { 620 clock->freq = NULL; 621 continue; 622 } 623 624 clock->freq = devm_kcalloc(dev, 625 clock->nfreqs, 626 sizeof(*clock->freq), 627 GFP_KERNEL); 628 if (!clock->freq) 629 return -ENOMEM; 630 631 for (j = 0; j < clock->nfreqs; j++) 632 clock->freq[j] = res->clock_rate[i][j]; 633 } 634 635 /* Regulator */ 636 637 csid->vdda = devm_regulator_get(dev, res->regulator[0]); 638 if (IS_ERR(csid->vdda)) { 639 dev_err(dev, "could not get regulator\n"); 640 return PTR_ERR(csid->vdda); 641 } 642 643 init_completion(&csid->reset_complete); 644 645 return 0; 646 } 647 648 /* 649 * msm_csid_get_csid_id - Get CSID HW module id 650 * @entity: Pointer to CSID media entity structure 651 * @id: Return CSID HW module id here 652 */ 653 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id) 654 { 655 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 656 struct csid_device *csid = v4l2_get_subdevdata(sd); 657 658 *id = csid->id; 659 } 660 661 /* 662 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter 663 * @lane_cfg - CSI2 lane configuration 664 * 665 * Return lane assign 666 */ 667 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg) 668 { 669 u32 lane_assign = 0; 670 int i; 671 672 for (i = 0; i < lane_cfg->num_data; i++) 673 lane_assign |= lane_cfg->data[i].pos << (i * 4); 674 675 return lane_assign; 676 } 677 678 /* 679 * csid_link_setup - Setup CSID connections 680 * @entity: Pointer to media entity structure 681 * @local: Pointer to local pad 682 * @remote: Pointer to remote pad 683 * @flags: Link flags 684 * 685 * Return 0 on success 686 */ 687 static int csid_link_setup(struct media_entity *entity, 688 const struct media_pad *local, 689 const struct media_pad *remote, u32 flags) 690 { 691 if (flags & MEDIA_LNK_FL_ENABLED) 692 if (media_entity_remote_pad(local)) 693 return -EBUSY; 694 695 if ((local->flags & MEDIA_PAD_FL_SINK) && 696 (flags & MEDIA_LNK_FL_ENABLED)) { 697 struct v4l2_subdev *sd; 698 struct csid_device *csid; 699 struct csiphy_device *csiphy; 700 struct csiphy_lanes_cfg *lane_cfg; 701 struct v4l2_subdev_format format = { 0 }; 702 703 sd = media_entity_to_v4l2_subdev(entity); 704 csid = v4l2_get_subdevdata(sd); 705 706 /* If test generator is enabled */ 707 /* do not allow a link from CSIPHY to CSID */ 708 if (csid->testgen_mode->cur.val != 0) 709 return -EBUSY; 710 711 sd = media_entity_to_v4l2_subdev(remote->entity); 712 csiphy = v4l2_get_subdevdata(sd); 713 714 /* If a sensor is not linked to CSIPHY */ 715 /* do no allow a link from CSIPHY to CSID */ 716 if (!csiphy->cfg.csi2) 717 return -EPERM; 718 719 csid->phy.csiphy_id = csiphy->id; 720 721 lane_cfg = &csiphy->cfg.csi2->lane_cfg; 722 csid->phy.lane_cnt = lane_cfg->num_data; 723 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg); 724 725 /* Reset format on source pad to sink pad format */ 726 format.pad = MSM_CSID_PAD_SRC; 727 format.which = V4L2_SUBDEV_FORMAT_ACTIVE; 728 csid_set_format(&csid->subdev, NULL, &format); 729 } 730 731 return 0; 732 } 733 734 static const struct v4l2_subdev_core_ops csid_core_ops = { 735 .s_power = csid_set_power, 736 .subscribe_event = v4l2_ctrl_subdev_subscribe_event, 737 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 738 }; 739 740 static const struct v4l2_subdev_video_ops csid_video_ops = { 741 .s_stream = csid_set_stream, 742 }; 743 744 static const struct v4l2_subdev_pad_ops csid_pad_ops = { 745 .enum_mbus_code = csid_enum_mbus_code, 746 .enum_frame_size = csid_enum_frame_size, 747 .get_fmt = csid_get_format, 748 .set_fmt = csid_set_format, 749 }; 750 751 static const struct v4l2_subdev_ops csid_v4l2_ops = { 752 .core = &csid_core_ops, 753 .video = &csid_video_ops, 754 .pad = &csid_pad_ops, 755 }; 756 757 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = { 758 .open = csid_init_formats, 759 }; 760 761 static const struct media_entity_operations csid_media_ops = { 762 .link_setup = csid_link_setup, 763 .link_validate = v4l2_subdev_link_validate, 764 }; 765 766 /* 767 * msm_csid_register_entity - Register subdev node for CSID module 768 * @csid: CSID device 769 * @v4l2_dev: V4L2 device 770 * 771 * Return 0 on success or a negative error code otherwise 772 */ 773 int msm_csid_register_entity(struct csid_device *csid, 774 struct v4l2_device *v4l2_dev) 775 { 776 struct v4l2_subdev *sd = &csid->subdev; 777 struct media_pad *pads = csid->pads; 778 struct device *dev = csid->camss->dev; 779 int ret; 780 781 v4l2_subdev_init(sd, &csid_v4l2_ops); 782 sd->internal_ops = &csid_v4l2_internal_ops; 783 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | 784 V4L2_SUBDEV_FL_HAS_EVENTS; 785 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", 786 MSM_CSID_NAME, csid->id); 787 v4l2_set_subdevdata(sd, csid); 788 789 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1); 790 if (ret < 0) { 791 dev_err(dev, "Failed to init ctrl handler: %d\n", ret); 792 return ret; 793 } 794 795 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls, 796 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN, 797 csid->testgen.nmodes, 0, 0, 798 csid->testgen.modes); 799 800 if (csid->ctrls.error) { 801 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error); 802 ret = csid->ctrls.error; 803 goto free_ctrl; 804 } 805 806 csid->subdev.ctrl_handler = &csid->ctrls; 807 808 ret = csid_init_formats(sd, NULL); 809 if (ret < 0) { 810 dev_err(dev, "Failed to init format: %d\n", ret); 811 goto free_ctrl; 812 } 813 814 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 815 pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE; 816 817 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; 818 sd->entity.ops = &csid_media_ops; 819 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads); 820 if (ret < 0) { 821 dev_err(dev, "Failed to init media entity: %d\n", ret); 822 goto free_ctrl; 823 } 824 825 ret = v4l2_device_register_subdev(v4l2_dev, sd); 826 if (ret < 0) { 827 dev_err(dev, "Failed to register subdev: %d\n", ret); 828 goto media_cleanup; 829 } 830 831 return 0; 832 833 media_cleanup: 834 media_entity_cleanup(&sd->entity); 835 free_ctrl: 836 v4l2_ctrl_handler_free(&csid->ctrls); 837 838 return ret; 839 } 840 841 /* 842 * msm_csid_unregister_entity - Unregister CSID module subdev node 843 * @csid: CSID device 844 */ 845 void msm_csid_unregister_entity(struct csid_device *csid) 846 { 847 v4l2_device_unregister_subdev(&csid->subdev); 848 media_entity_cleanup(&csid->subdev.entity); 849 v4l2_ctrl_handler_free(&csid->ctrls); 850 } 851