Version:  2.0.40 2.2.26 2.4.37 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 3.16 3.17 3.18

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 <linux/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         void __iomem            *base_emma;
211         struct clk              *clk_emma_ahb, *clk_emma_ipg;
212 
213         struct v4l2_m2m_dev     *m2m_dev;
214         struct vb2_alloc_ctx    *alloc_ctx;
215 };
216 
217 struct emmaprp_ctx {
218         struct emmaprp_dev      *dev;
219         /* Abort requested by m2m */
220         int                     aborting;
221         struct emmaprp_q_data   q_data[2];
222         struct v4l2_m2m_ctx     *m2m_ctx;
223 };
224 
225 static struct emmaprp_q_data *get_q_data(struct emmaprp_ctx *ctx,
226                                          enum v4l2_buf_type type)
227 {
228         switch (type) {
229         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
230                 return &(ctx->q_data[V4L2_M2M_SRC]);
231         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
232                 return &(ctx->q_data[V4L2_M2M_DST]);
233         default:
234                 BUG();
235         }
236         return NULL;
237 }
238 
239 /*
240  * mem2mem callbacks
241  */
242 static void emmaprp_job_abort(void *priv)
243 {
244         struct emmaprp_ctx *ctx = priv;
245         struct emmaprp_dev *pcdev = ctx->dev;
246 
247         ctx->aborting = 1;
248 
249         dprintk(pcdev, "Aborting task\n");
250 
251         v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->m2m_ctx);
252 }
253 
254 static void emmaprp_lock(void *priv)
255 {
256         struct emmaprp_ctx *ctx = priv;
257         struct emmaprp_dev *pcdev = ctx->dev;
258         mutex_lock(&pcdev->dev_mutex);
259 }
260 
261 static void emmaprp_unlock(void *priv)
262 {
263         struct emmaprp_ctx *ctx = priv;
264         struct emmaprp_dev *pcdev = ctx->dev;
265         mutex_unlock(&pcdev->dev_mutex);
266 }
267 
268 static inline void emmaprp_dump_regs(struct emmaprp_dev *pcdev)
269 {
270         dprintk(pcdev,
271                 "eMMa-PrP Registers:\n"
272                 "  SOURCE_Y_PTR = 0x%08X\n"
273                 "  SRC_FRAME_SIZE = 0x%08X\n"
274                 "  DEST_Y_PTR = 0x%08X\n"
275                 "  DEST_CR_PTR = 0x%08X\n"
276                 "  DEST_CB_PTR = 0x%08X\n"
277                 "  CH2_OUT_IMAGE_SIZE = 0x%08X\n"
278                 "  CNTL = 0x%08X\n",
279                 readl(pcdev->base_emma + PRP_SOURCE_Y_PTR),
280                 readl(pcdev->base_emma + PRP_SRC_FRAME_SIZE),
281                 readl(pcdev->base_emma + PRP_DEST_Y_PTR),
282                 readl(pcdev->base_emma + PRP_DEST_CR_PTR),
283                 readl(pcdev->base_emma + PRP_DEST_CB_PTR),
284                 readl(pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE),
285                 readl(pcdev->base_emma + PRP_CNTL));
286 }
287 
288 static void emmaprp_device_run(void *priv)
289 {
290         struct emmaprp_ctx *ctx = priv;
291         struct emmaprp_q_data *s_q_data, *d_q_data;
292         struct vb2_buffer *src_buf, *dst_buf;
293         struct emmaprp_dev *pcdev = ctx->dev;
294         unsigned int s_width, s_height;
295         unsigned int d_width, d_height;
296         unsigned int d_size;
297         dma_addr_t p_in, p_out;
298         u32 tmp;
299 
300         src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
301         dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
302 
303         s_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
304         s_width = s_q_data->width;
305         s_height = s_q_data->height;
306 
307         d_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
308         d_width = d_q_data->width;
309         d_height = d_q_data->height;
310         d_size = d_width * d_height;
311 
312         p_in = vb2_dma_contig_plane_dma_addr(src_buf, 0);
313         p_out = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
314         if (!p_in || !p_out) {
315                 v4l2_err(&pcdev->v4l2_dev,
316                          "Acquiring kernel pointers to buffers failed\n");
317                 return;
318         }
319 
320         /* Input frame parameters */
321         writel(p_in, pcdev->base_emma + PRP_SOURCE_Y_PTR);
322         writel(PRP_SIZE_WIDTH(s_width) | PRP_SIZE_HEIGHT(s_height),
323                pcdev->base_emma + PRP_SRC_FRAME_SIZE);
324 
325         /* Output frame parameters */
326         writel(p_out, pcdev->base_emma + PRP_DEST_Y_PTR);
327         writel(p_out + d_size, pcdev->base_emma + PRP_DEST_CB_PTR);
328         writel(p_out + d_size + (d_size >> 2),
329                pcdev->base_emma + PRP_DEST_CR_PTR);
330         writel(PRP_SIZE_WIDTH(d_width) | PRP_SIZE_HEIGHT(d_height),
331                pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE);
332 
333         /* IRQ configuration */
334         tmp = readl(pcdev->base_emma + PRP_INTR_CNTL);
335         writel(tmp | PRP_INTR_RDERR |
336                 PRP_INTR_CH2WERR |
337                 PRP_INTR_CH2FC,
338                 pcdev->base_emma + PRP_INTR_CNTL);
339 
340         emmaprp_dump_regs(pcdev);
341 
342         /* Enable transfer */
343         tmp = readl(pcdev->base_emma + PRP_CNTL);
344         writel(tmp | PRP_CNTL_CH2_OUT_YUV420 |
345                 PRP_CNTL_DATA_IN_YUV422 |
346                 PRP_CNTL_CH2EN,
347                 pcdev->base_emma + PRP_CNTL);
348 }
349 
350 static irqreturn_t emmaprp_irq(int irq_emma, void *data)
351 {
352         struct emmaprp_dev *pcdev = data;
353         struct emmaprp_ctx *curr_ctx;
354         struct vb2_buffer *src_vb, *dst_vb;
355         unsigned long flags;
356         u32 irqst;
357 
358         /* Check irq flags and clear irq */
359         irqst = readl(pcdev->base_emma + PRP_INTRSTATUS);
360         writel(irqst, pcdev->base_emma + PRP_INTRSTATUS);
361         dprintk(pcdev, "irqst = 0x%08x\n", irqst);
362 
363         curr_ctx = v4l2_m2m_get_curr_priv(pcdev->m2m_dev);
364         if (curr_ctx == NULL) {
365                 pr_err("Instance released before the end of transaction\n");
366                 return IRQ_HANDLED;
367         }
368 
369         if (!curr_ctx->aborting) {
370                 if ((irqst & PRP_INTR_ST_RDERR) ||
371                 (irqst & PRP_INTR_ST_CH2WERR)) {
372                         pr_err("PrP bus error occurred, this transfer is probably corrupted\n");
373                         writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
374                 } else if (irqst & PRP_INTR_ST_CH2B1CI) { /* buffer ready */
375                         src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
376                         dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
377 
378                         dst_vb->v4l2_buf.timestamp = src_vb->v4l2_buf.timestamp;
379                         dst_vb->v4l2_buf.flags &=
380                                 ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
381                         dst_vb->v4l2_buf.flags |=
382                                 src_vb->v4l2_buf.flags
383                                 & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
384                         dst_vb->v4l2_buf.timecode = src_vb->v4l2_buf.timecode;
385 
386                         spin_lock_irqsave(&pcdev->irqlock, flags);
387                         v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
388                         v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
389                         spin_unlock_irqrestore(&pcdev->irqlock, flags);
390                 }
391         }
392 
393         v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->m2m_ctx);
394         return IRQ_HANDLED;
395 }
396 
397 /*
398  * video ioctls
399  */
400 static int vidioc_querycap(struct file *file, void *priv,
401                            struct v4l2_capability *cap)
402 {
403         strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
404         strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
405         /*
406          * This is only a mem-to-mem video device. The capture and output
407          * device capability flags are left only for backward compatibility
408          * and are scheduled for removal.
409          */
410         cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
411                             V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
412         return 0;
413 }
414 
415 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
416 {
417         int i, num;
418         struct emmaprp_fmt *fmt;
419 
420         num = 0;
421 
422         for (i = 0; i < NUM_FORMATS; ++i) {
423                 if (formats[i].types & type) {
424                         /* index-th format of type type found ? */
425                         if (num == f->index)
426                                 break;
427                         /* Correct type but haven't reached our index yet,
428                          * just increment per-type index */
429                         ++num;
430                 }
431         }
432 
433         if (i < NUM_FORMATS) {
434                 /* Format found */
435                 fmt = &formats[i];
436                 strlcpy(f->description, fmt->name, sizeof(f->description) - 1);
437                 f->pixelformat = fmt->fourcc;
438                 return 0;
439         }
440 
441         /* Format not found */
442         return -EINVAL;
443 }
444 
445 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
446                                    struct v4l2_fmtdesc *f)
447 {
448         return enum_fmt(f, MEM2MEM_CAPTURE);
449 }
450 
451 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
452                                    struct v4l2_fmtdesc *f)
453 {
454         return enum_fmt(f, MEM2MEM_OUTPUT);
455 }
456 
457 static int vidioc_g_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
458 {
459         struct vb2_queue *vq;
460         struct emmaprp_q_data *q_data;
461 
462         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
463         if (!vq)
464                 return -EINVAL;
465 
466         q_data = get_q_data(ctx, f->type);
467 
468         f->fmt.pix.width        = q_data->width;
469         f->fmt.pix.height       = q_data->height;
470         f->fmt.pix.field        = V4L2_FIELD_NONE;
471         f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
472         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
473                 f->fmt.pix.bytesperline = q_data->width * 3 / 2;
474         else /* YUYV */
475                 f->fmt.pix.bytesperline = q_data->width * 2;
476         f->fmt.pix.sizeimage    = q_data->sizeimage;
477 
478         return 0;
479 }
480 
481 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
482                                 struct v4l2_format *f)
483 {
484         return vidioc_g_fmt(priv, f);
485 }
486 
487 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
488                                 struct v4l2_format *f)
489 {
490         return vidioc_g_fmt(priv, f);
491 }
492 
493 static int vidioc_try_fmt(struct v4l2_format *f)
494 {
495         enum v4l2_field field;
496 
497 
498         if (!find_format(f))
499                 return -EINVAL;
500 
501         field = f->fmt.pix.field;
502         if (field == V4L2_FIELD_ANY)
503                 field = V4L2_FIELD_NONE;
504         else if (V4L2_FIELD_NONE != field)
505                 return -EINVAL;
506 
507         /* V4L2 specification suggests the driver corrects the format struct
508          * if any of the dimensions is unsupported */
509         f->fmt.pix.field = field;
510 
511         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
512                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
513                                       W_ALIGN_YUV420, &f->fmt.pix.height,
514                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
515                 f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
516         } else {
517                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
518                                       W_ALIGN_OTHERS, &f->fmt.pix.height,
519                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
520                 f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
521         }
522         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
523 
524         return 0;
525 }
526 
527 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
528                                   struct v4l2_format *f)
529 {
530         struct emmaprp_fmt *fmt;
531         struct emmaprp_ctx *ctx = priv;
532 
533         fmt = find_format(f);
534         if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
535                 v4l2_err(&ctx->dev->v4l2_dev,
536                          "Fourcc format (0x%08x) invalid.\n",
537                          f->fmt.pix.pixelformat);
538                 return -EINVAL;
539         }
540 
541         return vidioc_try_fmt(f);
542 }
543 
544 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
545                                   struct v4l2_format *f)
546 {
547         struct emmaprp_fmt *fmt;
548         struct emmaprp_ctx *ctx = priv;
549 
550         fmt = find_format(f);
551         if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
552                 v4l2_err(&ctx->dev->v4l2_dev,
553                          "Fourcc format (0x%08x) invalid.\n",
554                          f->fmt.pix.pixelformat);
555                 return -EINVAL;
556         }
557 
558         return vidioc_try_fmt(f);
559 }
560 
561 static int vidioc_s_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
562 {
563         struct emmaprp_q_data *q_data;
564         struct vb2_queue *vq;
565         int ret;
566 
567         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
568         if (!vq)
569                 return -EINVAL;
570 
571         q_data = get_q_data(ctx, f->type);
572         if (!q_data)
573                 return -EINVAL;
574 
575         if (vb2_is_busy(vq)) {
576                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
577                 return -EBUSY;
578         }
579 
580         ret = vidioc_try_fmt(f);
581         if (ret)
582                 return ret;
583 
584         q_data->fmt             = find_format(f);
585         q_data->width           = f->fmt.pix.width;
586         q_data->height          = f->fmt.pix.height;
587         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
588                 q_data->sizeimage = q_data->width * q_data->height * 3 / 2;
589         else /* YUYV */
590                 q_data->sizeimage = q_data->width * q_data->height * 2;
591 
592         dprintk(ctx->dev,
593                 "Setting format for type %d, wxh: %dx%d, fmt: %d\n",
594                 f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
595 
596         return 0;
597 }
598 
599 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
600                                 struct v4l2_format *f)
601 {
602         int ret;
603 
604         ret = vidioc_try_fmt_vid_cap(file, priv, f);
605         if (ret)
606                 return ret;
607 
608         return vidioc_s_fmt(priv, f);
609 }
610 
611 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
612                                 struct v4l2_format *f)
613 {
614         int ret;
615 
616         ret = vidioc_try_fmt_vid_out(file, priv, f);
617         if (ret)
618                 return ret;
619 
620         return vidioc_s_fmt(priv, f);
621 }
622 
623 static int vidioc_reqbufs(struct file *file, void *priv,
624                           struct v4l2_requestbuffers *reqbufs)
625 {
626         struct emmaprp_ctx *ctx = priv;
627 
628         return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
629 }
630 
631 static int vidioc_querybuf(struct file *file, void *priv,
632                            struct v4l2_buffer *buf)
633 {
634         struct emmaprp_ctx *ctx = priv;
635 
636         return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
637 }
638 
639 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
640 {
641         struct emmaprp_ctx *ctx = priv;
642 
643         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
644 }
645 
646 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
647 {
648         struct emmaprp_ctx *ctx = priv;
649 
650         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
651 }
652 
653 static int vidioc_streamon(struct file *file, void *priv,
654                            enum v4l2_buf_type type)
655 {
656         struct emmaprp_ctx *ctx = priv;
657 
658         return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
659 }
660 
661 static int vidioc_streamoff(struct file *file, void *priv,
662                             enum v4l2_buf_type type)
663 {
664         struct emmaprp_ctx *ctx = priv;
665 
666         return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
667 }
668 
669 static const struct v4l2_ioctl_ops emmaprp_ioctl_ops = {
670         .vidioc_querycap        = vidioc_querycap,
671 
672         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
673         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
674         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
675         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
676 
677         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
678         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
679         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
680         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
681 
682         .vidioc_reqbufs         = vidioc_reqbufs,
683         .vidioc_querybuf        = vidioc_querybuf,
684 
685         .vidioc_qbuf            = vidioc_qbuf,
686         .vidioc_dqbuf           = vidioc_dqbuf,
687 
688         .vidioc_streamon        = vidioc_streamon,
689         .vidioc_streamoff       = vidioc_streamoff,
690 };
691 
692 
693 /*
694  * Queue operations
695  */
696 static int emmaprp_queue_setup(struct vb2_queue *vq,
697                                 const struct v4l2_format *fmt,
698                                 unsigned int *nbuffers, unsigned int *nplanes,
699                                 unsigned int sizes[], void *alloc_ctxs[])
700 {
701         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vq);
702         struct emmaprp_q_data *q_data;
703         unsigned int size, count = *nbuffers;
704 
705         q_data = get_q_data(ctx, vq->type);
706 
707         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
708                 size = q_data->width * q_data->height * 3 / 2;
709         else
710                 size = q_data->width * q_data->height * 2;
711 
712         while (size * count > MEM2MEM_VID_MEM_LIMIT)
713                 (count)--;
714 
715         *nplanes = 1;
716         *nbuffers = count;
717         sizes[0] = size;
718 
719         alloc_ctxs[0] = ctx->dev->alloc_ctx;
720 
721         dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
722 
723         return 0;
724 }
725 
726 static int emmaprp_buf_prepare(struct vb2_buffer *vb)
727 {
728         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
729         struct emmaprp_q_data *q_data;
730 
731         dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
732 
733         q_data = get_q_data(ctx, vb->vb2_queue->type);
734 
735         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
736                 dprintk(ctx->dev, "%s data will not fit into plane"
737                                   "(%lu < %lu)\n", __func__,
738                                   vb2_plane_size(vb, 0),
739                                   (long)q_data->sizeimage);
740                 return -EINVAL;
741         }
742 
743         vb2_set_plane_payload(vb, 0, q_data->sizeimage);
744 
745         return 0;
746 }
747 
748 static void emmaprp_buf_queue(struct vb2_buffer *vb)
749 {
750         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
751         v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
752 }
753 
754 static struct vb2_ops emmaprp_qops = {
755         .queue_setup     = emmaprp_queue_setup,
756         .buf_prepare     = emmaprp_buf_prepare,
757         .buf_queue       = emmaprp_buf_queue,
758 };
759 
760 static int queue_init(void *priv, struct vb2_queue *src_vq,
761                       struct vb2_queue *dst_vq)
762 {
763         struct emmaprp_ctx *ctx = priv;
764         int ret;
765 
766         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
767         src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
768         src_vq->drv_priv = ctx;
769         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
770         src_vq->ops = &emmaprp_qops;
771         src_vq->mem_ops = &vb2_dma_contig_memops;
772         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
773 
774         ret = vb2_queue_init(src_vq);
775         if (ret)
776                 return ret;
777 
778         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
779         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
780         dst_vq->drv_priv = ctx;
781         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
782         dst_vq->ops = &emmaprp_qops;
783         dst_vq->mem_ops = &vb2_dma_contig_memops;
784         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
785 
786         return vb2_queue_init(dst_vq);
787 }
788 
789 /*
790  * File operations
791  */
792 static int emmaprp_open(struct file *file)
793 {
794         struct emmaprp_dev *pcdev = video_drvdata(file);
795         struct emmaprp_ctx *ctx;
796 
797         ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
798         if (!ctx)
799                 return -ENOMEM;
800 
801         file->private_data = ctx;
802         ctx->dev = pcdev;
803 
804         if (mutex_lock_interruptible(&pcdev->dev_mutex)) {
805                 kfree(ctx);
806                 return -ERESTARTSYS;
807         }
808 
809         ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
810 
811         if (IS_ERR(ctx->m2m_ctx)) {
812                 int ret = PTR_ERR(ctx->m2m_ctx);
813 
814                 mutex_unlock(&pcdev->dev_mutex);
815                 kfree(ctx);
816                 return ret;
817         }
818 
819         clk_prepare_enable(pcdev->clk_emma_ipg);
820         clk_prepare_enable(pcdev->clk_emma_ahb);
821         ctx->q_data[V4L2_M2M_SRC].fmt = &formats[1];
822         ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
823         mutex_unlock(&pcdev->dev_mutex);
824 
825         dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
826 
827         return 0;
828 }
829 
830 static int emmaprp_release(struct file *file)
831 {
832         struct emmaprp_dev *pcdev = video_drvdata(file);
833         struct emmaprp_ctx *ctx = file->private_data;
834 
835         dprintk(pcdev, "Releasing instance %p\n", ctx);
836 
837         mutex_lock(&pcdev->dev_mutex);
838         clk_disable_unprepare(pcdev->clk_emma_ahb);
839         clk_disable_unprepare(pcdev->clk_emma_ipg);
840         v4l2_m2m_ctx_release(ctx->m2m_ctx);
841         mutex_unlock(&pcdev->dev_mutex);
842         kfree(ctx);
843 
844         return 0;
845 }
846 
847 static unsigned int emmaprp_poll(struct file *file,
848                                  struct poll_table_struct *wait)
849 {
850         struct emmaprp_dev *pcdev = video_drvdata(file);
851         struct emmaprp_ctx *ctx = file->private_data;
852         unsigned int res;
853 
854         mutex_lock(&pcdev->dev_mutex);
855         res = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
856         mutex_unlock(&pcdev->dev_mutex);
857         return res;
858 }
859 
860 static int emmaprp_mmap(struct file *file, struct vm_area_struct *vma)
861 {
862         struct emmaprp_dev *pcdev = video_drvdata(file);
863         struct emmaprp_ctx *ctx = file->private_data;
864         int ret;
865 
866         if (mutex_lock_interruptible(&pcdev->dev_mutex))
867                 return -ERESTARTSYS;
868         ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
869         mutex_unlock(&pcdev->dev_mutex);
870         return ret;
871 }
872 
873 static const struct v4l2_file_operations emmaprp_fops = {
874         .owner          = THIS_MODULE,
875         .open           = emmaprp_open,
876         .release        = emmaprp_release,
877         .poll           = emmaprp_poll,
878         .unlocked_ioctl = video_ioctl2,
879         .mmap           = emmaprp_mmap,
880 };
881 
882 static struct video_device emmaprp_videodev = {
883         .name           = MEM2MEM_NAME,
884         .fops           = &emmaprp_fops,
885         .ioctl_ops      = &emmaprp_ioctl_ops,
886         .minor          = -1,
887         .release        = video_device_release,
888         .vfl_dir        = VFL_DIR_M2M,
889 };
890 
891 static struct v4l2_m2m_ops m2m_ops = {
892         .device_run     = emmaprp_device_run,
893         .job_abort      = emmaprp_job_abort,
894         .lock           = emmaprp_lock,
895         .unlock         = emmaprp_unlock,
896 };
897 
898 static int emmaprp_probe(struct platform_device *pdev)
899 {
900         struct emmaprp_dev *pcdev;
901         struct video_device *vfd;
902         struct resource *res;
903         int irq, ret;
904 
905         pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
906         if (!pcdev)
907                 return -ENOMEM;
908 
909         spin_lock_init(&pcdev->irqlock);
910 
911         pcdev->clk_emma_ipg = devm_clk_get(&pdev->dev, "ipg");
912         if (IS_ERR(pcdev->clk_emma_ipg)) {
913                 return PTR_ERR(pcdev->clk_emma_ipg);
914         }
915 
916         pcdev->clk_emma_ahb = devm_clk_get(&pdev->dev, "ahb");
917         if (IS_ERR(pcdev->clk_emma_ahb))
918                 return PTR_ERR(pcdev->clk_emma_ahb);
919 
920         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
921         pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res);
922         if (IS_ERR(pcdev->base_emma))
923                 return PTR_ERR(pcdev->base_emma);
924 
925         ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
926         if (ret)
927                 return ret;
928 
929         mutex_init(&pcdev->dev_mutex);
930 
931         vfd = video_device_alloc();
932         if (!vfd) {
933                 v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n");
934                 ret = -ENOMEM;
935                 goto unreg_dev;
936         }
937 
938         *vfd = emmaprp_videodev;
939         vfd->lock = &pcdev->dev_mutex;
940         vfd->v4l2_dev = &pcdev->v4l2_dev;
941 
942         video_set_drvdata(vfd, pcdev);
943         snprintf(vfd->name, sizeof(vfd->name), "%s", emmaprp_videodev.name);
944         pcdev->vfd = vfd;
945         v4l2_info(&pcdev->v4l2_dev, EMMAPRP_MODULE_NAME
946                         " Device registered as /dev/video%d\n", vfd->num);
947 
948         platform_set_drvdata(pdev, pcdev);
949 
950         irq = platform_get_irq(pdev, 0);
951         ret = devm_request_irq(&pdev->dev, irq, emmaprp_irq, 0,
952                                dev_name(&pdev->dev), pcdev);
953         if (ret)
954                 goto rel_vdev;
955 
956         pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
957         if (IS_ERR(pcdev->alloc_ctx)) {
958                 v4l2_err(&pcdev->v4l2_dev, "Failed to alloc vb2 context\n");
959                 ret = PTR_ERR(pcdev->alloc_ctx);
960                 goto rel_vdev;
961         }
962 
963         pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
964         if (IS_ERR(pcdev->m2m_dev)) {
965                 v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
966                 ret = PTR_ERR(pcdev->m2m_dev);
967                 goto rel_ctx;
968         }
969 
970         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
971         if (ret) {
972                 v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
973                 goto rel_m2m;
974         }
975 
976         return 0;
977 
978 
979 rel_m2m:
980         v4l2_m2m_release(pcdev->m2m_dev);
981 rel_ctx:
982         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
983 rel_vdev:
984         video_device_release(vfd);
985 unreg_dev:
986         v4l2_device_unregister(&pcdev->v4l2_dev);
987 
988         mutex_destroy(&pcdev->dev_mutex);
989 
990         return ret;
991 }
992 
993 static int emmaprp_remove(struct platform_device *pdev)
994 {
995         struct emmaprp_dev *pcdev = platform_get_drvdata(pdev);
996 
997         v4l2_info(&pcdev->v4l2_dev, "Removing " EMMAPRP_MODULE_NAME);
998 
999         video_unregister_device(pcdev->vfd);
1000         v4l2_m2m_release(pcdev->m2m_dev);
1001         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
1002         v4l2_device_unregister(&pcdev->v4l2_dev);
1003         mutex_destroy(&pcdev->dev_mutex);
1004 
1005         return 0;
1006 }
1007 
1008 static struct platform_driver emmaprp_pdrv = {
1009         .probe          = emmaprp_probe,
1010         .remove         = emmaprp_remove,
1011         .driver         = {
1012                 .name   = MEM2MEM_NAME,
1013                 .owner  = THIS_MODULE,
1014         },
1015 };
1016 module_platform_driver(emmaprp_pdrv);
1017 

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