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