drivers/media/platform/nxp/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 #include <linux/clk.h>
   9 #include <linux/completion.h>
  10 #include <linux/container_of.h>
  11 #include <linux/delay.h>
  12 #include <linux/device.h>
  13 #include <linux/dma-mapping.h>
  14 #include <linux/err.h>
  15 #include <linux/interrupt.h>
  16 #include <linux/io.h>
  17 #include <linux/jiffies.h>
  18 #include <linux/kernel.h>
  19 #include <linux/list.h>
  20 #include <linux/math.h>
  21 #include <linux/minmax.h>
  22 #include <linux/module.h>
  23 #include <linux/mutex.h>
  24 #include <linux/of.h>
  25 #include <linux/platform_device.h>
  26 #include <linux/property.h>
  27 #include <linux/slab.h>
  28 #include <linux/spinlock.h>
  29 #include <linux/string.h>
  30 #include <linux/timekeeping.h>
  31 #include <linux/types.h>
  32
  33 #include <media/media-device.h>
  34 #include <media/media-entity.h>
  35 #include <media/v4l2-async.h>
  36 #include <media/v4l2-common.h>
  37 #include <media/v4l2-dev.h>
  38 #include <media/v4l2-device.h>
  39 #include <media/v4l2-fh.h>
  40 #include <media/v4l2-ioctl.h>
  41 #include <media/v4l2-mc.h>
  42 #include <media/v4l2-subdev.h>
  43 #include <media/videobuf2-core.h>
  44 #include <media/videobuf2-dma-contig.h>
  45 #include <media/videobuf2-v4l2.h>
  46
  47 #define IMX7_CSI_PAD_SINK               0
  48 #define IMX7_CSI_PAD_SRC                1
  49 #define IMX7_CSI_PADS_NUM               2
  50
  51 /* csi control reg 1 */
  52 #define BIT_SWAP16_EN                   BIT(31)
  53 #define BIT_EXT_VSYNC                   BIT(30)
  54 #define BIT_EOF_INT_EN                  BIT(29)
  55 #define BIT_PRP_IF_EN                   BIT(28)
  56 #define BIT_CCIR_MODE                   BIT(27)
  57 #define BIT_COF_INT_EN                  BIT(26)
  58 #define BIT_SF_OR_INTEN                 BIT(25)
  59 #define BIT_RF_OR_INTEN                 BIT(24)
  60 #define BIT_SFF_DMA_DONE_INTEN          BIT(22)
  61 #define BIT_STATFF_INTEN                BIT(21)
  62 #define BIT_FB2_DMA_DONE_INTEN          BIT(20)
  63 #define BIT_FB1_DMA_DONE_INTEN          BIT(19)
  64 #define BIT_RXFF_INTEN                  BIT(18)
  65 #define BIT_SOF_POL                     BIT(17)
  66 #define BIT_SOF_INTEN                   BIT(16)
  67 #define BIT_MCLKDIV(n)                  ((n) << 12)
  68 #define BIT_MCLKDIV_MASK                (0xf << 12)
  69 #define BIT_HSYNC_POL                   BIT(11)
  70 #define BIT_CCIR_EN                     BIT(10)
  71 #define BIT_MCLKEN                      BIT(9)
  72 #define BIT_FCC                         BIT(8)
  73 #define BIT_PACK_DIR                    BIT(7)
  74 #define BIT_CLR_STATFIFO                BIT(6)
  75 #define BIT_CLR_RXFIFO                  BIT(5)
  76 #define BIT_GCLK_MODE                   BIT(4)
  77 #define BIT_INV_DATA                    BIT(3)
  78 #define BIT_INV_PCLK                    BIT(2)
  79 #define BIT_REDGE                       BIT(1)
  80 #define BIT_PIXEL_BIT                   BIT(0)
  81
  82 /* control reg 2 */
  83 #define BIT_DMA_BURST_TYPE_RFF_INCR4    (1 << 30)
  84 #define BIT_DMA_BURST_TYPE_RFF_INCR8    (2 << 30)
  85 #define BIT_DMA_BURST_TYPE_RFF_INCR16   (3 << 30)
  86 #define BIT_DMA_BURST_TYPE_RFF_MASK     (3 << 30)
  87
  88 /* control reg 3 */
  89 #define BIT_FRMCNT(n)                   ((n) << 16)
  90 #define BIT_FRMCNT_MASK                 (0xffff << 16)
  91 #define BIT_FRMCNT_RST                  BIT(15)
  92 #define BIT_DMA_REFLASH_RFF             BIT(14)
  93 #define BIT_DMA_REFLASH_SFF             BIT(13)
  94 #define BIT_DMA_REQ_EN_RFF              BIT(12)
  95 #define BIT_DMA_REQ_EN_SFF              BIT(11)
  96 #define BIT_STATFF_LEVEL(n)             ((n) << 8)
  97 #define BIT_STATFF_LEVEL_MASK           (0x7 << 8)
  98 #define BIT_HRESP_ERR_EN                BIT(7)
  99 #define BIT_RXFF_LEVEL(n)               ((n) << 4)
 100 #define BIT_RXFF_LEVEL_MASK             (0x7 << 4)
 101 #define BIT_TWO_8BIT_SENSOR             BIT(3)
 102 #define BIT_ZERO_PACK_EN                BIT(2)
 103 #define BIT_ECC_INT_EN                  BIT(1)
 104 #define BIT_ECC_AUTO_EN                 BIT(0)
 105
 106 /* csi status reg */
 107 #define BIT_ADDR_CH_ERR_INT             BIT(28)
 108 #define BIT_FIELD0_INT                  BIT(27)
 109 #define BIT_FIELD1_INT                  BIT(26)
 110 #define BIT_SFF_OR_INT                  BIT(25)
 111 #define BIT_RFF_OR_INT                  BIT(24)
 112 #define BIT_DMA_TSF_DONE_SFF            BIT(22)
 113 #define BIT_STATFF_INT                  BIT(21)
 114 #define BIT_DMA_TSF_DONE_FB2            BIT(20)
 115 #define BIT_DMA_TSF_DONE_FB1            BIT(19)
 116 #define BIT_RXFF_INT                    BIT(18)
 117 #define BIT_EOF_INT                     BIT(17)
 118 #define BIT_SOF_INT                     BIT(16)
 119 #define BIT_F2_INT                      BIT(15)
 120 #define BIT_F1_INT                      BIT(14)
 121 #define BIT_COF_INT                     BIT(13)
 122 #define BIT_HRESP_ERR_INT               BIT(7)
 123 #define BIT_ECC_INT                     BIT(1)
 124 #define BIT_DRDY                        BIT(0)
 125
 126 /* csi image parameter reg */
 127 #define BIT_IMAGE_WIDTH(n)              ((n) << 16)
 128 #define BIT_IMAGE_HEIGHT(n)             (n)
 129
 130 /* csi control reg 18 */
 131 #define BIT_CSI_HW_ENABLE               BIT(31)
 132 #define BIT_MIPI_DATA_FORMAT_RAW8       (0x2a << 25)
 133 #define BIT_MIPI_DATA_FORMAT_RAW10      (0x2b << 25)
 134 #define BIT_MIPI_DATA_FORMAT_RAW12      (0x2c << 25)
 135 #define BIT_MIPI_DATA_FORMAT_RAW14      (0x2d << 25)
 136 #define BIT_MIPI_DATA_FORMAT_YUV422_8B  (0x1e << 25)
 137 #define BIT_MIPI_DATA_FORMAT_MASK       (0x3f << 25)
 138 #define BIT_DATA_FROM_MIPI              BIT(22)
 139 #define BIT_MIPI_YU_SWAP                BIT(21)
 140 #define BIT_MIPI_DOUBLE_CMPNT           BIT(20)
 141 #define BIT_MASK_OPTION_FIRST_FRAME     (0 << 18)
 142 #define BIT_MASK_OPTION_CSI_EN          (1 << 18)
 143 #define BIT_MASK_OPTION_SECOND_FRAME    (2 << 18)
 144 #define BIT_MASK_OPTION_ON_DATA         (3 << 18)
 145 #define BIT_BASEADDR_CHG_ERR_EN         BIT(9)
 146 #define BIT_BASEADDR_SWITCH_SEL         BIT(5)
 147 #define BIT_BASEADDR_SWITCH_EN          BIT(4)
 148 #define BIT_PARALLEL24_EN               BIT(3)
 149 #define BIT_DEINTERLACE_EN              BIT(2)
 150 #define BIT_TVDECODER_IN_EN             BIT(1)
 151 #define BIT_NTSC_EN                     BIT(0)
 152
 153 #define CSI_MCLK_VF                     1
 154 #define CSI_MCLK_ENC                    2
 155 #define CSI_MCLK_RAW                    4
 156 #define CSI_MCLK_I2C                    8
 157
 158 #define CSI_CSICR1                      0x00
 159 #define CSI_CSICR2                      0x04
 160 #define CSI_CSICR3                      0x08
 161 #define CSI_STATFIFO                    0x0c
 162 #define CSI_CSIRXFIFO                   0x10
 163 #define CSI_CSIRXCNT                    0x14
 164 #define CSI_CSISR                       0x18
 165
 166 #define CSI_CSIDBG                      0x1c
 167 #define CSI_CSIDMASA_STATFIFO           0x20
 168 #define CSI_CSIDMATS_STATFIFO           0x24
 169 #define CSI_CSIDMASA_FB1                0x28
 170 #define CSI_CSIDMASA_FB2                0x2c
 171 #define CSI_CSIFBUF_PARA                0x30
 172 #define CSI_CSIIMAG_PARA                0x34
 173
 174 #define CSI_CSICR18                     0x48
 175 #define CSI_CSICR19                     0x4c
 176
 177 #define IMX7_CSI_VIDEO_NAME             "imx-capture"
 178 /* In bytes, per queue */
 179 #define IMX7_CSI_VIDEO_MEM_LIMIT        SZ_512M
 180 #define IMX7_CSI_VIDEO_EOF_TIMEOUT      2000
 181
 182 #define IMX7_CSI_DEF_MBUS_CODE          MEDIA_BUS_FMT_UYVY8_2X8
 183 #define IMX7_CSI_DEF_PIX_FORMAT         V4L2_PIX_FMT_UYVY
 184 #define IMX7_CSI_DEF_PIX_WIDTH          640
 185 #define IMX7_CSI_DEF_PIX_HEIGHT         480
 186
 187 enum imx_csi_model {
 188         IMX7_CSI_IMX7 = 0,
 189         IMX7_CSI_IMX8MQ,
 190 };
 191
 192 struct imx7_csi_pixfmt {
 193         /* the in-memory FourCC pixel format */
 194         u32     fourcc;
 195         /*
 196          * the set of equivalent media bus codes for the fourcc.
 197          * NOTE! codes pointer is NULL for in-memory-only formats.
 198          */
 199         const u32 *codes;
 200         int     bpp;     /* total bpp */
 201         bool    yuv;
 202 };
 203
 204 struct imx7_csi_vb2_buffer {
 205         struct vb2_v4l2_buffer vbuf;
 206         struct list_head list;
 207 };
 208
 209 static inline struct imx7_csi_vb2_buffer *
 210 to_imx7_csi_vb2_buffer(struct vb2_buffer *vb)
 211 {
 212         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 213
 214         return container_of(vbuf, struct imx7_csi_vb2_buffer, vbuf);
 215 }
 216
 217 struct imx7_csi_dma_buf {
 218         void *virt;
 219         dma_addr_t dma_addr;
 220         unsigned long len;
 221 };
 222
 223 struct imx7_csi {
 224         struct device *dev;
 225
 226         /* Resources and locks */
 227         void __iomem *regbase;
 228         int irq;
 229         struct clk *mclk;
 230
 231         spinlock_t irqlock; /* Protects last_eof */
 232
 233         /* Media and V4L2 device */
 234         struct media_device mdev;
 235         struct v4l2_device v4l2_dev;
 236         struct v4l2_async_notifier notifier;
 237         struct media_pipeline pipe;
 238
 239         struct v4l2_subdev *src_sd;
 240         bool is_csi2;
 241
 242         /* V4L2 subdev */
 243         struct v4l2_subdev sd;
 244         struct media_pad pad[IMX7_CSI_PADS_NUM];
 245
 246         /* Video device */
 247         struct video_device *vdev;              /* Video device */
 248         struct media_pad vdev_pad;              /* Video device pad */
 249
 250         struct v4l2_pix_format vdev_fmt;        /* The user format */
 251         const struct imx7_csi_pixfmt *vdev_cc;
 252         struct v4l2_rect vdev_compose;          /* The compose rectangle */
 253
 254         struct mutex vdev_mutex;                /* Protect vdev operations */
 255
 256         struct vb2_queue q;                     /* The videobuf2 queue */
 257         struct list_head ready_q;               /* List of queued buffers */
 258         spinlock_t q_lock;                      /* Protect ready_q */
 259
 260         /* Buffers and streaming state */
 261         struct imx7_csi_vb2_buffer *active_vb2_buf[2];
 262         struct imx7_csi_dma_buf underrun_buf;
 263
 264         bool is_streaming;
 265         int buf_num;
 266         u32 frame_sequence;
 267
 268         bool last_eof;
 269         struct completion last_eof_completion;
 270
 271         enum imx_csi_model model;
 272 };
 273
 274 static struct imx7_csi *
 275 imx7_csi_notifier_to_dev(struct v4l2_async_notifier *n)
 276 {
 277         return container_of(n, struct imx7_csi, notifier);
 278 }
 279
 280 /* -----------------------------------------------------------------------------
 281  * Hardware Configuration
 282  */
 283
 284 static u32 imx7_csi_reg_read(struct imx7_csi *csi, unsigned int offset)
 285 {
 286         return readl(csi->regbase + offset);
 287 }
 288
 289 static void imx7_csi_reg_write(struct imx7_csi *csi, unsigned int value,
 290                                unsigned int offset)
 291 {
 292         writel(value, csi->regbase + offset);
 293 }
 294
 295 static u32 imx7_csi_irq_clear(struct imx7_csi *csi)
 296 {
 297         u32 isr;
 298
 299         isr = imx7_csi_reg_read(csi, CSI_CSISR);
 300         imx7_csi_reg_write(csi, isr, CSI_CSISR);
 301
 302         return isr;
 303 }
 304
 305 static void imx7_csi_init_default(struct imx7_csi *csi)
 306 {
 307         imx7_csi_reg_write(csi, BIT_SOF_POL | BIT_REDGE | BIT_GCLK_MODE |
 308                            BIT_HSYNC_POL | BIT_FCC | BIT_MCLKDIV(1) |
 309                            BIT_MCLKEN, CSI_CSICR1);
 310         imx7_csi_reg_write(csi, 0, CSI_CSICR2);
 311         imx7_csi_reg_write(csi, BIT_FRMCNT_RST, CSI_CSICR3);
 312
 313         imx7_csi_reg_write(csi, BIT_IMAGE_WIDTH(IMX7_CSI_DEF_PIX_WIDTH) |
 314                            BIT_IMAGE_HEIGHT(IMX7_CSI_DEF_PIX_HEIGHT),
 315                            CSI_CSIIMAG_PARA);
 316
 317         imx7_csi_reg_write(csi, BIT_DMA_REFLASH_RFF, CSI_CSICR3);
 318 }
 319
 320 static void imx7_csi_hw_enable_irq(struct imx7_csi *csi)
 321 {
 322         u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
 323
 324         cr1 |= BIT_RFF_OR_INT;
 325         cr1 |= BIT_FB1_DMA_DONE_INTEN;
 326         cr1 |= BIT_FB2_DMA_DONE_INTEN;
 327
 328         imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
 329 }
 330
 331 static void imx7_csi_hw_disable_irq(struct imx7_csi *csi)
 332 {
 333         u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1);
 334
 335         cr1 &= ~BIT_RFF_OR_INT;
 336         cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
 337         cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
 338
 339         imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
 340 }
 341
 342 static void imx7_csi_hw_enable(struct imx7_csi *csi)
 343 {
 344         u32 cr = imx7_csi_reg_read(csi, CSI_CSICR18);
 345
 346         cr |= BIT_CSI_HW_ENABLE;
 347
 348         imx7_csi_reg_write(csi, cr, CSI_CSICR18);
 349 }
 350
 351 static void imx7_csi_hw_disable(struct imx7_csi *csi)
 352 {
 353         u32 cr = imx7_csi_reg_read(csi, CSI_CSICR18);
 354
 355         cr &= ~BIT_CSI_HW_ENABLE;
 356
 357         imx7_csi_reg_write(csi, cr, CSI_CSICR18);
 358 }
 359
 360 static void imx7_csi_dma_reflash(struct imx7_csi *csi)
 361 {
 362         u32 cr3;
 363
 364         cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
 365         cr3 |= BIT_DMA_REFLASH_RFF;
 366         imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
 367 }
 368
 369 static void imx7_csi_rx_fifo_clear(struct imx7_csi *csi)
 370 {
 371         u32 cr1 = imx7_csi_reg_read(csi, CSI_CSICR1) & ~BIT_FCC;
 372
 373         imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
 374         imx7_csi_reg_write(csi, cr1 | BIT_CLR_RXFIFO, CSI_CSICR1);
 375         imx7_csi_reg_write(csi, cr1 | BIT_FCC, CSI_CSICR1);
 376 }
 377
 378 static void imx7_csi_dmareq_rff_enable(struct imx7_csi *csi)
 379 {
 380         u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
 381
 382         cr3 |= BIT_DMA_REQ_EN_RFF;
 383         cr3 |= BIT_HRESP_ERR_EN;
 384         cr3 &= ~BIT_RXFF_LEVEL_MASK;
 385         cr3 |= BIT_RXFF_LEVEL(2);
 386
 387         imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
 388 }
 389
 390 static void imx7_csi_dmareq_rff_disable(struct imx7_csi *csi)
 391 {
 392         u32 cr3 = imx7_csi_reg_read(csi, CSI_CSICR3);
 393
 394         cr3 &= ~BIT_DMA_REQ_EN_RFF;
 395         cr3 &= ~BIT_HRESP_ERR_EN;
 396         imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
 397 }
 398
 399 static void imx7_csi_update_buf(struct imx7_csi *csi, dma_addr_t dma_addr,
 400                                 int buf_num)
 401 {
 402         if (buf_num == 1)
 403                 imx7_csi_reg_write(csi, dma_addr, CSI_CSIDMASA_FB2);
 404         else
 405                 imx7_csi_reg_write(csi, dma_addr, CSI_CSIDMASA_FB1);
 406 }
 407
 408 static struct imx7_csi_vb2_buffer *imx7_csi_video_next_buf(struct imx7_csi *csi);
 409
 410 static void imx7_csi_setup_vb2_buf(struct imx7_csi *csi)
 411 {
 412         struct imx7_csi_vb2_buffer *buf;
 413         struct vb2_buffer *vb2_buf;
 414         int i;
 415
 416         for (i = 0; i < 2; i++) {
 417                 dma_addr_t dma_addr;
 418
 419                 buf = imx7_csi_video_next_buf(csi);
 420                 if (buf) {
 421                         csi->active_vb2_buf[i] = buf;
 422                         vb2_buf = &buf->vbuf.vb2_buf;
 423                         dma_addr = vb2_dma_contig_plane_dma_addr(vb2_buf, 0);
 424                 } else {
 425                         csi->active_vb2_buf[i] = NULL;
 426                         dma_addr = csi->underrun_buf.dma_addr;
 427                 }
 428
 429                 imx7_csi_update_buf(csi, dma_addr, i);
 430         }
 431 }
 432
 433 static void imx7_csi_dma_unsetup_vb2_buf(struct imx7_csi *csi,
 434                                          enum vb2_buffer_state return_status)
 435 {
 436         struct imx7_csi_vb2_buffer *buf;
 437         int i;
 438
 439         /* return any remaining active frames with return_status */
 440         for (i = 0; i < 2; i++) {
 441                 buf = csi->active_vb2_buf[i];
 442                 if (buf) {
 443                         struct vb2_buffer *vb = &buf->vbuf.vb2_buf;
 444
 445                         vb->timestamp = ktime_get_ns();
 446                         vb2_buffer_done(vb, return_status);
 447                         csi->active_vb2_buf[i] = NULL;
 448                 }
 449         }
 450 }
 451
 452 static void imx7_csi_free_dma_buf(struct imx7_csi *csi,
 453                                   struct imx7_csi_dma_buf *buf)
 454 {
 455         if (buf->virt)
 456                 dma_free_coherent(csi->dev, buf->len, buf->virt, buf->dma_addr);
 457
 458         buf->virt = NULL;
 459         buf->dma_addr = 0;
 460 }
 461
 462 static int imx7_csi_alloc_dma_buf(struct imx7_csi *csi,
 463                                   struct imx7_csi_dma_buf *buf, int size)
 464 {
 465         imx7_csi_free_dma_buf(csi, buf);
 466
 467         buf->len = PAGE_ALIGN(size);
 468         buf->virt = dma_alloc_coherent(csi->dev, buf->len, &buf->dma_addr,
 469                                        GFP_DMA | GFP_KERNEL);
 470         if (!buf->virt)
 471                 return -ENOMEM;
 472
 473         return 0;
 474 }
 475
 476 static int imx7_csi_dma_setup(struct imx7_csi *csi)
 477 {
 478         int ret;
 479
 480         ret = imx7_csi_alloc_dma_buf(csi, &csi->underrun_buf,
 481                                      csi->vdev_fmt.sizeimage);
 482         if (ret < 0) {
 483                 v4l2_warn(&csi->sd, "consider increasing the CMA area\n");
 484                 return ret;
 485         }
 486
 487         csi->frame_sequence = 0;
 488         csi->last_eof = false;
 489         init_completion(&csi->last_eof_completion);
 490
 491         imx7_csi_setup_vb2_buf(csi);
 492
 493         return 0;
 494 }
 495
 496 static void imx7_csi_dma_cleanup(struct imx7_csi *csi,
 497                                  enum vb2_buffer_state return_status)
 498 {
 499         imx7_csi_dma_unsetup_vb2_buf(csi, return_status);
 500         imx7_csi_free_dma_buf(csi, &csi->underrun_buf);
 501 }
 502
 503 static void imx7_csi_dma_stop(struct imx7_csi *csi)
 504 {
 505         unsigned long timeout_jiffies;
 506         unsigned long flags;
 507         int ret;
 508
 509         /* mark next EOF interrupt as the last before stream off */
 510         spin_lock_irqsave(&csi->irqlock, flags);
 511         csi->last_eof = true;
 512         spin_unlock_irqrestore(&csi->irqlock, flags);
 513
 514         /*
 515          * and then wait for interrupt handler to mark completion.
 516          */
 517         timeout_jiffies = msecs_to_jiffies(IMX7_CSI_VIDEO_EOF_TIMEOUT);
 518         ret = wait_for_completion_timeout(&csi->last_eof_completion,
 519                                           timeout_jiffies);
 520         if (ret == 0)
 521                 v4l2_warn(&csi->sd, "wait last EOF timeout\n");
 522
 523         imx7_csi_hw_disable_irq(csi);
 524 }
 525
 526 static void imx7_csi_configure(struct imx7_csi *csi,
 527                                struct v4l2_subdev_state *sd_state)
 528 {
 529         struct v4l2_pix_format *out_pix = &csi->vdev_fmt;
 530         int width = out_pix->width;
 531         u32 stride = 0;
 532         u32 cr3 = BIT_FRMCNT_RST;
 533         u32 cr1, cr18;
 534
 535         cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
 536
 537         cr18 &= ~(BIT_CSI_HW_ENABLE | BIT_MIPI_DATA_FORMAT_MASK |
 538                   BIT_DATA_FROM_MIPI | BIT_MIPI_DOUBLE_CMPNT |
 539                   BIT_BASEADDR_CHG_ERR_EN | BIT_BASEADDR_SWITCH_SEL |
 540                   BIT_BASEADDR_SWITCH_EN | BIT_DEINTERLACE_EN);
 541
 542         if (out_pix->field == V4L2_FIELD_INTERLACED) {
 543                 cr18 |= BIT_DEINTERLACE_EN;
 544                 stride = out_pix->width;
 545         }
 546
 547         if (!csi->is_csi2) {
 548                 cr1 = BIT_SOF_POL | BIT_REDGE | BIT_GCLK_MODE | BIT_HSYNC_POL
 549                     | BIT_FCC | BIT_MCLKDIV(1) | BIT_MCLKEN;
 550
 551                 cr18 |= BIT_BASEADDR_SWITCH_EN | BIT_BASEADDR_SWITCH_SEL |
 552                         BIT_BASEADDR_CHG_ERR_EN;
 553
 554                 if (out_pix->pixelformat == V4L2_PIX_FMT_UYVY ||
 555                     out_pix->pixelformat == V4L2_PIX_FMT_YUYV)
 556                         width *= 2;
 557         } else {
 558                 const struct v4l2_mbus_framefmt *sink_fmt;
 559
 560                 sink_fmt = v4l2_subdev_state_get_format(sd_state,
 561                                                         IMX7_CSI_PAD_SINK);
 562
 563                 cr1 = BIT_SOF_POL | BIT_REDGE | BIT_HSYNC_POL | BIT_FCC
 564                     | BIT_MCLKDIV(1) | BIT_MCLKEN;
 565
 566                 cr18 |= BIT_DATA_FROM_MIPI;
 567
 568                 switch (sink_fmt->code) {
 569                 case MEDIA_BUS_FMT_Y8_1X8:
 570                 case MEDIA_BUS_FMT_SBGGR8_1X8:
 571                 case MEDIA_BUS_FMT_SGBRG8_1X8:
 572                 case MEDIA_BUS_FMT_SGRBG8_1X8:
 573                 case MEDIA_BUS_FMT_SRGGB8_1X8:
 574                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW8;
 575                         break;
 576                 case MEDIA_BUS_FMT_Y10_1X10:
 577                 case MEDIA_BUS_FMT_SBGGR10_1X10:
 578                 case MEDIA_BUS_FMT_SGBRG10_1X10:
 579                 case MEDIA_BUS_FMT_SGRBG10_1X10:
 580                 case MEDIA_BUS_FMT_SRGGB10_1X10:
 581                         cr3 |= BIT_TWO_8BIT_SENSOR;
 582                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW10;
 583                         break;
 584                 case MEDIA_BUS_FMT_Y12_1X12:
 585                 case MEDIA_BUS_FMT_SBGGR12_1X12:
 586                 case MEDIA_BUS_FMT_SGBRG12_1X12:
 587                 case MEDIA_BUS_FMT_SGRBG12_1X12:
 588                 case MEDIA_BUS_FMT_SRGGB12_1X12:
 589                         cr3 |= BIT_TWO_8BIT_SENSOR;
 590                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW12;
 591                         break;
 592                 case MEDIA_BUS_FMT_Y14_1X14:
 593                 case MEDIA_BUS_FMT_SBGGR14_1X14:
 594                 case MEDIA_BUS_FMT_SGBRG14_1X14:
 595                 case MEDIA_BUS_FMT_SGRBG14_1X14:
 596                 case MEDIA_BUS_FMT_SRGGB14_1X14:
 597                         cr3 |= BIT_TWO_8BIT_SENSOR;
 598                         cr18 |= BIT_MIPI_DATA_FORMAT_RAW14;
 599                         break;
 600
 601                 /*
 602                  * The CSI bridge has a 16-bit input bus. Depending on the
 603                  * connected source, data may be transmitted with 8 or 10 bits
 604                  * per clock sample (in bits [9:2] or [9:0] respectively) or
 605                  * with 16 bits per clock sample (in bits [15:0]). The data is
 606                  * then packed into a 32-bit FIFO (as shown in figure 13-11 of
 607                  * the i.MX8MM reference manual rev. 3).
 608                  *
 609                  * The data packing in a 32-bit FIFO input word is controlled by
 610                  * the CR3 TWO_8BIT_SENSOR field (also known as SENSOR_16BITS in
 611                  * the i.MX8MM reference manual). When set to 0, data packing
 612                  * groups four 8-bit input samples (bits [9:2]). When set to 1,
 613                  * data packing groups two 16-bit input samples (bits [15:0]).
 614                  *
 615                  * The register field CR18 MIPI_DOUBLE_CMPNT also needs to be
 616                  * configured according to the input format for YUV 4:2:2 data.
 617                  * The field controls the gasket between the CSI-2 receiver and
 618                  * the CSI bridge. On i.MX7 and i.MX8MM, the field must be set
 619                  * to 1 when the CSIS outputs 16-bit samples. On i.MX8MQ, the
 620                  * gasket ignores the MIPI_DOUBLE_CMPNT bit and YUV 4:2:2 always
 621                  * uses 16-bit samples. Setting MIPI_DOUBLE_CMPNT in that case
 622                  * has no effect, but doesn't cause any issue.
 623                  */
 624                 case MEDIA_BUS_FMT_UYVY8_2X8:
 625                 case MEDIA_BUS_FMT_YUYV8_2X8:
 626                         cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B;
 627                         break;
 628                 case MEDIA_BUS_FMT_UYVY8_1X16:
 629                 case MEDIA_BUS_FMT_YUYV8_1X16:
 630                         cr3 |= BIT_TWO_8BIT_SENSOR;
 631                         cr18 |= BIT_MIPI_DATA_FORMAT_YUV422_8B |
 632                                 BIT_MIPI_DOUBLE_CMPNT;
 633                         break;
 634                 }
 635         }
 636
 637         imx7_csi_reg_write(csi, cr1, CSI_CSICR1);
 638         imx7_csi_reg_write(csi, BIT_DMA_BURST_TYPE_RFF_INCR16, CSI_CSICR2);
 639         imx7_csi_reg_write(csi, cr3, CSI_CSICR3);
 640         imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
 641
 642         imx7_csi_reg_write(csi, (width * out_pix->height) >> 2, CSI_CSIRXCNT);
 643         imx7_csi_reg_write(csi, BIT_IMAGE_WIDTH(width) |
 644                            BIT_IMAGE_HEIGHT(out_pix->height),
 645                            CSI_CSIIMAG_PARA);
 646         imx7_csi_reg_write(csi, stride, CSI_CSIFBUF_PARA);
 647 }
 648
 649 static int imx7_csi_init(struct imx7_csi *csi,
 650                          struct v4l2_subdev_state *sd_state)
 651 {
 652         int ret;
 653
 654         ret = clk_prepare_enable(csi->mclk);
 655         if (ret < 0)
 656                 return ret;
 657
 658         imx7_csi_configure(csi, sd_state);
 659
 660         ret = imx7_csi_dma_setup(csi);
 661         if (ret < 0) {
 662                 clk_disable_unprepare(csi->mclk);
 663                 return ret;
 664         }
 665
 666         return 0;
 667 }
 668
 669 static void imx7_csi_deinit(struct imx7_csi *csi,
 670                             enum vb2_buffer_state return_status)
 671 {
 672         imx7_csi_dma_cleanup(csi, return_status);
 673         imx7_csi_init_default(csi);
 674         imx7_csi_dmareq_rff_disable(csi);
 675         clk_disable_unprepare(csi->mclk);
 676 }
 677
 678 static void imx7_csi_baseaddr_switch_on_second_frame(struct imx7_csi *csi)
 679 {
 680         u32 cr18 = imx7_csi_reg_read(csi, CSI_CSICR18);
 681
 682         cr18 |= BIT_BASEADDR_SWITCH_EN | BIT_BASEADDR_SWITCH_SEL |
 683                 BIT_BASEADDR_CHG_ERR_EN;
 684         cr18 |= BIT_MASK_OPTION_SECOND_FRAME;
 685         imx7_csi_reg_write(csi, cr18, CSI_CSICR18);
 686 }
 687
 688 static void imx7_csi_enable(struct imx7_csi *csi)
 689 {
 690         /* Clear the Rx FIFO and reflash the DMA controller. */
 691         imx7_csi_rx_fifo_clear(csi);
 692         imx7_csi_dma_reflash(csi);
 693
 694         usleep_range(2000, 3000);
 695
 696         /* Clear and enable the interrupts. */
 697         imx7_csi_irq_clear(csi);
 698         imx7_csi_hw_enable_irq(csi);
 699
 700         /* Enable the RxFIFO DMA and the CSI. */
 701         imx7_csi_dmareq_rff_enable(csi);
 702         imx7_csi_hw_enable(csi);
 703
 704         if (csi->model == IMX7_CSI_IMX8MQ)
 705                 imx7_csi_baseaddr_switch_on_second_frame(csi);
 706 }
 707
 708 static void imx7_csi_disable(struct imx7_csi *csi)
 709 {
 710         imx7_csi_dma_stop(csi);
 711
 712         imx7_csi_dmareq_rff_disable(csi);
 713
 714         imx7_csi_hw_disable_irq(csi);
 715
 716         imx7_csi_hw_disable(csi);
 717 }
 718
 719 /* -----------------------------------------------------------------------------
 720  * Interrupt Handling
 721  */
 722
 723 static void imx7_csi_error_recovery(struct imx7_csi *csi)
 724 {
 725         imx7_csi_hw_disable(csi);
 726
 727         imx7_csi_rx_fifo_clear(csi);
 728
 729         imx7_csi_dma_reflash(csi);
 730
 731         imx7_csi_hw_enable(csi);
 732 }
 733
 734 static void imx7_csi_vb2_buf_done(struct imx7_csi *csi)
 735 {
 736         struct imx7_csi_vb2_buffer *done, *next;
 737         struct vb2_buffer *vb;
 738         dma_addr_t dma_addr;
 739
 740         done = csi->active_vb2_buf[csi->buf_num];
 741         if (done) {
 742                 done->vbuf.field = csi->vdev_fmt.field;
 743                 done->vbuf.sequence = csi->frame_sequence;
 744                 vb = &done->vbuf.vb2_buf;
 745                 vb->timestamp = ktime_get_ns();
 746                 vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
 747         }
 748         csi->frame_sequence++;
 749
 750         /* get next queued buffer */
 751         next = imx7_csi_video_next_buf(csi);
 752         if (next) {
 753                 dma_addr = vb2_dma_contig_plane_dma_addr(&next->vbuf.vb2_buf, 0);
 754                 csi->active_vb2_buf[csi->buf_num] = next;
 755         } else {
 756                 dma_addr = csi->underrun_buf.dma_addr;
 757                 csi->active_vb2_buf[csi->buf_num] = NULL;
 758         }
 759
 760         imx7_csi_update_buf(csi, dma_addr, csi->buf_num);
 761 }
 762
 763 static irqreturn_t imx7_csi_irq_handler(int irq, void *data)
 764 {
 765         struct imx7_csi *csi =  data;
 766         u32 status;
 767
 768         spin_lock(&csi->irqlock);
 769
 770         status = imx7_csi_irq_clear(csi);
 771
 772         if (status & BIT_RFF_OR_INT) {
 773                 dev_warn(csi->dev, "Rx fifo overflow\n");
 774                 imx7_csi_error_recovery(csi);
 775         }
 776
 777         if (status & BIT_HRESP_ERR_INT) {
 778                 dev_warn(csi->dev, "Hresponse error detected\n");
 779                 imx7_csi_error_recovery(csi);
 780         }
 781
 782         if (status & BIT_ADDR_CH_ERR_INT) {
 783                 imx7_csi_hw_disable(csi);
 784
 785                 imx7_csi_dma_reflash(csi);
 786
 787                 imx7_csi_hw_enable(csi);
 788         }
 789
 790         if ((status & BIT_DMA_TSF_DONE_FB1) &&
 791             (status & BIT_DMA_TSF_DONE_FB2)) {
 792                 /*
 793                  * For both FB1 and FB2 interrupter bits set case,
 794                  * CSI DMA is work in one of FB1 and FB2 buffer,
 795                  * but software can not know the state.
 796                  * Skip it to avoid base address updated
 797                  * when csi work in field0 and field1 will write to
 798                  * new base address.
 799                  */
 800         } else if (status & BIT_DMA_TSF_DONE_FB1) {
 801                 csi->buf_num = 0;
 802         } else if (status & BIT_DMA_TSF_DONE_FB2) {
 803                 csi->buf_num = 1;
 804         }
 805
 806         if ((status & BIT_DMA_TSF_DONE_FB1) ||
 807             (status & BIT_DMA_TSF_DONE_FB2)) {
 808                 imx7_csi_vb2_buf_done(csi);
 809
 810                 if (csi->last_eof) {
 811                         complete(&csi->last_eof_completion);
 812                         csi->last_eof = false;
 813                 }
 814         }
 815
 816         spin_unlock(&csi->irqlock);
 817
 818         return IRQ_HANDLED;
 819 }
 820
 821 /* -----------------------------------------------------------------------------
 822  * Format Helpers
 823  */
 824
 825 #define IMX_BUS_FMTS(fmt...) (const u32[]) {fmt, 0}
 826
 827 /*
 828  * List of supported pixel formats for the subdevs. Keep V4L2_PIX_FMT_UYVY and
 829  * MEDIA_BUS_FMT_UYVY8_2X8 first to match IMX7_CSI_DEF_PIX_FORMAT and
 830  * IMX7_CSI_DEF_MBUS_CODE.
 831  *
 832  * TODO: Restrict the supported formats list based on the SoC integration.
 833  *
 834  * The CSI bridge can be configured to sample pixel components from the Rx queue
 835  * in single (8bpp) or double (16bpp) component modes. Image format variants
 836  * with different sample sizes (ie YUYV_2X8 vs YUYV_1X16) determine the pixel
 837  * components sampling size per each clock cycle and their packing mode (see
 838  * imx7_csi_configure() for details).
 839  *
 840  * As the CSI bridge can be interfaced with different IP blocks depending on the
 841  * SoC model it is integrated on, the Rx queue sampling size should match the
 842  * size of the samples transferred by the transmitting IP block. To avoid
 843  * misconfigurations of the capture pipeline, the enumeration of the supported
 844  * formats should be restricted to match the pixel source transmitting mode.
 845  *
 846  * Example: i.MX8MM SoC integrates the CSI bridge with the Samsung CSIS CSI-2
 847  * receiver which operates in dual pixel sampling mode. The CSI bridge should
 848  * only expose the 1X16 formats variant which instructs it to operate in dual
 849  * pixel sampling mode. When the CSI bridge is instead integrated on an i.MX7,
 850  * which supports both serial and parallel input, it should expose both
 851  * variants.
 852  *
 853  * This currently only applies to YUYV formats, but other formats might need to
 854  * be handled in the same way.
 855  */
 856 static const struct imx7_csi_pixfmt pixel_formats[] = {
 857         /*** YUV formats start here ***/
 858         {
 859                 .fourcc = V4L2_PIX_FMT_UYVY,
 860                 .codes  = IMX_BUS_FMTS(
 861                         MEDIA_BUS_FMT_UYVY8_2X8,
 862                         MEDIA_BUS_FMT_UYVY8_1X16
 863                 ),
 864                 .yuv    = true,
 865                 .bpp    = 16,
 866         }, {
 867                 .fourcc = V4L2_PIX_FMT_YUYV,
 868                 .codes  = IMX_BUS_FMTS(
 869                         MEDIA_BUS_FMT_YUYV8_2X8,
 870                         MEDIA_BUS_FMT_YUYV8_1X16
 871                 ),
 872                 .yuv    = true,
 873                 .bpp    = 16,
 874         },
 875         /*** raw bayer and grayscale formats start here ***/
 876         {
 877                 .fourcc = V4L2_PIX_FMT_SBGGR8,
 878                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR8_1X8),
 879                 .bpp    = 8,
 880         }, {
 881                 .fourcc = V4L2_PIX_FMT_SGBRG8,
 882                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG8_1X8),
 883                 .bpp    = 8,
 884         }, {
 885                 .fourcc = V4L2_PIX_FMT_SGRBG8,
 886                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG8_1X8),
 887                 .bpp    = 8,
 888         }, {
 889                 .fourcc = V4L2_PIX_FMT_SRGGB8,
 890                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB8_1X8),
 891                 .bpp    = 8,
 892         }, {
 893                 .fourcc = V4L2_PIX_FMT_SBGGR10,
 894                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR10_1X10),
 895                 .bpp    = 16,
 896         }, {
 897                 .fourcc = V4L2_PIX_FMT_SGBRG10,
 898                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG10_1X10),
 899                 .bpp    = 16,
 900         }, {
 901                 .fourcc = V4L2_PIX_FMT_SGRBG10,
 902                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG10_1X10),
 903                 .bpp    = 16,
 904         }, {
 905                 .fourcc = V4L2_PIX_FMT_SRGGB10,
 906                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB10_1X10),
 907                 .bpp    = 16,
 908         }, {
 909                 .fourcc = V4L2_PIX_FMT_SBGGR12,
 910                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR12_1X12),
 911                 .bpp    = 16,
 912         }, {
 913                 .fourcc = V4L2_PIX_FMT_SGBRG12,
 914                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG12_1X12),
 915                 .bpp    = 16,
 916         }, {
 917                 .fourcc = V4L2_PIX_FMT_SGRBG12,
 918                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG12_1X12),
 919                 .bpp    = 16,
 920         }, {
 921                 .fourcc = V4L2_PIX_FMT_SRGGB12,
 922                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB12_1X12),
 923                 .bpp    = 16,
 924         }, {
 925                 .fourcc = V4L2_PIX_FMT_SBGGR14,
 926                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SBGGR14_1X14),
 927                 .bpp    = 16,
 928         }, {
 929                 .fourcc = V4L2_PIX_FMT_SGBRG14,
 930                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGBRG14_1X14),
 931                 .bpp    = 16,
 932         }, {
 933                 .fourcc = V4L2_PIX_FMT_SGRBG14,
 934                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SGRBG14_1X14),
 935                 .bpp    = 16,
 936         }, {
 937                 .fourcc = V4L2_PIX_FMT_SRGGB14,
 938                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_SRGGB14_1X14),
 939                 .bpp    = 16,
 940         }, {
 941                 .fourcc = V4L2_PIX_FMT_GREY,
 942                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y8_1X8),
 943                 .bpp    = 8,
 944         }, {
 945                 .fourcc = V4L2_PIX_FMT_Y10,
 946                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y10_1X10),
 947                 .bpp    = 16,
 948         }, {
 949                 .fourcc = V4L2_PIX_FMT_Y12,
 950                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y12_1X12),
 951                 .bpp    = 16,
 952         }, {
 953                 .fourcc = V4L2_PIX_FMT_Y14,
 954                 .codes  = IMX_BUS_FMTS(MEDIA_BUS_FMT_Y14_1X14),
 955                 .bpp    = 16,
 956         },
 957 };
 958
 959 /*
 960  * Search in the pixel_formats[] array for an entry with the given fourcc
 961  * return it.
 962  */
 963 static const struct imx7_csi_pixfmt *imx7_csi_find_pixel_format(u32 fourcc)
 964 {
 965         unsigned int i;
 966
 967         for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
 968                 const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
 969
 970                 if (fmt->fourcc == fourcc)
 971                         return fmt;
 972         }
 973
 974         return NULL;
 975 }
 976
 977 /*
 978  * Search in the pixel_formats[] array for an entry with the given media
 979  * bus code and return it.
 980  */
 981 static const struct imx7_csi_pixfmt *imx7_csi_find_mbus_format(u32 code)
 982 {
 983         unsigned int i;
 984
 985         for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
 986                 const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
 987                 unsigned int j;
 988
 989                 if (!fmt->codes)
 990                         continue;
 991
 992                 for (j = 0; fmt->codes[j]; j++) {
 993                         if (code == fmt->codes[j])
 994                                 return fmt;
 995                 }
 996         }
 997
 998         return NULL;
 999 }
1000
1001 /*
1002  * Enumerate entries in the pixel_formats[] array that match the
1003  * requested search criteria. Return the media-bus code that matches
1004  * the search criteria at the requested match index.
1005  *
1006  * @code: The returned media-bus code that matches the search criteria at
1007  *        the requested match index.
1008  * @index: The requested match index.
1009  */
1010 static int imx7_csi_enum_mbus_formats(u32 *code, u32 index)
1011 {
1012         unsigned int i;
1013
1014         for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
1015                 const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
1016                 unsigned int j;
1017
1018                 if (!fmt->codes)
1019                         continue;
1020
1021                 for (j = 0; fmt->codes[j]; j++) {
1022                         if (index == 0) {
1023                                 *code = fmt->codes[j];
1024                                 return 0;
1025                         }
1026
1027                         index--;
1028                 }
1029         }
1030
1031         return -EINVAL;
1032 }
1033
1034 /* -----------------------------------------------------------------------------
1035  * Video Capture Device - IOCTLs
1036  */
1037
1038 static int imx7_csi_video_querycap(struct file *file, void *fh,
1039                                    struct v4l2_capability *cap)
1040 {
1041         struct imx7_csi *csi = video_drvdata(file);
1042
1043         strscpy(cap->driver, IMX7_CSI_VIDEO_NAME, sizeof(cap->driver));
1044         strscpy(cap->card, IMX7_CSI_VIDEO_NAME, sizeof(cap->card));
1045         snprintf(cap->bus_info, sizeof(cap->bus_info),
1046                  "platform:%s", dev_name(csi->dev));
1047
1048         return 0;
1049 }
1050
1051 static int imx7_csi_video_enum_fmt_vid_cap(struct file *file, void *fh,
1052                                            struct v4l2_fmtdesc *f)
1053 {
1054         unsigned int index = f->index;
1055         unsigned int i;
1056
1057         for (i = 0; i < ARRAY_SIZE(pixel_formats); i++) {
1058                 const struct imx7_csi_pixfmt *fmt = &pixel_formats[i];
1059
1060                 /*
1061                  * If a media bus code is specified, only consider formats that
1062                  * match it.
1063                  */
1064                 if (f->mbus_code) {
1065                         unsigned int j;
1066
1067                         if (!fmt->codes)
1068                                 continue;
1069
1070                         for (j = 0; fmt->codes[j]; j++) {
1071                                 if (f->mbus_code == fmt->codes[j])
1072                                         break;
1073                         }
1074
1075                         if (!fmt->codes[j])
1076                                 continue;
1077                 }
1078
1079                 if (index == 0) {
1080                         f->pixelformat = fmt->fourcc;
1081                         return 0;
1082                 }
1083
1084                 index--;
1085         }
1086
1087         return -EINVAL;
1088 }
1089
1090 static int imx7_csi_video_enum_framesizes(struct file *file, void *fh,
1091                                           struct v4l2_frmsizeenum *fsize)
1092 {
1093         const struct imx7_csi_pixfmt *cc;
1094         u32 walign;
1095
1096         if (fsize->index > 0)
1097                 return -EINVAL;
1098
1099         cc = imx7_csi_find_pixel_format(fsize->pixel_format);
1100         if (!cc)
1101                 return -EINVAL;
1102
1103         /*
1104          * The width alignment is 8 bytes as indicated by the
1105          * CSI_IMAG_PARA.IMAGE_WIDTH documentation. Convert it to pixels.
1106          */
1107         walign = 8 * 8 / cc->bpp;
1108
1109         fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
1110         fsize->stepwise.min_width = walign;
1111         fsize->stepwise.max_width = round_down(65535U, walign);
1112         fsize->stepwise.min_height = 1;
1113         fsize->stepwise.max_height = 65535;
1114         fsize->stepwise.step_width = walign;
1115         fsize->stepwise.step_height = 1;
1116
1117         return 0;
1118 }
1119
1120 static int imx7_csi_video_g_fmt_vid_cap(struct file *file, void *fh,
1121                                         struct v4l2_format *f)
1122 {
1123         struct imx7_csi *csi = video_drvdata(file);
1124
1125         f->fmt.pix = csi->vdev_fmt;
1126
1127         return 0;
1128 }
1129
1130 static const struct imx7_csi_pixfmt *
1131 __imx7_csi_video_try_fmt(struct v4l2_pix_format *pixfmt,
1132                          struct v4l2_rect *compose)
1133 {
1134         const struct imx7_csi_pixfmt *cc;
1135         u32 walign;
1136
1137         if (compose) {
1138                 compose->width = pixfmt->width;
1139                 compose->height = pixfmt->height;
1140         }
1141
1142         /*
1143          * Find the pixel format, default to the first supported format if not
1144          * found.
1145          */
1146         cc = imx7_csi_find_pixel_format(pixfmt->pixelformat);
1147         if (!cc) {
1148                 pixfmt->pixelformat = IMX7_CSI_DEF_PIX_FORMAT;
1149                 cc = imx7_csi_find_pixel_format(pixfmt->pixelformat);
1150         }
1151
1152         /*
1153          * The width alignment is 8 bytes as indicated by the
1154          * CSI_IMAG_PARA.IMAGE_WIDTH documentation. Convert it to pixels.
1155          *
1156          * TODO: Implement configurable stride support.
1157          */
1158         walign = 8 * 8 / cc->bpp;
1159         pixfmt->width = clamp(round_up(pixfmt->width, walign), walign,
1160                               round_down(65535U, walign));
1161         pixfmt->height = clamp(pixfmt->height, 1U, 65535U);
1162
1163         pixfmt->bytesperline = pixfmt->width * cc->bpp / 8;
1164         pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
1165         pixfmt->field = V4L2_FIELD_NONE;
1166
1167         return cc;
1168 }
1169
1170 static int imx7_csi_video_try_fmt_vid_cap(struct file *file, void *fh,
1171                                           struct v4l2_format *f)
1172 {
1173         __imx7_csi_video_try_fmt(&f->fmt.pix, NULL);
1174         return 0;
1175 }
1176
1177 static int imx7_csi_video_s_fmt_vid_cap(struct file *file, void *fh,
1178                                         struct v4l2_format *f)
1179 {
1180         struct imx7_csi *csi = video_drvdata(file);
1181         const struct imx7_csi_pixfmt *cc;
1182
1183         if (vb2_is_busy(&csi->q)) {
1184                 dev_err(csi->dev, "%s queue busy\n", __func__);
1185                 return -EBUSY;
1186         }
1187
1188         cc = __imx7_csi_video_try_fmt(&f->fmt.pix, &csi->vdev_compose);
1189
1190         csi->vdev_cc = cc;
1191         csi->vdev_fmt = f->fmt.pix;
1192
1193         return 0;
1194 }
1195
1196 static int imx7_csi_video_g_selection(struct file *file, void *fh,
1197                                       struct v4l2_selection *s)
1198 {
1199         struct imx7_csi *csi = video_drvdata(file);
1200
1201         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1202                 return -EINVAL;
1203
1204         switch (s->target) {
1205         case V4L2_SEL_TGT_COMPOSE:
1206         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1207         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1208                 /* The compose rectangle is fixed to the source format. */
1209                 s->r = csi->vdev_compose;
1210                 break;
1211         case V4L2_SEL_TGT_COMPOSE_PADDED:
1212                 /*
1213                  * The hardware writes with a configurable but fixed DMA burst
1214                  * size. If the source format width is not burst size aligned,
1215                  * the written frame contains padding to the right.
1216                  */
1217                 s->r.left = 0;
1218                 s->r.top = 0;
1219                 s->r.width = csi->vdev_fmt.width;
1220                 s->r.height = csi->vdev_fmt.height;
1221                 break;
1222         default:
1223                 return -EINVAL;
1224         }
1225
1226         return 0;
1227 }
1228
1229 static const struct v4l2_ioctl_ops imx7_csi_video_ioctl_ops = {
1230         .vidioc_querycap                = imx7_csi_video_querycap,
1231
1232         .vidioc_enum_fmt_vid_cap        = imx7_csi_video_enum_fmt_vid_cap,
1233         .vidioc_enum_framesizes         = imx7_csi_video_enum_framesizes,
1234
1235         .vidioc_g_fmt_vid_cap           = imx7_csi_video_g_fmt_vid_cap,
1236         .vidioc_try_fmt_vid_cap         = imx7_csi_video_try_fmt_vid_cap,
1237         .vidioc_s_fmt_vid_cap           = imx7_csi_video_s_fmt_vid_cap,
1238
1239         .vidioc_g_selection             = imx7_csi_video_g_selection,
1240
1241         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1242         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1243         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1244         .vidioc_querybuf                = vb2_ioctl_querybuf,
1245         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1246         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1247         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1248         .vidioc_streamon                = vb2_ioctl_streamon,
1249         .vidioc_streamoff               = vb2_ioctl_streamoff,
1250 };
1251
1252 /* -----------------------------------------------------------------------------
1253  * Video Capture Device - Queue Operations
1254  */
1255
1256 static int imx7_csi_video_queue_setup(struct vb2_queue *vq,
1257                                       unsigned int *nbuffers,
1258                                       unsigned int *nplanes,
1259                                       unsigned int sizes[],
1260                                       struct device *alloc_devs[])
1261 {
1262         struct imx7_csi *csi = vb2_get_drv_priv(vq);
1263         unsigned int q_num_bufs = vb2_get_num_buffers(vq);
1264         struct v4l2_pix_format *pix = &csi->vdev_fmt;
1265         unsigned int count = *nbuffers;
1266
1267         if (vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1268                 return -EINVAL;
1269
1270         if (*nplanes) {
1271                 if (*nplanes != 1 || sizes[0] < pix->sizeimage)
1272                         return -EINVAL;
1273                 count += q_num_bufs;
1274         }
1275
1276         count = min_t(__u32, IMX7_CSI_VIDEO_MEM_LIMIT / pix->sizeimage, count);
1277
1278         if (*nplanes)
1279                 *nbuffers = (count < q_num_bufs) ? 0 :
1280                         count - q_num_bufs;
1281         else
1282                 *nbuffers = count;
1283
1284         *nplanes = 1;
1285         sizes[0] = pix->sizeimage;
1286
1287         return 0;
1288 }
1289
1290 static int imx7_csi_video_buf_init(struct vb2_buffer *vb)
1291 {
1292         struct imx7_csi_vb2_buffer *buf = to_imx7_csi_vb2_buffer(vb);
1293
1294         INIT_LIST_HEAD(&buf->list);
1295
1296         return 0;
1297 }
1298
1299 static int imx7_csi_video_buf_prepare(struct vb2_buffer *vb)
1300 {
1301         struct imx7_csi *csi = vb2_get_drv_priv(vb->vb2_queue);
1302         struct v4l2_pix_format *pix = &csi->vdev_fmt;
1303
1304         if (vb2_plane_size(vb, 0) < pix->sizeimage) {
1305                 dev_err(csi->dev,
1306                         "data will not fit into plane (%lu < %lu)\n",
1307                         vb2_plane_size(vb, 0), (long)pix->sizeimage);
1308                 return -EINVAL;
1309         }
1310
1311         vb2_set_plane_payload(vb, 0, pix->sizeimage);
1312
1313         return 0;
1314 }
1315
1316 static bool imx7_csi_fast_track_buffer(struct imx7_csi *csi,
1317                                        struct imx7_csi_vb2_buffer *buf)
1318 {
1319         unsigned long flags;
1320         dma_addr_t dma_addr;
1321         int buf_num;
1322         u32 isr;
1323
1324         if (!csi->is_streaming)
1325                 return false;
1326
1327         dma_addr = vb2_dma_contig_plane_dma_addr(&buf->vbuf.vb2_buf, 0);
1328
1329         /*
1330          * buf_num holds the framebuffer ID of the most recently (*not* the
1331          * next anticipated) triggered interrupt. Without loss of generality,
1332          * if buf_num is 0, the hardware is capturing to FB2. If FB1 has been
1333          * programmed with a dummy buffer (as indicated by active_vb2_buf[0]
1334          * being NULL), then we can fast-track the new buffer by programming
1335          * its address in FB1 before the hardware completes FB2, instead of
1336          * adding it to the buffer queue and incurring a delay of one
1337          * additional frame.
1338          *
1339          * The irqlock prevents races with the interrupt handler that updates
1340          * buf_num when it programs the next buffer, but we can still race with
1341          * the hardware if we program the buffer in FB1 just after the hardware
1342          * completes FB2 and switches to FB1 and before buf_num can be updated
1343          * by the interrupt handler for FB2.  The fast-tracked buffer would
1344          * then be ignored by the hardware while the driver would think it has
1345          * successfully been processed.
1346          *
1347          * To avoid this problem, if we can't avoid the race, we can detect
1348          * that we have lost it by checking, after programming the buffer in
1349          * FB1, if the interrupt flag indicating completion of FB2 has been
1350          * raised. If that is not the case, fast-tracking succeeded, and we can
1351          * update active_vb2_buf[0]. Otherwise, we may or may not have lost the
1352          * race (as the interrupt flag may have been raised just after
1353          * programming FB1 and before we read the interrupt status register),
1354          * and we need to assume the worst case of a race loss and queue the
1355          * buffer through the slow path.
1356          */
1357
1358         spin_lock_irqsave(&csi->irqlock, flags);
1359
1360         buf_num = csi->buf_num;
1361         if (csi->active_vb2_buf[buf_num]) {
1362                 spin_unlock_irqrestore(&csi->irqlock, flags);
1363                 return false;
1364         }
1365
1366         imx7_csi_update_buf(csi, dma_addr, buf_num);
1367
1368         isr = imx7_csi_reg_read(csi, CSI_CSISR);
1369         if (isr & (buf_num ? BIT_DMA_TSF_DONE_FB1 : BIT_DMA_TSF_DONE_FB2)) {
1370                 /*
1371                  * The interrupt for the /other/ FB just came (the isr hasn't
1372                  * run yet though, because we have the lock here); we can't be
1373                  * sure we've programmed buf_num FB in time, so queue the buffer
1374                  * to the buffer queue normally. No need to undo writing the FB
1375                  * register, since we won't return it as active_vb2_buf is NULL,
1376                  * so it's okay to potentially write it to both FB1 and FB2;
1377                  * only the one where it was queued normally will be returned.
1378                  */
1379                 spin_unlock_irqrestore(&csi->irqlock, flags);
1380                 return false;
1381         }
1382
1383         csi->active_vb2_buf[buf_num] = buf;
1384
1385         spin_unlock_irqrestore(&csi->irqlock, flags);
1386         return true;
1387 }
1388
1389 static void imx7_csi_video_buf_queue(struct vb2_buffer *vb)
1390 {
1391         struct imx7_csi *csi = vb2_get_drv_priv(vb->vb2_queue);
1392         struct imx7_csi_vb2_buffer *buf = to_imx7_csi_vb2_buffer(vb);
1393         unsigned long flags;
1394
1395         if (imx7_csi_fast_track_buffer(csi, buf))
1396                 return;
1397
1398         spin_lock_irqsave(&csi->q_lock, flags);
1399
1400         list_add_tail(&buf->list, &csi->ready_q);
1401
1402         spin_unlock_irqrestore(&csi->q_lock, flags);
1403 }
1404
1405 static int imx7_csi_video_validate_fmt(struct imx7_csi *csi)
1406 {
1407         struct v4l2_subdev_format fmt_src = {
1408                 .pad = IMX7_CSI_PAD_SRC,
1409                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1410         };
1411         const struct imx7_csi_pixfmt *cc;
1412         int ret;
1413
1414         /* Retrieve the media bus format on the source subdev. */
1415         ret = v4l2_subdev_call_state_active(&csi->sd, pad, get_fmt, &fmt_src);
1416         if (ret)
1417                 return ret;
1418
1419         /*
1420          * Verify that the media bus size matches the size set on the video
1421          * node. It is sufficient to check the compose rectangle size without
1422          * checking the rounded size from pix_fmt, as the rounded size is
1423          * derived directly from the compose rectangle size, and will thus
1424          * always match if the compose rectangle matches.
1425          */
1426         if (csi->vdev_compose.width != fmt_src.format.width ||
1427             csi->vdev_compose.height != fmt_src.format.height)
1428                 return -EPIPE;
1429
1430         /*
1431          * Verify that the media bus code is compatible with the pixel format
1432          * set on the video node.
1433          */
1434         cc = imx7_csi_find_mbus_format(fmt_src.format.code);
1435         if (!cc || csi->vdev_cc->yuv != cc->yuv)
1436                 return -EPIPE;
1437
1438         return 0;
1439 }
1440
1441 static int imx7_csi_video_start_streaming(struct vb2_queue *vq,
1442                                           unsigned int count)
1443 {
1444         struct imx7_csi *csi = vb2_get_drv_priv(vq);
1445         struct imx7_csi_vb2_buffer *buf, *tmp;
1446         unsigned long flags;
1447         int ret;
1448
1449         ret = imx7_csi_video_validate_fmt(csi);
1450         if (ret) {
1451                 dev_err(csi->dev, "capture format not valid\n");
1452                 goto err_buffers;
1453         }
1454
1455         mutex_lock(&csi->mdev.graph_mutex);
1456
1457         ret = __video_device_pipeline_start(csi->vdev, &csi->pipe);
1458         if (ret)
1459                 goto err_unlock;
1460
1461         ret = v4l2_subdev_call(&csi->sd, video, s_stream, 1);
1462         if (ret)
1463                 goto err_stop;
1464
1465         mutex_unlock(&csi->mdev.graph_mutex);
1466
1467         return 0;
1468
1469 err_stop:
1470         __video_device_pipeline_stop(csi->vdev);
1471 err_unlock:
1472         mutex_unlock(&csi->mdev.graph_mutex);
1473         dev_err(csi->dev, "pipeline start failed with %d\n", ret);
1474 err_buffers:
1475         spin_lock_irqsave(&csi->q_lock, flags);
1476         list_for_each_entry_safe(buf, tmp, &csi->ready_q, list) {
1477                 list_del(&buf->list);
1478                 vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED);
1479         }
1480         spin_unlock_irqrestore(&csi->q_lock, flags);
1481         return ret;
1482 }
1483
1484 static void imx7_csi_video_stop_streaming(struct vb2_queue *vq)
1485 {
1486         struct imx7_csi *csi = vb2_get_drv_priv(vq);
1487         struct imx7_csi_vb2_buffer *frame;
1488         struct imx7_csi_vb2_buffer *tmp;
1489         unsigned long flags;
1490
1491         mutex_lock(&csi->mdev.graph_mutex);
1492         v4l2_subdev_call(&csi->sd, video, s_stream, 0);
1493         __video_device_pipeline_stop(csi->vdev);
1494         mutex_unlock(&csi->mdev.graph_mutex);
1495
1496         /* release all active buffers */
1497         spin_lock_irqsave(&csi->q_lock, flags);
1498         list_for_each_entry_safe(frame, tmp, &csi->ready_q, list) {
1499                 list_del(&frame->list);
1500                 vb2_buffer_done(&frame->vbuf.vb2_buf, VB2_BUF_STATE_ERROR);
1501         }
1502         spin_unlock_irqrestore(&csi->q_lock, flags);
1503 }
1504
1505 static const struct vb2_ops imx7_csi_video_qops = {
1506         .queue_setup     = imx7_csi_video_queue_setup,
1507         .buf_init        = imx7_csi_video_buf_init,
1508         .buf_prepare     = imx7_csi_video_buf_prepare,
1509         .buf_queue       = imx7_csi_video_buf_queue,
1510         .wait_prepare    = vb2_ops_wait_prepare,
1511         .wait_finish     = vb2_ops_wait_finish,
1512         .start_streaming = imx7_csi_video_start_streaming,
1513         .stop_streaming  = imx7_csi_video_stop_streaming,
1514 };
1515
1516 /* -----------------------------------------------------------------------------
1517  * Video Capture Device - File Operations
1518  */
1519
1520 static int imx7_csi_video_open(struct file *file)
1521 {
1522         struct imx7_csi *csi = video_drvdata(file);
1523         int ret;
1524
1525         if (mutex_lock_interruptible(&csi->vdev_mutex))
1526                 return -ERESTARTSYS;
1527
1528         ret = v4l2_fh_open(file);
1529         if (ret) {
1530                 dev_err(csi->dev, "v4l2_fh_open failed\n");
1531                 goto out;
1532         }
1533
1534         ret = v4l2_pipeline_pm_get(&csi->vdev->entity);
1535         if (ret)
1536                 v4l2_fh_release(file);
1537
1538 out:
1539         mutex_unlock(&csi->vdev_mutex);
1540         return ret;
1541 }
1542
1543 static int imx7_csi_video_release(struct file *file)
1544 {
1545         struct imx7_csi *csi = video_drvdata(file);
1546         struct vb2_queue *vq = &csi->q;
1547
1548         mutex_lock(&csi->vdev_mutex);
1549
1550         if (file->private_data == vq->owner) {
1551                 vb2_queue_release(vq);
1552                 vq->owner = NULL;
1553         }
1554
1555         v4l2_pipeline_pm_put(&csi->vdev->entity);
1556
1557         v4l2_fh_release(file);
1558         mutex_unlock(&csi->vdev_mutex);
1559         return 0;
1560 }
1561
1562 static const struct v4l2_file_operations imx7_csi_video_fops = {
1563         .owner          = THIS_MODULE,
1564         .open           = imx7_csi_video_open,
1565         .release        = imx7_csi_video_release,
1566         .poll           = vb2_fop_poll,
1567         .unlocked_ioctl = video_ioctl2,
1568         .mmap           = vb2_fop_mmap,
1569 };
1570
1571 /* -----------------------------------------------------------------------------
1572  * Video Capture Device - Init & Cleanup
1573  */
1574
1575 static struct imx7_csi_vb2_buffer *imx7_csi_video_next_buf(struct imx7_csi *csi)
1576 {
1577         struct imx7_csi_vb2_buffer *buf = NULL;
1578         unsigned long flags;
1579
1580         spin_lock_irqsave(&csi->q_lock, flags);
1581
1582         /* get next queued buffer */
1583         if (!list_empty(&csi->ready_q)) {
1584                 buf = list_entry(csi->ready_q.next, struct imx7_csi_vb2_buffer,
1585                                  list);
1586                 list_del(&buf->list);
1587         }
1588
1589         spin_unlock_irqrestore(&csi->q_lock, flags);
1590
1591         return buf;
1592 }
1593
1594 static void imx7_csi_video_init_format(struct imx7_csi *csi)
1595 {
1596         struct v4l2_pix_format *pixfmt = &csi->vdev_fmt;
1597
1598         pixfmt->width = IMX7_CSI_DEF_PIX_WIDTH;
1599         pixfmt->height = IMX7_CSI_DEF_PIX_HEIGHT;
1600
1601         csi->vdev_cc = __imx7_csi_video_try_fmt(pixfmt, &csi->vdev_compose);
1602 }
1603
1604 static int imx7_csi_video_register(struct imx7_csi *csi)
1605 {
1606         struct v4l2_subdev *sd = &csi->sd;
1607         struct v4l2_device *v4l2_dev = sd->v4l2_dev;
1608         struct video_device *vdev = csi->vdev;
1609         int ret;
1610
1611         vdev->v4l2_dev = v4l2_dev;
1612
1613         /* Initialize the default format and compose rectangle. */
1614         imx7_csi_video_init_format(csi);
1615
1616         /* Register the video device. */
1617         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1618         if (ret) {
1619                 dev_err(csi->dev, "Failed to register video device\n");
1620                 return ret;
1621         }
1622
1623         dev_info(csi->dev, "Registered %s as /dev/%s\n", vdev->name,
1624                  video_device_node_name(vdev));
1625
1626         /* Create the link from the CSI subdev to the video device. */
1627         ret = media_create_pad_link(&sd->entity, IMX7_CSI_PAD_SRC,
1628                                     &vdev->entity, 0, MEDIA_LNK_FL_IMMUTABLE |
1629                                     MEDIA_LNK_FL_ENABLED);
1630         if (ret) {
1631                 dev_err(csi->dev, "failed to create link to device node\n");
1632                 video_unregister_device(vdev);
1633                 return ret;
1634         }
1635
1636         return 0;
1637 }
1638
1639 static void imx7_csi_video_unregister(struct imx7_csi *csi)
1640 {
1641         media_entity_cleanup(&csi->vdev->entity);
1642         video_unregister_device(csi->vdev);
1643 }
1644
1645 static int imx7_csi_video_init(struct imx7_csi *csi)
1646 {
1647         struct video_device *vdev;
1648         struct vb2_queue *vq;
1649         int ret;
1650
1651         mutex_init(&csi->vdev_mutex);
1652         INIT_LIST_HEAD(&csi->ready_q);
1653         spin_lock_init(&csi->q_lock);
1654
1655         /* Allocate and initialize the video device. */
1656         vdev = video_device_alloc();
1657         if (!vdev)
1658                 return -ENOMEM;
1659
1660         vdev->fops = &imx7_csi_video_fops;
1661         vdev->ioctl_ops = &imx7_csi_video_ioctl_ops;
1662         vdev->minor = -1;
1663         vdev->release = video_device_release;
1664         vdev->vfl_dir = VFL_DIR_RX;
1665         vdev->tvnorms = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM;
1666         vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
1667                          | V4L2_CAP_IO_MC;
1668         vdev->lock = &csi->vdev_mutex;
1669         vdev->queue = &csi->q;
1670
1671         snprintf(vdev->name, sizeof(vdev->name), "%s capture", csi->sd.name);
1672
1673         video_set_drvdata(vdev, csi);
1674         csi->vdev = vdev;
1675
1676         /* Initialize the video device pad. */
1677         csi->vdev_pad.flags = MEDIA_PAD_FL_SINK;
1678         ret = media_entity_pads_init(&vdev->entity, 1, &csi->vdev_pad);
1679         if (ret) {
1680                 video_device_release(vdev);
1681                 return ret;
1682         }
1683
1684         /* Initialize the vb2 queue. */
1685         vq = &csi->q;
1686         vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1687         vq->io_modes = VB2_MMAP | VB2_DMABUF;
1688         vq->drv_priv = csi;
1689         vq->buf_struct_size = sizeof(struct imx7_csi_vb2_buffer);
1690         vq->ops = &imx7_csi_video_qops;
1691         vq->mem_ops = &vb2_dma_contig_memops;
1692         vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1693         vq->lock = &csi->vdev_mutex;
1694         vq->min_queued_buffers = 2;
1695         vq->dev = csi->dev;
1696
1697         ret = vb2_queue_init(vq);
1698         if (ret) {
1699                 dev_err(csi->dev, "vb2_queue_init failed\n");
1700                 video_device_release(vdev);
1701                 return ret;
1702         }
1703
1704         return 0;
1705 }
1706
1707 /* -----------------------------------------------------------------------------
1708  * V4L2 Subdev Operations
1709  */
1710
1711 static int imx7_csi_s_stream(struct v4l2_subdev *sd, int enable)
1712 {
1713         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1714         struct v4l2_subdev_state *sd_state;
1715         int ret = 0;
1716
1717         sd_state = v4l2_subdev_lock_and_get_active_state(sd);
1718
1719         if (enable) {
1720                 ret = imx7_csi_init(csi, sd_state);
1721                 if (ret < 0)
1722                         goto out_unlock;
1723
1724                 ret = v4l2_subdev_call(csi->src_sd, video, s_stream, 1);
1725                 if (ret < 0) {
1726                         imx7_csi_deinit(csi, VB2_BUF_STATE_QUEUED);
1727                         goto out_unlock;
1728                 }
1729
1730                 imx7_csi_enable(csi);
1731         } else {
1732                 imx7_csi_disable(csi);
1733
1734                 v4l2_subdev_call(csi->src_sd, video, s_stream, 0);
1735
1736                 imx7_csi_deinit(csi, VB2_BUF_STATE_ERROR);
1737         }
1738
1739         csi->is_streaming = !!enable;
1740
1741 out_unlock:
1742         v4l2_subdev_unlock_state(sd_state);
1743
1744         return ret;
1745 }
1746
1747 static int imx7_csi_init_state(struct v4l2_subdev *sd,
1748                                struct v4l2_subdev_state *sd_state)
1749 {
1750         const struct imx7_csi_pixfmt *cc;
1751         int i;
1752
1753         cc = imx7_csi_find_mbus_format(IMX7_CSI_DEF_MBUS_CODE);
1754
1755         for (i = 0; i < IMX7_CSI_PADS_NUM; i++) {
1756                 struct v4l2_mbus_framefmt *mf =
1757                         v4l2_subdev_state_get_format(sd_state, i);
1758
1759                 mf->code = IMX7_CSI_DEF_MBUS_CODE;
1760                 mf->width = IMX7_CSI_DEF_PIX_WIDTH;
1761                 mf->height = IMX7_CSI_DEF_PIX_HEIGHT;
1762                 mf->field = V4L2_FIELD_NONE;
1763
1764                 mf->colorspace = V4L2_COLORSPACE_SRGB;
1765                 mf->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(mf->colorspace);
1766                 mf->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(mf->colorspace);
1767                 mf->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(!cc->yuv,
1768                                         mf->colorspace, mf->ycbcr_enc);
1769         }
1770
1771         return 0;
1772 }
1773
1774 static int imx7_csi_enum_mbus_code(struct v4l2_subdev *sd,
1775                                    struct v4l2_subdev_state *sd_state,
1776                                    struct v4l2_subdev_mbus_code_enum *code)
1777 {
1778         struct v4l2_mbus_framefmt *in_fmt;
1779         int ret = 0;
1780
1781         in_fmt = v4l2_subdev_state_get_format(sd_state, IMX7_CSI_PAD_SINK);
1782
1783         switch (code->pad) {
1784         case IMX7_CSI_PAD_SINK:
1785                 ret = imx7_csi_enum_mbus_formats(&code->code, code->index);
1786                 break;
1787
1788         case IMX7_CSI_PAD_SRC:
1789                 if (code->index != 0) {
1790                         ret = -EINVAL;
1791                         break;
1792                 }
1793
1794                 code->code = in_fmt->code;
1795                 break;
1796
1797         default:
1798                 ret = -EINVAL;
1799                 break;
1800         }
1801
1802         return ret;
1803 }
1804
1805 /*
1806  * Default the colorspace in tryfmt to SRGB if set to an unsupported
1807  * colorspace or not initialized. Then set the remaining colorimetry
1808  * parameters based on the colorspace if they are uninitialized.
1809  *
1810  * tryfmt->code must be set on entry.
1811  */
1812 static void imx7_csi_try_colorimetry(struct v4l2_mbus_framefmt *tryfmt)
1813 {
1814         const struct imx7_csi_pixfmt *cc;
1815         bool is_rgb = false;
1816
1817         cc = imx7_csi_find_mbus_format(tryfmt->code);
1818         if (cc && !cc->yuv)
1819                 is_rgb = true;
1820
1821         switch (tryfmt->colorspace) {
1822         case V4L2_COLORSPACE_SMPTE170M:
1823         case V4L2_COLORSPACE_REC709:
1824         case V4L2_COLORSPACE_JPEG:
1825         case V4L2_COLORSPACE_SRGB:
1826         case V4L2_COLORSPACE_BT2020:
1827         case V4L2_COLORSPACE_OPRGB:
1828         case V4L2_COLORSPACE_DCI_P3:
1829         case V4L2_COLORSPACE_RAW:
1830                 break;
1831         default:
1832                 tryfmt->colorspace = V4L2_COLORSPACE_SRGB;
1833                 break;
1834         }
1835
1836         if (tryfmt->xfer_func == V4L2_XFER_FUNC_DEFAULT)
1837                 tryfmt->xfer_func =
1838                         V4L2_MAP_XFER_FUNC_DEFAULT(tryfmt->colorspace);
1839
1840         if (tryfmt->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT)
1841                 tryfmt->ycbcr_enc =
1842                         V4L2_MAP_YCBCR_ENC_DEFAULT(tryfmt->colorspace);
1843
1844         if (tryfmt->quantization == V4L2_QUANTIZATION_DEFAULT)
1845                 tryfmt->quantization =
1846                         V4L2_MAP_QUANTIZATION_DEFAULT(is_rgb,
1847                                                       tryfmt->colorspace,
1848                                                       tryfmt->ycbcr_enc);
1849 }
1850
1851 static void imx7_csi_try_fmt(struct v4l2_subdev *sd,
1852                              struct v4l2_subdev_state *sd_state,
1853                              struct v4l2_subdev_format *sdformat,
1854                              const struct imx7_csi_pixfmt **cc)
1855 {
1856         const struct imx7_csi_pixfmt *in_cc;
1857         struct v4l2_mbus_framefmt *in_fmt;
1858         u32 code;
1859
1860         in_fmt = v4l2_subdev_state_get_format(sd_state, IMX7_CSI_PAD_SINK);
1861
1862         switch (sdformat->pad) {
1863         case IMX7_CSI_PAD_SRC:
1864                 in_cc = imx7_csi_find_mbus_format(in_fmt->code);
1865
1866                 sdformat->format.width = in_fmt->width;
1867                 sdformat->format.height = in_fmt->height;
1868                 sdformat->format.code = in_fmt->code;
1869                 sdformat->format.field = in_fmt->field;
1870                 *cc = in_cc;
1871
1872                 sdformat->format.colorspace = in_fmt->colorspace;
1873                 sdformat->format.xfer_func = in_fmt->xfer_func;
1874                 sdformat->format.quantization = in_fmt->quantization;
1875                 sdformat->format.ycbcr_enc = in_fmt->ycbcr_enc;
1876                 break;
1877
1878         case IMX7_CSI_PAD_SINK:
1879                 *cc = imx7_csi_find_mbus_format(sdformat->format.code);
1880                 if (!*cc) {
1881                         code = IMX7_CSI_DEF_MBUS_CODE;
1882                         *cc = imx7_csi_find_mbus_format(code);
1883                         sdformat->format.code = code;
1884                 }
1885
1886                 if (sdformat->format.field != V4L2_FIELD_INTERLACED)
1887                         sdformat->format.field = V4L2_FIELD_NONE;
1888                 break;
1889         }
1890
1891         imx7_csi_try_colorimetry(&sdformat->format);
1892 }
1893
1894 static int imx7_csi_set_fmt(struct v4l2_subdev *sd,
1895                             struct v4l2_subdev_state *sd_state,
1896                             struct v4l2_subdev_format *sdformat)
1897 {
1898         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1899         const struct imx7_csi_pixfmt *outcc;
1900         struct v4l2_mbus_framefmt *outfmt;
1901         const struct imx7_csi_pixfmt *cc;
1902         struct v4l2_mbus_framefmt *fmt;
1903         struct v4l2_subdev_format format;
1904
1905         if (csi->is_streaming)
1906                 return -EBUSY;
1907
1908         imx7_csi_try_fmt(sd, sd_state, sdformat, &cc);
1909
1910         fmt = v4l2_subdev_state_get_format(sd_state, sdformat->pad);
1911
1912         *fmt = sdformat->format;
1913
1914         if (sdformat->pad == IMX7_CSI_PAD_SINK) {
1915                 /* propagate format to source pads */
1916                 format.pad = IMX7_CSI_PAD_SRC;
1917                 format.which = sdformat->which;
1918                 format.format = sdformat->format;
1919                 imx7_csi_try_fmt(sd, sd_state, &format, &outcc);
1920
1921                 outfmt = v4l2_subdev_state_get_format(sd_state,
1922                                                       IMX7_CSI_PAD_SRC);
1923                 *outfmt = format.format;
1924         }
1925
1926         return 0;
1927 }
1928
1929 static int imx7_csi_pad_link_validate(struct v4l2_subdev *sd,
1930                                       struct media_link *link,
1931                                       struct v4l2_subdev_format *source_fmt,
1932                                       struct v4l2_subdev_format *sink_fmt)
1933 {
1934         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1935         struct media_pad *pad = NULL;
1936         unsigned int i;
1937         int ret;
1938
1939         /*
1940          * Validate the source link, and record whether the source uses the
1941          * parallel input or the CSI-2 receiver.
1942          */
1943         ret = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
1944         if (ret)
1945                 return ret;
1946
1947         switch (csi->src_sd->entity.function) {
1948         case MEDIA_ENT_F_VID_IF_BRIDGE:
1949                 /* The input is the CSI-2 receiver. */
1950                 csi->is_csi2 = true;
1951                 break;
1952
1953         case MEDIA_ENT_F_VID_MUX:
1954                 /* The input is the mux, check its input. */
1955                 for (i = 0; i < csi->src_sd->entity.num_pads; i++) {
1956                         struct media_pad *spad = &csi->src_sd->entity.pads[i];
1957
1958                         if (!(spad->flags & MEDIA_PAD_FL_SINK))
1959                                 continue;
1960
1961                         pad = media_pad_remote_pad_first(spad);
1962                         if (pad)
1963                                 break;
1964                 }
1965
1966                 if (!pad)
1967                         return -ENODEV;
1968
1969                 csi->is_csi2 = pad->entity->function == MEDIA_ENT_F_VID_IF_BRIDGE;
1970                 break;
1971
1972         default:
1973                 /*
1974                  * The input is an external entity, it must use the parallel
1975                  * bus.
1976                  */
1977                 csi->is_csi2 = false;
1978                 break;
1979         }
1980
1981         return 0;
1982 }
1983
1984 static int imx7_csi_registered(struct v4l2_subdev *sd)
1985 {
1986         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
1987         int ret;
1988
1989         ret = imx7_csi_video_init(csi);
1990         if (ret)
1991                 return ret;
1992
1993         ret = imx7_csi_video_register(csi);
1994         if (ret)
1995                 return ret;
1996
1997         ret = v4l2_device_register_subdev_nodes(&csi->v4l2_dev);
1998         if (ret)
1999                 goto err_unreg;
2000
2001         ret = media_device_register(&csi->mdev);
2002         if (ret)
2003                 goto err_unreg;
2004
2005         return 0;
2006
2007 err_unreg:
2008         imx7_csi_video_unregister(csi);
2009         return ret;
2010 }
2011
2012 static void imx7_csi_unregistered(struct v4l2_subdev *sd)
2013 {
2014         struct imx7_csi *csi = v4l2_get_subdevdata(sd);
2015
2016         imx7_csi_video_unregister(csi);
2017 }
2018
2019 static const struct v4l2_subdev_video_ops imx7_csi_video_ops = {
2020         .s_stream       = imx7_csi_s_stream,
2021 };
2022
2023 static const struct v4l2_subdev_pad_ops imx7_csi_pad_ops = {
2024         .enum_mbus_code = imx7_csi_enum_mbus_code,
2025         .get_fmt        = v4l2_subdev_get_fmt,
2026         .set_fmt        = imx7_csi_set_fmt,
2027         .link_validate  = imx7_csi_pad_link_validate,
2028 };
2029
2030 static const struct v4l2_subdev_ops imx7_csi_subdev_ops = {
2031         .video          = &imx7_csi_video_ops,
2032         .pad            = &imx7_csi_pad_ops,
2033 };
2034
2035 static const struct v4l2_subdev_internal_ops imx7_csi_internal_ops = {
2036         .init_state     = imx7_csi_init_state,
2037         .registered     = imx7_csi_registered,
2038         .unregistered   = imx7_csi_unregistered,
2039 };
2040
2041 /* -----------------------------------------------------------------------------
2042  * Media Entity Operations
2043  */
2044
2045 static const struct media_entity_operations imx7_csi_entity_ops = {
2046         .link_validate  = v4l2_subdev_link_validate,
2047         .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
2048 };
2049
2050 /* -----------------------------------------------------------------------------
2051  * Probe & Remove
2052  */
2053
2054 static int imx7_csi_notify_bound(struct v4l2_async_notifier *notifier,
2055                                  struct v4l2_subdev *sd,
2056                                  struct v4l2_async_connection *asd)
2057 {
2058         struct imx7_csi *csi = imx7_csi_notifier_to_dev(notifier);
2059         struct media_pad *sink = &csi->sd.entity.pads[IMX7_CSI_PAD_SINK];
2060
2061         csi->src_sd = sd;
2062
2063         return v4l2_create_fwnode_links_to_pad(sd, sink, MEDIA_LNK_FL_ENABLED |
2064                                                MEDIA_LNK_FL_IMMUTABLE);
2065 }
2066
2067 static int imx7_csi_notify_complete(struct v4l2_async_notifier *notifier)
2068 {
2069         struct imx7_csi *csi = imx7_csi_notifier_to_dev(notifier);
2070
2071         return v4l2_device_register_subdev_nodes(&csi->v4l2_dev);
2072 }
2073
2074 static const struct v4l2_async_notifier_operations imx7_csi_notify_ops = {
2075         .bound = imx7_csi_notify_bound,
2076         .complete = imx7_csi_notify_complete,
2077 };
2078
2079 static int imx7_csi_async_register(struct imx7_csi *csi)
2080 {
2081         struct v4l2_async_connection *asd;
2082         struct fwnode_handle *ep;
2083         int ret;
2084
2085         v4l2_async_nf_init(&csi->notifier, &csi->v4l2_dev);
2086
2087         ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi->dev), 0, 0,
2088                                              FWNODE_GRAPH_ENDPOINT_NEXT);
2089         if (!ep) {
2090                 ret = dev_err_probe(csi->dev, -ENOTCONN,
2091                                     "Failed to get remote endpoint\n");
2092                 goto error;
2093         }
2094
2095         asd = v4l2_async_nf_add_fwnode_remote(&csi->notifier, ep,
2096                                               struct v4l2_async_connection);
2097
2098         fwnode_handle_put(ep);
2099
2100         if (IS_ERR(asd)) {
2101                 ret = dev_err_probe(csi->dev, PTR_ERR(asd),
2102                                     "Failed to add remote subdev to notifier\n");
2103                 goto error;
2104         }
2105
2106         csi->notifier.ops = &imx7_csi_notify_ops;
2107
2108         ret = v4l2_async_nf_register(&csi->notifier);
2109         if (ret)
2110                 goto error;
2111
2112         return 0;
2113
2114 error:
2115         v4l2_async_nf_cleanup(&csi->notifier);
2116         return ret;
2117 }
2118
2119 static void imx7_csi_media_cleanup(struct imx7_csi *csi)
2120 {
2121         v4l2_device_unregister(&csi->v4l2_dev);
2122         media_device_unregister(&csi->mdev);
2123         v4l2_subdev_cleanup(&csi->sd);
2124         media_device_cleanup(&csi->mdev);
2125 }
2126
2127 static const struct media_device_ops imx7_csi_media_ops = {
2128         .link_notify = v4l2_pipeline_link_notify,
2129 };
2130
2131 static int imx7_csi_media_dev_init(struct imx7_csi *csi)
2132 {
2133         int ret;
2134
2135         strscpy(csi->mdev.model, "imx-media", sizeof(csi->mdev.model));
2136         csi->mdev.ops = &imx7_csi_media_ops;
2137         csi->mdev.dev = csi->dev;
2138
2139         csi->v4l2_dev.mdev = &csi->mdev;
2140         strscpy(csi->v4l2_dev.name, "imx-media",
2141                 sizeof(csi->v4l2_dev.name));
2142         snprintf(csi->mdev.bus_info, sizeof(csi->mdev.bus_info),
2143                  "platform:%s", dev_name(csi->mdev.dev));
2144
2145         media_device_init(&csi->mdev);
2146
2147         ret = v4l2_device_register(csi->dev, &csi->v4l2_dev);
2148         if (ret < 0) {
2149                 v4l2_err(&csi->v4l2_dev,
2150                          "Failed to register v4l2_device: %d\n", ret);
2151                 goto cleanup;
2152         }
2153
2154         return 0;
2155
2156 cleanup:
2157         media_device_cleanup(&csi->mdev);
2158
2159         return ret;
2160 }
2161
2162 static int imx7_csi_media_init(struct imx7_csi *csi)
2163 {
2164         unsigned int i;
2165         int ret;
2166
2167         /* add media device */
2168         ret = imx7_csi_media_dev_init(csi);
2169         if (ret)
2170                 return ret;
2171
2172         v4l2_subdev_init(&csi->sd, &imx7_csi_subdev_ops);
2173         v4l2_set_subdevdata(&csi->sd, csi);
2174         csi->sd.internal_ops = &imx7_csi_internal_ops;
2175         csi->sd.entity.ops = &imx7_csi_entity_ops;
2176         csi->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
2177         csi->sd.dev = csi->dev;
2178         csi->sd.owner = THIS_MODULE;
2179         csi->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
2180         snprintf(csi->sd.name, sizeof(csi->sd.name), "csi");
2181
2182         for (i = 0; i < IMX7_CSI_PADS_NUM; i++)
2183                 csi->pad[i].flags = (i == IMX7_CSI_PAD_SINK) ?
2184                         MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
2185
2186         ret = media_entity_pads_init(&csi->sd.entity, IMX7_CSI_PADS_NUM,
2187                                      csi->pad);
2188         if (ret)
2189                 goto error;
2190
2191         ret = v4l2_subdev_init_finalize(&csi->sd);
2192         if (ret)
2193                 goto error;
2194
2195         ret = v4l2_device_register_subdev(&csi->v4l2_dev, &csi->sd);
2196         if (ret)
2197                 goto error;
2198
2199         return 0;
2200
2201 error:
2202         imx7_csi_media_cleanup(csi);
2203         return ret;
2204 }
2205
2206 static int imx7_csi_probe(struct platform_device *pdev)
2207 {
2208         struct device *dev = &pdev->dev;
2209         struct imx7_csi *csi;
2210         int ret;
2211
2212         csi = devm_kzalloc(&pdev->dev, sizeof(*csi), GFP_KERNEL);
2213         if (!csi)
2214                 return -ENOMEM;
2215
2216         csi->dev = dev;
2217         platform_set_drvdata(pdev, csi);
2218
2219         spin_lock_init(&csi->irqlock);
2220
2221         /* Acquire resources and install interrupt handler. */
2222         csi->mclk = devm_clk_get(&pdev->dev, "mclk");
2223         if (IS_ERR(csi->mclk)) {
2224                 ret = PTR_ERR(csi->mclk);
2225                 dev_err(dev, "Failed to get mclk: %d", ret);
2226                 return ret;
2227         }
2228
2229         csi->irq = platform_get_irq(pdev, 0);
2230         if (csi->irq < 0)
2231                 return csi->irq;
2232
2233         csi->regbase = devm_platform_ioremap_resource(pdev, 0);
2234         if (IS_ERR(csi->regbase))
2235                 return PTR_ERR(csi->regbase);
2236
2237         csi->model = (enum imx_csi_model)(uintptr_t)of_device_get_match_data(&pdev->dev);
2238
2239         ret = devm_request_irq(dev, csi->irq, imx7_csi_irq_handler, 0, "csi",
2240                                (void *)csi);
2241         if (ret < 0) {
2242                 dev_err(dev, "Request CSI IRQ failed.\n");
2243                 return ret;
2244         }
2245
2246         /* Initialize all the media device infrastructure. */
2247         ret = imx7_csi_media_init(csi);
2248         if (ret)
2249                 return ret;
2250
2251         ret = imx7_csi_async_register(csi);
2252         if (ret)
2253                 goto err_media_cleanup;
2254
2255         return 0;
2256
2257 err_media_cleanup:
2258         imx7_csi_media_cleanup(csi);
2259
2260         return ret;
2261 }
2262
2263 static void imx7_csi_remove(struct platform_device *pdev)
2264 {
2265         struct imx7_csi *csi = platform_get_drvdata(pdev);
2266
2267         imx7_csi_media_cleanup(csi);
2268
2269         v4l2_async_nf_unregister(&csi->notifier);
2270         v4l2_async_nf_cleanup(&csi->notifier);
2271         v4l2_async_unregister_subdev(&csi->sd);
2272 }
2273
2274 static const struct of_device_id imx7_csi_of_match[] = {
2275         { .compatible = "fsl,imx8mq-csi", .data = (void *)IMX7_CSI_IMX8MQ },
2276         { .compatible = "fsl,imx7-csi", .data = (void *)IMX7_CSI_IMX7 },
2277         { .compatible = "fsl,imx6ul-csi", .data = (void *)IMX7_CSI_IMX7 },
2278         { },
2279 };
2280 MODULE_DEVICE_TABLE(of, imx7_csi_of_match);
2281
2282 static struct platform_driver imx7_csi_driver = {
2283         .probe = imx7_csi_probe,
2284         .remove_new = imx7_csi_remove,
2285         .driver = {
2286                 .of_match_table = imx7_csi_of_match,
2287                 .name = "imx7-csi",
2288         },
2289 };
2290 module_platform_driver(imx7_csi_driver);
2291
2292 MODULE_DESCRIPTION("i.MX7 CSI subdev driver");
2293 MODULE_AUTHOR("Rui Miguel Silva <rui.silva@linaro.org>");
2294 MODULE_LICENSE("GPL v2");
2295 MODULE_ALIAS("platform:imx7-csi");