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