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

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