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