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