drivers/media/platform/qcom/camss/camss-csid.c

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