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 csid->camss->version == CAMSS_660) { 388 switch (sink_code) { 389 case MEDIA_BUS_FMT_SBGGR10_1X10: 390 { 391 u32 src_code[] = { 392 MEDIA_BUS_FMT_SBGGR10_1X10, 393 MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 394 }; 395 396 return csid_find_code(src_code, ARRAY_SIZE(src_code), 397 index, src_req_code); 398 } 399 case MEDIA_BUS_FMT_Y10_1X10: 400 { 401 u32 src_code[] = { 402 MEDIA_BUS_FMT_Y10_1X10, 403 MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 404 }; 405 406 return csid_find_code(src_code, ARRAY_SIZE(src_code), 407 index, src_req_code); 408 } 409 default: 410 if (index > 0) 411 return 0; 412 413 return sink_code; 414 } 415 } else { 416 return 0; 417 } 418 } 419 420 static const struct csid_format *csid_get_fmt_entry( 421 const struct csid_format *formats, 422 unsigned int nformat, 423 u32 code) 424 { 425 unsigned int i; 426 427 for (i = 0; i < nformat; i++) 428 if (code == formats[i].code) 429 return &formats[i]; 430 431 WARN(1, "Unknown format\n"); 432 433 return &formats[0]; 434 } 435 436 /* 437 * csid_isr - CSID module interrupt handler 438 * @irq: Interrupt line 439 * @dev: CSID device 440 * 441 * Return IRQ_HANDLED on success 442 */ 443 static irqreturn_t csid_isr(int irq, void *dev) 444 { 445 struct csid_device *csid = dev; 446 enum camss_version ver = csid->camss->version; 447 u32 value; 448 449 value = readl_relaxed(csid->base + CAMSS_CSID_IRQ_STATUS(ver)); 450 writel_relaxed(value, csid->base + CAMSS_CSID_IRQ_CLEAR_CMD(ver)); 451 452 if ((value >> 11) & 0x1) 453 complete(&csid->reset_complete); 454 455 return IRQ_HANDLED; 456 } 457 458 /* 459 * csid_set_clock_rates - Calculate and set clock rates on CSID module 460 * @csiphy: CSID device 461 */ 462 static int csid_set_clock_rates(struct csid_device *csid) 463 { 464 struct device *dev = csid->camss->dev; 465 u32 pixel_clock; 466 int i, j; 467 int ret; 468 469 ret = camss_get_pixel_clock(&csid->subdev.entity, &pixel_clock); 470 if (ret) 471 pixel_clock = 0; 472 473 for (i = 0; i < csid->nclocks; i++) { 474 struct camss_clock *clock = &csid->clock[i]; 475 476 if (!strcmp(clock->name, "csi0") || 477 !strcmp(clock->name, "csi1") || 478 !strcmp(clock->name, "csi2") || 479 !strcmp(clock->name, "csi3")) { 480 const struct csid_format *f = csid_get_fmt_entry( 481 csid->formats, 482 csid->nformats, 483 csid->fmt[MSM_CSIPHY_PAD_SINK].code); 484 u8 num_lanes = csid->phy.lane_cnt; 485 u64 min_rate = pixel_clock * f->bpp / 486 (2 * num_lanes * 4); 487 long rate; 488 489 camss_add_clock_margin(&min_rate); 490 491 for (j = 0; j < clock->nfreqs; j++) 492 if (min_rate < clock->freq[j]) 493 break; 494 495 if (j == clock->nfreqs) { 496 dev_err(dev, 497 "Pixel clock is too high for CSID\n"); 498 return -EINVAL; 499 } 500 501 /* if sensor pixel clock is not available */ 502 /* set highest possible CSID clock rate */ 503 if (min_rate == 0) 504 j = clock->nfreqs - 1; 505 506 rate = clk_round_rate(clock->clk, clock->freq[j]); 507 if (rate < 0) { 508 dev_err(dev, "clk round rate failed: %ld\n", 509 rate); 510 return -EINVAL; 511 } 512 513 ret = clk_set_rate(clock->clk, rate); 514 if (ret < 0) { 515 dev_err(dev, "clk set rate failed: %d\n", ret); 516 return ret; 517 } 518 } 519 } 520 521 return 0; 522 } 523 524 /* 525 * csid_reset - Trigger reset on CSID module and wait to complete 526 * @csid: CSID device 527 * 528 * Return 0 on success or a negative error code otherwise 529 */ 530 static int csid_reset(struct csid_device *csid) 531 { 532 unsigned long time; 533 534 reinit_completion(&csid->reset_complete); 535 536 writel_relaxed(0x7fff, csid->base + 537 CAMSS_CSID_RST_CMD(csid->camss->version)); 538 539 time = wait_for_completion_timeout(&csid->reset_complete, 540 msecs_to_jiffies(CSID_RESET_TIMEOUT_MS)); 541 if (!time) { 542 dev_err(csid->camss->dev, "CSID reset timeout\n"); 543 return -EIO; 544 } 545 546 return 0; 547 } 548 549 /* 550 * csid_set_power - Power on/off CSID module 551 * @sd: CSID V4L2 subdevice 552 * @on: Requested power state 553 * 554 * Return 0 on success or a negative error code otherwise 555 */ 556 static int csid_set_power(struct v4l2_subdev *sd, int on) 557 { 558 struct csid_device *csid = v4l2_get_subdevdata(sd); 559 struct device *dev = csid->camss->dev; 560 int ret; 561 562 if (on) { 563 u32 hw_version; 564 565 ret = pm_runtime_get_sync(dev); 566 if (ret < 0) { 567 pm_runtime_put_sync(dev); 568 return ret; 569 } 570 571 ret = regulator_enable(csid->vdda); 572 if (ret < 0) { 573 pm_runtime_put_sync(dev); 574 return ret; 575 } 576 577 ret = csid_set_clock_rates(csid); 578 if (ret < 0) { 579 regulator_disable(csid->vdda); 580 pm_runtime_put_sync(dev); 581 return ret; 582 } 583 584 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev); 585 if (ret < 0) { 586 regulator_disable(csid->vdda); 587 pm_runtime_put_sync(dev); 588 return ret; 589 } 590 591 enable_irq(csid->irq); 592 593 ret = csid_reset(csid); 594 if (ret < 0) { 595 disable_irq(csid->irq); 596 camss_disable_clocks(csid->nclocks, csid->clock); 597 regulator_disable(csid->vdda); 598 pm_runtime_put_sync(dev); 599 return ret; 600 } 601 602 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION); 603 dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version); 604 } else { 605 disable_irq(csid->irq); 606 camss_disable_clocks(csid->nclocks, csid->clock); 607 ret = regulator_disable(csid->vdda); 608 pm_runtime_put_sync(dev); 609 } 610 611 return ret; 612 } 613 614 /* 615 * csid_set_stream - Enable/disable streaming on CSID module 616 * @sd: CSID V4L2 subdevice 617 * @enable: Requested streaming state 618 * 619 * Main configuration of CSID module is also done here. 620 * 621 * Return 0 on success or a negative error code otherwise 622 */ 623 static int csid_set_stream(struct v4l2_subdev *sd, int enable) 624 { 625 struct csid_device *csid = v4l2_get_subdevdata(sd); 626 struct csid_testgen_config *tg = &csid->testgen; 627 enum camss_version ver = csid->camss->version; 628 u32 val; 629 630 if (enable) { 631 u8 vc = 0; /* Virtual Channel 0 */ 632 u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */ 633 u8 dt, dt_shift, df; 634 int ret; 635 636 ret = v4l2_ctrl_handler_setup(&csid->ctrls); 637 if (ret < 0) { 638 dev_err(csid->camss->dev, 639 "could not sync v4l2 controls: %d\n", ret); 640 return ret; 641 } 642 643 if (!tg->enabled && 644 !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) 645 return -ENOLINK; 646 647 if (tg->enabled) { 648 /* Config Test Generator */ 649 struct v4l2_mbus_framefmt *f = 650 &csid->fmt[MSM_CSID_PAD_SRC]; 651 const struct csid_format *format = csid_get_fmt_entry( 652 csid->formats, csid->nformats, f->code); 653 u32 num_bytes_per_line = 654 f->width * format->bpp * format->spp / 8; 655 u32 num_lines = f->height; 656 657 /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */ 658 /* 1:0 VC */ 659 val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) | 660 ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13); 661 writel_relaxed(val, csid->base + 662 CAMSS_CSID_TG_VC_CFG(ver)); 663 664 /* 28:16 bytes per lines, 12:0 num of lines */ 665 val = ((num_bytes_per_line & 0x1fff) << 16) | 666 (num_lines & 0x1fff); 667 writel_relaxed(val, csid->base + 668 CAMSS_CSID_TG_DT_n_CGG_0(ver, 0)); 669 670 dt = format->data_type; 671 672 /* 5:0 data type */ 673 val = dt; 674 writel_relaxed(val, csid->base + 675 CAMSS_CSID_TG_DT_n_CGG_1(ver, 0)); 676 677 /* 2:0 output test pattern */ 678 val = tg->payload_mode; 679 writel_relaxed(val, csid->base + 680 CAMSS_CSID_TG_DT_n_CGG_2(ver, 0)); 681 682 df = format->decode_format; 683 } else { 684 struct v4l2_mbus_framefmt *f = 685 &csid->fmt[MSM_CSID_PAD_SINK]; 686 const struct csid_format *format = csid_get_fmt_entry( 687 csid->formats, csid->nformats, f->code); 688 struct csid_phy_config *phy = &csid->phy; 689 690 val = phy->lane_cnt - 1; 691 val |= phy->lane_assign << 4; 692 693 writel_relaxed(val, 694 csid->base + CAMSS_CSID_CORE_CTRL_0); 695 696 val = phy->csiphy_id << 17; 697 val |= 0x9; 698 699 writel_relaxed(val, 700 csid->base + CAMSS_CSID_CORE_CTRL_1); 701 702 dt = format->data_type; 703 df = format->decode_format; 704 } 705 706 /* Config LUT */ 707 708 dt_shift = (cid % 4) * 8; 709 710 val = readl_relaxed(csid->base + 711 CAMSS_CSID_CID_LUT_VC_n(ver, vc)); 712 val &= ~(0xff << dt_shift); 713 val |= dt << dt_shift; 714 writel_relaxed(val, csid->base + 715 CAMSS_CSID_CID_LUT_VC_n(ver, vc)); 716 717 val = CAMSS_CSID_CID_n_CFG_ISPIF_EN; 718 val |= CAMSS_CSID_CID_n_CFG_RDI_EN; 719 val |= df << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT; 720 val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP; 721 722 if (csid->camss->version == CAMSS_8x96 || 723 csid->camss->version == CAMSS_660) { 724 u32 sink_code = csid->fmt[MSM_CSID_PAD_SINK].code; 725 u32 src_code = csid->fmt[MSM_CSID_PAD_SRC].code; 726 727 if ((sink_code == MEDIA_BUS_FMT_SBGGR10_1X10 && 728 src_code == MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE) || 729 (sink_code == MEDIA_BUS_FMT_Y10_1X10 && 730 src_code == MEDIA_BUS_FMT_Y10_2X8_PADHI_LE)) { 731 val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_PLAIN_PACKING; 732 val |= CAMSS_CSID_CID_n_CFG_PLAIN_FORMAT_16; 733 val |= CAMSS_CSID_CID_n_CFG_PLAIN_ALIGNMENT_LSB; 734 } 735 } 736 737 writel_relaxed(val, csid->base + 738 CAMSS_CSID_CID_n_CFG(ver, cid)); 739 740 if (tg->enabled) { 741 val = CAMSS_CSID_TG_CTRL_ENABLE; 742 writel_relaxed(val, csid->base + 743 CAMSS_CSID_TG_CTRL(ver)); 744 } 745 } else { 746 if (tg->enabled) { 747 val = CAMSS_CSID_TG_CTRL_DISABLE; 748 writel_relaxed(val, csid->base + 749 CAMSS_CSID_TG_CTRL(ver)); 750 } 751 } 752 753 return 0; 754 } 755 756 /* 757 * __csid_get_format - Get pointer to format structure 758 * @csid: CSID device 759 * @cfg: V4L2 subdev pad configuration 760 * @pad: pad from which format is requested 761 * @which: TRY or ACTIVE format 762 * 763 * Return pointer to TRY or ACTIVE format structure 764 */ 765 static struct v4l2_mbus_framefmt * 766 __csid_get_format(struct csid_device *csid, 767 struct v4l2_subdev_pad_config *cfg, 768 unsigned int pad, 769 enum v4l2_subdev_format_whence which) 770 { 771 if (which == V4L2_SUBDEV_FORMAT_TRY) 772 return v4l2_subdev_get_try_format(&csid->subdev, cfg, pad); 773 774 return &csid->fmt[pad]; 775 } 776 777 /* 778 * csid_try_format - Handle try format by pad subdev method 779 * @csid: CSID device 780 * @cfg: V4L2 subdev pad configuration 781 * @pad: pad on which format is requested 782 * @fmt: pointer to v4l2 format structure 783 * @which: wanted subdev format 784 */ 785 static void csid_try_format(struct csid_device *csid, 786 struct v4l2_subdev_pad_config *cfg, 787 unsigned int pad, 788 struct v4l2_mbus_framefmt *fmt, 789 enum v4l2_subdev_format_whence which) 790 { 791 unsigned int i; 792 793 switch (pad) { 794 case MSM_CSID_PAD_SINK: 795 /* Set format on sink pad */ 796 797 for (i = 0; i < csid->nformats; i++) 798 if (fmt->code == csid->formats[i].code) 799 break; 800 801 /* If not found, use UYVY as default */ 802 if (i >= csid->nformats) 803 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8; 804 805 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 806 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 807 808 fmt->field = V4L2_FIELD_NONE; 809 fmt->colorspace = V4L2_COLORSPACE_SRGB; 810 811 break; 812 813 case MSM_CSID_PAD_SRC: 814 if (csid->testgen_mode->cur.val == 0) { 815 /* Test generator is disabled, */ 816 /* keep pad formats in sync */ 817 u32 code = fmt->code; 818 819 *fmt = *__csid_get_format(csid, cfg, 820 MSM_CSID_PAD_SINK, which); 821 fmt->code = csid_src_pad_code(csid, fmt->code, 0, code); 822 } else { 823 /* Test generator is enabled, set format on source */ 824 /* pad to allow test generator usage */ 825 826 for (i = 0; i < csid->nformats; i++) 827 if (csid->formats[i].code == fmt->code) 828 break; 829 830 /* If not found, use UYVY as default */ 831 if (i >= csid->nformats) 832 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8; 833 834 fmt->width = clamp_t(u32, fmt->width, 1, 8191); 835 fmt->height = clamp_t(u32, fmt->height, 1, 8191); 836 837 fmt->field = V4L2_FIELD_NONE; 838 } 839 break; 840 } 841 842 fmt->colorspace = V4L2_COLORSPACE_SRGB; 843 } 844 845 /* 846 * csid_enum_mbus_code - Handle pixel format enumeration 847 * @sd: CSID V4L2 subdevice 848 * @cfg: V4L2 subdev pad configuration 849 * @code: pointer to v4l2_subdev_mbus_code_enum structure 850 * return -EINVAL or zero on success 851 */ 852 static int csid_enum_mbus_code(struct v4l2_subdev *sd, 853 struct v4l2_subdev_pad_config *cfg, 854 struct v4l2_subdev_mbus_code_enum *code) 855 { 856 struct csid_device *csid = v4l2_get_subdevdata(sd); 857 858 if (code->pad == MSM_CSID_PAD_SINK) { 859 if (code->index >= csid->nformats) 860 return -EINVAL; 861 862 code->code = csid->formats[code->index].code; 863 } else { 864 if (csid->testgen_mode->cur.val == 0) { 865 struct v4l2_mbus_framefmt *sink_fmt; 866 867 sink_fmt = __csid_get_format(csid, cfg, 868 MSM_CSID_PAD_SINK, 869 code->which); 870 871 code->code = csid_src_pad_code(csid, sink_fmt->code, 872 code->index, 0); 873 if (!code->code) 874 return -EINVAL; 875 } else { 876 if (code->index >= csid->nformats) 877 return -EINVAL; 878 879 code->code = csid->formats[code->index].code; 880 } 881 } 882 883 return 0; 884 } 885 886 /* 887 * csid_enum_frame_size - Handle frame size enumeration 888 * @sd: CSID V4L2 subdevice 889 * @cfg: V4L2 subdev pad configuration 890 * @fse: pointer to v4l2_subdev_frame_size_enum structure 891 * return -EINVAL or zero on success 892 */ 893 static int csid_enum_frame_size(struct v4l2_subdev *sd, 894 struct v4l2_subdev_pad_config *cfg, 895 struct v4l2_subdev_frame_size_enum *fse) 896 { 897 struct csid_device *csid = v4l2_get_subdevdata(sd); 898 struct v4l2_mbus_framefmt format; 899 900 if (fse->index != 0) 901 return -EINVAL; 902 903 format.code = fse->code; 904 format.width = 1; 905 format.height = 1; 906 csid_try_format(csid, cfg, fse->pad, &format, fse->which); 907 fse->min_width = format.width; 908 fse->min_height = format.height; 909 910 if (format.code != fse->code) 911 return -EINVAL; 912 913 format.code = fse->code; 914 format.width = -1; 915 format.height = -1; 916 csid_try_format(csid, cfg, fse->pad, &format, fse->which); 917 fse->max_width = format.width; 918 fse->max_height = format.height; 919 920 return 0; 921 } 922 923 /* 924 * csid_get_format - Handle get format by pads subdev method 925 * @sd: CSID V4L2 subdevice 926 * @cfg: V4L2 subdev pad configuration 927 * @fmt: pointer to v4l2 subdev format structure 928 * 929 * Return -EINVAL or zero on success 930 */ 931 static int csid_get_format(struct v4l2_subdev *sd, 932 struct v4l2_subdev_pad_config *cfg, 933 struct v4l2_subdev_format *fmt) 934 { 935 struct csid_device *csid = v4l2_get_subdevdata(sd); 936 struct v4l2_mbus_framefmt *format; 937 938 format = __csid_get_format(csid, cfg, fmt->pad, fmt->which); 939 if (format == NULL) 940 return -EINVAL; 941 942 fmt->format = *format; 943 944 return 0; 945 } 946 947 /* 948 * csid_set_format - Handle set format by pads subdev method 949 * @sd: CSID V4L2 subdevice 950 * @cfg: V4L2 subdev pad configuration 951 * @fmt: pointer to v4l2 subdev format structure 952 * 953 * Return -EINVAL or zero on success 954 */ 955 static int csid_set_format(struct v4l2_subdev *sd, 956 struct v4l2_subdev_pad_config *cfg, 957 struct v4l2_subdev_format *fmt) 958 { 959 struct csid_device *csid = v4l2_get_subdevdata(sd); 960 struct v4l2_mbus_framefmt *format; 961 962 format = __csid_get_format(csid, cfg, fmt->pad, fmt->which); 963 if (format == NULL) 964 return -EINVAL; 965 966 csid_try_format(csid, cfg, fmt->pad, &fmt->format, fmt->which); 967 *format = fmt->format; 968 969 /* Propagate the format from sink to source */ 970 if (fmt->pad == MSM_CSID_PAD_SINK) { 971 format = __csid_get_format(csid, cfg, MSM_CSID_PAD_SRC, 972 fmt->which); 973 974 *format = fmt->format; 975 csid_try_format(csid, cfg, MSM_CSID_PAD_SRC, format, 976 fmt->which); 977 } 978 979 return 0; 980 } 981 982 /* 983 * csid_init_formats - Initialize formats on all pads 984 * @sd: CSID V4L2 subdevice 985 * @fh: V4L2 subdev file handle 986 * 987 * Initialize all pad formats with default values. 988 * 989 * Return 0 on success or a negative error code otherwise 990 */ 991 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) 992 { 993 struct v4l2_subdev_format format = { 994 .pad = MSM_CSID_PAD_SINK, 995 .which = fh ? V4L2_SUBDEV_FORMAT_TRY : 996 V4L2_SUBDEV_FORMAT_ACTIVE, 997 .format = { 998 .code = MEDIA_BUS_FMT_UYVY8_2X8, 999 .width = 1920, 1000 .height = 1080 1001 } 1002 }; 1003 1004 return csid_set_format(sd, fh ? fh->pad : NULL, &format); 1005 } 1006 1007 static const char * const csid_test_pattern_menu[] = { 1008 "Disabled", 1009 "Incrementing", 1010 "Alternating 0x55/0xAA", 1011 "All Zeros 0x00", 1012 "All Ones 0xFF", 1013 "Pseudo-random Data", 1014 }; 1015 1016 /* 1017 * csid_set_test_pattern - Set test generator's pattern mode 1018 * @csid: CSID device 1019 * @value: desired test pattern mode 1020 * 1021 * Return 0 on success or a negative error code otherwise 1022 */ 1023 static int csid_set_test_pattern(struct csid_device *csid, s32 value) 1024 { 1025 struct csid_testgen_config *tg = &csid->testgen; 1026 1027 /* If CSID is linked to CSIPHY, do not allow to enable test generator */ 1028 if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) 1029 return -EBUSY; 1030 1031 tg->enabled = !!value; 1032 1033 switch (value) { 1034 case 1: 1035 tg->payload_mode = CSID_PAYLOAD_MODE_INCREMENTING; 1036 break; 1037 case 2: 1038 tg->payload_mode = CSID_PAYLOAD_MODE_ALTERNATING_55_AA; 1039 break; 1040 case 3: 1041 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ZEROES; 1042 break; 1043 case 4: 1044 tg->payload_mode = CSID_PAYLOAD_MODE_ALL_ONES; 1045 break; 1046 case 5: 1047 tg->payload_mode = CSID_PAYLOAD_MODE_RANDOM; 1048 break; 1049 } 1050 1051 return 0; 1052 } 1053 1054 /* 1055 * csid_s_ctrl - Handle set control subdev method 1056 * @ctrl: pointer to v4l2 control structure 1057 * 1058 * Return 0 on success or a negative error code otherwise 1059 */ 1060 static int csid_s_ctrl(struct v4l2_ctrl *ctrl) 1061 { 1062 struct csid_device *csid = container_of(ctrl->handler, 1063 struct csid_device, ctrls); 1064 int ret = -EINVAL; 1065 1066 switch (ctrl->id) { 1067 case V4L2_CID_TEST_PATTERN: 1068 ret = csid_set_test_pattern(csid, ctrl->val); 1069 break; 1070 } 1071 1072 return ret; 1073 } 1074 1075 static const struct v4l2_ctrl_ops csid_ctrl_ops = { 1076 .s_ctrl = csid_s_ctrl, 1077 }; 1078 1079 /* 1080 * msm_csid_subdev_init - Initialize CSID device structure and resources 1081 * @csid: CSID device 1082 * @res: CSID module resources table 1083 * @id: CSID module id 1084 * 1085 * Return 0 on success or a negative error code otherwise 1086 */ 1087 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid, 1088 const struct resources *res, u8 id) 1089 { 1090 struct device *dev = camss->dev; 1091 struct platform_device *pdev = to_platform_device(dev); 1092 struct resource *r; 1093 int i, j; 1094 int ret; 1095 1096 csid->camss = camss; 1097 csid->id = id; 1098 1099 if (camss->version == CAMSS_8x16) { 1100 csid->formats = csid_formats_8x16; 1101 csid->nformats = 1102 ARRAY_SIZE(csid_formats_8x16); 1103 } else if (camss->version == CAMSS_8x96 || 1104 camss->version == CAMSS_660) { 1105 csid->formats = csid_formats_8x96; 1106 csid->nformats = 1107 ARRAY_SIZE(csid_formats_8x96); 1108 } else { 1109 return -EINVAL; 1110 } 1111 1112 /* Memory */ 1113 1114 r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]); 1115 csid->base = devm_ioremap_resource(dev, r); 1116 if (IS_ERR(csid->base)) { 1117 dev_err(dev, "could not map memory\n"); 1118 return PTR_ERR(csid->base); 1119 } 1120 1121 /* Interrupt */ 1122 1123 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, 1124 res->interrupt[0]); 1125 if (!r) { 1126 dev_err(dev, "missing IRQ\n"); 1127 return -EINVAL; 1128 } 1129 1130 csid->irq = r->start; 1131 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d", 1132 dev_name(dev), MSM_CSID_NAME, csid->id); 1133 ret = devm_request_irq(dev, csid->irq, csid_isr, 1134 IRQF_TRIGGER_RISING, csid->irq_name, csid); 1135 if (ret < 0) { 1136 dev_err(dev, "request_irq failed: %d\n", ret); 1137 return ret; 1138 } 1139 1140 disable_irq(csid->irq); 1141 1142 /* Clocks */ 1143 1144 csid->nclocks = 0; 1145 while (res->clock[csid->nclocks]) 1146 csid->nclocks++; 1147 1148 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock), 1149 GFP_KERNEL); 1150 if (!csid->clock) 1151 return -ENOMEM; 1152 1153 for (i = 0; i < csid->nclocks; i++) { 1154 struct camss_clock *clock = &csid->clock[i]; 1155 1156 clock->clk = devm_clk_get(dev, res->clock[i]); 1157 if (IS_ERR(clock->clk)) 1158 return PTR_ERR(clock->clk); 1159 1160 clock->name = res->clock[i]; 1161 1162 clock->nfreqs = 0; 1163 while (res->clock_rate[i][clock->nfreqs]) 1164 clock->nfreqs++; 1165 1166 if (!clock->nfreqs) { 1167 clock->freq = NULL; 1168 continue; 1169 } 1170 1171 clock->freq = devm_kcalloc(dev, 1172 clock->nfreqs, 1173 sizeof(*clock->freq), 1174 GFP_KERNEL); 1175 if (!clock->freq) 1176 return -ENOMEM; 1177 1178 for (j = 0; j < clock->nfreqs; j++) 1179 clock->freq[j] = res->clock_rate[i][j]; 1180 } 1181 1182 /* Regulator */ 1183 1184 csid->vdda = devm_regulator_get(dev, res->regulator[0]); 1185 if (IS_ERR(csid->vdda)) { 1186 dev_err(dev, "could not get regulator\n"); 1187 return PTR_ERR(csid->vdda); 1188 } 1189 1190 init_completion(&csid->reset_complete); 1191 1192 return 0; 1193 } 1194 1195 /* 1196 * msm_csid_get_csid_id - Get CSID HW module id 1197 * @entity: Pointer to CSID media entity structure 1198 * @id: Return CSID HW module id here 1199 */ 1200 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id) 1201 { 1202 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 1203 struct csid_device *csid = v4l2_get_subdevdata(sd); 1204 1205 *id = csid->id; 1206 } 1207 1208 /* 1209 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter 1210 * @lane_cfg - CSI2 lane configuration 1211 * 1212 * Return lane assign 1213 */ 1214 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg) 1215 { 1216 u32 lane_assign = 0; 1217 int i; 1218 1219 for (i = 0; i < lane_cfg->num_data; i++) 1220 lane_assign |= lane_cfg->data[i].pos << (i * 4); 1221 1222 return lane_assign; 1223 } 1224 1225 /* 1226 * csid_link_setup - Setup CSID connections 1227 * @entity: Pointer to media entity structure 1228 * @local: Pointer to local pad 1229 * @remote: Pointer to remote pad 1230 * @flags: Link flags 1231 * 1232 * Return 0 on success 1233 */ 1234 static int csid_link_setup(struct media_entity *entity, 1235 const struct media_pad *local, 1236 const struct media_pad *remote, u32 flags) 1237 { 1238 if (flags & MEDIA_LNK_FL_ENABLED) 1239 if (media_entity_remote_pad(local)) 1240 return -EBUSY; 1241 1242 if ((local->flags & MEDIA_PAD_FL_SINK) && 1243 (flags & MEDIA_LNK_FL_ENABLED)) { 1244 struct v4l2_subdev *sd; 1245 struct csid_device *csid; 1246 struct csiphy_device *csiphy; 1247 struct csiphy_lanes_cfg *lane_cfg; 1248 struct v4l2_subdev_format format = { 0 }; 1249 1250 sd = media_entity_to_v4l2_subdev(entity); 1251 csid = v4l2_get_subdevdata(sd); 1252 1253 /* If test generator is enabled */ 1254 /* do not allow a link from CSIPHY to CSID */ 1255 if (csid->testgen_mode->cur.val != 0) 1256 return -EBUSY; 1257 1258 sd = media_entity_to_v4l2_subdev(remote->entity); 1259 csiphy = v4l2_get_subdevdata(sd); 1260 1261 /* If a sensor is not linked to CSIPHY */ 1262 /* do no allow a link from CSIPHY to CSID */ 1263 if (!csiphy->cfg.csi2) 1264 return -EPERM; 1265 1266 csid->phy.csiphy_id = csiphy->id; 1267 1268 lane_cfg = &csiphy->cfg.csi2->lane_cfg; 1269 csid->phy.lane_cnt = lane_cfg->num_data; 1270 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg); 1271 1272 /* Reset format on source pad to sink pad format */ 1273 format.pad = MSM_CSID_PAD_SRC; 1274 format.which = V4L2_SUBDEV_FORMAT_ACTIVE; 1275 csid_set_format(&csid->subdev, NULL, &format); 1276 } 1277 1278 return 0; 1279 } 1280 1281 static const struct v4l2_subdev_core_ops csid_core_ops = { 1282 .s_power = csid_set_power, 1283 .subscribe_event = v4l2_ctrl_subdev_subscribe_event, 1284 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 1285 }; 1286 1287 static const struct v4l2_subdev_video_ops csid_video_ops = { 1288 .s_stream = csid_set_stream, 1289 }; 1290 1291 static const struct v4l2_subdev_pad_ops csid_pad_ops = { 1292 .enum_mbus_code = csid_enum_mbus_code, 1293 .enum_frame_size = csid_enum_frame_size, 1294 .get_fmt = csid_get_format, 1295 .set_fmt = csid_set_format, 1296 }; 1297 1298 static const struct v4l2_subdev_ops csid_v4l2_ops = { 1299 .core = &csid_core_ops, 1300 .video = &csid_video_ops, 1301 .pad = &csid_pad_ops, 1302 }; 1303 1304 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = { 1305 .open = csid_init_formats, 1306 }; 1307 1308 static const struct media_entity_operations csid_media_ops = { 1309 .link_setup = csid_link_setup, 1310 .link_validate = v4l2_subdev_link_validate, 1311 }; 1312 1313 /* 1314 * msm_csid_register_entity - Register subdev node for CSID module 1315 * @csid: CSID device 1316 * @v4l2_dev: V4L2 device 1317 * 1318 * Return 0 on success or a negative error code otherwise 1319 */ 1320 int msm_csid_register_entity(struct csid_device *csid, 1321 struct v4l2_device *v4l2_dev) 1322 { 1323 struct v4l2_subdev *sd = &csid->subdev; 1324 struct media_pad *pads = csid->pads; 1325 struct device *dev = csid->camss->dev; 1326 int ret; 1327 1328 v4l2_subdev_init(sd, &csid_v4l2_ops); 1329 sd->internal_ops = &csid_v4l2_internal_ops; 1330 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | 1331 V4L2_SUBDEV_FL_HAS_EVENTS; 1332 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", 1333 MSM_CSID_NAME, csid->id); 1334 v4l2_set_subdevdata(sd, csid); 1335 1336 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1); 1337 if (ret < 0) { 1338 dev_err(dev, "Failed to init ctrl handler: %d\n", ret); 1339 return ret; 1340 } 1341 1342 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls, 1343 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN, 1344 ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0, 1345 csid_test_pattern_menu); 1346 1347 if (csid->ctrls.error) { 1348 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error); 1349 ret = csid->ctrls.error; 1350 goto free_ctrl; 1351 } 1352 1353 csid->subdev.ctrl_handler = &csid->ctrls; 1354 1355 ret = csid_init_formats(sd, NULL); 1356 if (ret < 0) { 1357 dev_err(dev, "Failed to init format: %d\n", ret); 1358 goto free_ctrl; 1359 } 1360 1361 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 1362 pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE; 1363 1364 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; 1365 sd->entity.ops = &csid_media_ops; 1366 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads); 1367 if (ret < 0) { 1368 dev_err(dev, "Failed to init media entity: %d\n", ret); 1369 goto free_ctrl; 1370 } 1371 1372 ret = v4l2_device_register_subdev(v4l2_dev, sd); 1373 if (ret < 0) { 1374 dev_err(dev, "Failed to register subdev: %d\n", ret); 1375 goto media_cleanup; 1376 } 1377 1378 return 0; 1379 1380 media_cleanup: 1381 media_entity_cleanup(&sd->entity); 1382 free_ctrl: 1383 v4l2_ctrl_handler_free(&csid->ctrls); 1384 1385 return ret; 1386 } 1387 1388 /* 1389 * msm_csid_unregister_entity - Unregister CSID module subdev node 1390 * @csid: CSID device 1391 */ 1392 void msm_csid_unregister_entity(struct csid_device *csid) 1393 { 1394 v4l2_device_unregister_subdev(&csid->subdev); 1395 media_entity_cleanup(&csid->subdev.entity); 1396 v4l2_ctrl_handler_free(&csid->ctrls); 1397 } 1398