1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * camss-csiphy.c 4 * 5 * Qualcomm MSM Camera Subsystem - CSIPHY Module 6 * 7 * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved. 8 * Copyright (C) 2016-2018 Linaro Ltd. 9 */ 10 #include <linux/clk.h> 11 #include <linux/delay.h> 12 #include <linux/interrupt.h> 13 #include <linux/kernel.h> 14 #include <linux/of.h> 15 #include <linux/platform_device.h> 16 #include <linux/pm_runtime.h> 17 #include <media/media-entity.h> 18 #include <media/v4l2-device.h> 19 #include <media/v4l2-subdev.h> 20 21 #include "camss-csiphy.h" 22 #include "camss.h" 23 24 #define MSM_CSIPHY_NAME "msm_csiphy" 25 26 static const struct { 27 u32 code; 28 u8 bpp; 29 } csiphy_formats[] = { 30 { 31 MEDIA_BUS_FMT_UYVY8_2X8, 32 8, 33 }, 34 { 35 MEDIA_BUS_FMT_VYUY8_2X8, 36 8, 37 }, 38 { 39 MEDIA_BUS_FMT_YUYV8_2X8, 40 8, 41 }, 42 { 43 MEDIA_BUS_FMT_YVYU8_2X8, 44 8, 45 }, 46 { 47 MEDIA_BUS_FMT_SBGGR8_1X8, 48 8, 49 }, 50 { 51 MEDIA_BUS_FMT_SGBRG8_1X8, 52 8, 53 }, 54 { 55 MEDIA_BUS_FMT_SGRBG8_1X8, 56 8, 57 }, 58 { 59 MEDIA_BUS_FMT_SRGGB8_1X8, 60 8, 61 }, 62 { 63 MEDIA_BUS_FMT_SBGGR10_1X10, 64 10, 65 }, 66 { 67 MEDIA_BUS_FMT_SGBRG10_1X10, 68 10, 69 }, 70 { 71 MEDIA_BUS_FMT_SGRBG10_1X10, 72 10, 73 }, 74 { 75 MEDIA_BUS_FMT_SRGGB10_1X10, 76 10, 77 }, 78 { 79 MEDIA_BUS_FMT_SBGGR12_1X12, 80 12, 81 }, 82 { 83 MEDIA_BUS_FMT_SGBRG12_1X12, 84 12, 85 }, 86 { 87 MEDIA_BUS_FMT_SGRBG12_1X12, 88 12, 89 }, 90 { 91 MEDIA_BUS_FMT_SRGGB12_1X12, 92 12, 93 } 94 }; 95 96 /* 97 * csiphy_get_bpp - map media bus format to bits per pixel 98 * @code: media bus format code 99 * 100 * Return number of bits per pixel 101 */ 102 static u8 csiphy_get_bpp(u32 code) 103 { 104 unsigned int i; 105 106 for (i = 0; i < ARRAY_SIZE(csiphy_formats); i++) 107 if (code == csiphy_formats[i].code) 108 return csiphy_formats[i].bpp; 109 110 WARN(1, "Unknown format\n"); 111 112 return csiphy_formats[0].bpp; 113 } 114 115 /* 116 * csiphy_set_clock_rates - Calculate and set clock rates on CSIPHY module 117 * @csiphy: CSIPHY device 118 */ 119 static int csiphy_set_clock_rates(struct csiphy_device *csiphy) 120 { 121 struct device *dev = csiphy->camss->dev; 122 u32 pixel_clock; 123 int i, j; 124 int ret; 125 126 ret = camss_get_pixel_clock(&csiphy->subdev.entity, &pixel_clock); 127 if (ret) 128 pixel_clock = 0; 129 130 for (i = 0; i < csiphy->nclocks; i++) { 131 struct camss_clock *clock = &csiphy->clock[i]; 132 133 if (!strcmp(clock->name, "csiphy0_timer") || 134 !strcmp(clock->name, "csiphy1_timer") || 135 !strcmp(clock->name, "csiphy2_timer")) { 136 u8 bpp = csiphy_get_bpp( 137 csiphy->fmt[MSM_CSIPHY_PAD_SINK].code); 138 u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data; 139 u64 min_rate = pixel_clock * bpp / (2 * num_lanes * 4); 140 long round_rate; 141 142 camss_add_clock_margin(&min_rate); 143 144 for (j = 0; j < clock->nfreqs; j++) 145 if (min_rate < clock->freq[j]) 146 break; 147 148 if (j == clock->nfreqs) { 149 dev_err(dev, 150 "Pixel clock is too high for CSIPHY\n"); 151 return -EINVAL; 152 } 153 154 /* if sensor pixel clock is not available */ 155 /* set highest possible CSIPHY clock rate */ 156 if (min_rate == 0) 157 j = clock->nfreqs - 1; 158 159 round_rate = clk_round_rate(clock->clk, clock->freq[j]); 160 if (round_rate < 0) { 161 dev_err(dev, "clk round rate failed: %ld\n", 162 round_rate); 163 return -EINVAL; 164 } 165 166 csiphy->timer_clk_rate = round_rate; 167 168 ret = clk_set_rate(clock->clk, csiphy->timer_clk_rate); 169 if (ret < 0) { 170 dev_err(dev, "clk set rate failed: %d\n", ret); 171 return ret; 172 } 173 } 174 } 175 176 return 0; 177 } 178 179 /* 180 * csiphy_set_power - Power on/off CSIPHY module 181 * @sd: CSIPHY V4L2 subdevice 182 * @on: Requested power state 183 * 184 * Return 0 on success or a negative error code otherwise 185 */ 186 static int csiphy_set_power(struct v4l2_subdev *sd, int on) 187 { 188 struct csiphy_device *csiphy = v4l2_get_subdevdata(sd); 189 struct device *dev = csiphy->camss->dev; 190 191 if (on) { 192 int ret; 193 194 ret = pm_runtime_get_sync(dev); 195 if (ret < 0) 196 return ret; 197 198 ret = csiphy_set_clock_rates(csiphy); 199 if (ret < 0) { 200 pm_runtime_put_sync(dev); 201 return ret; 202 } 203 204 ret = camss_enable_clocks(csiphy->nclocks, csiphy->clock, dev); 205 if (ret < 0) { 206 pm_runtime_put_sync(dev); 207 return ret; 208 } 209 210 enable_irq(csiphy->irq); 211 212 csiphy->ops->reset(csiphy); 213 214 csiphy->ops->hw_version_read(csiphy, dev); 215 } else { 216 disable_irq(csiphy->irq); 217 218 camss_disable_clocks(csiphy->nclocks, csiphy->clock); 219 220 pm_runtime_put_sync(dev); 221 } 222 223 return 0; 224 } 225 226 /* 227 * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter 228 * @lane_cfg - CSI2 lane configuration 229 * 230 * Return lane mask 231 */ 232 static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg) 233 { 234 u8 lane_mask; 235 int i; 236 237 lane_mask = 1 << lane_cfg->clk.pos; 238 239 for (i = 0; i < lane_cfg->num_data; i++) 240 lane_mask |= 1 << lane_cfg->data[i].pos; 241 242 return lane_mask; 243 } 244 245 /* 246 * csiphy_stream_on - Enable streaming on CSIPHY module 247 * @csiphy: CSIPHY device 248 * 249 * Helper function to enable streaming on CSIPHY module. 250 * Main configuration of CSIPHY module is also done here. 251 * 252 * Return 0 on success or a negative error code otherwise 253 */ 254 static int csiphy_stream_on(struct csiphy_device *csiphy) 255 { 256 struct csiphy_config *cfg = &csiphy->cfg; 257 u32 pixel_clock; 258 u8 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg); 259 u8 bpp = csiphy_get_bpp(csiphy->fmt[MSM_CSIPHY_PAD_SINK].code); 260 u8 val; 261 int ret; 262 263 ret = camss_get_pixel_clock(&csiphy->subdev.entity, &pixel_clock); 264 if (ret) { 265 dev_err(csiphy->camss->dev, 266 "Cannot get CSI2 transmitter's pixel clock\n"); 267 return -EINVAL; 268 } 269 if (!pixel_clock) { 270 dev_err(csiphy->camss->dev, 271 "Got pixel clock == 0, cannot continue\n"); 272 return -EINVAL; 273 } 274 275 val = readl_relaxed(csiphy->base_clk_mux); 276 if (cfg->combo_mode && (lane_mask & 0x18) == 0x18) { 277 val &= ~0xf0; 278 val |= cfg->csid_id << 4; 279 } else { 280 val &= ~0xf; 281 val |= cfg->csid_id; 282 } 283 writel_relaxed(val, csiphy->base_clk_mux); 284 wmb(); 285 286 csiphy->ops->lanes_enable(csiphy, cfg, pixel_clock, bpp, lane_mask); 287 288 return 0; 289 } 290 291 /* 292 * csiphy_stream_off - Disable streaming on CSIPHY module 293 * @csiphy: CSIPHY device 294 * 295 * Helper function to disable streaming on CSIPHY module 296 */ 297 static void csiphy_stream_off(struct csiphy_device *csiphy) 298 { 299 u8 lane_mask = csiphy_get_lane_mask(&csiphy->cfg.csi2->lane_cfg); 300 301 csiphy->ops->lanes_disable(csiphy, lane_mask); 302 } 303 304 305 /* 306 * csiphy_set_stream - Enable/disable streaming on CSIPHY module 307 * @sd: CSIPHY V4L2 subdevice 308 * @enable: Requested streaming state 309 * 310 * Return 0 on success or a negative error code otherwise 311 */ 312 static int csiphy_set_stream(struct v4l2_subdev *sd, int enable) 313 { 314 struct csiphy_device *csiphy = v4l2_get_subdevdata(sd); 315 int ret = 0; 316 317 if (enable) 318 ret = csiphy_stream_on(csiphy); 319 else 320 csiphy_stream_off(csiphy); 321 322 return ret; 323 } 324 325 /* 326 * __csiphy_get_format - Get pointer to format structure 327 * @csiphy: CSIPHY device 328 * @cfg: V4L2 subdev pad configuration 329 * @pad: pad from which format is requested 330 * @which: TRY or ACTIVE format 331 * 332 * Return pointer to TRY or ACTIVE format structure 333 */ 334 static struct v4l2_mbus_framefmt * 335 __csiphy_get_format(struct csiphy_device *csiphy, 336 struct v4l2_subdev_pad_config *cfg, 337 unsigned int pad, 338 enum v4l2_subdev_format_whence which) 339 { 340 if (which == V4L2_SUBDEV_FORMAT_TRY) 341 return v4l2_subdev_get_try_format(&csiphy->subdev, cfg, pad); 342 343 return &csiphy->fmt[pad]; 344 } 345 346 /* 347 * csiphy_try_format - Handle try format by pad subdev method 348 * @csiphy: CSIPHY device 349 * @cfg: V4L2 subdev pad configuration 350 * @pad: pad on which format is requested 351 * @fmt: pointer to v4l2 format structure 352 * @which: wanted subdev format 353 */ 354 static void csiphy_try_format(struct csiphy_device *csiphy, 355 struct v4l2_subdev_pad_config *cfg, 356 unsigned int pad, 357 struct v4l2_mbus_framefmt *fmt, 358 enum v4l2_subdev_format_whence which) 359 { 360 unsigned int i; 361 362 switch (pad) { 363 case MSM_CSIPHY_PAD_SINK: 364 /* Set format on sink pad */ 365 366 for (i = 0; i < ARRAY_SIZE(csiphy_formats); i++) 367 if (fmt->code == csiphy_formats[i].code) 368 break; 369 370 /* If not found, use UYVY as default */ 371 if (i >= ARRAY_SIZE(csiphy_formats)) 372 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8; 373 374 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 375 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 376 377 fmt->field = V4L2_FIELD_NONE; 378 fmt->colorspace = V4L2_COLORSPACE_SRGB; 379 380 break; 381 382 case MSM_CSIPHY_PAD_SRC: 383 /* Set and return a format same as sink pad */ 384 385 *fmt = *__csiphy_get_format(csiphy, cfg, MSM_CSID_PAD_SINK, 386 which); 387 388 break; 389 } 390 } 391 392 /* 393 * csiphy_enum_mbus_code - Handle pixel format enumeration 394 * @sd: CSIPHY V4L2 subdevice 395 * @cfg: V4L2 subdev pad configuration 396 * @code: pointer to v4l2_subdev_mbus_code_enum structure 397 * return -EINVAL or zero on success 398 */ 399 static int csiphy_enum_mbus_code(struct v4l2_subdev *sd, 400 struct v4l2_subdev_pad_config *cfg, 401 struct v4l2_subdev_mbus_code_enum *code) 402 { 403 struct csiphy_device *csiphy = v4l2_get_subdevdata(sd); 404 struct v4l2_mbus_framefmt *format; 405 406 if (code->pad == MSM_CSIPHY_PAD_SINK) { 407 if (code->index >= ARRAY_SIZE(csiphy_formats)) 408 return -EINVAL; 409 410 code->code = csiphy_formats[code->index].code; 411 } else { 412 if (code->index > 0) 413 return -EINVAL; 414 415 format = __csiphy_get_format(csiphy, cfg, MSM_CSIPHY_PAD_SINK, 416 code->which); 417 418 code->code = format->code; 419 } 420 421 return 0; 422 } 423 424 /* 425 * csiphy_enum_frame_size - Handle frame size enumeration 426 * @sd: CSIPHY V4L2 subdevice 427 * @cfg: V4L2 subdev pad configuration 428 * @fse: pointer to v4l2_subdev_frame_size_enum structure 429 * return -EINVAL or zero on success 430 */ 431 static int csiphy_enum_frame_size(struct v4l2_subdev *sd, 432 struct v4l2_subdev_pad_config *cfg, 433 struct v4l2_subdev_frame_size_enum *fse) 434 { 435 struct csiphy_device *csiphy = v4l2_get_subdevdata(sd); 436 struct v4l2_mbus_framefmt format; 437 438 if (fse->index != 0) 439 return -EINVAL; 440 441 format.code = fse->code; 442 format.width = 1; 443 format.height = 1; 444 csiphy_try_format(csiphy, cfg, fse->pad, &format, fse->which); 445 fse->min_width = format.width; 446 fse->min_height = format.height; 447 448 if (format.code != fse->code) 449 return -EINVAL; 450 451 format.code = fse->code; 452 format.width = -1; 453 format.height = -1; 454 csiphy_try_format(csiphy, cfg, fse->pad, &format, fse->which); 455 fse->max_width = format.width; 456 fse->max_height = format.height; 457 458 return 0; 459 } 460 461 /* 462 * csiphy_get_format - Handle get format by pads subdev method 463 * @sd: CSIPHY V4L2 subdevice 464 * @cfg: V4L2 subdev pad configuration 465 * @fmt: pointer to v4l2 subdev format structure 466 * 467 * Return -EINVAL or zero on success 468 */ 469 static int csiphy_get_format(struct v4l2_subdev *sd, 470 struct v4l2_subdev_pad_config *cfg, 471 struct v4l2_subdev_format *fmt) 472 { 473 struct csiphy_device *csiphy = v4l2_get_subdevdata(sd); 474 struct v4l2_mbus_framefmt *format; 475 476 format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which); 477 if (format == NULL) 478 return -EINVAL; 479 480 fmt->format = *format; 481 482 return 0; 483 } 484 485 /* 486 * csiphy_set_format - Handle set format by pads subdev method 487 * @sd: CSIPHY V4L2 subdevice 488 * @cfg: V4L2 subdev pad configuration 489 * @fmt: pointer to v4l2 subdev format structure 490 * 491 * Return -EINVAL or zero on success 492 */ 493 static int csiphy_set_format(struct v4l2_subdev *sd, 494 struct v4l2_subdev_pad_config *cfg, 495 struct v4l2_subdev_format *fmt) 496 { 497 struct csiphy_device *csiphy = v4l2_get_subdevdata(sd); 498 struct v4l2_mbus_framefmt *format; 499 500 format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which); 501 if (format == NULL) 502 return -EINVAL; 503 504 csiphy_try_format(csiphy, cfg, fmt->pad, &fmt->format, fmt->which); 505 *format = fmt->format; 506 507 /* Propagate the format from sink to source */ 508 if (fmt->pad == MSM_CSIPHY_PAD_SINK) { 509 format = __csiphy_get_format(csiphy, cfg, MSM_CSIPHY_PAD_SRC, 510 fmt->which); 511 512 *format = fmt->format; 513 csiphy_try_format(csiphy, cfg, MSM_CSIPHY_PAD_SRC, format, 514 fmt->which); 515 } 516 517 return 0; 518 } 519 520 /* 521 * csiphy_init_formats - Initialize formats on all pads 522 * @sd: CSIPHY V4L2 subdevice 523 * @fh: V4L2 subdev file handle 524 * 525 * Initialize all pad formats with default values. 526 * 527 * Return 0 on success or a negative error code otherwise 528 */ 529 static int csiphy_init_formats(struct v4l2_subdev *sd, 530 struct v4l2_subdev_fh *fh) 531 { 532 struct v4l2_subdev_format format = { 533 .pad = MSM_CSIPHY_PAD_SINK, 534 .which = fh ? V4L2_SUBDEV_FORMAT_TRY : 535 V4L2_SUBDEV_FORMAT_ACTIVE, 536 .format = { 537 .code = MEDIA_BUS_FMT_UYVY8_2X8, 538 .width = 1920, 539 .height = 1080 540 } 541 }; 542 543 return csiphy_set_format(sd, fh ? fh->pad : NULL, &format); 544 } 545 546 /* 547 * msm_csiphy_subdev_init - Initialize CSIPHY device structure and resources 548 * @csiphy: CSIPHY device 549 * @res: CSIPHY module resources table 550 * @id: CSIPHY module id 551 * 552 * Return 0 on success or a negative error code otherwise 553 */ 554 int msm_csiphy_subdev_init(struct camss *camss, 555 struct csiphy_device *csiphy, 556 const struct resources *res, u8 id) 557 { 558 struct device *dev = camss->dev; 559 struct platform_device *pdev = to_platform_device(dev); 560 struct resource *r; 561 int i, j; 562 int ret; 563 564 csiphy->camss = camss; 565 csiphy->id = id; 566 csiphy->cfg.combo_mode = 0; 567 568 if (camss->version == CAMSS_8x16) 569 csiphy->ops = &csiphy_ops_2ph_1_0; 570 else 571 return -EINVAL; 572 573 /* Memory */ 574 575 r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]); 576 csiphy->base = devm_ioremap_resource(dev, r); 577 if (IS_ERR(csiphy->base)) { 578 dev_err(dev, "could not map memory\n"); 579 return PTR_ERR(csiphy->base); 580 } 581 582 r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[1]); 583 csiphy->base_clk_mux = devm_ioremap_resource(dev, r); 584 if (IS_ERR(csiphy->base_clk_mux)) { 585 dev_err(dev, "could not map memory\n"); 586 return PTR_ERR(csiphy->base_clk_mux); 587 } 588 589 /* Interrupt */ 590 591 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, 592 res->interrupt[0]); 593 if (!r) { 594 dev_err(dev, "missing IRQ\n"); 595 return -EINVAL; 596 } 597 598 csiphy->irq = r->start; 599 snprintf(csiphy->irq_name, sizeof(csiphy->irq_name), "%s_%s%d", 600 dev_name(dev), MSM_CSIPHY_NAME, csiphy->id); 601 602 ret = devm_request_irq(dev, csiphy->irq, csiphy->ops->isr, 603 IRQF_TRIGGER_RISING, csiphy->irq_name, csiphy); 604 if (ret < 0) { 605 dev_err(dev, "request_irq failed: %d\n", ret); 606 return ret; 607 } 608 609 disable_irq(csiphy->irq); 610 611 /* Clocks */ 612 613 csiphy->nclocks = 0; 614 while (res->clock[csiphy->nclocks]) 615 csiphy->nclocks++; 616 617 csiphy->clock = devm_kcalloc(dev, 618 csiphy->nclocks, sizeof(*csiphy->clock), 619 GFP_KERNEL); 620 if (!csiphy->clock) 621 return -ENOMEM; 622 623 for (i = 0; i < csiphy->nclocks; i++) { 624 struct camss_clock *clock = &csiphy->clock[i]; 625 626 clock->clk = devm_clk_get(dev, res->clock[i]); 627 if (IS_ERR(clock->clk)) 628 return PTR_ERR(clock->clk); 629 630 clock->name = res->clock[i]; 631 632 clock->nfreqs = 0; 633 while (res->clock_rate[i][clock->nfreqs]) 634 clock->nfreqs++; 635 636 if (!clock->nfreqs) { 637 clock->freq = NULL; 638 continue; 639 } 640 641 clock->freq = devm_kcalloc(dev, 642 clock->nfreqs, 643 sizeof(*clock->freq), 644 GFP_KERNEL); 645 if (!clock->freq) 646 return -ENOMEM; 647 648 for (j = 0; j < clock->nfreqs; j++) 649 clock->freq[j] = res->clock_rate[i][j]; 650 } 651 652 return 0; 653 } 654 655 /* 656 * csiphy_link_setup - Setup CSIPHY connections 657 * @entity: Pointer to media entity structure 658 * @local: Pointer to local pad 659 * @remote: Pointer to remote pad 660 * @flags: Link flags 661 * 662 * Rreturn 0 on success 663 */ 664 static int csiphy_link_setup(struct media_entity *entity, 665 const struct media_pad *local, 666 const struct media_pad *remote, u32 flags) 667 { 668 if ((local->flags & MEDIA_PAD_FL_SOURCE) && 669 (flags & MEDIA_LNK_FL_ENABLED)) { 670 struct v4l2_subdev *sd; 671 struct csiphy_device *csiphy; 672 struct csid_device *csid; 673 674 if (media_entity_remote_pad(local)) 675 return -EBUSY; 676 677 sd = media_entity_to_v4l2_subdev(entity); 678 csiphy = v4l2_get_subdevdata(sd); 679 680 sd = media_entity_to_v4l2_subdev(remote->entity); 681 csid = v4l2_get_subdevdata(sd); 682 683 csiphy->cfg.csid_id = csid->id; 684 } 685 686 return 0; 687 } 688 689 static const struct v4l2_subdev_core_ops csiphy_core_ops = { 690 .s_power = csiphy_set_power, 691 }; 692 693 static const struct v4l2_subdev_video_ops csiphy_video_ops = { 694 .s_stream = csiphy_set_stream, 695 }; 696 697 static const struct v4l2_subdev_pad_ops csiphy_pad_ops = { 698 .enum_mbus_code = csiphy_enum_mbus_code, 699 .enum_frame_size = csiphy_enum_frame_size, 700 .get_fmt = csiphy_get_format, 701 .set_fmt = csiphy_set_format, 702 }; 703 704 static const struct v4l2_subdev_ops csiphy_v4l2_ops = { 705 .core = &csiphy_core_ops, 706 .video = &csiphy_video_ops, 707 .pad = &csiphy_pad_ops, 708 }; 709 710 static const struct v4l2_subdev_internal_ops csiphy_v4l2_internal_ops = { 711 .open = csiphy_init_formats, 712 }; 713 714 static const struct media_entity_operations csiphy_media_ops = { 715 .link_setup = csiphy_link_setup, 716 .link_validate = v4l2_subdev_link_validate, 717 }; 718 719 /* 720 * msm_csiphy_register_entity - Register subdev node for CSIPHY module 721 * @csiphy: CSIPHY device 722 * @v4l2_dev: V4L2 device 723 * 724 * Return 0 on success or a negative error code otherwise 725 */ 726 int msm_csiphy_register_entity(struct csiphy_device *csiphy, 727 struct v4l2_device *v4l2_dev) 728 { 729 struct v4l2_subdev *sd = &csiphy->subdev; 730 struct media_pad *pads = csiphy->pads; 731 struct device *dev = csiphy->camss->dev; 732 int ret; 733 734 v4l2_subdev_init(sd, &csiphy_v4l2_ops); 735 sd->internal_ops = &csiphy_v4l2_internal_ops; 736 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 737 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", 738 MSM_CSIPHY_NAME, csiphy->id); 739 v4l2_set_subdevdata(sd, csiphy); 740 741 ret = csiphy_init_formats(sd, NULL); 742 if (ret < 0) { 743 dev_err(dev, "Failed to init format: %d\n", ret); 744 return ret; 745 } 746 747 pads[MSM_CSIPHY_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 748 pads[MSM_CSIPHY_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE; 749 750 sd->entity.function = MEDIA_ENT_F_IO_V4L; 751 sd->entity.ops = &csiphy_media_ops; 752 ret = media_entity_pads_init(&sd->entity, MSM_CSIPHY_PADS_NUM, pads); 753 if (ret < 0) { 754 dev_err(dev, "Failed to init media entity: %d\n", ret); 755 return ret; 756 } 757 758 ret = v4l2_device_register_subdev(v4l2_dev, sd); 759 if (ret < 0) { 760 dev_err(dev, "Failed to register subdev: %d\n", ret); 761 media_entity_cleanup(&sd->entity); 762 } 763 764 return ret; 765 } 766 767 /* 768 * msm_csiphy_unregister_entity - Unregister CSIPHY module subdev node 769 * @csiphy: CSIPHY device 770 */ 771 void msm_csiphy_unregister_entity(struct csiphy_device *csiphy) 772 { 773 v4l2_device_unregister_subdev(&csiphy->subdev); 774 media_entity_cleanup(&csiphy->subdev.entity); 775 } 776