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