Version:  2.0.40 2.2.26 2.4.37 3.8 3.9 3.10 3.11 3.12 3.13 3.14 3.15 3.16 3.17 3.18 3.19 4.0 4.1 4.2 4.3 4.4

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_v4l2_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->timestamp = src_vb->timestamp;
379                         dst_vb->flags &=
380                                 ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
381                         dst_vb->flags |=
382                                 src_vb->flags
383                                 & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
384                         dst_vb->timecode = src_vb->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         cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
406         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
407         return 0;
408 }
409 
410 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
411 {
412         int i, num;
413         struct emmaprp_fmt *fmt;
414 
415         num = 0;
416 
417         for (i = 0; i < NUM_FORMATS; ++i) {
418                 if (formats[i].types & type) {
419                         /* index-th format of type type found ? */
420                         if (num == f->index)
421                                 break;
422                         /* Correct type but haven't reached our index yet,
423                          * just increment per-type index */
424                         ++num;
425                 }
426         }
427 
428         if (i < NUM_FORMATS) {
429                 /* Format found */
430                 fmt = &formats[i];
431                 strlcpy(f->description, fmt->name, sizeof(f->description) - 1);
432                 f->pixelformat = fmt->fourcc;
433                 return 0;
434         }
435 
436         /* Format not found */
437         return -EINVAL;
438 }
439 
440 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
441                                    struct v4l2_fmtdesc *f)
442 {
443         return enum_fmt(f, MEM2MEM_CAPTURE);
444 }
445 
446 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
447                                    struct v4l2_fmtdesc *f)
448 {
449         return enum_fmt(f, MEM2MEM_OUTPUT);
450 }
451 
452 static int vidioc_g_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
453 {
454         struct vb2_queue *vq;
455         struct emmaprp_q_data *q_data;
456 
457         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
458         if (!vq)
459                 return -EINVAL;
460 
461         q_data = get_q_data(ctx, f->type);
462 
463         f->fmt.pix.width        = q_data->width;
464         f->fmt.pix.height       = q_data->height;
465         f->fmt.pix.field        = V4L2_FIELD_NONE;
466         f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
467         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
468                 f->fmt.pix.bytesperline = q_data->width * 3 / 2;
469         else /* YUYV */
470                 f->fmt.pix.bytesperline = q_data->width * 2;
471         f->fmt.pix.sizeimage    = q_data->sizeimage;
472 
473         return 0;
474 }
475 
476 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
477                                 struct v4l2_format *f)
478 {
479         return vidioc_g_fmt(priv, f);
480 }
481 
482 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
483                                 struct v4l2_format *f)
484 {
485         return vidioc_g_fmt(priv, f);
486 }
487 
488 static int vidioc_try_fmt(struct v4l2_format *f)
489 {
490         enum v4l2_field field;
491 
492 
493         if (!find_format(f))
494                 return -EINVAL;
495 
496         field = f->fmt.pix.field;
497         if (field == V4L2_FIELD_ANY)
498                 field = V4L2_FIELD_NONE;
499         else if (V4L2_FIELD_NONE != field)
500                 return -EINVAL;
501 
502         /* V4L2 specification suggests the driver corrects the format struct
503          * if any of the dimensions is unsupported */
504         f->fmt.pix.field = field;
505 
506         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
507                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
508                                       W_ALIGN_YUV420, &f->fmt.pix.height,
509                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
510                 f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
511         } else {
512                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
513                                       W_ALIGN_OTHERS, &f->fmt.pix.height,
514                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
515                 f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
516         }
517         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
518 
519         return 0;
520 }
521 
522 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
523                                   struct v4l2_format *f)
524 {
525         struct emmaprp_fmt *fmt;
526         struct emmaprp_ctx *ctx = priv;
527 
528         fmt = find_format(f);
529         if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
530                 v4l2_err(&ctx->dev->v4l2_dev,
531                          "Fourcc format (0x%08x) invalid.\n",
532                          f->fmt.pix.pixelformat);
533                 return -EINVAL;
534         }
535 
536         return vidioc_try_fmt(f);
537 }
538 
539 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
540                                   struct v4l2_format *f)
541 {
542         struct emmaprp_fmt *fmt;
543         struct emmaprp_ctx *ctx = priv;
544 
545         fmt = find_format(f);
546         if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
547                 v4l2_err(&ctx->dev->v4l2_dev,
548                          "Fourcc format (0x%08x) invalid.\n",
549                          f->fmt.pix.pixelformat);
550                 return -EINVAL;
551         }
552 
553         return vidioc_try_fmt(f);
554 }
555 
556 static int vidioc_s_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
557 {
558         struct emmaprp_q_data *q_data;
559         struct vb2_queue *vq;
560         int ret;
561 
562         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
563         if (!vq)
564                 return -EINVAL;
565 
566         q_data = get_q_data(ctx, f->type);
567         if (!q_data)
568                 return -EINVAL;
569 
570         if (vb2_is_busy(vq)) {
571                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
572                 return -EBUSY;
573         }
574 
575         ret = vidioc_try_fmt(f);
576         if (ret)
577                 return ret;
578 
579         q_data->fmt             = find_format(f);
580         q_data->width           = f->fmt.pix.width;
581         q_data->height          = f->fmt.pix.height;
582         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
583                 q_data->sizeimage = q_data->width * q_data->height * 3 / 2;
584         else /* YUYV */
585                 q_data->sizeimage = q_data->width * q_data->height * 2;
586 
587         dprintk(ctx->dev,
588                 "Setting format for type %d, wxh: %dx%d, fmt: %d\n",
589                 f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
590 
591         return 0;
592 }
593 
594 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
595                                 struct v4l2_format *f)
596 {
597         int ret;
598 
599         ret = vidioc_try_fmt_vid_cap(file, priv, f);
600         if (ret)
601                 return ret;
602 
603         return vidioc_s_fmt(priv, f);
604 }
605 
606 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
607                                 struct v4l2_format *f)
608 {
609         int ret;
610 
611         ret = vidioc_try_fmt_vid_out(file, priv, f);
612         if (ret)
613                 return ret;
614 
615         return vidioc_s_fmt(priv, f);
616 }
617 
618 static int vidioc_reqbufs(struct file *file, void *priv,
619                           struct v4l2_requestbuffers *reqbufs)
620 {
621         struct emmaprp_ctx *ctx = priv;
622 
623         return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
624 }
625 
626 static int vidioc_querybuf(struct file *file, void *priv,
627                            struct v4l2_buffer *buf)
628 {
629         struct emmaprp_ctx *ctx = priv;
630 
631         return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
632 }
633 
634 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
635 {
636         struct emmaprp_ctx *ctx = priv;
637 
638         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
639 }
640 
641 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
642 {
643         struct emmaprp_ctx *ctx = priv;
644 
645         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
646 }
647 
648 static int vidioc_streamon(struct file *file, void *priv,
649                            enum v4l2_buf_type type)
650 {
651         struct emmaprp_ctx *ctx = priv;
652 
653         return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
654 }
655 
656 static int vidioc_streamoff(struct file *file, void *priv,
657                             enum v4l2_buf_type type)
658 {
659         struct emmaprp_ctx *ctx = priv;
660 
661         return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
662 }
663 
664 static const struct v4l2_ioctl_ops emmaprp_ioctl_ops = {
665         .vidioc_querycap        = vidioc_querycap,
666 
667         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
668         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
669         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
670         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
671 
672         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
673         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
674         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
675         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
676 
677         .vidioc_reqbufs         = vidioc_reqbufs,
678         .vidioc_querybuf        = vidioc_querybuf,
679 
680         .vidioc_qbuf            = vidioc_qbuf,
681         .vidioc_dqbuf           = vidioc_dqbuf,
682 
683         .vidioc_streamon        = vidioc_streamon,
684         .vidioc_streamoff       = vidioc_streamoff,
685 };
686 
687 
688 /*
689  * Queue operations
690  */
691 static int emmaprp_queue_setup(struct vb2_queue *vq,
692                                 const void *parg,
693                                 unsigned int *nbuffers, unsigned int *nplanes,
694                                 unsigned int sizes[], void *alloc_ctxs[])
695 {
696         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vq);
697         struct emmaprp_q_data *q_data;
698         unsigned int size, count = *nbuffers;
699 
700         q_data = get_q_data(ctx, vq->type);
701 
702         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
703                 size = q_data->width * q_data->height * 3 / 2;
704         else
705                 size = q_data->width * q_data->height * 2;
706 
707         while (size * count > MEM2MEM_VID_MEM_LIMIT)
708                 (count)--;
709 
710         *nplanes = 1;
711         *nbuffers = count;
712         sizes[0] = size;
713 
714         alloc_ctxs[0] = ctx->dev->alloc_ctx;
715 
716         dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
717 
718         return 0;
719 }
720 
721 static int emmaprp_buf_prepare(struct vb2_buffer *vb)
722 {
723         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
724         struct emmaprp_q_data *q_data;
725 
726         dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
727 
728         q_data = get_q_data(ctx, vb->vb2_queue->type);
729 
730         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
731                 dprintk(ctx->dev, "%s data will not fit into plane"
732                                   "(%lu < %lu)\n", __func__,
733                                   vb2_plane_size(vb, 0),
734                                   (long)q_data->sizeimage);
735                 return -EINVAL;
736         }
737 
738         vb2_set_plane_payload(vb, 0, q_data->sizeimage);
739 
740         return 0;
741 }
742 
743 static void emmaprp_buf_queue(struct vb2_buffer *vb)
744 {
745         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
746         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
747         v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
748 }
749 
750 static struct vb2_ops emmaprp_qops = {
751         .queue_setup     = emmaprp_queue_setup,
752         .buf_prepare     = emmaprp_buf_prepare,
753         .buf_queue       = emmaprp_buf_queue,
754 };
755 
756 static int queue_init(void *priv, struct vb2_queue *src_vq,
757                       struct vb2_queue *dst_vq)
758 {
759         struct emmaprp_ctx *ctx = priv;
760         int ret;
761 
762         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
763         src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
764         src_vq->drv_priv = ctx;
765         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
766         src_vq->ops = &emmaprp_qops;
767         src_vq->mem_ops = &vb2_dma_contig_memops;
768         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
769 
770         ret = vb2_queue_init(src_vq);
771         if (ret)
772                 return ret;
773 
774         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
775         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
776         dst_vq->drv_priv = ctx;
777         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
778         dst_vq->ops = &emmaprp_qops;
779         dst_vq->mem_ops = &vb2_dma_contig_memops;
780         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
781 
782         return vb2_queue_init(dst_vq);
783 }
784 
785 /*
786  * File operations
787  */
788 static int emmaprp_open(struct file *file)
789 {
790         struct emmaprp_dev *pcdev = video_drvdata(file);
791         struct emmaprp_ctx *ctx;
792 
793         ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
794         if (!ctx)
795                 return -ENOMEM;
796 
797         file->private_data = ctx;
798         ctx->dev = pcdev;
799 
800         if (mutex_lock_interruptible(&pcdev->dev_mutex)) {
801                 kfree(ctx);
802                 return -ERESTARTSYS;
803         }
804 
805         ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
806 
807         if (IS_ERR(ctx->m2m_ctx)) {
808                 int ret = PTR_ERR(ctx->m2m_ctx);
809 
810                 mutex_unlock(&pcdev->dev_mutex);
811                 kfree(ctx);
812                 return ret;
813         }
814 
815         clk_prepare_enable(pcdev->clk_emma_ipg);
816         clk_prepare_enable(pcdev->clk_emma_ahb);
817         ctx->q_data[V4L2_M2M_SRC].fmt = &formats[1];
818         ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
819         mutex_unlock(&pcdev->dev_mutex);
820 
821         dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
822 
823         return 0;
824 }
825 
826 static int emmaprp_release(struct file *file)
827 {
828         struct emmaprp_dev *pcdev = video_drvdata(file);
829         struct emmaprp_ctx *ctx = file->private_data;
830 
831         dprintk(pcdev, "Releasing instance %p\n", ctx);
832 
833         mutex_lock(&pcdev->dev_mutex);
834         clk_disable_unprepare(pcdev->clk_emma_ahb);
835         clk_disable_unprepare(pcdev->clk_emma_ipg);
836         v4l2_m2m_ctx_release(ctx->m2m_ctx);
837         mutex_unlock(&pcdev->dev_mutex);
838         kfree(ctx);
839 
840         return 0;
841 }
842 
843 static unsigned int emmaprp_poll(struct file *file,
844                                  struct poll_table_struct *wait)
845 {
846         struct emmaprp_dev *pcdev = video_drvdata(file);
847         struct emmaprp_ctx *ctx = file->private_data;
848         unsigned int res;
849 
850         mutex_lock(&pcdev->dev_mutex);
851         res = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
852         mutex_unlock(&pcdev->dev_mutex);
853         return res;
854 }
855 
856 static int emmaprp_mmap(struct file *file, struct vm_area_struct *vma)
857 {
858         struct emmaprp_dev *pcdev = video_drvdata(file);
859         struct emmaprp_ctx *ctx = file->private_data;
860         int ret;
861 
862         if (mutex_lock_interruptible(&pcdev->dev_mutex))
863                 return -ERESTARTSYS;
864         ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
865         mutex_unlock(&pcdev->dev_mutex);
866         return ret;
867 }
868 
869 static const struct v4l2_file_operations emmaprp_fops = {
870         .owner          = THIS_MODULE,
871         .open           = emmaprp_open,
872         .release        = emmaprp_release,
873         .poll           = emmaprp_poll,
874         .unlocked_ioctl = video_ioctl2,
875         .mmap           = emmaprp_mmap,
876 };
877 
878 static struct video_device emmaprp_videodev = {
879         .name           = MEM2MEM_NAME,
880         .fops           = &emmaprp_fops,
881         .ioctl_ops      = &emmaprp_ioctl_ops,
882         .minor          = -1,
883         .release        = video_device_release,
884         .vfl_dir        = VFL_DIR_M2M,
885 };
886 
887 static struct v4l2_m2m_ops m2m_ops = {
888         .device_run     = emmaprp_device_run,
889         .job_abort      = emmaprp_job_abort,
890         .lock           = emmaprp_lock,
891         .unlock         = emmaprp_unlock,
892 };
893 
894 static int emmaprp_probe(struct platform_device *pdev)
895 {
896         struct emmaprp_dev *pcdev;
897         struct video_device *vfd;
898         struct resource *res;
899         int irq, ret;
900 
901         pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
902         if (!pcdev)
903                 return -ENOMEM;
904 
905         spin_lock_init(&pcdev->irqlock);
906 
907         pcdev->clk_emma_ipg = devm_clk_get(&pdev->dev, "ipg");
908         if (IS_ERR(pcdev->clk_emma_ipg)) {
909                 return PTR_ERR(pcdev->clk_emma_ipg);
910         }
911 
912         pcdev->clk_emma_ahb = devm_clk_get(&pdev->dev, "ahb");
913         if (IS_ERR(pcdev->clk_emma_ahb))
914                 return PTR_ERR(pcdev->clk_emma_ahb);
915 
916         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
917         pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res);
918         if (IS_ERR(pcdev->base_emma))
919                 return PTR_ERR(pcdev->base_emma);
920 
921         ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
922         if (ret)
923                 return ret;
924 
925         mutex_init(&pcdev->dev_mutex);
926 
927         vfd = video_device_alloc();
928         if (!vfd) {
929                 v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n");
930                 ret = -ENOMEM;
931                 goto unreg_dev;
932         }
933 
934         *vfd = emmaprp_videodev;
935         vfd->lock = &pcdev->dev_mutex;
936         vfd->v4l2_dev = &pcdev->v4l2_dev;
937 
938         video_set_drvdata(vfd, pcdev);
939         snprintf(vfd->name, sizeof(vfd->name), "%s", emmaprp_videodev.name);
940         pcdev->vfd = vfd;
941         v4l2_info(&pcdev->v4l2_dev, EMMAPRP_MODULE_NAME
942                         " Device registered as /dev/video%d\n", vfd->num);
943 
944         platform_set_drvdata(pdev, pcdev);
945 
946         irq = platform_get_irq(pdev, 0);
947         ret = devm_request_irq(&pdev->dev, irq, emmaprp_irq, 0,
948                                dev_name(&pdev->dev), pcdev);
949         if (ret)
950                 goto rel_vdev;
951 
952         pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
953         if (IS_ERR(pcdev->alloc_ctx)) {
954                 v4l2_err(&pcdev->v4l2_dev, "Failed to alloc vb2 context\n");
955                 ret = PTR_ERR(pcdev->alloc_ctx);
956                 goto rel_vdev;
957         }
958 
959         pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
960         if (IS_ERR(pcdev->m2m_dev)) {
961                 v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
962                 ret = PTR_ERR(pcdev->m2m_dev);
963                 goto rel_ctx;
964         }
965 
966         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
967         if (ret) {
968                 v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
969                 goto rel_m2m;
970         }
971 
972         return 0;
973 
974 
975 rel_m2m:
976         v4l2_m2m_release(pcdev->m2m_dev);
977 rel_ctx:
978         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
979 rel_vdev:
980         video_device_release(vfd);
981 unreg_dev:
982         v4l2_device_unregister(&pcdev->v4l2_dev);
983 
984         mutex_destroy(&pcdev->dev_mutex);
985 
986         return ret;
987 }
988 
989 static int emmaprp_remove(struct platform_device *pdev)
990 {
991         struct emmaprp_dev *pcdev = platform_get_drvdata(pdev);
992 
993         v4l2_info(&pcdev->v4l2_dev, "Removing " EMMAPRP_MODULE_NAME);
994 
995         video_unregister_device(pcdev->vfd);
996         v4l2_m2m_release(pcdev->m2m_dev);
997         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
998         v4l2_device_unregister(&pcdev->v4l2_dev);
999         mutex_destroy(&pcdev->dev_mutex);
1000 
1001         return 0;
1002 }
1003 
1004 static struct platform_driver emmaprp_pdrv = {
1005         .probe          = emmaprp_probe,
1006         .remove         = emmaprp_remove,
1007         .driver         = {
1008                 .name   = MEM2MEM_NAME,
1009         },
1010 };
1011 module_platform_driver(emmaprp_pdrv);
1012 

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