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