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