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