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