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