drivers/staging/media/imx/imx6-mipi-csi2.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * MIPI CSI-2 Receiver Subdev for Freescale i.MX6 SOC.
   4  *
   5  * Copyright (c) 2012-2017 Mentor Graphics Inc.
   6  */
   7 #include <linux/clk.h>
   8 #include <linux/interrupt.h>
   9 #include <linux/io.h>
  10 #include <linux/iopoll.h>
  11 #include <linux/irq.h>
  12 #include <linux/module.h>
  13 #include <linux/of_graph.h>
  14 #include <linux/platform_device.h>
  15 #include <media/v4l2-device.h>
  16 #include <media/v4l2-fwnode.h>
  17 #include <media/v4l2-mc.h>
  18 #include <media/v4l2-subdev.h>
  19 #include "imx-media.h"
  20
  21 /*
  22  * there must be 5 pads: 1 input pad from sensor, and
  23  * the 4 virtual channel output pads
  24  */
  25 #define CSI2_SINK_PAD       0
  26 #define CSI2_NUM_SINK_PADS  1
  27 #define CSI2_NUM_SRC_PADS   4
  28 #define CSI2_NUM_PADS       5
  29
  30 /*
  31  * The default maximum bit-rate per lane in Mbps, if the
  32  * source subdev does not provide V4L2_CID_LINK_FREQ.
  33  */
  34 #define CSI2_DEFAULT_MAX_MBPS 849
  35
  36 struct csi2_dev {
  37         struct device          *dev;
  38         struct v4l2_subdev      sd;
  39         struct v4l2_async_notifier notifier;
  40         struct media_pad       pad[CSI2_NUM_PADS];
  41         struct clk             *dphy_clk;
  42         struct clk             *pllref_clk;
  43         struct clk             *pix_clk; /* what is this? */
  44         void __iomem           *base;
  45
  46         struct v4l2_subdev      *remote;
  47         unsigned int            remote_pad;
  48         unsigned short          data_lanes;
  49
  50         /* lock to protect all members below */
  51         struct mutex lock;
  52
  53         struct v4l2_mbus_framefmt format_mbus;
  54
  55         int                     stream_count;
  56         struct v4l2_subdev      *src_sd;
  57         bool                    sink_linked[CSI2_NUM_SRC_PADS];
  58 };
  59
  60 #define DEVICE_NAME "imx6-mipi-csi2"
  61
  62 /* Register offsets */
  63 #define CSI2_VERSION            0x000
  64 #define CSI2_N_LANES            0x004
  65 #define CSI2_PHY_SHUTDOWNZ      0x008
  66 #define CSI2_DPHY_RSTZ          0x00c
  67 #define CSI2_RESETN             0x010
  68 #define CSI2_PHY_STATE          0x014
  69 #define PHY_STOPSTATEDATA_BIT   4
  70 #define PHY_STOPSTATEDATA(n)    BIT(PHY_STOPSTATEDATA_BIT + (n))
  71 #define PHY_RXCLKACTIVEHS       BIT(8)
  72 #define PHY_RXULPSCLKNOT        BIT(9)
  73 #define PHY_STOPSTATECLK        BIT(10)
  74 #define CSI2_DATA_IDS_1         0x018
  75 #define CSI2_DATA_IDS_2         0x01c
  76 #define CSI2_ERR1               0x020
  77 #define CSI2_ERR2               0x024
  78 #define CSI2_MSK1               0x028
  79 #define CSI2_MSK2               0x02c
  80 #define CSI2_PHY_TST_CTRL0      0x030
  81 #define PHY_TESTCLR             BIT(0)
  82 #define PHY_TESTCLK             BIT(1)
  83 #define CSI2_PHY_TST_CTRL1      0x034
  84 #define PHY_TESTEN              BIT(16)
  85 /*
  86  * i.MX CSI2IPU Gasket registers follow. The CSI2IPU gasket is
  87  * not part of the MIPI CSI-2 core, but its registers fall in the
  88  * same register map range.
  89  */
  90 #define CSI2IPU_GASKET          0xf00
  91 #define CSI2IPU_YUV422_YUYV     BIT(2)
  92
  93 static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev)
  94 {
  95         return container_of(sdev, struct csi2_dev, sd);
  96 }
  97
  98 static inline struct csi2_dev *notifier_to_dev(struct v4l2_async_notifier *n)
  99 {
 100         return container_of(n, struct csi2_dev, notifier);
 101 }
 102
 103 /*
 104  * The required sequence of MIPI CSI-2 startup as specified in the i.MX6
 105  * reference manual is as follows:
 106  *
 107  * 1. Deassert presetn signal (global reset).
 108  *        It's not clear what this "global reset" signal is (maybe APB
 109  *        global reset), but in any case this step would be probably
 110  *        be carried out during driver load in csi2_probe().
 111  *
 112  * 2. Configure MIPI Camera Sensor to put all Tx lanes in LP-11 state.
 113  *        This must be carried out by the MIPI sensor's s_power(ON) subdev
 114  *        op.
 115  *
 116  * 3. D-PHY initialization.
 117  * 4. CSI2 Controller programming (Set N_LANES, deassert PHY_SHUTDOWNZ,
 118  *    deassert PHY_RSTZ, deassert CSI2_RESETN).
 119  * 5. Read the PHY status register (PHY_STATE) to confirm that all data and
 120  *    clock lanes of the D-PHY are in LP-11 state.
 121  * 6. Configure the MIPI Camera Sensor to start transmitting a clock on the
 122  *    D-PHY clock lane.
 123  * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE)
 124  *    to confirm that the D-PHY is receiving a clock on the D-PHY clock lane.
 125  *
 126  * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step
 127  * 6 is accomplished by calling the source subdev's s_stream(ON) between
 128  * steps 5 and 7.
 129  */
 130
 131 static void csi2_enable(struct csi2_dev *csi2, bool enable)
 132 {
 133         if (enable) {
 134                 writel(0x1, csi2->base + CSI2_PHY_SHUTDOWNZ);
 135                 writel(0x1, csi2->base + CSI2_DPHY_RSTZ);
 136                 writel(0x1, csi2->base + CSI2_RESETN);
 137         } else {
 138                 writel(0x0, csi2->base + CSI2_PHY_SHUTDOWNZ);
 139                 writel(0x0, csi2->base + CSI2_DPHY_RSTZ);
 140                 writel(0x0, csi2->base + CSI2_RESETN);
 141         }
 142 }
 143
 144 static void csi2_set_lanes(struct csi2_dev *csi2, unsigned int lanes)
 145 {
 146         writel(lanes - 1, csi2->base + CSI2_N_LANES);
 147 }
 148
 149 static void dw_mipi_csi2_phy_write(struct csi2_dev *csi2,
 150                                    u32 test_code, u32 test_data)
 151 {
 152         /* Clear PHY test interface */
 153         writel(PHY_TESTCLR, csi2->base + CSI2_PHY_TST_CTRL0);
 154         writel(0x0, csi2->base + CSI2_PHY_TST_CTRL1);
 155         writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
 156
 157         /* Raise test interface strobe signal */
 158         writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
 159
 160         /* Configure address write on falling edge and lower strobe signal */
 161         writel(PHY_TESTEN | test_code, csi2->base + CSI2_PHY_TST_CTRL1);
 162         writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
 163
 164         /* Configure data write on rising edge and raise strobe signal */
 165         writel(test_data, csi2->base + CSI2_PHY_TST_CTRL1);
 166         writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0);
 167
 168         /* Clear strobe signal */
 169         writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0);
 170 }
 171
 172 /*
 173  * This table is based on the table documented at
 174  * https://community.nxp.com/docs/DOC-94312. It assumes
 175  * a 27MHz D-PHY pll reference clock.
 176  */
 177 static const struct {
 178         u32 max_mbps;
 179         u32 hsfreqrange_sel;
 180 } hsfreq_map[] = {
 181         { 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02},
 182         {140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24},
 183         {200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46},
 184         {270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a},
 185         {400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e},
 186         {600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12},
 187         {800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54},
 188         {1000, 0x74},
 189 };
 190
 191 static int max_mbps_to_hsfreqrange_sel(u32 max_mbps)
 192 {
 193         int i;
 194
 195         for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++)
 196                 if (hsfreq_map[i].max_mbps > max_mbps)
 197                         return hsfreq_map[i].hsfreqrange_sel;
 198
 199         return -EINVAL;
 200 }
 201
 202 static int csi2_dphy_init(struct csi2_dev *csi2)
 203 {
 204         struct v4l2_ctrl *ctrl;
 205         u32 mbps_per_lane;
 206         int sel;
 207
 208         ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler,
 209                               V4L2_CID_LINK_FREQ);
 210         if (!ctrl)
 211                 mbps_per_lane = CSI2_DEFAULT_MAX_MBPS;
 212         else
 213                 mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val],
 214                                                  USEC_PER_SEC);
 215
 216         sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane);
 217         if (sel < 0)
 218                 return sel;
 219
 220         dw_mipi_csi2_phy_write(csi2, 0x44, sel);
 221
 222         return 0;
 223 }
 224
 225 /*
 226  * Waits for ultra-low-power state on D-PHY clock lane. This is currently
 227  * unused and may not be needed at all, but keep around just in case.
 228  */
 229 static int __maybe_unused csi2_dphy_wait_ulp(struct csi2_dev *csi2)
 230 {
 231         u32 reg;
 232         int ret;
 233
 234         /* wait for ULP on clock lane */
 235         ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
 236                                  !(reg & PHY_RXULPSCLKNOT), 0, 500000);
 237         if (ret) {
 238                 v4l2_err(&csi2->sd, "ULP timeout, phy_state = 0x%08x\n", reg);
 239                 return ret;
 240         }
 241
 242         /* wait until no errors on bus */
 243         ret = readl_poll_timeout(csi2->base + CSI2_ERR1, reg,
 244                                  reg == 0x0, 0, 500000);
 245         if (ret) {
 246                 v4l2_err(&csi2->sd, "stable bus timeout, err1 = 0x%08x\n", reg);
 247                 return ret;
 248         }
 249
 250         return 0;
 251 }
 252
 253 /* Waits for low-power LP-11 state on data and clock lanes. */
 254 static void csi2_dphy_wait_stopstate(struct csi2_dev *csi2, unsigned int lanes)
 255 {
 256         u32 mask, reg;
 257         int ret;
 258
 259         mask = PHY_STOPSTATECLK | (((1 << lanes) - 1) << PHY_STOPSTATEDATA_BIT);
 260
 261         ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
 262                                  (reg & mask) == mask, 0, 500000);
 263         if (ret) {
 264                 v4l2_warn(&csi2->sd, "LP-11 wait timeout, likely a sensor driver bug, expect capture failures.\n");
 265                 v4l2_warn(&csi2->sd, "phy_state = 0x%08x\n", reg);
 266         }
 267 }
 268
 269 /* Wait for active clock on the clock lane. */
 270 static int csi2_dphy_wait_clock_lane(struct csi2_dev *csi2)
 271 {
 272         u32 reg;
 273         int ret;
 274
 275         ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg,
 276                                  (reg & PHY_RXCLKACTIVEHS), 0, 500000);
 277         if (ret) {
 278                 v4l2_err(&csi2->sd, "clock lane timeout, phy_state = 0x%08x\n",
 279                          reg);
 280                 return ret;
 281         }
 282
 283         return 0;
 284 }
 285
 286 /* Setup the i.MX CSI2IPU Gasket */
 287 static void csi2ipu_gasket_init(struct csi2_dev *csi2)
 288 {
 289         u32 reg = 0;
 290
 291         switch (csi2->format_mbus.code) {
 292         case MEDIA_BUS_FMT_YUYV8_2X8:
 293         case MEDIA_BUS_FMT_YUYV8_1X16:
 294                 reg = CSI2IPU_YUV422_YUYV;
 295                 break;
 296         default:
 297                 break;
 298         }
 299
 300         writel(reg, csi2->base + CSI2IPU_GASKET);
 301 }
 302
 303 static int csi2_get_active_lanes(struct csi2_dev *csi2, unsigned int *lanes)
 304 {
 305         struct v4l2_mbus_config mbus_config = { 0 };
 306         unsigned int num_lanes = UINT_MAX;
 307         int ret;
 308
 309         *lanes = csi2->data_lanes;
 310
 311         ret = v4l2_subdev_call(csi2->remote, pad, get_mbus_config,
 312                                csi2->remote_pad, &mbus_config);
 313         if (ret == -ENOIOCTLCMD) {
 314                 dev_dbg(csi2->dev, "No remote mbus configuration available\n");
 315                 return 0;
 316         }
 317
 318         if (ret) {
 319                 dev_err(csi2->dev, "Failed to get remote mbus configuration\n");
 320                 return ret;
 321         }
 322
 323         if (mbus_config.type != V4L2_MBUS_CSI2_DPHY) {
 324                 dev_err(csi2->dev, "Unsupported media bus type %u\n",
 325                         mbus_config.type);
 326                 return -EINVAL;
 327         }
 328
 329         switch (mbus_config.flags & V4L2_MBUS_CSI2_LANES) {
 330         case V4L2_MBUS_CSI2_1_LANE:
 331                 num_lanes = 1;
 332                 break;
 333         case V4L2_MBUS_CSI2_2_LANE:
 334                 num_lanes = 2;
 335                 break;
 336         case V4L2_MBUS_CSI2_3_LANE:
 337                 num_lanes = 3;
 338                 break;
 339         case V4L2_MBUS_CSI2_4_LANE:
 340                 num_lanes = 4;
 341                 break;
 342         default:
 343                 num_lanes = csi2->data_lanes;
 344                 break;
 345         }
 346
 347         if (num_lanes > csi2->data_lanes) {
 348                 dev_err(csi2->dev,
 349                         "Unsupported mbus config: too many data lanes %u\n",
 350                         num_lanes);
 351                 return -EINVAL;
 352         }
 353
 354         *lanes = num_lanes;
 355
 356         return 0;
 357 }
 358
 359 static int csi2_start(struct csi2_dev *csi2)
 360 {
 361         unsigned int lanes;
 362         int ret;
 363
 364         ret = clk_prepare_enable(csi2->pix_clk);
 365         if (ret)
 366                 return ret;
 367
 368         /* setup the gasket */
 369         csi2ipu_gasket_init(csi2);
 370
 371         /* Step 3 */
 372         ret = csi2_dphy_init(csi2);
 373         if (ret)
 374                 goto err_disable_clk;
 375
 376         ret = csi2_get_active_lanes(csi2, &lanes);
 377         if (ret)
 378                 goto err_disable_clk;
 379
 380         /* Step 4 */
 381         csi2_set_lanes(csi2, lanes);
 382         csi2_enable(csi2, true);
 383
 384         /* Step 5 */
 385         csi2_dphy_wait_stopstate(csi2, lanes);
 386
 387         /* Step 6 */
 388         ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1);
 389         ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
 390         if (ret)
 391                 goto err_assert_reset;
 392
 393         /* Step 7 */
 394         ret = csi2_dphy_wait_clock_lane(csi2);
 395         if (ret)
 396                 goto err_stop_upstream;
 397
 398         return 0;
 399
 400 err_stop_upstream:
 401         v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
 402 err_assert_reset:
 403         csi2_enable(csi2, false);
 404 err_disable_clk:
 405         clk_disable_unprepare(csi2->pix_clk);
 406         return ret;
 407 }
 408
 409 static void csi2_stop(struct csi2_dev *csi2)
 410 {
 411         /* stop upstream */
 412         v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
 413
 414         csi2_enable(csi2, false);
 415         clk_disable_unprepare(csi2->pix_clk);
 416 }
 417
 418 /*
 419  * V4L2 subdev operations.
 420  */
 421
 422 static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
 423 {
 424         struct csi2_dev *csi2 = sd_to_dev(sd);
 425         int i, ret = 0;
 426
 427         mutex_lock(&csi2->lock);
 428
 429         if (!csi2->src_sd) {
 430                 ret = -EPIPE;
 431                 goto out;
 432         }
 433
 434         for (i = 0; i < CSI2_NUM_SRC_PADS; i++) {
 435                 if (csi2->sink_linked[i])
 436                         break;
 437         }
 438         if (i >= CSI2_NUM_SRC_PADS) {
 439                 ret = -EPIPE;
 440                 goto out;
 441         }
 442
 443         /*
 444          * enable/disable streaming only if stream_count is
 445          * going from 0 to 1 / 1 to 0.
 446          */
 447         if (csi2->stream_count != !enable)
 448                 goto update_count;
 449
 450         dev_dbg(csi2->dev, "stream %s\n", enable ? "ON" : "OFF");
 451         if (enable)
 452                 ret = csi2_start(csi2);
 453         else
 454                 csi2_stop(csi2);
 455         if (ret)
 456                 goto out;
 457
 458 update_count:
 459         csi2->stream_count += enable ? 1 : -1;
 460         if (csi2->stream_count < 0)
 461                 csi2->stream_count = 0;
 462 out:
 463         mutex_unlock(&csi2->lock);
 464         return ret;
 465 }
 466
 467 static int csi2_link_setup(struct media_entity *entity,
 468                            const struct media_pad *local,
 469                            const struct media_pad *remote, u32 flags)
 470 {
 471         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 472         struct csi2_dev *csi2 = sd_to_dev(sd);
 473         struct v4l2_subdev *remote_sd;
 474         int ret = 0;
 475
 476         dev_dbg(csi2->dev, "link setup %s -> %s", remote->entity->name,
 477                 local->entity->name);
 478
 479         remote_sd = media_entity_to_v4l2_subdev(remote->entity);
 480
 481         mutex_lock(&csi2->lock);
 482
 483         if (local->flags & MEDIA_PAD_FL_SOURCE) {
 484                 if (flags & MEDIA_LNK_FL_ENABLED) {
 485                         if (csi2->sink_linked[local->index - 1]) {
 486                                 ret = -EBUSY;
 487                                 goto out;
 488                         }
 489                         csi2->sink_linked[local->index - 1] = true;
 490                 } else {
 491                         csi2->sink_linked[local->index - 1] = false;
 492                 }
 493         } else {
 494                 if (flags & MEDIA_LNK_FL_ENABLED) {
 495                         if (csi2->src_sd) {
 496                                 ret = -EBUSY;
 497                                 goto out;
 498                         }
 499                         csi2->src_sd = remote_sd;
 500                 } else {
 501                         csi2->src_sd = NULL;
 502                 }
 503         }
 504
 505 out:
 506         mutex_unlock(&csi2->lock);
 507         return ret;
 508 }
 509
 510 static struct v4l2_mbus_framefmt *
 511 __csi2_get_fmt(struct csi2_dev *csi2, struct v4l2_subdev_state *sd_state,
 512                unsigned int pad, enum v4l2_subdev_format_whence which)
 513 {
 514         if (which == V4L2_SUBDEV_FORMAT_TRY)
 515                 return v4l2_subdev_get_try_format(&csi2->sd, sd_state, pad);
 516         else
 517                 return &csi2->format_mbus;
 518 }
 519
 520 static int csi2_get_fmt(struct v4l2_subdev *sd,
 521                         struct v4l2_subdev_state *sd_state,
 522                         struct v4l2_subdev_format *sdformat)
 523 {
 524         struct csi2_dev *csi2 = sd_to_dev(sd);
 525         struct v4l2_mbus_framefmt *fmt;
 526
 527         mutex_lock(&csi2->lock);
 528
 529         fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
 530
 531         sdformat->format = *fmt;
 532
 533         mutex_unlock(&csi2->lock);
 534
 535         return 0;
 536 }
 537
 538 static int csi2_set_fmt(struct v4l2_subdev *sd,
 539                         struct v4l2_subdev_state *sd_state,
 540                         struct v4l2_subdev_format *sdformat)
 541 {
 542         struct csi2_dev *csi2 = sd_to_dev(sd);
 543         struct v4l2_mbus_framefmt *fmt;
 544         int ret = 0;
 545
 546         if (sdformat->pad >= CSI2_NUM_PADS)
 547                 return -EINVAL;
 548
 549         mutex_lock(&csi2->lock);
 550
 551         if (csi2->stream_count > 0) {
 552                 ret = -EBUSY;
 553                 goto out;
 554         }
 555
 556         /* Output pads mirror active input pad, no limits on input pads */
 557         if (sdformat->pad != CSI2_SINK_PAD)
 558                 sdformat->format = csi2->format_mbus;
 559
 560         fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
 561
 562         *fmt = sdformat->format;
 563 out:
 564         mutex_unlock(&csi2->lock);
 565         return ret;
 566 }
 567
 568 static int csi2_registered(struct v4l2_subdev *sd)
 569 {
 570         struct csi2_dev *csi2 = sd_to_dev(sd);
 571
 572         /* set a default mbus format  */
 573         return imx_media_init_mbus_fmt(&csi2->format_mbus,
 574                                       IMX_MEDIA_DEF_PIX_WIDTH,
 575                                       IMX_MEDIA_DEF_PIX_HEIGHT, 0,
 576                                       V4L2_FIELD_NONE, NULL);
 577 }
 578
 579 static const struct media_entity_operations csi2_entity_ops = {
 580         .link_setup = csi2_link_setup,
 581         .link_validate = v4l2_subdev_link_validate,
 582         .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
 583 };
 584
 585 static const struct v4l2_subdev_video_ops csi2_video_ops = {
 586         .s_stream = csi2_s_stream,
 587 };
 588
 589 static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
 590         .init_cfg = imx_media_init_cfg,
 591         .get_fmt = csi2_get_fmt,
 592         .set_fmt = csi2_set_fmt,
 593 };
 594
 595 static const struct v4l2_subdev_ops csi2_subdev_ops = {
 596         .video = &csi2_video_ops,
 597         .pad = &csi2_pad_ops,
 598 };
 599
 600 static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
 601         .registered = csi2_registered,
 602 };
 603
 604 static int csi2_notify_bound(struct v4l2_async_notifier *notifier,
 605                              struct v4l2_subdev *sd,
 606                              struct v4l2_async_subdev *asd)
 607 {
 608         struct csi2_dev *csi2 = notifier_to_dev(notifier);
 609         struct media_pad *sink = &csi2->sd.entity.pads[CSI2_SINK_PAD];
 610         int pad;
 611
 612         pad = media_entity_get_fwnode_pad(&sd->entity, asd->match.fwnode,
 613                                           MEDIA_PAD_FL_SOURCE);
 614         if (pad < 0) {
 615                 dev_err(csi2->dev, "Failed to find pad for %s\n", sd->name);
 616                 return pad;
 617         }
 618
 619         csi2->remote = sd;
 620         csi2->remote_pad = pad;
 621
 622         dev_dbg(csi2->dev, "Bound %s pad: %d\n", sd->name, pad);
 623
 624         return v4l2_create_fwnode_links_to_pad(sd, sink, 0);
 625 }
 626
 627 static void csi2_notify_unbind(struct v4l2_async_notifier *notifier,
 628                                struct v4l2_subdev *sd,
 629                                struct v4l2_async_subdev *asd)
 630 {
 631         struct csi2_dev *csi2 = notifier_to_dev(notifier);
 632
 633         csi2->remote = NULL;
 634 }
 635
 636 static const struct v4l2_async_notifier_operations csi2_notify_ops = {
 637         .bound = csi2_notify_bound,
 638         .unbind = csi2_notify_unbind,
 639 };
 640
 641 static int csi2_async_register(struct csi2_dev *csi2)
 642 {
 643         struct v4l2_fwnode_endpoint vep = {
 644                 .bus_type = V4L2_MBUS_CSI2_DPHY,
 645         };
 646         struct v4l2_async_subdev *asd;
 647         struct fwnode_handle *ep;
 648         int ret;
 649
 650         v4l2_async_notifier_init(&csi2->notifier);
 651
 652         ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi2->dev), 0, 0,
 653                                              FWNODE_GRAPH_ENDPOINT_NEXT);
 654         if (!ep)
 655                 return -ENOTCONN;
 656
 657         ret = v4l2_fwnode_endpoint_parse(ep, &vep);
 658         if (ret)
 659                 goto err_parse;
 660
 661         csi2->data_lanes = vep.bus.mipi_csi2.num_data_lanes;
 662
 663         dev_dbg(csi2->dev, "data lanes: %d\n", vep.bus.mipi_csi2.num_data_lanes);
 664         dev_dbg(csi2->dev, "flags: 0x%08x\n", vep.bus.mipi_csi2.flags);
 665
 666         asd = v4l2_async_notifier_add_fwnode_remote_subdev(
 667                 &csi2->notifier, ep, struct v4l2_async_subdev);
 668         fwnode_handle_put(ep);
 669
 670         if (IS_ERR(asd))
 671                 return PTR_ERR(asd);
 672
 673         csi2->notifier.ops = &csi2_notify_ops;
 674
 675         ret = v4l2_async_subdev_notifier_register(&csi2->sd,
 676                                                   &csi2->notifier);
 677         if (ret)
 678                 return ret;
 679
 680         return v4l2_async_register_subdev(&csi2->sd);
 681
 682 err_parse:
 683         fwnode_handle_put(ep);
 684         return ret;
 685 }
 686
 687 static int csi2_probe(struct platform_device *pdev)
 688 {
 689         struct csi2_dev *csi2;
 690         struct resource *res;
 691         int i, ret;
 692
 693         csi2 = devm_kzalloc(&pdev->dev, sizeof(*csi2), GFP_KERNEL);
 694         if (!csi2)
 695                 return -ENOMEM;
 696
 697         csi2->dev = &pdev->dev;
 698
 699         v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
 700         v4l2_set_subdevdata(&csi2->sd, &pdev->dev);
 701         csi2->sd.internal_ops = &csi2_internal_ops;
 702         csi2->sd.entity.ops = &csi2_entity_ops;
 703         csi2->sd.dev = &pdev->dev;
 704         csi2->sd.owner = THIS_MODULE;
 705         csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 706         strscpy(csi2->sd.name, DEVICE_NAME, sizeof(csi2->sd.name));
 707         csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 708         csi2->sd.grp_id = IMX_MEDIA_GRP_ID_CSI2;
 709
 710         for (i = 0; i < CSI2_NUM_PADS; i++) {
 711                 csi2->pad[i].flags = (i == CSI2_SINK_PAD) ?
 712                 MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
 713         }
 714
 715         ret = media_entity_pads_init(&csi2->sd.entity, CSI2_NUM_PADS,
 716                                      csi2->pad);
 717         if (ret)
 718                 return ret;
 719
 720         csi2->pllref_clk = devm_clk_get(&pdev->dev, "ref");
 721         if (IS_ERR(csi2->pllref_clk)) {
 722                 v4l2_err(&csi2->sd, "failed to get pll reference clock\n");
 723                 return PTR_ERR(csi2->pllref_clk);
 724         }
 725
 726         csi2->dphy_clk = devm_clk_get(&pdev->dev, "dphy");
 727         if (IS_ERR(csi2->dphy_clk)) {
 728                 v4l2_err(&csi2->sd, "failed to get dphy clock\n");
 729                 return PTR_ERR(csi2->dphy_clk);
 730         }
 731
 732         csi2->pix_clk = devm_clk_get(&pdev->dev, "pix");
 733         if (IS_ERR(csi2->pix_clk)) {
 734                 v4l2_err(&csi2->sd, "failed to get pixel clock\n");
 735                 return PTR_ERR(csi2->pix_clk);
 736         }
 737
 738         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 739         if (!res) {
 740                 v4l2_err(&csi2->sd, "failed to get platform resources\n");
 741                 return -ENODEV;
 742         }
 743
 744         csi2->base = devm_ioremap(&pdev->dev, res->start, PAGE_SIZE);
 745         if (!csi2->base)
 746                 return -ENOMEM;
 747
 748         mutex_init(&csi2->lock);
 749
 750         ret = clk_prepare_enable(csi2->pllref_clk);
 751         if (ret) {
 752                 v4l2_err(&csi2->sd, "failed to enable pllref_clk\n");
 753                 goto rmmutex;
 754         }
 755
 756         ret = clk_prepare_enable(csi2->dphy_clk);
 757         if (ret) {
 758                 v4l2_err(&csi2->sd, "failed to enable dphy_clk\n");
 759                 goto pllref_off;
 760         }
 761
 762         platform_set_drvdata(pdev, &csi2->sd);
 763
 764         ret = csi2_async_register(csi2);
 765         if (ret)
 766                 goto clean_notifier;
 767
 768         return 0;
 769
 770 clean_notifier:
 771         v4l2_async_notifier_unregister(&csi2->notifier);
 772         v4l2_async_notifier_cleanup(&csi2->notifier);
 773         clk_disable_unprepare(csi2->dphy_clk);
 774 pllref_off:
 775         clk_disable_unprepare(csi2->pllref_clk);
 776 rmmutex:
 777         mutex_destroy(&csi2->lock);
 778         return ret;
 779 }
 780
 781 static int csi2_remove(struct platform_device *pdev)
 782 {
 783         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
 784         struct csi2_dev *csi2 = sd_to_dev(sd);
 785
 786         v4l2_async_notifier_unregister(&csi2->notifier);
 787         v4l2_async_notifier_cleanup(&csi2->notifier);
 788         v4l2_async_unregister_subdev(sd);
 789         clk_disable_unprepare(csi2->dphy_clk);
 790         clk_disable_unprepare(csi2->pllref_clk);
 791         mutex_destroy(&csi2->lock);
 792         media_entity_cleanup(&sd->entity);
 793
 794         return 0;
 795 }
 796
 797 static const struct of_device_id csi2_dt_ids[] = {
 798         { .compatible = "fsl,imx6-mipi-csi2", },
 799         { /* sentinel */ }
 800 };
 801 MODULE_DEVICE_TABLE(of, csi2_dt_ids);
 802
 803 static struct platform_driver csi2_driver = {
 804         .driver = {
 805                 .name = DEVICE_NAME,
 806                 .of_match_table = csi2_dt_ids,
 807         },
 808         .probe = csi2_probe,
 809         .remove = csi2_remove,
 810 };
 811
 812 module_platform_driver(csi2_driver);
 813
 814 MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver");
 815 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
 816 MODULE_LICENSE("GPL");