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