drivers/media/platform/cadence/cdns-csi2rx.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Driver for Cadence MIPI-CSI2 RX Controller v1.3
   4  *
   5  * Copyright (C) 2017 Cadence Design Systems Inc.
   6  */
   7
   8 #include <linux/clk.h>
   9 #include <linux/delay.h>
  10 #include <linux/io.h>
  11 #include <linux/iopoll.h>
  12 #include <linux/module.h>
  13 #include <linux/of.h>
  14 #include <linux/of_graph.h>
  15 #include <linux/phy/phy.h>
  16 #include <linux/platform_device.h>
  17 #include <linux/reset.h>
  18 #include <linux/slab.h>
  19
  20 #include <media/v4l2-ctrls.h>
  21 #include <media/v4l2-device.h>
  22 #include <media/v4l2-fwnode.h>
  23 #include <media/v4l2-subdev.h>
  24
  25 #define CSI2RX_DEVICE_CFG_REG                   0x000
  26
  27 #define CSI2RX_SOFT_RESET_REG                   0x004
  28 #define CSI2RX_SOFT_RESET_PROTOCOL                      BIT(1)
  29 #define CSI2RX_SOFT_RESET_FRONT                         BIT(0)
  30
  31 #define CSI2RX_STATIC_CFG_REG                   0x008
  32 #define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane)       ((plane) << (16 + (llane) * 4))
  33 #define CSI2RX_STATIC_CFG_LANES_MASK                    GENMASK(11, 8)
  34
  35 #define CSI2RX_DPHY_LANE_CTRL_REG               0x40
  36 #define CSI2RX_DPHY_CL_RST                      BIT(16)
  37 #define CSI2RX_DPHY_DL_RST(i)                   BIT((i) + 12)
  38 #define CSI2RX_DPHY_CL_EN                       BIT(4)
  39 #define CSI2RX_DPHY_DL_EN(i)                    BIT(i)
  40
  41 #define CSI2RX_STREAM_BASE(n)           (((n) + 1) * 0x100)
  42
  43 #define CSI2RX_STREAM_CTRL_REG(n)               (CSI2RX_STREAM_BASE(n) + 0x000)
  44 #define CSI2RX_STREAM_CTRL_SOFT_RST                     BIT(4)
  45 #define CSI2RX_STREAM_CTRL_STOP                         BIT(1)
  46 #define CSI2RX_STREAM_CTRL_START                        BIT(0)
  47
  48 #define CSI2RX_STREAM_STATUS_REG(n)             (CSI2RX_STREAM_BASE(n) + 0x004)
  49 #define CSI2RX_STREAM_STATUS_RDY                        BIT(31)
  50
  51 #define CSI2RX_STREAM_DATA_CFG_REG(n)           (CSI2RX_STREAM_BASE(n) + 0x008)
  52 #define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n)             BIT((n) + 16)
  53
  54 #define CSI2RX_STREAM_CFG_REG(n)                (CSI2RX_STREAM_BASE(n) + 0x00c)
  55 #define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF           (1 << 8)
  56
  57 #define CSI2RX_LANES_MAX        4
  58 #define CSI2RX_STREAMS_MAX      4
  59
  60 enum csi2rx_pads {
  61         CSI2RX_PAD_SINK,
  62         CSI2RX_PAD_SOURCE_STREAM0,
  63         CSI2RX_PAD_SOURCE_STREAM1,
  64         CSI2RX_PAD_SOURCE_STREAM2,
  65         CSI2RX_PAD_SOURCE_STREAM3,
  66         CSI2RX_PAD_MAX,
  67 };
  68
  69 struct csi2rx_fmt {
  70         u32                             code;
  71         u8                              bpp;
  72 };
  73
  74 struct csi2rx_priv {
  75         struct device                   *dev;
  76         unsigned int                    count;
  77
  78         /*
  79          * Used to prevent race conditions between multiple,
  80          * concurrent calls to start and stop.
  81          */
  82         struct mutex                    lock;
  83
  84         void __iomem                    *base;
  85         struct clk                      *sys_clk;
  86         struct clk                      *p_clk;
  87         struct clk                      *pixel_clk[CSI2RX_STREAMS_MAX];
  88         struct reset_control            *sys_rst;
  89         struct reset_control            *p_rst;
  90         struct reset_control            *pixel_rst[CSI2RX_STREAMS_MAX];
  91         struct phy                      *dphy;
  92
  93         u8                              lanes[CSI2RX_LANES_MAX];
  94         u8                              num_lanes;
  95         u8                              max_lanes;
  96         u8                              max_streams;
  97         bool                            has_internal_dphy;
  98
  99         struct v4l2_subdev              subdev;
 100         struct v4l2_async_notifier      notifier;
 101         struct media_pad                pads[CSI2RX_PAD_MAX];
 102
 103         /* Remote source */
 104         struct v4l2_subdev              *source_subdev;
 105         int                             source_pad;
 106 };
 107
 108 static const struct csi2rx_fmt formats[] = {
 109         { .code = MEDIA_BUS_FMT_YUYV8_1X16, .bpp = 16, },
 110         { .code = MEDIA_BUS_FMT_UYVY8_1X16, .bpp = 16, },
 111         { .code = MEDIA_BUS_FMT_YVYU8_1X16, .bpp = 16, },
 112         { .code = MEDIA_BUS_FMT_VYUY8_1X16, .bpp = 16, },
 113         { .code = MEDIA_BUS_FMT_SBGGR8_1X8, .bpp = 8, },
 114         { .code = MEDIA_BUS_FMT_SGBRG8_1X8, .bpp = 8, },
 115         { .code = MEDIA_BUS_FMT_SGRBG8_1X8, .bpp = 8, },
 116         { .code = MEDIA_BUS_FMT_SRGGB8_1X8, .bpp = 8, },
 117         { .code = MEDIA_BUS_FMT_SBGGR10_1X10, .bpp = 10, },
 118         { .code = MEDIA_BUS_FMT_SGBRG10_1X10, .bpp = 10, },
 119         { .code = MEDIA_BUS_FMT_SGRBG10_1X10, .bpp = 10, },
 120         { .code = MEDIA_BUS_FMT_SRGGB10_1X10, .bpp = 10, },
 121 };
 122
 123 static const struct csi2rx_fmt *csi2rx_get_fmt_by_code(u32 code)
 124 {
 125         unsigned int i;
 126
 127         for (i = 0; i < ARRAY_SIZE(formats); i++)
 128                 if (formats[i].code == code)
 129                         return &formats[i];
 130
 131         return NULL;
 132 }
 133
 134 static inline
 135 struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev)
 136 {
 137         return container_of(subdev, struct csi2rx_priv, subdev);
 138 }
 139
 140 static void csi2rx_reset(struct csi2rx_priv *csi2rx)
 141 {
 142         unsigned int i;
 143
 144         /* Reset module */
 145         writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT,
 146                csi2rx->base + CSI2RX_SOFT_RESET_REG);
 147         /* Reset individual streams. */
 148         for (i = 0; i < csi2rx->max_streams; i++) {
 149                 writel(CSI2RX_STREAM_CTRL_SOFT_RST,
 150                        csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
 151         }
 152
 153         usleep_range(10, 20);
 154
 155         /* Clear resets */
 156         writel(0, csi2rx->base + CSI2RX_SOFT_RESET_REG);
 157         for (i = 0; i < csi2rx->max_streams; i++)
 158                 writel(0, csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
 159 }
 160
 161 static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
 162 {
 163         union phy_configure_opts opts = { };
 164         struct phy_configure_opts_mipi_dphy *cfg = &opts.mipi_dphy;
 165         struct v4l2_subdev_format sd_fmt = {
 166                 .which  = V4L2_SUBDEV_FORMAT_ACTIVE,
 167                 .pad    = CSI2RX_PAD_SINK,
 168         };
 169         const struct csi2rx_fmt *fmt;
 170         s64 link_freq;
 171         int ret;
 172
 173         ret = v4l2_subdev_call_state_active(&csi2rx->subdev, pad, get_fmt,
 174                                             &sd_fmt);
 175         if (ret < 0)
 176                 return ret;
 177
 178         fmt = csi2rx_get_fmt_by_code(sd_fmt.format.code);
 179
 180         link_freq = v4l2_get_link_freq(csi2rx->source_subdev->ctrl_handler,
 181                                        fmt->bpp, 2 * csi2rx->num_lanes);
 182         if (link_freq < 0)
 183                 return link_freq;
 184
 185         ret = phy_mipi_dphy_get_default_config_for_hsclk(link_freq,
 186                                                          csi2rx->num_lanes, cfg);
 187         if (ret)
 188                 return ret;
 189
 190         ret = phy_power_on(csi2rx->dphy);
 191         if (ret)
 192                 return ret;
 193
 194         ret = phy_configure(csi2rx->dphy, &opts);
 195         if (ret) {
 196                 phy_power_off(csi2rx->dphy);
 197                 return ret;
 198         }
 199
 200         return 0;
 201 }
 202
 203 static int csi2rx_start(struct csi2rx_priv *csi2rx)
 204 {
 205         unsigned int i;
 206         unsigned long lanes_used = 0;
 207         u32 reg;
 208         int ret;
 209
 210         ret = clk_prepare_enable(csi2rx->p_clk);
 211         if (ret)
 212                 return ret;
 213
 214         reset_control_deassert(csi2rx->p_rst);
 215         csi2rx_reset(csi2rx);
 216
 217         reg = csi2rx->num_lanes << 8;
 218         for (i = 0; i < csi2rx->num_lanes; i++) {
 219                 reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csi2rx->lanes[i]);
 220                 set_bit(csi2rx->lanes[i], &lanes_used);
 221         }
 222
 223         /*
 224          * Even the unused lanes need to be mapped. In order to avoid
 225          * to map twice to the same physical lane, keep the lanes used
 226          * in the previous loop, and only map unused physical lanes to
 227          * the rest of our logical lanes.
 228          */
 229         for (i = csi2rx->num_lanes; i < csi2rx->max_lanes; i++) {
 230                 unsigned int idx = find_first_zero_bit(&lanes_used,
 231                                                        csi2rx->max_lanes);
 232                 set_bit(idx, &lanes_used);
 233                 reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, i + 1);
 234         }
 235
 236         writel(reg, csi2rx->base + CSI2RX_STATIC_CFG_REG);
 237
 238         ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true);
 239         if (ret)
 240                 goto err_disable_pclk;
 241
 242         /* Enable DPHY clk and data lanes. */
 243         if (csi2rx->dphy) {
 244                 reg = CSI2RX_DPHY_CL_EN | CSI2RX_DPHY_CL_RST;
 245                 for (i = 0; i < csi2rx->num_lanes; i++) {
 246                         reg |= CSI2RX_DPHY_DL_EN(csi2rx->lanes[i] - 1);
 247                         reg |= CSI2RX_DPHY_DL_RST(csi2rx->lanes[i] - 1);
 248                 }
 249
 250                 writel(reg, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
 251         }
 252
 253         /*
 254          * Create a static mapping between the CSI virtual channels
 255          * and the output stream.
 256          *
 257          * This should be enhanced, but v4l2 lacks the support for
 258          * changing that mapping dynamically.
 259          *
 260          * We also cannot enable and disable independent streams here,
 261          * hence the reference counting.
 262          */
 263         for (i = 0; i < csi2rx->max_streams; i++) {
 264                 ret = clk_prepare_enable(csi2rx->pixel_clk[i]);
 265                 if (ret)
 266                         goto err_disable_pixclk;
 267
 268                 reset_control_deassert(csi2rx->pixel_rst[i]);
 269
 270                 writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF,
 271                        csi2rx->base + CSI2RX_STREAM_CFG_REG(i));
 272
 273                 /*
 274                  * Enable one virtual channel. When multiple virtual channels
 275                  * are supported this will have to be changed.
 276                  */
 277                 writel(CSI2RX_STREAM_DATA_CFG_VC_SELECT(0),
 278                        csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i));
 279
 280                 writel(CSI2RX_STREAM_CTRL_START,
 281                        csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
 282         }
 283
 284         ret = clk_prepare_enable(csi2rx->sys_clk);
 285         if (ret)
 286                 goto err_disable_pixclk;
 287
 288         reset_control_deassert(csi2rx->sys_rst);
 289
 290         if (csi2rx->dphy) {
 291                 ret = csi2rx_configure_ext_dphy(csi2rx);
 292                 if (ret) {
 293                         dev_err(csi2rx->dev,
 294                                 "Failed to configure external DPHY: %d\n", ret);
 295                         goto err_disable_sysclk;
 296                 }
 297         }
 298
 299         clk_disable_unprepare(csi2rx->p_clk);
 300
 301         return 0;
 302
 303 err_disable_sysclk:
 304         clk_disable_unprepare(csi2rx->sys_clk);
 305 err_disable_pixclk:
 306         for (; i > 0; i--) {
 307                 reset_control_assert(csi2rx->pixel_rst[i - 1]);
 308                 clk_disable_unprepare(csi2rx->pixel_clk[i - 1]);
 309         }
 310
 311 err_disable_pclk:
 312         clk_disable_unprepare(csi2rx->p_clk);
 313
 314         return ret;
 315 }
 316
 317 static void csi2rx_stop(struct csi2rx_priv *csi2rx)
 318 {
 319         unsigned int i;
 320         u32 val;
 321         int ret;
 322
 323         clk_prepare_enable(csi2rx->p_clk);
 324         reset_control_assert(csi2rx->sys_rst);
 325         clk_disable_unprepare(csi2rx->sys_clk);
 326
 327         for (i = 0; i < csi2rx->max_streams; i++) {
 328                 writel(CSI2RX_STREAM_CTRL_STOP,
 329                        csi2rx->base + CSI2RX_STREAM_CTRL_REG(i));
 330
 331                 ret = readl_relaxed_poll_timeout(csi2rx->base +
 332                                                  CSI2RX_STREAM_STATUS_REG(i),
 333                                                  val,
 334                                                  !(val & CSI2RX_STREAM_STATUS_RDY),
 335                                                  10, 10000);
 336                 if (ret)
 337                         dev_warn(csi2rx->dev,
 338                                  "Failed to stop streaming on pad%u\n", i);
 339
 340                 reset_control_assert(csi2rx->pixel_rst[i]);
 341                 clk_disable_unprepare(csi2rx->pixel_clk[i]);
 342         }
 343
 344         reset_control_assert(csi2rx->p_rst);
 345         clk_disable_unprepare(csi2rx->p_clk);
 346
 347         if (v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, false))
 348                 dev_warn(csi2rx->dev, "Couldn't disable our subdev\n");
 349
 350         if (csi2rx->dphy) {
 351                 writel(0, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
 352
 353                 if (phy_power_off(csi2rx->dphy))
 354                         dev_warn(csi2rx->dev, "Couldn't power off DPHY\n");
 355         }
 356 }
 357
 358 static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable)
 359 {
 360         struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
 361         int ret = 0;
 362
 363         mutex_lock(&csi2rx->lock);
 364
 365         if (enable) {
 366                 /*
 367                  * If we're not the first users, there's no need to
 368                  * enable the whole controller.
 369                  */
 370                 if (!csi2rx->count) {
 371                         ret = csi2rx_start(csi2rx);
 372                         if (ret)
 373                                 goto out;
 374                 }
 375
 376                 csi2rx->count++;
 377         } else {
 378                 csi2rx->count--;
 379
 380                 /*
 381                  * Let the last user turn off the lights.
 382                  */
 383                 if (!csi2rx->count)
 384                         csi2rx_stop(csi2rx);
 385         }
 386
 387 out:
 388         mutex_unlock(&csi2rx->lock);
 389         return ret;
 390 }
 391
 392 static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
 393                           struct v4l2_subdev_state *state,
 394                           struct v4l2_subdev_format *format)
 395 {
 396         struct v4l2_mbus_framefmt *fmt;
 397         unsigned int i;
 398
 399         /* No transcoding, source and sink formats must match. */
 400         if (format->pad != CSI2RX_PAD_SINK)
 401                 return v4l2_subdev_get_fmt(subdev, state, format);
 402
 403         if (!csi2rx_get_fmt_by_code(format->format.code))
 404                 format->format.code = formats[0].code;
 405
 406         format->format.field = V4L2_FIELD_NONE;
 407
 408         /* Set sink format */
 409         fmt = v4l2_subdev_state_get_format(state, format->pad);
 410         *fmt = format->format;
 411
 412         /* Propagate to source formats */
 413         for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
 414                 fmt = v4l2_subdev_state_get_format(state, i);
 415                 *fmt = format->format;
 416         }
 417
 418         return 0;
 419 }
 420
 421 static int csi2rx_init_state(struct v4l2_subdev *subdev,
 422                              struct v4l2_subdev_state *state)
 423 {
 424         struct v4l2_subdev_format format = {
 425                 .pad = CSI2RX_PAD_SINK,
 426                 .format = {
 427                         .width = 640,
 428                         .height = 480,
 429                         .code = MEDIA_BUS_FMT_UYVY8_1X16,
 430                         .field = V4L2_FIELD_NONE,
 431                         .colorspace = V4L2_COLORSPACE_SRGB,
 432                         .ycbcr_enc = V4L2_YCBCR_ENC_601,
 433                         .quantization = V4L2_QUANTIZATION_LIM_RANGE,
 434                         .xfer_func = V4L2_XFER_FUNC_SRGB,
 435                 },
 436         };
 437
 438         return csi2rx_set_fmt(subdev, state, &format);
 439 }
 440
 441 static const struct v4l2_subdev_pad_ops csi2rx_pad_ops = {
 442         .get_fmt        = v4l2_subdev_get_fmt,
 443         .set_fmt        = csi2rx_set_fmt,
 444 };
 445
 446 static const struct v4l2_subdev_video_ops csi2rx_video_ops = {
 447         .s_stream       = csi2rx_s_stream,
 448 };
 449
 450 static const struct v4l2_subdev_ops csi2rx_subdev_ops = {
 451         .video          = &csi2rx_video_ops,
 452         .pad            = &csi2rx_pad_ops,
 453 };
 454
 455 static const struct v4l2_subdev_internal_ops csi2rx_internal_ops = {
 456         .init_state     = csi2rx_init_state,
 457 };
 458
 459 static const struct media_entity_operations csi2rx_media_ops = {
 460         .link_validate = v4l2_subdev_link_validate,
 461 };
 462
 463 static int csi2rx_async_bound(struct v4l2_async_notifier *notifier,
 464                               struct v4l2_subdev *s_subdev,
 465                               struct v4l2_async_connection *asd)
 466 {
 467         struct v4l2_subdev *subdev = notifier->sd;
 468         struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
 469
 470         csi2rx->source_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
 471                                                          s_subdev->fwnode,
 472                                                          MEDIA_PAD_FL_SOURCE);
 473         if (csi2rx->source_pad < 0) {
 474                 dev_err(csi2rx->dev, "Couldn't find output pad for subdev %s\n",
 475                         s_subdev->name);
 476                 return csi2rx->source_pad;
 477         }
 478
 479         csi2rx->source_subdev = s_subdev;
 480
 481         dev_dbg(csi2rx->dev, "Bound %s pad: %d\n", s_subdev->name,
 482                 csi2rx->source_pad);
 483
 484         return media_create_pad_link(&csi2rx->source_subdev->entity,
 485                                      csi2rx->source_pad,
 486                                      &csi2rx->subdev.entity, 0,
 487                                      MEDIA_LNK_FL_ENABLED |
 488                                      MEDIA_LNK_FL_IMMUTABLE);
 489 }
 490
 491 static const struct v4l2_async_notifier_operations csi2rx_notifier_ops = {
 492         .bound          = csi2rx_async_bound,
 493 };
 494
 495 static int csi2rx_get_resources(struct csi2rx_priv *csi2rx,
 496                                 struct platform_device *pdev)
 497 {
 498         unsigned char i;
 499         u32 dev_cfg;
 500         int ret;
 501
 502         csi2rx->base = devm_platform_ioremap_resource(pdev, 0);
 503         if (IS_ERR(csi2rx->base))
 504                 return PTR_ERR(csi2rx->base);
 505
 506         csi2rx->sys_clk = devm_clk_get(&pdev->dev, "sys_clk");
 507         if (IS_ERR(csi2rx->sys_clk)) {
 508                 dev_err(&pdev->dev, "Couldn't get sys clock\n");
 509                 return PTR_ERR(csi2rx->sys_clk);
 510         }
 511
 512         csi2rx->p_clk = devm_clk_get(&pdev->dev, "p_clk");
 513         if (IS_ERR(csi2rx->p_clk)) {
 514                 dev_err(&pdev->dev, "Couldn't get P clock\n");
 515                 return PTR_ERR(csi2rx->p_clk);
 516         }
 517
 518         csi2rx->sys_rst = devm_reset_control_get_optional_exclusive(&pdev->dev,
 519                                                                     "sys");
 520         if (IS_ERR(csi2rx->sys_rst))
 521                 return PTR_ERR(csi2rx->sys_rst);
 522
 523         csi2rx->p_rst = devm_reset_control_get_optional_exclusive(&pdev->dev,
 524                                                                   "reg_bank");
 525         if (IS_ERR(csi2rx->p_rst))
 526                 return PTR_ERR(csi2rx->p_rst);
 527
 528         csi2rx->dphy = devm_phy_optional_get(&pdev->dev, "dphy");
 529         if (IS_ERR(csi2rx->dphy)) {
 530                 dev_err(&pdev->dev, "Couldn't get external D-PHY\n");
 531                 return PTR_ERR(csi2rx->dphy);
 532         }
 533
 534         ret = clk_prepare_enable(csi2rx->p_clk);
 535         if (ret) {
 536                 dev_err(&pdev->dev, "Couldn't prepare and enable P clock\n");
 537                 return ret;
 538         }
 539
 540         dev_cfg = readl(csi2rx->base + CSI2RX_DEVICE_CFG_REG);
 541         clk_disable_unprepare(csi2rx->p_clk);
 542
 543         csi2rx->max_lanes = dev_cfg & 7;
 544         if (csi2rx->max_lanes > CSI2RX_LANES_MAX) {
 545                 dev_err(&pdev->dev, "Invalid number of lanes: %u\n",
 546                         csi2rx->max_lanes);
 547                 return -EINVAL;
 548         }
 549
 550         csi2rx->max_streams = (dev_cfg >> 4) & 7;
 551         if (csi2rx->max_streams > CSI2RX_STREAMS_MAX) {
 552                 dev_err(&pdev->dev, "Invalid number of streams: %u\n",
 553                         csi2rx->max_streams);
 554                 return -EINVAL;
 555         }
 556
 557         csi2rx->has_internal_dphy = dev_cfg & BIT(3) ? true : false;
 558
 559         /*
 560          * FIXME: Once we'll have internal D-PHY support, the check
 561          * will need to be removed.
 562          */
 563         if (!csi2rx->dphy && csi2rx->has_internal_dphy) {
 564                 dev_err(&pdev->dev, "Internal D-PHY not supported yet\n");
 565                 return -EINVAL;
 566         }
 567
 568         for (i = 0; i < csi2rx->max_streams; i++) {
 569                 char name[16];
 570
 571                 snprintf(name, sizeof(name), "pixel_if%u_clk", i);
 572                 csi2rx->pixel_clk[i] = devm_clk_get(&pdev->dev, name);
 573                 if (IS_ERR(csi2rx->pixel_clk[i])) {
 574                         dev_err(&pdev->dev, "Couldn't get clock %s\n", name);
 575                         return PTR_ERR(csi2rx->pixel_clk[i]);
 576                 }
 577
 578                 snprintf(name, sizeof(name), "pixel_if%u", i);
 579                 csi2rx->pixel_rst[i] =
 580                         devm_reset_control_get_optional_exclusive(&pdev->dev,
 581                                                                   name);
 582                 if (IS_ERR(csi2rx->pixel_rst[i]))
 583                         return PTR_ERR(csi2rx->pixel_rst[i]);
 584         }
 585
 586         return 0;
 587 }
 588
 589 static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx)
 590 {
 591         struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 };
 592         struct v4l2_async_connection *asd;
 593         struct fwnode_handle *fwh;
 594         struct device_node *ep;
 595         int ret;
 596
 597         ep = of_graph_get_endpoint_by_regs(csi2rx->dev->of_node, 0, 0);
 598         if (!ep)
 599                 return -EINVAL;
 600
 601         fwh = of_fwnode_handle(ep);
 602         ret = v4l2_fwnode_endpoint_parse(fwh, &v4l2_ep);
 603         if (ret) {
 604                 dev_err(csi2rx->dev, "Could not parse v4l2 endpoint\n");
 605                 of_node_put(ep);
 606                 return ret;
 607         }
 608
 609         if (v4l2_ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
 610                 dev_err(csi2rx->dev, "Unsupported media bus type: 0x%x\n",
 611                         v4l2_ep.bus_type);
 612                 of_node_put(ep);
 613                 return -EINVAL;
 614         }
 615
 616         memcpy(csi2rx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
 617                sizeof(csi2rx->lanes));
 618         csi2rx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
 619         if (csi2rx->num_lanes > csi2rx->max_lanes) {
 620                 dev_err(csi2rx->dev, "Unsupported number of data-lanes: %d\n",
 621                         csi2rx->num_lanes);
 622                 of_node_put(ep);
 623                 return -EINVAL;
 624         }
 625
 626         v4l2_async_subdev_nf_init(&csi2rx->notifier, &csi2rx->subdev);
 627
 628         asd = v4l2_async_nf_add_fwnode_remote(&csi2rx->notifier, fwh,
 629                                               struct v4l2_async_connection);
 630         of_node_put(ep);
 631         if (IS_ERR(asd)) {
 632                 v4l2_async_nf_cleanup(&csi2rx->notifier);
 633                 return PTR_ERR(asd);
 634         }
 635
 636         csi2rx->notifier.ops = &csi2rx_notifier_ops;
 637
 638         ret = v4l2_async_nf_register(&csi2rx->notifier);
 639         if (ret)
 640                 v4l2_async_nf_cleanup(&csi2rx->notifier);
 641
 642         return ret;
 643 }
 644
 645 static int csi2rx_probe(struct platform_device *pdev)
 646 {
 647         struct csi2rx_priv *csi2rx;
 648         unsigned int i;
 649         int ret;
 650
 651         csi2rx = kzalloc(sizeof(*csi2rx), GFP_KERNEL);
 652         if (!csi2rx)
 653                 return -ENOMEM;
 654         platform_set_drvdata(pdev, csi2rx);
 655         csi2rx->dev = &pdev->dev;
 656         mutex_init(&csi2rx->lock);
 657
 658         ret = csi2rx_get_resources(csi2rx, pdev);
 659         if (ret)
 660                 goto err_free_priv;
 661
 662         ret = csi2rx_parse_dt(csi2rx);
 663         if (ret)
 664                 goto err_free_priv;
 665
 666         csi2rx->subdev.owner = THIS_MODULE;
 667         csi2rx->subdev.dev = &pdev->dev;
 668         v4l2_subdev_init(&csi2rx->subdev, &csi2rx_subdev_ops);
 669         csi2rx->subdev.internal_ops = &csi2rx_internal_ops;
 670         v4l2_set_subdevdata(&csi2rx->subdev, &pdev->dev);
 671         snprintf(csi2rx->subdev.name, sizeof(csi2rx->subdev.name),
 672                  "%s.%s", KBUILD_MODNAME, dev_name(&pdev->dev));
 673
 674         /* Create our media pads */
 675         csi2rx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 676         csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 677         for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++)
 678                 csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE;
 679         csi2rx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 680         csi2rx->subdev.entity.ops = &csi2rx_media_ops;
 681
 682         ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX,
 683                                      csi2rx->pads);
 684         if (ret)
 685                 goto err_cleanup;
 686
 687         ret = v4l2_subdev_init_finalize(&csi2rx->subdev);
 688         if (ret)
 689                 goto err_cleanup;
 690
 691         ret = v4l2_async_register_subdev(&csi2rx->subdev);
 692         if (ret < 0)
 693                 goto err_free_state;
 694
 695         dev_info(&pdev->dev,
 696                  "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n",
 697                  csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams,
 698                  csi2rx->dphy ? "external" :
 699                  csi2rx->has_internal_dphy ? "internal" : "no");
 700
 701         return 0;
 702
 703 err_free_state:
 704         v4l2_subdev_cleanup(&csi2rx->subdev);
 705 err_cleanup:
 706         v4l2_async_nf_unregister(&csi2rx->notifier);
 707         v4l2_async_nf_cleanup(&csi2rx->notifier);
 708         media_entity_cleanup(&csi2rx->subdev.entity);
 709 err_free_priv:
 710         kfree(csi2rx);
 711         return ret;
 712 }
 713
 714 static void csi2rx_remove(struct platform_device *pdev)
 715 {
 716         struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev);
 717
 718         v4l2_async_nf_unregister(&csi2rx->notifier);
 719         v4l2_async_nf_cleanup(&csi2rx->notifier);
 720         v4l2_async_unregister_subdev(&csi2rx->subdev);
 721         v4l2_subdev_cleanup(&csi2rx->subdev);
 722         media_entity_cleanup(&csi2rx->subdev.entity);
 723         kfree(csi2rx);
 724 }
 725
 726 static const struct of_device_id csi2rx_of_table[] = {
 727         { .compatible = "starfive,jh7110-csi2rx" },
 728         { .compatible = "cdns,csi2rx" },
 729         { },
 730 };
 731 MODULE_DEVICE_TABLE(of, csi2rx_of_table);
 732
 733 static struct platform_driver csi2rx_driver = {
 734         .probe  = csi2rx_probe,
 735         .remove_new = csi2rx_remove,
 736
 737         .driver = {
 738                 .name           = "cdns-csi2rx",
 739                 .of_match_table = csi2rx_of_table,
 740         },
 741 };
 742 module_platform_driver(csi2rx_driver);
 743 MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>");
 744 MODULE_DESCRIPTION("Cadence CSI2-RX controller");
 745 MODULE_LICENSE("GPL");