Version:  2.0.40 2.2.26 2.4.37 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 3.19 4.0 4.1

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         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 struct v4l2_format *fmt,
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 emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
746         v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
747 }
748 
749 static struct vb2_ops emmaprp_qops = {
750         .queue_setup     = emmaprp_queue_setup,
751         .buf_prepare     = emmaprp_buf_prepare,
752         .buf_queue       = emmaprp_buf_queue,
753 };
754 
755 static int queue_init(void *priv, struct vb2_queue *src_vq,
756                       struct vb2_queue *dst_vq)
757 {
758         struct emmaprp_ctx *ctx = priv;
759         int ret;
760 
761         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
762         src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
763         src_vq->drv_priv = ctx;
764         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
765         src_vq->ops = &emmaprp_qops;
766         src_vq->mem_ops = &vb2_dma_contig_memops;
767         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
768 
769         ret = vb2_queue_init(src_vq);
770         if (ret)
771                 return ret;
772 
773         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
774         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
775         dst_vq->drv_priv = ctx;
776         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
777         dst_vq->ops = &emmaprp_qops;
778         dst_vq->mem_ops = &vb2_dma_contig_memops;
779         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
780 
781         return vb2_queue_init(dst_vq);
782 }
783 
784 /*
785  * File operations
786  */
787 static int emmaprp_open(struct file *file)
788 {
789         struct emmaprp_dev *pcdev = video_drvdata(file);
790         struct emmaprp_ctx *ctx;
791 
792         ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
793         if (!ctx)
794                 return -ENOMEM;
795 
796         file->private_data = ctx;
797         ctx->dev = pcdev;
798 
799         if (mutex_lock_interruptible(&pcdev->dev_mutex)) {
800                 kfree(ctx);
801                 return -ERESTARTSYS;
802         }
803 
804         ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
805 
806         if (IS_ERR(ctx->m2m_ctx)) {
807                 int ret = PTR_ERR(ctx->m2m_ctx);
808 
809                 mutex_unlock(&pcdev->dev_mutex);
810                 kfree(ctx);
811                 return ret;
812         }
813 
814         clk_prepare_enable(pcdev->clk_emma_ipg);
815         clk_prepare_enable(pcdev->clk_emma_ahb);
816         ctx->q_data[V4L2_M2M_SRC].fmt = &formats[1];
817         ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
818         mutex_unlock(&pcdev->dev_mutex);
819 
820         dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
821 
822         return 0;
823 }
824 
825 static int emmaprp_release(struct file *file)
826 {
827         struct emmaprp_dev *pcdev = video_drvdata(file);
828         struct emmaprp_ctx *ctx = file->private_data;
829 
830         dprintk(pcdev, "Releasing instance %p\n", ctx);
831 
832         mutex_lock(&pcdev->dev_mutex);
833         clk_disable_unprepare(pcdev->clk_emma_ahb);
834         clk_disable_unprepare(pcdev->clk_emma_ipg);
835         v4l2_m2m_ctx_release(ctx->m2m_ctx);
836         mutex_unlock(&pcdev->dev_mutex);
837         kfree(ctx);
838 
839         return 0;
840 }
841 
842 static unsigned int emmaprp_poll(struct file *file,
843                                  struct poll_table_struct *wait)
844 {
845         struct emmaprp_dev *pcdev = video_drvdata(file);
846         struct emmaprp_ctx *ctx = file->private_data;
847         unsigned int res;
848 
849         mutex_lock(&pcdev->dev_mutex);
850         res = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
851         mutex_unlock(&pcdev->dev_mutex);
852         return res;
853 }
854 
855 static int emmaprp_mmap(struct file *file, struct vm_area_struct *vma)
856 {
857         struct emmaprp_dev *pcdev = video_drvdata(file);
858         struct emmaprp_ctx *ctx = file->private_data;
859         int ret;
860 
861         if (mutex_lock_interruptible(&pcdev->dev_mutex))
862                 return -ERESTARTSYS;
863         ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
864         mutex_unlock(&pcdev->dev_mutex);
865         return ret;
866 }
867 
868 static const struct v4l2_file_operations emmaprp_fops = {
869         .owner          = THIS_MODULE,
870         .open           = emmaprp_open,
871         .release        = emmaprp_release,
872         .poll           = emmaprp_poll,
873         .unlocked_ioctl = video_ioctl2,
874         .mmap           = emmaprp_mmap,
875 };
876 
877 static struct video_device emmaprp_videodev = {
878         .name           = MEM2MEM_NAME,
879         .fops           = &emmaprp_fops,
880         .ioctl_ops      = &emmaprp_ioctl_ops,
881         .minor          = -1,
882         .release        = video_device_release,
883         .vfl_dir        = VFL_DIR_M2M,
884 };
885 
886 static struct v4l2_m2m_ops m2m_ops = {
887         .device_run     = emmaprp_device_run,
888         .job_abort      = emmaprp_job_abort,
889         .lock           = emmaprp_lock,
890         .unlock         = emmaprp_unlock,
891 };
892 
893 static int emmaprp_probe(struct platform_device *pdev)
894 {
895         struct emmaprp_dev *pcdev;
896         struct video_device *vfd;
897         struct resource *res;
898         int irq, ret;
899 
900         pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
901         if (!pcdev)
902                 return -ENOMEM;
903 
904         spin_lock_init(&pcdev->irqlock);
905 
906         pcdev->clk_emma_ipg = devm_clk_get(&pdev->dev, "ipg");
907         if (IS_ERR(pcdev->clk_emma_ipg)) {
908                 return PTR_ERR(pcdev->clk_emma_ipg);
909         }
910 
911         pcdev->clk_emma_ahb = devm_clk_get(&pdev->dev, "ahb");
912         if (IS_ERR(pcdev->clk_emma_ahb))
913                 return PTR_ERR(pcdev->clk_emma_ahb);
914 
915         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
916         pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res);
917         if (IS_ERR(pcdev->base_emma))
918                 return PTR_ERR(pcdev->base_emma);
919 
920         ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
921         if (ret)
922                 return ret;
923 
924         mutex_init(&pcdev->dev_mutex);
925 
926         vfd = video_device_alloc();
927         if (!vfd) {
928                 v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n");
929                 ret = -ENOMEM;
930                 goto unreg_dev;
931         }
932 
933         *vfd = emmaprp_videodev;
934         vfd->lock = &pcdev->dev_mutex;
935         vfd->v4l2_dev = &pcdev->v4l2_dev;
936 
937         video_set_drvdata(vfd, pcdev);
938         snprintf(vfd->name, sizeof(vfd->name), "%s", emmaprp_videodev.name);
939         pcdev->vfd = vfd;
940         v4l2_info(&pcdev->v4l2_dev, EMMAPRP_MODULE_NAME
941                         " Device registered as /dev/video%d\n", vfd->num);
942 
943         platform_set_drvdata(pdev, pcdev);
944 
945         irq = platform_get_irq(pdev, 0);
946         ret = devm_request_irq(&pdev->dev, irq, emmaprp_irq, 0,
947                                dev_name(&pdev->dev), pcdev);
948         if (ret)
949                 goto rel_vdev;
950 
951         pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
952         if (IS_ERR(pcdev->alloc_ctx)) {
953                 v4l2_err(&pcdev->v4l2_dev, "Failed to alloc vb2 context\n");
954                 ret = PTR_ERR(pcdev->alloc_ctx);
955                 goto rel_vdev;
956         }
957 
958         pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
959         if (IS_ERR(pcdev->m2m_dev)) {
960                 v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
961                 ret = PTR_ERR(pcdev->m2m_dev);
962                 goto rel_ctx;
963         }
964 
965         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
966         if (ret) {
967                 v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
968                 goto rel_m2m;
969         }
970 
971         return 0;
972 
973 
974 rel_m2m:
975         v4l2_m2m_release(pcdev->m2m_dev);
976 rel_ctx:
977         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
978 rel_vdev:
979         video_device_release(vfd);
980 unreg_dev:
981         v4l2_device_unregister(&pcdev->v4l2_dev);
982 
983         mutex_destroy(&pcdev->dev_mutex);
984 
985         return ret;
986 }
987 
988 static int emmaprp_remove(struct platform_device *pdev)
989 {
990         struct emmaprp_dev *pcdev = platform_get_drvdata(pdev);
991 
992         v4l2_info(&pcdev->v4l2_dev, "Removing " EMMAPRP_MODULE_NAME);
993 
994         video_unregister_device(pcdev->vfd);
995         v4l2_m2m_release(pcdev->m2m_dev);
996         vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx);
997         v4l2_device_unregister(&pcdev->v4l2_dev);
998         mutex_destroy(&pcdev->dev_mutex);
999 
1000         return 0;
1001 }
1002 
1003 static struct platform_driver emmaprp_pdrv = {
1004         .probe          = emmaprp_probe,
1005         .remove         = emmaprp_remove,
1006         .driver         = {
1007                 .name   = MEM2MEM_NAME,
1008         },
1009 };
1010 module_platform_driver(emmaprp_pdrv);
1011 

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