1 /* 2 * MIPI CSI-2 Receiver Subdev for Freescale i.MX6 SOC. 3 * 4 * Copyright (c) 2012-2017 Mentor Graphics Inc. 5 * 6 * This program is free software; you can redistribute it and/or modify 7 * it under the terms of the GNU General Public License as published by 8 * the Free Software Foundation; either version 2 of the License, or 9 * (at your option) any later version. 10 */ 11 #include <linux/clk.h> 12 #include <linux/interrupt.h> 13 #include <linux/io.h> 14 #include <linux/iopoll.h> 15 #include <linux/irq.h> 16 #include <linux/module.h> 17 #include <linux/of_graph.h> 18 #include <linux/platform_device.h> 19 #include <media/v4l2-device.h> 20 #include <media/v4l2-fwnode.h> 21 #include <media/v4l2-subdev.h> 22 #include "imx-media.h" 23 24 /* 25 * there must be 5 pads: 1 input pad from sensor, and 26 * the 4 virtual channel output pads 27 */ 28 #define CSI2_SINK_PAD 0 29 #define CSI2_NUM_SINK_PADS 1 30 #define CSI2_NUM_SRC_PADS 4 31 #define CSI2_NUM_PADS 5 32 33 /* 34 * The default maximum bit-rate per lane in Mbps, if the 35 * source subdev does not provide V4L2_CID_LINK_FREQ. 36 */ 37 #define CSI2_DEFAULT_MAX_MBPS 849 38 39 struct csi2_dev { 40 struct device *dev; 41 struct v4l2_subdev sd; 42 struct media_pad pad[CSI2_NUM_PADS]; 43 struct clk *dphy_clk; 44 struct clk *pllref_clk; 45 struct clk *pix_clk; /* what is this? */ 46 void __iomem *base; 47 struct v4l2_fwnode_bus_mipi_csi2 bus; 48 49 /* lock to protect all members below */ 50 struct mutex lock; 51 52 struct v4l2_mbus_framefmt format_mbus; 53 54 int stream_count; 55 struct v4l2_subdev *src_sd; 56 bool sink_linked[CSI2_NUM_SRC_PADS]; 57 }; 58 59 #define DEVICE_NAME "imx6-mipi-csi2" 60 61 /* Register offsets */ 62 #define CSI2_VERSION 0x000 63 #define CSI2_N_LANES 0x004 64 #define CSI2_PHY_SHUTDOWNZ 0x008 65 #define CSI2_DPHY_RSTZ 0x00c 66 #define CSI2_RESETN 0x010 67 #define CSI2_PHY_STATE 0x014 68 #define PHY_STOPSTATEDATA_BIT 4 69 #define PHY_STOPSTATEDATA(n) BIT(PHY_STOPSTATEDATA_BIT + (n)) 70 #define PHY_RXCLKACTIVEHS BIT(8) 71 #define PHY_RXULPSCLKNOT BIT(9) 72 #define PHY_STOPSTATECLK BIT(10) 73 #define CSI2_DATA_IDS_1 0x018 74 #define CSI2_DATA_IDS_2 0x01c 75 #define CSI2_ERR1 0x020 76 #define CSI2_ERR2 0x024 77 #define CSI2_MSK1 0x028 78 #define CSI2_MSK2 0x02c 79 #define CSI2_PHY_TST_CTRL0 0x030 80 #define PHY_TESTCLR BIT(0) 81 #define PHY_TESTCLK BIT(1) 82 #define CSI2_PHY_TST_CTRL1 0x034 83 #define PHY_TESTEN BIT(16) 84 /* 85 * i.MX CSI2IPU Gasket registers follow. The CSI2IPU gasket is 86 * not part of the MIPI CSI-2 core, but its registers fall in the 87 * same register map range. 88 */ 89 #define CSI2IPU_GASKET 0xf00 90 #define CSI2IPU_YUV422_YUYV BIT(2) 91 92 static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev) 93 { 94 return container_of(sdev, struct csi2_dev, sd); 95 } 96 97 /* 98 * The required sequence of MIPI CSI-2 startup as specified in the i.MX6 99 * reference manual is as follows: 100 * 101 * 1. Deassert presetn signal (global reset). 102 * It's not clear what this "global reset" signal is (maybe APB 103 * global reset), but in any case this step would be probably 104 * be carried out during driver load in csi2_probe(). 105 * 106 * 2. Configure MIPI Camera Sensor to put all Tx lanes in LP-11 state. 107 * This must be carried out by the MIPI sensor's s_power(ON) subdev 108 * op. 109 * 110 * 3. D-PHY initialization. 111 * 4. CSI2 Controller programming (Set N_LANES, deassert PHY_SHUTDOWNZ, 112 * deassert PHY_RSTZ, deassert CSI2_RESETN). 113 * 5. Read the PHY status register (PHY_STATE) to confirm that all data and 114 * clock lanes of the D-PHY are in LP-11 state. 115 * 6. Configure the MIPI Camera Sensor to start transmitting a clock on the 116 * D-PHY clock lane. 117 * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE) 118 * to confirm that the D-PHY is receiving a clock on the D-PHY clock lane. 119 * 120 * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step 121 * 6 is accomplished by calling the source subdev's s_stream(ON) between 122 * steps 5 and 7. 123 */ 124 125 static void csi2_enable(struct csi2_dev *csi2, bool enable) 126 { 127 if (enable) { 128 writel(0x1, csi2->base + CSI2_PHY_SHUTDOWNZ); 129 writel(0x1, csi2->base + CSI2_DPHY_RSTZ); 130 writel(0x1, csi2->base + CSI2_RESETN); 131 } else { 132 writel(0x0, csi2->base + CSI2_PHY_SHUTDOWNZ); 133 writel(0x0, csi2->base + CSI2_DPHY_RSTZ); 134 writel(0x0, csi2->base + CSI2_RESETN); 135 } 136 } 137 138 static void csi2_set_lanes(struct csi2_dev *csi2) 139 { 140 int lanes = csi2->bus.num_data_lanes; 141 142 writel(lanes - 1, csi2->base + CSI2_N_LANES); 143 } 144 145 static void dw_mipi_csi2_phy_write(struct csi2_dev *csi2, 146 u32 test_code, u32 test_data) 147 { 148 /* Clear PHY test interface */ 149 writel(PHY_TESTCLR, csi2->base + CSI2_PHY_TST_CTRL0); 150 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL1); 151 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0); 152 153 /* Raise test interface strobe signal */ 154 writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0); 155 156 /* Configure address write on falling edge and lower strobe signal */ 157 writel(PHY_TESTEN | test_code, csi2->base + CSI2_PHY_TST_CTRL1); 158 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0); 159 160 /* Configure data write on rising edge and raise strobe signal */ 161 writel(test_data, csi2->base + CSI2_PHY_TST_CTRL1); 162 writel(PHY_TESTCLK, csi2->base + CSI2_PHY_TST_CTRL0); 163 164 /* Clear strobe signal */ 165 writel(0x0, csi2->base + CSI2_PHY_TST_CTRL0); 166 } 167 168 /* 169 * This table is based on the table documented at 170 * https://community.nxp.com/docs/DOC-94312. It assumes 171 * a 27MHz D-PHY pll reference clock. 172 */ 173 static const struct { 174 u32 max_mbps; 175 u32 hsfreqrange_sel; 176 } hsfreq_map[] = { 177 { 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02}, 178 {140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24}, 179 {200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46}, 180 {270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a}, 181 {400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e}, 182 {600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12}, 183 {800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54}, 184 {1000, 0x74}, 185 }; 186 187 static int max_mbps_to_hsfreqrange_sel(u32 max_mbps) 188 { 189 int i; 190 191 for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++) 192 if (hsfreq_map[i].max_mbps > max_mbps) 193 return hsfreq_map[i].hsfreqrange_sel; 194 195 return -EINVAL; 196 } 197 198 static int csi2_dphy_init(struct csi2_dev *csi2) 199 { 200 struct v4l2_ctrl *ctrl; 201 u32 mbps_per_lane; 202 int sel; 203 204 ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler, 205 V4L2_CID_LINK_FREQ); 206 if (!ctrl) 207 mbps_per_lane = CSI2_DEFAULT_MAX_MBPS; 208 else 209 mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val], 210 USEC_PER_SEC); 211 212 sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane); 213 if (sel < 0) 214 return sel; 215 216 dw_mipi_csi2_phy_write(csi2, 0x44, sel); 217 218 return 0; 219 } 220 221 /* 222 * Waits for ultra-low-power state on D-PHY clock lane. This is currently 223 * unused and may not be needed at all, but keep around just in case. 224 */ 225 static int __maybe_unused csi2_dphy_wait_ulp(struct csi2_dev *csi2) 226 { 227 u32 reg; 228 int ret; 229 230 /* wait for ULP on clock lane */ 231 ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, 232 !(reg & PHY_RXULPSCLKNOT), 0, 500000); 233 if (ret) { 234 v4l2_err(&csi2->sd, "ULP timeout, phy_state = 0x%08x\n", reg); 235 return ret; 236 } 237 238 /* wait until no errors on bus */ 239 ret = readl_poll_timeout(csi2->base + CSI2_ERR1, reg, 240 reg == 0x0, 0, 500000); 241 if (ret) { 242 v4l2_err(&csi2->sd, "stable bus timeout, err1 = 0x%08x\n", reg); 243 return ret; 244 } 245 246 return 0; 247 } 248 249 /* Waits for low-power LP-11 state on data and clock lanes. */ 250 static int csi2_dphy_wait_stopstate(struct csi2_dev *csi2) 251 { 252 u32 mask, reg; 253 int ret; 254 255 mask = PHY_STOPSTATECLK | (((1 << csi2->bus.num_data_lanes) - 1) << 256 PHY_STOPSTATEDATA_BIT); 257 258 ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, 259 (reg & mask) == mask, 0, 500000); 260 if (ret) { 261 v4l2_err(&csi2->sd, "LP-11 timeout, phy_state = 0x%08x\n", reg); 262 return ret; 263 } 264 265 return 0; 266 } 267 268 /* Wait for active clock on the clock lane. */ 269 static int csi2_dphy_wait_clock_lane(struct csi2_dev *csi2) 270 { 271 u32 reg; 272 int ret; 273 274 ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, 275 (reg & PHY_RXCLKACTIVEHS), 0, 500000); 276 if (ret) { 277 v4l2_err(&csi2->sd, "clock lane timeout, phy_state = 0x%08x\n", 278 reg); 279 return ret; 280 } 281 282 return 0; 283 } 284 285 /* Setup the i.MX CSI2IPU Gasket */ 286 static void csi2ipu_gasket_init(struct csi2_dev *csi2) 287 { 288 u32 reg = 0; 289 290 switch (csi2->format_mbus.code) { 291 case MEDIA_BUS_FMT_YUYV8_2X8: 292 case MEDIA_BUS_FMT_YUYV8_1X16: 293 reg = CSI2IPU_YUV422_YUYV; 294 break; 295 default: 296 break; 297 } 298 299 writel(reg, csi2->base + CSI2IPU_GASKET); 300 } 301 302 static int csi2_start(struct csi2_dev *csi2) 303 { 304 int ret; 305 306 ret = clk_prepare_enable(csi2->pix_clk); 307 if (ret) 308 return ret; 309 310 /* setup the gasket */ 311 csi2ipu_gasket_init(csi2); 312 313 /* Step 3 */ 314 ret = csi2_dphy_init(csi2); 315 if (ret) 316 goto err_disable_clk; 317 318 /* Step 4 */ 319 csi2_set_lanes(csi2); 320 csi2_enable(csi2, true); 321 322 /* Step 5 */ 323 ret = csi2_dphy_wait_stopstate(csi2); 324 if (ret) 325 goto err_assert_reset; 326 327 /* Step 6 */ 328 ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1); 329 ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0; 330 if (ret) 331 goto err_assert_reset; 332 333 /* Step 7 */ 334 ret = csi2_dphy_wait_clock_lane(csi2); 335 if (ret) 336 goto err_stop_upstream; 337 338 return 0; 339 340 err_stop_upstream: 341 v4l2_subdev_call(csi2->src_sd, video, s_stream, 0); 342 err_assert_reset: 343 csi2_enable(csi2, false); 344 err_disable_clk: 345 clk_disable_unprepare(csi2->pix_clk); 346 return ret; 347 } 348 349 static void csi2_stop(struct csi2_dev *csi2) 350 { 351 /* stop upstream */ 352 v4l2_subdev_call(csi2->src_sd, video, s_stream, 0); 353 354 csi2_enable(csi2, false); 355 clk_disable_unprepare(csi2->pix_clk); 356 } 357 358 /* 359 * V4L2 subdev operations. 360 */ 361 362 static int csi2_s_stream(struct v4l2_subdev *sd, int enable) 363 { 364 struct csi2_dev *csi2 = sd_to_dev(sd); 365 int i, ret = 0; 366 367 mutex_lock(&csi2->lock); 368 369 if (!csi2->src_sd) { 370 ret = -EPIPE; 371 goto out; 372 } 373 374 for (i = 0; i < CSI2_NUM_SRC_PADS; i++) { 375 if (csi2->sink_linked[i]) 376 break; 377 } 378 if (i >= CSI2_NUM_SRC_PADS) { 379 ret = -EPIPE; 380 goto out; 381 } 382 383 /* 384 * enable/disable streaming only if stream_count is 385 * going from 0 to 1 / 1 to 0. 386 */ 387 if (csi2->stream_count != !enable) 388 goto update_count; 389 390 dev_dbg(csi2->dev, "stream %s\n", enable ? "ON" : "OFF"); 391 if (enable) 392 ret = csi2_start(csi2); 393 else 394 csi2_stop(csi2); 395 if (ret) 396 goto out; 397 398 update_count: 399 csi2->stream_count += enable ? 1 : -1; 400 if (csi2->stream_count < 0) 401 csi2->stream_count = 0; 402 out: 403 mutex_unlock(&csi2->lock); 404 return ret; 405 } 406 407 static int csi2_link_setup(struct media_entity *entity, 408 const struct media_pad *local, 409 const struct media_pad *remote, u32 flags) 410 { 411 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 412 struct csi2_dev *csi2 = sd_to_dev(sd); 413 struct v4l2_subdev *remote_sd; 414 int ret = 0; 415 416 dev_dbg(csi2->dev, "link setup %s -> %s", remote->entity->name, 417 local->entity->name); 418 419 remote_sd = media_entity_to_v4l2_subdev(remote->entity); 420 421 mutex_lock(&csi2->lock); 422 423 if (local->flags & MEDIA_PAD_FL_SOURCE) { 424 if (flags & MEDIA_LNK_FL_ENABLED) { 425 if (csi2->sink_linked[local->index - 1]) { 426 ret = -EBUSY; 427 goto out; 428 } 429 csi2->sink_linked[local->index - 1] = true; 430 } else { 431 csi2->sink_linked[local->index - 1] = false; 432 } 433 } else { 434 if (flags & MEDIA_LNK_FL_ENABLED) { 435 if (csi2->src_sd) { 436 ret = -EBUSY; 437 goto out; 438 } 439 csi2->src_sd = remote_sd; 440 } else { 441 csi2->src_sd = NULL; 442 } 443 } 444 445 out: 446 mutex_unlock(&csi2->lock); 447 return ret; 448 } 449 450 static struct v4l2_mbus_framefmt * 451 __csi2_get_fmt(struct csi2_dev *csi2, struct v4l2_subdev_pad_config *cfg, 452 unsigned int pad, enum v4l2_subdev_format_whence which) 453 { 454 if (which == V4L2_SUBDEV_FORMAT_TRY) 455 return v4l2_subdev_get_try_format(&csi2->sd, cfg, pad); 456 else 457 return &csi2->format_mbus; 458 } 459 460 static int csi2_get_fmt(struct v4l2_subdev *sd, 461 struct v4l2_subdev_pad_config *cfg, 462 struct v4l2_subdev_format *sdformat) 463 { 464 struct csi2_dev *csi2 = sd_to_dev(sd); 465 struct v4l2_mbus_framefmt *fmt; 466 467 mutex_lock(&csi2->lock); 468 469 fmt = __csi2_get_fmt(csi2, cfg, sdformat->pad, sdformat->which); 470 471 sdformat->format = *fmt; 472 473 mutex_unlock(&csi2->lock); 474 475 return 0; 476 } 477 478 static int csi2_set_fmt(struct v4l2_subdev *sd, 479 struct v4l2_subdev_pad_config *cfg, 480 struct v4l2_subdev_format *sdformat) 481 { 482 struct csi2_dev *csi2 = sd_to_dev(sd); 483 struct v4l2_mbus_framefmt *fmt; 484 int ret = 0; 485 486 if (sdformat->pad >= CSI2_NUM_PADS) 487 return -EINVAL; 488 489 mutex_lock(&csi2->lock); 490 491 if (csi2->stream_count > 0) { 492 ret = -EBUSY; 493 goto out; 494 } 495 496 /* Output pads mirror active input pad, no limits on input pads */ 497 if (sdformat->pad != CSI2_SINK_PAD) 498 sdformat->format = csi2->format_mbus; 499 500 fmt = __csi2_get_fmt(csi2, cfg, sdformat->pad, sdformat->which); 501 502 *fmt = sdformat->format; 503 out: 504 mutex_unlock(&csi2->lock); 505 return ret; 506 } 507 508 /* 509 * retrieve our pads parsed from the OF graph by the media device 510 */ 511 static int csi2_registered(struct v4l2_subdev *sd) 512 { 513 struct csi2_dev *csi2 = sd_to_dev(sd); 514 int i, ret; 515 516 for (i = 0; i < CSI2_NUM_PADS; i++) { 517 csi2->pad[i].flags = (i == CSI2_SINK_PAD) ? 518 MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE; 519 } 520 521 /* set a default mbus format */ 522 ret = imx_media_init_mbus_fmt(&csi2->format_mbus, 523 640, 480, 0, V4L2_FIELD_NONE, NULL); 524 if (ret) 525 return ret; 526 527 return media_entity_pads_init(&sd->entity, CSI2_NUM_PADS, csi2->pad); 528 } 529 530 static const struct media_entity_operations csi2_entity_ops = { 531 .link_setup = csi2_link_setup, 532 .link_validate = v4l2_subdev_link_validate, 533 }; 534 535 static const struct v4l2_subdev_video_ops csi2_video_ops = { 536 .s_stream = csi2_s_stream, 537 }; 538 539 static const struct v4l2_subdev_pad_ops csi2_pad_ops = { 540 .init_cfg = imx_media_init_cfg, 541 .get_fmt = csi2_get_fmt, 542 .set_fmt = csi2_set_fmt, 543 }; 544 545 static const struct v4l2_subdev_ops csi2_subdev_ops = { 546 .video = &csi2_video_ops, 547 .pad = &csi2_pad_ops, 548 }; 549 550 static const struct v4l2_subdev_internal_ops csi2_internal_ops = { 551 .registered = csi2_registered, 552 }; 553 554 static int csi2_parse_endpoints(struct csi2_dev *csi2) 555 { 556 struct device_node *node = csi2->dev->of_node; 557 struct device_node *epnode; 558 struct v4l2_fwnode_endpoint ep; 559 560 epnode = of_graph_get_endpoint_by_regs(node, 0, -1); 561 if (!epnode) { 562 v4l2_err(&csi2->sd, "failed to get sink endpoint node\n"); 563 return -EINVAL; 564 } 565 566 v4l2_fwnode_endpoint_parse(of_fwnode_handle(epnode), &ep); 567 of_node_put(epnode); 568 569 if (ep.bus_type != V4L2_MBUS_CSI2) { 570 v4l2_err(&csi2->sd, "invalid bus type, must be MIPI CSI2\n"); 571 return -EINVAL; 572 } 573 574 csi2->bus = ep.bus.mipi_csi2; 575 576 dev_dbg(csi2->dev, "data lanes: %d\n", csi2->bus.num_data_lanes); 577 dev_dbg(csi2->dev, "flags: 0x%08x\n", csi2->bus.flags); 578 return 0; 579 } 580 581 static int csi2_probe(struct platform_device *pdev) 582 { 583 struct csi2_dev *csi2; 584 struct resource *res; 585 int ret; 586 587 csi2 = devm_kzalloc(&pdev->dev, sizeof(*csi2), GFP_KERNEL); 588 if (!csi2) 589 return -ENOMEM; 590 591 csi2->dev = &pdev->dev; 592 593 v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops); 594 v4l2_set_subdevdata(&csi2->sd, &pdev->dev); 595 csi2->sd.internal_ops = &csi2_internal_ops; 596 csi2->sd.entity.ops = &csi2_entity_ops; 597 csi2->sd.dev = &pdev->dev; 598 csi2->sd.owner = THIS_MODULE; 599 csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE; 600 strcpy(csi2->sd.name, DEVICE_NAME); 601 csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; 602 csi2->sd.grp_id = IMX_MEDIA_GRP_ID_CSI2; 603 604 ret = csi2_parse_endpoints(csi2); 605 if (ret) 606 return ret; 607 608 csi2->pllref_clk = devm_clk_get(&pdev->dev, "ref"); 609 if (IS_ERR(csi2->pllref_clk)) { 610 v4l2_err(&csi2->sd, "failed to get pll reference clock\n"); 611 ret = PTR_ERR(csi2->pllref_clk); 612 return ret; 613 } 614 615 csi2->dphy_clk = devm_clk_get(&pdev->dev, "dphy"); 616 if (IS_ERR(csi2->dphy_clk)) { 617 v4l2_err(&csi2->sd, "failed to get dphy clock\n"); 618 ret = PTR_ERR(csi2->dphy_clk); 619 return ret; 620 } 621 622 csi2->pix_clk = devm_clk_get(&pdev->dev, "pix"); 623 if (IS_ERR(csi2->pix_clk)) { 624 v4l2_err(&csi2->sd, "failed to get pixel clock\n"); 625 ret = PTR_ERR(csi2->pix_clk); 626 return ret; 627 } 628 629 res = platform_get_resource(pdev, IORESOURCE_MEM, 0); 630 if (!res) { 631 v4l2_err(&csi2->sd, "failed to get platform resources\n"); 632 return -ENODEV; 633 } 634 635 csi2->base = devm_ioremap(&pdev->dev, res->start, PAGE_SIZE); 636 if (!csi2->base) { 637 v4l2_err(&csi2->sd, "failed to map CSI-2 registers\n"); 638 return -ENOMEM; 639 } 640 641 mutex_init(&csi2->lock); 642 643 ret = clk_prepare_enable(csi2->pllref_clk); 644 if (ret) { 645 v4l2_err(&csi2->sd, "failed to enable pllref_clk\n"); 646 goto rmmutex; 647 } 648 649 ret = clk_prepare_enable(csi2->dphy_clk); 650 if (ret) { 651 v4l2_err(&csi2->sd, "failed to enable dphy_clk\n"); 652 goto pllref_off; 653 } 654 655 platform_set_drvdata(pdev, &csi2->sd); 656 657 ret = v4l2_async_register_subdev(&csi2->sd); 658 if (ret) 659 goto dphy_off; 660 661 return 0; 662 663 dphy_off: 664 clk_disable_unprepare(csi2->dphy_clk); 665 pllref_off: 666 clk_disable_unprepare(csi2->pllref_clk); 667 rmmutex: 668 mutex_destroy(&csi2->lock); 669 return ret; 670 } 671 672 static int csi2_remove(struct platform_device *pdev) 673 { 674 struct v4l2_subdev *sd = platform_get_drvdata(pdev); 675 struct csi2_dev *csi2 = sd_to_dev(sd); 676 677 v4l2_async_unregister_subdev(sd); 678 clk_disable_unprepare(csi2->dphy_clk); 679 clk_disable_unprepare(csi2->pllref_clk); 680 mutex_destroy(&csi2->lock); 681 media_entity_cleanup(&sd->entity); 682 683 return 0; 684 } 685 686 static const struct of_device_id csi2_dt_ids[] = { 687 { .compatible = "fsl,imx6-mipi-csi2", }, 688 { /* sentinel */ } 689 }; 690 MODULE_DEVICE_TABLE(of, csi2_dt_ids); 691 692 static struct platform_driver csi2_driver = { 693 .driver = { 694 .name = DEVICE_NAME, 695 .of_match_table = csi2_dt_ids, 696 }, 697 .probe = csi2_probe, 698 .remove = csi2_remove, 699 }; 700 701 module_platform_driver(csi2_driver); 702 703 MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver"); 704 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>"); 705 MODULE_LICENSE("GPL"); 706