drivers/staging/media/imx/imx7-media-csi.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * V4L2 Capture CSI Subdev for Freescale i.MX6UL/L / i.MX7 SOC
   4  *
   5  * Copyright (c) 2019 Linaro Ltd
   6  *
   7  */
   8
   9 #include <linux/clk.h>
  10 #include <linux/delay.h>
  11 #include <linux/gcd.h>
  12 #include <linux/interrupt.h>
  13 #include <linux/mfd/syscon.h>
  14 #include <linux/module.h>
  15 #include <linux/of_graph.h>
  16 #include <linux/pinctrl/consumer.h>
  17 #include <linux/platform_device.h>
  18 #include <linux/regmap.h>
  19 #include <linux/types.h>
  20
  21 #include <media/v4l2-ctrls.h>
  22 #include <media/v4l2-device.h>
  23 #include <media/v4l2-event.h>
  24 #include <media/v4l2-fwnode.h>
  25 #include <media/v4l2-mc.h>
  26 #include <media/v4l2-subdev.h>
  27 #include <media/videobuf2-dma-contig.h>
  28
  29 #include <media/imx.h>
  30 #include "imx-media.h"
  31
  32 #define IMX7_CSI_PAD_SINK       0
  33 #define IMX7_CSI_PAD_SRC        1
  34 #define IMX7_CSI_PADS_NUM       2
  35
  36 /* reset values */
  37 #define CSICR1_RESET_VAL        0x40000800
  38 #define CSICR2_RESET_VAL        0x0
  39 #define CSICR3_RESET_VAL        0x0
  40
  41 /* csi control reg 1 */
  42 #define BIT_SWAP16_EN           BIT(31)
  43 #define BIT_EXT_VSYNC           BIT(30)
  44 #define BIT_EOF_INT_EN          BIT(29)
  45 #define BIT_PRP_IF_EN           BIT(28)
  46 #define BIT_CCIR_MODE           BIT(27)
  47 #define BIT_COF_INT_EN          BIT(26)
  48 #define BIT_SF_OR_INTEN         BIT(25)
  49 #define BIT_RF_OR_INTEN         BIT(24)
  50 #define BIT_SFF_DMA_DONE_INTEN  BIT(22)
  51 #define BIT_STATFF_INTEN        BIT(21)
  52 #define BIT_FB2_DMA_DONE_INTEN  BIT(20)
  53 #define BIT_FB1_DMA_DONE_INTEN  BIT(19)
  54 #define BIT_RXFF_INTEN          BIT(18)
  55 #define BIT_SOF_POL             BIT(17)
  56 #define BIT_SOF_INTEN           BIT(16)
  57 #define BIT_MCLKDIV             (0xF << 12)
  58 #define BIT_HSYNC_POL           BIT(11)
  59 #define BIT_CCIR_EN             BIT(10)
  60 #define BIT_MCLKEN              BIT(9)
  61 #define BIT_FCC                 BIT(8)
  62 #define BIT_PACK_DIR            BIT(7)
  63 #define BIT_CLR_STATFIFO        BIT(6)
  64 #define BIT_CLR_RXFIFO          BIT(5)
  65 #define BIT_GCLK_MODE           BIT(4)
  66 #define BIT_INV_DATA            BIT(3)
  67 #define BIT_INV_PCLK            BIT(2)
  68 #define BIT_REDGE               BIT(1)
  69 #define BIT_PIXEL_BIT           BIT(0)
  70
  71 #define SHIFT_MCLKDIV           12
  72
  73 /* control reg 3 */
  74 #define BIT_FRMCNT              (0xFFFF << 16)
  75 #define BIT_FRMCNT_RST          BIT(15)
  76 #define BIT_DMA_REFLASH_RFF     BIT(14)
  77 #define BIT_DMA_REFLASH_SFF     BIT(13)
  78 #define BIT_DMA_REQ_EN_RFF      BIT(12)
  79 #define BIT_DMA_REQ_EN_SFF      BIT(11)
  80 #define BIT_STATFF_LEVEL        (0x7 << 8)
  81 #define BIT_HRESP_ERR_EN        BIT(7)
  82 #define BIT_RXFF_LEVEL          (0x7 << 4)
  83 #define BIT_TWO_8BIT_SENSOR     BIT(3)
  84 #define BIT_ZERO_PACK_EN        BIT(2)
  85 #define BIT_ECC_INT_EN          BIT(1)
  86 #define BIT_ECC_AUTO_EN         BIT(0)
  87
  88 #define SHIFT_FRMCNT            16
  89 #define SHIFT_RXFIFO_LEVEL      4
  90
  91 /* csi status reg */
  92 #define BIT_ADDR_CH_ERR_INT     BIT(28)
  93 #define BIT_FIELD0_INT          BIT(27)
  94 #define BIT_FIELD1_INT          BIT(26)
  95 #define BIT_SFF_OR_INT          BIT(25)
  96 #define BIT_RFF_OR_INT          BIT(24)
  97 #define BIT_DMA_TSF_DONE_SFF    BIT(22)
  98 #define BIT_STATFF_INT          BIT(21)
  99 #define BIT_DMA_TSF_DONE_FB2    BIT(20)
 100 #define BIT_DMA_TSF_DONE_FB1    BIT(19)
 101 #define BIT_RXFF_INT            BIT(18)
 102 #define BIT_EOF_INT             BIT(17)
 103 #define BIT_SOF_INT             BIT(16)
 104 #define BIT_F2_INT              BIT(15)
 105 #define BIT_F1_INT              BIT(14)
 106 #define BIT_COF_INT             BIT(13)
 107 #define BIT_HRESP_ERR_INT       BIT(7)
 108 #define BIT_ECC_INT             BIT(1)
 109 #define BIT_DRDY                BIT(0)
 110
 111 /* csi control reg 18 */
 112 #define BIT_CSI_HW_ENABLE               BIT(31)
 113 #define BIT_MIPI_DATA_FORMAT_RAW8       (0x2a << 25)
 114 #define BIT_MIPI_DATA_FORMAT_RAW10      (0x2b << 25)
 115 #define BIT_MIPI_DATA_FORMAT_RAW12      (0x2c << 25)
 116 #define BIT_MIPI_DATA_FORMAT_RAW14      (0x2d << 25)
 117 #define BIT_MIPI_DATA_FORMAT_YUV422_8B  (0x1e << 25)
 118 #define BIT_MIPI_DATA_FORMAT_MASK       (0x3F << 25)
 119 #define BIT_MIPI_DATA_FORMAT_OFFSET     25
 120 #define BIT_DATA_FROM_MIPI              BIT(22)
 121 #define BIT_MIPI_YU_SWAP                BIT(21)
 122 #define BIT_MIPI_DOUBLE_CMPNT           BIT(20)
 123 #define BIT_BASEADDR_CHG_ERR_EN         BIT(9)
 124 #define BIT_BASEADDR_SWITCH_SEL         BIT(5)
 125 #define BIT_BASEADDR_SWITCH_EN          BIT(4)
 126 #define BIT_PARALLEL24_EN               BIT(3)
 127 #define BIT_DEINTERLACE_EN              BIT(2)
 128 #define BIT_TVDECODER_IN_EN             BIT(1)
 129 #define BIT_NTSC_EN                     BIT(0)
 130
 131 #define CSI_MCLK_VF             1
 132 #define CSI_MCLK_ENC            2
 133 #define CSI_MCLK_RAW            4
 134 #define CSI_MCLK_I2C            8
 135
 136 #define CSI_CSICR1              0x0
 137 #define CSI_CSICR2              0x4
 138 #define CSI_CSICR3              0x8
 139 #define CSI_STATFIFO            0xC
 140 #define CSI_CSIRXFIFO           0x10
 141 #define CSI_CSIRXCNT            0x14
 142 #define CSI_CSISR               0x18
 143
 144 #define CSI_CSIDBG              0x1C
 145 #define CSI_CSIDMASA_STATFIFO   0x20
 146 #define CSI_CSIDMATS_STATFIFO   0x24
 147 #define CSI_CSIDMASA_FB1        0x28
 148 #define CSI_CSIDMASA_FB2        0x2C
 149 #define CSI_CSIFBUF_PARA        0x30
 150 #define CSI_CSIIMAG_PARA        0x34
 151
 152 #define CSI_CSICR18             0x48
 153 #define CSI_CSICR19             0x4c
 154
 155 struct imx7_csi {
 156         struct device *dev;
 157         struct v4l2_subdev sd;
 158         struct imx_media_video_dev *vdev;
 159         struct imx_media_dev *imxmd;
 160         struct media_pad pad[IMX7_CSI_PADS_NUM];
 161
 162         /* lock to protect members below */
 163         struct mutex lock;
 164         /* lock to protect irq handler when stop streaming */
 165         spinlock_t irqlock;
 166
 167         struct v4l2_subdev *src_sd;
 168
 169         struct media_entity *sink;
 170
 171         struct v4l2_fwnode_endpoint upstream_ep;
 172
 173         struct v4l2_mbus_framefmt format_mbus[IMX7_CSI_PADS_NUM];
 174         const struct imx_media_pixfmt *cc[IMX7_CSI_PADS_NUM];
 175         struct v4l2_fract frame_interval[IMX7_CSI_PADS_NUM];
 176
 177         struct v4l2_ctrl_handler ctrl_hdlr;
 178
 179         void __iomem *regbase;
 180         int irq;
 181         struct clk *mclk;
 182
 183         /* active vb2 buffers to send to video dev sink */
 184         struct imx_media_buffer *active_vb2_buf[2];
 185         struct imx_media_dma_buf underrun_buf;
 186
 187         int buf_num;
 188         u32 frame_sequence;
 189
 190         bool last_eof;
 191         bool is_init;
 192         bool is_streaming;
 193         bool is_csi2;
 194
 195         struct completion last_eof_completion;
 196 };
 197
 198 static u32 imx7_csi_reg_read(struct imx7_csi *csi, unsigned int offset)
 199 {
 200         return readl(csi->regbase + offset);
 201 }
 202
 203 static void imx7_csi_reg_write(struct imx7_csi *csi, unsigned int value,
 204                                unsigned int offset)
 205 {
 206         writel(value, csi->regbase + offset);
 207 }
 208
 209 static void imx7_csi_hw_reset(struct imx7_csi *csi)
 210 {
 211         imx7_csi_reg_write(csi,
 212                            imx7_csi_reg_read(csi, CSI_CSICR3) | BIT_FRMCNT_RST,
 213                            CSI_CSICR3);
 214
 215         imx7_csi_reg_write(csi, CSICR1_RESET_VAL, CSI_CSICR1);
 216         imx7_csi_reg_write(csi, CSICR2_RESET_VAL, CSI_CSICR2);
 217         imx7_csi_reg_write(csi, CSICR3_RESET_VAL, CSI_CSICR3);
 218 }
 219
 220 static u32 imx7_csi_irq_clear(struct imx7_csi *csi)
 221 {
 222         u32 isr;
 223
 224         isr = imx7_csi_reg_read(csi, CSI_CSISR);
 225         imx7_csi_reg_write(csi, isr, CSI_CSISR);
 226
 227         return isr;
 228 }
 229
 230 static void imx7_csi_init_interface(struct imx7_csi *csi)
 231 {
 232         unsigned int val = 0;
 233         unsigned int imag_para;
 234
 235         val = BIT_SOF_POL | BIT_REDGE | BIT_GCLK_MODE | BIT_HSYNC_POL |
 236                 BIT_FCC | 1 << SHIFT_MCLKDIV | BIT_MCLKEN;
 237         imx7_csi_reg_write(csi, val, CSI_CSICR1);
 238
 239         imag_para = (800 << 16) | 600;
 240         imx7_csi_reg_write(csi, imag_para, CSI_CSIIMAG_PARA);
 241
 242         val = BIT_DMA_REFLASH_RFF;
 243         imx7_csi_reg_write(csi, val, CSI_CSICR3);
 244 }
 245
 246 static void imx7_csi_hw_enable_irq(struct imx7_csi *csi)
 247 {
 248         u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
 249
 250         cr1 |= BIT_SOF_INTEN;
 251         cr1 |= BIT_RFF_OR_INT;
 252
 253         /* still capture needs DMA interrupt */
 254         cr1 |= BIT_FB1_DMA_DONE_INTEN;
 255         cr1 |= BIT_FB2_DMA_DONE_INTEN;
 256
 257         cr1 |= BIT_EOF_INT_EN;
 258
 259         imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
 260 }
 261
 262 static void imx7_csi_hw_disable_irq(struct imx7_csi *csi)
 263 {
 264         u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
 265
 266         cr1 &= ~BIT_SOF_INTEN;
 267         cr1 &= ~BIT_RFF_OR_INT;
 268         cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
 269         cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
 270         cr1 &= ~BIT_EOF_INT_EN;
 271
 272         imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
 273 }
 274
 275 static void imx7_csi_hw_enable(struct imx7_csi *csi)
 276 {
 277         u32 cr = imx7_csi_reg_read(csi, CSI_CSICR18);
 278
 279         cr |= BIT_CSI_HW_ENABLE;
 280
 281         imx7_csi_reg_write(csi, cr, CSI_CSICR18);
 282 }
 283
 284 static void imx7_csi_hw_disable(struct imx7_csi *csi)
 285 {
 286         u32 cr = imx7_csi_reg_read(csi, CSI_CSICR18);
 287
 288         cr &= ~BIT_CSI_HW_ENABLE;
 289
 290         imx7_csi_reg_write(csi, cr, CSI_CSICR18);
 291 }
 292
 293 static void imx7_csi_dma_reflash(struct imx7_csi *csi)
 294 {
 295         u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR18);
 296
 297         cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
 298         cr3 |= BIT_DMA_REFLASH_RFF;
 299         imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
 300 }
 301
 302 static void imx7_csi_rx_fifo_clear(struct imx7_csi *csi)
 303 {
 304         u32 cr1;
 305
 306         cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
 307         imx7_csi_reg_write(csi, cr1 & ~BIT_FCC, CSI_CSICR1);
 308         cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
 309         imx7_csi_reg_write(csi, cr1 | BIT_CLR_RXFIFO, CSI_CSICR1);
 310
 311         cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
 312         imx7_csi_reg_write(csi, cr1 | BIT_FCC, CSI_CSICR1);
 313 }
 314
 315 static void imx7_csi_buf_stride_set(struct imx7_csi *csi, u32 stride)
 316 {
 317         imx7_csi_reg_write(csi, stride, CSI_CSIFBUF_PARA);
 318 }
 319
 320 static void imx7_csi_deinterlace_enable(struct imx7_csi *csi, bool enable)
 321 {
 322         u32 cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
 323
 324         if (enable)
 325                 cr18 |= BIT_DEINTERLACE_EN;
 326         else
 327                 cr18 &= ~BIT_DEINTERLACE_EN;
 328
 329         imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
 330 }
 331
 332 static void imx7_csi_dmareq_rff_enable(struct imx7_csi *csi)
 333 {
 334         u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
 335         u32 cr2 = imx7_csi_reg_read(csi, CSI_CSICR2);
 336
 337         /* Burst Type of DMA Transfer from RxFIFO. INCR16 */
 338         cr2 |= 0xC0000000;
 339
 340         cr3 |= BIT_DMA_REQ_EN_RFF;
 341         cr3 |= BIT_HRESP_ERR_EN;
 342         cr3 &= ~BIT_RXFF_LEVEL;
 343         cr3 |= 0x2 << 4;
 344
 345         imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
 346         imx7_csi_reg_write(csi, cr2, CSI_CSICR2);
 347 }
 348
 349 static void imx7_csi_dmareq_rff_disable(struct imx7_csi *csi)
 350 {
 351         u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
 352
 353         cr3 &= ~BIT_DMA_REQ_EN_RFF;
 354         cr3 &= ~BIT_HRESP_ERR_EN;
 355         imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
 356 }
 357
 358 static void imx7_csi_set_imagpara(struct imx7_csi *csi, int width, int height)
 359 {
 360         int imag_para;
 361         int rx_count;
 362
 363         rx_count = (width * height) >> 2;
 364         imx7_csi_reg_write(csi, rx_count, CSI_CSIRXCNT);
 365
 366         imag_para = (width << 16) | height;
 367         imx7_csi_reg_write(csi, imag_para, CSI_CSIIMAG_PARA);
 368
 369         /* reflash the embedded DMA controller */
 370         imx7_csi_dma_reflash(csi);
 371 }
 372
 373 static void imx7_csi_sw_reset(struct imx7_csi *csi)
 374 {
 375         imx7_csi_hw_disable(csi);
 376
 377         imx7_csi_rx_fifo_clear(csi);
 378
 379         imx7_csi_dma_reflash(csi);
 380
 381         usleep_range(2000, 3000);
 382
 383         imx7_csi_irq_clear(csi);
 384
 385         imx7_csi_hw_enable(csi);
 386 }
 387
 388 static void imx7_csi_error_recovery(struct imx7_csi *csi)
 389 {
 390         imx7_csi_hw_disable(csi);
 391
 392         imx7_csi_rx_fifo_clear(csi);
 393
 394         imx7_csi_dma_reflash(csi);
 395
 396         imx7_csi_hw_enable(csi);
 397 }
 398
 399 static int imx7_csi_init(struct imx7_csi *csi)
 400 {
 401         int ret;
 402
 403         if (csi->is_init)
 404                 return 0;
 405
 406         ret = clk_prepare_enable(csi->mclk);
 407         if (ret < 0)
 408                 return ret;
 409         imx7_csi_hw_reset(csi);
 410         imx7_csi_init_interface(csi);
 411         imx7_csi_dmareq_rff_enable(csi);
 412
 413         csi->is_init = true;
 414
 415         return 0;
 416 }
 417
 418 static void imx7_csi_deinit(struct imx7_csi *csi)
 419 {
 420         if (!csi->is_init)
 421                 return;
 422
 423         imx7_csi_hw_reset(csi);
 424         imx7_csi_init_interface(csi);
 425         imx7_csi_dmareq_rff_disable(csi);
 426         clk_disable_unprepare(csi->mclk);
 427
 428         csi->is_init = false;
 429 }
 430
 431 static int imx7_csi_get_upstream_endpoint(struct imx7_csi *csi,
 432                                           struct v4l2_fwnode_endpoint *ep,
 433                                           bool skip_mux)
 434 {
 435         struct device_node *endpoint, *port;
 436         struct media_entity *src;
 437         struct v4l2_subdev *sd;
 438         struct media_pad *pad;
 439
 440         if (!csi->src_sd)
 441                 return -EPIPE;
 442
 443         src = &csi->src_sd->entity;
 444
 445         /*
 446          * if the source is neither a mux or csi2 get the one directly upstream
 447          * from this csi
 448          */
 449         if (src->function != MEDIA_ENT_F_VID_IF_BRIDGE &&
 450             src->function != MEDIA_ENT_F_VID_MUX)
 451                 src = &csi->sd.entity;
 452
 453 skip_video_mux:
 454         /* get source pad of entity directly upstream from src */
 455         pad = imx_media_pipeline_pad(src, 0, 0, true);
 456         if (!pad)
 457                 return -ENODEV;
 458
 459         sd = media_entity_to_v4l2_subdev(pad->entity);
 460
 461         /* To get bus type we may need to skip video mux */
 462         if (skip_mux && src->function == MEDIA_ENT_F_VID_MUX) {
 463                 src = &sd->entity;
 464                 goto skip_video_mux;
 465         }
 466
 467         /*
 468          * NOTE: this assumes an OF-graph port id is the same as a
 469          * media pad index.
 470          */
 471         port = of_graph_get_port_by_id(sd->dev->of_node, pad->index);
 472         if (!port)
 473                 return -ENODEV;
 474
 475         endpoint = of_get_next_child(port, NULL);
 476         of_node_put(port);
 477         if (!endpoint)
 478                 return -ENODEV;
 479
 480         v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint), ep);
 481         of_node_put(endpoint);
 482
 483         return 0;
 484 }
 485
 486 static int imx7_csi_link_setup(struct media_entity *entity,
 487                                const struct media_pad *local,
 488                                const struct media_pad *remote, u32 flags)
 489 {
 490         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 491         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
 492         struct v4l2_subdev *remote_sd;
 493         int ret = 0;
 494
 495         dev_dbg(csi->dev, "link setup %s -> %s\n", remote->entity->name,
 496                 local->entity->name);
 497
 498         mutex_lock(&csi->lock);
 499
 500         if (local->flags & MEDIA_PAD_FL_SINK) {
 501                 if (!is_media_entity_v4l2_subdev(remote->entity)) {
 502                         ret = -EINVAL;
 503                         goto unlock;
 504                 }
 505
 506                 remote_sd = media_entity_to_v4l2_subdev(remote->entity);
 507
 508                 if (flags & MEDIA_LNK_FL_ENABLED) {
 509                         if (csi->src_sd) {
 510                                 ret = -EBUSY;
 511                                 goto unlock;
 512                         }
 513                         csi->src_sd = remote_sd;
 514                 } else {
 515                         csi->src_sd = NULL;
 516                 }
 517
 518                 goto init;
 519         }
 520
 521         /* source pad */
 522         if (flags & MEDIA_LNK_FL_ENABLED) {
 523                 if (csi->sink) {
 524                         ret = -EBUSY;
 525                         goto unlock;
 526                 }
 527                 csi->sink = remote->entity;
 528         } else {
 529                 v4l2_ctrl_handler_free(&csi->ctrl_hdlr);
 530                 v4l2_ctrl_handler_init(&csi->ctrl_hdlr, 0);
 531                 csi->sink = NULL;
 532         }
 533
 534 init:
 535         if (csi->sink || csi->src_sd)
 536                 ret = imx7_csi_init(csi);
 537         else
 538                 imx7_csi_deinit(csi);
 539
 540 unlock:
 541         mutex_unlock(&csi->lock);
 542
 543         return ret;
 544 }
 545
 546 static int imx7_csi_pad_link_validate(struct v4l2_subdev *sd,
 547                                       struct media_link *link,
 548                                       struct v4l2_subdev_format *source_fmt,
 549                                       struct v4l2_subdev_format *sink_fmt)
 550 {
 551         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
 552         struct v4l2_fwnode_endpoint upstream_ep = {};
 553         int ret;
 554
 555         ret = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
 556         if (ret)
 557                 return ret;
 558
 559         ret = imx7_csi_get_upstream_endpoint(csi, &upstream_ep, true);
 560         if (ret) {
 561                 v4l2_err(&csi->sd, "failed to find upstream endpoint\n");
 562                 return ret;
 563         }
 564
 565         mutex_lock(&csi->lock);
 566
 567         csi->upstream_ep = upstream_ep;
 568         csi->is_csi2 = (upstream_ep.bus_type == V4L2_MBUS_CSI2_DPHY);
 569
 570         mutex_unlock(&csi->lock);
 571
 572         return 0;
 573 }
 574
 575 static void imx7_csi_update_buf(struct imx7_csi *csi, dma_addr_t phys,
 576                                 int buf_num)
 577 {
 578         if (buf_num == 1)
 579                 imx7_csi_reg_write(csi, phys, CSI_CSIDMASA_FB2);
 580         else
 581                 imx7_csi_reg_write(csi, phys, CSI_CSIDMASA_FB1);
 582 }
 583
 584 static void imx7_csi_setup_vb2_buf(struct imx7_csi *csi)
 585 {
 586         struct imx_media_video_dev *vdev = csi->vdev;
 587         struct imx_media_buffer *buf;
 588         struct vb2_buffer *vb2_buf;
 589         dma_addr_t phys[2];
 590         int i;
 591
 592         for (i = 0; i < 2; i++) {
 593                 buf = imx_media_capture_device_next_buf(vdev);
 594                 if (buf) {
 595                         csi->active_vb2_buf[i] = buf;
 596                         vb2_buf = &buf->vbuf.vb2_buf;
 597                         phys[i] = vb2_dma_contig_plane_dma_addr(vb2_buf, 0);
 598                 } else {
 599                         csi->active_vb2_buf[i] = NULL;
 600                         phys[i] = csi->underrun_buf.phys;
 601                 }
 602
 603                 imx7_csi_update_buf(csi, phys[i], i);
 604         }
 605 }
 606
 607 static void imx7_csi_dma_unsetup_vb2_buf(struct imx7_csi *csi,
 608                                          enum vb2_buffer_state return_status)
 609 {
 610         struct imx_media_buffer *buf;
 611         int i;
 612
 613         /* return any remaining active frames with return_status */
 614         for (i = 0; i < 2; i++) {
 615                 buf = csi->active_vb2_buf[i];
 616                 if (buf) {
 617                         struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
 618
 619                         vb->timestamp = ktime_get_ns();
 620                         vb2_buffer_done(vb, return_status);
 621                 }
 622         }
 623 }
 624
 625 static void imx7_csi_vb2_buf_done(struct imx7_csi *csi)
 626 {
 627         struct imx_media_video_dev *vdev = csi->vdev;
 628         struct imx_media_buffer *done, *next;
 629         struct vb2_buffer *vb;
 630         dma_addr_t phys;
 631
 632         done = csi->active_vb2_buf[csi->buf_num];
 633         if (done) {
 634                 done->vbuf.field = vdev->fmt.fmt.pix.field;
 635                 done->vbuf.sequence = csi->frame_sequence;
 636                 vb = &done->vbuf.vb2_buf;
 637                 vb->timestamp = ktime_get_ns();
 638                 vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 639         }
 640         csi->frame_sequence++;
 641
 642         /* get next queued buffer */
 643         next = imx_media_capture_device_next_buf(vdev);
 644         if (next) {
 645                 phys = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
 646                 csi->active_vb2_buf[csi->buf_num] = next;
 647         } else {
 648                 phys = csi->underrun_buf.phys;
 649                 csi->active_vb2_buf[csi->buf_num] = NULL;
 650         }
 651
 652         imx7_csi_update_buf(csi, phys, csi->buf_num);
 653 }
 654
 655 static irqreturn_t imx7_csi_irq_handler(int irq, void *data)
 656 {
 657         struct imx7_csi *csi =  data;
 658         u32 status;
 659
 660         spin_lock(&csi->irqlock);
 661
 662         status = imx7_csi_irq_clear(csi);
 663
 664         if (status & BIT_RFF_OR_INT) {
 665                 dev_warn(csi->dev, "Rx fifo overflow\n");
 666                 imx7_csi_error_recovery(csi);
 667         }
 668
 669         if (status & BIT_HRESP_ERR_INT) {
 670                 dev_warn(csi->dev, "Hresponse error detected\n");
 671                 imx7_csi_error_recovery(csi);
 672         }
 673
 674         if (status & BIT_ADDR_CH_ERR_INT) {
 675                 imx7_csi_hw_disable(csi);
 676
 677                 imx7_csi_dma_reflash(csi);
 678
 679                 imx7_csi_hw_enable(csi);
 680         }
 681
 682         if ((status & BIT_DMA_TSF_DONE_FB1) &&
 683             (status & BIT_DMA_TSF_DONE_FB2)) {
 684                 /*
 685                  * For both FB1 and FB2 interrupter bits set case,
 686                  * CSI DMA is work in one of FB1 and FB2 buffer,
 687                  * but software can not know the state.
 688                  * Skip it to avoid base address updated
 689                  * when csi work in field0 and field1 will write to
 690                  * new base address.
 691                  */
 692         } else if (status & BIT_DMA_TSF_DONE_FB1) {
 693                 csi->buf_num = 0;
 694         } else if (status & BIT_DMA_TSF_DONE_FB2) {
 695                 csi->buf_num = 1;
 696         }
 697
 698         if ((status & BIT_DMA_TSF_DONE_FB1) ||
 699             (status & BIT_DMA_TSF_DONE_FB2)) {
 700                 imx7_csi_vb2_buf_done(csi);
 701
 702                 if (csi->last_eof) {
 703                         complete(&csi->last_eof_completion);
 704                         csi->last_eof = false;
 705                 }
 706         }
 707
 708         spin_unlock(&csi->irqlock);
 709
 710         return IRQ_HANDLED;
 711 }
 712
 713 static int imx7_csi_dma_start(struct imx7_csi *csi)
 714 {
 715         struct imx_media_video_dev *vdev = csi->vdev;
 716         struct v4l2_pix_format *out_pix = &vdev->fmt.fmt.pix;
 717         int ret;
 718
 719         ret = imx_media_alloc_dma_buf(csi->dev, &csi->underrun_buf,
 720                                       out_pix->sizeimage);
 721         if (ret < 0) {
 722                 v4l2_warn(&csi->sd, "consider increasing the CMA area\n");
 723                 return ret;
 724         }
 725
 726         csi->frame_sequence = 0;
 727         csi->last_eof = false;
 728         init_completion(&csi->last_eof_completion);
 729
 730         imx7_csi_setup_vb2_buf(csi);
 731
 732         return 0;
 733 }
 734
 735 static void imx7_csi_dma_stop(struct imx7_csi *csi)
 736 {
 737         unsigned long timeout_jiffies;
 738         unsigned long flags;
 739         int ret;
 740
 741         /* mark next EOF interrupt as the last before stream off */
 742         spin_lock_irqsave(&csi->irqlock, flags);
 743         csi->last_eof = true;
 744         spin_unlock_irqrestore(&csi->irqlock, flags);
 745
 746         /*
 747          * and then wait for interrupt handler to mark completion.
 748          */
 749         timeout_jiffies = msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT);
 750         ret = wait_for_completion_timeout(&csi->last_eof_completion,
 751                                           timeout_jiffies);
 752         if (ret == 0)
 753                 v4l2_warn(&csi->sd, "wait last EOF timeout\n");
 754
 755         imx7_csi_hw_disable_irq(csi);
 756
 757         imx7_csi_dma_unsetup_vb2_buf(csi, VB2_BUF_STATE_ERROR);
 758
 759         imx_media_free_dma_buf(csi->dev, &csi->underrun_buf);
 760 }
 761
 762 static int imx7_csi_configure(struct imx7_csi *csi)
 763 {
 764         struct imx_media_video_dev *vdev = csi->vdev;
 765         struct v4l2_pix_format *out_pix = &vdev->fmt.fmt.pix;
 766         __u32 in_code = csi->format_mbus[IMX7_CSI_PAD_SINK].code;
 767         u32 cr1, cr18;
 768         int width = out_pix->width;
 769
 770         if (out_pix->field == V4L2_FIELD_INTERLACED) {
 771                 imx7_csi_deinterlace_enable(csi, true);
 772                 imx7_csi_buf_stride_set(csi, out_pix->width);
 773         } else {
 774                 imx7_csi_deinterlace_enable(csi, false);
 775                 imx7_csi_buf_stride_set(csi, 0);
 776         }
 777
 778         cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
 779
 780         if (!csi->is_csi2) {
 781                 if (out_pix->pixelformat == V4L2_PIX_FMT_UYVY ||
 782                     out_pix->pixelformat == V4L2_PIX_FMT_YUYV)
 783                         width *= 2;
 784
 785                 imx7_csi_set_imagpara(csi, width, out_pix->height);
 786
 787                 cr18 |= (BIT_BASEADDR_SWITCH_EN | BIT_BASEADDR_SWITCH_SEL |
 788                         BIT_BASEADDR_CHG_ERR_EN);
 789                 imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
 790
 791                 return 0;
 792         }
 793
 794         imx7_csi_set_imagpara(csi, width, out_pix->height);
 795
 796         cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
 797         cr1 &= ~BIT_GCLK_MODE;
 798
 799         cr18 &= BIT_MIPI_DATA_FORMAT_MASK;
 800         cr18 |= BIT_DATA_FROM_MIPI;
 801
 802         switch (out_pix->pixelformat) {
 803         case V4L2_PIX_FMT_UYVY:
 804         case V4L2_PIX_FMT_YUYV:
 805                 cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B;
 806                 break;
 807         case V4L2_PIX_FMT_SBGGR8:
 808                 cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
 809                 break;
 810         case V4L2_PIX_FMT_SBGGR16:
 811                 if (in_code == MEDIA_BUS_FMT_SBGGR10_1X10)
 812                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
 813                 else if (in_code == MEDIA_BUS_FMT_SBGGR12_1X12)
 814                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW12;
 815                 else if (in_code == MEDIA_BUS_FMT_SBGGR14_1X14)
 816                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW14;
 817                 cr1 |= BIT_PIXEL_BIT;
 818                 break;
 819         default:
 820                 return -EINVAL;
 821         }
 822
 823         imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
 824         imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
 825
 826         return 0;
 827 }
 828
 829 static void imx7_csi_enable(struct imx7_csi *csi)
 830 {
 831         imx7_csi_sw_reset(csi);
 832
 833         imx7_csi_dmareq_rff_enable(csi);
 834         imx7_csi_hw_enable_irq(csi);
 835         imx7_csi_hw_enable(csi);
 836 }
 837
 838 static void imx7_csi_disable(struct imx7_csi *csi)
 839 {
 840         imx7_csi_dmareq_rff_disable(csi);
 841
 842         imx7_csi_hw_disable_irq(csi);
 843
 844         imx7_csi_buf_stride_set(csi, 0);
 845
 846         imx7_csi_hw_disable(csi);
 847 }
 848
 849 static int imx7_csi_streaming_start(struct imx7_csi *csi)
 850 {
 851         int ret;
 852
 853         ret = imx7_csi_dma_start(csi);
 854         if (ret < 0)
 855                 return ret;
 856
 857         ret = imx7_csi_configure(csi);
 858         if (ret < 0)
 859                 goto dma_stop;
 860
 861         imx7_csi_enable(csi);
 862
 863         return 0;
 864
 865 dma_stop:
 866         imx7_csi_dma_stop(csi);
 867
 868         return ret;
 869 }
 870
 871 static int imx7_csi_streaming_stop(struct imx7_csi *csi)
 872 {
 873         imx7_csi_dma_stop(csi);
 874
 875         imx7_csi_disable(csi);
 876
 877         return 0;
 878 }
 879
 880 static int imx7_csi_s_stream(struct v4l2_subdev *sd, int enable)
 881 {
 882         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
 883         int ret = 0;
 884
 885         mutex_lock(&csi->lock);
 886
 887         if (!csi->src_sd || !csi->sink) {
 888                 ret = -EPIPE;
 889                 goto out_unlock;
 890         }
 891
 892         if (csi->is_streaming == !!enable)
 893                 goto out_unlock;
 894
 895         if (enable) {
 896                 ret = v4l2_subdev_call(csi->src_sd, video, s_stream, 1);
 897                 if (ret < 0)
 898                         goto out_unlock;
 899
 900                 ret = imx7_csi_streaming_start(csi);
 901                 if (ret < 0) {
 902                         v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
 903                         goto out_unlock;
 904                 }
 905         } else {
 906                 imx7_csi_streaming_stop(csi);
 907
 908                 v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
 909         }
 910
 911         csi->is_streaming = !!enable;
 912
 913 out_unlock:
 914         mutex_unlock(&csi->lock);
 915
 916         return ret;
 917 }
 918
 919 static struct v4l2_mbus_framefmt *
 920 imx7_csi_get_format(struct imx7_csi *csi,
 921                     struct v4l2_subdev_pad_config *cfg,
 922                     unsigned int pad,
 923                     enum v4l2_subdev_format_whence which)
 924 {
 925         if (which == V4L2_SUBDEV_FORMAT_TRY)
 926                 return v4l2_subdev_get_try_format(&csi->sd, cfg, pad);
 927
 928         return &csi->format_mbus[pad];
 929 }
 930
 931 static int imx7_csi_enum_mbus_code(struct v4l2_subdev *sd,
 932                                    struct v4l2_subdev_pad_config *cfg,
 933                                    struct v4l2_subdev_mbus_code_enum *code)
 934 {
 935         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
 936         struct v4l2_mbus_framefmt *in_fmt;
 937         int ret = 0;
 938
 939         mutex_lock(&csi->lock);
 940
 941         in_fmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SINK, code->which);
 942
 943         switch (code->pad) {
 944         case IMX7_CSI_PAD_SINK:
 945                 ret = imx_media_enum_mbus_format(&code->code, code->index,
 946                                                  CS_SEL_ANY, true);
 947                 break;
 948         case IMX7_CSI_PAD_SRC:
 949                 if (code->index != 0) {
 950                         ret = -EINVAL;
 951                         goto out_unlock;
 952                 }
 953
 954                 code->code = in_fmt->code;
 955                 break;
 956         default:
 957                 ret = -EINVAL;
 958         }
 959
 960 out_unlock:
 961         mutex_unlock(&csi->lock);
 962
 963         return ret;
 964 }
 965
 966 static int imx7_csi_get_fmt(struct v4l2_subdev *sd,
 967                             struct v4l2_subdev_pad_config *cfg,
 968                             struct v4l2_subdev_format *sdformat)
 969 {
 970         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
 971         struct v4l2_mbus_framefmt *fmt;
 972         int ret = 0;
 973
 974         mutex_lock(&csi->lock);
 975
 976         fmt = imx7_csi_get_format(csi, cfg, sdformat->pad, sdformat->which);
 977         if (!fmt) {
 978                 ret = -EINVAL;
 979                 goto out_unlock;
 980         }
 981
 982         sdformat->format = *fmt;
 983
 984 out_unlock:
 985         mutex_unlock(&csi->lock);
 986
 987         return ret;
 988 }
 989
 990 static int imx7_csi_try_fmt(struct imx7_csi *csi,
 991                             struct v4l2_subdev_pad_config *cfg,
 992                             struct v4l2_subdev_format *sdformat,
 993                             const struct imx_media_pixfmt **cc)
 994 {
 995         const struct imx_media_pixfmt *in_cc;
 996         struct v4l2_mbus_framefmt *in_fmt;
 997         u32 code;
 998
 999         in_fmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SINK,
1000                                      sdformat->which);
1001         if (!in_fmt)
1002                 return -EINVAL;
1003
1004         switch (sdformat->pad) {
1005         case IMX7_CSI_PAD_SRC:
1006                 in_cc = imx_media_find_mbus_format(in_fmt->code, CS_SEL_ANY,
1007                                                    true);
1008
1009                 sdformat->format.width = in_fmt->width;
1010                 sdformat->format.height = in_fmt->height;
1011                 sdformat->format.code = in_fmt->code;
1012                 *cc = in_cc;
1013
1014                 sdformat->format.colorspace = in_fmt->colorspace;
1015                 sdformat->format.xfer_func = in_fmt->xfer_func;
1016                 break;
1017         case IMX7_CSI_PAD_SINK:
1018                 *cc = imx_media_find_mbus_format(sdformat->format.code,
1019                                                  CS_SEL_ANY, true);
1020                 if (!*cc) {
1021                         imx_media_enum_mbus_format(&code, 0, CS_SEL_ANY, false);
1022                         *cc = imx_media_find_mbus_format(code, CS_SEL_ANY,
1023                                                          false);
1024                         sdformat->format.code = (*cc)->codes[0];
1025                 }
1026                 break;
1027         default:
1028                 return -EINVAL;
1029         }
1030
1031         imx_media_try_colorimetry(&sdformat->format, false);
1032
1033         return 0;
1034 }
1035
1036 static int imx7_csi_set_fmt(struct v4l2_subdev *sd,
1037                             struct v4l2_subdev_pad_config *cfg,
1038                             struct v4l2_subdev_format *sdformat)
1039 {
1040         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1041         const struct imx_media_pixfmt *outcc;
1042         struct v4l2_mbus_framefmt *outfmt;
1043         const struct imx_media_pixfmt *cc;
1044         struct v4l2_mbus_framefmt *fmt;
1045         struct v4l2_subdev_format format;
1046         int ret = 0;
1047
1048         if (sdformat->pad >= IMX7_CSI_PADS_NUM)
1049                 return -EINVAL;
1050
1051         mutex_lock(&csi->lock);
1052
1053         if (csi->is_streaming) {
1054                 ret = -EBUSY;
1055                 goto out_unlock;
1056         }
1057
1058         ret = imx7_csi_try_fmt(csi, cfg, sdformat, &cc);
1059         if (ret < 0)
1060                 goto out_unlock;
1061
1062         fmt = imx7_csi_get_format(csi, cfg, sdformat->pad, sdformat->which);
1063         if (!fmt) {
1064                 ret = -EINVAL;
1065                 goto out_unlock;
1066         }
1067
1068         *fmt = sdformat->format;
1069
1070         if (sdformat->pad == IMX7_CSI_PAD_SINK) {
1071                 /* propagate format to source pads */
1072                 format.pad = IMX7_CSI_PAD_SRC;
1073                 format.which = sdformat->which;
1074                 format.format = sdformat->format;
1075                 if (imx7_csi_try_fmt(csi, cfg, &format, &outcc)) {
1076                         ret = -EINVAL;
1077                         goto out_unlock;
1078                 }
1079                 outfmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SRC,
1080                                              sdformat->which);
1081                 *outfmt = format.format;
1082
1083                 if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
1084                         csi->cc[IMX7_CSI_PAD_SRC] = outcc;
1085         }
1086
1087         if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
1088                 csi->cc[sdformat->pad] = cc;
1089
1090 out_unlock:
1091         mutex_unlock(&csi->lock);
1092
1093         return ret;
1094 }
1095
1096 static int imx7_csi_registered(struct v4l2_subdev *sd)
1097 {
1098         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1099         int ret;
1100         int i;
1101
1102         for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
1103                 csi->pad[i].flags = (i == IMX7_CSI_PAD_SINK) ?
1104                         MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
1105
1106                 /* set a default mbus format  */
1107                 ret = imx_media_init_mbus_fmt(&csi->format_mbus[i],
1108                                               800, 600, 0, V4L2_FIELD_NONE,
1109                                               &csi->cc[i]);
1110                 if (ret < 0)
1111                         return ret;
1112
1113                 /* init default frame interval */
1114                 csi->frame_interval[i].numerator = 1;
1115                 csi->frame_interval[i].denominator = 30;
1116         }
1117
1118         ret = media_entity_pads_init(&sd->entity, IMX7_CSI_PADS_NUM, csi->pad);
1119         if (ret < 0)
1120                 return ret;
1121
1122         return imx_media_capture_device_register(csi->vdev);
1123 }
1124
1125 static void imx7_csi_unregistered(struct v4l2_subdev *sd)
1126 {
1127         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1128
1129         imx_media_capture_device_unregister(csi->vdev);
1130 }
1131
1132 static int imx7_csi_init_cfg(struct v4l2_subdev *sd,
1133                              struct v4l2_subdev_pad_config *cfg)
1134 {
1135         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1136         struct v4l2_mbus_framefmt *mf;
1137         int ret;
1138         int i;
1139
1140         for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
1141                 mf = v4l2_subdev_get_try_format(sd, cfg, i);
1142
1143                 ret = imx_media_init_mbus_fmt(mf, 800, 600, 0, V4L2_FIELD_NONE,
1144                                               &csi->cc[i]);
1145                 if (ret < 0)
1146                         return ret;
1147         }
1148
1149         return 0;
1150 }
1151
1152 static const struct media_entity_operations imx7_csi_entity_ops = {
1153         .link_setup     = imx7_csi_link_setup,
1154         .link_validate  = v4l2_subdev_link_validate,
1155 };
1156
1157 static const struct v4l2_subdev_video_ops imx7_csi_video_ops = {
1158         .s_stream               = imx7_csi_s_stream,
1159 };
1160
1161 static const struct v4l2_subdev_pad_ops imx7_csi_pad_ops = {
1162         .init_cfg =             imx7_csi_init_cfg,
1163         .enum_mbus_code =       imx7_csi_enum_mbus_code,
1164         .get_fmt =              imx7_csi_get_fmt,
1165         .set_fmt =              imx7_csi_set_fmt,
1166         .link_validate =        imx7_csi_pad_link_validate,
1167 };
1168
1169 static const struct v4l2_subdev_ops imx7_csi_subdev_ops = {
1170         .video =        &imx7_csi_video_ops,
1171         .pad =          &imx7_csi_pad_ops,
1172 };
1173
1174 static const struct v4l2_subdev_internal_ops imx7_csi_internal_ops = {
1175         .registered     = imx7_csi_registered,
1176         .unregistered   = imx7_csi_unregistered,
1177 };
1178
1179 static int imx7_csi_parse_endpoint(struct device *dev,
1180                                    struct v4l2_fwnode_endpoint *vep,
1181                                    struct v4l2_async_subdev *asd)
1182 {
1183         return fwnode_device_is_available(asd->match.fwnode) ? 0 : -EINVAL;
1184 }
1185
1186 static int imx7_csi_probe(struct platform_device *pdev)
1187 {
1188         struct device *dev = &pdev->dev;
1189         struct device_node *node = dev->of_node;
1190         struct imx_media_dev *imxmd;
1191         struct imx7_csi *csi;
1192         int ret;
1193
1194         csi = devm_kzalloc(&pdev->dev, sizeof(*csi), GFP_KERNEL);
1195         if (!csi)
1196                 return -ENOMEM;
1197
1198         csi->dev = dev;
1199
1200         csi->mclk = devm_clk_get(&pdev->dev, "mclk");
1201         if (IS_ERR(csi->mclk)) {
1202                 ret = PTR_ERR(csi->mclk);
1203                 dev_err(dev, "Failed to get mclk: %d", ret);
1204                 return ret;
1205         }
1206
1207         csi->irq = platform_get_irq(pdev, 0);
1208         if (csi->irq < 0)
1209                 return csi->irq;
1210
1211         csi->regbase = devm_platform_ioremap_resource(pdev, 0);
1212         if (IS_ERR(csi->regbase))
1213                 return PTR_ERR(csi->regbase);
1214
1215         spin_lock_init(&csi->irqlock);
1216         mutex_init(&csi->lock);
1217
1218         /* install interrupt handler */
1219         ret = devm_request_irq(dev, csi->irq, imx7_csi_irq_handler, 0, "csi",
1220                                (void *)csi);
1221         if (ret < 0) {
1222                 dev_err(dev, "Request CSI IRQ failed.\n");
1223                 goto destroy_mutex;
1224         }
1225
1226         /* add media device */
1227         imxmd = imx_media_dev_init(dev, NULL);
1228         if (IS_ERR(imxmd)) {
1229                 ret = PTR_ERR(imxmd);
1230                 goto destroy_mutex;
1231         }
1232         platform_set_drvdata(pdev, &csi->sd);
1233
1234         ret = imx_media_of_add_csi(imxmd, node);
1235         if (ret < 0 && ret != -ENODEV && ret != -EEXIST)
1236                 goto cleanup;
1237
1238         ret = imx_media_dev_notifier_register(imxmd, NULL);
1239         if (ret < 0)
1240                 goto cleanup;
1241
1242         csi->imxmd = imxmd;
1243         v4l2_subdev_init(&csi->sd, &imx7_csi_subdev_ops);
1244         v4l2_set_subdevdata(&csi->sd, csi);
1245         csi->sd.internal_ops = &imx7_csi_internal_ops;
1246         csi->sd.entity.ops = &imx7_csi_entity_ops;
1247         csi->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1248         csi->sd.dev = &pdev->dev;
1249         csi->sd.owner = THIS_MODULE;
1250         csi->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
1251         csi->sd.grp_id = IMX_MEDIA_GRP_ID_CSI;
1252         snprintf(csi->sd.name, sizeof(csi->sd.name), "csi");
1253
1254         csi->vdev = imx_media_capture_device_init(csi->sd.dev, &csi->sd,
1255                                                   IMX7_CSI_PAD_SRC);
1256         if (IS_ERR(csi->vdev))
1257                 return PTR_ERR(csi->vdev);
1258
1259         v4l2_ctrl_handler_init(&csi->ctrl_hdlr, 0);
1260         csi->sd.ctrl_handler = &csi->ctrl_hdlr;
1261
1262         ret = v4l2_async_register_fwnode_subdev(&csi->sd,
1263                                                 sizeof(struct v4l2_async_subdev),
1264                                                 NULL, 0,
1265                                                 imx7_csi_parse_endpoint);
1266         if (ret)
1267                 goto free;
1268
1269         return 0;
1270
1271 free:
1272         imx_media_capture_device_unregister(csi->vdev);
1273         imx_media_capture_device_remove(csi->vdev);
1274         v4l2_ctrl_handler_free(&csi->ctrl_hdlr);
1275
1276 cleanup:
1277         v4l2_async_notifier_cleanup(&imxmd->notifier);
1278         v4l2_device_unregister(&imxmd->v4l2_dev);
1279         media_device_unregister(&imxmd->md);
1280         media_device_cleanup(&imxmd->md);
1281
1282 destroy_mutex:
1283         mutex_destroy(&csi->lock);
1284
1285         return ret;
1286 }
1287
1288 static int imx7_csi_remove(struct platform_device *pdev)
1289 {
1290         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
1291         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1292         struct imx_media_dev *imxmd = csi->imxmd;
1293
1294         v4l2_async_notifier_unregister(&imxmd->notifier);
1295         v4l2_async_notifier_cleanup(&imxmd->notifier);
1296
1297         media_device_unregister(&imxmd->md);
1298         v4l2_device_unregister(&imxmd->v4l2_dev);
1299         media_device_cleanup(&imxmd->md);
1300
1301         imx_media_capture_device_unregister(csi->vdev);
1302         imx_media_capture_device_remove(csi->vdev);
1303
1304         v4l2_async_unregister_subdev(sd);
1305         v4l2_ctrl_handler_free(&csi->ctrl_hdlr);
1306
1307         mutex_destroy(&csi->lock);
1308
1309         return 0;
1310 }
1311
1312 static const struct of_device_id imx7_csi_of_match[] = {
1313         { .compatible = "fsl,imx7-csi" },
1314         { .compatible = "fsl,imx6ul-csi" },
1315         { },
1316 };
1317 MODULE_DEVICE_TABLE(of, imx7_csi_of_match);
1318
1319 static struct platform_driver imx7_csi_driver = {
1320         .probe = imx7_csi_probe,
1321         .remove = imx7_csi_remove,
1322         .driver = {
1323                 .of_match_table = imx7_csi_of_match,
1324                 .name = "imx7-csi",
1325         },
1326 };
1327 module_platform_driver(imx7_csi_driver);
1328
1329 MODULE_DESCRIPTION("i.MX7 CSI subdev driver");
1330 MODULE_AUTHOR("Rui Miguel Silva <rui.silva@linaro.org>");
1331 MODULE_LICENSE("GPL v2");
1332 MODULE_ALIAS("platform:imx7-csi");