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