m2m-deinterlace.c

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