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