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 78struct 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 86static 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 208static 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 358static 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 378static 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 419static 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 */ 442static 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 */ 461static 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 */ 529static 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 */ 555static 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 pm_runtime_put_sync(dev); 567 return ret; 568 } 569 570 ret = regulator_enable(csid->vdda); 571 if (ret < 0) { 572 pm_runtime_put_sync(dev); 573 return ret; 574 } 575 576 ret = csid_set_clock_rates(csid); 577 if (ret < 0) { 578 regulator_disable(csid->vdda); 579 pm_runtime_put_sync(dev); 580 return ret; 581 } 582 583 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev); 584 if (ret < 0) { 585 regulator_disable(csid->vdda); 586 pm_runtime_put_sync(dev); 587 return ret; 588 } 589 590 enable_irq(csid->irq); 591 592 ret = csid_reset(csid); 593 if (ret < 0) { 594 disable_irq(csid->irq); 595 camss_disable_clocks(csid->nclocks, csid->clock); 596 regulator_disable(csid->vdda); 597 pm_runtime_put_sync(dev); 598 return ret; 599 } 600 601 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION); 602 dev_dbg(dev, "CSID HW Version = 0x%08x\n", hw_version); 603 } else { 604 disable_irq(csid->irq); 605 camss_disable_clocks(csid->nclocks, csid->clock); 606 ret = regulator_disable(csid->vdda); 607 pm_runtime_put_sync(dev); 608 } 609 610 return ret; 611} 612 613/* 614 * csid_set_stream - Enable/disable streaming on CSID module 615 * @sd: CSID V4L2 subdevice 616 * @enable: Requested streaming state 617 * 618 * Main configuration of CSID module is also done here. 619 * 620 * Return 0 on success or a negative error code otherwise 621 */ 622static int csid_set_stream(struct v4l2_subdev *sd, int enable) 623{ 624 struct csid_device *csid = v4l2_get_subdevdata(sd); 625 struct csid_testgen_config *tg = &csid->testgen; 626 enum camss_version ver = csid->camss->version; 627 u32 val; 628 629 if (enable) { 630 u8 vc = 0; /* Virtual Channel 0 */ 631 u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */ 632 u8 dt, dt_shift, df; 633 int ret; 634 635 ret = v4l2_ctrl_handler_setup(&csid->ctrls); 636 if (ret < 0) { 637 dev_err(csid->camss->dev, 638 "could not sync v4l2 controls: %d\n", ret); 639 return ret; 640 } 641 642 if (!tg->enabled && 643 !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK])) 644 return -ENOLINK; 645 646 if (tg->enabled) { 647 /* Config Test Generator */ 648 struct v4l2_mbus_framefmt *f = 649 &csid->fmt[MSM_CSID_PAD_SRC]; 650 const struct csid_format *format = csid_get_fmt_entry( 651 csid->formats, csid->nformats, f->code); 652 u32 num_bytes_per_line = 653 f->width * format->bpp * format->spp / 8; 654 u32 num_lines = f->height; 655 656 /* 31:24 V blank, 23:13 H blank, 3:2 num of active DT */ 657 /* 1:0 VC */ 658 val = ((CAMSS_CSID_TG_VC_CFG_V_BLANKING & 0xff) << 24) | 659 ((CAMSS_CSID_TG_VC_CFG_H_BLANKING & 0x7ff) << 13); 660 writel_relaxed(val, csid->base + 661 CAMSS_CSID_TG_VC_CFG(ver)); 662 663 /* 28:16 bytes per lines, 12:0 num of lines */ 664 val = ((num_bytes_per_line & 0x1fff) << 16) | 665 (num_lines & 0x1fff); 666 writel_relaxed(val, csid->base + 667 CAMSS_CSID_TG_DT_n_CGG_0(ver, 0)); 668 669 dt = format->data_type; 670 671 /* 5:0 data type */ 672 val = dt; 673 writel_relaxed(val, csid->base + 674 CAMSS_CSID_TG_DT_n_CGG_1(ver, 0)); 675 676 /* 2:0 output test pattern */ 677 val = tg->payload_mode; 678 writel_relaxed(val, csid->base + 679 CAMSS_CSID_TG_DT_n_CGG_2(ver, 0)); 680 681 df = format->decode_format; 682 } else { 683 struct v4l2_mbus_framefmt *f = 684 &csid->fmt[MSM_CSID_PAD_SINK]; 685 const struct csid_format *format = csid_get_fmt_entry( 686 csid->formats, csid->nformats, f->code); 687 struct csid_phy_config *phy = &csid->phy; 688 689 val = phy->lane_cnt - 1; 690 val |= phy->lane_assign << 4; 691 692 writel_relaxed(val, 693 csid->base + CAMSS_CSID_CORE_CTRL_0); 694 695 val = phy->csiphy_id << 17; 696 val |= 0x9; 697 698 writel_relaxed(val, 699 csid->base + CAMSS_CSID_CORE_CTRL_1); 700 701 dt = format->data_type; 702 df = format->decode_format; 703 } 704 705 /* Config LUT */ 706 707 dt_shift = (cid % 4) * 8; 708 709 val = readl_relaxed(csid->base + 710 CAMSS_CSID_CID_LUT_VC_n(ver, vc)); 711 val &= ~(0xff << dt_shift); 712 val |= dt << dt_shift; 713 writel_relaxed(val, csid->base + 714 CAMSS_CSID_CID_LUT_VC_n(ver, vc)); 715 716 val = CAMSS_CSID_CID_n_CFG_ISPIF_EN; 717 val |= CAMSS_CSID_CID_n_CFG_RDI_EN; 718 val |= df << CAMSS_CSID_CID_n_CFG_DECODE_FORMAT_SHIFT; 719 val |= CAMSS_CSID_CID_n_CFG_RDI_MODE_RAW_DUMP; 720 721 if (csid->camss->version == CAMSS_8x96) { 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 */ 763static 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 */ 783static 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 */ 850static 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 */ 891static 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 */ 929static 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 */ 953static 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 */ 989static 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 1005static 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 */ 1021static 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 */ 1058static 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 1073static 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 */ 1085int 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 csid->formats = csid_formats_8x96; 1103 csid->nformats = 1104 ARRAY_SIZE(csid_formats_8x96); 1105 } else { 1106 return -EINVAL; 1107 } 1108 1109 /* Memory */ 1110 1111 r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]); 1112 csid->base = devm_ioremap_resource(dev, r); 1113 if (IS_ERR(csid->base)) { 1114 dev_err(dev, "could not map memory\n"); 1115 return PTR_ERR(csid->base); 1116 } 1117 1118 /* Interrupt */ 1119 1120 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ, 1121 res->interrupt[0]); 1122 if (!r) { 1123 dev_err(dev, "missing IRQ\n"); 1124 return -EINVAL; 1125 } 1126 1127 csid->irq = r->start; 1128 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d", 1129 dev_name(dev), MSM_CSID_NAME, csid->id); 1130 ret = devm_request_irq(dev, csid->irq, csid_isr, 1131 IRQF_TRIGGER_RISING, csid->irq_name, csid); 1132 if (ret < 0) { 1133 dev_err(dev, "request_irq failed: %d\n", ret); 1134 return ret; 1135 } 1136 1137 disable_irq(csid->irq); 1138 1139 /* Clocks */ 1140 1141 csid->nclocks = 0; 1142 while (res->clock[csid->nclocks]) 1143 csid->nclocks++; 1144 1145 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock), 1146 GFP_KERNEL); 1147 if (!csid->clock) 1148 return -ENOMEM; 1149 1150 for (i = 0; i < csid->nclocks; i++) { 1151 struct camss_clock *clock = &csid->clock[i]; 1152 1153 clock->clk = devm_clk_get(dev, res->clock[i]); 1154 if (IS_ERR(clock->clk)) 1155 return PTR_ERR(clock->clk); 1156 1157 clock->name = res->clock[i]; 1158 1159 clock->nfreqs = 0; 1160 while (res->clock_rate[i][clock->nfreqs]) 1161 clock->nfreqs++; 1162 1163 if (!clock->nfreqs) { 1164 clock->freq = NULL; 1165 continue; 1166 } 1167 1168 clock->freq = devm_kcalloc(dev, 1169 clock->nfreqs, 1170 sizeof(*clock->freq), 1171 GFP_KERNEL); 1172 if (!clock->freq) 1173 return -ENOMEM; 1174 1175 for (j = 0; j < clock->nfreqs; j++) 1176 clock->freq[j] = res->clock_rate[i][j]; 1177 } 1178 1179 /* Regulator */ 1180 1181 csid->vdda = devm_regulator_get(dev, res->regulator[0]); 1182 if (IS_ERR(csid->vdda)) { 1183 dev_err(dev, "could not get regulator\n"); 1184 return PTR_ERR(csid->vdda); 1185 } 1186 1187 init_completion(&csid->reset_complete); 1188 1189 return 0; 1190} 1191 1192/* 1193 * msm_csid_get_csid_id - Get CSID HW module id 1194 * @entity: Pointer to CSID media entity structure 1195 * @id: Return CSID HW module id here 1196 */ 1197void msm_csid_get_csid_id(struct media_entity *entity, u8 *id) 1198{ 1199 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 1200 struct csid_device *csid = v4l2_get_subdevdata(sd); 1201 1202 *id = csid->id; 1203} 1204 1205/* 1206 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter 1207 * @lane_cfg - CSI2 lane configuration 1208 * 1209 * Return lane assign 1210 */ 1211static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg) 1212{ 1213 u32 lane_assign = 0; 1214 int i; 1215 1216 for (i = 0; i < lane_cfg->num_data; i++) 1217 lane_assign |= lane_cfg->data[i].pos << (i * 4); 1218 1219 return lane_assign; 1220} 1221 1222/* 1223 * csid_link_setup - Setup CSID connections 1224 * @entity: Pointer to media entity structure 1225 * @local: Pointer to local pad 1226 * @remote: Pointer to remote pad 1227 * @flags: Link flags 1228 * 1229 * Return 0 on success 1230 */ 1231static int csid_link_setup(struct media_entity *entity, 1232 const struct media_pad *local, 1233 const struct media_pad *remote, u32 flags) 1234{ 1235 if (flags & MEDIA_LNK_FL_ENABLED) 1236 if (media_entity_remote_pad(local)) 1237 return -EBUSY; 1238 1239 if ((local->flags & MEDIA_PAD_FL_SINK) && 1240 (flags & MEDIA_LNK_FL_ENABLED)) { 1241 struct v4l2_subdev *sd; 1242 struct csid_device *csid; 1243 struct csiphy_device *csiphy; 1244 struct csiphy_lanes_cfg *lane_cfg; 1245 struct v4l2_subdev_format format = { 0 }; 1246 1247 sd = media_entity_to_v4l2_subdev(entity); 1248 csid = v4l2_get_subdevdata(sd); 1249 1250 /* If test generator is enabled */ 1251 /* do not allow a link from CSIPHY to CSID */ 1252 if (csid->testgen_mode->cur.val != 0) 1253 return -EBUSY; 1254 1255 sd = media_entity_to_v4l2_subdev(remote->entity); 1256 csiphy = v4l2_get_subdevdata(sd); 1257 1258 /* If a sensor is not linked to CSIPHY */ 1259 /* do no allow a link from CSIPHY to CSID */ 1260 if (!csiphy->cfg.csi2) 1261 return -EPERM; 1262 1263 csid->phy.csiphy_id = csiphy->id; 1264 1265 lane_cfg = &csiphy->cfg.csi2->lane_cfg; 1266 csid->phy.lane_cnt = lane_cfg->num_data; 1267 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg); 1268 1269 /* Reset format on source pad to sink pad format */ 1270 format.pad = MSM_CSID_PAD_SRC; 1271 format.which = V4L2_SUBDEV_FORMAT_ACTIVE; 1272 csid_set_format(&csid->subdev, NULL, &format); 1273 } 1274 1275 return 0; 1276} 1277 1278static const struct v4l2_subdev_core_ops csid_core_ops = { 1279 .s_power = csid_set_power, 1280 .subscribe_event = v4l2_ctrl_subdev_subscribe_event, 1281 .unsubscribe_event = v4l2_event_subdev_unsubscribe, 1282}; 1283 1284static const struct v4l2_subdev_video_ops csid_video_ops = { 1285 .s_stream = csid_set_stream, 1286}; 1287 1288static const struct v4l2_subdev_pad_ops csid_pad_ops = { 1289 .enum_mbus_code = csid_enum_mbus_code, 1290 .enum_frame_size = csid_enum_frame_size, 1291 .get_fmt = csid_get_format, 1292 .set_fmt = csid_set_format, 1293}; 1294 1295static const struct v4l2_subdev_ops csid_v4l2_ops = { 1296 .core = &csid_core_ops, 1297 .video = &csid_video_ops, 1298 .pad = &csid_pad_ops, 1299}; 1300 1301static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = { 1302 .open = csid_init_formats, 1303}; 1304 1305static const struct media_entity_operations csid_media_ops = { 1306 .link_setup = csid_link_setup, 1307 .link_validate = v4l2_subdev_link_validate, 1308}; 1309 1310/* 1311 * msm_csid_register_entity - Register subdev node for CSID module 1312 * @csid: CSID device 1313 * @v4l2_dev: V4L2 device 1314 * 1315 * Return 0 on success or a negative error code otherwise 1316 */ 1317int msm_csid_register_entity(struct csid_device *csid, 1318 struct v4l2_device *v4l2_dev) 1319{ 1320 struct v4l2_subdev *sd = &csid->subdev; 1321 struct media_pad *pads = csid->pads; 1322 struct device *dev = csid->camss->dev; 1323 int ret; 1324 1325 v4l2_subdev_init(sd, &csid_v4l2_ops); 1326 sd->internal_ops = &csid_v4l2_internal_ops; 1327 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | 1328 V4L2_SUBDEV_FL_HAS_EVENTS; 1329 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", 1330 MSM_CSID_NAME, csid->id); 1331 v4l2_set_subdevdata(sd, csid); 1332 1333 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1); 1334 if (ret < 0) { 1335 dev_err(dev, "Failed to init ctrl handler: %d\n", ret); 1336 return ret; 1337 } 1338 1339 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls, 1340 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN, 1341 ARRAY_SIZE(csid_test_pattern_menu) - 1, 0, 0, 1342 csid_test_pattern_menu); 1343 1344 if (csid->ctrls.error) { 1345 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error); 1346 ret = csid->ctrls.error; 1347 goto free_ctrl; 1348 } 1349 1350 csid->subdev.ctrl_handler = &csid->ctrls; 1351 1352 ret = csid_init_formats(sd, NULL); 1353 if (ret < 0) { 1354 dev_err(dev, "Failed to init format: %d\n", ret); 1355 goto free_ctrl; 1356 } 1357 1358 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK; 1359 pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE; 1360 1361 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; 1362 sd->entity.ops = &csid_media_ops; 1363 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads); 1364 if (ret < 0) { 1365 dev_err(dev, "Failed to init media entity: %d\n", ret); 1366 goto free_ctrl; 1367 } 1368 1369 ret = v4l2_device_register_subdev(v4l2_dev, sd); 1370 if (ret < 0) { 1371 dev_err(dev, "Failed to register subdev: %d\n", ret); 1372 goto media_cleanup; 1373 } 1374 1375 return 0; 1376 1377media_cleanup: 1378 media_entity_cleanup(&sd->entity); 1379free_ctrl: 1380 v4l2_ctrl_handler_free(&csid->ctrls); 1381 1382 return ret; 1383} 1384 1385/* 1386 * msm_csid_unregister_entity - Unregister CSID module subdev node 1387 * @csid: CSID device 1388 */ 1389void msm_csid_unregister_entity(struct csid_device *csid) 1390{ 1391 v4l2_device_unregister_subdev(&csid->subdev); 1392 media_entity_cleanup(&csid->subdev.entity); 1393 v4l2_ctrl_handler_free(&csid->ctrls); 1394} 1395