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

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