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