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_get_try_format(&csi2->sd, 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 .init_cfg = imx_media_init_cfg, 623 .get_fmt = csi2_get_fmt, 624 .set_fmt = csi2_set_fmt, 625 }; 626 627 static const struct v4l2_subdev_ops csi2_subdev_ops = { 628 .core = &csi2_core_ops, 629 .video = &csi2_video_ops, 630 .pad = &csi2_pad_ops, 631 }; 632 633 static const struct v4l2_subdev_internal_ops csi2_internal_ops = { 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_subdev *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_subdev *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_subdev *asd; 680 struct fwnode_handle *ep; 681 int ret; 682 683 v4l2_async_nf_init(&csi2->notifier); 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_subdev); 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_subdev_nf_register(&csi2->sd, &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"); 847