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 deinterlace_lock(void *priv)
185 {
186 	struct deinterlace_ctx *ctx = priv;
187 	struct deinterlace_dev *pcdev = ctx->dev;
188 	mutex_lock(&pcdev->dev_mutex);
189 }
190 
191 static void deinterlace_unlock(void *priv)
192 {
193 	struct deinterlace_ctx *ctx = priv;
194 	struct deinterlace_dev *pcdev = ctx->dev;
195 	mutex_unlock(&pcdev->dev_mutex);
196 }
197 
198 static void dma_callback(void *data)
199 {
200 	struct deinterlace_ctx *curr_ctx = data;
201 	struct deinterlace_dev *pcdev = curr_ctx->dev;
202 	struct vb2_v4l2_buffer *src_vb, *dst_vb;
203 
204 	atomic_set(&pcdev->busy, 0);
205 
206 	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
207 	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
208 
209 	dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
210 	dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
211 	dst_vb->flags |=
212 		src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
213 	dst_vb->timecode = src_vb->timecode;
214 
215 	v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
216 	v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
217 
218 	v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->m2m_ctx);
219 
220 	dprintk(pcdev, "dma transfers completed.\n");
221 }
222 
223 static void deinterlace_issue_dma(struct deinterlace_ctx *ctx, int op,
224 				  int do_callback)
225 {
226 	struct deinterlace_q_data *s_q_data;
227 	struct vb2_v4l2_buffer *src_buf, *dst_buf;
228 	struct deinterlace_dev *pcdev = ctx->dev;
229 	struct dma_chan *chan = pcdev->dma_chan;
230 	struct dma_device *dmadev = chan->device;
231 	struct dma_async_tx_descriptor *tx;
232 	unsigned int s_width, s_height;
233 	unsigned int s_size;
234 	dma_addr_t p_in, p_out;
235 	enum dma_ctrl_flags flags;
236 
237 	src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
238 	dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
239 
240 	s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
241 	s_width	= s_q_data->width;
242 	s_height = s_q_data->height;
243 	s_size = s_width * s_height;
244 
245 	p_in = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
246 	p_out = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf,
247 							  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;
297 	case YUV420_DMA_Y_ODD_DOUBLING:
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;
304 	case YUV420_DMA_U_ODD_DOUBLING:
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;
311 	case YUV420_DMA_V_ODD_DOUBLING:
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;
332 	case YUYV_DMA_EVEN_DOUBLING:
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;
348 
349 	tx = dmadev->device_prep_interleaved_dma(chan, ctx->xt, flags);
350 	if (tx == NULL) {
351 		v4l2_warn(&pcdev->v4l2_dev, "DMA interleaved prep error\n");
352 		return;
353 	}
354 
355 	if (do_callback) {
356 		tx->callback = dma_callback;
357 		tx->callback_param = ctx;
358 	}
359 
360 	ctx->cookie = dmaengine_submit(tx);
361 	if (dma_submit_error(ctx->cookie)) {
362 		v4l2_warn(&pcdev->v4l2_dev,
363 			  "DMA submit error %d with src=0x%x dst=0x%x len=0x%x\n",
364 			  ctx->cookie, (unsigned)p_in, (unsigned)p_out,
365 			  s_size * 3/2);
366 		return;
367 	}
368 
369 	dma_async_issue_pending(chan);
370 }
371 
372 static void deinterlace_device_run(void *priv)
373 {
374 	struct deinterlace_ctx *ctx = priv;
375 	struct deinterlace_q_data *dst_q_data;
376 
377 	atomic_set(&ctx->dev->busy, 1);
378 
379 	dprintk(ctx->dev, "%s: DMA try issue.\n", __func__);
380 
381 	dst_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
382 
383 	/*
384 	 * 4 possible field conversions are possible at the moment:
385 	 *  V4L2_FIELD_SEQ_TB --> V4L2_FIELD_INTERLACED_TB:
386 	 *	two separate fields in the same input buffer are interlaced
387 	 * 	in the output buffer using weaving. Top field comes first.
388 	 *  V4L2_FIELD_SEQ_TB --> V4L2_FIELD_NONE:
389 	 * 	top field from the input buffer is copied to the output buffer
390 	 * 	using line doubling. Bottom field from the input buffer is discarded.
391 	 * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_INTERLACED_BT:
392 	 *	two separate fields in the same input buffer are interlaced
393 	 * 	in the output buffer using weaving. Bottom field comes first.
394 	 * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_NONE:
395 	 * 	bottom field from the input buffer is copied to the output buffer
396 	 * 	using line doubling. Top field from the input buffer is discarded.
397 	 */
398 	switch (dst_q_data->fmt->fourcc) {
399 	case V4L2_PIX_FMT_YUV420:
400 		switch (dst_q_data->field) {
401 		case V4L2_FIELD_INTERLACED_TB:
402 		case V4L2_FIELD_INTERLACED_BT:
403 			dprintk(ctx->dev, "%s: yuv420 interlaced tb.\n",
404 				__func__);
405 			deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
406 			deinterlace_issue_dma(ctx, YUV420_DMA_Y_EVEN, 0);
407 			deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
408 			deinterlace_issue_dma(ctx, YUV420_DMA_U_EVEN, 0);
409 			deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
410 			deinterlace_issue_dma(ctx, YUV420_DMA_V_EVEN, 1);
411 			break;
412 		case V4L2_FIELD_NONE:
413 		default:
414 			dprintk(ctx->dev, "%s: yuv420 interlaced line doubling.\n",
415 				__func__);
416 			deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0);
417 			deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD_DOUBLING, 0);
418 			deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0);
419 			deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD_DOUBLING, 0);
420 			deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0);
421 			deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD_DOUBLING, 1);
422 			break;
423 		}
424 		break;
425 	case V4L2_PIX_FMT_YUYV:
426 	default:
427 		switch (dst_q_data->field) {
428 		case V4L2_FIELD_INTERLACED_TB:
429 		case V4L2_FIELD_INTERLACED_BT:
430 			dprintk(ctx->dev, "%s: yuyv interlaced_tb.\n",
431 				__func__);
432 			deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
433 			deinterlace_issue_dma(ctx, YUYV_DMA_EVEN, 1);
434 			break;
435 		case V4L2_FIELD_NONE:
436 		default:
437 			dprintk(ctx->dev, "%s: yuyv interlaced line doubling.\n",
438 				__func__);
439 			deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0);
440 			deinterlace_issue_dma(ctx, YUYV_DMA_EVEN_DOUBLING, 1);
441 			break;
442 		}
443 		break;
444 	}
445 
446 	dprintk(ctx->dev, "%s: DMA issue done.\n", __func__);
447 }
448 
449 /*
450  * video ioctls
451  */
452 static int vidioc_querycap(struct file *file, void *priv,
453 			   struct v4l2_capability *cap)
454 {
455 	strlcpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
456 	strlcpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
457 	strlcpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->card));
458 	/*
459 	 * This is only a mem-to-mem video device. The capture and output
460 	 * device capability flags are left only for backward compatibility
461 	 * and are scheduled for removal.
462 	 */
463 	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
464 			   V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
465 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
466 
467 	return 0;
468 }
469 
470 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
471 {
472 	int i, num;
473 	struct deinterlace_fmt *fmt;
474 
475 	num = 0;
476 
477 	for (i = 0; i < NUM_FORMATS; ++i) {
478 		if (formats[i].types & type) {
479 			/* index-th format of type type found ? */
480 			if (num == f->index)
481 				break;
482 			/* Correct type but haven't reached our index yet,
483 			 * just increment per-type index */
484 			++num;
485 		}
486 	}
487 
488 	if (i < NUM_FORMATS) {
489 		/* Format found */
490 		fmt = &formats[i];
491 		strlcpy(f->description, fmt->name, sizeof(f->description));
492 		f->pixelformat = fmt->fourcc;
493 		return 0;
494 	}
495 
496 	/* Format not found */
497 	return -EINVAL;
498 }
499 
500 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
501 				   struct v4l2_fmtdesc *f)
502 {
503 	return enum_fmt(f, MEM2MEM_CAPTURE);
504 }
505 
506 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
507 				   struct v4l2_fmtdesc *f)
508 {
509 	return enum_fmt(f, MEM2MEM_OUTPUT);
510 }
511 
512 static int vidioc_g_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
513 {
514 	struct vb2_queue *vq;
515 	struct deinterlace_q_data *q_data;
516 
517 	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
518 	if (!vq)
519 		return -EINVAL;
520 
521 	q_data = get_q_data(f->type);
522 
523 	f->fmt.pix.width	= q_data->width;
524 	f->fmt.pix.height	= q_data->height;
525 	f->fmt.pix.field	= q_data->field;
526 	f->fmt.pix.pixelformat	= q_data->fmt->fourcc;
527 
528 	switch (q_data->fmt->fourcc) {
529 	case V4L2_PIX_FMT_YUV420:
530 		f->fmt.pix.bytesperline = q_data->width * 3 / 2;
531 		break;
532 	case V4L2_PIX_FMT_YUYV:
533 	default:
534 		f->fmt.pix.bytesperline = q_data->width * 2;
535 	}
536 
537 	f->fmt.pix.sizeimage	= q_data->sizeimage;
538 	f->fmt.pix.colorspace	= ctx->colorspace;
539 
540 	return 0;
541 }
542 
543 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
544 				struct v4l2_format *f)
545 {
546 	return vidioc_g_fmt(priv, f);
547 }
548 
549 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
550 				struct v4l2_format *f)
551 {
552 	return vidioc_g_fmt(priv, f);
553 }
554 
555 static int vidioc_try_fmt(struct v4l2_format *f, struct deinterlace_fmt *fmt)
556 {
557 	switch (f->fmt.pix.pixelformat) {
558 	case V4L2_PIX_FMT_YUV420:
559 		f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
560 		break;
561 	case V4L2_PIX_FMT_YUYV:
562 	default:
563 		f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
564 	}
565 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
566 
567 	return 0;
568 }
569 
570 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
571 				  struct v4l2_format *f)
572 {
573 	struct deinterlace_fmt *fmt;
574 	struct deinterlace_ctx *ctx = priv;
575 
576 	fmt = find_format(f);
577 	if (!fmt || !(fmt->types & MEM2MEM_CAPTURE))
578 		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
579 
580 	f->fmt.pix.colorspace = ctx->colorspace;
581 
582 	if (f->fmt.pix.field != V4L2_FIELD_INTERLACED_TB &&
583 	    f->fmt.pix.field != V4L2_FIELD_INTERLACED_BT &&
584 	    f->fmt.pix.field != V4L2_FIELD_NONE)
585 		f->fmt.pix.field = V4L2_FIELD_INTERLACED_TB;
586 
587 	return vidioc_try_fmt(f, fmt);
588 }
589 
590 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
591 				  struct v4l2_format *f)
592 {
593 	struct deinterlace_fmt *fmt;
594 
595 	fmt = find_format(f);
596 	if (!fmt || !(fmt->types & MEM2MEM_OUTPUT))
597 		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
598 
599 	if (!f->fmt.pix.colorspace)
600 		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
601 
602 	if (f->fmt.pix.field != V4L2_FIELD_SEQ_TB &&
603 	    f->fmt.pix.field != V4L2_FIELD_SEQ_BT)
604 		f->fmt.pix.field = V4L2_FIELD_SEQ_TB;
605 
606 	return vidioc_try_fmt(f, fmt);
607 }
608 
609 static int vidioc_s_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f)
610 {
611 	struct deinterlace_q_data *q_data;
612 	struct vb2_queue *vq;
613 
614 	vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
615 	if (!vq)
616 		return -EINVAL;
617 
618 	q_data = get_q_data(f->type);
619 	if (!q_data)
620 		return -EINVAL;
621 
622 	if (vb2_is_busy(vq)) {
623 		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
624 		return -EBUSY;
625 	}
626 
627 	q_data->fmt = find_format(f);
628 	if (!q_data->fmt) {
629 		v4l2_err(&ctx->dev->v4l2_dev,
630 			 "Couldn't set format type %d, wxh: %dx%d. fmt: %d, field: %d\n",
631 			f->type, f->fmt.pix.width, f->fmt.pix.height,
632 			f->fmt.pix.pixelformat, f->fmt.pix.field);
633 		return -EINVAL;
634 	}
635 
636 	q_data->width		= f->fmt.pix.width;
637 	q_data->height		= f->fmt.pix.height;
638 	q_data->field		= f->fmt.pix.field;
639 
640 	switch (f->fmt.pix.pixelformat) {
641 	case V4L2_PIX_FMT_YUV420:
642 		f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
643 		q_data->sizeimage = (q_data->width * q_data->height * 3) / 2;
644 		break;
645 	case V4L2_PIX_FMT_YUYV:
646 	default:
647 		f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
648 		q_data->sizeimage = q_data->width * q_data->height * 2;
649 	}
650 
651 	dprintk(ctx->dev,
652 		"Setting format for type %d, wxh: %dx%d, fmt: %d, field: %d\n",
653 		f->type, q_data->width, q_data->height, q_data->fmt->fourcc,
654 		q_data->field);
655 
656 	return 0;
657 }
658 
659 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
660 				struct v4l2_format *f)
661 {
662 	int ret;
663 
664 	ret = vidioc_try_fmt_vid_cap(file, priv, f);
665 	if (ret)
666 		return ret;
667 	return vidioc_s_fmt(priv, f);
668 }
669 
670 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
671 				struct v4l2_format *f)
672 {
673 	struct deinterlace_ctx *ctx = priv;
674 	int ret;
675 
676 	ret = vidioc_try_fmt_vid_out(file, priv, f);
677 	if (ret)
678 		return ret;
679 
680 	ret = vidioc_s_fmt(priv, f);
681 	if (!ret)
682 		ctx->colorspace = f->fmt.pix.colorspace;
683 
684 	return ret;
685 }
686 
687 static int vidioc_reqbufs(struct file *file, void *priv,
688 			  struct v4l2_requestbuffers *reqbufs)
689 {
690 	struct deinterlace_ctx *ctx = priv;
691 
692 	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
693 }
694 
695 static int vidioc_querybuf(struct file *file, void *priv,
696 			   struct v4l2_buffer *buf)
697 {
698 	struct deinterlace_ctx *ctx = priv;
699 
700 	return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
701 }
702 
703 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
704 {
705 	struct deinterlace_ctx *ctx = priv;
706 
707 	return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
708 }
709 
710 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
711 {
712 	struct deinterlace_ctx *ctx = priv;
713 
714 	return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
715 }
716 
717 static int vidioc_streamon(struct file *file, void *priv,
718 			   enum v4l2_buf_type type)
719 {
720 	struct deinterlace_q_data *s_q_data, *d_q_data;
721 	struct deinterlace_ctx *ctx = priv;
722 
723 	s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT);
724 	d_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE);
725 
726 	/* Check that src and dst queues have the same pix format */
727 	if (s_q_data->fmt->fourcc != d_q_data->fmt->fourcc) {
728 		v4l2_err(&ctx->dev->v4l2_dev,
729 			 "src and dst formats don't match.\n");
730 		return -EINVAL;
731 	}
732 
733 	/* Check that input and output deinterlacing types are compatible */
734 	switch (s_q_data->field) {
735 	case V4L2_FIELD_SEQ_BT:
736 		if (d_q_data->field != V4L2_FIELD_NONE &&
737 			d_q_data->field != V4L2_FIELD_INTERLACED_BT) {
738 			v4l2_err(&ctx->dev->v4l2_dev,
739 			 "src and dst field conversion [(%d)->(%d)] not supported.\n",
740 				s_q_data->field, d_q_data->field);
741 			return -EINVAL;
742 		}
743 		break;
744 	case V4L2_FIELD_SEQ_TB:
745 		if (d_q_data->field != V4L2_FIELD_NONE &&
746 			d_q_data->field != V4L2_FIELD_INTERLACED_TB) {
747 			v4l2_err(&ctx->dev->v4l2_dev,
748 			 "src and dst field conversion [(%d)->(%d)] not supported.\n",
749 				s_q_data->field, d_q_data->field);
750 			return -EINVAL;
751 		}
752 		break;
753 	default:
754 		return -EINVAL;
755 	}
756 
757 	return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
758 }
759 
760 static int vidioc_streamoff(struct file *file, void *priv,
761 			    enum v4l2_buf_type type)
762 {
763 	struct deinterlace_ctx *ctx = priv;
764 
765 	return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
766 }
767 
768 static const struct v4l2_ioctl_ops deinterlace_ioctl_ops = {
769 	.vidioc_querycap	= vidioc_querycap,
770 
771 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
772 	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
773 	.vidioc_try_fmt_vid_cap	= vidioc_try_fmt_vid_cap,
774 	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
775 
776 	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
777 	.vidioc_g_fmt_vid_out	= vidioc_g_fmt_vid_out,
778 	.vidioc_try_fmt_vid_out	= vidioc_try_fmt_vid_out,
779 	.vidioc_s_fmt_vid_out	= vidioc_s_fmt_vid_out,
780 
781 	.vidioc_reqbufs		= vidioc_reqbufs,
782 	.vidioc_querybuf	= vidioc_querybuf,
783 
784 	.vidioc_qbuf		= vidioc_qbuf,
785 	.vidioc_dqbuf		= vidioc_dqbuf,
786 
787 	.vidioc_streamon	= vidioc_streamon,
788 	.vidioc_streamoff	= vidioc_streamoff,
789 };
790 
791 
792 /*
793  * Queue operations
794  */
795 struct vb2_dc_conf {
796 	struct device           *dev;
797 };
798 
799 static int deinterlace_queue_setup(struct vb2_queue *vq,
800 				unsigned int *nbuffers, unsigned int *nplanes,
801 				unsigned int sizes[], struct device *alloc_devs[])
802 {
803 	struct deinterlace_ctx *ctx = vb2_get_drv_priv(vq);
804 	struct deinterlace_q_data *q_data;
805 	unsigned int size, count = *nbuffers;
806 
807 	q_data = get_q_data(vq->type);
808 
809 	switch (q_data->fmt->fourcc) {
810 	case V4L2_PIX_FMT_YUV420:
811 		size = q_data->width * q_data->height * 3 / 2;
812 		break;
813 	case V4L2_PIX_FMT_YUYV:
814 	default:
815 		size = q_data->width * q_data->height * 2;
816 	}
817 
818 	*nplanes = 1;
819 	*nbuffers = count;
820 	sizes[0] = size;
821 
822 	dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
823 
824 	return 0;
825 }
826 
827 static int deinterlace_buf_prepare(struct vb2_buffer *vb)
828 {
829 	struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
830 	struct deinterlace_q_data *q_data;
831 
832 	dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
833 
834 	q_data = get_q_data(vb->vb2_queue->type);
835 
836 	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
837 		dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
838 			__func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
839 		return -EINVAL;
840 	}
841 
842 	vb2_set_plane_payload(vb, 0, q_data->sizeimage);
843 
844 	return 0;
845 }
846 
847 static void deinterlace_buf_queue(struct vb2_buffer *vb)
848 {
849 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
850 	struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
851 
852 	v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
853 }
854 
855 static const struct vb2_ops deinterlace_qops = {
856 	.queue_setup	 = deinterlace_queue_setup,
857 	.buf_prepare	 = deinterlace_buf_prepare,
858 	.buf_queue	 = deinterlace_buf_queue,
859 };
860 
861 static int queue_init(void *priv, struct vb2_queue *src_vq,
862 		      struct vb2_queue *dst_vq)
863 {
864 	struct deinterlace_ctx *ctx = priv;
865 	int ret;
866 
867 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
868 	src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
869 	src_vq->drv_priv = ctx;
870 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
871 	src_vq->ops = &deinterlace_qops;
872 	src_vq->mem_ops = &vb2_dma_contig_memops;
873 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
874 	src_vq->dev = ctx->dev->v4l2_dev.dev;
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;
879 	q_data[V4L2_M2M_SRC].field = V4L2_FIELD_SEQ_TB;
880 
881 	ret = vb2_queue_init(src_vq);
882 	if (ret)
883 		return ret;
884 
885 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
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 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
892 	dst_vq->dev = ctx->dev->v4l2_dev.dev;
893 	q_data[V4L2_M2M_DST].fmt = &formats[0];
894 	q_data[V4L2_M2M_DST].width = 640;
895 	q_data[V4L2_M2M_DST].height = 480;
896 	q_data[V4L2_M2M_DST].sizeimage = (640 * 480 * 3) / 2;
897 	q_data[V4L2_M2M_SRC].field = V4L2_FIELD_INTERLACED_TB;
898 
899 	return vb2_queue_init(dst_vq);
900 }
901 
902 /*
903  * File operations
904  */
905 static int deinterlace_open(struct file *file)
906 {
907 	struct deinterlace_dev *pcdev = video_drvdata(file);
908 	struct deinterlace_ctx *ctx = NULL;
909 
910 	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
911 	if (!ctx)
912 		return -ENOMEM;
913 
914 	file->private_data = ctx;
915 	ctx->dev = pcdev;
916 
917 	ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
918 	if (IS_ERR(ctx->m2m_ctx)) {
919 		int ret = PTR_ERR(ctx->m2m_ctx);
920 
921 		kfree(ctx);
922 		return ret;
923 	}
924 
925 	ctx->xt = kzalloc(sizeof(struct dma_interleaved_template) +
926 				sizeof(struct data_chunk), GFP_KERNEL);
927 	if (!ctx->xt) {
928 		kfree(ctx);
929 		return -ENOMEM;
930 	}
931 
932 	ctx->colorspace = V4L2_COLORSPACE_REC709;
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 
946 	v4l2_m2m_ctx_release(ctx->m2m_ctx);
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 const 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_empty,
988 	.vfl_dir	= VFL_DIR_M2M,
989 };
990 
991 static const 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;
1003 	dma_cap_mask_t mask;
1004 	int ret = 0;
1005 
1006 	pcdev = devm_kzalloc(&pdev->dev, 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 		return -ENODEV;
1017 
1018 	if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) {
1019 		dev_err(&pdev->dev, "DMA does not support INTERLEAVE\n");
1020 		ret = -ENODEV;
1021 		goto rel_dma;
1022 	}
1023 
1024 	ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
1025 	if (ret)
1026 		goto rel_dma;
1027 
1028 	atomic_set(&pcdev->busy, 0);
1029 	mutex_init(&pcdev->dev_mutex);
1030 
1031 	vfd = &pcdev->vfd;
1032 	*vfd = deinterlace_videodev;
1033 	vfd->lock = &pcdev->dev_mutex;
1034 	vfd->v4l2_dev = &pcdev->v4l2_dev;
1035 
1036 	ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
1037 	if (ret) {
1038 		v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
1039 		goto unreg_dev;
1040 	}
1041 
1042 	video_set_drvdata(vfd, pcdev);
1043 	snprintf(vfd->name, sizeof(vfd->name), "%s", deinterlace_videodev.name);
1044 	v4l2_info(&pcdev->v4l2_dev, MEM2MEM_TEST_MODULE_NAME
1045 			" Device registered as /dev/video%d\n", vfd->num);
1046 
1047 	platform_set_drvdata(pdev, pcdev);
1048 
1049 	pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
1050 	if (IS_ERR(pcdev->m2m_dev)) {
1051 		v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
1052 		ret = PTR_ERR(pcdev->m2m_dev);
1053 		goto err_m2m;
1054 	}
1055 
1056 	return 0;
1057 
1058 err_m2m:
1059 	video_unregister_device(&pcdev->vfd);
1060 unreg_dev:
1061 	v4l2_device_unregister(&pcdev->v4l2_dev);
1062 rel_dma:
1063 	dma_release_channel(pcdev->dma_chan);
1064 
1065 	return ret;
1066 }
1067 
1068 static int deinterlace_remove(struct platform_device *pdev)
1069 {
1070 	struct deinterlace_dev *pcdev = platform_get_drvdata(pdev);
1071 
1072 	v4l2_info(&pcdev->v4l2_dev, "Removing " MEM2MEM_TEST_MODULE_NAME);
1073 	v4l2_m2m_release(pcdev->m2m_dev);
1074 	video_unregister_device(&pcdev->vfd);
1075 	v4l2_device_unregister(&pcdev->v4l2_dev);
1076 	dma_release_channel(pcdev->dma_chan);
1077 
1078 	return 0;
1079 }
1080 
1081 static struct platform_driver deinterlace_pdrv = {
1082 	.probe		= deinterlace_probe,
1083 	.remove		= deinterlace_remove,
1084 	.driver		= {
1085 		.name	= MEM2MEM_NAME,
1086 	},
1087 };
1088 module_platform_driver(deinterlace_pdrv);
1089 
1090