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/kernel.h> 14 #include <linux/of.h> 15 #include <linux/platform_device.h> 16 #include <linux/pm_runtime.h> 17 #include <linux/regulator/consumer.h> 18 #include <media/media-entity.h> 19 #include <media/v4l2-device.h> 20 #include <media/v4l2-event.h> 21 #include <media/v4l2-subdev.h> 22 23 #include "camss-csid.h" 24 #include "camss.h" 25 26 #define MSM_CSID_NAME "msm_csid" 27 28 #define CAMSS_CSID_HW_VERSION 0x0 29 #define CAMSS_CSID_CORE_CTRL_0 0x004 30 #define CAMSS_CSID_CORE_CTRL_1 0x008 31 #define CAMSS_CSID_RST_CMD(v) ((v) == CAMSS_8x16 ? 0x00c : 0x010) 32 #define CAMSS_CSID_CID_LUT_VC_n(v, n) \ 33 (((v) == CAMSS_8x16 ? 0x010 : 0x014) + 0x4 * (n)) 34 #define CAMSS_CSID_CID_n_CFG(v, n) \ 35 (((v) == CAMSS_8x16 ? 0x020 : 0x024) + 0x4 * (n)) 36 #define CAMSS_CSID_CID_n_CFG_ISPIF_EN BIT(0) 37 #define CAMSS_CSID_CID_n_CFG_RDI_EN BIT(1) 38 #define CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT 4 39 #define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_8 (0 << 8) 40 #define CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16 (1 << 8) 41 #define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB (0 << 9) 42 #define CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_MSB (1 << 9) 43 #define CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP (0 << 10) 44 #define CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING (1 << 10) 45 #define CAMSS_CSID_IRQ_CLEAR_CMD(v) ((v) == CAMSS_8x16 ? 0x060 : 0x064) 46 #define CAMSS_CSID_IRQ_MASK(v) ((v) == CAMSS_8x16 ? 0x064 : 0x068) 47 #define CAMSS_CSID_IRQ_STATUS(v) ((v) == CAMSS_8x16 ? 0x068 : 0x06c) 48 #define CAMSS_CSID_TG_CTRL(v) ((v) == CAMSS_8x16 ? 0x0a0 : 0x0a8) 49 #define CAMSS_CSID_TG_CTRL_DISABLE 0xa06436 50 #define CAMSS_CSID_TG_CTRL_ENABLE 0xa06437 51 #define CAMSS_CSID_TG_VC_CFG(v) ((v) == CAMSS_8x16 ? 0x0a4 : 0x0ac) 52 #define CAMSS_CSID_TG_VC_CFG_H_BLANKING 0x3ff 53 #define CAMSS_CSID_TG_VC_CFG_V_BLANKING 0x7f 54 #define CAMSS_CSID_TG_DT_n_CGG_0(v, n) \ 55 (((v) == CAMSS_8x16 ? 0x0ac : 0x0b4) + 0xc * (n)) 56 #define CAMSS_CSID_TG_DT_n_CGG_1(v, n) \ 57 (((v) == CAMSS_8x16 ? 0x0b0 : 0x0b8) + 0xc * (n)) 58 #define CAMSS_CSID_TG_DT_n_CGG_2(v, n) \ 59 (((v) == CAMSS_8x16 ? 0x0b4 : 0x0bc) + 0xc * (n)) 60 61 #define DATA_TYPE_EMBEDDED_DATA_8BIT 0x12 62 #define DATA_TYPE_YUV422_8BIT 0x1e 63 #define DATA_TYPE_RAW_6BIT 0x28 64 #define DATA_TYPE_RAW_8BIT 0x2a 65 #define DATA_TYPE_RAW_10BIT 0x2b 66 #define DATA_TYPE_RAW_12BIT 0x2c 67 #define DATA_TYPE_RAW_14BIT 0x2d 68 69 #define DECODE_FORMAT_UNCOMPRESSED_6_BIT 0x0 70 #define DECODE_FORMAT_UNCOMPRESSED_8_BIT 0x1 71 #define DECODE_FORMAT_UNCOMPRESSED_10_BIT 0x2 72 #define DECODE_FORMAT_UNCOMPRESSED_12_BIT 0x3 73 #define DECODE_FORMAT_UNCOMPRESSED_14_BIT 0x8 74 75 #define CSID_RESET_TIMEOUT_MS 500 76 77 struct csid_format { 78 u32 code; 79 u8 data_type; 80 u8 decode_format; 81 u8 bpp; 82 u8 spp; /* bus samples per pixel */ 83 }; 84 85 static const struct csid_format csid_formats_8x16[] = { 86 { 87 MEDIA_BUS_FMT_UYVY8_2X8, 88 DATA_TYPE_YUV422_8BIT, 89 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 90 8, 91 2, 92 }, 93 { 94 MEDIA_BUS_FMT_VYUY8_2X8, 95 DATA_TYPE_YUV422_8BIT, 96 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 97 8, 98 2, 99 }, 100 { 101 MEDIA_BUS_FMT_YUYV8_2X8, 102 DATA_TYPE_YUV422_8BIT, 103 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 104 8, 105 2, 106 }, 107 { 108 MEDIA_BUS_FMT_YVYU8_2X8, 109 DATA_TYPE_YUV422_8BIT, 110 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 111 8, 112 2, 113 }, 114 { 115 MEDIA_BUS_FMT_SBGGR8_1X8, 116 DATA_TYPE_RAW_8BIT, 117 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 118 8, 119 1, 120 }, 121 { 122 MEDIA_BUS_FMT_SGBRG8_1X8, 123 DATA_TYPE_RAW_8BIT, 124 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 125 8, 126 1, 127 }, 128 { 129 MEDIA_BUS_FMT_SGRBG8_1X8, 130 DATA_TYPE_RAW_8BIT, 131 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 132 8, 133 1, 134 }, 135 { 136 MEDIA_BUS_FMT_SRGGB8_1X8, 137 DATA_TYPE_RAW_8BIT, 138 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 139 8, 140 1, 141 }, 142 { 143 MEDIA_BUS_FMT_SBGGR10_1X10, 144 DATA_TYPE_RAW_10BIT, 145 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 146 10, 147 1, 148 }, 149 { 150 MEDIA_BUS_FMT_SGBRG10_1X10, 151 DATA_TYPE_RAW_10BIT, 152 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 153 10, 154 1, 155 }, 156 { 157 MEDIA_BUS_FMT_SGRBG10_1X10, 158 DATA_TYPE_RAW_10BIT, 159 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 160 10, 161 1, 162 }, 163 { 164 MEDIA_BUS_FMT_SRGGB10_1X10, 165 DATA_TYPE_RAW_10BIT, 166 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 167 10, 168 1, 169 }, 170 { 171 MEDIA_BUS_FMT_SBGGR12_1X12, 172 DATA_TYPE_RAW_12BIT, 173 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 174 12, 175 1, 176 }, 177 { 178 MEDIA_BUS_FMT_SGBRG12_1X12, 179 DATA_TYPE_RAW_12BIT, 180 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 181 12, 182 1, 183 }, 184 { 185 MEDIA_BUS_FMT_SGRBG12_1X12, 186 DATA_TYPE_RAW_12BIT, 187 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 188 12, 189 1, 190 }, 191 { 192 MEDIA_BUS_FMT_SRGGB12_1X12, 193 DATA_TYPE_RAW_12BIT, 194 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 195 12, 196 1, 197 }, 198 { 199 MEDIA_BUS_FMT_Y10_1X10, 200 DATA_TYPE_RAW_10BIT, 201 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 202 10, 203 1, 204 }, 205 }; 206 207 static const struct csid_format csid_formats_8x96[] = { 208 { 209 MEDIA_BUS_FMT_UYVY8_2X8, 210 DATA_TYPE_YUV422_8BIT, 211 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 212 8, 213 2, 214 }, 215 { 216 MEDIA_BUS_FMT_VYUY8_2X8, 217 DATA_TYPE_YUV422_8BIT, 218 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 219 8, 220 2, 221 }, 222 { 223 MEDIA_BUS_FMT_YUYV8_2X8, 224 DATA_TYPE_YUV422_8BIT, 225 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 226 8, 227 2, 228 }, 229 { 230 MEDIA_BUS_FMT_YVYU8_2X8, 231 DATA_TYPE_YUV422_8BIT, 232 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 233 8, 234 2, 235 }, 236 { 237 MEDIA_BUS_FMT_SBGGR8_1X8, 238 DATA_TYPE_RAW_8BIT, 239 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 240 8, 241 1, 242 }, 243 { 244 MEDIA_BUS_FMT_SGBRG8_1X8, 245 DATA_TYPE_RAW_8BIT, 246 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 247 8, 248 1, 249 }, 250 { 251 MEDIA_BUS_FMT_SGRBG8_1X8, 252 DATA_TYPE_RAW_8BIT, 253 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 254 8, 255 1, 256 }, 257 { 258 MEDIA_BUS_FMT_SRGGB8_1X8, 259 DATA_TYPE_RAW_8BIT, 260 DECODE_FORMAT_UNCOMPRESSED_8_BIT, 261 8, 262 1, 263 }, 264 { 265 MEDIA_BUS_FMT_SBGGR10_1X10, 266 DATA_TYPE_RAW_10BIT, 267 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 268 10, 269 1, 270 }, 271 { 272 MEDIA_BUS_FMT_SGBRG10_1X10, 273 DATA_TYPE_RAW_10BIT, 274 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 275 10, 276 1, 277 }, 278 { 279 MEDIA_BUS_FMT_SGRBG10_1X10, 280 DATA_TYPE_RAW_10BIT, 281 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 282 10, 283 1, 284 }, 285 { 286 MEDIA_BUS_FMT_SRGGB10_1X10, 287 DATA_TYPE_RAW_10BIT, 288 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 289 10, 290 1, 291 }, 292 { 293 MEDIA_BUS_FMT_SBGGR12_1X12, 294 DATA_TYPE_RAW_12BIT, 295 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 296 12, 297 1, 298 }, 299 { 300 MEDIA_BUS_FMT_SGBRG12_1X12, 301 DATA_TYPE_RAW_12BIT, 302 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 303 12, 304 1, 305 }, 306 { 307 MEDIA_BUS_FMT_SGRBG12_1X12, 308 DATA_TYPE_RAW_12BIT, 309 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 310 12, 311 1, 312 }, 313 { 314 MEDIA_BUS_FMT_SRGGB12_1X12, 315 DATA_TYPE_RAW_12BIT, 316 DECODE_FORMAT_UNCOMPRESSED_12_BIT, 317 12, 318 1, 319 }, 320 { 321 MEDIA_BUS_FMT_SBGGR14_1X14, 322 DATA_TYPE_RAW_14BIT, 323 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 324 14, 325 1, 326 }, 327 { 328 MEDIA_BUS_FMT_SGBRG14_1X14, 329 DATA_TYPE_RAW_14BIT, 330 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 331 14, 332 1, 333 }, 334 { 335 MEDIA_BUS_FMT_SGRBG14_1X14, 336 DATA_TYPE_RAW_14BIT, 337 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 338 14, 339 1, 340 }, 341 { 342 MEDIA_BUS_FMT_SRGGB14_1X14, 343 DATA_TYPE_RAW_14BIT, 344 DECODE_FORMAT_UNCOMPRESSED_14_BIT, 345 14, 346 1, 347 }, 348 { 349 MEDIA_BUS_FMT_Y10_1X10, 350 DATA_TYPE_RAW_10BIT, 351 DECODE_FORMAT_UNCOMPRESSED_10_BIT, 352 10, 353 1, 354 }, 355 }; 356 357 static u32 csid_find_code(u32 *code, unsigned int n_code, 358 unsigned int index, u32 req_code) 359 { 360 int i; 361 362 if (!req_code && (index >= n_code)) 363 return 0; 364 365 for (i = 0; i < n_code; i++) 366 if (req_code) { 367 if (req_code == code[i]) 368 return req_code; 369 } else { 370 if (i == index) 371 return code[i]; 372 } 373 374 return code[0]; 375 } 376 377 static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code, 378 unsigned int index, u32 src_req_code) 379 { 380 if (csid->camss->version == CAMSS_8x16) { 381 if (index > 0) 382 return 0; 383 384 return sink_code; 385 } else if (csid->camss->version == CAMSS_8x96) { 386 switch (sink_code) { 387 case MEDIA_BUS_FMT_SBGGR10_1X10: 388 { 389 u32 src_code[] = { 390 MEDIA_BUS_FMT_SBGGR10_1X10, 391 MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 392 }; 393 394 return csid_find_code(src_code, ARRAY_SIZE(src_code), 395 index, src_req_code); 396 } 397 case MEDIA_BUS_FMT_Y10_1X10: 398 { 399 u32 src_code[] = { 400 MEDIA_BUS_FMT_Y10_1X10, 401 MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 402 }; 403 404 return csid_find_code(src_code, ARRAY_SIZE(src_code), 405 index, src_req_code); 406 } 407 default: 408 if (index > 0) 409 return 0; 410 411 return sink_code; 412 } 413 } else { 414 return 0; 415 } 416 } 417 418 static const struct csid_format *csid_get_fmt_entry( 419 const struct csid_format *formats, 420 unsigned int nformat, 421 u32 code) 422 { 423 unsigned int i; 424 425 for (i = 0; i < nformat; i++) 426 if (code == formats[i].code) 427 return &formats[i]; 428 429 WARN(1, "Unknown format\n"); 430 431 return &formats[0]; 432 } 433 434 /* 435 * csid_isr - CSID module interrupt handler 436 * @irq: Interrupt line 437 * @dev: CSID device 438 * 439 * Return IRQ_HANDLED on success 440 */ 441 static irqreturn_t csid_isr(int irq, void *dev) 442 { 443 struct csid_device *csid = dev; 444 enum camss_version ver = csid->camss->version; 445 u32 value; 446 447 value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS(ver)); 448 writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD(ver)); 449 450 if ((value >> 11) & 0x1) 451 complete(&csid->reset_complete); 452 453 return IRQ_HANDLED; 454 } 455 456 /* 457 * csid_set_clock_rates - Calculate and set clock rates on CSID module 458 * @csiphy: CSID device 459 */ 460 static int csid_set_clock_rates(struct csid_device *csid) 461 { 462 struct device *dev = csid->camss->dev; 463 u32 pixel_clock; 464 int i, j; 465 int ret; 466 467 ret = camss_get_pixel_clock(&csid->subdev.entity, &pixel_clock); 468 if (ret) 469 pixel_clock = 0; 470 471 for (i = 0; i < csid->nclocks; i++) { 472 struct camss_clock *clock = &csid->clock[i]; 473 474 if (!strcmp(clock->name, "csi0") || 475 !strcmp(clock->name, "csi1") || 476 !strcmp(clock->name, "csi2") || 477 !strcmp(clock->name, "csi3")) { 478 const struct csid_format *f = csid_get_fmt_entry( 479 csid->formats, 480 csid->nformats, 481 csid->fmt[MSM_CSIPHY_PAD_SINK].code); 482 u8 num_lanes = csid->phy.lane_cnt; 483 u64 min_rate = pixel_clock * f->bpp / 484 (2 * num_lanes * 4); 485 long rate; 486 487 camss_add_clock_margin(&min_rate); 488 489 for (j = 0; j < clock->nfreqs; j++) 490 if (min_rate < clock->freq[j]) 491 break; 492 493 if (j == clock->nfreqs) { 494 dev_err(dev, 495 "Pixel clock is too high for CSID\n"); 496 return -EINVAL; 497 } 498 499 /* if sensor pixel clock is not available */ 500 /* set highest possible CSID clock rate */ 501 if (min_rate == 0) 502 j = clock->nfreqs - 1; 503 504 rate = clk_round_rate(clock->clk, clock->freq[j]); 505 if (rate < 0) { 506 dev_err(dev, "clk round rate failed: %ld\n", 507 rate); 508 return -EINVAL; 509 } 510 511 ret = clk_set_rate(clock->clk, rate); 512 if (ret < 0) { 513 dev_err(dev, "clk set rate failed: %d\n", ret); 514 return ret; 515 } 516 } 517 } 518 519 return 0; 520 } 521 522 /* 523 * csid_reset - Trigger reset on CSID module and wait to complete 524 * @csid: CSID device 525 * 526 * Return 0 on success or a negative error code otherwise 527 */ 528 static int csid_reset(struct csid_device *csid) 529 { 530 unsigned long time; 531 532 reinit_completion(&csid->reset_complete); 533 534 writel_relaxed(0x7fff, csid->base + 535 CAMSS_CSID_RST_CMD(csid->camss->version)); 536 537 time = wait_for_completion_timeout(&csid->reset_complete, 538 msecs_to_jiffies(CSID_RESET_TIMEOUT_MS)); 539 if (!time) { 540 dev_err(csid->camss->dev, "CSID reset timeout\n"); 541 return -EIO; 542 } 543 544 return 0; 545 } 546 547 /* 548 * csid_set_power - Power on/off CSID module 549 * @sd: CSID V4L2 subdevice 550 * @on: Requested power state 551 * 552 * Return 0 on success or a negative error code otherwise 553 */ 554 static int csid_set_power(struct v4l2_subdev *sd, int on) 555 { 556 struct csid_device *csid = v4l2_get_subdevdata(sd); 557 struct device *dev = csid->camss->dev; 558 int ret; 559 560 if (on) { 561 u32 hw_version; 562 563 ret = pm_runtime_get_sync(dev); 564 if (ret < 0) 565 return ret; 566 567 ret = regulator_enable(csid->vdda); 568 if (ret < 0) { 569 pm_runtime_put_sync(dev); 570 return ret; 571 } 572 573 ret = csid_set_clock_rates(csid); 574 if (ret < 0) { 575 regulator_disable(csid->vdda); 576 pm_runtime_put_sync(dev); 577 return ret; 578 } 579 580 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev); 581 if (ret < 0) { 582 regulator_disable(csid->vdda); 583 pm_runtime_put_sync(dev); 584 return ret; 585 } 586 587 enable_irq(csid->irq); 588 589 ret = csid_reset(csid); 590 if (ret < 0) { 591 disable_irq(csid->irq); 592 camss_disable_clocks(csid->nclocks, csid->clock); 593 regulator_disable(csid->vdda); 594 pm_runtime_put_sync(dev); 595 return ret; 596 } 597 598 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION); 599 dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version); 600 } else { 601 disable_irq(csid->irq); 602 camss_disable_clocks(csid->nclocks, csid->clock); 603 ret = regulator_disable(csid->vdda); 604 pm_runtime_put_sync(dev); 605 } 606 607 return ret; 608 } 609 610 /* 611 * csid_set_stream - Enable/disable streaming on CSID module 612 * @sd: CSID V4L2 subdevice 613 * @enable: Requested streaming state 614 * 615 * Main configuration of CSID module is also done here. 616 * 617 * Return 0 on success or a negative error code otherwise 618 */ 619 static int csid_set_stream(struct v4l2_subdev *sd, int enable) 620 { 621 struct csid_device *csid = v4l2_get_subdevdata(sd); 622 struct csid_testgen_config *tg = &csid->testgen; 623 enum camss_version ver = csid->camss->version; 624 u32 val; 625 626 if (enable) { 627 u8 vc = 0; /* Virtual Channel 0 */ 628 u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */ 629 u8 dt, dt_shift, df; 630 int ret; 631 632 ret = v4l2_ctrl_handler_setup(&csid->ctrls); 633 if (ret < 0) { 634 dev_err(csid->camss->dev, 635 "could not sync v4l2 controls: %d\n", ret); 636 return ret; 637 } 638 639 if (!tg->enabled && 640 !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) 641 return -ENOLINK; 642 643 if (tg->enabled) { 644 /* Config Test Generator */ 645 struct v4l2_mbus_framefmt *f = 646 &csid->fmt[MSM_CSID_PAD_SRC]; 647 const struct csid_format *format = csid_get_fmt_entry( 648 csid->formats, csid->nformats, f->code); 649 u32 num_bytes_per_line = 650 f->width * format->bpp * format->spp / 8; 651 u32 num_lines = f->height; 652 653 /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */ 654 /* 1:0 VC */ 655 val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) | 656 ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13); 657 writel_relaxed(val, csid->base + 658 CAMSS_CSID_TG_VC_CFG(ver)); 659 660 /* 28:16 bytes per lines, 12:0 num of lines */ 661 val = ((num_bytes_per_line & 0x1fff) << 16) | 662 (num_lines & 0x1fff); 663 writel_relaxed(val, csid->base + 664 CAMSS_CSID_TG_DT_n_CGG_0(ver, 0)); 665 666 dt = format->data_type; 667 668 /* 5:0 data type */ 669 val = dt; 670 writel_relaxed(val, csid->base + 671 CAMSS_CSID_TG_DT_n_CGG_1(ver, 0)); 672 673 /* 2:0 output test pattern */ 674 val = tg->payload_mode; 675 writel_relaxed(val, csid->base + 676 CAMSS_CSID_TG_DT_n_CGG_2(ver, 0)); 677 678 df = format->decode_format; 679 } else { 680 struct v4l2_mbus_framefmt *f = 681 &csid->fmt[MSM_CSID_PAD_SINK]; 682 const struct csid_format *format = csid_get_fmt_entry( 683 csid->formats, csid->nformats, f->code); 684 struct csid_phy_config *phy = &csid->phy; 685 686 val = phy->lane_cnt - 1; 687 val |= phy->lane_assign << 4; 688 689 writel_relaxed(val, 690 csid->base + CAMSS_CSID_CORE_CTRL_0); 691 692 val = phy->csiphy_id << 17; 693 val |= 0x9; 694 695 writel_relaxed(val, 696 csid->base + CAMSS_CSID_CORE_CTRL_1); 697 698 dt = format->data_type; 699 df = format->decode_format; 700 } 701 702 /* Config LUT */ 703 704 dt_shift = (cid % 4) * 8; 705 706 val = readl_relaxed(csid->base + 707 CAMSS_CSID_CID_LUT_VC_n(ver, vc)); 708 val &= ~(0xff << dt_shift); 709 val |= dt << dt_shift; 710 writel_relaxed(val, csid->base + 711 CAMSS_CSID_CID_LUT_VC_n(ver, vc)); 712 713 val = CAMSS_CSID_CID_n_CFG_ISPIF_EN; 714 val |= CAMSS_CSID_CID_n_CFG_RDI_EN; 715 val |= df << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT; 716 val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP; 717 718 if (csid->camss->version == CAMSS_8x96) { 719 u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code; 720 u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code; 721 722 if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 && 723 src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) || 724 (sink_code == MEDIA_BUS_FMT_Y10_1X10 && 725 src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) { 726 val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING; 727 val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16; 728 val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB; 729 } 730 } 731 732 writel_relaxed(val, csid->base + 733 CAMSS_CSID_CID_n_CFG(ver, cid)); 734 735 if (tg->enabled) { 736 val = CAMSS_CSID_TG_CTRL_ENABLE; 737 writel_relaxed(val, csid->base + 738 CAMSS_CSID_TG_CTRL(ver)); 739 } 740 } else { 741 if (tg->enabled) { 742 val = CAMSS_CSID_TG_CTRL_DISABLE; 743 writel_relaxed(val, csid->base + 744 CAMSS_CSID_TG_CTRL(ver)); 745 } 746 } 747 748 return 0; 749 } 750 751 /* 752 * __csid_get_format - Get pointer to format structure 753 * @csid: CSID device 754 * @cfg: V4L2 subdev pad configuration 755 * @pad: pad from which format is requested 756 * @which: TRY or ACTIVE format 757 * 758 * Return pointer to TRY or ACTIVE format structure 759 */ 760 static struct v4l2_mbus_framefmt * 761 __csid_get_format(struct csid_device *csid, 762 struct v4l2_subdev_pad_config *cfg, 763 unsigned int pad, 764 enum v4l2_subdev_format_whence which) 765 { 766 if (which == V4L2_SUBDEV_FORMAT_TRY) 767 return v4l2_subdev_get_try_format(&csid->subdev, cfg, pad); 768 769 return &csid->fmt[pad]; 770 } 771 772 /* 773 * csid_try_format - Handle try format by pad subdev method 774 * @csid: CSID device 775 * @cfg: V4L2 subdev pad configuration 776 * @pad: pad on which format is requested 777 * @fmt: pointer to v4l2 format structure 778 * @which: wanted subdev format 779 */ 780 static void csid_try_format(struct csid_device *csid, 781 struct v4l2_subdev_pad_config *cfg, 782 unsigned int pad, 783 struct v4l2_mbus_framefmt *fmt, 784 enum v4l2_subdev_format_whence which) 785 { 786 unsigned int i; 787 788 switch (pad) { 789 case MSM_CSID_PAD_SINK: 790 /* Set format on sink pad */ 791 792 for (i = 0; i < csid->nformats; i++) 793 if (fmt->code == csid->formats[i].code) 794 break; 795 796 /* If not found, use UYVY as default */ 797 if (i >= csid->nformats) 798 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8; 799 800 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 801 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 802 803 fmt->field = V4L2_FIELD_NONE; 804 fmt->colorspace = V4L2_COLORSPACE_SRGB; 805 806 break; 807 808 case MSM_CSID_PAD_SRC: 809 if (csid->testgen_mode->cur.val == 0) { 810 /* Test generator is disabled, */ 811 /* keep pad formats in sync */ 812 u32 code = fmt->code; 813 814 *fmt = *__csid_get_format(csid, cfg, 815 MSM_CSID_PAD_SINK, which); 816 fmt->code = csid_src_pad_code(csid, fmt->code, 0, code); 817 } else { 818 /* Test generator is enabled, set format on source */ 819 /* pad to allow test generator usage */ 820 821 for (i = 0; i < csid->nformats; i++) 822 if (csid->formats[i].code == fmt->code) 823 break; 824 825 /* If not found, use UYVY as default */ 826 if (i >= csid->nformats) 827 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8; 828 829 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 830 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 831 832 fmt->field = V4L2_FIELD_NONE; 833 } 834 break; 835 } 836 837 fmt->colorspace = V4L2_COLORSPACE_SRGB; 838 } 839 840 /* 841 * csid_enum_mbus_code - Handle pixel format enumeration 842 * @sd: CSID V4L2 subdevice 843 * @cfg: V4L2 subdev pad configuration 844 * @code: pointer to v4l2_subdev_mbus_code_enum structure 845 * return -EINVAL or zero on success 846 */ 847 static int csid_enum_mbus_code(struct v4l2_subdev *sd, 848 struct v4l2_subdev_pad_config *cfg, 849 struct v4l2_subdev_mbus_code_enum *code) 850 { 851 struct csid_device *csid = v4l2_get_subdevdata(sd); 852 853 if (code->pad == MSM_CSID_PAD_SINK) { 854 if (code->index >= csid->nformats) 855 return -EINVAL; 856 857 code->code = csid->formats[code->index].code; 858 } else { 859 if (csid->testgen_mode->cur.val == 0) { 860 struct v4l2_mbus_framefmt *sink_fmt; 861 862 sink_fmt = __csid_get_format(csid, cfg, 863 MSM_CSID_PAD_SINK, 864 code->which); 865 866 code->code = csid_src_pad_code(csid, sink_fmt->code, 867 code->index, 0); 868 if (!code->code) 869 return -EINVAL; 870 } else { 871 if (code->index >= csid->nformats) 872 return -EINVAL; 873 874 code->code = csid->formats[code->index].code; 875 } 876 } 877 878 return 0; 879 } 880 881 /* 882 * csid_enum_frame_size - Handle frame size enumeration 883 * @sd: CSID V4L2 subdevice 884 * @cfg: V4L2 subdev pad configuration 885 * @fse: pointer to v4l2_subdev_frame_size_enum structure 886 * return -EINVAL or zero on success 887 */ 888 static int csid_enum_frame_size(struct v4l2_subdev *sd, 889 struct v4l2_subdev_pad_config *cfg, 890 struct v4l2_subdev_frame_size_enum *fse) 891 { 892 struct csid_device *csid = v4l2_get_subdevdata(sd); 893 struct v4l2_mbus_framefmt format; 894 895 if (fse->index != 0) 896 return -EINVAL; 897 898 format.code = fse->code; 899 format.width = 1; 900 format.height = 1; 901 csid_try_format(csid, cfg, fse->pad, &format, fse->which); 902 fse->min_width = format.width; 903 fse->min_height = format.height; 904 905 if (format.code != fse->code) 906 return -EINVAL; 907 908 format.code = fse->code; 909 format.width = -1; 910 format.height = -1; 911 csid_try_format(csid, cfg, fse->pad, &format, fse->which); 912 fse->max_width = format.width; 913 fse->max_height = format.height; 914 915 return 0; 916 } 917 918 /* 919 * csid_get_format - Handle get format by pads subdev method 920 * @sd: CSID V4L2 subdevice 921 * @cfg: V4L2 subdev pad configuration 922 * @fmt: pointer to v4l2 subdev format structure 923 * 924 * Return -EINVAL or zero on success 925 */ 926 static int csid_get_format(struct v4l2_subdev *sd, 927 struct v4l2_subdev_pad_config *cfg, 928 struct v4l2_subdev_format *fmt) 929 { 930 struct csid_device *csid = v4l2_get_subdevdata(sd); 931 struct v4l2_mbus_framefmt *format; 932 933 format = __csid_get_format(csid, cfg, fmt->pad, fmt->which); 934 if (format == NULL) 935 return -EINVAL; 936 937 fmt->format = *format; 938 939 return 0; 940 } 941 942 /* 943 * csid_set_format - Handle set format by pads subdev method 944 * @sd: CSID V4L2 subdevice 945 * @cfg: V4L2 subdev pad configuration 946 * @fmt: pointer to v4l2 subdev format structure 947 * 948 * Return -EINVAL or zero on success 949 */ 950 static int csid_set_format(struct v4l2_subdev *sd, 951 struct v4l2_subdev_pad_config *cfg, 952 struct v4l2_subdev_format *fmt) 953 { 954 struct csid_device *csid = v4l2_get_subdevdata(sd); 955 struct v4l2_mbus_framefmt *format; 956 957 format = __csid_get_format(csid, cfg, fmt->pad, fmt->which); 958 if (format == NULL) 959 return -EINVAL; 960 961 csid_try_format(csid, cfg, fmt->pad, &fmt->format, fmt->which); 962 *format = fmt->format; 963 964 /* Propagate the format from sink to source */ 965 if (fmt->pad == MSM_CSID_PAD_SINK) { 966 format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SRC, 967 fmt->which); 968 969 *format = fmt->format; 970 csid_try_format(csid, cfg, MSM_CSID_PAD_SRC, format, 971 fmt->which); 972 } 973 974 return 0; 975 } 976 977 /* 978 * csid_init_formats - Initialize formats on all pads 979 * @sd: CSID V4L2 subdevice 980 * @fh: V4L2 subdev file handle 981 * 982 * Initialize all pad formats with default values. 983 * 984 * Return 0 on success or a negative error code otherwise 985 */ 986 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) 987 { 988 struct v4l2_subdev_format format = { 989 .pad = MSM_CSID_PAD_SINK, 990 .which = fh ? V4L2_SUBDEV_FORMAT_TRY : 991 V4L2_SUBDEV_FORMAT_ACTIVE, 992 .format = { 993 .code = MEDIA_BUS_FMT_UYVY8_2X8, 994 .width = 1920, 995 .height = 1080 996 } 997 }; 998 999 return csid_set_format(sd, fh ? fh->pad : NULL, &format); 1000 } 1001 1002 static const char * const csid_test_pattern_menu[] = { 1003 "Disabled", 1004 "Incrementing", 1005 "Alternating 0x55/0xAA", 1006 "All Zeros 0x00", 1007 "All Ones 0xFF", 1008 "Pseudo-random Data", 1009 }; 1010 1011 /* 1012 * csid_set_test_pattern - Set test generator's pattern mode 1013 * @csid: CSID device 1014 * @value: desired test pattern mode 1015 * 1016 * Return 0 on success or a negative error code otherwise 1017 */ 1018 static int csid_set_test_pattern(struct csid_device *csid, s32 value) 1019 { 1020 struct csid_testgen_config *tg = &csid->testgen; 1021 1022 /* If CSID is linked to CSIPHY, do not allow to enable test generator */ 1023 if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) 1024 return -EBUSY; 1025 1026 tg->enabled = !!value; 1027 1028 switch (value) { 1029 case 1: 1030 tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING; 1031 break; 1032 case 2: 1033 tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA; 1034 break; 1035 case 3: 1036 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES; 1037 break; 1038 case 4: 1039 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES; 1040 break; 1041 case 5: 1042 tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM; 1043 break; 1044 } 1045 1046 return 0; 1047 } 1048 1049 /* 1050 * csid_s_ctrl - Handle set control subdev method 1051 * @ctrl: pointer to v4l2 control structure 1052 * 1053 * Return 0 on success or a negative error code otherwise 1054 */ 1055 static int csid_s_ctrl(struct v4l2_ctrl *ctrl) 1056 { 1057 struct csid_device *csid = container_of(ctrl->handler, 1058 struct csid_device, ctrls); 1059 int ret = -EINVAL; 1060 1061 switch (ctrl->id) { 1062 case V4L2_CID_TEST_PATTERN: 1063 ret = csid_set_test_pattern(csid, ctrl->val); 1064 break; 1065 } 1066 1067 return ret; 1068 } 1069 1070 static const struct v4l2_ctrl_ops csid_ctrl_ops = { 1071 .s_ctrl = csid_s_ctrl, 1072 }; 1073 1074 /* 1075 * msm_csid_subdev_init - Initialize CSID device structure and resources 1076 * @csid: CSID device 1077 * @res: CSID module resources table 1078 * @id: CSID module id 1079 * 1080 * Return 0 on success or a negative error code otherwise 1081 */ 1082 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid, 1083 const struct resources *res, u8 id) 1084 { 1085 struct device *dev = camss->dev; 1086 struct platform_device *pdev = to_platform_device(dev); 1087 struct resource *r; 1088 int i, j; 1089 int ret; 1090 1091 csid->camss = camss; 1092 csid->id = id; 1093 1094 if (camss->version == CAMSS_8x16) { 1095 csid->formats = csid_formats_8x16; 1096 csid->nformats = 1097 ARRAY_SIZE(csid_formats_8x16); 1098 } else if (camss->version == CAMSS_8x96) { 1099 csid->formats = csid_formats_8x96; 1100 csid->nformats = 1101 ARRAY_SIZE(csid_formats_8x96); 1102 } else { 1103 return -EINVAL; 1104 } 1105 1106 /* Memory */ 1107 1108 r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]); 1109 csid->base = devm_ioremap_resource(dev, r); 1110 if (IS_ERR(csid->base)) { 1111 dev_err(dev, "could not map memory\n"); 1112 return PTR_ERR(csid->base); 1113 } 1114 1115 /* Interrupt */ 1116 1117 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, 1118 res->interrupt[0]); 1119 if (!r) { 1120 dev_err(dev, "missing IRQ\n"); 1121 return -EINVAL; 1122 } 1123 1124 csid->irq = r->start; 1125 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d", 1126 dev_name(dev), MSM_CSID_NAME, csid->id); 1127 ret = devm_request_irq(dev, csid->irq, csid_isr, 1128 IRQF_TRIGGER_RISING, csid->irq_name, csid); 1129 if (ret < 0) { 1130 dev_err(dev, "request_irq failed: %d\n", ret); 1131 return ret; 1132 } 1133 1134 disable_irq(csid->irq); 1135 1136 /* Clocks */ 1137 1138 csid->nclocks = 0; 1139 while (res->clock[csid->nclocks]) 1140 csid->nclocks++; 1141 1142 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock), 1143 GFP_KERNEL); 1144 if (!csid->clock) 1145 return -ENOMEM; 1146 1147 for (i = 0; i < csid->nclocks; i++) { 1148 struct camss_clock *clock = &csid->clock[i]; 1149 1150 clock->clk = devm_clk_get(dev, res->clock[i]); 1151 if (IS_ERR(clock->clk)) 1152 return PTR_ERR(clock->clk); 1153 1154 clock->name = res->clock[i]; 1155 1156 clock->nfreqs = 0; 1157 while (res->clock_rate[i][clock->nfreqs]) 1158 clock->nfreqs++; 1159 1160 if (!clock->nfreqs) { 1161 clock->freq = NULL; 1162 continue; 1163 } 1164 1165 clock->freq = devm_kcalloc(dev, 1166 clock->nfreqs, 1167 sizeof(*clock->freq), 1168 GFP_KERNEL); 1169 if (!clock->freq) 1170 return -ENOMEM; 1171 1172 for (j = 0; j < clock->nfreqs; j++) 1173 clock->freq[j] = res->clock_rate[i][j]; 1174 } 1175 1176 /* Regulator */ 1177 1178 csid->vdda = devm_regulator_get(dev, res->regulator[0]); 1179 if (IS_ERR(csid->vdda)) { 1180 dev_err(dev, "could not get regulator\n"); 1181 return PTR_ERR(csid->vdda); 1182 } 1183 1184 init_completion(&csid->reset_complete); 1185 1186 return 0; 1187 } 1188 1189 /* 1190 * msm_csid_get_csid_id - Get CSID HW module id 1191 * @entity: Pointer to CSID media entity structure 1192 * @id: Return CSID HW module id here 1193 */ 1194 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id) 1195 { 1196 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 1197 struct csid_device *csid = v4l2_get_subdevdata(sd); 1198 1199 *id = csid->id; 1200 } 1201 1202 /* 1203 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter 1204 * @lane_cfg - CSI2 lane configuration 1205 * 1206 * Return lane assign 1207 */ 1208 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg) 1209 { 1210 u32 lane_assign = 0; 1211 int i; 1212 1213 for (i = 0; i < lane_cfg->num_data; i++) 1214 lane_assign |= lane_cfg->data[i].pos << (i * 4); 1215 1216 return lane_assign; 1217 } 1218 1219 /* 1220 * csid_link_setup - Setup CSID connections 1221 * @entity: Pointer to media entity structure 1222 * @local: Pointer to local pad 1223 * @remote: Pointer to remote pad 1224 * @flags: Link flags 1225 * 1226 * Return 0 on success 1227 */ 1228 static int csid_link_setup(struct media_entity *entity, 1229 const struct media_pad *local, 1230 const struct media_pad *remote, u32 flags) 1231 { 1232 if (flags & MEDIA_LNK_FL_ENABLED) 1233 if (media_entity_remote_pad(local)) 1234 return -EBUSY; 1235 1236 if ((local->flags & MEDIA_PAD_FL_SINK) && 1237 (flags & MEDIA_LNK_FL_ENABLED)) { 1238 struct v4l2_subdev *sd; 1239 struct csid_device *csid; 1240 struct csiphy_device *csiphy; 1241 struct csiphy_lanes_cfg *lane_cfg; 1242 struct v4l2_subdev_format format = { 0 }; 1243 1244 sd = media_entity_to_v4l2_subdev(entity); 1245 csid = v4l2_get_subdevdata(sd); 1246 1247 /* If test generator is enabled */ 1248 /* do not allow a link from CSIPHY to CSID */ 1249 if (csid->testgen_mode->cur.val != 0) 1250 return -EBUSY; 1251 1252 sd = media_entity_to_v4l2_subdev(remote->entity); 1253 csiphy = v4l2_get_subdevdata(sd); 1254 1255 /* If a sensor is not linked to CSIPHY */ 1256 /* do no allow a link from CSIPHY to CSID */ 1257 if (!csiphy->cfg.csi2) 1258 return -EPERM; 1259 1260 csid->phy.csiphy_id = csiphy->id; 1261 1262 lane_cfg = &csiphy->cfg.csi2->lane_cfg; 1263 csid->phy.lane_cnt = lane_cfg->num_data; 1264 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg); 1265 1266 /* Reset format on source pad to sink pad format */ 1267 format.pad = MSM_CSID_PAD_SRC; 1268 format.which = V4L2_SUBDEV_FORMAT_ACTIVE; 1269 csid_set_format(&csid->subdev, NULL, &format); 1270 } 1271 1272 return 0; 1273 } 1274 1275 static const struct v4l2_subdev_core_ops csid_core_ops = { 1276 .s_power = csid_set_power, 1277 .subscribe_event = v4l2_ctrl_subdev_subscribe_event, 1278 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 1279 }; 1280 1281 static const struct v4l2_subdev_video_ops csid_video_ops = { 1282 .s_stream = csid_set_stream, 1283 }; 1284 1285 static const struct v4l2_subdev_pad_ops csid_pad_ops = { 1286 .enum_mbus_code = csid_enum_mbus_code, 1287 .enum_frame_size = csid_enum_frame_size, 1288 .get_fmt = csid_get_format, 1289 .set_fmt = csid_set_format, 1290 }; 1291 1292 static const struct v4l2_subdev_ops csid_v4l2_ops = { 1293 .core = &csid_core_ops, 1294 .video = &csid_video_ops, 1295 .pad = &csid_pad_ops, 1296 }; 1297 1298 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = { 1299 .open = csid_init_formats, 1300 }; 1301 1302 static const struct media_entity_operations csid_media_ops = { 1303 .link_setup = csid_link_setup, 1304 .link_validate = v4l2_subdev_link_validate, 1305 }; 1306 1307 /* 1308 * msm_csid_register_entity - Register subdev node for CSID module 1309 * @csid: CSID device 1310 * @v4l2_dev: V4L2 device 1311 * 1312 * Return 0 on success or a negative error code otherwise 1313 */ 1314 int msm_csid_register_entity(struct csid_device *csid, 1315 struct v4l2_device *v4l2_dev) 1316 { 1317 struct v4l2_subdev *sd = &csid->subdev; 1318 struct media_pad *pads = csid->pads; 1319 struct device *dev = csid->camss->dev; 1320 int ret; 1321 1322 v4l2_subdev_init(sd, &csid_v4l2_ops); 1323 sd->internal_ops = &csid_v4l2_internal_ops; 1324 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | 1325 V4L2_SUBDEV_FL_HAS_EVENTS; 1326 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", 1327 MSM_CSID_NAME, csid->id); 1328 v4l2_set_subdevdata(sd, csid); 1329 1330 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1); 1331 if (ret < 0) { 1332 dev_err(dev, "Failed to init ctrl handler: %d\n", ret); 1333 return ret; 1334 } 1335 1336 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls, 1337 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN, 1338 ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0, 1339 csid_test_pattern_menu); 1340 1341 if (csid->ctrls.error) { 1342 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error); 1343 ret = csid->ctrls.error; 1344 goto free_ctrl; 1345 } 1346 1347 csid->subdev.ctrl_handler = &csid->ctrls; 1348 1349 ret = csid_init_formats(sd, NULL); 1350 if (ret < 0) { 1351 dev_err(dev, "Failed to init format: %d\n", ret); 1352 goto free_ctrl; 1353 } 1354 1355 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 1356 pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE; 1357 1358 sd->entity.function = MEDIA_ENT_F_IO_V4L; 1359 sd->entity.ops = &csid_media_ops; 1360 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads); 1361 if (ret < 0) { 1362 dev_err(dev, "Failed to init media entity: %d\n", ret); 1363 goto free_ctrl; 1364 } 1365 1366 ret = v4l2_device_register_subdev(v4l2_dev, sd); 1367 if (ret < 0) { 1368 dev_err(dev, "Failed to register subdev: %d\n", ret); 1369 goto media_cleanup; 1370 } 1371 1372 return 0; 1373 1374 media_cleanup: 1375 media_entity_cleanup(&sd->entity); 1376 free_ctrl: 1377 v4l2_ctrl_handler_free(&csid->ctrls); 1378 1379 return ret; 1380 } 1381 1382 /* 1383 * msm_csid_unregister_entity - Unregister CSID module subdev node 1384 * @csid: CSID device 1385 */ 1386 void msm_csid_unregister_entity(struct csid_device *csid) 1387 { 1388 v4l2_device_unregister_subdev(&csid->subdev); 1389 media_entity_cleanup(&csid->subdev.entity); 1390 v4l2_ctrl_handler_free(&csid->ctrls); 1391 } 1392