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