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"); 746