Linux Kernel  3.7.1
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
m2m-deinterlace.c
Go to the documentation of this file.
1 /*
2  * V4L2 deinterlacing support.
3  *
4  * Copyright (c) 2012 Vista Silicon S.L.
5  * Javier Martin <[email protected]>
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by the
9  * Free Software Foundation; either version 2 of the
10  * License, or (at your option) any later version
11  */
12 
13 #include <linux/module.h>
14 #include <linux/slab.h>
15 #include <linux/interrupt.h>
16 #include <linux/dmaengine.h>
17 #include <linux/platform_device.h>
18 
19 #include <media/v4l2-mem2mem.h>
20 #include <media/v4l2-device.h>
21 #include <media/v4l2-ioctl.h>
23 
24 #define MEM2MEM_TEST_MODULE_NAME "mem2mem-deinterlace"
25 
26 MODULE_DESCRIPTION("mem2mem device which supports deinterlacing using dmaengine");
27 MODULE_AUTHOR("Javier Martin <[email protected]");
28 MODULE_LICENSE("GPL");
29 MODULE_VERSION("0.0.1");
30 
31 static bool debug = true;
32 module_param(debug, bool, 0644);
33 
34 /* Flags that indicate a format can be used for capture/output */
35 #define MEM2MEM_CAPTURE (1 << 0)
36 #define MEM2MEM_OUTPUT (1 << 1)
37 
38 #define MEM2MEM_NAME "m2m-deinterlace"
39 
40 #define dprintk(dev, fmt, arg...) \
41  v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
42 
44  char *name;
46  /* Types the format can be used for */
48 };
49 
50 static struct deinterlace_fmt formats[] = {
51  {
52  .name = "YUV 4:2:0 Planar",
53  .fourcc = V4L2_PIX_FMT_YUV420,
55  },
56  {
57  .name = "YUYV 4:2:2",
58  .fourcc = V4L2_PIX_FMT_YUYV,
60  },
61 };
62 
63 #define NUM_FORMATS ARRAY_SIZE(formats)
64 
65 /* Per-queue, driver-specific private data */
67  unsigned int width;
68  unsigned int height;
69  unsigned int sizeimage;
72 };
73 
74 enum {
77 };
78 
79 enum {
92 };
93 
94 /* Source and destination queue data */
95 static struct deinterlace_q_data q_data[2];
96 
97 static struct deinterlace_q_data *get_q_data(enum v4l2_buf_type type)
98 {
99  switch (type) {
101  return &q_data[V4L2_M2M_SRC];
103  return &q_data[V4L2_M2M_DST];
104  default:
105  BUG();
106  }
107  return NULL;
108 }
109 
110 static struct deinterlace_fmt *find_format(struct v4l2_format *f)
111 {
112  struct deinterlace_fmt *fmt;
113  unsigned int k;
114 
115  for (k = 0; k < NUM_FORMATS; k++) {
116  fmt = &formats[k];
117  if ((fmt->types & f->type) &&
118  (fmt->fourcc == f->fmt.pix.pixelformat))
119  break;
120  }
121 
122  if (k == NUM_FORMATS)
123  return NULL;
124 
125  return &formats[k];
126 }
127 
130  struct video_device *vfd;
131 
133  struct mutex dev_mutex;
135 
137 
139  struct vb2_alloc_ctx *alloc_ctx;
140 };
141 
144 
145  /* Abort requested by m2m */
146  int aborting;
151 };
152 
153 /*
154  * mem2mem callbacks
155  */
156 static int deinterlace_job_ready(void *priv)
157 {
158  struct deinterlace_ctx *ctx = priv;
159  struct deinterlace_dev *pcdev = ctx->dev;
160 
161  if ((v4l2_m2m_num_src_bufs_ready(ctx->m2m_ctx) > 0)
162  && (v4l2_m2m_num_dst_bufs_ready(ctx->m2m_ctx) > 0)
163  && (atomic_read(&ctx->dev->busy) == 0)) {
164  dprintk(pcdev, "Task ready\n");
165  return 1;
166  }
167 
168  dprintk(pcdev, "Task not ready to run\n");
169 
170  return 0;
171 }
172 
173 static void deinterlace_job_abort(void *priv)
174 {
175  struct deinterlace_ctx *ctx = priv;
176  struct deinterlace_dev *pcdev = ctx->dev;
177 
178  ctx->aborting = 1;
179 
180  dprintk(pcdev, "Aborting task\n");
181 
182  v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->m2m_ctx);
183 }
184 
185 static void deinterlace_lock(void *priv)
186 {
187  struct deinterlace_ctx *ctx = priv;
188  struct deinterlace_dev *pcdev = ctx->dev;
189  mutex_lock(&pcdev->dev_mutex);
190 }
191 
192 static void deinterlace_unlock(void *priv)
193 {
194  struct deinterlace_ctx *ctx = priv;
195  struct deinterlace_dev *pcdev = ctx->dev;
196  mutex_unlock(&pcdev->dev_mutex);
197 }
198 
199 static void dma_callback(void *data)
200 {
201  struct deinterlace_ctx *curr_ctx = data;
202  struct deinterlace_dev *pcdev = curr_ctx->dev;
203  struct vb2_buffer *src_vb, *dst_vb;
204 
205  atomic_set(&pcdev->busy, 0);
206 
207  src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
208  dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
209 
210  v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
211  v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
212 
213  v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->m2m_ctx);
214 
215  dprintk(pcdev, "dma transfers completed.\n");
216 }
217 
218 static void deinterlace_issue_dma(struct deinterlace_ctx *ctx, int op,
219  int do_callback)
220 {
221  struct deinterlace_q_data *s_q_data, *d_q_data;
222  struct vb2_buffer *src_buf, *dst_buf;
223  struct deinterlace_dev *pcdev = ctx->dev;
224  struct dma_chan *chan = pcdev->dma_chan;
225  struct dma_device *dmadev = chan->device;
226  struct dma_async_tx_descriptor *tx;
227  unsigned int s_width, s_height;
228  unsigned int d_width, d_height;
229  unsigned int d_size, s_size;
230  dma_addr_t p_in, p_out;
231  enum dma_ctrl_flags flags;
232 
233  src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
234  dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
235 
236  s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
237  s_width = s_q_data->width;
238  s_height = s_q_data->height;
239  s_size = s_width * s_height;
240 
241  d_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
242  d_width = d_q_data->width;
243  d_height = d_q_data->height;
244  d_size = d_width * d_height;
245 
246  p_in = (dma_addr_t)vb2_dma_contig_plane_dma_addr(src_buf, 0);
247  p_out = (dma_addr_t)vb2_dma_contig_plane_dma_addr(dst_buf, 0);
248  if (!p_in || !p_out) {
249  v4l2_err(&pcdev->v4l2_dev,
250  "Acquiring kernel pointers to buffers failed\n");
251  return;
252  }
253 
254  switch (op) {
255  case YUV420_DMA_Y_ODD:
256  ctx->xt->numf = s_height / 2;
257  ctx->xt->sgl[0].size = s_width;
258  ctx->xt->sgl[0].icg = s_width;
259  ctx->xt->src_start = p_in;
260  ctx->xt->dst_start = p_out;
261  break;
262  case YUV420_DMA_Y_EVEN:
263  ctx->xt->numf = s_height / 2;
264  ctx->xt->sgl[0].size = s_width;
265  ctx->xt->sgl[0].icg = s_width;
266  ctx->xt->src_start = p_in + s_size / 2;
267  ctx->xt->dst_start = p_out + s_width;
268  break;
269  case YUV420_DMA_U_ODD:
270  ctx->xt->numf = s_height / 4;
271  ctx->xt->sgl[0].size = s_width / 2;
272  ctx->xt->sgl[0].icg = s_width / 2;
273  ctx->xt->src_start = p_in + s_size;
274  ctx->xt->dst_start = p_out + s_size;
275  break;
276  case YUV420_DMA_U_EVEN:
277  ctx->xt->numf = s_height / 4;
278  ctx->xt->sgl[0].size = s_width / 2;
279  ctx->xt->sgl[0].icg = s_width / 2;
280  ctx->xt->src_start = p_in + (9 * s_size) / 8;
281  ctx->xt->dst_start = p_out + s_size + s_width / 2;
282  break;
283  case YUV420_DMA_V_ODD:
284  ctx->xt->numf = s_height / 4;
285  ctx->xt->sgl[0].size = s_width / 2;
286  ctx->xt->sgl[0].icg = s_width / 2;
287  ctx->xt->src_start = p_in + (5 * s_size) / 4;
288  ctx->xt->dst_start = p_out + (5 * s_size) / 4;
289  break;
290  case YUV420_DMA_V_EVEN:
291  ctx->xt->numf = s_height / 4;
292  ctx->xt->sgl[0].size = s_width / 2;
293  ctx->xt->sgl[0].icg = s_width / 2;
294  ctx->xt->src_start = p_in + (11 * s_size) / 8;
295  ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2;
296  break;
298  ctx->xt->numf = s_height / 2;
299  ctx->xt->sgl[0].size = s_width;
300  ctx->xt->sgl[0].icg = s_width;
301  ctx->xt->src_start = p_in;
302  ctx->xt->dst_start = p_out + s_width;
303  break;
305  ctx->xt->numf = s_height / 4;
306  ctx->xt->sgl[0].size = s_width / 2;
307  ctx->xt->sgl[0].icg = s_width / 2;
308  ctx->xt->src_start = p_in + s_size;
309  ctx->xt->dst_start = p_out + s_size + s_width / 2;
310  break;
312  ctx->xt->numf = s_height / 4;
313  ctx->xt->sgl[0].size = s_width / 2;
314  ctx->xt->sgl[0].icg = s_width / 2;
315  ctx->xt->src_start = p_in + (5 * s_size) / 4;
316  ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2;
317  break;
318  case YUYV_DMA_ODD:
319  ctx->xt->numf = s_height / 2;
320  ctx->xt->sgl[0].size = s_width * 2;
321  ctx->xt->sgl[0].icg = s_width * 2;
322  ctx->xt->src_start = p_in;
323  ctx->xt->dst_start = p_out;
324  break;
325  case YUYV_DMA_EVEN:
326  ctx->xt->numf = s_height / 2;
327  ctx->xt->sgl[0].size = s_width * 2;
328  ctx->xt->sgl[0].icg = s_width * 2;
329  ctx->xt->src_start = p_in + s_size;
330  ctx->xt->dst_start = p_out + s_width * 2;
331  break;
333  default:
334  ctx->xt->numf = s_height / 2;
335  ctx->xt->sgl[0].size = s_width * 2;
336  ctx->xt->sgl[0].icg = s_width * 2;
337  ctx->xt->src_start = p_in;
338  ctx->xt->dst_start = p_out + s_width * 2;
339  break;
340  }
341 
342  /* Common parameters for al transfers */
343  ctx->xt->frame_size = 1;
344  ctx->xt->dir = DMA_MEM_TO_MEM;
345  ctx->xt->src_sgl = false;
346  ctx->xt->dst_sgl = true;
347  flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT |
349 
350  tx = dmadev->device_prep_interleaved_dma(chan, ctx->xt, flags);
351  if (tx == NULL) {
352  v4l2_warn(&pcdev->v4l2_dev, "DMA interleaved prep error\n");
353  return;
354  }
355 
356  if (do_callback) {
357  tx->callback = dma_callback;
358  tx->callback_param = ctx;
359  }
360 
361  ctx->cookie = dmaengine_submit(tx);
362  if (dma_submit_error(ctx->cookie)) {
363  v4l2_warn(&pcdev->v4l2_dev,
364  "DMA submit error %d with src=0x%x dst=0x%x len=0x%x\n",
365  ctx->cookie, (unsigned)p_in, (unsigned)p_out,
366  s_size * 3/2);
367  return;
368  }
369 
370  dma_async_issue_pending(chan);
371 }
372 
373 static void deinterlace_device_run(void *priv)
374 {
375  struct deinterlace_ctx *ctx = priv;
376  struct deinterlace_q_data *dst_q_data;
377 
378  atomic_set(&ctx->dev->busy, 1);
379 
380  dprintk(ctx->dev, "%s: DMA try issue.\n", __func__);
381 
382  dst_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
383 
384  /*
385  * 4 possible field conversions are possible at the moment:
386  * V4L2_FIELD_SEQ_TB --> V4L2_FIELD_INTERLACED_TB:
387  * two separate fields in the same input buffer are interlaced
388  * in the output buffer using weaving. Top field comes first.
389  * V4L2_FIELD_SEQ_TB --> V4L2_FIELD_NONE:
390  * top field from the input buffer is copied to the output buffer
391  * using line doubling. Bottom field from the input buffer is discarded.
392  * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_INTERLACED_BT:
393  * two separate fields in the same input buffer are interlaced
394  * in the output buffer using weaving. Bottom field comes first.
395  * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_NONE:
396  * bottom field from the input buffer is copied to the output buffer
397  * using line doubling. Top field from the input buffer is discarded.
398  */
399  switch (dst_q_data->fmt->fourcc) {
400  case V4L2_PIX_FMT_YUV420:
401  switch (dst_q_data->field) {
404  dprintk(ctx->dev, "%s: yuv420 interlaced tb.\n",
405  __func__);
406  deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
407  deinterlace_issue_dma(ctx, YUV420_DMA_Y_EVEN, 0);
408  deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
409  deinterlace_issue_dma(ctx, YUV420_DMA_U_EVEN, 0);
410  deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
411  deinterlace_issue_dma(ctx, YUV420_DMA_V_EVEN, 1);
412  break;
413  case V4L2_FIELD_NONE:
414  default:
415  dprintk(ctx->dev, "%s: yuv420 interlaced line doubling.\n",
416  __func__);
417  deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
418  deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD_DOUBLING, 0);
419  deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
420  deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD_DOUBLING, 0);
421  deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
422  deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD_DOUBLING, 1);
423  break;
424  }
425  break;
426  case V4L2_PIX_FMT_YUYV:
427  default:
428  switch (dst_q_data->field) {
431  dprintk(ctx->dev, "%s: yuyv interlaced_tb.\n",
432  __func__);
433  deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
434  deinterlace_issue_dma(ctx, YUYV_DMA_EVEN, 1);
435  break;
436  case V4L2_FIELD_NONE:
437  default:
438  dprintk(ctx->dev, "%s: yuyv interlaced line doubling.\n",
439  __func__);
440  deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
441  deinterlace_issue_dma(ctx, YUYV_DMA_EVEN_DOUBLING, 1);
442  break;
443  }
444  break;
445  }
446 
447  dprintk(ctx->dev, "%s: DMA issue done.\n", __func__);
448 }
449 
450 /*
451  * video ioctls
452  */
453 static int vidioc_querycap(struct file *file, void *priv,
454  struct v4l2_capability *cap)
455 {
456  strlcpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
457  strlcpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
458  strlcpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->card));
459  /*
460  * This is only a mem-to-mem video device. The capture and output
461  * device capability flags are left only for backward compatibility
462  * and are scheduled for removal.
463  */
467 
468  return 0;
469 }
470 
471 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
472 {
473  int i, num;
474  struct deinterlace_fmt *fmt;
475 
476  num = 0;
477 
478  for (i = 0; i < NUM_FORMATS; ++i) {
479  if (formats[i].types & type) {
480  /* index-th format of type type found ? */
481  if (num == f->index)
482  break;
483  /* Correct type but haven't reached our index yet,
484  * just increment per-type index */
485  ++num;
486  }
487  }
488 
489  if (i < NUM_FORMATS) {
490  /* Format found */
491  fmt = &formats[i];
492  strlcpy(f->description, fmt->name, sizeof(f->description));
493  f->pixelformat = fmt->fourcc;
494  return 0;
495  }
496 
497  /* Format not found */
498  return -EINVAL;
499 }
500 
501 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
502  struct v4l2_fmtdesc *f)
503 {
504  return enum_fmt(f, MEM2MEM_CAPTURE);
505 }
506 
507 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
508  struct v4l2_fmtdesc *f)
509 {
510  return enum_fmt(f, MEM2MEM_OUTPUT);
511 }
512 
513 static int vidioc_g_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
514 {
515  struct vb2_queue *vq;
516  struct deinterlace_q_data *q_data;
517 
518  vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
519  if (!vq)
520  return -EINVAL;
521 
522  q_data = get_q_data(f->type);
523 
524  f->fmt.pix.width = q_data->width;
525  f->fmt.pix.height = q_data->height;
526  f->fmt.pix.field = q_data->field;
527  f->fmt.pix.pixelformat = q_data->fmt->fourcc;
528 
529  switch (q_data->fmt->fourcc) {
530  case V4L2_PIX_FMT_YUV420:
531  f->fmt.pix.bytesperline = q_data->width * 3 / 2;
532  break;
533  case V4L2_PIX_FMT_YUYV:
534  default:
535  f->fmt.pix.bytesperline = q_data->width * 2;
536  }
537 
538  f->fmt.pix.sizeimage = q_data->sizeimage;
539  f->fmt.pix.colorspace = ctx->colorspace;
540 
541  return 0;
542 }
543 
544 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
545  struct v4l2_format *f)
546 {
547  return vidioc_g_fmt(priv, f);
548 }
549 
550 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
551  struct v4l2_format *f)
552 {
553  return vidioc_g_fmt(priv, f);
554 }
555 
556 static int vidioc_try_fmt(struct v4l2_format *f, struct deinterlace_fmt *fmt)
557 {
558  switch (f->fmt.pix.pixelformat) {
559  case V4L2_PIX_FMT_YUV420:
560  f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
561  break;
562  case V4L2_PIX_FMT_YUYV:
563  default:
564  f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
565  }
566  f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
567 
568  return 0;
569 }
570 
571 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
572  struct v4l2_format *f)
573 {
574  struct deinterlace_fmt *fmt;
575  struct deinterlace_ctx *ctx = priv;
576 
577  fmt = find_format(f);
578  if (!fmt || !(fmt->types & MEM2MEM_CAPTURE))
579  f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
580 
581  f->fmt.pix.colorspace = ctx->colorspace;
582 
583  if (f->fmt.pix.field != V4L2_FIELD_INTERLACED_TB &&
584  f->fmt.pix.field != V4L2_FIELD_INTERLACED_BT &&
585  f->fmt.pix.field != V4L2_FIELD_NONE)
586  f->fmt.pix.field = V4L2_FIELD_INTERLACED_TB;
587 
588  return vidioc_try_fmt(f, fmt);
589 }
590 
591 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
592  struct v4l2_format *f)
593 {
594  struct deinterlace_fmt *fmt;
595 
596  fmt = find_format(f);
597  if (!fmt || !(fmt->types & MEM2MEM_OUTPUT))
598  f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
599 
600  if (!f->fmt.pix.colorspace)
601  f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
602 
603  if (f->fmt.pix.field != V4L2_FIELD_SEQ_TB &&
604  f->fmt.pix.field != V4L2_FIELD_SEQ_BT)
605  f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
606 
607  return vidioc_try_fmt(f, fmt);
608 }
609 
610 static int vidioc_s_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
611 {
612  struct deinterlace_q_data *q_data;
613  struct vb2_queue *vq;
614 
615  vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
616  if (!vq)
617  return -EINVAL;
618 
619  q_data = get_q_data(f->type);
620  if (!q_data)
621  return -EINVAL;
622 
623  if (vb2_is_busy(vq)) {
624  v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
625  return -EBUSY;
626  }
627 
628  q_data->fmt = find_format(f);
629  if (!q_data->fmt) {
630  v4l2_err(&ctx->dev->v4l2_dev,
631  "Couldn't set format type %d, wxh: %dx%d. fmt: %d, field: %d\n",
632  f->type, f->fmt.pix.width, f->fmt.pix.height,
633  f->fmt.pix.pixelformat, f->fmt.pix.field);
634  return -EINVAL;
635  }
636 
637  q_data->width = f->fmt.pix.width;
638  q_data->height = f->fmt.pix.height;
639  q_data->field = f->fmt.pix.field;
640 
641  switch (f->fmt.pix.pixelformat) {
642  case V4L2_PIX_FMT_YUV420:
643  f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
644  q_data->sizeimage = (q_data->width * q_data->height * 3) / 2;
645  break;
646  case V4L2_PIX_FMT_YUYV:
647  default:
648  f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
649  q_data->sizeimage = q_data->width * q_data->height * 2;
650  }
651 
652  dprintk(ctx->dev,
653  "Setting format for type %d, wxh: %dx%d, fmt: %d, field: %d\n",
654  f->type, q_data->width, q_data->height, q_data->fmt->fourcc,
655  q_data->field);
656 
657  return 0;
658 }
659 
660 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
661  struct v4l2_format *f)
662 {
663  int ret;
664 
665  ret = vidioc_try_fmt_vid_cap(file, priv, f);
666  if (ret)
667  return ret;
668  return vidioc_s_fmt(priv, f);
669 }
670 
671 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
672  struct v4l2_format *f)
673 {
674  struct deinterlace_ctx *ctx = priv;
675  int ret;
676 
677  ret = vidioc_try_fmt_vid_out(file, priv, f);
678  if (ret)
679  return ret;
680 
681  ret = vidioc_s_fmt(priv, f);
682  if (!ret)
683  ctx->colorspace = f->fmt.pix.colorspace;
684 
685  return ret;
686 }
687 
688 static int vidioc_reqbufs(struct file *file, void *priv,
689  struct v4l2_requestbuffers *reqbufs)
690 {
691  struct deinterlace_ctx *ctx = priv;
692 
693  return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
694 }
695 
696 static int vidioc_querybuf(struct file *file, void *priv,
697  struct v4l2_buffer *buf)
698 {
699  struct deinterlace_ctx *ctx = priv;
700 
701  return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
702 }
703 
704 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
705 {
706  struct deinterlace_ctx *ctx = priv;
707 
708  return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
709 }
710 
711 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
712 {
713  struct deinterlace_ctx *ctx = priv;
714 
715  return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
716 }
717 
718 static int vidioc_streamon(struct file *file, void *priv,
719  enum v4l2_buf_type type)
720 {
721  struct deinterlace_q_data *s_q_data, *d_q_data;
722  struct deinterlace_ctx *ctx = priv;
723 
724  s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
725  d_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
726 
727  /* Check that src and dst queues have the same pix format */
728  if (s_q_data->fmt->fourcc != d_q_data->fmt->fourcc) {
729  v4l2_err(&ctx->dev->v4l2_dev,
730  "src and dst formats don't match.\n");
731  return -EINVAL;
732  }
733 
734  /* Check that input and output deinterlacing types are compatible */
735  switch (s_q_data->field) {
736  case V4L2_FIELD_SEQ_BT:
737  if (d_q_data->field != V4L2_FIELD_NONE &&
738  d_q_data->field != V4L2_FIELD_INTERLACED_BT) {
739  v4l2_err(&ctx->dev->v4l2_dev,
740  "src and dst field conversion [(%d)->(%d)] not supported.\n",
741  s_q_data->field, d_q_data->field);
742  return -EINVAL;
743  }
744  break;
745  case V4L2_FIELD_SEQ_TB:
746  if (d_q_data->field != V4L2_FIELD_NONE &&
747  d_q_data->field != V4L2_FIELD_INTERLACED_TB) {
748  v4l2_err(&ctx->dev->v4l2_dev,
749  "src and dst field conversion [(%d)->(%d)] not supported.\n",
750  s_q_data->field, d_q_data->field);
751  return -EINVAL;
752  }
753  break;
754  default:
755  return -EINVAL;
756  }
757 
758  return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
759 }
760 
761 static int vidioc_streamoff(struct file *file, void *priv,
762  enum v4l2_buf_type type)
763 {
764  struct deinterlace_ctx *ctx = priv;
765 
766  return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
767 }
768 
769 static const struct v4l2_ioctl_ops deinterlace_ioctl_ops = {
770  .vidioc_querycap = vidioc_querycap,
771 
772  .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
773  .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
774  .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
775  .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
776 
777  .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
778  .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out,
779  .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
780  .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,
781 
782  .vidioc_reqbufs = vidioc_reqbufs,
783  .vidioc_querybuf = vidioc_querybuf,
784 
785  .vidioc_qbuf = vidioc_qbuf,
786  .vidioc_dqbuf = vidioc_dqbuf,
787 
788  .vidioc_streamon = vidioc_streamon,
789  .vidioc_streamoff = vidioc_streamoff,
790 };
791 
792 
793 /*
794  * Queue operations
795  */
796 struct vb2_dc_conf {
797  struct device *dev;
798 };
799 
800 static int deinterlace_queue_setup(struct vb2_queue *vq,
801  const struct v4l2_format *fmt,
802  unsigned int *nbuffers, unsigned int *nplanes,
803  unsigned int sizes[], void *alloc_ctxs[])
804 {
805  struct deinterlace_ctx *ctx = vb2_get_drv_priv(vq);
806  struct deinterlace_q_data *q_data;
807  unsigned int size, count = *nbuffers;
808 
809  q_data = get_q_data(vq->type);
810 
811  switch (q_data->fmt->fourcc) {
812  case V4L2_PIX_FMT_YUV420:
813  size = q_data->width * q_data->height * 3 / 2;
814  break;
815  case V4L2_PIX_FMT_YUYV:
816  default:
817  size = q_data->width * q_data->height * 2;
818  }
819 
820  *nplanes = 1;
821  *nbuffers = count;
822  sizes[0] = size;
823 
824  alloc_ctxs[0] = ctx->dev->alloc_ctx;
825 
826  dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
827 
828  return 0;
829 }
830 
831 static int deinterlace_buf_prepare(struct vb2_buffer *vb)
832 {
833  struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
834  struct deinterlace_q_data *q_data;
835 
836  dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
837 
838  q_data = get_q_data(vb->vb2_queue->type);
839 
840  if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
841  dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
842  __func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
843  return -EINVAL;
844  }
845 
846  vb2_set_plane_payload(vb, 0, q_data->sizeimage);
847 
848  return 0;
849 }
850 
851 static void deinterlace_buf_queue(struct vb2_buffer *vb)
852 {
853  struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
854  v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
855 }
856 
857 static struct vb2_ops deinterlace_qops = {
858  .queue_setup = deinterlace_queue_setup,
859  .buf_prepare = deinterlace_buf_prepare,
860  .buf_queue = deinterlace_buf_queue,
861 };
862 
863 static int queue_init(void *priv, struct vb2_queue *src_vq,
864  struct vb2_queue *dst_vq)
865 {
866  struct deinterlace_ctx *ctx = priv;
867  int ret;
868 
870  src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
871  src_vq->drv_priv = ctx;
872  src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
873  src_vq->ops = &deinterlace_qops;
874  src_vq->mem_ops = &vb2_dma_contig_memops;
875  q_data[V4L2_M2M_SRC].fmt = &formats[0];
876  q_data[V4L2_M2M_SRC].width = 640;
877  q_data[V4L2_M2M_SRC].height = 480;
878  q_data[V4L2_M2M_SRC].sizeimage = (640 * 480 * 3) / 2;
880 
881  ret = vb2_queue_init(src_vq);
882  if (ret)
883  return ret;
884 
886  dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
887  dst_vq->drv_priv = ctx;
888  dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
889  dst_vq->ops = &deinterlace_qops;
890  dst_vq->mem_ops = &vb2_dma_contig_memops;
891  q_data[V4L2_M2M_DST].fmt = &formats[0];
892  q_data[V4L2_M2M_DST].width = 640;
893  q_data[V4L2_M2M_DST].height = 480;
894  q_data[V4L2_M2M_DST].sizeimage = (640 * 480 * 3) / 2;
896 
897  return vb2_queue_init(dst_vq);
898 }
899 
900 /*
901  * File operations
902  */
903 static int deinterlace_open(struct file *file)
904 {
905  struct deinterlace_dev *pcdev = video_drvdata(file);
906  struct deinterlace_ctx *ctx = NULL;
907 
908  ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
909  if (!ctx)
910  return -ENOMEM;
911 
912  file->private_data = ctx;
913  ctx->dev = pcdev;
914 
915  ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
916  if (IS_ERR(ctx->m2m_ctx)) {
917  int ret = PTR_ERR(ctx->m2m_ctx);
918 
919  kfree(ctx);
920  return ret;
921  }
922 
923  ctx->xt = kzalloc(sizeof(struct dma_async_tx_descriptor) +
924  sizeof(struct data_chunk), GFP_KERNEL);
925  if (!ctx->xt) {
926  int ret = PTR_ERR(ctx->xt);
927 
928  kfree(ctx);
929  return ret;
930  }
931 
933 
934  dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
935 
936  return 0;
937 }
938 
939 static int deinterlace_release(struct file *file)
940 {
941  struct deinterlace_dev *pcdev = video_drvdata(file);
942  struct deinterlace_ctx *ctx = file->private_data;
943 
944  dprintk(pcdev, "Releasing instance %p\n", ctx);
945 
947  kfree(ctx->xt);
948  kfree(ctx);
949 
950  return 0;
951 }
952 
953 static unsigned int deinterlace_poll(struct file *file,
954  struct poll_table_struct *wait)
955 {
956  struct deinterlace_ctx *ctx = file->private_data;
957  int ret;
958 
959  deinterlace_lock(ctx);
960  ret = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
961  deinterlace_unlock(ctx);
962 
963  return ret;
964 }
965 
966 static int deinterlace_mmap(struct file *file, struct vm_area_struct *vma)
967 {
968  struct deinterlace_ctx *ctx = file->private_data;
969 
970  return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
971 }
972 
973 static const struct v4l2_file_operations deinterlace_fops = {
974  .owner = THIS_MODULE,
975  .open = deinterlace_open,
976  .release = deinterlace_release,
977  .poll = deinterlace_poll,
978  .unlocked_ioctl = video_ioctl2,
979  .mmap = deinterlace_mmap,
980 };
981 
982 static struct video_device deinterlace_videodev = {
983  .name = MEM2MEM_NAME,
984  .fops = &deinterlace_fops,
985  .ioctl_ops = &deinterlace_ioctl_ops,
986  .minor = -1,
987  .release = video_device_release,
988  .vfl_dir = VFL_DIR_M2M,
989 };
990 
991 static struct v4l2_m2m_ops m2m_ops = {
992  .device_run = deinterlace_device_run,
993  .job_ready = deinterlace_job_ready,
994  .job_abort = deinterlace_job_abort,
995  .lock = deinterlace_lock,
996  .unlock = deinterlace_unlock,
997 };
998 
999 static int deinterlace_probe(struct platform_device *pdev)
1000 {
1001  struct deinterlace_dev *pcdev;
1002  struct video_device *vfd;
1004  int ret = 0;
1005 
1006  pcdev = kzalloc(sizeof *pcdev, GFP_KERNEL);
1007  if (!pcdev)
1008  return -ENOMEM;
1009 
1010  spin_lock_init(&pcdev->irqlock);
1011 
1012  dma_cap_zero(mask);
1013  dma_cap_set(DMA_INTERLEAVE, mask);
1014  pcdev->dma_chan = dma_request_channel(mask, NULL, pcdev);
1015  if (!pcdev->dma_chan)
1016  goto free_dev;
1017 
1018  if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) {
1019  v4l2_err(&pcdev->v4l2_dev, "DMA does not support INTERLEAVE\n");
1020  goto rel_dma;
1021  }
1022 
1023  ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
1024  if (ret)
1025  goto rel_dma;
1026 
1027  atomic_set(&pcdev->busy, 0);
1028  mutex_init(&pcdev->dev_mutex);
1029 
1030  vfd = video_device_alloc();
1031  if (!vfd) {
1032  v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n");
1033  ret = -ENOMEM;
1034  goto unreg_dev;
1035  }
1036 
1037  *vfd = deinterlace_videodev;
1038  vfd->lock = &pcdev->dev_mutex;
1039 
1040  ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
1041  if (ret) {
1042  v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
1043  goto rel_vdev;
1044  }
1045 
1046  video_set_drvdata(vfd, pcdev);
1047  snprintf(vfd->name, sizeof(vfd->name), "%s", deinterlace_videodev.name);
1048  pcdev->vfd = vfd;
1050  " Device registered as /dev/video%d\n", vfd->num);
1051 
1052  platform_set_drvdata(pdev, pcdev);
1053 
1054  pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
1055  if (IS_ERR(pcdev->alloc_ctx)) {
1056  v4l2_err(&pcdev->v4l2_dev, "Failed to alloc vb2 context\n");
1057  ret = PTR_ERR(pcdev->alloc_ctx);
1058  goto err_ctx;
1059  }
1060 
1061  pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
1062  if (IS_ERR(pcdev->m2m_dev)) {
1063  v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
1064  ret = PTR_ERR(pcdev->m2m_dev);
1065  goto err_m2m;
1066  }
1067 
1068  return 0;
1069 
1070  v4l2_m2m_release(pcdev->m2m_dev);
1071 err_m2m:
1072  video_unregister_device(pcdev->vfd);
1073 err_ctx:
1075 rel_vdev:
1076  video_device_release(vfd);
1077 unreg_dev:
1079 rel_dma:
1080  dma_release_channel(pcdev->dma_chan);
1081 free_dev:
1082  kfree(pcdev);
1083 
1084  return ret;
1085 }
1086 
1087 static int deinterlace_remove(struct platform_device *pdev)
1088 {
1089  struct deinterlace_dev *pcdev =
1090  (struct deinterlace_dev *)platform_get_drvdata(pdev);
1091 
1092  v4l2_info(&pcdev->v4l2_dev, "Removing " MEM2MEM_TEST_MODULE_NAME);
1093  v4l2_m2m_release(pcdev->m2m_dev);
1094  video_unregister_device(pcdev->vfd);
1097  dma_release_channel(pcdev->dma_chan);
1098  kfree(pcdev);
1099 
1100  return 0;
1101 }
1102 
1103 static struct platform_driver deinterlace_pdrv = {
1104  .probe = deinterlace_probe,
1105  .remove = deinterlace_remove,
1106  .driver = {
1107  .name = MEM2MEM_NAME,
1108  .owner = THIS_MODULE,
1109  },
1110 };
1111 
1112 static void __exit deinterlace_exit(void)
1113 {
1114  platform_driver_unregister(&deinterlace_pdrv);
1115 }
1116 
1117 static int __init deinterlace_init(void)
1118 {
1119  return platform_driver_register(&deinterlace_pdrv);
1120 }
1121 
1122 module_init(deinterlace_init);
1123 module_exit(deinterlace_exit);
1124