drivers/media/platform/qcom/camss/camss-csiphy.c

   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
  27 struct csiphy_format {
  28         u32 code;
  29         u8 bpp;
  30 };
  31
  32 static 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
  52 static 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  */
  84 static 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  */
 102 static 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  */
 170 static 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                         pm_runtime_put_sync(dev);
 181                         return ret;
 182                 }
 183
 184                 ret = csiphy_set_clock_rates(csiphy);
 185                 if (ret < 0) {
 186                         pm_runtime_put_sync(dev);
 187                         return ret;
 188                 }
 189
 190                 ret = camss_enable_clocks(csiphy->nclocks, csiphy->clock, dev);
 191                 if (ret < 0) {
 192                         pm_runtime_put_sync(dev);
 193                         return ret;
 194                 }
 195
 196                 enable_irq(csiphy->irq);
 197
 198                 csiphy->ops->reset(csiphy);
 199
 200                 csiphy->ops->hw_version_read(csiphy, dev);
 201         } else {
 202                 disable_irq(csiphy->irq);
 203
 204                 camss_disable_clocks(csiphy->nclocks, csiphy->clock);
 205
 206                 pm_runtime_put_sync(dev);
 207         }
 208
 209         return 0;
 210 }
 211
 212 /*
 213  * csiphy_get_lane_mask - Calculate CSI2 lane mask configuration parameter
 214  * @lane_cfg - CSI2 lane configuration
 215  *
 216  * Return lane mask
 217  */
 218 static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
 219 {
 220         u8 lane_mask;
 221         int i;
 222
 223         lane_mask = 1 << lane_cfg->clk.pos;
 224
 225         for (i = 0; i < lane_cfg->num_data; i++)
 226                 lane_mask |= 1 << lane_cfg->data[i].pos;
 227
 228         return lane_mask;
 229 }
 230
 231 /*
 232  * csiphy_stream_on - Enable streaming on CSIPHY module
 233  * @csiphy: CSIPHY device
 234  *
 235  * Helper function to enable streaming on CSIPHY module.
 236  * Main configuration of CSIPHY module is also done here.
 237  *
 238  * Return 0 on success or a negative error code otherwise
 239  */
 240 static int csiphy_stream_on(struct csiphy_device *csiphy)
 241 {
 242         struct csiphy_config *cfg = &csiphy->cfg;
 243         u32 pixel_clock;
 244         u8 lane_mask = csiphy_get_lane_mask(&cfg->csi2->lane_cfg);
 245         u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
 246                                 csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
 247         u8 val;
 248         int ret;
 249
 250         ret = camss_get_pixel_clock(&csiphy->subdev.entity, &pixel_clock);
 251         if (ret) {
 252                 dev_err(csiphy->camss->dev,
 253                         "Cannot get CSI2 transmitter's pixel clock\n");
 254                 return -EINVAL;
 255         }
 256         if (!pixel_clock) {
 257                 dev_err(csiphy->camss->dev,
 258                         "Got pixel clock == 0, cannot continue\n");
 259                 return -EINVAL;
 260         }
 261
 262         val = readl_relaxed(csiphy->base_clk_mux);
 263         if (cfg->combo_mode && (lane_mask & 0x18) == 0x18) {
 264                 val &= ~0xf0;
 265                 val |= cfg->csid_id << 4;
 266         } else {
 267                 val &= ~0xf;
 268                 val |= cfg->csid_id;
 269         }
 270         writel_relaxed(val, csiphy->base_clk_mux);
 271         wmb();
 272
 273         csiphy->ops->lanes_enable(csiphy, cfg, pixel_clock, bpp, lane_mask);
 274
 275         return 0;
 276 }
 277
 278 /*
 279  * csiphy_stream_off - Disable streaming on CSIPHY module
 280  * @csiphy: CSIPHY device
 281  *
 282  * Helper function to disable streaming on CSIPHY module
 283  */
 284 static void csiphy_stream_off(struct csiphy_device *csiphy)
 285 {
 286         csiphy->ops->lanes_disable(csiphy, &csiphy->cfg);
 287 }
 288
 289
 290 /*
 291  * csiphy_set_stream - Enable/disable streaming on CSIPHY module
 292  * @sd: CSIPHY V4L2 subdevice
 293  * @enable: Requested streaming state
 294  *
 295  * Return 0 on success or a negative error code otherwise
 296  */
 297 static int csiphy_set_stream(struct v4l2_subdev *sd, int enable)
 298 {
 299         struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 300         int ret = 0;
 301
 302         if (enable)
 303                 ret = csiphy_stream_on(csiphy);
 304         else
 305                 csiphy_stream_off(csiphy);
 306
 307         return ret;
 308 }
 309
 310 /*
 311  * __csiphy_get_format - Get pointer to format structure
 312  * @csiphy: CSIPHY device
 313  * @cfg: V4L2 subdev pad configuration
 314  * @pad: pad from which format is requested
 315  * @which: TRY or ACTIVE format
 316  *
 317  * Return pointer to TRY or ACTIVE format structure
 318  */
 319 static struct v4l2_mbus_framefmt *
 320 __csiphy_get_format(struct csiphy_device *csiphy,
 321                     struct v4l2_subdev_pad_config *cfg,
 322                     unsigned int pad,
 323                     enum v4l2_subdev_format_whence which)
 324 {
 325         if (which == V4L2_SUBDEV_FORMAT_TRY)
 326                 return v4l2_subdev_get_try_format(&csiphy->subdev, cfg, pad);
 327
 328         return &csiphy->fmt[pad];
 329 }
 330
 331 /*
 332  * csiphy_try_format - Handle try format by pad subdev method
 333  * @csiphy: CSIPHY device
 334  * @cfg: V4L2 subdev pad configuration
 335  * @pad: pad on which format is requested
 336  * @fmt: pointer to v4l2 format structure
 337  * @which: wanted subdev format
 338  */
 339 static void csiphy_try_format(struct csiphy_device *csiphy,
 340                               struct v4l2_subdev_pad_config *cfg,
 341                               unsigned int pad,
 342                               struct v4l2_mbus_framefmt *fmt,
 343                               enum v4l2_subdev_format_whence which)
 344 {
 345         unsigned int i;
 346
 347         switch (pad) {
 348         case MSM_CSIPHY_PAD_SINK:
 349                 /* Set format on sink pad */
 350
 351                 for (i = 0; i < csiphy->nformats; i++)
 352                         if (fmt->code == csiphy->formats[i].code)
 353                                 break;
 354
 355                 /* If not found, use UYVY as default */
 356                 if (i >= csiphy->nformats)
 357                         fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
 358
 359                 fmt->width = clamp_t(u32, fmt->width, 1, 8191);
 360                 fmt->height = clamp_t(u32, fmt->height, 1, 8191);
 361
 362                 fmt->field = V4L2_FIELD_NONE;
 363                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
 364
 365                 break;
 366
 367         case MSM_CSIPHY_PAD_SRC:
 368                 /* Set and return a format same as sink pad */
 369
 370                 *fmt = *__csiphy_get_format(csiphy, cfg, MSM_CSID_PAD_SINK,
 371                                             which);
 372
 373                 break;
 374         }
 375 }
 376
 377 /*
 378  * csiphy_enum_mbus_code - Handle pixel format enumeration
 379  * @sd: CSIPHY V4L2 subdevice
 380  * @cfg: V4L2 subdev pad configuration
 381  * @code: pointer to v4l2_subdev_mbus_code_enum structure
 382  * return -EINVAL or zero on success
 383  */
 384 static int csiphy_enum_mbus_code(struct v4l2_subdev *sd,
 385                                  struct v4l2_subdev_pad_config *cfg,
 386                                  struct v4l2_subdev_mbus_code_enum *code)
 387 {
 388         struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 389         struct v4l2_mbus_framefmt *format;
 390
 391         if (code->pad == MSM_CSIPHY_PAD_SINK) {
 392                 if (code->index >= csiphy->nformats)
 393                         return -EINVAL;
 394
 395                 code->code = csiphy->formats[code->index].code;
 396         } else {
 397                 if (code->index > 0)
 398                         return -EINVAL;
 399
 400                 format = __csiphy_get_format(csiphy, cfg, MSM_CSIPHY_PAD_SINK,
 401                                              code->which);
 402
 403                 code->code = format->code;
 404         }
 405
 406         return 0;
 407 }
 408
 409 /*
 410  * csiphy_enum_frame_size - Handle frame size enumeration
 411  * @sd: CSIPHY V4L2 subdevice
 412  * @cfg: V4L2 subdev pad configuration
 413  * @fse: pointer to v4l2_subdev_frame_size_enum structure
 414  * return -EINVAL or zero on success
 415  */
 416 static int csiphy_enum_frame_size(struct v4l2_subdev *sd,
 417                                   struct v4l2_subdev_pad_config *cfg,
 418                                   struct v4l2_subdev_frame_size_enum *fse)
 419 {
 420         struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 421         struct v4l2_mbus_framefmt format;
 422
 423         if (fse->index != 0)
 424                 return -EINVAL;
 425
 426         format.code = fse->code;
 427         format.width = 1;
 428         format.height = 1;
 429         csiphy_try_format(csiphy, cfg, fse->pad, &format, fse->which);
 430         fse->min_width = format.width;
 431         fse->min_height = format.height;
 432
 433         if (format.code != fse->code)
 434                 return -EINVAL;
 435
 436         format.code = fse->code;
 437         format.width = -1;
 438         format.height = -1;
 439         csiphy_try_format(csiphy, cfg, fse->pad, &format, fse->which);
 440         fse->max_width = format.width;
 441         fse->max_height = format.height;
 442
 443         return 0;
 444 }
 445
 446 /*
 447  * csiphy_get_format - Handle get format by pads subdev method
 448  * @sd: CSIPHY V4L2 subdevice
 449  * @cfg: V4L2 subdev pad configuration
 450  * @fmt: pointer to v4l2 subdev format structure
 451  *
 452  * Return -EINVAL or zero on success
 453  */
 454 static int csiphy_get_format(struct v4l2_subdev *sd,
 455                              struct v4l2_subdev_pad_config *cfg,
 456                              struct v4l2_subdev_format *fmt)
 457 {
 458         struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 459         struct v4l2_mbus_framefmt *format;
 460
 461         format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which);
 462         if (format == NULL)
 463                 return -EINVAL;
 464
 465         fmt->format = *format;
 466
 467         return 0;
 468 }
 469
 470 /*
 471  * csiphy_set_format - Handle set format by pads subdev method
 472  * @sd: CSIPHY V4L2 subdevice
 473  * @cfg: V4L2 subdev pad configuration
 474  * @fmt: pointer to v4l2 subdev format structure
 475  *
 476  * Return -EINVAL or zero on success
 477  */
 478 static int csiphy_set_format(struct v4l2_subdev *sd,
 479                              struct v4l2_subdev_pad_config *cfg,
 480                              struct v4l2_subdev_format *fmt)
 481 {
 482         struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
 483         struct v4l2_mbus_framefmt *format;
 484
 485         format = __csiphy_get_format(csiphy, cfg, fmt->pad, fmt->which);
 486         if (format == NULL)
 487                 return -EINVAL;
 488
 489         csiphy_try_format(csiphy, cfg, fmt->pad, &fmt->format, fmt->which);
 490         *format = fmt->format;
 491
 492         /* Propagate the format from sink to source */
 493         if (fmt->pad == MSM_CSIPHY_PAD_SINK) {
 494                 format = __csiphy_get_format(csiphy, cfg, MSM_CSIPHY_PAD_SRC,
 495                                              fmt->which);
 496
 497                 *format = fmt->format;
 498                 csiphy_try_format(csiphy, cfg, MSM_CSIPHY_PAD_SRC, format,
 499                                   fmt->which);
 500         }
 501
 502         return 0;
 503 }
 504
 505 /*
 506  * csiphy_init_formats - Initialize formats on all pads
 507  * @sd: CSIPHY V4L2 subdevice
 508  * @fh: V4L2 subdev file handle
 509  *
 510  * Initialize all pad formats with default values.
 511  *
 512  * Return 0 on success or a negative error code otherwise
 513  */
 514 static int csiphy_init_formats(struct v4l2_subdev *sd,
 515                                struct v4l2_subdev_fh *fh)
 516 {
 517         struct v4l2_subdev_format format = {
 518                 .pad = MSM_CSIPHY_PAD_SINK,
 519                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
 520                               V4L2_SUBDEV_FORMAT_ACTIVE,
 521                 .format = {
 522                         .code = MEDIA_BUS_FMT_UYVY8_2X8,
 523                         .width = 1920,
 524                         .height = 1080
 525                 }
 526         };
 527
 528         return csiphy_set_format(sd, fh ? fh->pad : NULL, &format);
 529 }
 530
 531 /*
 532  * msm_csiphy_subdev_init - Initialize CSIPHY device structure and resources
 533  * @csiphy: CSIPHY device
 534  * @res: CSIPHY module resources table
 535  * @id: CSIPHY module id
 536  *
 537  * Return 0 on success or a negative error code otherwise
 538  */
 539 int msm_csiphy_subdev_init(struct camss *camss,
 540                            struct csiphy_device *csiphy,
 541                            const struct resources *res, u8 id)
 542 {
 543         struct device *dev = camss->dev;
 544         struct platform_device *pdev = to_platform_device(dev);
 545         struct resource *r;
 546         int i, j;
 547         int ret;
 548
 549         csiphy->camss = camss;
 550         csiphy->id = id;
 551         csiphy->cfg.combo_mode = 0;
 552
 553         if (camss->version == CAMSS_8x16) {
 554                 csiphy->ops = &csiphy_ops_2ph_1_0;
 555                 csiphy->formats = csiphy_formats_8x16;
 556                 csiphy->nformats = ARRAY_SIZE(csiphy_formats_8x16);
 557         } else if (camss->version == CAMSS_8x96) {
 558                 csiphy->ops = &csiphy_ops_3ph_1_0;
 559                 csiphy->formats = csiphy_formats_8x96;
 560                 csiphy->nformats = ARRAY_SIZE(csiphy_formats_8x96);
 561         } else {
 562                 return -EINVAL;
 563         }
 564
 565         /* Memory */
 566
 567         r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[0]);
 568         csiphy->base = devm_ioremap_resource(dev, r);
 569         if (IS_ERR(csiphy->base)) {
 570                 dev_err(dev, "could not map memory\n");
 571                 return PTR_ERR(csiphy->base);
 572         }
 573
 574         r = platform_get_resource_byname(pdev, IORESOURCE_MEM, res->reg[1]);
 575         csiphy->base_clk_mux = devm_ioremap_resource(dev, r);
 576         if (IS_ERR(csiphy->base_clk_mux)) {
 577                 dev_err(dev, "could not map memory\n");
 578                 return PTR_ERR(csiphy->base_clk_mux);
 579         }
 580
 581         /* Interrupt */
 582
 583         r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
 584                                          res->interrupt[0]);
 585         if (!r) {
 586                 dev_err(dev, "missing IRQ\n");
 587                 return -EINVAL;
 588         }
 589
 590         csiphy->irq = r->start;
 591         snprintf(csiphy->irq_name, sizeof(csiphy->irq_name), "%s_%s%d",
 592                  dev_name(dev), MSM_CSIPHY_NAME, csiphy->id);
 593
 594         ret = devm_request_irq(dev, csiphy->irq, csiphy->ops->isr,
 595                                IRQF_TRIGGER_RISING, csiphy->irq_name, csiphy);
 596         if (ret < 0) {
 597                 dev_err(dev, "request_irq failed: %d\n", ret);
 598                 return ret;
 599         }
 600
 601         disable_irq(csiphy->irq);
 602
 603         /* Clocks */
 604
 605         csiphy->nclocks = 0;
 606         while (res->clock[csiphy->nclocks])
 607                 csiphy->nclocks++;
 608
 609         csiphy->clock = devm_kcalloc(dev,
 610                                      csiphy->nclocks, sizeof(*csiphy->clock),
 611                                      GFP_KERNEL);
 612         if (!csiphy->clock)
 613                 return -ENOMEM;
 614
 615         for (i = 0; i < csiphy->nclocks; i++) {
 616                 struct camss_clock *clock = &csiphy->clock[i];
 617
 618                 clock->clk = devm_clk_get(dev, res->clock[i]);
 619                 if (IS_ERR(clock->clk))
 620                         return PTR_ERR(clock->clk);
 621
 622                 clock->name = res->clock[i];
 623
 624                 clock->nfreqs = 0;
 625                 while (res->clock_rate[i][clock->nfreqs])
 626                         clock->nfreqs++;
 627
 628                 if (!clock->nfreqs) {
 629                         clock->freq = NULL;
 630                         continue;
 631                 }
 632
 633                 clock->freq = devm_kcalloc(dev,
 634                                            clock->nfreqs,
 635                                            sizeof(*clock->freq),
 636                                            GFP_KERNEL);
 637                 if (!clock->freq)
 638                         return -ENOMEM;
 639
 640                 for (j = 0; j < clock->nfreqs; j++)
 641                         clock->freq[j] = res->clock_rate[i][j];
 642         }
 643
 644         return 0;
 645 }
 646
 647 /*
 648  * csiphy_link_setup - Setup CSIPHY connections
 649  * @entity: Pointer to media entity structure
 650  * @local: Pointer to local pad
 651  * @remote: Pointer to remote pad
 652  * @flags: Link flags
 653  *
 654  * Rreturn 0 on success
 655  */
 656 static int csiphy_link_setup(struct media_entity *entity,
 657                              const struct media_pad *local,
 658                              const struct media_pad *remote, u32 flags)
 659 {
 660         if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
 661             (flags & MEDIA_LNK_FL_ENABLED)) {
 662                 struct v4l2_subdev *sd;
 663                 struct csiphy_device *csiphy;
 664                 struct csid_device *csid;
 665
 666                 if (media_entity_remote_pad(local))
 667                         return -EBUSY;
 668
 669                 sd = media_entity_to_v4l2_subdev(entity);
 670                 csiphy = v4l2_get_subdevdata(sd);
 671
 672                 sd = media_entity_to_v4l2_subdev(remote->entity);
 673                 csid = v4l2_get_subdevdata(sd);
 674
 675                 csiphy->cfg.csid_id = csid->id;
 676         }
 677
 678         return 0;
 679 }
 680
 681 static const struct v4l2_subdev_core_ops csiphy_core_ops = {
 682         .s_power = csiphy_set_power,
 683 };
 684
 685 static const struct v4l2_subdev_video_ops csiphy_video_ops = {
 686         .s_stream = csiphy_set_stream,
 687 };
 688
 689 static const struct v4l2_subdev_pad_ops csiphy_pad_ops = {
 690         .enum_mbus_code = csiphy_enum_mbus_code,
 691         .enum_frame_size = csiphy_enum_frame_size,
 692         .get_fmt = csiphy_get_format,
 693         .set_fmt = csiphy_set_format,
 694 };
 695
 696 static const struct v4l2_subdev_ops csiphy_v4l2_ops = {
 697         .core = &csiphy_core_ops,
 698         .video = &csiphy_video_ops,
 699         .pad = &csiphy_pad_ops,
 700 };
 701
 702 static const struct v4l2_subdev_internal_ops csiphy_v4l2_internal_ops = {
 703         .open = csiphy_init_formats,
 704 };
 705
 706 static const struct media_entity_operations csiphy_media_ops = {
 707         .link_setup = csiphy_link_setup,
 708         .link_validate = v4l2_subdev_link_validate,
 709 };
 710
 711 /*
 712  * msm_csiphy_register_entity - Register subdev node for CSIPHY module
 713  * @csiphy: CSIPHY device
 714  * @v4l2_dev: V4L2 device
 715  *
 716  * Return 0 on success or a negative error code otherwise
 717  */
 718 int msm_csiphy_register_entity(struct csiphy_device *csiphy,
 719                                struct v4l2_device *v4l2_dev)
 720 {
 721         struct v4l2_subdev *sd = &csiphy->subdev;
 722         struct media_pad *pads = csiphy->pads;
 723         struct device *dev = csiphy->camss->dev;
 724         int ret;
 725
 726         v4l2_subdev_init(sd, &csiphy_v4l2_ops);
 727         sd->internal_ops = &csiphy_v4l2_internal_ops;
 728         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 729         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
 730                  MSM_CSIPHY_NAME, csiphy->id);
 731         v4l2_set_subdevdata(sd, csiphy);
 732
 733         ret = csiphy_init_formats(sd, NULL);
 734         if (ret < 0) {
 735                 dev_err(dev, "Failed to init format: %d\n", ret);
 736                 return ret;
 737         }
 738
 739         pads[MSM_CSIPHY_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 740         pads[MSM_CSIPHY_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
 741
 742         sd->entity.function = MEDIA_ENT_F_IO_V4L;
 743         sd->entity.ops = &csiphy_media_ops;
 744         ret = media_entity_pads_init(&sd->entity, MSM_CSIPHY_PADS_NUM, pads);
 745         if (ret < 0) {
 746                 dev_err(dev, "Failed to init media entity: %d\n", ret);
 747                 return ret;
 748         }
 749
 750         ret = v4l2_device_register_subdev(v4l2_dev, sd);
 751         if (ret < 0) {
 752                 dev_err(dev, "Failed to register subdev: %d\n", ret);
 753                 media_entity_cleanup(&sd->entity);
 754         }
 755
 756         return ret;
 757 }
 758
 759 /*
 760  * msm_csiphy_unregister_entity - Unregister CSIPHY module subdev node
 761  * @csiphy: CSIPHY device
 762  */
 763 void msm_csiphy_unregister_entity(struct csiphy_device *csiphy)
 764 {
 765         v4l2_device_unregister_subdev(&csiphy->subdev);
 766         media_entity_cleanup(&csiphy->subdev.entity);
 767 }