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;
 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_GREY:
 808                 if (in_code == MEDIA_BUS_FMT_Y8_1X8)
 809                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
 810                 else if (in_code == MEDIA_BUS_FMT_Y10_1X10)
 811                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
 812                 else
 813                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW12;
 814                 break;
 815         case V4L2_PIX_FMT_Y10:
 816                 cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
 817                 cr1 |= BIT_PIXEL_BIT;
 818                 break;
 819         case V4L2_PIX_FMT_Y12:
 820                 cr18 |= BIT_MIPI_DATA_FORMAT_RAW12;
 821                 cr1 |= BIT_PIXEL_BIT;
 822                 break;
 823         case V4L2_PIX_FMT_SBGGR8:
 824                 cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
 825                 break;
 826         case V4L2_PIX_FMT_SBGGR16:
 827                 if (in_code == MEDIA_BUS_FMT_SBGGR10_1X10)
 828                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
 829                 else if (in_code == MEDIA_BUS_FMT_SBGGR12_1X12)
 830                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW12;
 831                 else if (in_code == MEDIA_BUS_FMT_SBGGR14_1X14)
 832                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW14;
 833                 cr1 |= BIT_PIXEL_BIT;
 834                 break;
 835         default:
 836                 return -EINVAL;
 837         }
 838
 839         imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
 840         imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
 841
 842         return 0;
 843 }
 844
 845 static void imx7_csi_enable(struct imx7_csi *csi)
 846 {
 847         imx7_csi_sw_reset(csi);
 848
 849         imx7_csi_dmareq_rff_enable(csi);
 850         imx7_csi_hw_enable_irq(csi);
 851         imx7_csi_hw_enable(csi);
 852 }
 853
 854 static void imx7_csi_disable(struct imx7_csi *csi)
 855 {
 856         imx7_csi_dmareq_rff_disable(csi);
 857
 858         imx7_csi_hw_disable_irq(csi);
 859
 860         imx7_csi_buf_stride_set(csi, 0);
 861
 862         imx7_csi_hw_disable(csi);
 863 }
 864
 865 static int imx7_csi_streaming_start(struct imx7_csi *csi)
 866 {
 867         int ret;
 868
 869         ret = imx7_csi_dma_start(csi);
 870         if (ret < 0)
 871                 return ret;
 872
 873         ret = imx7_csi_configure(csi);
 874         if (ret < 0)
 875                 goto dma_stop;
 876
 877         imx7_csi_enable(csi);
 878
 879         return 0;
 880
 881 dma_stop:
 882         imx7_csi_dma_stop(csi);
 883
 884         return ret;
 885 }
 886
 887 static int imx7_csi_streaming_stop(struct imx7_csi *csi)
 888 {
 889         imx7_csi_dma_stop(csi);
 890
 891         imx7_csi_disable(csi);
 892
 893         return 0;
 894 }
 895
 896 static int imx7_csi_s_stream(struct v4l2_subdev *sd, int enable)
 897 {
 898         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
 899         int ret = 0;
 900
 901         mutex_lock(&csi->lock);
 902
 903         if (!csi->src_sd || !csi->sink) {
 904                 ret = -EPIPE;
 905                 goto out_unlock;
 906         }
 907
 908         if (csi->is_streaming == !!enable)
 909                 goto out_unlock;
 910
 911         if (enable) {
 912                 ret = v4l2_subdev_call(csi->src_sd, video, s_stream, 1);
 913                 if (ret < 0)
 914                         goto out_unlock;
 915
 916                 ret = imx7_csi_streaming_start(csi);
 917                 if (ret < 0) {
 918                         v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
 919                         goto out_unlock;
 920                 }
 921         } else {
 922                 imx7_csi_streaming_stop(csi);
 923
 924                 v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
 925         }
 926
 927         csi->is_streaming = !!enable;
 928
 929 out_unlock:
 930         mutex_unlock(&csi->lock);
 931
 932         return ret;
 933 }
 934
 935 static struct v4l2_mbus_framefmt *
 936 imx7_csi_get_format(struct imx7_csi *csi,
 937                     struct v4l2_subdev_pad_config *cfg,
 938                     unsigned int pad,
 939                     enum v4l2_subdev_format_whence which)
 940 {
 941         if (which == V4L2_SUBDEV_FORMAT_TRY)
 942                 return v4l2_subdev_get_try_format(&csi->sd, cfg, pad);
 943
 944         return &csi->format_mbus[pad];
 945 }
 946
 947 static int imx7_csi_enum_mbus_code(struct v4l2_subdev *sd,
 948                                    struct v4l2_subdev_pad_config *cfg,
 949                                    struct v4l2_subdev_mbus_code_enum *code)
 950 {
 951         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
 952         struct v4l2_mbus_framefmt *in_fmt;
 953         int ret = 0;
 954
 955         mutex_lock(&csi->lock);
 956
 957         in_fmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SINK, code->which);
 958
 959         switch (code->pad) {
 960         case IMX7_CSI_PAD_SINK:
 961                 ret = imx_media_enum_mbus_format(&code->code, code->index,
 962                                                  CS_SEL_ANY, true);
 963                 break;
 964         case IMX7_CSI_PAD_SRC:
 965                 if (code->index != 0) {
 966                         ret = -EINVAL;
 967                         goto out_unlock;
 968                 }
 969
 970                 code->code = in_fmt->code;
 971                 break;
 972         default:
 973                 ret = -EINVAL;
 974         }
 975
 976 out_unlock:
 977         mutex_unlock(&csi->lock);
 978
 979         return ret;
 980 }
 981
 982 static int imx7_csi_get_fmt(struct v4l2_subdev *sd,
 983                             struct v4l2_subdev_pad_config *cfg,
 984                             struct v4l2_subdev_format *sdformat)
 985 {
 986         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
 987         struct v4l2_mbus_framefmt *fmt;
 988         int ret = 0;
 989
 990         mutex_lock(&csi->lock);
 991
 992         fmt = imx7_csi_get_format(csi, cfg, sdformat->pad, sdformat->which);
 993         if (!fmt) {
 994                 ret = -EINVAL;
 995                 goto out_unlock;
 996         }
 997
 998         sdformat->format = *fmt;
 999
1000 out_unlock:
1001         mutex_unlock(&csi->lock);
1002
1003         return ret;
1004 }
1005
1006 static int imx7_csi_try_fmt(struct imx7_csi *csi,
1007                             struct v4l2_subdev_pad_config *cfg,
1008                             struct v4l2_subdev_format *sdformat,
1009                             const struct imx_media_pixfmt **cc)
1010 {
1011         const struct imx_media_pixfmt *in_cc;
1012         struct v4l2_mbus_framefmt *in_fmt;
1013         u32 code;
1014
1015         in_fmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SINK,
1016                                      sdformat->which);
1017         if (!in_fmt)
1018                 return -EINVAL;
1019
1020         switch (sdformat->pad) {
1021         case IMX7_CSI_PAD_SRC:
1022                 in_cc = imx_media_find_mbus_format(in_fmt->code, CS_SEL_ANY,
1023                                                    true);
1024
1025                 sdformat->format.width = in_fmt->width;
1026                 sdformat->format.height = in_fmt->height;
1027                 sdformat->format.code = in_fmt->code;
1028                 sdformat->format.field = in_fmt->field;
1029                 *cc = in_cc;
1030
1031                 sdformat->format.colorspace = in_fmt->colorspace;
1032                 sdformat->format.xfer_func = in_fmt->xfer_func;
1033                 sdformat->format.quantization = in_fmt->quantization;
1034                 sdformat->format.ycbcr_enc = in_fmt->ycbcr_enc;
1035                 break;
1036         case IMX7_CSI_PAD_SINK:
1037                 *cc = imx_media_find_mbus_format(sdformat->format.code,
1038                                                  CS_SEL_ANY, true);
1039                 if (!*cc) {
1040                         imx_media_enum_mbus_format(&code, 0, CS_SEL_ANY, false);
1041                         *cc = imx_media_find_mbus_format(code, CS_SEL_ANY,
1042                                                          false);
1043                         sdformat->format.code = (*cc)->codes[0];
1044                 }
1045
1046                 if (sdformat->format.field != V4L2_FIELD_INTERLACED)
1047                         sdformat->format.field = V4L2_FIELD_NONE;
1048                 break;
1049         default:
1050                 return -EINVAL;
1051         }
1052
1053         imx_media_try_colorimetry(&sdformat->format, false);
1054
1055         return 0;
1056 }
1057
1058 static int imx7_csi_set_fmt(struct v4l2_subdev *sd,
1059                             struct v4l2_subdev_pad_config *cfg,
1060                             struct v4l2_subdev_format *sdformat)
1061 {
1062         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1063         const struct imx_media_pixfmt *outcc;
1064         struct v4l2_mbus_framefmt *outfmt;
1065         const struct imx_media_pixfmt *cc;
1066         struct v4l2_mbus_framefmt *fmt;
1067         struct v4l2_subdev_format format;
1068         int ret = 0;
1069
1070         if (sdformat->pad >= IMX7_CSI_PADS_NUM)
1071                 return -EINVAL;
1072
1073         mutex_lock(&csi->lock);
1074
1075         if (csi->is_streaming) {
1076                 ret = -EBUSY;
1077                 goto out_unlock;
1078         }
1079
1080         ret = imx7_csi_try_fmt(csi, cfg, sdformat, &cc);
1081         if (ret < 0)
1082                 goto out_unlock;
1083
1084         fmt = imx7_csi_get_format(csi, cfg, sdformat->pad, sdformat->which);
1085         if (!fmt) {
1086                 ret = -EINVAL;
1087                 goto out_unlock;
1088         }
1089
1090         *fmt = sdformat->format;
1091
1092         if (sdformat->pad == IMX7_CSI_PAD_SINK) {
1093                 /* propagate format to source pads */
1094                 format.pad = IMX7_CSI_PAD_SRC;
1095                 format.which = sdformat->which;
1096                 format.format = sdformat->format;
1097                 if (imx7_csi_try_fmt(csi, cfg, &format, &outcc)) {
1098                         ret = -EINVAL;
1099                         goto out_unlock;
1100                 }
1101                 outfmt = imx7_csi_get_format(csi, cfg, IMX7_CSI_PAD_SRC,
1102                                              sdformat->which);
1103                 *outfmt = format.format;
1104
1105                 if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
1106                         csi->cc[IMX7_CSI_PAD_SRC] = outcc;
1107         }
1108
1109         if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
1110                 csi->cc[sdformat->pad] = cc;
1111
1112 out_unlock:
1113         mutex_unlock(&csi->lock);
1114
1115         return ret;
1116 }
1117
1118 static int imx7_csi_registered(struct v4l2_subdev *sd)
1119 {
1120         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1121         int ret;
1122         int i;
1123
1124         for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
1125                 /* set a default mbus format  */
1126                 ret = imx_media_init_mbus_fmt(&csi->format_mbus[i],
1127                                               800, 600, 0, V4L2_FIELD_NONE,
1128                                               &csi->cc[i]);
1129                 if (ret < 0)
1130                         return ret;
1131
1132                 /* init default frame interval */
1133                 csi->frame_interval[i].numerator = 1;
1134                 csi->frame_interval[i].denominator = 30;
1135         }
1136
1137         csi->vdev = imx_media_capture_device_init(csi->sd.dev, &csi->sd,
1138                                                   IMX7_CSI_PAD_SRC);
1139         if (IS_ERR(csi->vdev))
1140                 return PTR_ERR(csi->vdev);
1141
1142         ret = imx_media_capture_device_register(csi->vdev);
1143         if (ret)
1144                 imx_media_capture_device_remove(csi->vdev);
1145
1146         return ret;
1147 }
1148
1149 static void imx7_csi_unregistered(struct v4l2_subdev *sd)
1150 {
1151         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1152
1153         imx_media_capture_device_unregister(csi->vdev);
1154         imx_media_capture_device_remove(csi->vdev);
1155 }
1156
1157 static int imx7_csi_init_cfg(struct v4l2_subdev *sd,
1158                              struct v4l2_subdev_pad_config *cfg)
1159 {
1160         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1161         struct v4l2_mbus_framefmt *mf;
1162         int ret;
1163         int i;
1164
1165         for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
1166                 mf = v4l2_subdev_get_try_format(sd, cfg, i);
1167
1168                 ret = imx_media_init_mbus_fmt(mf, 800, 600, 0, V4L2_FIELD_NONE,
1169                                               &csi->cc[i]);
1170                 if (ret < 0)
1171                         return ret;
1172         }
1173
1174         return 0;
1175 }
1176
1177 static const struct media_entity_operations imx7_csi_entity_ops = {
1178         .link_setup     = imx7_csi_link_setup,
1179         .link_validate  = v4l2_subdev_link_validate,
1180 };
1181
1182 static const struct v4l2_subdev_video_ops imx7_csi_video_ops = {
1183         .s_stream               = imx7_csi_s_stream,
1184 };
1185
1186 static const struct v4l2_subdev_pad_ops imx7_csi_pad_ops = {
1187         .init_cfg =             imx7_csi_init_cfg,
1188         .enum_mbus_code =       imx7_csi_enum_mbus_code,
1189         .get_fmt =              imx7_csi_get_fmt,
1190         .set_fmt =              imx7_csi_set_fmt,
1191         .link_validate =        imx7_csi_pad_link_validate,
1192 };
1193
1194 static const struct v4l2_subdev_ops imx7_csi_subdev_ops = {
1195         .video =        &imx7_csi_video_ops,
1196         .pad =          &imx7_csi_pad_ops,
1197 };
1198
1199 static const struct v4l2_subdev_internal_ops imx7_csi_internal_ops = {
1200         .registered     = imx7_csi_registered,
1201         .unregistered   = imx7_csi_unregistered,
1202 };
1203
1204 static int imx7_csi_parse_endpoint(struct device *dev,
1205                                    struct v4l2_fwnode_endpoint *vep,
1206                                    struct v4l2_async_subdev *asd)
1207 {
1208         return fwnode_device_is_available(asd->match.fwnode) ? 0 : -EINVAL;
1209 }
1210
1211 static int imx7_csi_probe(struct platform_device *pdev)
1212 {
1213         struct device *dev = &pdev->dev;
1214         struct device_node *node = dev->of_node;
1215         struct imx_media_dev *imxmd;
1216         struct imx7_csi *csi;
1217         int i, ret;
1218
1219         csi = devm_kzalloc(&pdev->dev, sizeof(*csi), GFP_KERNEL);
1220         if (!csi)
1221                 return -ENOMEM;
1222
1223         csi->dev = dev;
1224
1225         csi->mclk = devm_clk_get(&pdev->dev, "mclk");
1226         if (IS_ERR(csi->mclk)) {
1227                 ret = PTR_ERR(csi->mclk);
1228                 dev_err(dev, "Failed to get mclk: %d", ret);
1229                 return ret;
1230         }
1231
1232         csi->irq = platform_get_irq(pdev, 0);
1233         if (csi->irq < 0)
1234                 return csi->irq;
1235
1236         csi->regbase = devm_platform_ioremap_resource(pdev, 0);
1237         if (IS_ERR(csi->regbase))
1238                 return PTR_ERR(csi->regbase);
1239
1240         spin_lock_init(&csi->irqlock);
1241         mutex_init(&csi->lock);
1242
1243         /* install interrupt handler */
1244         ret = devm_request_irq(dev, csi->irq, imx7_csi_irq_handler, 0, "csi",
1245                                (void *)csi);
1246         if (ret < 0) {
1247                 dev_err(dev, "Request CSI IRQ failed.\n");
1248                 goto destroy_mutex;
1249         }
1250
1251         /* add media device */
1252         imxmd = imx_media_dev_init(dev, NULL);
1253         if (IS_ERR(imxmd)) {
1254                 ret = PTR_ERR(imxmd);
1255                 goto destroy_mutex;
1256         }
1257         platform_set_drvdata(pdev, &csi->sd);
1258
1259         ret = imx_media_of_add_csi(imxmd, node);
1260         if (ret < 0 && ret != -ENODEV && ret != -EEXIST)
1261                 goto cleanup;
1262
1263         ret = imx_media_dev_notifier_register(imxmd, NULL);
1264         if (ret < 0)
1265                 goto cleanup;
1266
1267         csi->imxmd = imxmd;
1268         v4l2_subdev_init(&csi->sd, &imx7_csi_subdev_ops);
1269         v4l2_set_subdevdata(&csi->sd, csi);
1270         csi->sd.internal_ops = &imx7_csi_internal_ops;
1271         csi->sd.entity.ops = &imx7_csi_entity_ops;
1272         csi->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1273         csi->sd.dev = &pdev->dev;
1274         csi->sd.owner = THIS_MODULE;
1275         csi->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
1276         csi->sd.grp_id = IMX_MEDIA_GRP_ID_CSI;
1277         snprintf(csi->sd.name, sizeof(csi->sd.name), "csi");
1278
1279         v4l2_ctrl_handler_init(&csi->ctrl_hdlr, 0);
1280         csi->sd.ctrl_handler = &csi->ctrl_hdlr;
1281
1282         for (i = 0; i < IMX7_CSI_PADS_NUM; i++)
1283                 csi->pad[i].flags = (i == IMX7_CSI_PAD_SINK) ?
1284                         MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
1285
1286         ret = media_entity_pads_init(&csi->sd.entity, IMX7_CSI_PADS_NUM,
1287                                      csi->pad);
1288         if (ret < 0)
1289                 goto free;
1290
1291         ret = v4l2_async_register_fwnode_subdev(&csi->sd,
1292                                                 sizeof(struct v4l2_async_subdev),
1293                                                 NULL, 0,
1294                                                 imx7_csi_parse_endpoint);
1295         if (ret)
1296                 goto free;
1297
1298         return 0;
1299
1300 free:
1301         v4l2_ctrl_handler_free(&csi->ctrl_hdlr);
1302
1303 cleanup:
1304         v4l2_async_notifier_cleanup(&imxmd->notifier);
1305         v4l2_device_unregister(&imxmd->v4l2_dev);
1306         media_device_unregister(&imxmd->md);
1307         media_device_cleanup(&imxmd->md);
1308
1309 destroy_mutex:
1310         mutex_destroy(&csi->lock);
1311
1312         return ret;
1313 }
1314
1315 static int imx7_csi_remove(struct platform_device *pdev)
1316 {
1317         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
1318         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1319         struct imx_media_dev *imxmd = csi->imxmd;
1320
1321         v4l2_async_notifier_unregister(&imxmd->notifier);
1322         v4l2_async_notifier_cleanup(&imxmd->notifier);
1323
1324         media_device_unregister(&imxmd->md);
1325         v4l2_device_unregister(&imxmd->v4l2_dev);
1326         media_device_cleanup(&imxmd->md);
1327
1328         v4l2_async_unregister_subdev(sd);
1329         v4l2_ctrl_handler_free(&csi->ctrl_hdlr);
1330
1331         mutex_destroy(&csi->lock);
1332
1333         return 0;
1334 }
1335
1336 static const struct of_device_id imx7_csi_of_match[] = {
1337         { .compatible = "fsl,imx7-csi" },
1338         { .compatible = "fsl,imx6ul-csi" },
1339         { },
1340 };
1341 MODULE_DEVICE_TABLE(of, imx7_csi_of_match);
1342
1343 static struct platform_driver imx7_csi_driver = {
1344         .probe = imx7_csi_probe,
1345         .remove = imx7_csi_remove,
1346         .driver = {
1347                 .of_match_table = imx7_csi_of_match,
1348                 .name = "imx7-csi",
1349         },
1350 };
1351 module_platform_driver(imx7_csi_driver);
1352
1353 MODULE_DESCRIPTION("i.MX7 CSI subdev driver");
1354 MODULE_AUTHOR("Rui Miguel Silva <rui.silva@linaro.org>");
1355 MODULE_LICENSE("GPL v2");
1356 MODULE_ALIAS("platform:imx7-csi");