linux/drivers/media/platform/qcom/camss/camss-csiphy.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * camss-csiphy.c
   4 *
   5 * Qualcomm MSM Camera Subsystem - CSIPHY Module
   6 *
   7 * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
   8 * Copyright (C) 2016-2018 Linaro Ltd.
   9 */
  10#include <linux/clk.h>
  11#include <linux/delay.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 <media/media-entity.h>
  19#include <media/v4l2-device.h>
  20#include <media/v4l2-subdev.h>
  21
  22#include "camss-csiphy.h"
  23#include "camss.h"
  24
  25#define MSM_CSIPHY_NAME "msm_csiphy"
  26
  27struct csiphy_format {
  28        u32 code;
  29        u8 bpp;
  30};
  31
  32static const struct csiphy_format csiphy_formats_8x16[] = {
  33        { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
  34        { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
  35        { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
  36        { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
  37        { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
  38        { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
  39        { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
  40        { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
  41        { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
  42        { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
  43        { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
  44        { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
  45        { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
  46        { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
  47        { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
  48        { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
  49        { MEDIA_BUS_FMT_Y10_1X10, 10 },
  50};
  51
  52static const struct csiphy_format csiphy_formats_8x96[] = {
  53        { MEDIA_BUS_FMT_UYVY8_2X8, 8 },
  54        { MEDIA_BUS_FMT_VYUY8_2X8, 8 },
  55        { MEDIA_BUS_FMT_YUYV8_2X8, 8 },
  56        { MEDIA_BUS_FMT_YVYU8_2X8, 8 },
  57        { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
  58        { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
  59        { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
  60        { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
  61        { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
  62        { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
  63        { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
  64        { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
  65        { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
  66        { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
  67        { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
  68        { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
  69        { MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
  70        { MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
  71        { MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
  72        { MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
  73        { MEDIA_BUS_FMT_Y10_1X10, 10 },
  74};
  75
  76/*
  77 * csiphy_get_bpp - map media bus format to bits per pixel
  78 * @formats: supported media bus formats array
  79 * @nformats: size of @formats array
  80 * @code: media bus format code
  81 *
  82 * Return number of bits per pixel
  83 */
  84static u8 csiphy_get_bpp(const struct csiphy_format *formats,
  85                         unsigned int nformats, u32 code)
  86{
  87        unsigned int i;
  88
  89        for (i = 0; i < nformats; i++)
  90                if (code == formats[i].code)
  91                        return formats[i].bpp;
  92
  93        WARN(1, "Unknown format\n");
  94
  95        return formats[0].bpp;
  96}
  97
  98/*
  99 * csiphy_set_clock_rates - Calculate and set clock rates on CSIPHY module
 100 * @csiphy: CSIPHY device
 101 */
 102static int csiphy_set_clock_rates(struct csiphy_device *csiphy)
 103{
 104        struct device *dev = csiphy->camss->dev;
 105        u32 pixel_clock;
 106        int i, j;
 107        int ret;
 108
 109        ret = camss_get_pixel_clock(&csiphy->subdev.entity, &pixel_clock);
 110        if (ret)
 111                pixel_clock = 0;
 112
 113        for (i = 0; i < csiphy->nclocks; i++) {
 114                struct camss_clock *clock = &csiphy->clock[i];
 115
 116                if (!strcmp(clock->name, "csiphy0_timer") ||
 117                    !strcmp(clock->name, "csiphy1_timer") ||
 118                    !strcmp(clock->name, "csiphy2_timer")) {
 119                        u8 bpp = csiphy_get_bpp(csiphy->formats,
 120                                        csiphy->nformats,
 121                                        csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
 122                        u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
 123                        u64 min_rate = pixel_clock * bpp / (2 * num_lanes * 4);
 124                        long round_rate;
 125
 126                        camss_add_clock_margin(&min_rate);
 127
 128                        for (j = 0; j < clock->nfreqs; j++)
 129                                if (min_rate < clock->freq[j])
 130                                        break;
 131
 132                        if (j == clock->nfreqs) {
 133                                dev_err(dev,
 134                                        "Pixel clock is too high for CSIPHY\n");
 135                                return -EINVAL;
 136                        }
 137
 138                        /* if sensor pixel clock is not available */
 139                        /* set highest possible CSIPHY clock rate */
 140                        if (min_rate == 0)
 141                                j = clock->nfreqs - 1;
 142
 143                        round_rate = clk_round_rate(clock->clk, clock->freq[j]);
 144                        if (round_rate < 0) {
 145                                dev_err(dev, "clk round rate failed: %ld\n",
 146                                        round_rate);
 147                                return -EINVAL;
 148                        }
 149
 150                        csiphy->timer_clk_rate = round_rate;
 151
 152                        ret = clk_set_rate(clock->clk, csiphy->timer_clk_rate);
 153                        if (ret < 0) {
 154                                dev_err(dev, "clk set rate failed: %d\n", ret);
 155                                return ret;
 156                        }
 157                }
 158        }
 159
 160        return 0;
 161}
 162
 163/*
 164 * csiphy_set_power - Power on/off CSIPHY module
 165 * @sd: CSIPHY V4L2 subdevice
 166 * @on: Requested power state
 167 *
 168 * Return 0 on success or a negative error code otherwise
 169 */
 170static int csiphy_set_power(struct v4l2_subdev *sd, int on)
 171{
 172        struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 173        struct device *dev = csiphy->camss->dev;
 174
 175        if (on) {
 176                int ret;
 177
 178                ret = pm_runtime_get_sync(dev);
 179                if (ret < 0)
 180                        return ret;
 181
 182                ret = csiphy_set_clock_rates(csiphy);
 183                if (ret < 0) {
 184                        pm_runtime_put_sync(dev);
 185                        return ret;
 186                }
 187
 188                ret = camss_enable_clocks(csiphy->nclocks, csiphy->clock, dev);
 189                if (ret < 0) {
 190                        pm_runtime_put_sync(dev);
 191                        return ret;
 192                }
 193
 194                enable_irq(csiphy->irq);
 195
 196                csiphy->ops->reset(csiphy);
 197
 198                csiphy->ops->hw_version_read(csiphy, dev);
 199        } else {
 200                disable_irq(csiphy->irq);
 201
 202                camss_disable_clocks(csiphy->nclocks, csiphy->clock);
 203
 204                pm_runtime_put_sync(dev);
 205        }
 206
 207        return 0;
 208}
 209
 210/*
 211 * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
 212 * @lane_cfg - CSI2 lane configuration
 213 *
 214 * Return lane mask
 215 */
 216static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
 217{
 218        u8 lane_mask;
 219        int i;
 220
 221        lane_mask = 1 << lane_cfg->clk.pos;
 222
 223        for (i = 0; i < lane_cfg->num_data; i++)
 224                lane_mask |= 1 << lane_cfg->data[i].pos;
 225
 226        return lane_mask;
 227}
 228
 229/*
 230 * csiphy_stream_on - Enable streaming on CSIPHY module
 231 * @csiphy: CSIPHY device
 232 *
 233 * Helper function to enable streaming on CSIPHY module.
 234 * Main configuration of CSIPHY module is also done here.
 235 *
 236 * Return 0 on success or a negative error code otherwise
 237 */
 238static int csiphy_stream_on(struct csiphy_device *csiphy)
 239{
 240        struct csiphy_config *cfg = &csiphy->cfg;
 241        u32 pixel_clock;
 242        u8 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg);
 243        u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
 244                                csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
 245        u8 val;
 246        int ret;
 247
 248        ret = camss_get_pixel_clock(&csiphy->subdev.entity, &pixel_clock);
 249        if (ret) {
 250                dev_err(csiphy->camss->dev,
 251                        "Cannot get CSI2 transmitter's pixel clock\n");
 252                return -EINVAL;
 253        }
 254        if (!pixel_clock) {
 255                dev_err(csiphy->camss->dev,
 256                        "Got pixel clock == 0, cannot continue\n");
 257                return -EINVAL;
 258        }
 259
 260        val = readl_relaxed(csiphy->base_clk_mux);
 261        if (cfg->combo_mode && (lane_mask & 0x18) == 0x18) {
 262                val &= ~0xf0;
 263                val |= cfg->csid_id << 4;
 264        } else {
 265                val &= ~0xf;
 266                val |= cfg->csid_id;
 267        }
 268        writel_relaxed(val, csiphy->base_clk_mux);
 269        wmb();
 270
 271        csiphy->ops->lanes_enable(csiphy, cfg, pixel_clock, bpp, lane_mask);
 272
 273        return 0;
 274}
 275
 276/*
 277 * csiphy_stream_off - Disable streaming on CSIPHY module
 278 * @csiphy: CSIPHY device
 279 *
 280 * Helper function to disable streaming on CSIPHY module
 281 */
 282static void csiphy_stream_off(struct csiphy_device *csiphy)
 283{
 284        csiphy->ops->lanes_disable(csiphy, &csiphy->cfg);
 285}
 286
 287
 288/*
 289 * csiphy_set_stream - Enable/disable streaming on CSIPHY module
 290 * @sd: CSIPHY V4L2 subdevice
 291 * @enable: Requested streaming state
 292 *
 293 * Return 0 on success or a negative error code otherwise
 294 */
 295static int csiphy_set_stream(struct v4l2_subdev *sd, int enable)
 296{
 297        struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 298        int ret = 0;
 299
 300        if (enable)
 301                ret = csiphy_stream_on(csiphy);
 302        else
 303                csiphy_stream_off(csiphy);
 304
 305        return ret;
 306}
 307
 308/*
 309 * __csiphy_get_format - Get pointer to format structure
 310 * @csiphy: CSIPHY device
 311 * @cfg: V4L2 subdev pad configuration
 312 * @pad: pad from which format is requested
 313 * @which: TRY or ACTIVE format
 314 *
 315 * Return pointer to TRY or ACTIVE format structure
 316 */
 317static struct v4l2_mbus_framefmt *
 318__csiphy_get_format(struct csiphy_device *csiphy,
 319                    struct v4l2_subdev_pad_config *cfg,
 320                    unsigned int pad,
 321                    enum v4l2_subdev_format_whence which)
 322{
 323        if (which == V4L2_SUBDEV_FORMAT_TRY)
 324                return v4l2_subdev_get_try_format(&csiphy->subdev, cfg, pad);
 325
 326        return &csiphy->fmt[pad];
 327}
 328
 329/*
 330 * csiphy_try_format - Handle try format by pad subdev method
 331 * @csiphy: CSIPHY device
 332 * @cfg: V4L2 subdev pad configuration
 333 * @pad: pad on which format is requested
 334 * @fmt: pointer to v4l2 format structure
 335 * @which: wanted subdev format
 336 */
 337static void csiphy_try_format(struct csiphy_device *csiphy,
 338                              struct v4l2_subdev_pad_config *cfg,
 339                              unsigned int pad,
 340                              struct v4l2_mbus_framefmt *fmt,
 341                              enum v4l2_subdev_format_whence which)
 342{
 343        unsigned int i;
 344
 345        switch (pad) {
 346        case MSM_CSIPHY_PAD_SINK:
 347                /* Set format on sink pad */
 348
 349                for (i = 0; i < csiphy->nformats; i++)
 350                        if (fmt->code == csiphy->formats[i].code)
 351                                break;
 352
 353                /* If not found, use UYVY as default */
 354                if (i >= csiphy->nformats)
 355                        fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 356
 357                fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 358                fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 359
 360                fmt->field = V4L2_FIELD_NONE;
 361                fmt->colorspace = V4L2_COLORSPACE_SRGB;
 362
 363                break;
 364
 365        case MSM_CSIPHY_PAD_SRC:
 366                /* Set and return a format same as sink pad */
 367
 368                *fmt = *__csiphy_get_format(csiphy, cfg, MSM_CSID_PAD_SINK,
 369                                            which);
 370
 371                break;
 372        }
 373}
 374
 375/*
 376 * csiphy_enum_mbus_code - Handle pixel format enumeration
 377 * @sd: CSIPHY V4L2 subdevice
 378 * @cfg: V4L2 subdev pad configuration
 379 * @code: pointer to v4l2_subdev_mbus_code_enum structure
 380 * return -EINVAL or zero on success
 381 */
 382static int csiphy_enum_mbus_code(struct v4l2_subdev *sd,
 383                                 struct v4l2_subdev_pad_config *cfg,
 384                                 struct v4l2_subdev_mbus_code_enum *code)
 385{
 386        struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 387        struct v4l2_mbus_framefmt *format;
 388
 389        if (code->pad == MSM_CSIPHY_PAD_SINK) {
 390                if (code->index >= csiphy->nformats)
 391                        return -EINVAL;
 392
 393                code->code = csiphy->formats[code->index].code;
 394        } else {
 395                if (code->index > 0)
 396                        return -EINVAL;
 397
 398                format = __csiphy_get_format(csiphy, cfg, MSM_CSIPHY_PAD_SINK,
 399                                             code->which);
 400
 401                code->code = format->code;
 402        }
 403
 404        return 0;
 405}
 406
 407/*
 408 * csiphy_enum_frame_size - Handle frame size enumeration
 409 * @sd: CSIPHY V4L2 subdevice
 410 * @cfg: V4L2 subdev pad configuration
 411 * @fse: pointer to v4l2_subdev_frame_size_enum structure
 412 * return -EINVAL or zero on success
 413 */
 414static int csiphy_enum_frame_size(struct v4l2_subdev *sd,
 415                                  struct v4l2_subdev_pad_config *cfg,
 416                                  struct v4l2_subdev_frame_size_enum *fse)
 417{
 418        struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 419        struct v4l2_mbus_framefmt format;
 420
 421        if (fse->index != 0)
 422                return -EINVAL;
 423
 424        format.code = fse->code;
 425        format.width = 1;
 426        format.height = 1;
 427        csiphy_try_format(csiphy, cfg, fse->pad, &format, fse->which);
 428        fse->min_width = format.width;
 429        fse->min_height = format.height;
 430
 431        if (format.code != fse->code)
 432                return -EINVAL;
 433
 434        format.code = fse->code;
 435        format.width = -1;
 436        format.height = -1;
 437        csiphy_try_format(csiphy, cfg, fse->pad, &format, fse->which);
 438        fse->max_width = format.width;
 439        fse->max_height = format.height;
 440
 441        return 0;
 442}
 443
 444/*
 445 * csiphy_get_format - Handle get format by pads subdev method
 446 * @sd: CSIPHY V4L2 subdevice
 447 * @cfg: V4L2 subdev pad configuration
 448 * @fmt: pointer to v4l2 subdev format structure
 449 *
 450 * Return -EINVAL or zero on success
 451 */
 452static int csiphy_get_format(struct v4l2_subdev *sd,
 453                             struct v4l2_subdev_pad_config *cfg,
 454                             struct v4l2_subdev_format *fmt)
 455{
 456        struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 457        struct v4l2_mbus_framefmt *format;
 458
 459        format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which);
 460        if (format == NULL)
 461                return -EINVAL;
 462
 463        fmt->format = *format;
 464
 465        return 0;
 466}
 467
 468/*
 469 * csiphy_set_format - Handle set format by pads subdev method
 470 * @sd: CSIPHY V4L2 subdevice
 471 * @cfg: V4L2 subdev pad configuration
 472 * @fmt: pointer to v4l2 subdev format structure
 473 *
 474 * Return -EINVAL or zero on success
 475 */
 476static int csiphy_set_format(struct v4l2_subdev *sd,
 477                             struct v4l2_subdev_pad_config *cfg,
 478                             struct v4l2_subdev_format *fmt)
 479{
 480        struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 481        struct v4l2_mbus_framefmt *format;
 482
 483        format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which);
 484        if (format == NULL)
 485                return -EINVAL;
 486
 487        csiphy_try_format(csiphy, cfg, fmt->pad, &fmt->format, fmt->which);
 488        *format = fmt->format;
 489
 490        /* Propagate the format from sink to source */
 491        if (fmt->pad == MSM_CSIPHY_PAD_SINK) {
 492                format = __csiphy_get_format(csiphy, cfg, MSM_CSIPHY_PAD_SRC,
 493                                             fmt->which);
 494
 495                *format = fmt->format;
 496                csiphy_try_format(csiphy, cfg, MSM_CSIPHY_PAD_SRC, format,
 497                                  fmt->which);
 498        }
 499
 500        return 0;
 501}
 502
 503/*
 504 * csiphy_init_formats - Initialize formats on all pads
 505 * @sd: CSIPHY V4L2 subdevice
 506 * @fh: V4L2 subdev file handle
 507 *
 508 * Initialize all pad formats with default values.
 509 *
 510 * Return 0 on success or a negative error code otherwise
 511 */
 512static int csiphy_init_formats(struct v4l2_subdev *sd,
 513                               struct v4l2_subdev_fh *fh)
 514{
 515        struct v4l2_subdev_format format = {
 516                .pad = MSM_CSIPHY_PAD_SINK,
 517                .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
 518                              V4L2_SUBDEV_FORMAT_ACTIVE,
 519                .format = {
 520                        .code = MEDIA_BUS_FMT_UYVY8_2X8,
 521                        .width = 1920,
 522                        .height = 1080
 523                }
 524        };
 525
 526        return csiphy_set_format(sd, fh ? fh->pad : NULL, &format);
 527}
 528
 529/*
 530 * msm_csiphy_subdev_init - Initialize CSIPHY device structure and resources
 531 * @csiphy: CSIPHY device
 532 * @res: CSIPHY module resources table
 533 * @id: CSIPHY module id
 534 *
 535 * Return 0 on success or a negative error code otherwise
 536 */
 537int msm_csiphy_subdev_init(struct camss *camss,
 538                           struct csiphy_device *csiphy,
 539                           const struct resources *res, u8 id)
 540{
 541        struct device *dev = camss->dev;
 542        struct platform_device *pdev = to_platform_device(dev);
 543        struct resource *r;
 544        int i, j;
 545        int ret;
 546
 547        csiphy->camss = camss;
 548        csiphy->id = id;
 549        csiphy->cfg.combo_mode = 0;
 550
 551        if (camss->version == CAMSS_8x16) {
 552                csiphy->ops = &csiphy_ops_2ph_1_0;
 553                csiphy->formats = csiphy_formats_8x16;
 554                csiphy->nformats = ARRAY_SIZE(csiphy_formats_8x16);
 555        } else if (camss->version == CAMSS_8x96) {
 556                csiphy->ops = &csiphy_ops_3ph_1_0;
 557                csiphy->formats = csiphy_formats_8x96;
 558                csiphy->nformats = ARRAY_SIZE(csiphy_formats_8x96);
 559        } else {
 560                return -EINVAL;
 561        }
 562
 563        /* Memory */
 564
 565        r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
 566        csiphy->base = devm_ioremap_resource(dev, r);
 567        if (IS_ERR(csiphy->base)) {
 568                dev_err(dev, "could not map memory\n");
 569                return PTR_ERR(csiphy->base);
 570        }
 571
 572        r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[1]);
 573        csiphy->base_clk_mux = devm_ioremap_resource(dev, r);
 574        if (IS_ERR(csiphy->base_clk_mux)) {
 575                dev_err(dev, "could not map memory\n");
 576                return PTR_ERR(csiphy->base_clk_mux);
 577        }
 578
 579        /* Interrupt */
 580
 581        r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
 582                                         res->interrupt[0]);
 583        if (!r) {
 584                dev_err(dev, "missing IRQ\n");
 585                return -EINVAL;
 586        }
 587
 588        csiphy->irq = r->start;
 589        snprintf(csiphy->irq_name, sizeof(csiphy->irq_name), "%s_%s%d",
 590                 dev_name(dev), MSM_CSIPHY_NAME, csiphy->id);
 591
 592        ret = devm_request_irq(dev, csiphy->irq, csiphy->ops->isr,
 593                               IRQF_TRIGGER_RISING, csiphy->irq_name, csiphy);
 594        if (ret < 0) {
 595                dev_err(dev, "request_irq failed: %d\n", ret);
 596                return ret;
 597        }
 598
 599        disable_irq(csiphy->irq);
 600
 601        /* Clocks */
 602
 603        csiphy->nclocks = 0;
 604        while (res->clock[csiphy->nclocks])
 605                csiphy->nclocks++;
 606
 607        csiphy->clock = devm_kcalloc(dev,
 608                                     csiphy->nclocks, sizeof(*csiphy->clock),
 609                                     GFP_KERNEL);
 610        if (!csiphy->clock)
 611                return -ENOMEM;
 612
 613        for (i = 0; i < csiphy->nclocks; i++) {
 614                struct camss_clock *clock = &csiphy->clock[i];
 615
 616                clock->clk = devm_clk_get(dev, res->clock[i]);
 617                if (IS_ERR(clock->clk))
 618                        return PTR_ERR(clock->clk);
 619
 620                clock->name = res->clock[i];
 621
 622                clock->nfreqs = 0;
 623                while (res->clock_rate[i][clock->nfreqs])
 624                        clock->nfreqs++;
 625
 626                if (!clock->nfreqs) {
 627                        clock->freq = NULL;
 628                        continue;
 629                }
 630
 631                clock->freq = devm_kcalloc(dev,
 632                                           clock->nfreqs,
 633                                           sizeof(*clock->freq),
 634                                           GFP_KERNEL);
 635                if (!clock->freq)
 636                        return -ENOMEM;
 637
 638                for (j = 0; j < clock->nfreqs; j++)
 639                        clock->freq[j] = res->clock_rate[i][j];
 640        }
 641
 642        return 0;
 643}
 644
 645/*
 646 * csiphy_link_setup - Setup CSIPHY connections
 647 * @entity: Pointer to media entity structure
 648 * @local: Pointer to local pad
 649 * @remote: Pointer to remote pad
 650 * @flags: Link flags
 651 *
 652 * Rreturn 0 on success
 653 */
 654static int csiphy_link_setup(struct media_entity *entity,
 655                             const struct media_pad *local,
 656                             const struct media_pad *remote, u32 flags)
 657{
 658        if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
 659            (flags & MEDIA_LNK_FL_ENABLED)) {
 660                struct v4l2_subdev *sd;
 661                struct csiphy_device *csiphy;
 662                struct csid_device *csid;
 663
 664                if (media_entity_remote_pad(local))
 665                        return -EBUSY;
 666
 667                sd = media_entity_to_v4l2_subdev(entity);
 668                csiphy = v4l2_get_subdevdata(sd);
 669
 670                sd = media_entity_to_v4l2_subdev(remote->entity);
 671                csid = v4l2_get_subdevdata(sd);
 672
 673                csiphy->cfg.csid_id = csid->id;
 674        }
 675
 676        return 0;
 677}
 678
 679static const struct v4l2_subdev_core_ops csiphy_core_ops = {
 680        .s_power = csiphy_set_power,
 681};
 682
 683static const struct v4l2_subdev_video_ops csiphy_video_ops = {
 684        .s_stream = csiphy_set_stream,
 685};
 686
 687static const struct v4l2_subdev_pad_ops csiphy_pad_ops = {
 688        .enum_mbus_code = csiphy_enum_mbus_code,
 689        .enum_frame_size = csiphy_enum_frame_size,
 690        .get_fmt = csiphy_get_format,
 691        .set_fmt = csiphy_set_format,
 692};
 693
 694static const struct v4l2_subdev_ops csiphy_v4l2_ops = {
 695        .core = &csiphy_core_ops,
 696        .video = &csiphy_video_ops,
 697        .pad = &csiphy_pad_ops,
 698};
 699
 700static const struct v4l2_subdev_internal_ops csiphy_v4l2_internal_ops = {
 701        .open = csiphy_init_formats,
 702};
 703
 704static const struct media_entity_operations csiphy_media_ops = {
 705        .link_setup = csiphy_link_setup,
 706        .link_validate = v4l2_subdev_link_validate,
 707};
 708
 709/*
 710 * msm_csiphy_register_entity - Register subdev node for CSIPHY module
 711 * @csiphy: CSIPHY device
 712 * @v4l2_dev: V4L2 device
 713 *
 714 * Return 0 on success or a negative error code otherwise
 715 */
 716int msm_csiphy_register_entity(struct csiphy_device *csiphy,
 717                               struct v4l2_device *v4l2_dev)
 718{
 719        struct v4l2_subdev *sd = &csiphy->subdev;
 720        struct media_pad *pads = csiphy->pads;
 721        struct device *dev = csiphy->camss->dev;
 722        int ret;
 723
 724        v4l2_subdev_init(sd, &csiphy_v4l2_ops);
 725        sd->internal_ops = &csiphy_v4l2_internal_ops;
 726        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 727        snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
 728                 MSM_CSIPHY_NAME, csiphy->id);
 729        v4l2_set_subdevdata(sd, csiphy);
 730
 731        ret = csiphy_init_formats(sd, NULL);
 732        if (ret < 0) {
 733                dev_err(dev, "Failed to init format: %d\n", ret);
 734                return ret;
 735        }
 736
 737        pads[MSM_CSIPHY_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 738        pads[MSM_CSIPHY_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
 739
 740        sd->entity.function = MEDIA_ENT_F_IO_V4L;
 741        sd->entity.ops = &csiphy_media_ops;
 742        ret = media_entity_pads_init(&sd->entity, MSM_CSIPHY_PADS_NUM, pads);
 743        if (ret < 0) {
 744                dev_err(dev, "Failed to init media entity: %d\n", ret);
 745                return ret;
 746        }
 747
 748        ret = v4l2_device_register_subdev(v4l2_dev, sd);
 749        if (ret < 0) {
 750                dev_err(dev, "Failed to register subdev: %d\n", ret);
 751                media_entity_cleanup(&sd->entity);
 752        }
 753
 754        return ret;
 755}
 756
 757/*
 758 * msm_csiphy_unregister_entity - Unregister CSIPHY module subdev node
 759 * @csiphy: CSIPHY device
 760 */
 761void msm_csiphy_unregister_entity(struct csiphy_device *csiphy)
 762{
 763        v4l2_device_unregister_subdev(&csiphy->subdev);
 764        media_entity_cleanup(&csiphy->subdev.entity);
 765}
 766