Version:  2.0.40 2.2.26 2.4.37 2.6.39 3.0 3.1 3.2 3.3 3.4 3.5 3.6 3.7 3.8 3.9 3.10 3.11 3.12 3.13 3.14 3.15

Linux/drivers/media/platform/mx2_emmaprp.c

  1 /*
  2  * Support eMMa-PrP through mem2mem framework.
  3  *
  4  * eMMa-PrP is a piece of HW that allows fetching buffers
  5  * from one memory location and do several operations on
  6  * them such as scaling or format conversion giving, as a result
  7  * a new processed buffer in another memory location.
  8  *
  9  * Based on mem2mem_testdev.c by Pawel Osciak.
 10  *
 11  * Copyright (c) 2011 Vista Silicon S.L.
 12  * Javier Martin <javier.martin@vista-silicon.com>
 13  *
 14  * This program is free software; you can redistribute it and/or modify
 15  * it under the terms of the GNU General Public License as published by the
 16  * Free Software Foundation; either version 2 of the
 17  * License, or (at your option) any later version
 18  */
 19 #include <linux/module.h>
 20 #include <linux/clk.h>
 21 #include <linux/slab.h>
 22 #include <linux/interrupt.h>
 23 #include <linux/io.h>
 24 
 25 #include <linux/platform_device.h>
 26 #include <media/v4l2-mem2mem.h>
 27 #include <media/v4l2-device.h>
 28 #include <media/v4l2-ioctl.h>
 29 #include <media/videobuf2-dma-contig.h>
 30 #include <asm/sizes.h>
 31 
 32 #define EMMAPRP_MODULE_NAME "mem2mem-emmaprp"
 33 
 34 MODULE_DESCRIPTION("Mem-to-mem device which supports eMMa-PrP present in mx2 SoCs");
 35 MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com");
 36 MODULE_LICENSE("GPL");
 37 MODULE_VERSION("0.0.1");
 38 
 39 static bool debug;
 40 module_param(debug, bool, 0644);
 41 
 42 #define MIN_W 32
 43 #define MIN_H 32
 44 #define MAX_W 2040
 45 #define MAX_H 2046
 46 
 47 #define S_ALIGN         1 /* multiple of 2 */
 48 #define W_ALIGN_YUV420  3 /* multiple of 8 */
 49 #define W_ALIGN_OTHERS  2 /* multiple of 4 */
 50 #define H_ALIGN         1 /* multiple of 2 */
 51 
 52 /* Flags that indicate a format can be used for capture/output */
 53 #define MEM2MEM_CAPTURE (1 << 0)
 54 #define MEM2MEM_OUTPUT  (1 << 1)
 55 
 56 #define MEM2MEM_NAME            "m2m-emmaprp"
 57 
 58 /* In bytes, per queue */
 59 #define MEM2MEM_VID_MEM_LIMIT   SZ_16M
 60 
 61 #define dprintk(dev, fmt, arg...) \
 62         v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
 63 
 64 /* EMMA PrP */
 65 #define PRP_CNTL                        0x00
 66 #define PRP_INTR_CNTL                   0x04
 67 #define PRP_INTRSTATUS                  0x08
 68 #define PRP_SOURCE_Y_PTR                0x0c
 69 #define PRP_SOURCE_CB_PTR               0x10
 70 #define PRP_SOURCE_CR_PTR               0x14
 71 #define PRP_DEST_RGB1_PTR               0x18
 72 #define PRP_DEST_RGB2_PTR               0x1c
 73 #define PRP_DEST_Y_PTR                  0x20
 74 #define PRP_DEST_CB_PTR                 0x24
 75 #define PRP_DEST_CR_PTR                 0x28
 76 #define PRP_SRC_FRAME_SIZE              0x2c
 77 #define PRP_DEST_CH1_LINE_STRIDE        0x30
 78 #define PRP_SRC_PIXEL_FORMAT_CNTL       0x34
 79 #define PRP_CH1_PIXEL_FORMAT_CNTL       0x38
 80 #define PRP_CH1_OUT_IMAGE_SIZE          0x3c
 81 #define PRP_CH2_OUT_IMAGE_SIZE          0x40
 82 #define PRP_SRC_LINE_STRIDE             0x44
 83 #define PRP_CSC_COEF_012                0x48
 84 #define PRP_CSC_COEF_345                0x4c
 85 #define PRP_CSC_COEF_678                0x50
 86 #define PRP_CH1_RZ_HORI_COEF1           0x54
 87 #define PRP_CH1_RZ_HORI_COEF2           0x58
 88 #define PRP_CH1_RZ_HORI_VALID           0x5c
 89 #define PRP_CH1_RZ_VERT_COEF1           0x60
 90 #define PRP_CH1_RZ_VERT_COEF2           0x64
 91 #define PRP_CH1_RZ_VERT_VALID           0x68
 92 #define PRP_CH2_RZ_HORI_COEF1           0x6c
 93 #define PRP_CH2_RZ_HORI_COEF2           0x70
 94 #define PRP_CH2_RZ_HORI_VALID           0x74
 95 #define PRP_CH2_RZ_VERT_COEF1           0x78
 96 #define PRP_CH2_RZ_VERT_COEF2           0x7c
 97 #define PRP_CH2_RZ_VERT_VALID           0x80
 98 
 99 #define PRP_CNTL_CH1EN          (1 << 0)
100 #define PRP_CNTL_CH2EN          (1 << 1)
101 #define PRP_CNTL_CSIEN          (1 << 2)
102 #define PRP_CNTL_DATA_IN_YUV420 (0 << 3)
103 #define PRP_CNTL_DATA_IN_YUV422 (1 << 3)
104 #define PRP_CNTL_DATA_IN_RGB16  (2 << 3)
105 #define PRP_CNTL_DATA_IN_RGB32  (3 << 3)
106 #define PRP_CNTL_CH1_OUT_RGB8   (0 << 5)
107 #define PRP_CNTL_CH1_OUT_RGB16  (1 << 5)
108 #define PRP_CNTL_CH1_OUT_RGB32  (2 << 5)
109 #define PRP_CNTL_CH1_OUT_YUV422 (3 << 5)
110 #define PRP_CNTL_CH2_OUT_YUV420 (0 << 7)
111 #define PRP_CNTL_CH2_OUT_YUV422 (1 << 7)
112 #define PRP_CNTL_CH2_OUT_YUV444 (2 << 7)
113 #define PRP_CNTL_CH1_LEN        (1 << 9)
114 #define PRP_CNTL_CH2_LEN        (1 << 10)
115 #define PRP_CNTL_SKIP_FRAME     (1 << 11)
116 #define PRP_CNTL_SWRST          (1 << 12)
117 #define PRP_CNTL_CLKEN          (1 << 13)
118 #define PRP_CNTL_WEN            (1 << 14)
119 #define PRP_CNTL_CH1BYP         (1 << 15)
120 #define PRP_CNTL_IN_TSKIP(x)    ((x) << 16)
121 #define PRP_CNTL_CH1_TSKIP(x)   ((x) << 19)
122 #define PRP_CNTL_CH2_TSKIP(x)   ((x) << 22)
123 #define PRP_CNTL_INPUT_FIFO_LEVEL(x)    ((x) << 25)
124 #define PRP_CNTL_RZ_FIFO_LEVEL(x)       ((x) << 27)
125 #define PRP_CNTL_CH2B1EN        (1 << 29)
126 #define PRP_CNTL_CH2B2EN        (1 << 30)
127 #define PRP_CNTL_CH2FEN         (1 << 31)
128 
129 #define PRP_SIZE_HEIGHT(x)      (x)
130 #define PRP_SIZE_WIDTH(x)       ((x) << 16)
131 
132 /* IRQ Enable and status register */
133 #define PRP_INTR_RDERR          (1 << 0)
134 #define PRP_INTR_CH1WERR        (1 << 1)
135 #define PRP_INTR_CH2WERR        (1 << 2)
136 #define PRP_INTR_CH1FC          (1 << 3)
137 #define PRP_INTR_CH2FC          (1 << 5)
138 #define PRP_INTR_LBOVF          (1 << 7)
139 #define PRP_INTR_CH2OVF         (1 << 8)
140 
141 #define PRP_INTR_ST_RDERR       (1 << 0)
142 #define PRP_INTR_ST_CH1WERR     (1 << 1)
143 #define PRP_INTR_ST_CH2WERR     (1 << 2)
144 #define PRP_INTR_ST_CH2B2CI     (1 << 3)
145 #define PRP_INTR_ST_CH2B1CI     (1 << 4)
146 #define PRP_INTR_ST_CH1B2CI     (1 << 5)
147 #define PRP_INTR_ST_CH1B1CI     (1 << 6)
148 #define PRP_INTR_ST_LBOVF       (1 << 7)
149 #define PRP_INTR_ST_CH2OVF      (1 << 8)
150 
151 struct emmaprp_fmt {
152         char    *name;
153         u32     fourcc;
154         /* Types the format can be used for */
155         u32     types;
156 };
157 
158 static struct emmaprp_fmt formats[] = {
159         {
160                 .name   = "YUV 4:2:0 Planar",
161                 .fourcc = V4L2_PIX_FMT_YUV420,
162                 .types  = MEM2MEM_CAPTURE,
163         },
164         {
165                 .name   = "4:2:2, packed, YUYV",
166                 .fourcc = V4L2_PIX_FMT_YUYV,
167                 .types  = MEM2MEM_OUTPUT,
168         },
169 };
170 
171 /* Per-queue, driver-specific private data */
172 struct emmaprp_q_data {
173         unsigned int            width;
174         unsigned int            height;
175         unsigned int            sizeimage;
176         struct emmaprp_fmt      *fmt;
177 };
178 
179 enum {
180         V4L2_M2M_SRC = 0,
181         V4L2_M2M_DST = 1,
182 };
183 
184 #define NUM_FORMATS ARRAY_SIZE(formats)
185 
186 static struct emmaprp_fmt *find_format(struct v4l2_format *f)
187 {
188         struct emmaprp_fmt *fmt;
189         unsigned int k;
190 
191         for (k = 0; k < NUM_FORMATS; k++) {
192                 fmt = &formats[k];
193                 if (fmt->fourcc == f->fmt.pix.pixelformat)
194                         break;
195         }
196 
197         if (k == NUM_FORMATS)
198                 return NULL;
199 
200         return &formats[k];
201 }
202 
203 struct emmaprp_dev {
204         struct v4l2_device      v4l2_dev;
205         struct video_device     *vfd;
206 
207         struct mutex            dev_mutex;
208         spinlock_t              irqlock;
209 
210         int                     irq_emma;
211         void __iomem            *base_emma;
212         struct clk              *clk_emma_ahb, *clk_emma_ipg;
213         struct resource         *res_emma;
214 
215         struct v4l2_m2m_dev     *m2m_dev;
216         struct vb2_alloc_ctx    *alloc_ctx;
217 };
218 
219 struct emmaprp_ctx {
220         struct emmaprp_dev      *dev;
221         /* Abort requested by m2m */
222         int                     aborting;
223         struct emmaprp_q_data   q_data[2];
224         struct v4l2_m2m_ctx     *m2m_ctx;
225 };
226 
227 static struct emmaprp_q_data *get_q_data(struct emmaprp_ctx *ctx,
228                                          enum v4l2_buf_type type)
229 {
230         switch (type) {
231         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
232                 return &(ctx->q_data[V4L2_M2M_SRC]);
233         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
234                 return &(ctx->q_data[V4L2_M2M_DST]);
235         default:
236                 BUG();
237         }
238         return NULL;
239 }
240 
241 /*
242  * mem2mem callbacks
243  */
244 static void emmaprp_job_abort(void *priv)
245 {
246         struct emmaprp_ctx *ctx = priv;
247         struct emmaprp_dev *pcdev = ctx->dev;
248 
249         ctx->aborting = 1;
250 
251         dprintk(pcdev, "Aborting task\n");
252 
253         v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->m2m_ctx);
254 }
255 
256 static void emmaprp_lock(void *priv)
257 {
258         struct emmaprp_ctx *ctx = priv;
259         struct emmaprp_dev *pcdev = ctx->dev;
260         mutex_lock(&pcdev->dev_mutex);
261 }
262 
263 static void emmaprp_unlock(void *priv)
264 {
265         struct emmaprp_ctx *ctx = priv;
266         struct emmaprp_dev *pcdev = ctx->dev;
267         mutex_unlock(&pcdev->dev_mutex);
268 }
269 
270 static inline void emmaprp_dump_regs(struct emmaprp_dev *pcdev)
271 {
272         dprintk(pcdev,
273                 "eMMa-PrP Registers:\n"
274                 "  SOURCE_Y_PTR = 0x%08X\n"
275                 "  SRC_FRAME_SIZE = 0x%08X\n"
276                 "  DEST_Y_PTR = 0x%08X\n"
277                 "  DEST_CR_PTR = 0x%08X\n"
278                 "  DEST_CB_PTR = 0x%08X\n"
279                 "  CH2_OUT_IMAGE_SIZE = 0x%08X\n"
280                 "  CNTL = 0x%08X\n",
281                 readl(pcdev->base_emma + PRP_SOURCE_Y_PTR),
282                 readl(pcdev->base_emma + PRP_SRC_FRAME_SIZE),
283                 readl(pcdev->base_emma + PRP_DEST_Y_PTR),
284                 readl(pcdev->base_emma + PRP_DEST_CR_PTR),
285                 readl(pcdev->base_emma + PRP_DEST_CB_PTR),
286                 readl(pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE),
287                 readl(pcdev->base_emma + PRP_CNTL));
288 }
289 
290 static void emmaprp_device_run(void *priv)
291 {
292         struct emmaprp_ctx *ctx = priv;
293         struct emmaprp_q_data *s_q_data, *d_q_data;
294         struct vb2_buffer *src_buf, *dst_buf;
295         struct emmaprp_dev *pcdev = ctx->dev;
296         unsigned int s_width, s_height;
297         unsigned int d_width, d_height;
298         unsigned int d_size;
299         dma_addr_t p_in, p_out;
300         u32 tmp;
301 
302         src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
303         dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
304 
305         s_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
306         s_width = s_q_data->width;
307         s_height = s_q_data->height;
308 
309         d_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
310         d_width = d_q_data->width;
311         d_height = d_q_data->height;
312         d_size = d_width * d_height;
313 
314         p_in = vb2_dma_contig_plane_dma_addr(src_buf, 0);
315         p_out = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
316         if (!p_in || !p_out) {
317                 v4l2_err(&pcdev->v4l2_dev,
318                          "Acquiring kernel pointers to buffers failed\n");
319                 return;
320         }
321 
322         /* Input frame parameters */
323         writel(p_in, pcdev->base_emma + PRP_SOURCE_Y_PTR);
324         writel(PRP_SIZE_WIDTH(s_width) | PRP_SIZE_HEIGHT(s_height),
325                pcdev->base_emma + PRP_SRC_FRAME_SIZE);
326 
327         /* Output frame parameters */
328         writel(p_out, pcdev->base_emma + PRP_DEST_Y_PTR);
329         writel(p_out + d_size, pcdev->base_emma + PRP_DEST_CB_PTR);
330         writel(p_out + d_size + (d_size >> 2),
331                pcdev->base_emma + PRP_DEST_CR_PTR);
332         writel(PRP_SIZE_WIDTH(d_width) | PRP_SIZE_HEIGHT(d_height),
333                pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE);
334 
335         /* IRQ configuration */
336         tmp = readl(pcdev->base_emma + PRP_INTR_CNTL);
337         writel(tmp | PRP_INTR_RDERR |
338                 PRP_INTR_CH2WERR |
339                 PRP_INTR_CH2FC,
340                 pcdev->base_emma + PRP_INTR_CNTL);
341 
342         emmaprp_dump_regs(pcdev);
343 
344         /* Enable transfer */
345         tmp = readl(pcdev->base_emma + PRP_CNTL);
346         writel(tmp | PRP_CNTL_CH2_OUT_YUV420 |
347                 PRP_CNTL_DATA_IN_YUV422 |
348                 PRP_CNTL_CH2EN,
349                 pcdev->base_emma + PRP_CNTL);
350 }
351 
352 static irqreturn_t emmaprp_irq(int irq_emma, void *data)
353 {
354         struct emmaprp_dev *pcdev = data;
355         struct emmaprp_ctx *curr_ctx;
356         struct vb2_buffer *src_vb, *dst_vb;
357         unsigned long flags;
358         u32 irqst;
359 
360         /* Check irq flags and clear irq */
361         irqst = readl(pcdev->base_emma + PRP_INTRSTATUS);
362         writel(irqst, pcdev->base_emma + PRP_INTRSTATUS);
363         dprintk(pcdev, "irqst = 0x%08x\n", irqst);
364 
365         curr_ctx = v4l2_m2m_get_curr_priv(pcdev->m2m_dev);
366         if (curr_ctx == NULL) {
367                 pr_err("Instance released before the end of transaction\n");
368                 return IRQ_HANDLED;
369         }
370 
371         if (!curr_ctx->aborting) {
372                 if ((irqst & PRP_INTR_ST_RDERR) ||
373                 (irqst & PRP_INTR_ST_CH2WERR)) {
374                         pr_err("PrP bus error occurred, this transfer is probably corrupted\n");
375                         writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
376                 } else if (irqst & PRP_INTR_ST_CH2B1CI) { /* buffer ready */
377                         src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
378                         dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
379 
380                         dst_vb->v4l2_buf.timestamp = src_vb->v4l2_buf.timestamp;
381                         dst_vb->v4l2_buf.flags &=
382                                 ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
383                         dst_vb->v4l2_buf.flags |=
384                                 src_vb->v4l2_buf.flags
385                                 & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
386                         dst_vb->v4l2_buf.timecode = src_vb->v4l2_buf.timecode;
387 
388                         spin_lock_irqsave(&pcdev->irqlock, flags);
389                         v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
390                         v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
391                         spin_unlock_irqrestore(&pcdev->irqlock, flags);
392                 }
393         }
394 
395         v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->m2m_ctx);
396         return IRQ_HANDLED;
397 }
398 
399 /*
400  * video ioctls
401  */
402 static int vidioc_querycap(struct file *file, void *priv,
403                            struct v4l2_capability *cap)
404 {
405         strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
406         strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
407         /*
408          * This is only a mem-to-mem video device. The capture and output
409          * device capability flags are left only for backward compatibility
410          * and are scheduled for removal.
411          */
412         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
413                             V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
414         return 0;
415 }
416 
417 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
418 {
419         int i, num;
420         struct emmaprp_fmt *fmt;
421 
422         num = 0;
423 
424         for (i = 0; i < NUM_FORMATS; ++i) {
425                 if (formats[i].types & type) {
426                         /* index-th format of type type found ? */
427                         if (num == f->index)
428                                 break;
429                         /* Correct type but haven't reached our index yet,
430                          * just increment per-type index */
431                         ++num;
432                 }
433         }
434 
435         if (i < NUM_FORMATS) {
436                 /* Format found */
437                 fmt = &formats[i];
438                 strlcpy(f->description, fmt->name, sizeof(f->description) - 1);
439                 f->pixelformat = fmt->fourcc;
440                 return 0;
441         }
442 
443         /* Format not found */
444         return -EINVAL;
445 }
446 
447 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
448                                    struct v4l2_fmtdesc *f)
449 {
450         return enum_fmt(f, MEM2MEM_CAPTURE);
451 }
452 
453 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
454                                    struct v4l2_fmtdesc *f)
455 {
456         return enum_fmt(f, MEM2MEM_OUTPUT);
457 }
458 
459 static int vidioc_g_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
460 {
461         struct vb2_queue *vq;
462         struct emmaprp_q_data *q_data;
463 
464         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
465         if (!vq)
466                 return -EINVAL;
467 
468         q_data = get_q_data(ctx, f->type);
469 
470         f->fmt.pix.width        = q_data->width;
471         f->fmt.pix.height       = q_data->height;
472         f->fmt.pix.field        = V4L2_FIELD_NONE;
473         f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
474         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
475                 f->fmt.pix.bytesperline = q_data->width * 3 / 2;
476         else /* YUYV */
477                 f->fmt.pix.bytesperline = q_data->width * 2;
478         f->fmt.pix.sizeimage    = q_data->sizeimage;
479 
480         return 0;
481 }
482 
483 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
484                                 struct v4l2_format *f)
485 {
486         return vidioc_g_fmt(priv, f);
487 }
488 
489 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
490                                 struct v4l2_format *f)
491 {
492         return vidioc_g_fmt(priv, f);
493 }
494 
495 static int vidioc_try_fmt(struct v4l2_format *f)
496 {
497         enum v4l2_field field;
498 
499 
500         if (!find_format(f))
501                 return -EINVAL;
502 
503         field = f->fmt.pix.field;
504         if (field == V4L2_FIELD_ANY)
505                 field = V4L2_FIELD_NONE;
506         else if (V4L2_FIELD_NONE != field)
507                 return -EINVAL;
508 
509         /* V4L2 specification suggests the driver corrects the format struct
510          * if any of the dimensions is unsupported */
511         f->fmt.pix.field = field;
512 
513         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
514                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
515                                       W_ALIGN_YUV420, &f->fmt.pix.height,
516                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
517                 f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
518         } else {
519                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
520                                       W_ALIGN_OTHERS, &f->fmt.pix.height,
521                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
522                 f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
523         }
524         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
525 
526         return 0;
527 }
528 
529 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
530                                   struct v4l2_format *f)
531 {
532         struct emmaprp_fmt *fmt;
533         struct emmaprp_ctx *ctx = priv;
534 
535         fmt = find_format(f);
536         if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
537                 v4l2_err(&ctx->dev->v4l2_dev,
538                          "Fourcc format (0x%08x) invalid.\n",
539                          f->fmt.pix.pixelformat);
540                 return -EINVAL;
541         }
542 
543         return vidioc_try_fmt(f);
544 }
545 
546 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
547                                   struct v4l2_format *f)
548 {
549         struct emmaprp_fmt *fmt;
550         struct emmaprp_ctx *ctx = priv;
551 
552         fmt = find_format(f);
553         if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
554                 v4l2_err(&ctx->dev->v4l2_dev,
555                          "Fourcc format (0x%08x) invalid.\n",
556                          f->fmt.pix.pixelformat);
557                 return -EINVAL;
558         }
559 
560         return vidioc_try_fmt(f);
561 }
562 
563 static int vidioc_s_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
564 {
565         struct emmaprp_q_data *q_data;
566         struct vb2_queue *vq;
567         int ret;
568 
569         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
570         if (!vq)
571                 return -EINVAL;
572 
573         q_data = get_q_data(ctx, f->type);
574         if (!q_data)
575                 return -EINVAL;
576 
577         if (vb2_is_busy(vq)) {
578                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
579                 return -EBUSY;
580         }
581 
582         ret = vidioc_try_fmt(f);
583         if (ret)
584                 return ret;
585 
586         q_data->fmt             = find_format(f);
587         q_data->width           = f->fmt.pix.width;
588         q_data->height          = f->fmt.pix.height;
589         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
590                 q_data->sizeimage = q_data->width * q_data->height * 3 / 2;
591         else /* YUYV */
592                 q_data->sizeimage = q_data->width * q_data->height * 2;
593 
594         dprintk(ctx->dev,
595                 "Setting format for type %d, wxh: %dx%d, fmt: %d\n",
596                 f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
597 
598         return 0;
599 }
600 
601 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
602                                 struct v4l2_format *f)
603 {
604         int ret;
605 
606         ret = vidioc_try_fmt_vid_cap(file, priv, f);
607         if (ret)
608                 return ret;
609 
610         return vidioc_s_fmt(priv, f);
611 }
612 
613 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
614                                 struct v4l2_format *f)
615 {
616         int ret;
617 
618         ret = vidioc_try_fmt_vid_out(file, priv, f);
619         if (ret)
620                 return ret;
621 
622         return vidioc_s_fmt(priv, f);
623 }
624 
625 static int vidioc_reqbufs(struct file *file, void *priv,
626                           struct v4l2_requestbuffers *reqbufs)
627 {
628         struct emmaprp_ctx *ctx = priv;
629 
630         return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
631 }
632 
633 static int vidioc_querybuf(struct file *file, void *priv,
634                            struct v4l2_buffer *buf)
635 {
636         struct emmaprp_ctx *ctx = priv;
637 
638         return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
639 }
640 
641 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
642 {
643         struct emmaprp_ctx *ctx = priv;
644 
645         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
646 }
647 
648 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
649 {
650         struct emmaprp_ctx *ctx = priv;
651 
652         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
653 }
654 
655 static int vidioc_streamon(struct file *file, void *priv,
656                            enum v4l2_buf_type type)
657 {
658         struct emmaprp_ctx *ctx = priv;
659 
660         return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
661 }
662 
663 static int vidioc_streamoff(struct file *file, void *priv,
664                             enum v4l2_buf_type type)
665 {
666         struct emmaprp_ctx *ctx = priv;
667 
668         return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
669 }
670 
671 static const struct v4l2_ioctl_ops emmaprp_ioctl_ops = {
672         .vidioc_querycap        = vidioc_querycap,
673 
674         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
675         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
676         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
677         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
678 
679         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
680         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
681         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
682         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
683 
684         .vidioc_reqbufs         = vidioc_reqbufs,
685         .vidioc_querybuf        = vidioc_querybuf,
686 
687         .vidioc_qbuf            = vidioc_qbuf,
688         .vidioc_dqbuf           = vidioc_dqbuf,
689 
690         .vidioc_streamon        = vidioc_streamon,
691         .vidioc_streamoff       = vidioc_streamoff,
692 };
693 
694 
695 /*
696  * Queue operations
697  */
698 static int emmaprp_queue_setup(struct vb2_queue *vq,
699                                 const struct v4l2_format *fmt,
700                                 unsigned int *nbuffers, unsigned int *nplanes,
701                                 unsigned int sizes[], void *alloc_ctxs[])
702 {
703         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vq);
704         struct emmaprp_q_data *q_data;
705         unsigned int size, count = *nbuffers;
706 
707         q_data = get_q_data(ctx, vq->type);
708 
709         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
710                 size = q_data->width * q_data->height * 3 / 2;
711         else
712                 size = q_data->width * q_data->height * 2;
713 
714         while (size * count > MEM2MEM_VID_MEM_LIMIT)
715                 (count)--;
716 
717         *nplanes = 1;
718         *nbuffers = count;
719         sizes[0] = size;
720 
721         alloc_ctxs[0] = ctx->dev->alloc_ctx;
722 
723         dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
724 
725         return 0;
726 }
727 
728 static int emmaprp_buf_prepare(struct vb2_buffer *vb)
729 {
730         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
731         struct emmaprp_q_data *q_data;
732 
733         dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
734 
735         q_data = get_q_data(ctx, vb->vb2_queue->type);
736 
737         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
738                 dprintk(ctx->dev, "%s data will not fit into plane"
739                                   "(%lu < %lu)\n", __func__,
740                                   vb2_plane_size(vb, 0),
741                                   (long)q_data->sizeimage);
742                 return -EINVAL;
743         }
744 
745         vb2_set_plane_payload(vb, 0, q_data->sizeimage);
746 
747         return 0;
748 }
749 
750 static void emmaprp_buf_queue(struct vb2_buffer *vb)
751 {
752         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
753         v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
754 }
755 
756 static struct vb2_ops emmaprp_qops = {
757         .queue_setup     = emmaprp_queue_setup,
758         .buf_prepare     = emmaprp_buf_prepare,
759         .buf_queue       = emmaprp_buf_queue,
760 };
761 
762 static int queue_init(void *priv, struct vb2_queue *src_vq,
763                       struct vb2_queue *dst_vq)
764 {
765         struct emmaprp_ctx *ctx = priv;
766         int ret;
767 
768         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
769         src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
770         src_vq->drv_priv = ctx;
771         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
772         src_vq->ops = &emmaprp_qops;
773         src_vq->mem_ops = &vb2_dma_contig_memops;
774         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
775 
776         ret = vb2_queue_init(src_vq);
777         if (ret)
778                 return ret;
779 
780         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
781         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
782         dst_vq->drv_priv = ctx;
783         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
784         dst_vq->ops = &emmaprp_qops;
785         dst_vq->mem_ops = &vb2_dma_contig_memops;
786         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
787 
788         return vb2_queue_init(dst_vq);
789 }
790 
791 /*
792  * File operations
793  */
794 static int emmaprp_open(struct file *file)
795 {
796         struct emmaprp_dev *pcdev = video_drvdata(file);
797         struct emmaprp_ctx *ctx;
798 
799         ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
800         if (!ctx)
801                 return -ENOMEM;
802 
803         file->private_data = ctx;
804         ctx->dev = pcdev;
805 
806         if (mutex_lock_interruptible(&pcdev->dev_mutex)) {
807                 kfree(ctx);
808                 return -ERESTARTSYS;
809         }
810 
811         ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
812 
813         if (IS_ERR(ctx->m2m_ctx)) {
814                 int ret = PTR_ERR(ctx->m2m_ctx);
815 
816                 mutex_unlock(&pcdev->dev_mutex);
817                 kfree(ctx);
818                 return ret;
819         }
820 
821         clk_prepare_enable(pcdev->clk_emma_ipg);
822         clk_prepare_enable(pcdev->clk_emma_ahb);
823         ctx->q_data[V4L2_M2M_SRC].fmt = &formats[1];
824         ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
825         mutex_unlock(&pcdev->dev_mutex);
826 
827         dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
828 
829         return 0;
830 }
831 
832 static int emmaprp_release(struct file *file)
833 {
834         struct emmaprp_dev *pcdev = video_drvdata(file);
835         struct emmaprp_ctx *ctx = file->private_data;
836 
837         dprintk(pcdev, "Releasing instance %p\n", ctx);
838 
839         mutex_lock(&pcdev->dev_mutex);
840         clk_disable_unprepare(pcdev->clk_emma_ahb);
841         clk_disable_unprepare(pcdev->clk_emma_ipg);
842         v4l2_m2m_ctx_release(ctx->m2m_ctx);
843         mutex_unlock(&pcdev->dev_mutex);
844         kfree(ctx);
845 
846         return 0;
847 }
848 
849 static unsigned int emmaprp_poll(struct file *file,
850                                  struct poll_table_struct *wait)
851 {
852         struct emmaprp_dev *pcdev = video_drvdata(file);
853         struct emmaprp_ctx *ctx = file->private_data;
854         unsigned int res;
855 
856         mutex_lock(&pcdev->dev_mutex);
857         res = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
858         mutex_unlock(&pcdev->dev_mutex);
859         return res;
860 }
861 
862 static int emmaprp_mmap(struct file *file, struct vm_area_struct *vma)
863 {
864         struct emmaprp_dev *pcdev = video_drvdata(file);
865         struct emmaprp_ctx *ctx = file->private_data;
866         int ret;
867 
868         if (mutex_lock_interruptible(&pcdev->dev_mutex))
869                 return -ERESTARTSYS;
870         ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
871         mutex_unlock(&pcdev->dev_mutex);
872         return ret;
873 }
874 
875 static const struct v4l2_file_operations emmaprp_fops = {
876         .owner          = THIS_MODULE,
877         .open           = emmaprp_open,
878         .release        = emmaprp_release,
879         .poll           = emmaprp_poll,
880         .unlocked_ioctl = video_ioctl2,
881         .mmap           = emmaprp_mmap,
882 };
883 
884 static struct video_device emmaprp_videodev = {
885         .name           = MEM2MEM_NAME,
886         .fops           = &emmaprp_fops,
887         .ioctl_ops      = &emmaprp_ioctl_ops,
888         .minor          = -1,
889         .release        = video_device_release,
890         .vfl_dir        = VFL_DIR_M2M,
891 };
892 
893 static struct v4l2_m2m_ops m2m_ops = {
894         .device_run     = emmaprp_device_run,
895         .job_abort      = emmaprp_job_abort,
896         .lock           = emmaprp_lock,
897         .unlock         = emmaprp_unlock,
898 };
899 
900 static int emmaprp_probe(struct platform_device *pdev)
901 {
902         struct emmaprp_dev *pcdev;
903         struct video_device *vfd;
904         struct resource *res_emma;
905         int irq_emma;
906         int ret;
907 
908         pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
909         if (!pcdev)
910                 return -ENOMEM;
911 
912         spin_lock_init(&pcdev->irqlock);
913 
914         pcdev->clk_emma_ipg = devm_clk_get(&pdev->dev, "ipg");
915         if (IS_ERR(pcdev->clk_emma_ipg)) {
916                 return PTR_ERR(pcdev->clk_emma_ipg);
917         }
918 
919         pcdev->clk_emma_ahb = devm_clk_get(&pdev->dev, "ahb");
920         if (IS_ERR(pcdev->clk_emma_ahb))
921                 return PTR_ERR(pcdev->clk_emma_ahb);
922 
923         irq_emma = platform_get_irq(pdev, 0);
924         res_emma = platform_get_resource(pdev, IORESOURCE_MEM, 0);
925         if (irq_emma < 0 || res_emma == NULL) {
926                 dev_err(&pdev->dev, "Missing platform resources data\n");
927                 return -ENODEV;
928         }
929 
930         ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
931         if (ret)
932                 return ret;
933 
934         mutex_init(&pcdev->dev_mutex);
935 
936         vfd = video_device_alloc();
937         if (!vfd) {
938                 v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n");
939                 ret = -ENOMEM;
940                 goto unreg_dev;
941         }
942 
943         *vfd = emmaprp_videodev;
944         vfd->lock = &pcdev->dev_mutex;
945         vfd->v4l2_dev = &pcdev->v4l2_dev;
946 
947         video_set_drvdata(vfd, pcdev);
948         snprintf(vfd->name, sizeof(vfd->name), "%s", emmaprp_videodev.name);
949         pcdev->vfd = vfd;
950         v4l2_info(&pcdev->v4l2_dev, EMMAPRP_MODULE_NAME
951                         " Device registered as /dev/video%d\n", vfd->num);
952 
953         platform_set_drvdata(pdev, pcdev);
954 
955         pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res_emma);
956         if (IS_ERR(pcdev->base_emma)) {
957                 ret = PTR_ERR(pcdev->base_emma);
958                 goto rel_vdev;
959         }
960 
961         pcdev->irq_emma = irq_emma;
962         pcdev->res_emma = res_emma;
963 
964         if (devm_request_irq(&pdev->dev, pcdev->irq_emma, emmaprp_irq,
965                              0, MEM2MEM_NAME, pcdev) < 0) {
966                 ret = -ENODEV;
967                 goto rel_vdev;
968         }
969 
970         pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
971         if (IS_ERR(pcdev->alloc_ctx)) {
972                 v4l2_err(&pcdev->v4l2_dev, "Failed to alloc vb2 context\n");
973                 ret = PTR_ERR(pcdev->alloc_ctx);
974                 goto rel_vdev;
975         }
976 
977         pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
978         if (IS_ERR(pcdev->m2m_dev)) {
979                 v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
980                 ret = PTR_ERR(pcdev->m2m_dev);
981                 goto rel_ctx;
982         }
983 
984         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
985         if (ret) {
986                 v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
987                 goto rel_m2m;
988         }
989 
990         return 0;
991 
992 
993 rel_m2m:
994         v4l2_m2m_release(pcdev->m2m_dev);
995 rel_ctx:
996         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
997 rel_vdev:
998         video_device_release(vfd);
999 unreg_dev:
1000         v4l2_device_unregister(&pcdev->v4l2_dev);
1001 
1002         return ret;
1003 }
1004 
1005 static int emmaprp_remove(struct platform_device *pdev)
1006 {
1007         struct emmaprp_dev *pcdev = platform_get_drvdata(pdev);
1008 
1009         v4l2_info(&pcdev->v4l2_dev, "Removing " EMMAPRP_MODULE_NAME);
1010 
1011         video_unregister_device(pcdev->vfd);
1012         v4l2_m2m_release(pcdev->m2m_dev);
1013         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
1014         v4l2_device_unregister(&pcdev->v4l2_dev);
1015 
1016         return 0;
1017 }
1018 
1019 static struct platform_driver emmaprp_pdrv = {
1020         .probe          = emmaprp_probe,
1021         .remove         = emmaprp_remove,
1022         .driver         = {
1023                 .name   = MEM2MEM_NAME,
1024                 .owner  = THIS_MODULE,
1025         },
1026 };
1027 module_platform_driver(emmaprp_pdrv);
1028 

This page was automatically generated by LXR 0.3.1 (source).  •  Linux is a registered trademark of Linus Torvalds  •  Contact us