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