Pileus Git - ~andy/linux/blob - drivers/media/video/mx2_camera.c

   1 /*
   2  * V4L2 Driver for i.MX27/i.MX25 camera host
   3  *
   4  * Copyright (C) 2008, Sascha Hauer, Pengutronix
   5  * Copyright (C) 2010, Baruch Siach, Orex Computed Radiography
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  */
  12
  13 #include <linux/init.h>
  14 #include <linux/module.h>
  15 #include <linux/io.h>
  16 #include <linux/delay.h>
  17 #include <linux/slab.h>
  18 #include <linux/dma-mapping.h>
  19 #include <linux/errno.h>
  20 #include <linux/fs.h>
  21 #include <linux/interrupt.h>
  22 #include <linux/kernel.h>
  23 #include <linux/mm.h>
  24 #include <linux/moduleparam.h>
  25 #include <linux/time.h>
  26 #include <linux/device.h>
  27 #include <linux/platform_device.h>
  28 #include <linux/mutex.h>
  29 #include <linux/clk.h>
  30
  31 #include <media/v4l2-common.h>
  32 #include <media/v4l2-dev.h>
  33 #include <media/videobuf-core.h>
  34 #include <media/videobuf-dma-contig.h>
  35 #include <media/soc_camera.h>
  36 #include <media/soc_mediabus.h>
  37
  38 #include <linux/videodev2.h>
  39
  40 #include <mach/mx2_cam.h>
  41 #ifdef CONFIG_MACH_MX27
  42 #include <mach/dma-mx1-mx2.h>
  43 #endif
  44 #include <mach/hardware.h>
  45
  46 #include <asm/dma.h>
  47
  48 #define MX2_CAM_DRV_NAME "mx2-camera"
  49 #define MX2_CAM_VERSION "0.0.6"
  50 #define MX2_CAM_DRIVER_DESCRIPTION "i.MX2x_Camera"
  51
  52 /* reset values */
  53 #define CSICR1_RESET_VAL        0x40000800
  54 #define CSICR2_RESET_VAL        0x0
  55 #define CSICR3_RESET_VAL        0x0
  56
  57 /* csi control reg 1 */
  58 #define CSICR1_SWAP16_EN        (1 << 31)
  59 #define CSICR1_EXT_VSYNC        (1 << 30)
  60 #define CSICR1_EOF_INTEN        (1 << 29)
  61 #define CSICR1_PRP_IF_EN        (1 << 28)
  62 #define CSICR1_CCIR_MODE        (1 << 27)
  63 #define CSICR1_COF_INTEN        (1 << 26)
  64 #define CSICR1_SF_OR_INTEN      (1 << 25)
  65 #define CSICR1_RF_OR_INTEN      (1 << 24)
  66 #define CSICR1_STATFF_LEVEL     (3 << 22)
  67 #define CSICR1_STATFF_INTEN     (1 << 21)
  68 #define CSICR1_RXFF_LEVEL(l)    (((l) & 3) << 19)       /* MX27 */
  69 #define CSICR1_FB2_DMA_INTEN    (1 << 20)               /* MX25 */
  70 #define CSICR1_FB1_DMA_INTEN    (1 << 19)               /* MX25 */
  71 #define CSICR1_RXFF_INTEN       (1 << 18)
  72 #define CSICR1_SOF_POL          (1 << 17)
  73 #define CSICR1_SOF_INTEN        (1 << 16)
  74 #define CSICR1_MCLKDIV(d)       (((d) & 0xF) << 12)
  75 #define CSICR1_HSYNC_POL        (1 << 11)
  76 #define CSICR1_CCIR_EN          (1 << 10)
  77 #define CSICR1_MCLKEN           (1 << 9)
  78 #define CSICR1_FCC              (1 << 8)
  79 #define CSICR1_PACK_DIR         (1 << 7)
  80 #define CSICR1_CLR_STATFIFO     (1 << 6)
  81 #define CSICR1_CLR_RXFIFO       (1 << 5)
  82 #define CSICR1_GCLK_MODE        (1 << 4)
  83 #define CSICR1_INV_DATA         (1 << 3)
  84 #define CSICR1_INV_PCLK         (1 << 2)
  85 #define CSICR1_REDGE            (1 << 1)
  86
  87 #define SHIFT_STATFF_LEVEL      22
  88 #define SHIFT_RXFF_LEVEL        19
  89 #define SHIFT_MCLKDIV           12
  90
  91 /* control reg 3 */
  92 #define CSICR3_FRMCNT           (0xFFFF << 16)
  93 #define CSICR3_FRMCNT_RST       (1 << 15)
  94 #define CSICR3_DMA_REFLASH_RFF  (1 << 14)
  95 #define CSICR3_DMA_REFLASH_SFF  (1 << 13)
  96 #define CSICR3_DMA_REQ_EN_RFF   (1 << 12)
  97 #define CSICR3_DMA_REQ_EN_SFF   (1 << 11)
  98 #define CSICR3_RXFF_LEVEL(l)    (((l) & 7) << 4)        /* MX25 */
  99 #define CSICR3_CSI_SUP          (1 << 3)
 100 #define CSICR3_ZERO_PACK_EN     (1 << 2)
 101 #define CSICR3_ECC_INT_EN       (1 << 1)
 102 #define CSICR3_ECC_AUTO_EN      (1 << 0)
 103
 104 #define SHIFT_FRMCNT            16
 105
 106 /* csi status reg */
 107 #define CSISR_SFF_OR_INT        (1 << 25)
 108 #define CSISR_RFF_OR_INT        (1 << 24)
 109 #define CSISR_STATFF_INT        (1 << 21)
 110 #define CSISR_DMA_TSF_FB2_INT   (1 << 20)       /* MX25 */
 111 #define CSISR_DMA_TSF_FB1_INT   (1 << 19)       /* MX25 */
 112 #define CSISR_RXFF_INT          (1 << 18)
 113 #define CSISR_EOF_INT           (1 << 17)
 114 #define CSISR_SOF_INT           (1 << 16)
 115 #define CSISR_F2_INT            (1 << 15)
 116 #define CSISR_F1_INT            (1 << 14)
 117 #define CSISR_COF_INT           (1 << 13)
 118 #define CSISR_ECC_INT           (1 << 1)
 119 #define CSISR_DRDY              (1 << 0)
 120
 121 #define CSICR1                  0x00
 122 #define CSICR2                  0x04
 123 #define CSISR                   (cpu_is_mx27() ? 0x08 : 0x18)
 124 #define CSISTATFIFO             0x0c
 125 #define CSIRFIFO                0x10
 126 #define CSIRXCNT                0x14
 127 #define CSICR3                  (cpu_is_mx27() ? 0x1C : 0x08)
 128 #define CSIDMASA_STATFIFO       0x20
 129 #define CSIDMATA_STATFIFO       0x24
 130 #define CSIDMASA_FB1            0x28
 131 #define CSIDMASA_FB2            0x2c
 132 #define CSIFBUF_PARA            0x30
 133 #define CSIIMAG_PARA            0x34
 134
 135 /* EMMA PrP */
 136 #define PRP_CNTL                        0x00
 137 #define PRP_INTR_CNTL                   0x04
 138 #define PRP_INTRSTATUS                  0x08
 139 #define PRP_SOURCE_Y_PTR                0x0c
 140 #define PRP_SOURCE_CB_PTR               0x10
 141 #define PRP_SOURCE_CR_PTR               0x14
 142 #define PRP_DEST_RGB1_PTR               0x18
 143 #define PRP_DEST_RGB2_PTR               0x1c
 144 #define PRP_DEST_Y_PTR                  0x20
 145 #define PRP_DEST_CB_PTR                 0x24
 146 #define PRP_DEST_CR_PTR                 0x28
 147 #define PRP_SRC_FRAME_SIZE              0x2c
 148 #define PRP_DEST_CH1_LINE_STRIDE        0x30
 149 #define PRP_SRC_PIXEL_FORMAT_CNTL       0x34
 150 #define PRP_CH1_PIXEL_FORMAT_CNTL       0x38
 151 #define PRP_CH1_OUT_IMAGE_SIZE          0x3c
 152 #define PRP_CH2_OUT_IMAGE_SIZE          0x40
 153 #define PRP_SRC_LINE_STRIDE             0x44
 154 #define PRP_CSC_COEF_012                0x48
 155 #define PRP_CSC_COEF_345                0x4c
 156 #define PRP_CSC_COEF_678                0x50
 157 #define PRP_CH1_RZ_HORI_COEF1           0x54
 158 #define PRP_CH1_RZ_HORI_COEF2           0x58
 159 #define PRP_CH1_RZ_HORI_VALID           0x5c
 160 #define PRP_CH1_RZ_VERT_COEF1           0x60
 161 #define PRP_CH1_RZ_VERT_COEF2           0x64
 162 #define PRP_CH1_RZ_VERT_VALID           0x68
 163 #define PRP_CH2_RZ_HORI_COEF1           0x6c
 164 #define PRP_CH2_RZ_HORI_COEF2           0x70
 165 #define PRP_CH2_RZ_HORI_VALID           0x74
 166 #define PRP_CH2_RZ_VERT_COEF1           0x78
 167 #define PRP_CH2_RZ_VERT_COEF2           0x7c
 168 #define PRP_CH2_RZ_VERT_VALID           0x80
 169
 170 #define PRP_CNTL_CH1EN          (1 << 0)
 171 #define PRP_CNTL_CH2EN          (1 << 1)
 172 #define PRP_CNTL_CSIEN          (1 << 2)
 173 #define PRP_CNTL_DATA_IN_YUV420 (0 << 3)
 174 #define PRP_CNTL_DATA_IN_YUV422 (1 << 3)
 175 #define PRP_CNTL_DATA_IN_RGB16  (2 << 3)
 176 #define PRP_CNTL_DATA_IN_RGB32  (3 << 3)
 177 #define PRP_CNTL_CH1_OUT_RGB8   (0 << 5)
 178 #define PRP_CNTL_CH1_OUT_RGB16  (1 << 5)
 179 #define PRP_CNTL_CH1_OUT_RGB32  (2 << 5)
 180 #define PRP_CNTL_CH1_OUT_YUV422 (3 << 5)
 181 #define PRP_CNTL_CH2_OUT_YUV420 (0 << 7)
 182 #define PRP_CNTL_CH2_OUT_YUV422 (1 << 7)
 183 #define PRP_CNTL_CH2_OUT_YUV444 (2 << 7)
 184 #define PRP_CNTL_CH1_LEN        (1 << 9)
 185 #define PRP_CNTL_CH2_LEN        (1 << 10)
 186 #define PRP_CNTL_SKIP_FRAME     (1 << 11)
 187 #define PRP_CNTL_SWRST          (1 << 12)
 188 #define PRP_CNTL_CLKEN          (1 << 13)
 189 #define PRP_CNTL_WEN            (1 << 14)
 190 #define PRP_CNTL_CH1BYP         (1 << 15)
 191 #define PRP_CNTL_IN_TSKIP(x)    ((x) << 16)
 192 #define PRP_CNTL_CH1_TSKIP(x)   ((x) << 19)
 193 #define PRP_CNTL_CH2_TSKIP(x)   ((x) << 22)
 194 #define PRP_CNTL_INPUT_FIFO_LEVEL(x)    ((x) << 25)
 195 #define PRP_CNTL_RZ_FIFO_LEVEL(x)       ((x) << 27)
 196 #define PRP_CNTL_CH2B1EN        (1 << 29)
 197 #define PRP_CNTL_CH2B2EN        (1 << 30)
 198 #define PRP_CNTL_CH2FEN         (1 << 31)
 199
 200 /* IRQ Enable and status register */
 201 #define PRP_INTR_RDERR          (1 << 0)
 202 #define PRP_INTR_CH1WERR        (1 << 1)
 203 #define PRP_INTR_CH2WERR        (1 << 2)
 204 #define PRP_INTR_CH1FC          (1 << 3)
 205 #define PRP_INTR_CH2FC          (1 << 5)
 206 #define PRP_INTR_LBOVF          (1 << 7)
 207 #define PRP_INTR_CH2OVF         (1 << 8)
 208
 209 #define mx27_camera_emma(pcdev) (cpu_is_mx27() && pcdev->use_emma)
 210
 211 #define MAX_VIDEO_MEM   16
 212
 213 struct mx2_prp_cfg {
 214         int channel;
 215         u32 in_fmt;
 216         u32 out_fmt;
 217         u32 src_pixel;
 218         u32 ch1_pixel;
 219         u32 irq_flags;
 220 };
 221
 222 /* prp configuration for a client-host fmt pair */
 223 struct mx2_fmt_cfg {
 224         enum v4l2_mbus_pixelcode        in_fmt;
 225         u32                             out_fmt;
 226         struct mx2_prp_cfg              cfg;
 227 };
 228
 229 struct mx2_camera_dev {
 230         struct device           *dev;
 231         struct soc_camera_host  soc_host;
 232         struct soc_camera_device *icd;
 233         struct clk              *clk_csi, *clk_emma;
 234
 235         unsigned int            irq_csi, irq_emma;
 236         void __iomem            *base_csi, *base_emma;
 237         unsigned long           base_dma;
 238
 239         struct mx2_camera_platform_data *pdata;
 240         struct resource         *res_csi, *res_emma;
 241         unsigned long           platform_flags;
 242
 243         struct list_head        capture;
 244         struct list_head        active_bufs;
 245
 246         spinlock_t              lock;
 247
 248         int                     dma;
 249         struct mx2_buffer       *active;
 250         struct mx2_buffer       *fb1_active;
 251         struct mx2_buffer       *fb2_active;
 252
 253         int                     use_emma;
 254
 255         u32                     csicr1;
 256
 257         void                    *discard_buffer;
 258         dma_addr_t              discard_buffer_dma;
 259         size_t                  discard_size;
 260         struct mx2_fmt_cfg      *emma_prp;
 261 };
 262
 263 /* buffer for one video frame */
 264 struct mx2_buffer {
 265         /* common v4l buffer stuff -- must be first */
 266         struct videobuf_buffer          vb;
 267
 268         enum v4l2_mbus_pixelcode        code;
 269
 270         int bufnum;
 271 };
 272
 273 static struct mx2_fmt_cfg mx27_emma_prp_table[] = {
 274         /*
 275          * This is a generic configuration which is valid for most
 276          * prp input-output format combinations.
 277          * We set the incomming and outgoing pixelformat to a
 278          * 16 Bit wide format and adjust the bytesperline
 279          * accordingly. With this configuration the inputdata
 280          * will not be changed by the emma and could be any type
 281          * of 16 Bit Pixelformat.
 282          */
 283         {
 284                 .in_fmt         = 0,
 285                 .out_fmt        = 0,
 286                 .cfg            = {
 287                         .channel        = 1,
 288                         .in_fmt         = PRP_CNTL_DATA_IN_RGB16,
 289                         .out_fmt        = PRP_CNTL_CH1_OUT_RGB16,
 290                         .src_pixel      = 0x2ca00565, /* RGB565 */
 291                         .ch1_pixel      = 0x2ca00565, /* RGB565 */
 292                         .irq_flags      = PRP_INTR_RDERR | PRP_INTR_CH1WERR |
 293                                                 PRP_INTR_CH1FC | PRP_INTR_LBOVF,
 294                 }
 295         },
 296         {
 297                 .in_fmt         = V4L2_MBUS_FMT_YUYV8_2X8,
 298                 .out_fmt        = V4L2_PIX_FMT_YUV420,
 299                 .cfg            = {
 300                         .channel        = 2,
 301                         .in_fmt         = PRP_CNTL_DATA_IN_YUV422,
 302                         .out_fmt        = PRP_CNTL_CH2_OUT_YUV420,
 303                         .src_pixel      = 0x22000888, /* YUV422 (YUYV) */
 304                         .irq_flags      = PRP_INTR_RDERR | PRP_INTR_CH2WERR |
 305                                         PRP_INTR_CH2FC | PRP_INTR_LBOVF |
 306                                         PRP_INTR_CH2OVF,
 307                 }
 308         },
 309 };
 310
 311 static struct mx2_fmt_cfg *mx27_emma_prp_get_format(
 312                                         enum v4l2_mbus_pixelcode in_fmt,
 313                                         u32 out_fmt)
 314 {
 315         int i;
 316
 317         for (i = 1; i < ARRAY_SIZE(mx27_emma_prp_table); i++)
 318                 if ((mx27_emma_prp_table[i].in_fmt == in_fmt) &&
 319                                 (mx27_emma_prp_table[i].out_fmt == out_fmt)) {
 320                         return &mx27_emma_prp_table[i];
 321                 }
 322         /* If no match return the most generic configuration */
 323         return &mx27_emma_prp_table[0];
 324 };
 325
 326 static void mx2_camera_deactivate(struct mx2_camera_dev *pcdev)
 327 {
 328         unsigned long flags;
 329
 330         clk_disable(pcdev->clk_csi);
 331         writel(0, pcdev->base_csi + CSICR1);
 332         if (mx27_camera_emma(pcdev)) {
 333                 writel(0, pcdev->base_emma + PRP_CNTL);
 334         } else if (cpu_is_mx25()) {
 335                 spin_lock_irqsave(&pcdev->lock, flags);
 336                 pcdev->fb1_active = NULL;
 337                 pcdev->fb2_active = NULL;
 338                 writel(0, pcdev->base_csi + CSIDMASA_FB1);
 339                 writel(0, pcdev->base_csi + CSIDMASA_FB2);
 340                 spin_unlock_irqrestore(&pcdev->lock, flags);
 341         }
 342 }
 343
 344 /*
 345  * The following two functions absolutely depend on the fact, that
 346  * there can be only one camera on mx2 camera sensor interface
 347  */
 348 static int mx2_camera_add_device(struct soc_camera_device *icd)
 349 {
 350         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 351         struct mx2_camera_dev *pcdev = ici->priv;
 352         int ret;
 353         u32 csicr1;
 354
 355         if (pcdev->icd)
 356                 return -EBUSY;
 357
 358         ret = clk_enable(pcdev->clk_csi);
 359         if (ret < 0)
 360                 return ret;
 361
 362         csicr1 = CSICR1_MCLKEN;
 363
 364         if (mx27_camera_emma(pcdev)) {
 365                 csicr1 |= CSICR1_PRP_IF_EN | CSICR1_FCC |
 366                         CSICR1_RXFF_LEVEL(0);
 367         } else if (cpu_is_mx27())
 368                 csicr1 |= CSICR1_SOF_INTEN | CSICR1_RXFF_LEVEL(2);
 369
 370         pcdev->csicr1 = csicr1;
 371         writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
 372
 373         pcdev->icd = icd;
 374
 375         dev_info(icd->parent, "Camera driver attached to camera %d\n",
 376                  icd->devnum);
 377
 378         return 0;
 379 }
 380
 381 static void mx2_camera_remove_device(struct soc_camera_device *icd)
 382 {
 383         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 384         struct mx2_camera_dev *pcdev = ici->priv;
 385
 386         BUG_ON(icd != pcdev->icd);
 387
 388         dev_info(icd->parent, "Camera driver detached from camera %d\n",
 389                  icd->devnum);
 390
 391         mx2_camera_deactivate(pcdev);
 392
 393         if (pcdev->discard_buffer) {
 394                 dma_free_coherent(ici->v4l2_dev.dev, pcdev->discard_size,
 395                                 pcdev->discard_buffer,
 396                                 pcdev->discard_buffer_dma);
 397                 pcdev->discard_buffer = NULL;
 398         }
 399
 400         pcdev->icd = NULL;
 401 }
 402
 403 #ifdef CONFIG_MACH_MX27
 404 static void mx27_camera_dma_enable(struct mx2_camera_dev *pcdev)
 405 {
 406         u32 tmp;
 407
 408         imx_dma_enable(pcdev->dma);
 409
 410         tmp = readl(pcdev->base_csi + CSICR1);
 411         tmp |= CSICR1_RF_OR_INTEN;
 412         writel(tmp, pcdev->base_csi + CSICR1);
 413 }
 414
 415 static irqreturn_t mx27_camera_irq(int irq_csi, void *data)
 416 {
 417         struct mx2_camera_dev *pcdev = data;
 418         u32 status = readl(pcdev->base_csi + CSISR);
 419
 420         if (status & CSISR_SOF_INT && pcdev->active) {
 421                 u32 tmp;
 422
 423                 tmp = readl(pcdev->base_csi + CSICR1);
 424                 writel(tmp | CSICR1_CLR_RXFIFO, pcdev->base_csi + CSICR1);
 425                 mx27_camera_dma_enable(pcdev);
 426         }
 427
 428         writel(CSISR_SOF_INT | CSISR_RFF_OR_INT, pcdev->base_csi + CSISR);
 429
 430         return IRQ_HANDLED;
 431 }
 432 #else
 433 static irqreturn_t mx27_camera_irq(int irq_csi, void *data)
 434 {
 435         return IRQ_NONE;
 436 }
 437 #endif /* CONFIG_MACH_MX27 */
 438
 439 static void mx25_camera_frame_done(struct mx2_camera_dev *pcdev, int fb,
 440                 int state)
 441 {
 442         struct videobuf_buffer *vb;
 443         struct mx2_buffer *buf;
 444         struct mx2_buffer **fb_active = fb == 1 ? &pcdev->fb1_active :
 445                 &pcdev->fb2_active;
 446         u32 fb_reg = fb == 1 ? CSIDMASA_FB1 : CSIDMASA_FB2;
 447         unsigned long flags;
 448
 449         spin_lock_irqsave(&pcdev->lock, flags);
 450
 451         if (*fb_active == NULL)
 452                 goto out;
 453
 454         vb = &(*fb_active)->vb;
 455         dev_dbg(pcdev->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 456                 vb, vb->baddr, vb->bsize);
 457
 458         vb->state = state;
 459         do_gettimeofday(&vb->ts);
 460         vb->field_count++;
 461
 462         wake_up(&vb->done);
 463
 464         if (list_empty(&pcdev->capture)) {
 465                 buf = NULL;
 466                 writel(0, pcdev->base_csi + fb_reg);
 467         } else {
 468                 buf = list_entry(pcdev->capture.next, struct mx2_buffer,
 469                                 vb.queue);
 470                 vb = &buf->vb;
 471                 list_del(&vb->queue);
 472                 vb->state = VIDEOBUF_ACTIVE;
 473                 writel(videobuf_to_dma_contig(vb), pcdev->base_csi + fb_reg);
 474         }
 475
 476         *fb_active = buf;
 477
 478 out:
 479         spin_unlock_irqrestore(&pcdev->lock, flags);
 480 }
 481
 482 static irqreturn_t mx25_camera_irq(int irq_csi, void *data)
 483 {
 484         struct mx2_camera_dev *pcdev = data;
 485         u32 status = readl(pcdev->base_csi + CSISR);
 486
 487         if (status & CSISR_DMA_TSF_FB1_INT)
 488                 mx25_camera_frame_done(pcdev, 1, VIDEOBUF_DONE);
 489         else if (status & CSISR_DMA_TSF_FB2_INT)
 490                 mx25_camera_frame_done(pcdev, 2, VIDEOBUF_DONE);
 491
 492         /* FIXME: handle CSISR_RFF_OR_INT */
 493
 494         writel(status, pcdev->base_csi + CSISR);
 495
 496         return IRQ_HANDLED;
 497 }
 498
 499 /*
 500  *  Videobuf operations
 501  */
 502 static int mx2_videobuf_setup(struct videobuf_queue *vq, unsigned int *count,
 503                               unsigned int *size)
 504 {
 505         struct soc_camera_device *icd = vq->priv_data;
 506         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 507                         icd->current_fmt->host_fmt);
 508
 509         dev_dbg(icd->parent, "count=%d, size=%d\n", *count, *size);
 510
 511         if (bytes_per_line < 0)
 512                 return bytes_per_line;
 513
 514         *size = bytes_per_line * icd->user_height;
 515
 516         if (0 == *count)
 517                 *count = 32;
 518         if (*size * *count > MAX_VIDEO_MEM * 1024 * 1024)
 519                 *count = (MAX_VIDEO_MEM * 1024 * 1024) / *size;
 520
 521         return 0;
 522 }
 523
 524 static void free_buffer(struct videobuf_queue *vq, struct mx2_buffer *buf)
 525 {
 526         struct soc_camera_device *icd = vq->priv_data;
 527         struct videobuf_buffer *vb = &buf->vb;
 528
 529         dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 530                 vb, vb->baddr, vb->bsize);
 531
 532         /*
 533          * This waits until this buffer is out of danger, i.e., until it is no
 534          * longer in state VIDEOBUF_QUEUED or VIDEOBUF_ACTIVE
 535          */
 536         videobuf_waiton(vq, vb, 0, 0);
 537
 538         videobuf_dma_contig_free(vq, vb);
 539         dev_dbg(icd->parent, "%s freed\n", __func__);
 540
 541         vb->state = VIDEOBUF_NEEDS_INIT;
 542 }
 543
 544 static int mx2_videobuf_prepare(struct videobuf_queue *vq,
 545                 struct videobuf_buffer *vb, enum v4l2_field field)
 546 {
 547         struct soc_camera_device *icd = vq->priv_data;
 548         struct mx2_buffer *buf = container_of(vb, struct mx2_buffer, vb);
 549         int bytes_per_line = soc_mbus_bytes_per_line(icd->user_width,
 550                         icd->current_fmt->host_fmt);
 551         int ret = 0;
 552
 553         dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 554                 vb, vb->baddr, vb->bsize);
 555
 556         if (bytes_per_line < 0)
 557                 return bytes_per_line;
 558
 559 #ifdef DEBUG
 560         /*
 561          * This can be useful if you want to see if we actually fill
 562          * the buffer with something
 563          */
 564         memset((void *)vb->baddr, 0xaa, vb->bsize);
 565 #endif
 566
 567         if (buf->code   != icd->current_fmt->code ||
 568             vb->width   != icd->user_width ||
 569             vb->height  != icd->user_height ||
 570             vb->field   != field) {
 571                 buf->code       = icd->current_fmt->code;
 572                 vb->width       = icd->user_width;
 573                 vb->height      = icd->user_height;
 574                 vb->field       = field;
 575                 vb->state       = VIDEOBUF_NEEDS_INIT;
 576         }
 577
 578         vb->size = bytes_per_line * vb->height;
 579         if (vb->baddr && vb->bsize < vb->size) {
 580                 ret = -EINVAL;
 581                 goto out;
 582         }
 583
 584         if (vb->state == VIDEOBUF_NEEDS_INIT) {
 585                 ret = videobuf_iolock(vq, vb, NULL);
 586                 if (ret)
 587                         goto fail;
 588
 589                 vb->state = VIDEOBUF_PREPARED;
 590         }
 591
 592         return 0;
 593
 594 fail:
 595         free_buffer(vq, buf);
 596 out:
 597         return ret;
 598 }
 599
 600 static void mx2_videobuf_queue(struct videobuf_queue *vq,
 601                                struct videobuf_buffer *vb)
 602 {
 603         struct soc_camera_device *icd = vq->priv_data;
 604         struct soc_camera_host *ici =
 605                 to_soc_camera_host(icd->parent);
 606         struct mx2_camera_dev *pcdev = ici->priv;
 607         struct mx2_buffer *buf = container_of(vb, struct mx2_buffer, vb);
 608         unsigned long flags;
 609
 610         dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 611                 vb, vb->baddr, vb->bsize);
 612
 613         spin_lock_irqsave(&pcdev->lock, flags);
 614
 615         vb->state = VIDEOBUF_QUEUED;
 616         list_add_tail(&vb->queue, &pcdev->capture);
 617
 618         if (mx27_camera_emma(pcdev)) {
 619                 goto out;
 620 #ifdef CONFIG_MACH_MX27
 621         } else if (cpu_is_mx27()) {
 622                 int ret;
 623
 624                 if (pcdev->active == NULL) {
 625                         ret = imx_dma_setup_single(pcdev->dma,
 626                                         videobuf_to_dma_contig(vb), vb->size,
 627                                         (u32)pcdev->base_dma + 0x10,
 628                                         DMA_MODE_READ);
 629                         if (ret) {
 630                                 vb->state = VIDEOBUF_ERROR;
 631                                 wake_up(&vb->done);
 632                                 goto out;
 633                         }
 634
 635                         vb->state = VIDEOBUF_ACTIVE;
 636                         pcdev->active = buf;
 637                 }
 638 #endif
 639         } else { /* cpu_is_mx25() */
 640                 u32 csicr3, dma_inten = 0;
 641
 642                 if (pcdev->fb1_active == NULL) {
 643                         writel(videobuf_to_dma_contig(vb),
 644                                         pcdev->base_csi + CSIDMASA_FB1);
 645                         pcdev->fb1_active = buf;
 646                         dma_inten = CSICR1_FB1_DMA_INTEN;
 647                 } else if (pcdev->fb2_active == NULL) {
 648                         writel(videobuf_to_dma_contig(vb),
 649                                         pcdev->base_csi + CSIDMASA_FB2);
 650                         pcdev->fb2_active = buf;
 651                         dma_inten = CSICR1_FB2_DMA_INTEN;
 652                 }
 653
 654                 if (dma_inten) {
 655                         list_del(&vb->queue);
 656                         vb->state = VIDEOBUF_ACTIVE;
 657
 658                         csicr3 = readl(pcdev->base_csi + CSICR3);
 659
 660                         /* Reflash DMA */
 661                         writel(csicr3 | CSICR3_DMA_REFLASH_RFF,
 662                                         pcdev->base_csi + CSICR3);
 663
 664                         /* clear & enable interrupts */
 665                         writel(dma_inten, pcdev->base_csi + CSISR);
 666                         pcdev->csicr1 |= dma_inten;
 667                         writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
 668
 669                         /* enable DMA */
 670                         csicr3 |= CSICR3_DMA_REQ_EN_RFF | CSICR3_RXFF_LEVEL(1);
 671                         writel(csicr3, pcdev->base_csi + CSICR3);
 672                 }
 673         }
 674
 675 out:
 676         spin_unlock_irqrestore(&pcdev->lock, flags);
 677 }
 678
 679 static void mx2_videobuf_release(struct videobuf_queue *vq,
 680                                  struct videobuf_buffer *vb)
 681 {
 682         struct soc_camera_device *icd = vq->priv_data;
 683         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 684         struct mx2_camera_dev *pcdev = ici->priv;
 685         struct mx2_buffer *buf = container_of(vb, struct mx2_buffer, vb);
 686         unsigned long flags;
 687
 688 #ifdef DEBUG
 689         dev_dbg(icd->parent, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
 690                 vb, vb->baddr, vb->bsize);
 691
 692         switch (vb->state) {
 693         case VIDEOBUF_ACTIVE:
 694                 dev_info(icd->parent, "%s (active)\n", __func__);
 695                 break;
 696         case VIDEOBUF_QUEUED:
 697                 dev_info(icd->parent, "%s (queued)\n", __func__);
 698                 break;
 699         case VIDEOBUF_PREPARED:
 700                 dev_info(icd->parent, "%s (prepared)\n", __func__);
 701                 break;
 702         default:
 703                 dev_info(icd->parent, "%s (unknown) %d\n", __func__,
 704                                 vb->state);
 705                 break;
 706         }
 707 #endif
 708
 709         /*
 710          * Terminate only queued but inactive buffers. Active buffers are
 711          * released when they become inactive after videobuf_waiton().
 712          *
 713          * FIXME: implement forced termination of active buffers for mx27 and
 714          * mx27 eMMA, so that the user won't get stuck in an uninterruptible
 715          * state. This requires a specific handling for each of the these DMA
 716          * types.
 717          */
 718         spin_lock_irqsave(&pcdev->lock, flags);
 719         if (vb->state == VIDEOBUF_QUEUED) {
 720                 list_del(&vb->queue);
 721                 vb->state = VIDEOBUF_ERROR;
 722         } else if (cpu_is_mx25() && vb->state == VIDEOBUF_ACTIVE) {
 723                 if (pcdev->fb1_active == buf) {
 724                         pcdev->csicr1 &= ~CSICR1_FB1_DMA_INTEN;
 725                         writel(0, pcdev->base_csi + CSIDMASA_FB1);
 726                         pcdev->fb1_active = NULL;
 727                 } else if (pcdev->fb2_active == buf) {
 728                         pcdev->csicr1 &= ~CSICR1_FB2_DMA_INTEN;
 729                         writel(0, pcdev->base_csi + CSIDMASA_FB2);
 730                         pcdev->fb2_active = NULL;
 731                 }
 732                 writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
 733                 vb->state = VIDEOBUF_ERROR;
 734         }
 735         spin_unlock_irqrestore(&pcdev->lock, flags);
 736
 737         free_buffer(vq, buf);
 738 }
 739
 740 static struct videobuf_queue_ops mx2_videobuf_ops = {
 741         .buf_setup      = mx2_videobuf_setup,
 742         .buf_prepare    = mx2_videobuf_prepare,
 743         .buf_queue      = mx2_videobuf_queue,
 744         .buf_release    = mx2_videobuf_release,
 745 };
 746
 747 static void mx2_camera_init_videobuf(struct videobuf_queue *q,
 748                               struct soc_camera_device *icd)
 749 {
 750         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 751         struct mx2_camera_dev *pcdev = ici->priv;
 752
 753         videobuf_queue_dma_contig_init(q, &mx2_videobuf_ops, pcdev->dev,
 754                         &pcdev->lock, V4L2_BUF_TYPE_VIDEO_CAPTURE,
 755                         V4L2_FIELD_NONE, sizeof(struct mx2_buffer),
 756                         icd, &icd->video_lock);
 757 }
 758
 759 #define MX2_BUS_FLAGS   (V4L2_MBUS_MASTER | \
 760                         V4L2_MBUS_VSYNC_ACTIVE_HIGH | \
 761                         V4L2_MBUS_VSYNC_ACTIVE_LOW | \
 762                         V4L2_MBUS_HSYNC_ACTIVE_HIGH | \
 763                         V4L2_MBUS_HSYNC_ACTIVE_LOW | \
 764                         V4L2_MBUS_PCLK_SAMPLE_RISING | \
 765                         V4L2_MBUS_PCLK_SAMPLE_FALLING | \
 766                         V4L2_MBUS_DATA_ACTIVE_HIGH | \
 767                         V4L2_MBUS_DATA_ACTIVE_LOW)
 768
 769 static int mx27_camera_emma_prp_reset(struct mx2_camera_dev *pcdev)
 770 {
 771         u32 cntl;
 772         int count = 0;
 773
 774         cntl = readl(pcdev->base_emma + PRP_CNTL);
 775         writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
 776         while (count++ < 100) {
 777                 if (!(readl(pcdev->base_emma + PRP_CNTL) & PRP_CNTL_SWRST))
 778                         return 0;
 779                 barrier();
 780                 udelay(1);
 781         }
 782
 783         return -ETIMEDOUT;
 784 }
 785
 786 static void mx27_camera_emma_buf_init(struct soc_camera_device *icd,
 787                 int bytesperline)
 788 {
 789         struct soc_camera_host *ici =
 790                 to_soc_camera_host(icd->parent);
 791         struct mx2_camera_dev *pcdev = ici->priv;
 792         struct mx2_fmt_cfg *prp = pcdev->emma_prp;
 793         u32 imgsize = pcdev->icd->user_height * pcdev->icd->user_width;
 794
 795         if (prp->cfg.channel == 1) {
 796                 writel(pcdev->discard_buffer_dma,
 797                                 pcdev->base_emma + PRP_DEST_RGB1_PTR);
 798                 writel(pcdev->discard_buffer_dma,
 799                                 pcdev->base_emma + PRP_DEST_RGB2_PTR);
 800
 801                 writel(PRP_CNTL_CH1EN |
 802                                 PRP_CNTL_CSIEN |
 803                                 prp->cfg.in_fmt |
 804                                 prp->cfg.out_fmt |
 805                                 PRP_CNTL_CH1_LEN |
 806                                 PRP_CNTL_CH1BYP |
 807                                 PRP_CNTL_CH1_TSKIP(0) |
 808                                 PRP_CNTL_IN_TSKIP(0),
 809                                 pcdev->base_emma + PRP_CNTL);
 810
 811                 writel((icd->user_width << 16) | icd->user_height,
 812                         pcdev->base_emma + PRP_SRC_FRAME_SIZE);
 813                 writel((icd->user_width << 16) | icd->user_height,
 814                         pcdev->base_emma + PRP_CH1_OUT_IMAGE_SIZE);
 815                 writel(bytesperline,
 816                         pcdev->base_emma + PRP_DEST_CH1_LINE_STRIDE);
 817                 writel(prp->cfg.src_pixel,
 818                         pcdev->base_emma + PRP_SRC_PIXEL_FORMAT_CNTL);
 819                 writel(prp->cfg.ch1_pixel,
 820                         pcdev->base_emma + PRP_CH1_PIXEL_FORMAT_CNTL);
 821         } else { /* channel 2 */
 822                 writel(pcdev->discard_buffer_dma,
 823                         pcdev->base_emma + PRP_DEST_Y_PTR);
 824                 writel(pcdev->discard_buffer_dma,
 825                         pcdev->base_emma + PRP_SOURCE_Y_PTR);
 826
 827                 if (prp->cfg.out_fmt == PRP_CNTL_CH2_OUT_YUV420) {
 828                         writel(pcdev->discard_buffer_dma + imgsize,
 829                                 pcdev->base_emma + PRP_DEST_CB_PTR);
 830                         writel(pcdev->discard_buffer_dma + ((5 * imgsize) / 4),
 831                                 pcdev->base_emma + PRP_DEST_CR_PTR);
 832                         writel(pcdev->discard_buffer_dma + imgsize,
 833                                 pcdev->base_emma + PRP_SOURCE_CB_PTR);
 834                         writel(pcdev->discard_buffer_dma + ((5 * imgsize) / 4),
 835                                 pcdev->base_emma + PRP_SOURCE_CR_PTR);
 836                 }
 837
 838                 writel(PRP_CNTL_CH2EN |
 839                         PRP_CNTL_CSIEN |
 840                         prp->cfg.in_fmt |
 841                         prp->cfg.out_fmt |
 842                         PRP_CNTL_CH2_LEN |
 843                         PRP_CNTL_CH2_TSKIP(0) |
 844                         PRP_CNTL_IN_TSKIP(0),
 845                         pcdev->base_emma + PRP_CNTL);
 846
 847                 writel((icd->user_width << 16) | icd->user_height,
 848                         pcdev->base_emma + PRP_SRC_FRAME_SIZE);
 849
 850                 writel((icd->user_width << 16) | icd->user_height,
 851                         pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE);
 852
 853                 writel(prp->cfg.src_pixel,
 854                         pcdev->base_emma + PRP_SRC_PIXEL_FORMAT_CNTL);
 855
 856         }
 857
 858         /* Enable interrupts */
 859         writel(prp->cfg.irq_flags, pcdev->base_emma + PRP_INTR_CNTL);
 860 }
 861
 862 static int mx2_camera_set_bus_param(struct soc_camera_device *icd)
 863 {
 864         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 865         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 866         struct mx2_camera_dev *pcdev = ici->priv;
 867         struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,};
 868         unsigned long common_flags;
 869         int ret;
 870         int bytesperline;
 871         u32 csicr1 = pcdev->csicr1;
 872
 873         ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg);
 874         if (!ret) {
 875                 common_flags = soc_mbus_config_compatible(&cfg, MX2_BUS_FLAGS);
 876                 if (!common_flags) {
 877                         dev_warn(icd->parent,
 878                                  "Flags incompatible: camera 0x%x, host 0x%x\n",
 879                                  cfg.flags, MX2_BUS_FLAGS);
 880                         return -EINVAL;
 881                 }
 882         } else if (ret != -ENOIOCTLCMD) {
 883                 return ret;
 884         } else {
 885                 common_flags = MX2_BUS_FLAGS;
 886         }
 887
 888         if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) &&
 889             (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) {
 890                 if (pcdev->platform_flags & MX2_CAMERA_HSYNC_HIGH)
 891                         common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW;
 892                 else
 893                         common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH;
 894         }
 895
 896         if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) &&
 897             (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) {
 898                 if (pcdev->platform_flags & MX2_CAMERA_PCLK_SAMPLE_RISING)
 899                         common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING;
 900                 else
 901                         common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING;
 902         }
 903
 904         cfg.flags = common_flags;
 905         ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg);
 906         if (ret < 0 && ret != -ENOIOCTLCMD) {
 907                 dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n",
 908                         common_flags, ret);
 909                 return ret;
 910         }
 911
 912         if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
 913                 csicr1 |= CSICR1_REDGE;
 914         if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
 915                 csicr1 |= CSICR1_SOF_POL;
 916         if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
 917                 csicr1 |= CSICR1_HSYNC_POL;
 918         if (pcdev->platform_flags & MX2_CAMERA_SWAP16)
 919                 csicr1 |= CSICR1_SWAP16_EN;
 920         if (pcdev->platform_flags & MX2_CAMERA_EXT_VSYNC)
 921                 csicr1 |= CSICR1_EXT_VSYNC;
 922         if (pcdev->platform_flags & MX2_CAMERA_CCIR)
 923                 csicr1 |= CSICR1_CCIR_EN;
 924         if (pcdev->platform_flags & MX2_CAMERA_CCIR_INTERLACE)
 925                 csicr1 |= CSICR1_CCIR_MODE;
 926         if (pcdev->platform_flags & MX2_CAMERA_GATED_CLOCK)
 927                 csicr1 |= CSICR1_GCLK_MODE;
 928         if (pcdev->platform_flags & MX2_CAMERA_INV_DATA)
 929                 csicr1 |= CSICR1_INV_DATA;
 930         if (pcdev->platform_flags & MX2_CAMERA_PACK_DIR_MSB)
 931                 csicr1 |= CSICR1_PACK_DIR;
 932
 933         pcdev->csicr1 = csicr1;
 934
 935         bytesperline = soc_mbus_bytes_per_line(icd->user_width,
 936                         icd->current_fmt->host_fmt);
 937         if (bytesperline < 0)
 938                 return bytesperline;
 939
 940         if (mx27_camera_emma(pcdev)) {
 941                 ret = mx27_camera_emma_prp_reset(pcdev);
 942                 if (ret)
 943                         return ret;
 944
 945                 if (pcdev->discard_buffer)
 946                         dma_free_coherent(ici->v4l2_dev.dev,
 947                                 pcdev->discard_size, pcdev->discard_buffer,
 948                                 pcdev->discard_buffer_dma);
 949
 950                 /*
 951                  * I didn't manage to properly enable/disable the prp
 952                  * on a per frame basis during running transfers,
 953                  * thus we allocate a buffer here and use it to
 954                  * discard frames when no buffer is available.
 955                  * Feel free to work on this ;)
 956                  */
 957                 pcdev->discard_size = icd->user_height * bytesperline;
 958                 pcdev->discard_buffer = dma_alloc_coherent(ici->v4l2_dev.dev,
 959                                 pcdev->discard_size, &pcdev->discard_buffer_dma,
 960                                 GFP_KERNEL);
 961                 if (!pcdev->discard_buffer)
 962                         return -ENOMEM;
 963
 964                 mx27_camera_emma_buf_init(icd, bytesperline);
 965         } else if (cpu_is_mx25()) {
 966                 writel((bytesperline * icd->user_height) >> 2,
 967                                 pcdev->base_csi + CSIRXCNT);
 968                 writel((bytesperline << 16) | icd->user_height,
 969                                 pcdev->base_csi + CSIIMAG_PARA);
 970         }
 971
 972         writel(pcdev->csicr1, pcdev->base_csi + CSICR1);
 973
 974         return 0;
 975 }
 976
 977 static int mx2_camera_set_crop(struct soc_camera_device *icd,
 978                                 struct v4l2_crop *a)
 979 {
 980         struct v4l2_rect *rect = &a->c;
 981         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 982         struct v4l2_mbus_framefmt mf;
 983         int ret;
 984
 985         soc_camera_limit_side(&rect->left, &rect->width, 0, 2, 4096);
 986         soc_camera_limit_side(&rect->top, &rect->height, 0, 2, 4096);
 987
 988         ret = v4l2_subdev_call(sd, video, s_crop, a);
 989         if (ret < 0)
 990                 return ret;
 991
 992         /* The capture device might have changed its output  */
 993         ret = v4l2_subdev_call(sd, video, g_mbus_fmt, &mf);
 994         if (ret < 0)
 995                 return ret;
 996
 997         dev_dbg(icd->parent, "Sensor cropped %dx%d\n",
 998                 mf.width, mf.height);
 999
1000         icd->user_width         = mf.width;
1001         icd->user_height        = mf.height;
1002
1003         return ret;
1004 }
1005
1006 static int mx2_camera_get_formats(struct soc_camera_device *icd,
1007                                   unsigned int idx,
1008                                   struct soc_camera_format_xlate *xlate)
1009 {
1010         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1011         const struct soc_mbus_pixelfmt *fmt;
1012         struct device *dev = icd->parent;
1013         enum v4l2_mbus_pixelcode code;
1014         int ret, formats = 0;
1015
1016         ret = v4l2_subdev_call(sd, video, enum_mbus_fmt, idx, &code);
1017         if (ret < 0)
1018                 /* no more formats */
1019                 return 0;
1020
1021         fmt = soc_mbus_get_fmtdesc(code);
1022         if (!fmt) {
1023                 dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
1024                 return 0;
1025         }
1026
1027         if (code == V4L2_MBUS_FMT_YUYV8_2X8) {
1028                 formats++;
1029                 if (xlate) {
1030                         /*
1031                          * CH2 can output YUV420 which is a standard format in
1032                          * soc_mediabus.c
1033                          */
1034                         xlate->host_fmt =
1035                                 soc_mbus_get_fmtdesc(V4L2_MBUS_FMT_YUYV8_1_5X8);
1036                         xlate->code     = code;
1037                         dev_dbg(dev, "Providing host format %s for sensor code %d\n",
1038                                xlate->host_fmt->name, code);
1039                         xlate++;
1040                 }
1041         }
1042
1043         /* Generic pass-trough */
1044         formats++;
1045         if (xlate) {
1046                 xlate->host_fmt = fmt;
1047                 xlate->code     = code;
1048                 xlate++;
1049         }
1050         return formats;
1051 }
1052
1053 static int mx2_camera_set_fmt(struct soc_camera_device *icd,
1054                                struct v4l2_format *f)
1055 {
1056         struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
1057         struct mx2_camera_dev *pcdev = ici->priv;
1058         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1059         const struct soc_camera_format_xlate *xlate;
1060         struct v4l2_pix_format *pix = &f->fmt.pix;
1061         struct v4l2_mbus_framefmt mf;
1062         int ret;
1063
1064         xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat);
1065         if (!xlate) {
1066                 dev_warn(icd->parent, "Format %x not found\n",
1067                                 pix->pixelformat);
1068                 return -EINVAL;
1069         }
1070
1071         mf.width        = pix->width;
1072         mf.height       = pix->height;
1073         mf.field        = pix->field;
1074         mf.colorspace   = pix->colorspace;
1075         mf.code         = xlate->code;
1076
1077         ret = v4l2_subdev_call(sd, video, s_mbus_fmt, &mf);
1078         if (ret < 0 && ret != -ENOIOCTLCMD)
1079                 return ret;
1080
1081         if (mf.code != xlate->code)
1082                 return -EINVAL;
1083
1084         pix->width              = mf.width;
1085         pix->height             = mf.height;
1086         pix->field              = mf.field;
1087         pix->colorspace         = mf.colorspace;
1088         icd->current_fmt        = xlate;
1089
1090         if (mx27_camera_emma(pcdev))
1091                 pcdev->emma_prp = mx27_emma_prp_get_format(xlate->code,
1092                                                 xlate->host_fmt->fourcc);
1093
1094         return 0;
1095 }
1096
1097 static int mx2_camera_try_fmt(struct soc_camera_device *icd,
1098                                   struct v4l2_format *f)
1099 {
1100         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
1101         const struct soc_camera_format_xlate *xlate;
1102         struct v4l2_pix_format *pix = &f->fmt.pix;
1103         struct v4l2_mbus_framefmt mf;
1104         __u32 pixfmt = pix->pixelformat;
1105         unsigned int width_limit;
1106         int ret;
1107
1108         xlate = soc_camera_xlate_by_fourcc(icd, pixfmt);
1109         if (pixfmt && !xlate) {
1110                 dev_warn(icd->parent, "Format %x not found\n", pixfmt);
1111                 return -EINVAL;
1112         }
1113
1114         /* FIXME: implement MX27 limits */
1115
1116         /* limit to MX25 hardware capabilities */
1117         if (cpu_is_mx25()) {
1118                 if (xlate->host_fmt->bits_per_sample <= 8)
1119                         width_limit = 0xffff * 4;
1120                 else
1121                         width_limit = 0xffff * 2;
1122                 /* CSIIMAG_PARA limit */
1123                 if (pix->width > width_limit)
1124                         pix->width = width_limit;
1125                 if (pix->height > 0xffff)
1126                         pix->height = 0xffff;
1127
1128                 pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
1129                                 xlate->host_fmt);
1130                 if (pix->bytesperline < 0)
1131                         return pix->bytesperline;
1132                 pix->sizeimage = pix->height * pix->bytesperline;
1133                 /* Check against the CSIRXCNT limit */
1134                 if (pix->sizeimage > 4 * 0x3ffff) {
1135                         /* Adjust geometry, preserve aspect ratio */
1136                         unsigned int new_height = int_sqrt(4 * 0x3ffff *
1137                                         pix->height / pix->bytesperline);
1138                         pix->width = new_height * pix->width / pix->height;
1139                         pix->height = new_height;
1140                         pix->bytesperline = soc_mbus_bytes_per_line(pix->width,
1141                                                         xlate->host_fmt);
1142                         BUG_ON(pix->bytesperline < 0);
1143                 }
1144         }
1145
1146         /* limit to sensor capabilities */
1147         mf.width        = pix->width;
1148         mf.height       = pix->height;
1149         mf.field        = pix->field;
1150         mf.colorspace   = pix->colorspace;
1151         mf.code         = xlate->code;
1152
1153         ret = v4l2_subdev_call(sd, video, try_mbus_fmt, &mf);
1154         if (ret < 0)
1155                 return ret;
1156
1157         if (mf.field == V4L2_FIELD_ANY)
1158                 mf.field = V4L2_FIELD_NONE;
1159         /*
1160          * Driver supports interlaced images provided they have
1161          * both fields so that they can be processed as if they
1162          * were progressive.
1163          */
1164         if (mf.field != V4L2_FIELD_NONE && !V4L2_FIELD_HAS_BOTH(mf.field)) {
1165                 dev_err(icd->parent, "Field type %d unsupported.\n",
1166                                 mf.field);
1167                 return -EINVAL;
1168         }
1169
1170         pix->width      = mf.width;
1171         pix->height     = mf.height;
1172         pix->field      = mf.field;
1173         pix->colorspace = mf.colorspace;
1174
1175         return 0;
1176 }
1177
1178 static int mx2_camera_querycap(struct soc_camera_host *ici,
1179                                struct v4l2_capability *cap)
1180 {
1181         /* cap->name is set by the friendly caller:-> */
1182         strlcpy(cap->card, MX2_CAM_DRIVER_DESCRIPTION, sizeof(cap->card));
1183         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1184
1185         return 0;
1186 }
1187
1188 static int mx2_camera_reqbufs(struct soc_camera_device *icd,
1189                               struct v4l2_requestbuffers *p)
1190 {
1191         int i;
1192
1193         for (i = 0; i < p->count; i++) {
1194                 struct mx2_buffer *buf = container_of(icd->vb_vidq.bufs[i],
1195                                                       struct mx2_buffer, vb);
1196                 INIT_LIST_HEAD(&buf->vb.queue);
1197         }
1198
1199         return 0;
1200 }
1201
1202 #ifdef CONFIG_MACH_MX27
1203 static void mx27_camera_frame_done(struct mx2_camera_dev *pcdev, int state)
1204 {
1205         struct videobuf_buffer *vb;
1206         struct mx2_buffer *buf;
1207         unsigned long flags;
1208         int ret;
1209
1210         spin_lock_irqsave(&pcdev->lock, flags);
1211
1212         if (!pcdev->active) {
1213                 dev_err(pcdev->dev, "%s called with no active buffer!\n",
1214                                 __func__);
1215                 goto out;
1216         }
1217
1218         vb = &pcdev->active->vb;
1219         buf = container_of(vb, struct mx2_buffer, vb);
1220         WARN_ON(list_empty(&vb->queue));
1221         dev_dbg(pcdev->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__,
1222                 vb, vb->baddr, vb->bsize);
1223
1224         /* _init is used to debug races, see comment in pxa_camera_reqbufs() */
1225         list_del_init(&vb->queue);
1226         vb->state = state;
1227         do_gettimeofday(&vb->ts);
1228         vb->field_count++;
1229
1230         wake_up(&vb->done);
1231
1232         if (list_empty(&pcdev->capture)) {
1233                 pcdev->active = NULL;
1234                 goto out;
1235         }
1236
1237         pcdev->active = list_entry(pcdev->capture.next,
1238                         struct mx2_buffer, vb.queue);
1239
1240         vb = &pcdev->active->vb;
1241         vb->state = VIDEOBUF_ACTIVE;
1242
1243         ret = imx_dma_setup_single(pcdev->dma, videobuf_to_dma_contig(vb),
1244                         vb->size, (u32)pcdev->base_dma + 0x10, DMA_MODE_READ);
1245
1246         if (ret) {
1247                 vb->state = VIDEOBUF_ERROR;
1248                 pcdev->active = NULL;
1249                 wake_up(&vb->done);
1250         }
1251
1252 out:
1253         spin_unlock_irqrestore(&pcdev->lock, flags);
1254 }
1255
1256 static void mx27_camera_dma_err_callback(int channel, void *data, int err)
1257 {
1258         struct mx2_camera_dev *pcdev = data;
1259
1260         mx27_camera_frame_done(pcdev, VIDEOBUF_ERROR);
1261 }
1262
1263 static void mx27_camera_dma_callback(int channel, void *data)
1264 {
1265         struct mx2_camera_dev *pcdev = data;
1266
1267         mx27_camera_frame_done(pcdev, VIDEOBUF_DONE);
1268 }
1269
1270 #define DMA_REQ_CSI_RX          31 /* FIXME: Add this to a resource */
1271
1272 static int __devinit mx27_camera_dma_init(struct platform_device *pdev,
1273                 struct mx2_camera_dev *pcdev)
1274 {
1275         int err;
1276
1277         pcdev->dma = imx_dma_request_by_prio("CSI RX DMA", DMA_PRIO_HIGH);
1278         if (pcdev->dma < 0) {
1279                 dev_err(&pdev->dev, "%s failed to request DMA channel\n",
1280                                 __func__);
1281                 return pcdev->dma;
1282         }
1283
1284         err = imx_dma_setup_handlers(pcdev->dma, mx27_camera_dma_callback,
1285                                         mx27_camera_dma_err_callback, pcdev);
1286         if (err) {
1287                 dev_err(&pdev->dev, "%s failed to set DMA callback\n",
1288                                 __func__);
1289                 goto err_out;
1290         }
1291
1292         err = imx_dma_config_channel(pcdev->dma,
1293                         IMX_DMA_MEMSIZE_32 | IMX_DMA_TYPE_FIFO,
1294                         IMX_DMA_MEMSIZE_32 | IMX_DMA_TYPE_LINEAR,
1295                         DMA_REQ_CSI_RX, 1);
1296         if (err) {
1297                 dev_err(&pdev->dev, "%s failed to config DMA channel\n",
1298                                 __func__);
1299                 goto err_out;
1300         }
1301
1302         imx_dma_config_burstlen(pcdev->dma, 64);
1303
1304         return 0;
1305
1306 err_out:
1307         imx_dma_free(pcdev->dma);
1308
1309         return err;
1310 }
1311 #endif /* CONFIG_MACH_MX27 */
1312
1313 static unsigned int mx2_camera_poll(struct file *file, poll_table *pt)
1314 {
1315         struct soc_camera_device *icd = file->private_data;
1316
1317         return videobuf_poll_stream(file, &icd->vb_vidq, pt);
1318 }
1319
1320 static struct soc_camera_host_ops mx2_soc_camera_host_ops = {
1321         .owner          = THIS_MODULE,
1322         .add            = mx2_camera_add_device,
1323         .remove         = mx2_camera_remove_device,
1324         .set_fmt        = mx2_camera_set_fmt,
1325         .set_crop       = mx2_camera_set_crop,
1326         .get_formats    = mx2_camera_get_formats,
1327         .try_fmt        = mx2_camera_try_fmt,
1328         .init_videobuf  = mx2_camera_init_videobuf,
1329         .reqbufs        = mx2_camera_reqbufs,
1330         .poll           = mx2_camera_poll,
1331         .querycap       = mx2_camera_querycap,
1332         .set_bus_param  = mx2_camera_set_bus_param,
1333 };
1334
1335 static void mx27_camera_frame_done_emma(struct mx2_camera_dev *pcdev,
1336                 int bufnum, int state)
1337 {
1338         u32 imgsize = pcdev->icd->user_height * pcdev->icd->user_width;
1339         struct mx2_fmt_cfg *prp = pcdev->emma_prp;
1340         struct mx2_buffer *buf;
1341         struct videobuf_buffer *vb;
1342         unsigned long phys;
1343
1344         if (!list_empty(&pcdev->active_bufs)) {
1345                 buf = list_entry(pcdev->active_bufs.next,
1346                         struct mx2_buffer, vb.queue);
1347
1348                 BUG_ON(buf->bufnum != bufnum);
1349
1350                 vb = &buf->vb;
1351 #ifdef DEBUG
1352                 phys = videobuf_to_dma_contig(vb);
1353                 if (prp->cfg.channel == 1) {
1354                         if (readl(pcdev->base_emma + PRP_DEST_RGB1_PTR +
1355                                 4 * bufnum) != phys) {
1356                                 dev_err(pcdev->dev, "%p != %p\n", phys,
1357                                                 readl(pcdev->base_emma +
1358                                                         PRP_DEST_RGB1_PTR +
1359                                                         4 * bufnum));
1360                         }
1361                 } else {
1362                         if (readl(pcdev->base_emma + PRP_DEST_Y_PTR -
1363                                 0x14 * bufnum) != phys) {
1364                                 dev_err(pcdev->dev, "%p != %p\n", phys,
1365                                                 readl(pcdev->base_emma +
1366                                                         PRP_DEST_Y_PTR -
1367                                                         0x14 * bufnum));
1368                         }
1369                 }
1370 #endif
1371                 dev_dbg(pcdev->dev, "%s (vb=0x%p) 0x%08lx %d\n", __func__, vb,
1372                                 vb->baddr, vb->bsize);
1373
1374                 list_del(&vb->queue);
1375                 vb->state = state;
1376                 do_gettimeofday(&vb->ts);
1377                 vb->field_count++;
1378
1379                 wake_up(&vb->done);
1380         }
1381
1382         if (list_empty(&pcdev->capture)) {
1383                 if (prp->cfg.channel == 1) {
1384                         writel(pcdev->discard_buffer_dma, pcdev->base_emma +
1385                                         PRP_DEST_RGB1_PTR + 4 * bufnum);
1386                 } else {
1387                         writel(pcdev->discard_buffer_dma, pcdev->base_emma +
1388                                                 PRP_DEST_Y_PTR -
1389                                                 0x14 * bufnum);
1390                         if (prp->out_fmt == V4L2_PIX_FMT_YUV420) {
1391                                 writel(pcdev->discard_buffer_dma + imgsize,
1392                                        pcdev->base_emma + PRP_DEST_CB_PTR -
1393                                        0x14 * bufnum);
1394                                 writel(pcdev->discard_buffer_dma +
1395                                        ((5 * imgsize) / 4), pcdev->base_emma +
1396                                        PRP_DEST_CR_PTR - 0x14 * bufnum);
1397                         }
1398                 }
1399                 return;
1400         }
1401
1402         buf = list_entry(pcdev->capture.next,
1403                         struct mx2_buffer, vb.queue);
1404
1405         buf->bufnum = !bufnum;
1406
1407         list_move_tail(pcdev->capture.next, &pcdev->active_bufs);
1408
1409         vb = &buf->vb;
1410         vb->state = VIDEOBUF_ACTIVE;
1411
1412         phys = videobuf_to_dma_contig(vb);
1413         if (prp->cfg.channel == 1) {
1414                 writel(phys, pcdev->base_emma + PRP_DEST_RGB1_PTR + 4 * bufnum);
1415         } else {
1416                 writel(phys, pcdev->base_emma +
1417                                 PRP_DEST_Y_PTR - 0x14 * bufnum);
1418                 if (prp->cfg.out_fmt == PRP_CNTL_CH2_OUT_YUV420) {
1419                         writel(phys + imgsize, pcdev->base_emma +
1420                                         PRP_DEST_CB_PTR - 0x14 * bufnum);
1421                         writel(phys + ((5 * imgsize) / 4), pcdev->base_emma +
1422                                         PRP_DEST_CR_PTR - 0x14 * bufnum);
1423                 }
1424         }
1425 }
1426
1427 static irqreturn_t mx27_camera_emma_irq(int irq_emma, void *data)
1428 {
1429         struct mx2_camera_dev *pcdev = data;
1430         unsigned int status = readl(pcdev->base_emma + PRP_INTRSTATUS);
1431         struct mx2_buffer *buf;
1432
1433         if (status & (1 << 7)) { /* overflow */
1434                 u32 cntl;
1435                 /*
1436                  * We only disable channel 1 here since this is the only
1437                  * enabled channel
1438                  *
1439                  * FIXME: the correct DMA overflow handling should be resetting
1440                  * the buffer, returning an error frame, and continuing with
1441                  * the next one.
1442                  */
1443                 cntl = readl(pcdev->base_emma + PRP_CNTL);
1444                 writel(cntl & ~(PRP_CNTL_CH1EN | PRP_CNTL_CH2EN),
1445                        pcdev->base_emma + PRP_CNTL);
1446                 writel(cntl, pcdev->base_emma + PRP_CNTL);
1447         }
1448         if ((((status & (3 << 5)) == (3 << 5)) ||
1449                 ((status & (3 << 3)) == (3 << 3)))
1450                         && !list_empty(&pcdev->active_bufs)) {
1451                 /*
1452                  * Both buffers have triggered, process the one we're expecting
1453                  * to first
1454                  */
1455                 buf = list_entry(pcdev->active_bufs.next,
1456                         struct mx2_buffer, vb.queue);
1457                 mx27_camera_frame_done_emma(pcdev, buf->bufnum, VIDEOBUF_DONE);
1458                 status &= ~(1 << (6 - buf->bufnum)); /* mark processed */
1459         }
1460         if ((status & (1 << 6)) || (status & (1 << 4)))
1461                 mx27_camera_frame_done_emma(pcdev, 0, VIDEOBUF_DONE);
1462         if ((status & (1 << 5)) || (status & (1 << 3)))
1463                 mx27_camera_frame_done_emma(pcdev, 1, VIDEOBUF_DONE);
1464
1465         writel(status, pcdev->base_emma + PRP_INTRSTATUS);
1466
1467         return IRQ_HANDLED;
1468 }
1469
1470 static int __devinit mx27_camera_emma_init(struct mx2_camera_dev *pcdev)
1471 {
1472         struct resource *res_emma = pcdev->res_emma;
1473         int err = 0;
1474
1475         if (!request_mem_region(res_emma->start, resource_size(res_emma),
1476                                 MX2_CAM_DRV_NAME)) {
1477                 err = -EBUSY;
1478                 goto out;
1479         }
1480
1481         pcdev->base_emma = ioremap(res_emma->start, resource_size(res_emma));
1482         if (!pcdev->base_emma) {
1483                 err = -ENOMEM;
1484                 goto exit_release;
1485         }
1486
1487         err = request_irq(pcdev->irq_emma, mx27_camera_emma_irq, 0,
1488                         MX2_CAM_DRV_NAME, pcdev);
1489         if (err) {
1490                 dev_err(pcdev->dev, "Camera EMMA interrupt register failed \n");
1491                 goto exit_iounmap;
1492         }
1493
1494         pcdev->clk_emma = clk_get(NULL, "emma");
1495         if (IS_ERR(pcdev->clk_emma)) {
1496                 err = PTR_ERR(pcdev->clk_emma);
1497                 goto exit_free_irq;
1498         }
1499
1500         clk_enable(pcdev->clk_emma);
1501
1502         err = mx27_camera_emma_prp_reset(pcdev);
1503         if (err)
1504                 goto exit_clk_emma_put;
1505
1506         return err;
1507
1508 exit_clk_emma_put:
1509         clk_disable(pcdev->clk_emma);
1510         clk_put(pcdev->clk_emma);
1511 exit_free_irq:
1512         free_irq(pcdev->irq_emma, pcdev);
1513 exit_iounmap:
1514         iounmap(pcdev->base_emma);
1515 exit_release:
1516         release_mem_region(res_emma->start, resource_size(res_emma));
1517 out:
1518         return err;
1519 }
1520
1521 static int __devinit mx2_camera_probe(struct platform_device *pdev)
1522 {
1523         struct mx2_camera_dev *pcdev;
1524         struct resource *res_csi, *res_emma;
1525         void __iomem *base_csi;
1526         int irq_csi, irq_emma;
1527         irq_handler_t mx2_cam_irq_handler = cpu_is_mx25() ? mx25_camera_irq
1528                 : mx27_camera_irq;
1529         int err = 0;
1530
1531         dev_dbg(&pdev->dev, "initialising\n");
1532
1533         res_csi = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1534         irq_csi = platform_get_irq(pdev, 0);
1535         if (res_csi == NULL || irq_csi < 0) {
1536                 dev_err(&pdev->dev, "Missing platform resources data\n");
1537                 err = -ENODEV;
1538                 goto exit;
1539         }
1540
1541         pcdev = kzalloc(sizeof(*pcdev), GFP_KERNEL);
1542         if (!pcdev) {
1543                 dev_err(&pdev->dev, "Could not allocate pcdev\n");
1544                 err = -ENOMEM;
1545                 goto exit;
1546         }
1547
1548         pcdev->clk_csi = clk_get(&pdev->dev, NULL);
1549         if (IS_ERR(pcdev->clk_csi)) {
1550                 err = PTR_ERR(pcdev->clk_csi);
1551                 goto exit_kfree;
1552         }
1553
1554         dev_dbg(&pdev->dev, "Camera clock frequency: %ld\n",
1555                         clk_get_rate(pcdev->clk_csi));
1556
1557         /* Initialize DMA */
1558 #ifdef CONFIG_MACH_MX27
1559         if (cpu_is_mx27()) {
1560                 err = mx27_camera_dma_init(pdev, pcdev);
1561                 if (err)
1562                         goto exit_clk_put;
1563         }
1564 #endif /* CONFIG_MACH_MX27 */
1565
1566         pcdev->res_csi = res_csi;
1567         pcdev->pdata = pdev->dev.platform_data;
1568         if (pcdev->pdata) {
1569                 long rate;
1570
1571                 pcdev->platform_flags = pcdev->pdata->flags;
1572
1573                 rate = clk_round_rate(pcdev->clk_csi, pcdev->pdata->clk * 2);
1574                 if (rate <= 0) {
1575                         err = -ENODEV;
1576                         goto exit_dma_free;
1577                 }
1578                 err = clk_set_rate(pcdev->clk_csi, rate);
1579                 if (err < 0)
1580                         goto exit_dma_free;
1581         }
1582
1583         INIT_LIST_HEAD(&pcdev->capture);
1584         INIT_LIST_HEAD(&pcdev->active_bufs);
1585         spin_lock_init(&pcdev->lock);
1586
1587         /*
1588          * Request the regions.
1589          */
1590         if (!request_mem_region(res_csi->start, resource_size(res_csi),
1591                                 MX2_CAM_DRV_NAME)) {
1592                 err = -EBUSY;
1593                 goto exit_dma_free;
1594         }
1595
1596         base_csi = ioremap(res_csi->start, resource_size(res_csi));
1597         if (!base_csi) {
1598                 err = -ENOMEM;
1599                 goto exit_release;
1600         }
1601         pcdev->irq_csi = irq_csi;
1602         pcdev->base_csi = base_csi;
1603         pcdev->base_dma = res_csi->start;
1604         pcdev->dev = &pdev->dev;
1605
1606         err = request_irq(pcdev->irq_csi, mx2_cam_irq_handler, 0,
1607                         MX2_CAM_DRV_NAME, pcdev);
1608         if (err) {
1609                 dev_err(pcdev->dev, "Camera interrupt register failed \n");
1610                 goto exit_iounmap;
1611         }
1612
1613         if (cpu_is_mx27()) {
1614                 /* EMMA support */
1615                 res_emma = platform_get_resource(pdev, IORESOURCE_MEM, 1);
1616                 irq_emma = platform_get_irq(pdev, 1);
1617
1618                 if (res_emma && irq_emma >= 0) {
1619                         dev_info(&pdev->dev, "Using EMMA\n");
1620                         pcdev->use_emma = 1;
1621                         pcdev->res_emma = res_emma;
1622                         pcdev->irq_emma = irq_emma;
1623                         if (mx27_camera_emma_init(pcdev))
1624                                 goto exit_free_irq;
1625                 }
1626         }
1627
1628         pcdev->soc_host.drv_name        = MX2_CAM_DRV_NAME,
1629         pcdev->soc_host.ops             = &mx2_soc_camera_host_ops,
1630         pcdev->soc_host.priv            = pcdev;
1631         pcdev->soc_host.v4l2_dev.dev    = &pdev->dev;
1632         pcdev->soc_host.nr              = pdev->id;
1633         err = soc_camera_host_register(&pcdev->soc_host);
1634         if (err)
1635                 goto exit_free_emma;
1636
1637         dev_info(&pdev->dev, "MX2 Camera (CSI) driver probed, clock frequency: %ld\n",
1638                         clk_get_rate(pcdev->clk_csi));
1639
1640         return 0;
1641
1642 exit_free_emma:
1643         if (mx27_camera_emma(pcdev)) {
1644                 free_irq(pcdev->irq_emma, pcdev);
1645                 clk_disable(pcdev->clk_emma);
1646                 clk_put(pcdev->clk_emma);
1647                 iounmap(pcdev->base_emma);
1648                 release_mem_region(res_emma->start, resource_size(res_emma));
1649         }
1650 exit_free_irq:
1651         free_irq(pcdev->irq_csi, pcdev);
1652 exit_iounmap:
1653         iounmap(base_csi);
1654 exit_release:
1655         release_mem_region(res_csi->start, resource_size(res_csi));
1656 exit_dma_free:
1657 #ifdef CONFIG_MACH_MX27
1658         if (cpu_is_mx27())
1659                 imx_dma_free(pcdev->dma);
1660 exit_clk_put:
1661         clk_put(pcdev->clk_csi);
1662 #endif /* CONFIG_MACH_MX27 */
1663 exit_kfree:
1664         kfree(pcdev);
1665 exit:
1666         return err;
1667 }
1668
1669 static int __devexit mx2_camera_remove(struct platform_device *pdev)
1670 {
1671         struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev);
1672         struct mx2_camera_dev *pcdev = container_of(soc_host,
1673                         struct mx2_camera_dev, soc_host);
1674         struct resource *res;
1675
1676         clk_put(pcdev->clk_csi);
1677 #ifdef CONFIG_MACH_MX27
1678         if (cpu_is_mx27())
1679                 imx_dma_free(pcdev->dma);
1680 #endif /* CONFIG_MACH_MX27 */
1681         free_irq(pcdev->irq_csi, pcdev);
1682         if (mx27_camera_emma(pcdev))
1683                 free_irq(pcdev->irq_emma, pcdev);
1684
1685         soc_camera_host_unregister(&pcdev->soc_host);
1686
1687         iounmap(pcdev->base_csi);
1688
1689         if (mx27_camera_emma(pcdev)) {
1690                 clk_disable(pcdev->clk_emma);
1691                 clk_put(pcdev->clk_emma);
1692                 iounmap(pcdev->base_emma);
1693                 res = pcdev->res_emma;
1694                 release_mem_region(res->start, resource_size(res));
1695         }
1696
1697         res = pcdev->res_csi;
1698         release_mem_region(res->start, resource_size(res));
1699
1700         kfree(pcdev);
1701
1702         dev_info(&pdev->dev, "MX2 Camera driver unloaded\n");
1703
1704         return 0;
1705 }
1706
1707 static struct platform_driver mx2_camera_driver = {
1708         .driver         = {
1709                 .name   = MX2_CAM_DRV_NAME,
1710         },
1711         .remove         = __devexit_p(mx2_camera_remove),
1712 };
1713
1714
1715 static int __init mx2_camera_init(void)
1716 {
1717         return platform_driver_probe(&mx2_camera_driver, &mx2_camera_probe);
1718 }
1719
1720 static void __exit mx2_camera_exit(void)
1721 {
1722         return platform_driver_unregister(&mx2_camera_driver);
1723 }
1724
1725 module_init(mx2_camera_init);
1726 module_exit(mx2_camera_exit);
1727
1728 MODULE_DESCRIPTION("i.MX27/i.MX25 SoC Camera Host driver");
1729 MODULE_AUTHOR("Sascha Hauer <sha@pengutronix.de>");
1730 MODULE_LICENSE("GPL");
1731 MODULE_VERSION(MX2_CAM_VERSION);