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

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * camss-csid.c
   4  *
   5  * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
   6  *
   7  * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
   8  * Copyright (C) 2015-2018 Linaro Ltd.
   9  */
  10 #include <linux/clk.h>
  11 #include <linux/completion.h>
  12 #include <linux/interrupt.h>
  13 #include <linux/io.h>
  14 #include <linux/kernel.h>
  15 #include <linux/of.h>
  16 #include <linux/platform_device.h>
  17 #include <linux/pm_runtime.h>
  18 #include <linux/regulator/consumer.h>
  19 #include <media/media-entity.h>
  20 #include <media/v4l2-device.h>
  21 #include <media/v4l2-event.h>
  22 #include <media/v4l2-subdev.h>
  23
  24 #include "camss-csid.h"
  25 #include "camss-csid-gen1.h"
  26 #include "camss.h"
  27
  28 #define MSM_CSID_NAME "msm_csid"
  29
  30 const char * const csid_testgen_modes[] = {
  31         "Disabled",
  32         "Incrementing",
  33         "Alternating 0x55/0xAA",
  34         "All Zeros 0x00",
  35         "All Ones 0xFF",
  36         "Pseudo-random Data",
  37         "User Specified",
  38         "Complex pattern",
  39         "Color box",
  40         "Color bars",
  41         NULL
  42 };
  43
  44 u32 csid_find_code(u32 *codes, unsigned int ncodes,
  45                    unsigned int match_format_idx, u32 match_code)
  46 {
  47         int i;
  48
  49         if (!match_code && (match_format_idx >= ncodes))
  50                 return 0;
  51
  52         for (i = 0; i < ncodes; i++)
  53                 if (match_code) {
  54                         if (codes[i] == match_code)
  55                                 return match_code;
  56                 } else {
  57                         if (i == match_format_idx)
  58                                 return codes[i];
  59                 }
  60
  61         return codes[0];
  62 }
  63
  64 const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
  65                                              unsigned int nformats,
  66                                              u32 code)
  67 {
  68         unsigned int i;
  69
  70         for (i = 0; i < nformats; i++)
  71                 if (code == formats[i].code)
  72                         return &formats[i];
  73
  74         WARN(1, "Unknown format\n");
  75
  76         return &formats[0];
  77 }
  78
  79 /*
  80  * csid_set_clock_rates - Calculate and set clock rates on CSID module
  81  * @csiphy: CSID device
  82  */
  83 static int csid_set_clock_rates(struct csid_device *csid)
  84 {
  85         struct device *dev = csid->camss->dev;
  86         const struct csid_format *fmt;
  87         s64 link_freq;
  88         int i, j;
  89         int ret;
  90
  91         fmt = csid_get_fmt_entry(csid->formats, csid->nformats,
  92                                  csid->fmt[MSM_CSIPHY_PAD_SINK].code);
  93         link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,
  94                                         csid->phy.lane_cnt);
  95         if (link_freq < 0)
  96                 link_freq = 0;
  97
  98         for (i = 0; i < csid->nclocks; i++) {
  99                 struct camss_clock *clock = &csid->clock[i];
 100
 101                 if (!strcmp(clock->name, "csi0") ||
 102                     !strcmp(clock->name, "csi1") ||
 103                     !strcmp(clock->name, "csi2") ||
 104                     !strcmp(clock->name, "csi3")) {
 105                         u64 min_rate = link_freq / 4;
 106                         long rate;
 107
 108                         camss_add_clock_margin(&min_rate);
 109
 110                         for (j = 0; j < clock->nfreqs; j++)
 111                                 if (min_rate < clock->freq[j])
 112                                         break;
 113
 114                         if (j == clock->nfreqs) {
 115                                 dev_err(dev,
 116                                         "Pixel clock is too high for CSID\n");
 117                                 return -EINVAL;
 118                         }
 119
 120                         /* if sensor pixel clock is not available */
 121                         /* set highest possible CSID clock rate */
 122                         if (min_rate == 0)
 123                                 j = clock->nfreqs - 1;
 124
 125                         rate = clk_round_rate(clock->clk, clock->freq[j]);
 126                         if (rate < 0) {
 127                                 dev_err(dev, "clk round rate failed: %ld\n",
 128                                         rate);
 129                                 return -EINVAL;
 130                         }
 131
 132                         ret = clk_set_rate(clock->clk, rate);
 133                         if (ret < 0) {
 134                                 dev_err(dev, "clk set rate failed: %d\n", ret);
 135                                 return ret;
 136                         }
 137                 } else if (clock->nfreqs) {
 138                         clk_set_rate(clock->clk, clock->freq[0]);
 139                 }
 140         }
 141
 142         return 0;
 143 }
 144
 145 /*
 146  * csid_set_power - Power on/off CSID module
 147  * @sd: CSID V4L2 subdevice
 148  * @on: Requested power state
 149  *
 150  * Return 0 on success or a negative error code otherwise
 151  */
 152 static int csid_set_power(struct v4l2_subdev *sd, int on)
 153 {
 154         struct csid_device *csid = v4l2_get_subdevdata(sd);
 155         struct device *dev = csid->camss->dev;
 156         int ret;
 157
 158         if (on) {
 159                 ret = pm_runtime_resume_and_get(dev);
 160                 if (ret < 0)
 161                         return ret;
 162
 163                 ret = regulator_enable(csid->vdda);
 164                 if (ret < 0) {
 165                         pm_runtime_put_sync(dev);
 166                         return ret;
 167                 }
 168
 169                 ret = csid_set_clock_rates(csid);
 170                 if (ret < 0) {
 171                         regulator_disable(csid->vdda);
 172                         pm_runtime_put_sync(dev);
 173                         return ret;
 174                 }
 175
 176                 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
 177                 if (ret < 0) {
 178                         regulator_disable(csid->vdda);
 179                         pm_runtime_put_sync(dev);
 180                         return ret;
 181                 }
 182
 183                 enable_irq(csid->irq);
 184
 185                 ret = csid->ops->reset(csid);
 186                 if (ret < 0) {
 187                         disable_irq(csid->irq);
 188                         camss_disable_clocks(csid->nclocks, csid->clock);
 189                         regulator_disable(csid->vdda);
 190                         pm_runtime_put_sync(dev);
 191                         return ret;
 192                 }
 193
 194                 csid->ops->hw_version(csid);
 195         } else {
 196                 disable_irq(csid->irq);
 197                 camss_disable_clocks(csid->nclocks, csid->clock);
 198                 ret = regulator_disable(csid->vdda);
 199                 pm_runtime_put_sync(dev);
 200         }
 201
 202         return ret;
 203 }
 204
 205 /*
 206  * csid_set_stream - Enable/disable streaming on CSID module
 207  * @sd: CSID V4L2 subdevice
 208  * @enable: Requested streaming state
 209  *
 210  * Main configuration of CSID module is also done here.
 211  *
 212  * Return 0 on success or a negative error code otherwise
 213  */
 214 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 215 {
 216         struct csid_device *csid = v4l2_get_subdevdata(sd);
 217         int ret;
 218
 219         if (enable) {
 220                 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
 221                 if (ret < 0) {
 222                         dev_err(csid->camss->dev,
 223                                 "could not sync v4l2 controls: %d\n", ret);
 224                         return ret;
 225                 }
 226
 227                 if (!csid->testgen.enabled &&
 228                     !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
 229                         return -ENOLINK;
 230         }
 231
 232         csid->ops->configure_stream(csid, enable);
 233
 234         return 0;
 235 }
 236
 237 /*
 238  * __csid_get_format - Get pointer to format structure
 239  * @csid: CSID device
 240  * @cfg: V4L2 subdev pad configuration
 241  * @pad: pad from which format is requested
 242  * @which: TRY or ACTIVE format
 243  *
 244  * Return pointer to TRY or ACTIVE format structure
 245  */
 246 static struct v4l2_mbus_framefmt *
 247 __csid_get_format(struct csid_device *csid,
 248                   struct v4l2_subdev_state *sd_state,
 249                   unsigned int pad,
 250                   enum v4l2_subdev_format_whence which)
 251 {
 252         if (which == V4L2_SUBDEV_FORMAT_TRY)
 253                 return v4l2_subdev_get_try_format(&csid->subdev, sd_state,
 254                                                   pad);
 255
 256         return &csid->fmt[pad];
 257 }
 258
 259 /*
 260  * csid_try_format - Handle try format by pad subdev method
 261  * @csid: CSID device
 262  * @cfg: V4L2 subdev pad configuration
 263  * @pad: pad on which format is requested
 264  * @fmt: pointer to v4l2 format structure
 265  * @which: wanted subdev format
 266  */
 267 static void csid_try_format(struct csid_device *csid,
 268                             struct v4l2_subdev_state *sd_state,
 269                             unsigned int pad,
 270                             struct v4l2_mbus_framefmt *fmt,
 271                             enum v4l2_subdev_format_whence which)
 272 {
 273         unsigned int i;
 274
 275         switch (pad) {
 276         case MSM_CSID_PAD_SINK:
 277                 /* Set format on sink pad */
 278
 279                 for (i = 0; i < csid->nformats; i++)
 280                         if (fmt->code == csid->formats[i].code)
 281                                 break;
 282
 283                 /* If not found, use UYVY as default */
 284                 if (i >= csid->nformats)
 285                         fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 286
 287                 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 288                 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 289
 290                 fmt->field = V4L2_FIELD_NONE;
 291                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
 292
 293                 break;
 294
 295         case MSM_CSID_PAD_SRC:
 296                 if (csid->testgen_mode->cur.val == 0) {
 297                         /* Test generator is disabled, */
 298                         /* keep pad formats in sync */
 299                         u32 code = fmt->code;
 300
 301                         *fmt = *__csid_get_format(csid, sd_state,
 302                                                       MSM_CSID_PAD_SINK, which);
 303                         fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
 304                 } else {
 305                         /* Test generator is enabled, set format on source */
 306                         /* pad to allow test generator usage */
 307
 308                         for (i = 0; i < csid->nformats; i++)
 309                                 if (csid->formats[i].code == fmt->code)
 310                                         break;
 311
 312                         /* If not found, use UYVY as default */
 313                         if (i >= csid->nformats)
 314                                 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 315
 316                         fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 317                         fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 318
 319                         fmt->field = V4L2_FIELD_NONE;
 320                 }
 321                 break;
 322         }
 323
 324         fmt->colorspace = V4L2_COLORSPACE_SRGB;
 325 }
 326
 327 /*
 328  * csid_enum_mbus_code - Handle pixel format enumeration
 329  * @sd: CSID V4L2 subdevice
 330  * @cfg: V4L2 subdev pad configuration
 331  * @code: pointer to v4l2_subdev_mbus_code_enum structure
 332  * return -EINVAL or zero on success
 333  */
 334 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
 335                                struct v4l2_subdev_state *sd_state,
 336                                struct v4l2_subdev_mbus_code_enum *code)
 337 {
 338         struct csid_device *csid = v4l2_get_subdevdata(sd);
 339
 340         if (code->pad == MSM_CSID_PAD_SINK) {
 341                 if (code->index >= csid->nformats)
 342                         return -EINVAL;
 343
 344                 code->code = csid->formats[code->index].code;
 345         } else {
 346                 if (csid->testgen_mode->cur.val == 0) {
 347                         struct v4l2_mbus_framefmt *sink_fmt;
 348
 349                         sink_fmt = __csid_get_format(csid, sd_state,
 350                                                      MSM_CSID_PAD_SINK,
 351                                                      code->which);
 352
 353                         code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
 354                                                        code->index, 0);
 355                         if (!code->code)
 356                                 return -EINVAL;
 357                 } else {
 358                         if (code->index >= csid->nformats)
 359                                 return -EINVAL;
 360
 361                         code->code = csid->formats[code->index].code;
 362                 }
 363         }
 364
 365         return 0;
 366 }
 367
 368 /*
 369  * csid_enum_frame_size - Handle frame size enumeration
 370  * @sd: CSID V4L2 subdevice
 371  * @cfg: V4L2 subdev pad configuration
 372  * @fse: pointer to v4l2_subdev_frame_size_enum structure
 373  * return -EINVAL or zero on success
 374  */
 375 static int csid_enum_frame_size(struct v4l2_subdev *sd,
 376                                 struct v4l2_subdev_state *sd_state,
 377                                 struct v4l2_subdev_frame_size_enum *fse)
 378 {
 379         struct csid_device *csid = v4l2_get_subdevdata(sd);
 380         struct v4l2_mbus_framefmt format;
 381
 382         if (fse->index != 0)
 383                 return -EINVAL;
 384
 385         format.code = fse->code;
 386         format.width = 1;
 387         format.height = 1;
 388         csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
 389         fse->min_width = format.width;
 390         fse->min_height = format.height;
 391
 392         if (format.code != fse->code)
 393                 return -EINVAL;
 394
 395         format.code = fse->code;
 396         format.width = -1;
 397         format.height = -1;
 398         csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
 399         fse->max_width = format.width;
 400         fse->max_height = format.height;
 401
 402         return 0;
 403 }
 404
 405 /*
 406  * csid_get_format - Handle get format by pads subdev method
 407  * @sd: CSID V4L2 subdevice
 408  * @cfg: V4L2 subdev pad configuration
 409  * @fmt: pointer to v4l2 subdev format structure
 410  *
 411  * Return -EINVAL or zero on success
 412  */
 413 static int csid_get_format(struct v4l2_subdev *sd,
 414                            struct v4l2_subdev_state *sd_state,
 415                            struct v4l2_subdev_format *fmt)
 416 {
 417         struct csid_device *csid = v4l2_get_subdevdata(sd);
 418         struct v4l2_mbus_framefmt *format;
 419
 420         format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
 421         if (format == NULL)
 422                 return -EINVAL;
 423
 424         fmt->format = *format;
 425
 426         return 0;
 427 }
 428
 429 /*
 430  * csid_set_format - Handle set format by pads subdev method
 431  * @sd: CSID V4L2 subdevice
 432  * @cfg: V4L2 subdev pad configuration
 433  * @fmt: pointer to v4l2 subdev format structure
 434  *
 435  * Return -EINVAL or zero on success
 436  */
 437 static int csid_set_format(struct v4l2_subdev *sd,
 438                            struct v4l2_subdev_state *sd_state,
 439                            struct v4l2_subdev_format *fmt)
 440 {
 441         struct csid_device *csid = v4l2_get_subdevdata(sd);
 442         struct v4l2_mbus_framefmt *format;
 443
 444         format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
 445         if (format == NULL)
 446                 return -EINVAL;
 447
 448         csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which);
 449         *format = fmt->format;
 450
 451         /* Propagate the format from sink to source */
 452         if (fmt->pad == MSM_CSID_PAD_SINK) {
 453                 format = __csid_get_format(csid, sd_state, MSM_CSID_PAD_SRC,
 454                                            fmt->which);
 455
 456                 *format = fmt->format;
 457                 csid_try_format(csid, sd_state, MSM_CSID_PAD_SRC, format,
 458                                 fmt->which);
 459         }
 460
 461         return 0;
 462 }
 463
 464 /*
 465  * csid_init_formats - Initialize formats on all pads
 466  * @sd: CSID V4L2 subdevice
 467  * @fh: V4L2 subdev file handle
 468  *
 469  * Initialize all pad formats with default values.
 470  *
 471  * Return 0 on success or a negative error code otherwise
 472  */
 473 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 474 {
 475         struct v4l2_subdev_format format = {
 476                 .pad = MSM_CSID_PAD_SINK,
 477                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
 478                               V4L2_SUBDEV_FORMAT_ACTIVE,
 479                 .format = {
 480                         .code = MEDIA_BUS_FMT_UYVY8_2X8,
 481                         .width = 1920,
 482                         .height = 1080
 483                 }
 484         };
 485
 486         return csid_set_format(sd, fh ? fh->state : NULL, &format);
 487 }
 488
 489 /*
 490  * csid_set_test_pattern - Set test generator's pattern mode
 491  * @csid: CSID device
 492  * @value: desired test pattern mode
 493  *
 494  * Return 0 on success or a negative error code otherwise
 495  */
 496 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
 497 {
 498         struct csid_testgen_config *tg = &csid->testgen;
 499
 500         /* If CSID is linked to CSIPHY, do not allow to enable test generator */
 501         if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
 502                 return -EBUSY;
 503
 504         tg->enabled = !!value;
 505
 506         return csid->ops->configure_testgen_pattern(csid, value);
 507 }
 508
 509 /*
 510  * csid_s_ctrl - Handle set control subdev method
 511  * @ctrl: pointer to v4l2 control structure
 512  *
 513  * Return 0 on success or a negative error code otherwise
 514  */
 515 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
 516 {
 517         struct csid_device *csid = container_of(ctrl->handler,
 518                                                 struct csid_device, ctrls);
 519         int ret = -EINVAL;
 520
 521         switch (ctrl->id) {
 522         case V4L2_CID_TEST_PATTERN:
 523                 ret = csid_set_test_pattern(csid, ctrl->val);
 524                 break;
 525         }
 526
 527         return ret;
 528 }
 529
 530 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
 531         .s_ctrl = csid_s_ctrl,
 532 };
 533
 534 /*
 535  * msm_csid_subdev_init - Initialize CSID device structure and resources
 536  * @csid: CSID device
 537  * @res: CSID module resources table
 538  * @id: CSID module id
 539  *
 540  * Return 0 on success or a negative error code otherwise
 541  */
 542 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 543                          const struct resources *res, u8 id)
 544 {
 545         struct device *dev = camss->dev;
 546         struct platform_device *pdev = to_platform_device(dev);
 547         struct resource *r;
 548         int i, j;
 549         int ret;
 550
 551         csid->camss = camss;
 552         csid->id = id;
 553
 554         if (camss->version == CAMSS_8x16) {
 555                 csid->ops = &csid_ops_4_1;
 556         } else if (camss->version == CAMSS_8x96 ||
 557                    camss->version == CAMSS_660) {
 558                 csid->ops = &csid_ops_4_7;
 559         } else if (camss->version == CAMSS_845) {
 560                 csid->ops = &csid_ops_170;
 561         } else {
 562                 return -EINVAL;
 563         }
 564         csid->ops->subdev_init(csid);
 565
 566         /* Memory */
 567
 568         csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
 569         if (IS_ERR(csid->base))
 570                 return PTR_ERR(csid->base);
 571
 572         /* Interrupt */
 573
 574         r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
 575                                          res->interrupt[0]);
 576         if (!r) {
 577                 dev_err(dev, "missing IRQ\n");
 578                 return -EINVAL;
 579         }
 580
 581         csid->irq = r->start;
 582         snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
 583                  dev_name(dev), MSM_CSID_NAME, csid->id);
 584         ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
 585                                IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
 586                                csid->irq_name, csid);
 587         if (ret < 0) {
 588                 dev_err(dev, "request_irq failed: %d\n", ret);
 589                 return ret;
 590         }
 591
 592         /* Clocks */
 593
 594         csid->nclocks = 0;
 595         while (res->clock[csid->nclocks])
 596                 csid->nclocks++;
 597
 598         csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),
 599                                     GFP_KERNEL);
 600         if (!csid->clock)
 601                 return -ENOMEM;
 602
 603         for (i = 0; i < csid->nclocks; i++) {
 604                 struct camss_clock *clock = &csid->clock[i];
 605
 606                 clock->clk = devm_clk_get(dev, res->clock[i]);
 607                 if (IS_ERR(clock->clk))
 608                         return PTR_ERR(clock->clk);
 609
 610                 clock->name = res->clock[i];
 611
 612                 clock->nfreqs = 0;
 613                 while (res->clock_rate[i][clock->nfreqs])
 614                         clock->nfreqs++;
 615
 616                 if (!clock->nfreqs) {
 617                         clock->freq = NULL;
 618                         continue;
 619                 }
 620
 621                 clock->freq = devm_kcalloc(dev,
 622                                            clock->nfreqs,
 623                                            sizeof(*clock->freq),
 624                                            GFP_KERNEL);
 625                 if (!clock->freq)
 626                         return -ENOMEM;
 627
 628                 for (j = 0; j < clock->nfreqs; j++)
 629                         clock->freq[j] = res->clock_rate[i][j];
 630         }
 631
 632         /* Regulator */
 633
 634         csid->vdda = devm_regulator_get(dev, res->regulator[0]);
 635         if (IS_ERR(csid->vdda)) {
 636                 dev_err(dev, "could not get regulator\n");
 637                 return PTR_ERR(csid->vdda);
 638         }
 639
 640         init_completion(&csid->reset_complete);
 641
 642         return 0;
 643 }
 644
 645 /*
 646  * msm_csid_get_csid_id - Get CSID HW module id
 647  * @entity: Pointer to CSID media entity structure
 648  * @id: Return CSID HW module id here
 649  */
 650 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
 651 {
 652         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 653         struct csid_device *csid = v4l2_get_subdevdata(sd);
 654
 655         *id = csid->id;
 656 }
 657
 658 /*
 659  * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
 660  * @lane_cfg - CSI2 lane configuration
 661  *
 662  * Return lane assign
 663  */
 664 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
 665 {
 666         u32 lane_assign = 0;
 667         int i;
 668
 669         for (i = 0; i < lane_cfg->num_data; i++)
 670                 lane_assign |= lane_cfg->data[i].pos << (i * 4);
 671
 672         return lane_assign;
 673 }
 674
 675 /*
 676  * csid_link_setup - Setup CSID connections
 677  * @entity: Pointer to media entity structure
 678  * @local: Pointer to local pad
 679  * @remote: Pointer to remote pad
 680  * @flags: Link flags
 681  *
 682  * Return 0 on success
 683  */
 684 static int csid_link_setup(struct media_entity *entity,
 685                            const struct media_pad *local,
 686                            const struct media_pad *remote, u32 flags)
 687 {
 688         if (flags & MEDIA_LNK_FL_ENABLED)
 689                 if (media_entity_remote_pad(local))
 690                         return -EBUSY;
 691
 692         if ((local->flags & MEDIA_PAD_FL_SINK) &&
 693             (flags & MEDIA_LNK_FL_ENABLED)) {
 694                 struct v4l2_subdev *sd;
 695                 struct csid_device *csid;
 696                 struct csiphy_device *csiphy;
 697                 struct csiphy_lanes_cfg *lane_cfg;
 698                 struct v4l2_subdev_format format = { 0 };
 699
 700                 sd = media_entity_to_v4l2_subdev(entity);
 701                 csid = v4l2_get_subdevdata(sd);
 702
 703                 /* If test generator is enabled */
 704                 /* do not allow a link from CSIPHY to CSID */
 705                 if (csid->testgen_mode->cur.val != 0)
 706                         return -EBUSY;
 707
 708                 sd = media_entity_to_v4l2_subdev(remote->entity);
 709                 csiphy = v4l2_get_subdevdata(sd);
 710
 711                 /* If a sensor is not linked to CSIPHY */
 712                 /* do no allow a link from CSIPHY to CSID */
 713                 if (!csiphy->cfg.csi2)
 714                         return -EPERM;
 715
 716                 csid->phy.csiphy_id = csiphy->id;
 717
 718                 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
 719                 csid->phy.lane_cnt = lane_cfg->num_data;
 720                 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
 721
 722                 /* Reset format on source pad to sink pad format */
 723                 format.pad = MSM_CSID_PAD_SRC;
 724                 format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 725                 csid_set_format(&csid->subdev, NULL, &format);
 726         }
 727
 728         return 0;
 729 }
 730
 731 static const struct v4l2_subdev_core_ops csid_core_ops = {
 732         .s_power = csid_set_power,
 733         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 734         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
 735 };
 736
 737 static const struct v4l2_subdev_video_ops csid_video_ops = {
 738         .s_stream = csid_set_stream,
 739 };
 740
 741 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
 742         .enum_mbus_code = csid_enum_mbus_code,
 743         .enum_frame_size = csid_enum_frame_size,
 744         .get_fmt = csid_get_format,
 745         .set_fmt = csid_set_format,
 746 };
 747
 748 static const struct v4l2_subdev_ops csid_v4l2_ops = {
 749         .core = &csid_core_ops,
 750         .video = &csid_video_ops,
 751         .pad = &csid_pad_ops,
 752 };
 753
 754 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
 755         .open = csid_init_formats,
 756 };
 757
 758 static const struct media_entity_operations csid_media_ops = {
 759         .link_setup = csid_link_setup,
 760         .link_validate = v4l2_subdev_link_validate,
 761 };
 762
 763 /*
 764  * msm_csid_register_entity - Register subdev node for CSID module
 765  * @csid: CSID device
 766  * @v4l2_dev: V4L2 device
 767  *
 768  * Return 0 on success or a negative error code otherwise
 769  */
 770 int msm_csid_register_entity(struct csid_device *csid,
 771                              struct v4l2_device *v4l2_dev)
 772 {
 773         struct v4l2_subdev *sd = &csid->subdev;
 774         struct media_pad *pads = csid->pads;
 775         struct device *dev = csid->camss->dev;
 776         int ret;
 777
 778         v4l2_subdev_init(sd, &csid_v4l2_ops);
 779         sd->internal_ops = &csid_v4l2_internal_ops;
 780         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
 781                      V4L2_SUBDEV_FL_HAS_EVENTS;
 782         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
 783                  MSM_CSID_NAME, csid->id);
 784         v4l2_set_subdevdata(sd, csid);
 785
 786         ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
 787         if (ret < 0) {
 788                 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
 789                 return ret;
 790         }
 791
 792         csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
 793                                 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
 794                                 csid->testgen.nmodes, 0, 0,
 795                                 csid->testgen.modes);
 796
 797         if (csid->ctrls.error) {
 798                 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
 799                 ret = csid->ctrls.error;
 800                 goto free_ctrl;
 801         }
 802
 803         csid->subdev.ctrl_handler = &csid->ctrls;
 804
 805         ret = csid_init_formats(sd, NULL);
 806         if (ret < 0) {
 807                 dev_err(dev, "Failed to init format: %d\n", ret);
 808                 goto free_ctrl;
 809         }
 810
 811         pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 812         pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
 813
 814         sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
 815         sd->entity.ops = &csid_media_ops;
 816         ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
 817         if (ret < 0) {
 818                 dev_err(dev, "Failed to init media entity: %d\n", ret);
 819                 goto free_ctrl;
 820         }
 821
 822         ret = v4l2_device_register_subdev(v4l2_dev, sd);
 823         if (ret < 0) {
 824                 dev_err(dev, "Failed to register subdev: %d\n", ret);
 825                 goto media_cleanup;
 826         }
 827
 828         return 0;
 829
 830 media_cleanup:
 831         media_entity_cleanup(&sd->entity);
 832 free_ctrl:
 833         v4l2_ctrl_handler_free(&csid->ctrls);
 834
 835         return ret;
 836 }
 837
 838 /*
 839  * msm_csid_unregister_entity - Unregister CSID module subdev node
 840  * @csid: CSID device
 841  */
 842 void msm_csid_unregister_entity(struct csid_device *csid)
 843 {
 844         v4l2_device_unregister_subdev(&csid->subdev);
 845         media_entity_cleanup(&csid->subdev.entity);
 846         v4l2_ctrl_handler_free(&csid->ctrls);
 847 }