5e2b4df48b3c93ee7ce89c4bcb08cc2000660358
[sfrench/cifs-2.6.git] / drivers / media / platform / marvell-ccic / mcam-core.c
1 /*
2  * The Marvell camera core.  This device appears in a number of settings,
3  * so it needs platform-specific support outside of the core.
4  *
5  * Copyright 2011 Jonathan Corbet corbet@lwn.net
6  */
7 #include <linux/kernel.h>
8 #include <linux/module.h>
9 #include <linux/fs.h>
10 #include <linux/mm.h>
11 #include <linux/i2c.h>
12 #include <linux/interrupt.h>
13 #include <linux/spinlock.h>
14 #include <linux/slab.h>
15 #include <linux/device.h>
16 #include <linux/wait.h>
17 #include <linux/list.h>
18 #include <linux/dma-mapping.h>
19 #include <linux/delay.h>
20 #include <linux/vmalloc.h>
21 #include <linux/io.h>
22 #include <linux/clk.h>
23 #include <linux/videodev2.h>
24 #include <media/v4l2-device.h>
25 #include <media/v4l2-ioctl.h>
26 #include <media/v4l2-ctrls.h>
27 #include <media/v4l2-event.h>
28 #include <media/ov7670.h>
29 #include <media/videobuf2-vmalloc.h>
30 #include <media/videobuf2-dma-contig.h>
31 #include <media/videobuf2-dma-sg.h>
32
33 #include "mcam-core.h"
34
35 #ifdef MCAM_MODE_VMALLOC
36 /*
37  * Internal DMA buffer management.  Since the controller cannot do S/G I/O,
38  * we must have physically contiguous buffers to bring frames into.
39  * These parameters control how many buffers we use, whether we
40  * allocate them at load time (better chance of success, but nails down
41  * memory) or when somebody tries to use the camera (riskier), and,
42  * for load-time allocation, how big they should be.
43  *
44  * The controller can cycle through three buffers.  We could use
45  * more by flipping pointers around, but it probably makes little
46  * sense.
47  */
48
49 static bool alloc_bufs_at_read;
50 module_param(alloc_bufs_at_read, bool, 0444);
51 MODULE_PARM_DESC(alloc_bufs_at_read,
52                 "Non-zero value causes DMA buffers to be allocated when the "
53                 "video capture device is read, rather than at module load "
54                 "time.  This saves memory, but decreases the chances of "
55                 "successfully getting those buffers.  This parameter is "
56                 "only used in the vmalloc buffer mode");
57
58 static int n_dma_bufs = 3;
59 module_param(n_dma_bufs, uint, 0644);
60 MODULE_PARM_DESC(n_dma_bufs,
61                 "The number of DMA buffers to allocate.  Can be either two "
62                 "(saves memory, makes timing tighter) or three.");
63
64 static int dma_buf_size = VGA_WIDTH * VGA_HEIGHT * 2;  /* Worst case */
65 module_param(dma_buf_size, uint, 0444);
66 MODULE_PARM_DESC(dma_buf_size,
67                 "The size of the allocated DMA buffers.  If actual operating "
68                 "parameters require larger buffers, an attempt to reallocate "
69                 "will be made.");
70 #else /* MCAM_MODE_VMALLOC */
71 static const bool alloc_bufs_at_read;
72 static const int n_dma_bufs = 3;  /* Used by S/G_PARM */
73 #endif /* MCAM_MODE_VMALLOC */
74
75 static bool flip;
76 module_param(flip, bool, 0444);
77 MODULE_PARM_DESC(flip,
78                 "If set, the sensor will be instructed to flip the image "
79                 "vertically.");
80
81 static int buffer_mode = -1;
82 module_param(buffer_mode, int, 0444);
83 MODULE_PARM_DESC(buffer_mode,
84                 "Set the buffer mode to be used; default is to go with what "
85                 "the platform driver asks for.  Set to 0 for vmalloc, 1 for "
86                 "DMA contiguous.");
87
88 /*
89  * Status flags.  Always manipulated with bit operations.
90  */
91 #define CF_BUF0_VALID    0      /* Buffers valid - first three */
92 #define CF_BUF1_VALID    1
93 #define CF_BUF2_VALID    2
94 #define CF_DMA_ACTIVE    3      /* A frame is incoming */
95 #define CF_CONFIG_NEEDED 4      /* Must configure hardware */
96 #define CF_SINGLE_BUFFER 5      /* Running with a single buffer */
97 #define CF_SG_RESTART    6      /* SG restart needed */
98 #define CF_FRAME_SOF0    7      /* Frame 0 started */
99 #define CF_FRAME_SOF1    8
100 #define CF_FRAME_SOF2    9
101
102 #define sensor_call(cam, o, f, args...) \
103         v4l2_subdev_call(cam->sensor, o, f, ##args)
104
105 static struct mcam_format_struct {
106         __u8 *desc;
107         __u32 pixelformat;
108         int bpp;   /* Bytes per pixel */
109         bool planar;
110         u32 mbus_code;
111 } mcam_formats[] = {
112         {
113                 .desc           = "YUYV 4:2:2",
114                 .pixelformat    = V4L2_PIX_FMT_YUYV,
115                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
116                 .bpp            = 2,
117                 .planar         = false,
118         },
119         {
120                 .desc           = "YVYU 4:2:2",
121                 .pixelformat    = V4L2_PIX_FMT_YVYU,
122                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
123                 .bpp            = 2,
124                 .planar         = false,
125         },
126         {
127                 .desc           = "YUV 4:2:0 PLANAR",
128                 .pixelformat    = V4L2_PIX_FMT_YUV420,
129                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
130                 .bpp            = 1,
131                 .planar         = true,
132         },
133         {
134                 .desc           = "YVU 4:2:0 PLANAR",
135                 .pixelformat    = V4L2_PIX_FMT_YVU420,
136                 .mbus_code      = MEDIA_BUS_FMT_YUYV8_2X8,
137                 .bpp            = 1,
138                 .planar         = true,
139         },
140         {
141                 .desc           = "XRGB 444",
142                 .pixelformat    = V4L2_PIX_FMT_XRGB444,
143                 .mbus_code      = MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE,
144                 .bpp            = 2,
145                 .planar         = false,
146         },
147         {
148                 .desc           = "RGB 565",
149                 .pixelformat    = V4L2_PIX_FMT_RGB565,
150                 .mbus_code      = MEDIA_BUS_FMT_RGB565_2X8_LE,
151                 .bpp            = 2,
152                 .planar         = false,
153         },
154         {
155                 .desc           = "Raw RGB Bayer",
156                 .pixelformat    = V4L2_PIX_FMT_SBGGR8,
157                 .mbus_code      = MEDIA_BUS_FMT_SBGGR8_1X8,
158                 .bpp            = 1,
159                 .planar         = false,
160         },
161 };
162 #define N_MCAM_FMTS ARRAY_SIZE(mcam_formats)
163
164 static struct mcam_format_struct *mcam_find_format(u32 pixelformat)
165 {
166         unsigned i;
167
168         for (i = 0; i < N_MCAM_FMTS; i++)
169                 if (mcam_formats[i].pixelformat == pixelformat)
170                         return mcam_formats + i;
171         /* Not found? Then return the first format. */
172         return mcam_formats;
173 }
174
175 /*
176  * The default format we use until somebody says otherwise.
177  */
178 static const struct v4l2_pix_format mcam_def_pix_format = {
179         .width          = VGA_WIDTH,
180         .height         = VGA_HEIGHT,
181         .pixelformat    = V4L2_PIX_FMT_YUYV,
182         .field          = V4L2_FIELD_NONE,
183         .bytesperline   = VGA_WIDTH*2,
184         .sizeimage      = VGA_WIDTH*VGA_HEIGHT*2,
185         .colorspace     = V4L2_COLORSPACE_SRGB,
186 };
187
188 static const u32 mcam_def_mbus_code = MEDIA_BUS_FMT_YUYV8_2X8;
189
190
191 /*
192  * The two-word DMA descriptor format used by the Armada 610 and like.  There
193  * Is a three-word format as well (set C1_DESC_3WORD) where the third
194  * word is a pointer to the next descriptor, but we don't use it.  Two-word
195  * descriptors have to be contiguous in memory.
196  */
197 struct mcam_dma_desc {
198         u32 dma_addr;
199         u32 segment_len;
200 };
201
202 /*
203  * Our buffer type for working with videobuf2.  Note that the vb2
204  * developers have decreed that struct vb2_buffer must be at the
205  * beginning of this structure.
206  */
207 struct mcam_vb_buffer {
208         struct vb2_buffer vb_buf;
209         struct list_head queue;
210         struct mcam_dma_desc *dma_desc; /* Descriptor virtual address */
211         dma_addr_t dma_desc_pa;         /* Descriptor physical address */
212         int dma_desc_nent;              /* Number of mapped descriptors */
213 };
214
215 static inline struct mcam_vb_buffer *vb_to_mvb(struct vb2_buffer *vb)
216 {
217         return container_of(vb, struct mcam_vb_buffer, vb_buf);
218 }
219
220 /*
221  * Hand a completed buffer back to user space.
222  */
223 static void mcam_buffer_done(struct mcam_camera *cam, int frame,
224                 struct vb2_buffer *vbuf)
225 {
226         vbuf->v4l2_buf.bytesused = cam->pix_format.sizeimage;
227         vbuf->v4l2_buf.sequence = cam->buf_seq[frame];
228         vbuf->v4l2_buf.field = V4L2_FIELD_NONE;
229         v4l2_get_timestamp(&vbuf->v4l2_buf.timestamp);
230         vb2_set_plane_payload(vbuf, 0, cam->pix_format.sizeimage);
231         vb2_buffer_done(vbuf, VB2_BUF_STATE_DONE);
232 }
233
234
235
236 /*
237  * Debugging and related.
238  */
239 #define cam_err(cam, fmt, arg...) \
240         dev_err((cam)->dev, fmt, ##arg);
241 #define cam_warn(cam, fmt, arg...) \
242         dev_warn((cam)->dev, fmt, ##arg);
243 #define cam_dbg(cam, fmt, arg...) \
244         dev_dbg((cam)->dev, fmt, ##arg);
245
246
247 /*
248  * Flag manipulation helpers
249  */
250 static void mcam_reset_buffers(struct mcam_camera *cam)
251 {
252         int i;
253
254         cam->next_buf = -1;
255         for (i = 0; i < cam->nbufs; i++) {
256                 clear_bit(i, &cam->flags);
257                 clear_bit(CF_FRAME_SOF0 + i, &cam->flags);
258         }
259 }
260
261 static inline int mcam_needs_config(struct mcam_camera *cam)
262 {
263         return test_bit(CF_CONFIG_NEEDED, &cam->flags);
264 }
265
266 static void mcam_set_config_needed(struct mcam_camera *cam, int needed)
267 {
268         if (needed)
269                 set_bit(CF_CONFIG_NEEDED, &cam->flags);
270         else
271                 clear_bit(CF_CONFIG_NEEDED, &cam->flags);
272 }
273
274 /* ------------------------------------------------------------------- */
275 /*
276  * Make the controller start grabbing images.  Everything must
277  * be set up before doing this.
278  */
279 static void mcam_ctlr_start(struct mcam_camera *cam)
280 {
281         /* set_bit performs a read, so no other barrier should be
282            needed here */
283         mcam_reg_set_bit(cam, REG_CTRL0, C0_ENABLE);
284 }
285
286 static void mcam_ctlr_stop(struct mcam_camera *cam)
287 {
288         mcam_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
289 }
290
291 static void mcam_enable_mipi(struct mcam_camera *mcam)
292 {
293         /* Using MIPI mode and enable MIPI */
294         cam_dbg(mcam, "camera: DPHY3=0x%x, DPHY5=0x%x, DPHY6=0x%x\n",
295                         mcam->dphy[0], mcam->dphy[1], mcam->dphy[2]);
296         mcam_reg_write(mcam, REG_CSI2_DPHY3, mcam->dphy[0]);
297         mcam_reg_write(mcam, REG_CSI2_DPHY5, mcam->dphy[1]);
298         mcam_reg_write(mcam, REG_CSI2_DPHY6, mcam->dphy[2]);
299
300         if (!mcam->mipi_enabled) {
301                 if (mcam->lane > 4 || mcam->lane <= 0) {
302                         cam_warn(mcam, "lane number error\n");
303                         mcam->lane = 1; /* set the default value */
304                 }
305                 /*
306                  * 0x41 actives 1 lane
307                  * 0x43 actives 2 lanes
308                  * 0x45 actives 3 lanes (never happen)
309                  * 0x47 actives 4 lanes
310                  */
311                 mcam_reg_write(mcam, REG_CSI2_CTRL0,
312                         CSI2_C0_MIPI_EN | CSI2_C0_ACT_LANE(mcam->lane));
313                 mcam_reg_write(mcam, REG_CLKCTRL,
314                         (mcam->mclk_src << 29) | mcam->mclk_div);
315
316                 mcam->mipi_enabled = true;
317         }
318 }
319
320 static void mcam_disable_mipi(struct mcam_camera *mcam)
321 {
322         /* Using Parallel mode or disable MIPI */
323         mcam_reg_write(mcam, REG_CSI2_CTRL0, 0x0);
324         mcam_reg_write(mcam, REG_CSI2_DPHY3, 0x0);
325         mcam_reg_write(mcam, REG_CSI2_DPHY5, 0x0);
326         mcam_reg_write(mcam, REG_CSI2_DPHY6, 0x0);
327         mcam->mipi_enabled = false;
328 }
329
330 static bool mcam_fmt_is_planar(__u32 pfmt)
331 {
332         struct mcam_format_struct *f;
333
334         f = mcam_find_format(pfmt);
335         return f->planar;
336 }
337
338 static void mcam_write_yuv_bases(struct mcam_camera *cam,
339                                  unsigned frame, dma_addr_t base)
340 {
341         struct v4l2_pix_format *fmt = &cam->pix_format;
342         u32 pixel_count = fmt->width * fmt->height;
343         dma_addr_t y, u = 0, v = 0;
344
345         y = base;
346
347         switch (fmt->pixelformat) {
348         case V4L2_PIX_FMT_YUV420:
349                 u = y + pixel_count;
350                 v = u + pixel_count / 4;
351                 break;
352         case V4L2_PIX_FMT_YVU420:
353                 v = y + pixel_count;
354                 u = v + pixel_count / 4;
355                 break;
356         default:
357                 break;
358         }
359
360         mcam_reg_write(cam, REG_Y0BAR + frame * 4, y);
361         if (mcam_fmt_is_planar(fmt->pixelformat)) {
362                 mcam_reg_write(cam, REG_U0BAR + frame * 4, u);
363                 mcam_reg_write(cam, REG_V0BAR + frame * 4, v);
364         }
365 }
366
367 /* ------------------------------------------------------------------- */
368
369 #ifdef MCAM_MODE_VMALLOC
370 /*
371  * Code specific to the vmalloc buffer mode.
372  */
373
374 /*
375  * Allocate in-kernel DMA buffers for vmalloc mode.
376  */
377 static int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
378 {
379         int i;
380
381         mcam_set_config_needed(cam, 1);
382         if (loadtime)
383                 cam->dma_buf_size = dma_buf_size;
384         else
385                 cam->dma_buf_size = cam->pix_format.sizeimage;
386         if (n_dma_bufs > 3)
387                 n_dma_bufs = 3;
388
389         cam->nbufs = 0;
390         for (i = 0; i < n_dma_bufs; i++) {
391                 cam->dma_bufs[i] = dma_alloc_coherent(cam->dev,
392                                 cam->dma_buf_size, cam->dma_handles + i,
393                                 GFP_KERNEL);
394                 if (cam->dma_bufs[i] == NULL) {
395                         cam_warn(cam, "Failed to allocate DMA buffer\n");
396                         break;
397                 }
398                 (cam->nbufs)++;
399         }
400
401         switch (cam->nbufs) {
402         case 1:
403                 dma_free_coherent(cam->dev, cam->dma_buf_size,
404                                 cam->dma_bufs[0], cam->dma_handles[0]);
405                 cam->nbufs = 0;
406         case 0:
407                 cam_err(cam, "Insufficient DMA buffers, cannot operate\n");
408                 return -ENOMEM;
409
410         case 2:
411                 if (n_dma_bufs > 2)
412                         cam_warn(cam, "Will limp along with only 2 buffers\n");
413                 break;
414         }
415         return 0;
416 }
417
418 static void mcam_free_dma_bufs(struct mcam_camera *cam)
419 {
420         int i;
421
422         for (i = 0; i < cam->nbufs; i++) {
423                 dma_free_coherent(cam->dev, cam->dma_buf_size,
424                                 cam->dma_bufs[i], cam->dma_handles[i]);
425                 cam->dma_bufs[i] = NULL;
426         }
427         cam->nbufs = 0;
428 }
429
430
431 /*
432  * Set up DMA buffers when operating in vmalloc mode
433  */
434 static void mcam_ctlr_dma_vmalloc(struct mcam_camera *cam)
435 {
436         /*
437          * Store the first two YUV buffers. Then either
438          * set the third if it exists, or tell the controller
439          * to just use two.
440          */
441         mcam_write_yuv_bases(cam, 0, cam->dma_handles[0]);
442         mcam_write_yuv_bases(cam, 1, cam->dma_handles[1]);
443         if (cam->nbufs > 2) {
444                 mcam_write_yuv_bases(cam, 2, cam->dma_handles[2]);
445                 mcam_reg_clear_bit(cam, REG_CTRL1, C1_TWOBUFS);
446         } else
447                 mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
448         if (cam->chip_id == MCAM_CAFE)
449                 mcam_reg_write(cam, REG_UBAR, 0); /* 32 bits only */
450 }
451
452 /*
453  * Copy data out to user space in the vmalloc case
454  */
455 static void mcam_frame_tasklet(unsigned long data)
456 {
457         struct mcam_camera *cam = (struct mcam_camera *) data;
458         int i;
459         unsigned long flags;
460         struct mcam_vb_buffer *buf;
461
462         spin_lock_irqsave(&cam->dev_lock, flags);
463         for (i = 0; i < cam->nbufs; i++) {
464                 int bufno = cam->next_buf;
465
466                 if (cam->state != S_STREAMING || bufno < 0)
467                         break;  /* I/O got stopped */
468                 if (++(cam->next_buf) >= cam->nbufs)
469                         cam->next_buf = 0;
470                 if (!test_bit(bufno, &cam->flags))
471                         continue;
472                 if (list_empty(&cam->buffers)) {
473                         cam->frame_state.singles++;
474                         break;  /* Leave it valid, hope for better later */
475                 }
476                 cam->frame_state.delivered++;
477                 clear_bit(bufno, &cam->flags);
478                 buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer,
479                                 queue);
480                 list_del_init(&buf->queue);
481                 /*
482                  * Drop the lock during the big copy.  This *should* be safe...
483                  */
484                 spin_unlock_irqrestore(&cam->dev_lock, flags);
485                 memcpy(vb2_plane_vaddr(&buf->vb_buf, 0), cam->dma_bufs[bufno],
486                                 cam->pix_format.sizeimage);
487                 mcam_buffer_done(cam, bufno, &buf->vb_buf);
488                 spin_lock_irqsave(&cam->dev_lock, flags);
489         }
490         spin_unlock_irqrestore(&cam->dev_lock, flags);
491 }
492
493
494 /*
495  * Make sure our allocated buffers are up to the task.
496  */
497 static int mcam_check_dma_buffers(struct mcam_camera *cam)
498 {
499         if (cam->nbufs > 0 && cam->dma_buf_size < cam->pix_format.sizeimage)
500                         mcam_free_dma_bufs(cam);
501         if (cam->nbufs == 0)
502                 return mcam_alloc_dma_bufs(cam, 0);
503         return 0;
504 }
505
506 static void mcam_vmalloc_done(struct mcam_camera *cam, int frame)
507 {
508         tasklet_schedule(&cam->s_tasklet);
509 }
510
511 #else /* MCAM_MODE_VMALLOC */
512
513 static inline int mcam_alloc_dma_bufs(struct mcam_camera *cam, int loadtime)
514 {
515         return 0;
516 }
517
518 static inline void mcam_free_dma_bufs(struct mcam_camera *cam)
519 {
520         return;
521 }
522
523 static inline int mcam_check_dma_buffers(struct mcam_camera *cam)
524 {
525         return 0;
526 }
527
528
529
530 #endif /* MCAM_MODE_VMALLOC */
531
532
533 #ifdef MCAM_MODE_DMA_CONTIG
534 /* ---------------------------------------------------------------------- */
535 /*
536  * DMA-contiguous code.
537  */
538
539 /*
540  * Set up a contiguous buffer for the given frame.  Here also is where
541  * the underrun strategy is set: if there is no buffer available, reuse
542  * the buffer from the other BAR and set the CF_SINGLE_BUFFER flag to
543  * keep the interrupt handler from giving that buffer back to user
544  * space.  In this way, we always have a buffer to DMA to and don't
545  * have to try to play games stopping and restarting the controller.
546  */
547 static void mcam_set_contig_buffer(struct mcam_camera *cam, int frame)
548 {
549         struct mcam_vb_buffer *buf;
550         dma_addr_t dma_handle;
551         struct vb2_buffer *vb;
552
553         /*
554          * If there are no available buffers, go into single mode
555          */
556         if (list_empty(&cam->buffers)) {
557                 buf = cam->vb_bufs[frame ^ 0x1];
558                 set_bit(CF_SINGLE_BUFFER, &cam->flags);
559                 cam->frame_state.singles++;
560         } else {
561                 /*
562                  * OK, we have a buffer we can use.
563                  */
564                 buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer,
565                                         queue);
566                 list_del_init(&buf->queue);
567                 clear_bit(CF_SINGLE_BUFFER, &cam->flags);
568         }
569
570         cam->vb_bufs[frame] = buf;
571         vb = &buf->vb_buf;
572
573         dma_handle = vb2_dma_contig_plane_dma_addr(vb, 0);
574         mcam_write_yuv_bases(cam, frame, dma_handle);
575 }
576
577 /*
578  * Initial B_DMA_contig setup.
579  */
580 static void mcam_ctlr_dma_contig(struct mcam_camera *cam)
581 {
582         mcam_reg_set_bit(cam, REG_CTRL1, C1_TWOBUFS);
583         cam->nbufs = 2;
584         mcam_set_contig_buffer(cam, 0);
585         mcam_set_contig_buffer(cam, 1);
586 }
587
588 /*
589  * Frame completion handling.
590  */
591 static void mcam_dma_contig_done(struct mcam_camera *cam, int frame)
592 {
593         struct mcam_vb_buffer *buf = cam->vb_bufs[frame];
594
595         if (!test_bit(CF_SINGLE_BUFFER, &cam->flags)) {
596                 cam->frame_state.delivered++;
597                 cam->vb_bufs[frame] = NULL;
598                 mcam_buffer_done(cam, frame, &buf->vb_buf);
599         }
600         mcam_set_contig_buffer(cam, frame);
601 }
602
603 #endif /* MCAM_MODE_DMA_CONTIG */
604
605 #ifdef MCAM_MODE_DMA_SG
606 /* ---------------------------------------------------------------------- */
607 /*
608  * Scatter/gather-specific code.
609  */
610
611 /*
612  * Set up the next buffer for S/G I/O; caller should be sure that
613  * the controller is stopped and a buffer is available.
614  */
615 static void mcam_sg_next_buffer(struct mcam_camera *cam)
616 {
617         struct mcam_vb_buffer *buf;
618
619         buf = list_first_entry(&cam->buffers, struct mcam_vb_buffer, queue);
620         list_del_init(&buf->queue);
621         /*
622          * Very Bad Not Good Things happen if you don't clear
623          * C1_DESC_ENA before making any descriptor changes.
624          */
625         mcam_reg_clear_bit(cam, REG_CTRL1, C1_DESC_ENA);
626         mcam_reg_write(cam, REG_DMA_DESC_Y, buf->dma_desc_pa);
627         mcam_reg_write(cam, REG_DESC_LEN_Y,
628                         buf->dma_desc_nent*sizeof(struct mcam_dma_desc));
629         mcam_reg_write(cam, REG_DESC_LEN_U, 0);
630         mcam_reg_write(cam, REG_DESC_LEN_V, 0);
631         mcam_reg_set_bit(cam, REG_CTRL1, C1_DESC_ENA);
632         cam->vb_bufs[0] = buf;
633 }
634
635 /*
636  * Initial B_DMA_sg setup
637  */
638 static void mcam_ctlr_dma_sg(struct mcam_camera *cam)
639 {
640         /*
641          * The list-empty condition can hit us at resume time
642          * if the buffer list was empty when the system was suspended.
643          */
644         if (list_empty(&cam->buffers)) {
645                 set_bit(CF_SG_RESTART, &cam->flags);
646                 return;
647         }
648
649         mcam_reg_clear_bit(cam, REG_CTRL1, C1_DESC_3WORD);
650         mcam_sg_next_buffer(cam);
651         cam->nbufs = 3;
652 }
653
654
655 /*
656  * Frame completion with S/G is trickier.  We can't muck with
657  * a descriptor chain on the fly, since the controller buffers it
658  * internally.  So we have to actually stop and restart; Marvell
659  * says this is the way to do it.
660  *
661  * Of course, stopping is easier said than done; experience shows
662  * that the controller can start a frame *after* C0_ENABLE has been
663  * cleared.  So when running in S/G mode, the controller is "stopped"
664  * on receipt of the start-of-frame interrupt.  That means we can
665  * safely change the DMA descriptor array here and restart things
666  * (assuming there's another buffer waiting to go).
667  */
668 static void mcam_dma_sg_done(struct mcam_camera *cam, int frame)
669 {
670         struct mcam_vb_buffer *buf = cam->vb_bufs[0];
671
672         /*
673          * If we're no longer supposed to be streaming, don't do anything.
674          */
675         if (cam->state != S_STREAMING)
676                 return;
677         /*
678          * If we have another buffer available, put it in and
679          * restart the engine.
680          */
681         if (!list_empty(&cam->buffers)) {
682                 mcam_sg_next_buffer(cam);
683                 mcam_ctlr_start(cam);
684         /*
685          * Otherwise set CF_SG_RESTART and the controller will
686          * be restarted once another buffer shows up.
687          */
688         } else {
689                 set_bit(CF_SG_RESTART, &cam->flags);
690                 cam->frame_state.singles++;
691                 cam->vb_bufs[0] = NULL;
692         }
693         /*
694          * Now we can give the completed frame back to user space.
695          */
696         cam->frame_state.delivered++;
697         mcam_buffer_done(cam, frame, &buf->vb_buf);
698 }
699
700
701 /*
702  * Scatter/gather mode requires stopping the controller between
703  * frames so we can put in a new DMA descriptor array.  If no new
704  * buffer exists at frame completion, the controller is left stopped;
705  * this function is charged with gettig things going again.
706  */
707 static void mcam_sg_restart(struct mcam_camera *cam)
708 {
709         mcam_ctlr_dma_sg(cam);
710         mcam_ctlr_start(cam);
711         clear_bit(CF_SG_RESTART, &cam->flags);
712 }
713
714 #else /* MCAM_MODE_DMA_SG */
715
716 static inline void mcam_sg_restart(struct mcam_camera *cam)
717 {
718         return;
719 }
720
721 #endif /* MCAM_MODE_DMA_SG */
722
723 /* ---------------------------------------------------------------------- */
724 /*
725  * Buffer-mode-independent controller code.
726  */
727
728 /*
729  * Image format setup
730  */
731 static void mcam_ctlr_image(struct mcam_camera *cam)
732 {
733         struct v4l2_pix_format *fmt = &cam->pix_format;
734         u32 widthy = 0, widthuv = 0, imgsz_h, imgsz_w;
735
736         cam_dbg(cam, "camera: bytesperline = %d; height = %d\n",
737                 fmt->bytesperline, fmt->sizeimage / fmt->bytesperline);
738         imgsz_h = (fmt->height << IMGSZ_V_SHIFT) & IMGSZ_V_MASK;
739         imgsz_w = (fmt->width * 2) & IMGSZ_H_MASK;
740
741         switch (fmt->pixelformat) {
742         case V4L2_PIX_FMT_YUYV:
743         case V4L2_PIX_FMT_YVYU:
744                 widthy = fmt->width * 2;
745                 widthuv = 0;
746                 break;
747         case V4L2_PIX_FMT_YUV420:
748         case V4L2_PIX_FMT_YVU420:
749                 widthy = fmt->width;
750                 widthuv = fmt->width / 2;
751                 break;
752         default:
753                 widthy = fmt->bytesperline;
754                 widthuv = 0;
755                 break;
756         }
757
758         mcam_reg_write_mask(cam, REG_IMGPITCH, widthuv << 16 | widthy,
759                         IMGP_YP_MASK | IMGP_UVP_MASK);
760         mcam_reg_write(cam, REG_IMGSIZE, imgsz_h | imgsz_w);
761         mcam_reg_write(cam, REG_IMGOFFSET, 0x0);
762
763         /*
764          * Tell the controller about the image format we are using.
765          */
766         switch (fmt->pixelformat) {
767         case V4L2_PIX_FMT_YUV420:
768         case V4L2_PIX_FMT_YVU420:
769                 mcam_reg_write_mask(cam, REG_CTRL0,
770                         C0_DF_YUV | C0_YUV_420PL | C0_YUVE_VYUY, C0_DF_MASK);
771                 break;
772         case V4L2_PIX_FMT_YUYV:
773                 mcam_reg_write_mask(cam, REG_CTRL0,
774                         C0_DF_YUV | C0_YUV_PACKED | C0_YUVE_NOSWAP, C0_DF_MASK);
775                 break;
776         case V4L2_PIX_FMT_YVYU:
777                 mcam_reg_write_mask(cam, REG_CTRL0,
778                         C0_DF_YUV | C0_YUV_PACKED | C0_YUVE_SWAP24, C0_DF_MASK);
779                 break;
780         case V4L2_PIX_FMT_XRGB444:
781                 mcam_reg_write_mask(cam, REG_CTRL0,
782                         C0_DF_RGB | C0_RGBF_444 | C0_RGB4_XBGR, C0_DF_MASK);
783                 break;
784         case V4L2_PIX_FMT_RGB565:
785                 mcam_reg_write_mask(cam, REG_CTRL0,
786                         C0_DF_RGB | C0_RGBF_565 | C0_RGB5_BGGR, C0_DF_MASK);
787                 break;
788         case V4L2_PIX_FMT_SBGGR8:
789                 mcam_reg_write_mask(cam, REG_CTRL0,
790                         C0_DF_RGB | C0_RGB5_GRBG, C0_DF_MASK);
791                 break;
792         default:
793                 cam_err(cam, "camera: unknown format: %#x\n", fmt->pixelformat);
794                 break;
795         }
796
797         /*
798          * Make sure it knows we want to use hsync/vsync.
799          */
800         mcam_reg_write_mask(cam, REG_CTRL0, C0_SIF_HVSYNC, C0_SIFM_MASK);
801         /*
802          * This field controls the generation of EOF(DVP only)
803          */
804         if (cam->bus_type != V4L2_MBUS_CSI2)
805                 mcam_reg_set_bit(cam, REG_CTRL0,
806                                 C0_EOF_VSYNC | C0_VEDGE_CTRL);
807 }
808
809
810 /*
811  * Configure the controller for operation; caller holds the
812  * device mutex.
813  */
814 static int mcam_ctlr_configure(struct mcam_camera *cam)
815 {
816         unsigned long flags;
817
818         spin_lock_irqsave(&cam->dev_lock, flags);
819         clear_bit(CF_SG_RESTART, &cam->flags);
820         cam->dma_setup(cam);
821         mcam_ctlr_image(cam);
822         mcam_set_config_needed(cam, 0);
823         spin_unlock_irqrestore(&cam->dev_lock, flags);
824         return 0;
825 }
826
827 static void mcam_ctlr_irq_enable(struct mcam_camera *cam)
828 {
829         /*
830          * Clear any pending interrupts, since we do not
831          * expect to have I/O active prior to enabling.
832          */
833         mcam_reg_write(cam, REG_IRQSTAT, FRAMEIRQS);
834         mcam_reg_set_bit(cam, REG_IRQMASK, FRAMEIRQS);
835 }
836
837 static void mcam_ctlr_irq_disable(struct mcam_camera *cam)
838 {
839         mcam_reg_clear_bit(cam, REG_IRQMASK, FRAMEIRQS);
840 }
841
842
843
844 static void mcam_ctlr_init(struct mcam_camera *cam)
845 {
846         unsigned long flags;
847
848         spin_lock_irqsave(&cam->dev_lock, flags);
849         /*
850          * Make sure it's not powered down.
851          */
852         mcam_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
853         /*
854          * Turn off the enable bit.  It sure should be off anyway,
855          * but it's good to be sure.
856          */
857         mcam_reg_clear_bit(cam, REG_CTRL0, C0_ENABLE);
858         /*
859          * Clock the sensor appropriately.  Controller clock should
860          * be 48MHz, sensor "typical" value is half that.
861          */
862         mcam_reg_write_mask(cam, REG_CLKCTRL, 2, CLK_DIV_MASK);
863         spin_unlock_irqrestore(&cam->dev_lock, flags);
864 }
865
866
867 /*
868  * Stop the controller, and don't return until we're really sure that no
869  * further DMA is going on.
870  */
871 static void mcam_ctlr_stop_dma(struct mcam_camera *cam)
872 {
873         unsigned long flags;
874
875         /*
876          * Theory: stop the camera controller (whether it is operating
877          * or not).  Delay briefly just in case we race with the SOF
878          * interrupt, then wait until no DMA is active.
879          */
880         spin_lock_irqsave(&cam->dev_lock, flags);
881         clear_bit(CF_SG_RESTART, &cam->flags);
882         mcam_ctlr_stop(cam);
883         cam->state = S_IDLE;
884         spin_unlock_irqrestore(&cam->dev_lock, flags);
885         /*
886          * This is a brutally long sleep, but experience shows that
887          * it can take the controller a while to get the message that
888          * it needs to stop grabbing frames.  In particular, we can
889          * sometimes (on mmp) get a frame at the end WITHOUT the
890          * start-of-frame indication.
891          */
892         msleep(150);
893         if (test_bit(CF_DMA_ACTIVE, &cam->flags))
894                 cam_err(cam, "Timeout waiting for DMA to end\n");
895                 /* This would be bad news - what now? */
896         spin_lock_irqsave(&cam->dev_lock, flags);
897         mcam_ctlr_irq_disable(cam);
898         spin_unlock_irqrestore(&cam->dev_lock, flags);
899 }
900
901 /*
902  * Power up and down.
903  */
904 static int mcam_ctlr_power_up(struct mcam_camera *cam)
905 {
906         unsigned long flags;
907         int ret;
908
909         spin_lock_irqsave(&cam->dev_lock, flags);
910         ret = cam->plat_power_up(cam);
911         if (ret) {
912                 spin_unlock_irqrestore(&cam->dev_lock, flags);
913                 return ret;
914         }
915         mcam_reg_clear_bit(cam, REG_CTRL1, C1_PWRDWN);
916         spin_unlock_irqrestore(&cam->dev_lock, flags);
917         msleep(5); /* Just to be sure */
918         return 0;
919 }
920
921 static void mcam_ctlr_power_down(struct mcam_camera *cam)
922 {
923         unsigned long flags;
924
925         spin_lock_irqsave(&cam->dev_lock, flags);
926         /*
927          * School of hard knocks department: be sure we do any register
928          * twiddling on the controller *before* calling the platform
929          * power down routine.
930          */
931         mcam_reg_set_bit(cam, REG_CTRL1, C1_PWRDWN);
932         cam->plat_power_down(cam);
933         spin_unlock_irqrestore(&cam->dev_lock, flags);
934 }
935
936 /* -------------------------------------------------------------------- */
937 /*
938  * Communications with the sensor.
939  */
940
941 static int __mcam_cam_reset(struct mcam_camera *cam)
942 {
943         return sensor_call(cam, core, reset, 0);
944 }
945
946 /*
947  * We have found the sensor on the i2c.  Let's try to have a
948  * conversation.
949  */
950 static int mcam_cam_init(struct mcam_camera *cam)
951 {
952         int ret;
953
954         if (cam->state != S_NOTREADY)
955                 cam_warn(cam, "Cam init with device in funky state %d",
956                                 cam->state);
957         ret = __mcam_cam_reset(cam);
958         /* Get/set parameters? */
959         cam->state = S_IDLE;
960         mcam_ctlr_power_down(cam);
961         return ret;
962 }
963
964 /*
965  * Configure the sensor to match the parameters we have.  Caller should
966  * hold s_mutex
967  */
968 static int mcam_cam_set_flip(struct mcam_camera *cam)
969 {
970         struct v4l2_control ctrl;
971
972         memset(&ctrl, 0, sizeof(ctrl));
973         ctrl.id = V4L2_CID_VFLIP;
974         ctrl.value = flip;
975         return sensor_call(cam, core, s_ctrl, &ctrl);
976 }
977
978
979 static int mcam_cam_configure(struct mcam_camera *cam)
980 {
981         struct v4l2_subdev_format format = {
982                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
983         };
984         int ret;
985
986         v4l2_fill_mbus_format(&format.format, &cam->pix_format, cam->mbus_code);
987         ret = sensor_call(cam, core, init, 0);
988         if (ret == 0)
989                 ret = sensor_call(cam, pad, set_fmt, NULL, &format);
990         /*
991          * OV7670 does weird things if flip is set *before* format...
992          */
993         ret += mcam_cam_set_flip(cam);
994         return ret;
995 }
996
997 /*
998  * Get everything ready, and start grabbing frames.
999  */
1000 static int mcam_read_setup(struct mcam_camera *cam)
1001 {
1002         int ret;
1003         unsigned long flags;
1004
1005         /*
1006          * Configuration.  If we still don't have DMA buffers,
1007          * make one last, desperate attempt.
1008          */
1009         if (cam->buffer_mode == B_vmalloc && cam->nbufs == 0 &&
1010                         mcam_alloc_dma_bufs(cam, 0))
1011                 return -ENOMEM;
1012
1013         if (mcam_needs_config(cam)) {
1014                 mcam_cam_configure(cam);
1015                 ret = mcam_ctlr_configure(cam);
1016                 if (ret)
1017                         return ret;
1018         }
1019
1020         /*
1021          * Turn it loose.
1022          */
1023         spin_lock_irqsave(&cam->dev_lock, flags);
1024         clear_bit(CF_DMA_ACTIVE, &cam->flags);
1025         mcam_reset_buffers(cam);
1026         /*
1027          * Update CSI2_DPHY value
1028          */
1029         if (cam->calc_dphy)
1030                 cam->calc_dphy(cam);
1031         cam_dbg(cam, "camera: DPHY sets: dphy3=0x%x, dphy5=0x%x, dphy6=0x%x\n",
1032                         cam->dphy[0], cam->dphy[1], cam->dphy[2]);
1033         if (cam->bus_type == V4L2_MBUS_CSI2)
1034                 mcam_enable_mipi(cam);
1035         else
1036                 mcam_disable_mipi(cam);
1037         mcam_ctlr_irq_enable(cam);
1038         cam->state = S_STREAMING;
1039         if (!test_bit(CF_SG_RESTART, &cam->flags))
1040                 mcam_ctlr_start(cam);
1041         spin_unlock_irqrestore(&cam->dev_lock, flags);
1042         return 0;
1043 }
1044
1045 /* ----------------------------------------------------------------------- */
1046 /*
1047  * Videobuf2 interface code.
1048  */
1049
1050 static int mcam_vb_queue_setup(struct vb2_queue *vq,
1051                 const struct v4l2_format *fmt, unsigned int *nbufs,
1052                 unsigned int *num_planes, unsigned int sizes[],
1053                 void *alloc_ctxs[])
1054 {
1055         struct mcam_camera *cam = vb2_get_drv_priv(vq);
1056         int minbufs = (cam->buffer_mode == B_DMA_contig) ? 3 : 2;
1057
1058         if (fmt && fmt->fmt.pix.sizeimage < cam->pix_format.sizeimage)
1059                 return -EINVAL;
1060         sizes[0] = fmt ? fmt->fmt.pix.sizeimage : cam->pix_format.sizeimage;
1061         *num_planes = 1; /* Someday we have to support planar formats... */
1062         if (*nbufs < minbufs)
1063                 *nbufs = minbufs;
1064         if (cam->buffer_mode == B_DMA_contig)
1065                 alloc_ctxs[0] = cam->vb_alloc_ctx;
1066         else if (cam->buffer_mode == B_DMA_sg)
1067                 alloc_ctxs[0] = cam->vb_alloc_ctx_sg;
1068         return 0;
1069 }
1070
1071
1072 static void mcam_vb_buf_queue(struct vb2_buffer *vb)
1073 {
1074         struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
1075         struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
1076         unsigned long flags;
1077         int start;
1078
1079         spin_lock_irqsave(&cam->dev_lock, flags);
1080         start = (cam->state == S_BUFWAIT) && !list_empty(&cam->buffers);
1081         list_add(&mvb->queue, &cam->buffers);
1082         if (cam->state == S_STREAMING && test_bit(CF_SG_RESTART, &cam->flags))
1083                 mcam_sg_restart(cam);
1084         spin_unlock_irqrestore(&cam->dev_lock, flags);
1085         if (start)
1086                 mcam_read_setup(cam);
1087 }
1088
1089 static void mcam_vb_requeue_bufs(struct vb2_queue *vq,
1090                                  enum vb2_buffer_state state)
1091 {
1092         struct mcam_camera *cam = vb2_get_drv_priv(vq);
1093         struct mcam_vb_buffer *buf, *node;
1094         unsigned long flags;
1095         unsigned i;
1096
1097         spin_lock_irqsave(&cam->dev_lock, flags);
1098         list_for_each_entry_safe(buf, node, &cam->buffers, queue) {
1099                 vb2_buffer_done(&buf->vb_buf, state);
1100                 list_del(&buf->queue);
1101         }
1102         for (i = 0; i < MAX_DMA_BUFS; i++) {
1103                 buf = cam->vb_bufs[i];
1104
1105                 if (buf) {
1106                         vb2_buffer_done(&buf->vb_buf, state);
1107                         cam->vb_bufs[i] = NULL;
1108                 }
1109         }
1110         spin_unlock_irqrestore(&cam->dev_lock, flags);
1111 }
1112
1113 /*
1114  * These need to be called with the mutex held from vb2
1115  */
1116 static int mcam_vb_start_streaming(struct vb2_queue *vq, unsigned int count)
1117 {
1118         struct mcam_camera *cam = vb2_get_drv_priv(vq);
1119         unsigned int frame;
1120         int ret;
1121
1122         if (cam->state != S_IDLE) {
1123                 mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_QUEUED);
1124                 return -EINVAL;
1125         }
1126         cam->frame_state.frames = 0;
1127         cam->frame_state.singles = 0;
1128         cam->frame_state.delivered = 0;
1129         cam->sequence = 0;
1130         /*
1131          * Videobuf2 sneakily hoards all the buffers and won't
1132          * give them to us until *after* streaming starts.  But
1133          * we can't actually start streaming until we have a
1134          * destination.  So go into a wait state and hope they
1135          * give us buffers soon.
1136          */
1137         if (cam->buffer_mode != B_vmalloc && list_empty(&cam->buffers)) {
1138                 cam->state = S_BUFWAIT;
1139                 return 0;
1140         }
1141
1142         /*
1143          * Ensure clear the left over frame flags
1144          * before every really start streaming
1145          */
1146         for (frame = 0; frame < cam->nbufs; frame++)
1147                 clear_bit(CF_FRAME_SOF0 + frame, &cam->flags);
1148
1149         ret = mcam_read_setup(cam);
1150         if (ret)
1151                 mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_QUEUED);
1152         return ret;
1153 }
1154
1155 static void mcam_vb_stop_streaming(struct vb2_queue *vq)
1156 {
1157         struct mcam_camera *cam = vb2_get_drv_priv(vq);
1158
1159         cam_dbg(cam, "stop_streaming: %d frames, %d singles, %d delivered\n",
1160                         cam->frame_state.frames, cam->frame_state.singles,
1161                         cam->frame_state.delivered);
1162         if (cam->state == S_BUFWAIT) {
1163                 /* They never gave us buffers */
1164                 cam->state = S_IDLE;
1165                 return;
1166         }
1167         if (cam->state != S_STREAMING)
1168                 return;
1169         mcam_ctlr_stop_dma(cam);
1170         /*
1171          * Reset the CCIC PHY after stopping streaming,
1172          * otherwise, the CCIC may be unstable.
1173          */
1174         if (cam->ctlr_reset)
1175                 cam->ctlr_reset(cam);
1176         /*
1177          * VB2 reclaims the buffers, so we need to forget
1178          * about them.
1179          */
1180         mcam_vb_requeue_bufs(vq, VB2_BUF_STATE_ERROR);
1181 }
1182
1183
1184 static const struct vb2_ops mcam_vb2_ops = {
1185         .queue_setup            = mcam_vb_queue_setup,
1186         .buf_queue              = mcam_vb_buf_queue,
1187         .start_streaming        = mcam_vb_start_streaming,
1188         .stop_streaming         = mcam_vb_stop_streaming,
1189         .wait_prepare           = vb2_ops_wait_prepare,
1190         .wait_finish            = vb2_ops_wait_finish,
1191 };
1192
1193
1194 #ifdef MCAM_MODE_DMA_SG
1195 /*
1196  * Scatter/gather mode uses all of the above functions plus a
1197  * few extras to deal with DMA mapping.
1198  */
1199 static int mcam_vb_sg_buf_init(struct vb2_buffer *vb)
1200 {
1201         struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
1202         struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
1203         int ndesc = cam->pix_format.sizeimage/PAGE_SIZE + 1;
1204
1205         mvb->dma_desc = dma_alloc_coherent(cam->dev,
1206                         ndesc * sizeof(struct mcam_dma_desc),
1207                         &mvb->dma_desc_pa, GFP_KERNEL);
1208         if (mvb->dma_desc == NULL) {
1209                 cam_err(cam, "Unable to get DMA descriptor array\n");
1210                 return -ENOMEM;
1211         }
1212         return 0;
1213 }
1214
1215 static int mcam_vb_sg_buf_prepare(struct vb2_buffer *vb)
1216 {
1217         struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
1218         struct sg_table *sg_table = vb2_dma_sg_plane_desc(vb, 0);
1219         struct mcam_dma_desc *desc = mvb->dma_desc;
1220         struct scatterlist *sg;
1221         int i;
1222
1223         for_each_sg(sg_table->sgl, sg, sg_table->nents, i) {
1224                 desc->dma_addr = sg_dma_address(sg);
1225                 desc->segment_len = sg_dma_len(sg);
1226                 desc++;
1227         }
1228         return 0;
1229 }
1230
1231 static void mcam_vb_sg_buf_cleanup(struct vb2_buffer *vb)
1232 {
1233         struct mcam_camera *cam = vb2_get_drv_priv(vb->vb2_queue);
1234         struct mcam_vb_buffer *mvb = vb_to_mvb(vb);
1235         int ndesc = cam->pix_format.sizeimage/PAGE_SIZE + 1;
1236
1237         dma_free_coherent(cam->dev, ndesc * sizeof(struct mcam_dma_desc),
1238                         mvb->dma_desc, mvb->dma_desc_pa);
1239 }
1240
1241
1242 static const struct vb2_ops mcam_vb2_sg_ops = {
1243         .queue_setup            = mcam_vb_queue_setup,
1244         .buf_init               = mcam_vb_sg_buf_init,
1245         .buf_prepare            = mcam_vb_sg_buf_prepare,
1246         .buf_queue              = mcam_vb_buf_queue,
1247         .buf_cleanup            = mcam_vb_sg_buf_cleanup,
1248         .start_streaming        = mcam_vb_start_streaming,
1249         .stop_streaming         = mcam_vb_stop_streaming,
1250         .wait_prepare           = vb2_ops_wait_prepare,
1251         .wait_finish            = vb2_ops_wait_finish,
1252 };
1253
1254 #endif /* MCAM_MODE_DMA_SG */
1255
1256 static int mcam_setup_vb2(struct mcam_camera *cam)
1257 {
1258         struct vb2_queue *vq = &cam->vb_queue;
1259
1260         memset(vq, 0, sizeof(*vq));
1261         vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1262         vq->drv_priv = cam;
1263         vq->lock = &cam->s_mutex;
1264         vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1265         vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
1266         vq->buf_struct_size = sizeof(struct mcam_vb_buffer);
1267         INIT_LIST_HEAD(&cam->buffers);
1268         switch (cam->buffer_mode) {
1269         case B_DMA_contig:
1270 #ifdef MCAM_MODE_DMA_CONTIG
1271                 vq->ops = &mcam_vb2_ops;
1272                 vq->mem_ops = &vb2_dma_contig_memops;
1273                 cam->dma_setup = mcam_ctlr_dma_contig;
1274                 cam->frame_complete = mcam_dma_contig_done;
1275                 cam->vb_alloc_ctx = vb2_dma_contig_init_ctx(cam->dev);
1276                 if (IS_ERR(cam->vb_alloc_ctx))
1277                         return PTR_ERR(cam->vb_alloc_ctx);
1278 #endif
1279                 break;
1280         case B_DMA_sg:
1281 #ifdef MCAM_MODE_DMA_SG
1282                 vq->ops = &mcam_vb2_sg_ops;
1283                 vq->mem_ops = &vb2_dma_sg_memops;
1284                 cam->dma_setup = mcam_ctlr_dma_sg;
1285                 cam->frame_complete = mcam_dma_sg_done;
1286                 cam->vb_alloc_ctx_sg = vb2_dma_sg_init_ctx(cam->dev);
1287                 if (IS_ERR(cam->vb_alloc_ctx_sg))
1288                         return PTR_ERR(cam->vb_alloc_ctx_sg);
1289 #endif
1290                 break;
1291         case B_vmalloc:
1292 #ifdef MCAM_MODE_VMALLOC
1293                 tasklet_init(&cam->s_tasklet, mcam_frame_tasklet,
1294                                 (unsigned long) cam);
1295                 vq->ops = &mcam_vb2_ops;
1296                 vq->mem_ops = &vb2_vmalloc_memops;
1297                 cam->dma_setup = mcam_ctlr_dma_vmalloc;
1298                 cam->frame_complete = mcam_vmalloc_done;
1299 #endif
1300                 break;
1301         }
1302         return vb2_queue_init(vq);
1303 }
1304
1305 static void mcam_cleanup_vb2(struct mcam_camera *cam)
1306 {
1307 #ifdef MCAM_MODE_DMA_CONTIG
1308         if (cam->buffer_mode == B_DMA_contig)
1309                 vb2_dma_contig_cleanup_ctx(cam->vb_alloc_ctx);
1310 #endif
1311 #ifdef MCAM_MODE_DMA_SG
1312         if (cam->buffer_mode == B_DMA_sg)
1313                 vb2_dma_sg_cleanup_ctx(cam->vb_alloc_ctx_sg);
1314 #endif
1315 }
1316
1317
1318 /* ---------------------------------------------------------------------- */
1319 /*
1320  * The long list of V4L2 ioctl() operations.
1321  */
1322
1323 static int mcam_vidioc_querycap(struct file *file, void *priv,
1324                 struct v4l2_capability *cap)
1325 {
1326         struct mcam_camera *cam = video_drvdata(file);
1327
1328         strcpy(cap->driver, "marvell_ccic");
1329         strcpy(cap->card, "marvell_ccic");
1330         strlcpy(cap->bus_info, cam->bus_info, sizeof(cap->bus_info));
1331         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
1332                 V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
1333         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
1334         return 0;
1335 }
1336
1337
1338 static int mcam_vidioc_enum_fmt_vid_cap(struct file *filp,
1339                 void *priv, struct v4l2_fmtdesc *fmt)
1340 {
1341         if (fmt->index >= N_MCAM_FMTS)
1342                 return -EINVAL;
1343         strlcpy(fmt->description, mcam_formats[fmt->index].desc,
1344                         sizeof(fmt->description));
1345         fmt->pixelformat = mcam_formats[fmt->index].pixelformat;
1346         return 0;
1347 }
1348
1349 static int mcam_vidioc_try_fmt_vid_cap(struct file *filp, void *priv,
1350                 struct v4l2_format *fmt)
1351 {
1352         struct mcam_camera *cam = video_drvdata(filp);
1353         struct mcam_format_struct *f;
1354         struct v4l2_pix_format *pix = &fmt->fmt.pix;
1355         struct v4l2_subdev_pad_config pad_cfg;
1356         struct v4l2_subdev_format format = {
1357                 .which = V4L2_SUBDEV_FORMAT_TRY,
1358         };
1359         int ret;
1360
1361         f = mcam_find_format(pix->pixelformat);
1362         pix->pixelformat = f->pixelformat;
1363         v4l2_fill_mbus_format(&format.format, pix, f->mbus_code);
1364         ret = sensor_call(cam, pad, set_fmt, &pad_cfg, &format);
1365         v4l2_fill_pix_format(pix, &format.format);
1366         pix->bytesperline = pix->width * f->bpp;
1367         switch (f->pixelformat) {
1368         case V4L2_PIX_FMT_YUV420:
1369         case V4L2_PIX_FMT_YVU420:
1370                 pix->sizeimage = pix->height * pix->bytesperline * 3 / 2;
1371                 break;
1372         default:
1373                 pix->sizeimage = pix->height * pix->bytesperline;
1374                 break;
1375         }
1376         pix->colorspace = V4L2_COLORSPACE_SRGB;
1377         return ret;
1378 }
1379
1380 static int mcam_vidioc_s_fmt_vid_cap(struct file *filp, void *priv,
1381                 struct v4l2_format *fmt)
1382 {
1383         struct mcam_camera *cam = video_drvdata(filp);
1384         struct mcam_format_struct *f;
1385         int ret;
1386
1387         /*
1388          * Can't do anything if the device is not idle
1389          * Also can't if there are streaming buffers in place.
1390          */
1391         if (cam->state != S_IDLE || vb2_is_busy(&cam->vb_queue))
1392                 return -EBUSY;
1393
1394         f = mcam_find_format(fmt->fmt.pix.pixelformat);
1395
1396         /*
1397          * See if the formatting works in principle.
1398          */
1399         ret = mcam_vidioc_try_fmt_vid_cap(filp, priv, fmt);
1400         if (ret)
1401                 return ret;
1402         /*
1403          * Now we start to change things for real, so let's do it
1404          * under lock.
1405          */
1406         cam->pix_format = fmt->fmt.pix;
1407         cam->mbus_code = f->mbus_code;
1408
1409         /*
1410          * Make sure we have appropriate DMA buffers.
1411          */
1412         if (cam->buffer_mode == B_vmalloc) {
1413                 ret = mcam_check_dma_buffers(cam);
1414                 if (ret)
1415                         goto out;
1416         }
1417         mcam_set_config_needed(cam, 1);
1418 out:
1419         return ret;
1420 }
1421
1422 /*
1423  * Return our stored notion of how the camera is/should be configured.
1424  * The V4l2 spec wants us to be smarter, and actually get this from
1425  * the camera (and not mess with it at open time).  Someday.
1426  */
1427 static int mcam_vidioc_g_fmt_vid_cap(struct file *filp, void *priv,
1428                 struct v4l2_format *f)
1429 {
1430         struct mcam_camera *cam = video_drvdata(filp);
1431
1432         f->fmt.pix = cam->pix_format;
1433         return 0;
1434 }
1435
1436 /*
1437  * We only have one input - the sensor - so minimize the nonsense here.
1438  */
1439 static int mcam_vidioc_enum_input(struct file *filp, void *priv,
1440                 struct v4l2_input *input)
1441 {
1442         if (input->index != 0)
1443                 return -EINVAL;
1444
1445         input->type = V4L2_INPUT_TYPE_CAMERA;
1446         strcpy(input->name, "Camera");
1447         return 0;
1448 }
1449
1450 static int mcam_vidioc_g_input(struct file *filp, void *priv, unsigned int *i)
1451 {
1452         *i = 0;
1453         return 0;
1454 }
1455
1456 static int mcam_vidioc_s_input(struct file *filp, void *priv, unsigned int i)
1457 {
1458         if (i != 0)
1459                 return -EINVAL;
1460         return 0;
1461 }
1462
1463 /*
1464  * G/S_PARM.  Most of this is done by the sensor, but we are
1465  * the level which controls the number of read buffers.
1466  */
1467 static int mcam_vidioc_g_parm(struct file *filp, void *priv,
1468                 struct v4l2_streamparm *parms)
1469 {
1470         struct mcam_camera *cam = video_drvdata(filp);
1471         int ret;
1472
1473         ret = sensor_call(cam, video, g_parm, parms);
1474         parms->parm.capture.readbuffers = n_dma_bufs;
1475         return ret;
1476 }
1477
1478 static int mcam_vidioc_s_parm(struct file *filp, void *priv,
1479                 struct v4l2_streamparm *parms)
1480 {
1481         struct mcam_camera *cam = video_drvdata(filp);
1482         int ret;
1483
1484         ret = sensor_call(cam, video, s_parm, parms);
1485         parms->parm.capture.readbuffers = n_dma_bufs;
1486         return ret;
1487 }
1488
1489 static int mcam_vidioc_enum_framesizes(struct file *filp, void *priv,
1490                 struct v4l2_frmsizeenum *sizes)
1491 {
1492         struct mcam_camera *cam = video_drvdata(filp);
1493         struct mcam_format_struct *f;
1494         struct v4l2_subdev_frame_size_enum fse = {
1495                 .index = sizes->index,
1496                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1497         };
1498         int ret;
1499
1500         f = mcam_find_format(sizes->pixel_format);
1501         if (f->pixelformat != sizes->pixel_format)
1502                 return -EINVAL;
1503         fse.code = f->mbus_code;
1504         ret = sensor_call(cam, pad, enum_frame_size, NULL, &fse);
1505         if (ret)
1506                 return ret;
1507         if (fse.min_width == fse.max_width &&
1508             fse.min_height == fse.max_height) {
1509                 sizes->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1510                 sizes->discrete.width = fse.min_width;
1511                 sizes->discrete.height = fse.min_height;
1512                 return 0;
1513         }
1514         sizes->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
1515         sizes->stepwise.min_width = fse.min_width;
1516         sizes->stepwise.max_width = fse.max_width;
1517         sizes->stepwise.min_height = fse.min_height;
1518         sizes->stepwise.max_height = fse.max_height;
1519         sizes->stepwise.step_width = 1;
1520         sizes->stepwise.step_height = 1;
1521         return 0;
1522 }
1523
1524 static int mcam_vidioc_enum_frameintervals(struct file *filp, void *priv,
1525                 struct v4l2_frmivalenum *interval)
1526 {
1527         struct mcam_camera *cam = video_drvdata(filp);
1528         struct mcam_format_struct *f;
1529         struct v4l2_subdev_frame_interval_enum fie = {
1530                 .index = interval->index,
1531                 .width = interval->width,
1532                 .height = interval->height,
1533                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1534         };
1535         int ret;
1536
1537         f = mcam_find_format(interval->pixel_format);
1538         if (f->pixelformat != interval->pixel_format)
1539                 return -EINVAL;
1540         fie.code = f->mbus_code;
1541         ret = sensor_call(cam, pad, enum_frame_interval, NULL, &fie);
1542         if (ret)
1543                 return ret;
1544         interval->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1545         interval->discrete = fie.interval;
1546         return 0;
1547 }
1548
1549 #ifdef CONFIG_VIDEO_ADV_DEBUG
1550 static int mcam_vidioc_g_register(struct file *file, void *priv,
1551                 struct v4l2_dbg_register *reg)
1552 {
1553         struct mcam_camera *cam = video_drvdata(file);
1554
1555         if (reg->reg > cam->regs_size - 4)
1556                 return -EINVAL;
1557         reg->val = mcam_reg_read(cam, reg->reg);
1558         reg->size = 4;
1559         return 0;
1560 }
1561
1562 static int mcam_vidioc_s_register(struct file *file, void *priv,
1563                 const struct v4l2_dbg_register *reg)
1564 {
1565         struct mcam_camera *cam = video_drvdata(file);
1566
1567         if (reg->reg > cam->regs_size - 4)
1568                 return -EINVAL;
1569         mcam_reg_write(cam, reg->reg, reg->val);
1570         return 0;
1571 }
1572 #endif
1573
1574 static const struct v4l2_ioctl_ops mcam_v4l_ioctl_ops = {
1575         .vidioc_querycap        = mcam_vidioc_querycap,
1576         .vidioc_enum_fmt_vid_cap = mcam_vidioc_enum_fmt_vid_cap,
1577         .vidioc_try_fmt_vid_cap = mcam_vidioc_try_fmt_vid_cap,
1578         .vidioc_s_fmt_vid_cap   = mcam_vidioc_s_fmt_vid_cap,
1579         .vidioc_g_fmt_vid_cap   = mcam_vidioc_g_fmt_vid_cap,
1580         .vidioc_enum_input      = mcam_vidioc_enum_input,
1581         .vidioc_g_input         = mcam_vidioc_g_input,
1582         .vidioc_s_input         = mcam_vidioc_s_input,
1583         .vidioc_reqbufs         = vb2_ioctl_reqbufs,
1584         .vidioc_create_bufs     = vb2_ioctl_create_bufs,
1585         .vidioc_querybuf        = vb2_ioctl_querybuf,
1586         .vidioc_qbuf            = vb2_ioctl_qbuf,
1587         .vidioc_dqbuf           = vb2_ioctl_dqbuf,
1588         .vidioc_expbuf          = vb2_ioctl_expbuf,
1589         .vidioc_streamon        = vb2_ioctl_streamon,
1590         .vidioc_streamoff       = vb2_ioctl_streamoff,
1591         .vidioc_g_parm          = mcam_vidioc_g_parm,
1592         .vidioc_s_parm          = mcam_vidioc_s_parm,
1593         .vidioc_enum_framesizes = mcam_vidioc_enum_framesizes,
1594         .vidioc_enum_frameintervals = mcam_vidioc_enum_frameintervals,
1595         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1596         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1597 #ifdef CONFIG_VIDEO_ADV_DEBUG
1598         .vidioc_g_register      = mcam_vidioc_g_register,
1599         .vidioc_s_register      = mcam_vidioc_s_register,
1600 #endif
1601 };
1602
1603 /* ---------------------------------------------------------------------- */
1604 /*
1605  * Our various file operations.
1606  */
1607 static int mcam_v4l_open(struct file *filp)
1608 {
1609         struct mcam_camera *cam = video_drvdata(filp);
1610         int ret;
1611
1612         mutex_lock(&cam->s_mutex);
1613         ret = v4l2_fh_open(filp);
1614         if (ret)
1615                 goto out;
1616         if (v4l2_fh_is_singular_file(filp)) {
1617                 ret = mcam_ctlr_power_up(cam);
1618                 if (ret)
1619                         goto out;
1620                 __mcam_cam_reset(cam);
1621                 mcam_set_config_needed(cam, 1);
1622         }
1623 out:
1624         mutex_unlock(&cam->s_mutex);
1625         if (ret)
1626                 v4l2_fh_release(filp);
1627         return ret;
1628 }
1629
1630
1631 static int mcam_v4l_release(struct file *filp)
1632 {
1633         struct mcam_camera *cam = video_drvdata(filp);
1634         bool last_open;
1635
1636         mutex_lock(&cam->s_mutex);
1637         last_open = v4l2_fh_is_singular_file(filp);
1638         _vb2_fop_release(filp, NULL);
1639         if (last_open) {
1640                 mcam_disable_mipi(cam);
1641                 mcam_ctlr_power_down(cam);
1642                 if (cam->buffer_mode == B_vmalloc && alloc_bufs_at_read)
1643                         mcam_free_dma_bufs(cam);
1644         }
1645
1646         mutex_unlock(&cam->s_mutex);
1647         return 0;
1648 }
1649
1650 static const struct v4l2_file_operations mcam_v4l_fops = {
1651         .owner = THIS_MODULE,
1652         .open = mcam_v4l_open,
1653         .release = mcam_v4l_release,
1654         .read = vb2_fop_read,
1655         .poll = vb2_fop_poll,
1656         .mmap = vb2_fop_mmap,
1657         .unlocked_ioctl = video_ioctl2,
1658 };
1659
1660
1661 /*
1662  * This template device holds all of those v4l2 methods; we
1663  * clone it for specific real devices.
1664  */
1665 static struct video_device mcam_v4l_template = {
1666         .name = "mcam",
1667         .fops = &mcam_v4l_fops,
1668         .ioctl_ops = &mcam_v4l_ioctl_ops,
1669         .release = video_device_release_empty,
1670 };
1671
1672 /* ---------------------------------------------------------------------- */
1673 /*
1674  * Interrupt handler stuff
1675  */
1676 static void mcam_frame_complete(struct mcam_camera *cam, int frame)
1677 {
1678         /*
1679          * Basic frame housekeeping.
1680          */
1681         set_bit(frame, &cam->flags);
1682         clear_bit(CF_DMA_ACTIVE, &cam->flags);
1683         cam->next_buf = frame;
1684         cam->buf_seq[frame] = cam->sequence++;
1685         cam->frame_state.frames++;
1686         /*
1687          * "This should never happen"
1688          */
1689         if (cam->state != S_STREAMING)
1690                 return;
1691         /*
1692          * Process the frame and set up the next one.
1693          */
1694         cam->frame_complete(cam, frame);
1695 }
1696
1697
1698 /*
1699  * The interrupt handler; this needs to be called from the
1700  * platform irq handler with the lock held.
1701  */
1702 int mccic_irq(struct mcam_camera *cam, unsigned int irqs)
1703 {
1704         unsigned int frame, handled = 0;
1705
1706         mcam_reg_write(cam, REG_IRQSTAT, FRAMEIRQS); /* Clear'em all */
1707         /*
1708          * Handle any frame completions.  There really should
1709          * not be more than one of these, or we have fallen
1710          * far behind.
1711          *
1712          * When running in S/G mode, the frame number lacks any
1713          * real meaning - there's only one descriptor array - but
1714          * the controller still picks a different one to signal
1715          * each time.
1716          */
1717         for (frame = 0; frame < cam->nbufs; frame++)
1718                 if (irqs & (IRQ_EOF0 << frame) &&
1719                         test_bit(CF_FRAME_SOF0 + frame, &cam->flags)) {
1720                         mcam_frame_complete(cam, frame);
1721                         handled = 1;
1722                         clear_bit(CF_FRAME_SOF0 + frame, &cam->flags);
1723                         if (cam->buffer_mode == B_DMA_sg)
1724                                 break;
1725                 }
1726         /*
1727          * If a frame starts, note that we have DMA active.  This
1728          * code assumes that we won't get multiple frame interrupts
1729          * at once; may want to rethink that.
1730          */
1731         for (frame = 0; frame < cam->nbufs; frame++) {
1732                 if (irqs & (IRQ_SOF0 << frame)) {
1733                         set_bit(CF_FRAME_SOF0 + frame, &cam->flags);
1734                         handled = IRQ_HANDLED;
1735                 }
1736         }
1737
1738         if (handled == IRQ_HANDLED) {
1739                 set_bit(CF_DMA_ACTIVE, &cam->flags);
1740                 if (cam->buffer_mode == B_DMA_sg)
1741                         mcam_ctlr_stop(cam);
1742         }
1743         return handled;
1744 }
1745
1746 /* ---------------------------------------------------------------------- */
1747 /*
1748  * Registration and such.
1749  */
1750 static struct ov7670_config sensor_cfg = {
1751         /*
1752          * Exclude QCIF mode, because it only captures a tiny portion
1753          * of the sensor FOV
1754          */
1755         .min_width = 320,
1756         .min_height = 240,
1757 };
1758
1759
1760 int mccic_register(struct mcam_camera *cam)
1761 {
1762         struct i2c_board_info ov7670_info = {
1763                 .type = "ov7670",
1764                 .addr = 0x42 >> 1,
1765                 .platform_data = &sensor_cfg,
1766         };
1767         int ret;
1768
1769         /*
1770          * Validate the requested buffer mode.
1771          */
1772         if (buffer_mode >= 0)
1773                 cam->buffer_mode = buffer_mode;
1774         if (cam->buffer_mode == B_DMA_sg &&
1775                         cam->chip_id == MCAM_CAFE) {
1776                 printk(KERN_ERR "marvell-cam: Cafe can't do S/G I/O, "
1777                         "attempting vmalloc mode instead\n");
1778                 cam->buffer_mode = B_vmalloc;
1779         }
1780         if (!mcam_buffer_mode_supported(cam->buffer_mode)) {
1781                 printk(KERN_ERR "marvell-cam: buffer mode %d unsupported\n",
1782                                 cam->buffer_mode);
1783                 return -EINVAL;
1784         }
1785         /*
1786          * Register with V4L
1787          */
1788         ret = v4l2_device_register(cam->dev, &cam->v4l2_dev);
1789         if (ret)
1790                 return ret;
1791
1792         mutex_init(&cam->s_mutex);
1793         cam->state = S_NOTREADY;
1794         mcam_set_config_needed(cam, 1);
1795         cam->pix_format = mcam_def_pix_format;
1796         cam->mbus_code = mcam_def_mbus_code;
1797         mcam_ctlr_init(cam);
1798
1799         /*
1800          * Get the v4l2 setup done.
1801          */
1802         ret = v4l2_ctrl_handler_init(&cam->ctrl_handler, 10);
1803         if (ret)
1804                 goto out_unregister;
1805         cam->v4l2_dev.ctrl_handler = &cam->ctrl_handler;
1806
1807         /*
1808          * Try to find the sensor.
1809          */
1810         sensor_cfg.clock_speed = cam->clock_speed;
1811         sensor_cfg.use_smbus = cam->use_smbus;
1812         cam->sensor_addr = ov7670_info.addr;
1813         cam->sensor = v4l2_i2c_new_subdev_board(&cam->v4l2_dev,
1814                         cam->i2c_adapter, &ov7670_info, NULL);
1815         if (cam->sensor == NULL) {
1816                 ret = -ENODEV;
1817                 goto out_unregister;
1818         }
1819
1820         ret = mcam_cam_init(cam);
1821         if (ret)
1822                 goto out_unregister;
1823
1824         ret = mcam_setup_vb2(cam);
1825         if (ret)
1826                 goto out_unregister;
1827
1828         mutex_lock(&cam->s_mutex);
1829         cam->vdev = mcam_v4l_template;
1830         cam->vdev.v4l2_dev = &cam->v4l2_dev;
1831         cam->vdev.lock = &cam->s_mutex;
1832         cam->vdev.queue = &cam->vb_queue;
1833         video_set_drvdata(&cam->vdev, cam);
1834         ret = video_register_device(&cam->vdev, VFL_TYPE_GRABBER, -1);
1835         if (ret) {
1836                 mutex_unlock(&cam->s_mutex);
1837                 goto out_unregister;
1838         }
1839
1840         /*
1841          * If so requested, try to get our DMA buffers now.
1842          */
1843         if (cam->buffer_mode == B_vmalloc && !alloc_bufs_at_read) {
1844                 if (mcam_alloc_dma_bufs(cam, 1))
1845                         cam_warn(cam, "Unable to alloc DMA buffers at load"
1846                                         " will try again later.");
1847         }
1848
1849         mutex_unlock(&cam->s_mutex);
1850         return 0;
1851
1852 out_unregister:
1853         v4l2_ctrl_handler_free(&cam->ctrl_handler);
1854         v4l2_device_unregister(&cam->v4l2_dev);
1855         return ret;
1856 }
1857
1858
1859 void mccic_shutdown(struct mcam_camera *cam)
1860 {
1861         /*
1862          * If we have no users (and we really, really should have no
1863          * users) the device will already be powered down.  Trying to
1864          * take it down again will wedge the machine, which is frowned
1865          * upon.
1866          */
1867         if (!list_empty(&cam->vdev.fh_list)) {
1868                 cam_warn(cam, "Removing a device with users!\n");
1869                 mcam_ctlr_power_down(cam);
1870         }
1871         mcam_cleanup_vb2(cam);
1872         if (cam->buffer_mode == B_vmalloc)
1873                 mcam_free_dma_bufs(cam);
1874         video_unregister_device(&cam->vdev);
1875         v4l2_ctrl_handler_free(&cam->ctrl_handler);
1876         v4l2_device_unregister(&cam->v4l2_dev);
1877 }
1878
1879 /*
1880  * Power management
1881  */
1882 #ifdef CONFIG_PM
1883
1884 void mccic_suspend(struct mcam_camera *cam)
1885 {
1886         mutex_lock(&cam->s_mutex);
1887         if (!list_empty(&cam->vdev.fh_list)) {
1888                 enum mcam_state cstate = cam->state;
1889
1890                 mcam_ctlr_stop_dma(cam);
1891                 mcam_ctlr_power_down(cam);
1892                 cam->state = cstate;
1893         }
1894         mutex_unlock(&cam->s_mutex);
1895 }
1896
1897 int mccic_resume(struct mcam_camera *cam)
1898 {
1899         int ret = 0;
1900
1901         mutex_lock(&cam->s_mutex);
1902         if (!list_empty(&cam->vdev.fh_list)) {
1903                 ret = mcam_ctlr_power_up(cam);
1904                 if (ret) {
1905                         mutex_unlock(&cam->s_mutex);
1906                         return ret;
1907                 }
1908                 __mcam_cam_reset(cam);
1909         } else {
1910                 mcam_ctlr_power_down(cam);
1911         }
1912         mutex_unlock(&cam->s_mutex);
1913
1914         set_bit(CF_CONFIG_NEEDED, &cam->flags);
1915         if (cam->state == S_STREAMING) {
1916                 /*
1917                  * If there was a buffer in the DMA engine at suspend
1918                  * time, put it back on the queue or we'll forget about it.
1919                  */
1920                 if (cam->buffer_mode == B_DMA_sg && cam->vb_bufs[0])
1921                         list_add(&cam->vb_bufs[0]->queue, &cam->buffers);
1922                 ret = mcam_read_setup(cam);
1923         }
1924         return ret;
1925 }
1926 #endif /* CONFIG_PM */