git.maquefel.me Git - linux.git/blob

   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 = csid->vdda ? regulator_enable(csid->vdda) : 0;
 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                         if (csid->vdda)
 172                                 regulator_disable(csid->vdda);
 173                         pm_runtime_put_sync(dev);
 174                         return ret;
 175                 }
 176
 177                 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);
 178                 if (ret < 0) {
 179                         if (csid->vdda)
 180                                 regulator_disable(csid->vdda);
 181                         pm_runtime_put_sync(dev);
 182                         return ret;
 183                 }
 184
 185                 enable_irq(csid->irq);
 186
 187                 ret = csid->ops->reset(csid);
 188                 if (ret < 0) {
 189                         disable_irq(csid->irq);
 190                         camss_disable_clocks(csid->nclocks, csid->clock);
 191                         if (csid->vdda)
 192                                 regulator_disable(csid->vdda);
 193                         pm_runtime_put_sync(dev);
 194                         return ret;
 195                 }
 196
 197                 csid->ops->hw_version(csid);
 198         } else {
 199                 disable_irq(csid->irq);
 200                 camss_disable_clocks(csid->nclocks, csid->clock);
 201                 ret = csid->vdda ? regulator_disable(csid->vdda) : 0;
 202                 pm_runtime_put_sync(dev);
 203         }
 204
 205         return ret;
 206 }
 207
 208 /*
 209  * csid_set_stream - Enable/disable streaming on CSID module
 210  * @sd: CSID V4L2 subdevice
 211  * @enable: Requested streaming state
 212  *
 213  * Main configuration of CSID module is also done here.
 214  *
 215  * Return 0 on success or a negative error code otherwise
 216  */
 217 static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 218 {
 219         struct csid_device *csid = v4l2_get_subdevdata(sd);
 220         int ret;
 221
 222         if (enable) {
 223                 ret = v4l2_ctrl_handler_setup(&csid->ctrls);
 224                 if (ret < 0) {
 225                         dev_err(csid->camss->dev,
 226                                 "could not sync v4l2 controls: %d\n", ret);
 227                         return ret;
 228                 }
 229
 230                 if (!csid->testgen.enabled &&
 231                     !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
 232                         return -ENOLINK;
 233         }
 234
 235         csid->ops->configure_stream(csid, enable);
 236
 237         return 0;
 238 }
 239
 240 /*
 241  * __csid_get_format - Get pointer to format structure
 242  * @csid: CSID device
 243  * @cfg: V4L2 subdev pad configuration
 244  * @pad: pad from which format is requested
 245  * @which: TRY or ACTIVE format
 246  *
 247  * Return pointer to TRY or ACTIVE format structure
 248  */
 249 static struct v4l2_mbus_framefmt *
 250 __csid_get_format(struct csid_device *csid,
 251                   struct v4l2_subdev_state *sd_state,
 252                   unsigned int pad,
 253                   enum v4l2_subdev_format_whence which)
 254 {
 255         if (which == V4L2_SUBDEV_FORMAT_TRY)
 256                 return v4l2_subdev_get_try_format(&csid->subdev, sd_state,
 257                                                   pad);
 258
 259         return &csid->fmt[pad];
 260 }
 261
 262 /*
 263  * csid_try_format - Handle try format by pad subdev method
 264  * @csid: CSID device
 265  * @cfg: V4L2 subdev pad configuration
 266  * @pad: pad on which format is requested
 267  * @fmt: pointer to v4l2 format structure
 268  * @which: wanted subdev format
 269  */
 270 static void csid_try_format(struct csid_device *csid,
 271                             struct v4l2_subdev_state *sd_state,
 272                             unsigned int pad,
 273                             struct v4l2_mbus_framefmt *fmt,
 274                             enum v4l2_subdev_format_whence which)
 275 {
 276         unsigned int i;
 277
 278         switch (pad) {
 279         case MSM_CSID_PAD_SINK:
 280                 /* Set format on sink pad */
 281
 282                 for (i = 0; i < csid->nformats; i++)
 283                         if (fmt->code == csid->formats[i].code)
 284                                 break;
 285
 286                 /* If not found, use UYVY as default */
 287                 if (i >= csid->nformats)
 288                         fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 289
 290                 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 291                 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 292
 293                 fmt->field = V4L2_FIELD_NONE;
 294                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
 295
 296                 break;
 297
 298         case MSM_CSID_PAD_SRC:
 299                 if (csid->testgen_mode->cur.val == 0) {
 300                         /* Test generator is disabled, */
 301                         /* keep pad formats in sync */
 302                         u32 code = fmt->code;
 303
 304                         *fmt = *__csid_get_format(csid, sd_state,
 305                                                       MSM_CSID_PAD_SINK, which);
 306                         fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
 307                 } else {
 308                         /* Test generator is enabled, set format on source */
 309                         /* pad to allow test generator usage */
 310
 311                         for (i = 0; i < csid->nformats; i++)
 312                                 if (csid->formats[i].code == fmt->code)
 313                                         break;
 314
 315                         /* If not found, use UYVY as default */
 316                         if (i >= csid->nformats)
 317                                 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 318
 319                         fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 320                         fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 321
 322                         fmt->field = V4L2_FIELD_NONE;
 323                 }
 324                 break;
 325         }
 326
 327         fmt->colorspace = V4L2_COLORSPACE_SRGB;
 328 }
 329
 330 /*
 331  * csid_enum_mbus_code - Handle pixel format enumeration
 332  * @sd: CSID V4L2 subdevice
 333  * @cfg: V4L2 subdev pad configuration
 334  * @code: pointer to v4l2_subdev_mbus_code_enum structure
 335  * return -EINVAL or zero on success
 336  */
 337 static int csid_enum_mbus_code(struct v4l2_subdev *sd,
 338                                struct v4l2_subdev_state *sd_state,
 339                                struct v4l2_subdev_mbus_code_enum *code)
 340 {
 341         struct csid_device *csid = v4l2_get_subdevdata(sd);
 342
 343         if (code->pad == MSM_CSID_PAD_SINK) {
 344                 if (code->index >= csid->nformats)
 345                         return -EINVAL;
 346
 347                 code->code = csid->formats[code->index].code;
 348         } else {
 349                 if (csid->testgen_mode->cur.val == 0) {
 350                         struct v4l2_mbus_framefmt *sink_fmt;
 351
 352                         sink_fmt = __csid_get_format(csid, sd_state,
 353                                                      MSM_CSID_PAD_SINK,
 354                                                      code->which);
 355
 356                         code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
 357                                                        code->index, 0);
 358                         if (!code->code)
 359                                 return -EINVAL;
 360                 } else {
 361                         if (code->index >= csid->nformats)
 362                                 return -EINVAL;
 363
 364                         code->code = csid->formats[code->index].code;
 365                 }
 366         }
 367
 368         return 0;
 369 }
 370
 371 /*
 372  * csid_enum_frame_size - Handle frame size enumeration
 373  * @sd: CSID V4L2 subdevice
 374  * @cfg: V4L2 subdev pad configuration
 375  * @fse: pointer to v4l2_subdev_frame_size_enum structure
 376  * return -EINVAL or zero on success
 377  */
 378 static int csid_enum_frame_size(struct v4l2_subdev *sd,
 379                                 struct v4l2_subdev_state *sd_state,
 380                                 struct v4l2_subdev_frame_size_enum *fse)
 381 {
 382         struct csid_device *csid = v4l2_get_subdevdata(sd);
 383         struct v4l2_mbus_framefmt format;
 384
 385         if (fse->index != 0)
 386                 return -EINVAL;
 387
 388         format.code = fse->code;
 389         format.width = 1;
 390         format.height = 1;
 391         csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
 392         fse->min_width = format.width;
 393         fse->min_height = format.height;
 394
 395         if (format.code != fse->code)
 396                 return -EINVAL;
 397
 398         format.code = fse->code;
 399         format.width = -1;
 400         format.height = -1;
 401         csid_try_format(csid, sd_state, fse->pad, &format, fse->which);
 402         fse->max_width = format.width;
 403         fse->max_height = format.height;
 404
 405         return 0;
 406 }
 407
 408 /*
 409  * csid_get_format - Handle get format by pads subdev method
 410  * @sd: CSID V4L2 subdevice
 411  * @cfg: V4L2 subdev pad configuration
 412  * @fmt: pointer to v4l2 subdev format structure
 413  *
 414  * Return -EINVAL or zero on success
 415  */
 416 static int csid_get_format(struct v4l2_subdev *sd,
 417                            struct v4l2_subdev_state *sd_state,
 418                            struct v4l2_subdev_format *fmt)
 419 {
 420         struct csid_device *csid = v4l2_get_subdevdata(sd);
 421         struct v4l2_mbus_framefmt *format;
 422
 423         format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
 424         if (format == NULL)
 425                 return -EINVAL;
 426
 427         fmt->format = *format;
 428
 429         return 0;
 430 }
 431
 432 /*
 433  * csid_set_format - Handle set format by pads subdev method
 434  * @sd: CSID V4L2 subdevice
 435  * @cfg: V4L2 subdev pad configuration
 436  * @fmt: pointer to v4l2 subdev format structure
 437  *
 438  * Return -EINVAL or zero on success
 439  */
 440 static int csid_set_format(struct v4l2_subdev *sd,
 441                            struct v4l2_subdev_state *sd_state,
 442                            struct v4l2_subdev_format *fmt)
 443 {
 444         struct csid_device *csid = v4l2_get_subdevdata(sd);
 445         struct v4l2_mbus_framefmt *format;
 446
 447         format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);
 448         if (format == NULL)
 449                 return -EINVAL;
 450
 451         csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which);
 452         *format = fmt->format;
 453
 454         /* Propagate the format from sink to source */
 455         if (fmt->pad == MSM_CSID_PAD_SINK) {
 456                 format = __csid_get_format(csid, sd_state, MSM_CSID_PAD_SRC,
 457                                            fmt->which);
 458
 459                 *format = fmt->format;
 460                 csid_try_format(csid, sd_state, MSM_CSID_PAD_SRC, format,
 461                                 fmt->which);
 462         }
 463
 464         return 0;
 465 }
 466
 467 /*
 468  * csid_init_formats - Initialize formats on all pads
 469  * @sd: CSID V4L2 subdevice
 470  * @fh: V4L2 subdev file handle
 471  *
 472  * Initialize all pad formats with default values.
 473  *
 474  * Return 0 on success or a negative error code otherwise
 475  */
 476 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 477 {
 478         struct v4l2_subdev_format format = {
 479                 .pad = MSM_CSID_PAD_SINK,
 480                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
 481                               V4L2_SUBDEV_FORMAT_ACTIVE,
 482                 .format = {
 483                         .code = MEDIA_BUS_FMT_UYVY8_2X8,
 484                         .width = 1920,
 485                         .height = 1080
 486                 }
 487         };
 488
 489         return csid_set_format(sd, fh ? fh->state : NULL, &format);
 490 }
 491
 492 /*
 493  * csid_set_test_pattern - Set test generator's pattern mode
 494  * @csid: CSID device
 495  * @value: desired test pattern mode
 496  *
 497  * Return 0 on success or a negative error code otherwise
 498  */
 499 static int csid_set_test_pattern(struct csid_device *csid, s32 value)
 500 {
 501         struct csid_testgen_config *tg = &csid->testgen;
 502
 503         /* If CSID is linked to CSIPHY, do not allow to enable test generator */
 504         if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))
 505                 return -EBUSY;
 506
 507         tg->enabled = !!value;
 508
 509         return csid->ops->configure_testgen_pattern(csid, value);
 510 }
 511
 512 /*
 513  * csid_s_ctrl - Handle set control subdev method
 514  * @ctrl: pointer to v4l2 control structure
 515  *
 516  * Return 0 on success or a negative error code otherwise
 517  */
 518 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)
 519 {
 520         struct csid_device *csid = container_of(ctrl->handler,
 521                                                 struct csid_device, ctrls);
 522         int ret = -EINVAL;
 523
 524         switch (ctrl->id) {
 525         case V4L2_CID_TEST_PATTERN:
 526                 ret = csid_set_test_pattern(csid, ctrl->val);
 527                 break;
 528         }
 529
 530         return ret;
 531 }
 532
 533 static const struct v4l2_ctrl_ops csid_ctrl_ops = {
 534         .s_ctrl = csid_s_ctrl,
 535 };
 536
 537 /*
 538  * msm_csid_subdev_init - Initialize CSID device structure and resources
 539  * @csid: CSID device
 540  * @res: CSID module resources table
 541  * @id: CSID module id
 542  *
 543  * Return 0 on success or a negative error code otherwise
 544  */
 545 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 546                          const struct resources *res, u8 id)
 547 {
 548         struct device *dev = camss->dev;
 549         struct platform_device *pdev = to_platform_device(dev);
 550         struct resource *r;
 551         int i, j;
 552         int ret;
 553
 554         csid->camss = camss;
 555         csid->id = id;
 556
 557         if (camss->version == CAMSS_8x16) {
 558                 csid->ops = &csid_ops_4_1;
 559         } else if (camss->version == CAMSS_8x96 ||
 560                    camss->version == CAMSS_660) {
 561                 csid->ops = &csid_ops_4_7;
 562         } else if (camss->version == CAMSS_845) {
 563                 csid->ops = &csid_ops_170;
 564         } else {
 565                 return -EINVAL;
 566         }
 567         csid->ops->subdev_init(csid);
 568
 569         /* Memory */
 570
 571         csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
 572         if (IS_ERR(csid->base))
 573                 return PTR_ERR(csid->base);
 574
 575         /* Interrupt */
 576
 577         r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
 578                                          res->interrupt[0]);
 579         if (!r) {
 580                 dev_err(dev, "missing IRQ\n");
 581                 return -EINVAL;
 582         }
 583
 584         csid->irq = r->start;
 585         snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
 586                  dev_name(dev), MSM_CSID_NAME, csid->id);
 587         ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
 588                                IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
 589                                csid->irq_name, csid);
 590         if (ret < 0) {
 591                 dev_err(dev, "request_irq failed: %d\n", ret);
 592                 return ret;
 593         }
 594
 595         /* Clocks */
 596
 597         csid->nclocks = 0;
 598         while (res->clock[csid->nclocks])
 599                 csid->nclocks++;
 600
 601         csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),
 602                                     GFP_KERNEL);
 603         if (!csid->clock)
 604                 return -ENOMEM;
 605
 606         for (i = 0; i < csid->nclocks; i++) {
 607                 struct camss_clock *clock = &csid->clock[i];
 608
 609                 clock->clk = devm_clk_get(dev, res->clock[i]);
 610                 if (IS_ERR(clock->clk))
 611                         return PTR_ERR(clock->clk);
 612
 613                 clock->name = res->clock[i];
 614
 615                 clock->nfreqs = 0;
 616                 while (res->clock_rate[i][clock->nfreqs])
 617                         clock->nfreqs++;
 618
 619                 if (!clock->nfreqs) {
 620                         clock->freq = NULL;
 621                         continue;
 622                 }
 623
 624                 clock->freq = devm_kcalloc(dev,
 625                                            clock->nfreqs,
 626                                            sizeof(*clock->freq),
 627                                            GFP_KERNEL);
 628                 if (!clock->freq)
 629                         return -ENOMEM;
 630
 631                 for (j = 0; j < clock->nfreqs; j++)
 632                         clock->freq[j] = res->clock_rate[i][j];
 633         }
 634
 635         /* Regulator */
 636
 637         csid->vdda = NULL;
 638         if (res->regulator[0])
 639                 csid->vdda = devm_regulator_get(dev, res->regulator[0]);
 640         if (IS_ERR(csid->vdda)) {
 641                 dev_err(dev, "could not get regulator\n");
 642                 return PTR_ERR(csid->vdda);
 643         }
 644
 645         init_completion(&csid->reset_complete);
 646
 647         return 0;
 648 }
 649
 650 /*
 651  * msm_csid_get_csid_id - Get CSID HW module id
 652  * @entity: Pointer to CSID media entity structure
 653  * @id: Return CSID HW module id here
 654  */
 655 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)
 656 {
 657         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 658         struct csid_device *csid = v4l2_get_subdevdata(sd);
 659
 660         *id = csid->id;
 661 }
 662
 663 /*
 664  * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter
 665  * @lane_cfg - CSI2 lane configuration
 666  *
 667  * Return lane assign
 668  */
 669 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)
 670 {
 671         u32 lane_assign = 0;
 672         int i;
 673
 674         for (i = 0; i < lane_cfg->num_data; i++)
 675                 lane_assign |= lane_cfg->data[i].pos << (i * 4);
 676
 677         return lane_assign;
 678 }
 679
 680 /*
 681  * csid_link_setup - Setup CSID connections
 682  * @entity: Pointer to media entity structure
 683  * @local: Pointer to local pad
 684  * @remote: Pointer to remote pad
 685  * @flags: Link flags
 686  *
 687  * Return 0 on success
 688  */
 689 static int csid_link_setup(struct media_entity *entity,
 690                            const struct media_pad *local,
 691                            const struct media_pad *remote, u32 flags)
 692 {
 693         if (flags & MEDIA_LNK_FL_ENABLED)
 694                 if (media_entity_remote_pad(local))
 695                         return -EBUSY;
 696
 697         if ((local->flags & MEDIA_PAD_FL_SINK) &&
 698             (flags & MEDIA_LNK_FL_ENABLED)) {
 699                 struct v4l2_subdev *sd;
 700                 struct csid_device *csid;
 701                 struct csiphy_device *csiphy;
 702                 struct csiphy_lanes_cfg *lane_cfg;
 703                 struct v4l2_subdev_format format = { 0 };
 704
 705                 sd = media_entity_to_v4l2_subdev(entity);
 706                 csid = v4l2_get_subdevdata(sd);
 707
 708                 /* If test generator is enabled */
 709                 /* do not allow a link from CSIPHY to CSID */
 710                 if (csid->testgen_mode->cur.val != 0)
 711                         return -EBUSY;
 712
 713                 sd = media_entity_to_v4l2_subdev(remote->entity);
 714                 csiphy = v4l2_get_subdevdata(sd);
 715
 716                 /* If a sensor is not linked to CSIPHY */
 717                 /* do no allow a link from CSIPHY to CSID */
 718                 if (!csiphy->cfg.csi2)
 719                         return -EPERM;
 720
 721                 csid->phy.csiphy_id = csiphy->id;
 722
 723                 lane_cfg = &csiphy->cfg.csi2->lane_cfg;
 724                 csid->phy.lane_cnt = lane_cfg->num_data;
 725                 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);
 726
 727                 /* Reset format on source pad to sink pad format */
 728                 format.pad = MSM_CSID_PAD_SRC;
 729                 format.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 730                 csid_set_format(&csid->subdev, NULL, &format);
 731         }
 732
 733         return 0;
 734 }
 735
 736 static const struct v4l2_subdev_core_ops csid_core_ops = {
 737         .s_power = csid_set_power,
 738         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 739         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
 740 };
 741
 742 static const struct v4l2_subdev_video_ops csid_video_ops = {
 743         .s_stream = csid_set_stream,
 744 };
 745
 746 static const struct v4l2_subdev_pad_ops csid_pad_ops = {
 747         .enum_mbus_code = csid_enum_mbus_code,
 748         .enum_frame_size = csid_enum_frame_size,
 749         .get_fmt = csid_get_format,
 750         .set_fmt = csid_set_format,
 751 };
 752
 753 static const struct v4l2_subdev_ops csid_v4l2_ops = {
 754         .core = &csid_core_ops,
 755         .video = &csid_video_ops,
 756         .pad = &csid_pad_ops,
 757 };
 758
 759 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {
 760         .open = csid_init_formats,
 761 };
 762
 763 static const struct media_entity_operations csid_media_ops = {
 764         .link_setup = csid_link_setup,
 765         .link_validate = v4l2_subdev_link_validate,
 766 };
 767
 768 /*
 769  * msm_csid_register_entity - Register subdev node for CSID module
 770  * @csid: CSID device
 771  * @v4l2_dev: V4L2 device
 772  *
 773  * Return 0 on success or a negative error code otherwise
 774  */
 775 int msm_csid_register_entity(struct csid_device *csid,
 776                              struct v4l2_device *v4l2_dev)
 777 {
 778         struct v4l2_subdev *sd = &csid->subdev;
 779         struct media_pad *pads = csid->pads;
 780         struct device *dev = csid->camss->dev;
 781         int ret;
 782
 783         v4l2_subdev_init(sd, &csid_v4l2_ops);
 784         sd->internal_ops = &csid_v4l2_internal_ops;
 785         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
 786                      V4L2_SUBDEV_FL_HAS_EVENTS;
 787         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
 788                  MSM_CSID_NAME, csid->id);
 789         v4l2_set_subdevdata(sd, csid);
 790
 791         ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
 792         if (ret < 0) {
 793                 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
 794                 return ret;
 795         }
 796
 797         csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
 798                                 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
 799                                 csid->testgen.nmodes, 0, 0,
 800                                 csid->testgen.modes);
 801
 802         if (csid->ctrls.error) {
 803                 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
 804                 ret = csid->ctrls.error;
 805                 goto free_ctrl;
 806         }
 807
 808         csid->subdev.ctrl_handler = &csid->ctrls;
 809
 810         ret = csid_init_formats(sd, NULL);
 811         if (ret < 0) {
 812                 dev_err(dev, "Failed to init format: %d\n", ret);
 813                 goto free_ctrl;
 814         }
 815
 816         pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 817         pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
 818
 819         sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
 820         sd->entity.ops = &csid_media_ops;
 821         ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);
 822         if (ret < 0) {
 823                 dev_err(dev, "Failed to init media entity: %d\n", ret);
 824                 goto free_ctrl;
 825         }
 826
 827         ret = v4l2_device_register_subdev(v4l2_dev, sd);
 828         if (ret < 0) {
 829                 dev_err(dev, "Failed to register subdev: %d\n", ret);
 830                 goto media_cleanup;
 831         }
 832
 833         return 0;
 834
 835 media_cleanup:
 836         media_entity_cleanup(&sd->entity);
 837 free_ctrl:
 838         v4l2_ctrl_handler_free(&csid->ctrls);
 839
 840         return ret;
 841 }
 842
 843 /*
 844  * msm_csid_unregister_entity - Unregister CSID module subdev node
 845  * @csid: CSID device
 846  */
 847 void msm_csid_unregister_entity(struct csid_device *csid)
 848 {
 849         v4l2_device_unregister_subdev(&csid->subdev);
 850         media_entity_cleanup(&csid->subdev.entity);
 851         v4l2_ctrl_handler_free(&csid->ctrls);
 852 }