1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * A virtual v4l2-mem2mem example device.
4  *
5  * This is a virtual device driver for testing mem-to-mem vb2 framework.
6  * It simulates a device that uses memory buffers for both source and
7  * destination, processes the data and issues an "irq" (simulated by a delayed
8  * workqueue).
9  * The device is capable of multi-instance, multi-buffer-per-transaction
10  * operation (via the mem2mem framework).
11  *
12  * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
13  * Pawel Osciak, <pawel@osciak.com>
14  * Marek Szyprowski, <m.szyprowski@samsung.com>
15  */
16 #include <linux/module.h>
17 #include <linux/delay.h>
18 #include <linux/fs.h>
19 #include <linux/sched.h>
20 #include <linux/slab.h>
21 
22 #include <linux/platform_device.h>
23 #include <media/v4l2-mem2mem.h>
24 #include <media/v4l2-device.h>
25 #include <media/v4l2-ioctl.h>
26 #include <media/v4l2-ctrls.h>
27 #include <media/v4l2-event.h>
28 #include <media/videobuf2-vmalloc.h>
29 
30 MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
31 MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
32 MODULE_LICENSE("GPL");
33 MODULE_VERSION("0.2");
34 MODULE_ALIAS("mem2mem_testdev");
35 
36 static unsigned int debug;
37 module_param(debug, uint, 0644);
38 MODULE_PARM_DESC(debug, "debug level");
39 
40 /* Default transaction time in msec */
41 static unsigned int default_transtime = 40; /* Max 25 fps */
42 module_param(default_transtime, uint, 0644);
43 MODULE_PARM_DESC(default_transtime, "default transaction time in ms");
44 
45 #define MIN_W 32
46 #define MIN_H 32
47 #define MAX_W 640
48 #define MAX_H 480
49 
50 /* Pixel alignment for non-bayer formats */
51 #define WIDTH_ALIGN 2
52 #define HEIGHT_ALIGN 1
53 
54 /* Pixel alignment for bayer formats */
55 #define BAYER_WIDTH_ALIGN  2
56 #define BAYER_HEIGHT_ALIGN 2
57 
58 /* Flags that indicate a format can be used for capture/output */
59 #define MEM2MEM_CAPTURE	BIT(0)
60 #define MEM2MEM_OUTPUT	BIT(1)
61 
62 #define MEM2MEM_NAME		"vim2m"
63 
64 /* Per queue */
65 #define MEM2MEM_DEF_NUM_BUFS	VIDEO_MAX_FRAME
66 /* In bytes, per queue */
67 #define MEM2MEM_VID_MEM_LIMIT	(16 * 1024 * 1024)
68 
69 /* Flags that indicate processing mode */
70 #define MEM2MEM_HFLIP	BIT(0)
71 #define MEM2MEM_VFLIP	BIT(1)
72 
73 #define dprintk(dev, lvl, fmt, arg...) \
74 	v4l2_dbg(lvl, debug, &(dev)->v4l2_dev, "%s: " fmt, __func__, ## arg)
75 
76 static void vim2m_dev_release(struct device *dev)
77 {}
78 
79 static struct platform_device vim2m_pdev = {
80 	.name		= MEM2MEM_NAME,
81 	.dev.release	= vim2m_dev_release,
82 };
83 
84 struct vim2m_fmt {
85 	u32	fourcc;
86 	int	depth;
87 	/* Types the format can be used for */
88 	u32     types;
89 };
90 
91 static struct vim2m_fmt formats[] = {
92 	{
93 		.fourcc	= V4L2_PIX_FMT_RGB565,  /* rrrrrggg gggbbbbb */
94 		.depth	= 16,
95 		.types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
96 	}, {
97 		.fourcc	= V4L2_PIX_FMT_RGB565X, /* gggbbbbb rrrrrggg */
98 		.depth	= 16,
99 		.types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
100 	}, {
101 		.fourcc	= V4L2_PIX_FMT_RGB24,
102 		.depth	= 24,
103 		.types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
104 	}, {
105 		.fourcc	= V4L2_PIX_FMT_BGR24,
106 		.depth	= 24,
107 		.types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
108 	}, {
109 		.fourcc	= V4L2_PIX_FMT_YUYV,
110 		.depth	= 16,
111 		.types  = MEM2MEM_CAPTURE,
112 	}, {
113 		.fourcc	= V4L2_PIX_FMT_SBGGR8,
114 		.depth	= 8,
115 		.types  = MEM2MEM_CAPTURE,
116 	}, {
117 		.fourcc	= V4L2_PIX_FMT_SGBRG8,
118 		.depth	= 8,
119 		.types  = MEM2MEM_CAPTURE,
120 	}, {
121 		.fourcc	= V4L2_PIX_FMT_SGRBG8,
122 		.depth	= 8,
123 		.types  = MEM2MEM_CAPTURE,
124 	}, {
125 		.fourcc	= V4L2_PIX_FMT_SRGGB8,
126 		.depth	= 8,
127 		.types  = MEM2MEM_CAPTURE,
128 	},
129 };
130 
131 #define NUM_FORMATS ARRAY_SIZE(formats)
132 
133 /* Per-queue, driver-specific private data */
134 struct vim2m_q_data {
135 	unsigned int		width;
136 	unsigned int		height;
137 	unsigned int		sizeimage;
138 	unsigned int		sequence;
139 	struct vim2m_fmt	*fmt;
140 };
141 
142 enum {
143 	V4L2_M2M_SRC = 0,
144 	V4L2_M2M_DST = 1,
145 };
146 
147 #define V4L2_CID_TRANS_TIME_MSEC	(V4L2_CID_USER_BASE + 0x1000)
148 #define V4L2_CID_TRANS_NUM_BUFS		(V4L2_CID_USER_BASE + 0x1001)
149 
150 static struct vim2m_fmt *find_format(u32 fourcc)
151 {
152 	struct vim2m_fmt *fmt;
153 	unsigned int k;
154 
155 	for (k = 0; k < NUM_FORMATS; k++) {
156 		fmt = &formats[k];
157 		if (fmt->fourcc == fourcc)
158 			break;
159 	}
160 
161 	if (k == NUM_FORMATS)
162 		return NULL;
163 
164 	return &formats[k];
165 }
166 
167 static void get_alignment(u32 fourcc,
168 			  unsigned int *walign, unsigned int *halign)
169 {
170 	switch (fourcc) {
171 	case V4L2_PIX_FMT_SBGGR8:
172 	case V4L2_PIX_FMT_SGBRG8:
173 	case V4L2_PIX_FMT_SGRBG8:
174 	case V4L2_PIX_FMT_SRGGB8:
175 		*walign = BAYER_WIDTH_ALIGN;
176 		*halign = BAYER_HEIGHT_ALIGN;
177 		return;
178 	default:
179 		*walign = WIDTH_ALIGN;
180 		*halign = HEIGHT_ALIGN;
181 		return;
182 	}
183 }
184 
185 struct vim2m_dev {
186 	struct v4l2_device	v4l2_dev;
187 	struct video_device	vfd;
188 #ifdef CONFIG_MEDIA_CONTROLLER
189 	struct media_device	mdev;
190 #endif
191 
192 	atomic_t		num_inst;
193 	struct mutex		dev_mutex;
194 
195 	struct v4l2_m2m_dev	*m2m_dev;
196 };
197 
198 struct vim2m_ctx {
199 	struct v4l2_fh		fh;
200 	struct vim2m_dev	*dev;
201 
202 	struct v4l2_ctrl_handler hdl;
203 
204 	/* Processed buffers in this transaction */
205 	u8			num_processed;
206 
207 	/* Transaction length (i.e. how many buffers per transaction) */
208 	u32			translen;
209 	/* Transaction time (i.e. simulated processing time) in milliseconds */
210 	u32			transtime;
211 
212 	struct mutex		vb_mutex;
213 	struct delayed_work	work_run;
214 
215 	/* Abort requested by m2m */
216 	int			aborting;
217 
218 	/* Processing mode */
219 	int			mode;
220 
221 	enum v4l2_colorspace	colorspace;
222 	enum v4l2_ycbcr_encoding ycbcr_enc;
223 	enum v4l2_xfer_func	xfer_func;
224 	enum v4l2_quantization	quant;
225 
226 	/* Source and destination queue data */
227 	struct vim2m_q_data   q_data[2];
228 };
229 
230 static inline struct vim2m_ctx *file2ctx(struct file *file)
231 {
232 	return container_of(file->private_data, struct vim2m_ctx, fh);
233 }
234 
235 static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,
236 				       enum v4l2_buf_type type)
237 {
238 	switch (type) {
239 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
240 		return &ctx->q_data[V4L2_M2M_SRC];
241 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
242 		return &ctx->q_data[V4L2_M2M_DST];
243 	default:
244 		return NULL;
245 	}
246 }
247 
248 static const char *type_name(enum v4l2_buf_type type)
249 {
250 	switch (type) {
251 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
252 		return "Output";
253 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
254 		return "Capture";
255 	default:
256 		return "Invalid";
257 	}
258 }
259 
260 #define CLIP(__color) \
261 	(u8)(((__color) > 0xff) ? 0xff : (((__color) < 0) ? 0 : (__color)))
262 
263 static void copy_line(struct vim2m_q_data *q_data_out,
264 		      u8 *src, u8 *dst, bool reverse)
265 {
266 	int x, depth = q_data_out->fmt->depth >> 3;
267 
268 	if (!reverse) {
269 		memcpy(dst, src, q_data_out->width * depth);
270 	} else {
271 		for (x = 0; x < q_data_out->width >> 1; x++) {
272 			memcpy(dst, src, depth);
273 			memcpy(dst + depth, src - depth, depth);
274 			src -= depth << 1;
275 			dst += depth << 1;
276 		}
277 		return;
278 	}
279 }
280 
281 static void copy_two_pixels(struct vim2m_q_data *q_data_in,
282 			    struct vim2m_q_data *q_data_out,
283 			    u8 *src[2], u8 **dst, int ypos, bool reverse)
284 {
285 	struct vim2m_fmt *out = q_data_out->fmt;
286 	struct vim2m_fmt *in = q_data_in->fmt;
287 	u8 _r[2], _g[2], _b[2], *r, *g, *b;
288 	int i;
289 
290 	/* Step 1: read two consecutive pixels from src pointer */
291 
292 	r = _r;
293 	g = _g;
294 	b = _b;
295 
296 	switch (in->fourcc) {
297 	case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
298 		for (i = 0; i < 2; i++) {
299 			u16 pix = le16_to_cpu(*(__le16 *)(src[i]));
300 
301 			*r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
302 			*g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
303 			*b++ = (u8)((pix & 0x1f) << 3) | 0x07;
304 		}
305 		break;
306 	case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
307 		for (i = 0; i < 2; i++) {
308 			u16 pix = be16_to_cpu(*(__be16 *)(src[i]));
309 
310 			*r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
311 			*g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
312 			*b++ = (u8)((pix & 0x1f) << 3) | 0x07;
313 		}
314 		break;
315 	default:
316 	case V4L2_PIX_FMT_RGB24:
317 		for (i = 0; i < 2; i++) {
318 			*r++ = src[i][0];
319 			*g++ = src[i][1];
320 			*b++ = src[i][2];
321 		}
322 		break;
323 	case V4L2_PIX_FMT_BGR24:
324 		for (i = 0; i < 2; i++) {
325 			*b++ = src[i][0];
326 			*g++ = src[i][1];
327 			*r++ = src[i][2];
328 		}
329 		break;
330 	}
331 
332 	/* Step 2: store two consecutive points, reversing them if needed */
333 
334 	r = _r;
335 	g = _g;
336 	b = _b;
337 
338 	switch (out->fourcc) {
339 	case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
340 		for (i = 0; i < 2; i++) {
341 			u16 pix;
342 			__le16 *dst_pix = (__le16 *)*dst;
343 
344 			pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
345 			      (*b >> 3);
346 
347 			*dst_pix = cpu_to_le16(pix);
348 
349 			*dst += 2;
350 		}
351 		return;
352 	case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
353 		for (i = 0; i < 2; i++) {
354 			u16 pix;
355 			__be16 *dst_pix = (__be16 *)*dst;
356 
357 			pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
358 			      (*b >> 3);
359 
360 			*dst_pix = cpu_to_be16(pix);
361 
362 			*dst += 2;
363 		}
364 		return;
365 	case V4L2_PIX_FMT_RGB24:
366 		for (i = 0; i < 2; i++) {
367 			*(*dst)++ = *r++;
368 			*(*dst)++ = *g++;
369 			*(*dst)++ = *b++;
370 		}
371 		return;
372 	case V4L2_PIX_FMT_BGR24:
373 		for (i = 0; i < 2; i++) {
374 			*(*dst)++ = *b++;
375 			*(*dst)++ = *g++;
376 			*(*dst)++ = *r++;
377 		}
378 		return;
379 	case V4L2_PIX_FMT_YUYV:
380 	default:
381 	{
382 		u8 y, y1, u, v;
383 
384 		y = ((8453  * (*r) + 16594 * (*g) +  3223 * (*b)
385 		     + 524288) >> 15);
386 		u = ((-4878 * (*r) - 9578  * (*g) + 14456 * (*b)
387 		     + 4210688) >> 15);
388 		v = ((14456 * (*r++) - 12105 * (*g++) - 2351 * (*b++)
389 		     + 4210688) >> 15);
390 		y1 = ((8453 * (*r) + 16594 * (*g) +  3223 * (*b)
391 		     + 524288) >> 15);
392 
393 		*(*dst)++ = y;
394 		*(*dst)++ = u;
395 
396 		*(*dst)++ = y1;
397 		*(*dst)++ = v;
398 		return;
399 	}
400 	case V4L2_PIX_FMT_SBGGR8:
401 		if (!(ypos & 1)) {
402 			*(*dst)++ = *b;
403 			*(*dst)++ = *++g;
404 		} else {
405 			*(*dst)++ = *g;
406 			*(*dst)++ = *++r;
407 		}
408 		return;
409 	case V4L2_PIX_FMT_SGBRG8:
410 		if (!(ypos & 1)) {
411 			*(*dst)++ = *g;
412 			*(*dst)++ = *++b;
413 		} else {
414 			*(*dst)++ = *r;
415 			*(*dst)++ = *++g;
416 		}
417 		return;
418 	case V4L2_PIX_FMT_SGRBG8:
419 		if (!(ypos & 1)) {
420 			*(*dst)++ = *g;
421 			*(*dst)++ = *++r;
422 		} else {
423 			*(*dst)++ = *b;
424 			*(*dst)++ = *++g;
425 		}
426 		return;
427 	case V4L2_PIX_FMT_SRGGB8:
428 		if (!(ypos & 1)) {
429 			*(*dst)++ = *r;
430 			*(*dst)++ = *++g;
431 		} else {
432 			*(*dst)++ = *g;
433 			*(*dst)++ = *++b;
434 		}
435 		return;
436 	}
437 }
438 
439 static int device_process(struct vim2m_ctx *ctx,
440 			  struct vb2_v4l2_buffer *in_vb,
441 			  struct vb2_v4l2_buffer *out_vb)
442 {
443 	struct vim2m_dev *dev = ctx->dev;
444 	struct vim2m_q_data *q_data_in, *q_data_out;
445 	u8 *p_in, *p_line, *p_in_x[2], *p, *p_out;
446 	unsigned int width, height, bytesperline, bytes_per_pixel;
447 	unsigned int x, y, y_in, y_out, x_int, x_fract, x_err, x_offset;
448 	int start, end, step;
449 
450 	q_data_in = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
451 	if (!q_data_in)
452 		return 0;
453 	bytesperline = (q_data_in->width * q_data_in->fmt->depth) >> 3;
454 	bytes_per_pixel = q_data_in->fmt->depth >> 3;
455 
456 	q_data_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
457 	if (!q_data_out)
458 		return 0;
459 
460 	/* As we're doing scaling, use the output dimensions here */
461 	height = q_data_out->height;
462 	width = q_data_out->width;
463 
464 	p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
465 	p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
466 	if (!p_in || !p_out) {
467 		v4l2_err(&dev->v4l2_dev,
468 			 "Acquiring kernel pointers to buffers failed\n");
469 		return -EFAULT;
470 	}
471 
472 	out_vb->sequence = q_data_out->sequence++;
473 	in_vb->sequence = q_data_in->sequence++;
474 	v4l2_m2m_buf_copy_metadata(in_vb, out_vb, true);
475 
476 	if (ctx->mode & MEM2MEM_VFLIP) {
477 		start = height - 1;
478 		end = -1;
479 		step = -1;
480 	} else {
481 		start = 0;
482 		end = height;
483 		step = 1;
484 	}
485 	y_out = 0;
486 
487 	/*
488 	 * When format and resolution are identical,
489 	 * we can use a faster copy logic
490 	 */
491 	if (q_data_in->fmt->fourcc == q_data_out->fmt->fourcc &&
492 	    q_data_in->width == q_data_out->width &&
493 	    q_data_in->height == q_data_out->height) {
494 		for (y = start; y != end; y += step, y_out++) {
495 			p = p_in + (y * bytesperline);
496 			if (ctx->mode & MEM2MEM_HFLIP)
497 				p += bytesperline - (q_data_in->fmt->depth >> 3);
498 
499 			copy_line(q_data_out, p, p_out,
500 				  ctx->mode & MEM2MEM_HFLIP);
501 
502 			p_out += bytesperline;
503 		}
504 		return 0;
505 	}
506 
507 	/* Slower algorithm with format conversion, hflip, vflip and scaler */
508 
509 	/* To speed scaler up, use Bresenham for X dimension */
510 	x_int = q_data_in->width / q_data_out->width;
511 	x_fract = q_data_in->width % q_data_out->width;
512 
513 	for (y = start; y != end; y += step, y_out++) {
514 		y_in = (y * q_data_in->height) / q_data_out->height;
515 		x_offset = 0;
516 		x_err = 0;
517 
518 		p_line = p_in + (y_in * bytesperline);
519 		if (ctx->mode & MEM2MEM_HFLIP)
520 			p_line += bytesperline - (q_data_in->fmt->depth >> 3);
521 		p_in_x[0] = p_line;
522 
523 		for (x = 0; x < width >> 1; x++) {
524 			x_offset += x_int;
525 			x_err += x_fract;
526 			if (x_err > width) {
527 				x_offset++;
528 				x_err -= width;
529 			}
530 
531 			if (ctx->mode & MEM2MEM_HFLIP)
532 				p_in_x[1] = p_line - x_offset * bytes_per_pixel;
533 			else
534 				p_in_x[1] = p_line + x_offset * bytes_per_pixel;
535 
536 			copy_two_pixels(q_data_in, q_data_out,
537 					p_in_x, &p_out, y_out,
538 					ctx->mode & MEM2MEM_HFLIP);
539 
540 			/* Calculate the next p_in_x0 */
541 			x_offset += x_int;
542 			x_err += x_fract;
543 			if (x_err > width) {
544 				x_offset++;
545 				x_err -= width;
546 			}
547 
548 			if (ctx->mode & MEM2MEM_HFLIP)
549 				p_in_x[0] = p_line - x_offset * bytes_per_pixel;
550 			else
551 				p_in_x[0] = p_line + x_offset * bytes_per_pixel;
552 		}
553 	}
554 
555 	return 0;
556 }
557 
558 /*
559  * mem2mem callbacks
560  */
561 
562 /*
563  * job_ready() - check whether an instance is ready to be scheduled to run
564  */
565 static int job_ready(void *priv)
566 {
567 	struct vim2m_ctx *ctx = priv;
568 
569 	if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen
570 	    || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) {
571 		dprintk(ctx->dev, 1, "Not enough buffers available\n");
572 		return 0;
573 	}
574 
575 	return 1;
576 }
577 
578 static void job_abort(void *priv)
579 {
580 	struct vim2m_ctx *ctx = priv;
581 
582 	/* Will cancel the transaction in the next interrupt handler */
583 	ctx->aborting = 1;
584 }
585 
586 /* device_run() - prepares and starts the device
587  *
588  * This simulates all the immediate preparations required before starting
589  * a device. This will be called by the framework when it decides to schedule
590  * a particular instance.
591  */
592 static void device_run(void *priv)
593 {
594 	struct vim2m_ctx *ctx = priv;
595 	struct vb2_v4l2_buffer *src_buf, *dst_buf;
596 
597 	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
598 	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
599 
600 	/* Apply request controls if any */
601 	v4l2_ctrl_request_setup(src_buf->vb2_buf.req_obj.req,
602 				&ctx->hdl);
603 
604 	device_process(ctx, src_buf, dst_buf);
605 
606 	/* Complete request controls if any */
607 	v4l2_ctrl_request_complete(src_buf->vb2_buf.req_obj.req,
608 				   &ctx->hdl);
609 
610 	/* Run delayed work, which simulates a hardware irq  */
611 	schedule_delayed_work(&ctx->work_run, msecs_to_jiffies(ctx->transtime));
612 }
613 
614 static void device_work(struct work_struct *w)
615 {
616 	struct vim2m_ctx *curr_ctx;
617 	struct vim2m_dev *vim2m_dev;
618 	struct vb2_v4l2_buffer *src_vb, *dst_vb;
619 
620 	curr_ctx = container_of(w, struct vim2m_ctx, work_run.work);
621 
622 	vim2m_dev = curr_ctx->dev;
623 
624 	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
625 	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
626 
627 	curr_ctx->num_processed++;
628 
629 	v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
630 	v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
631 
632 	if (curr_ctx->num_processed == curr_ctx->translen
633 	    || curr_ctx->aborting) {
634 		dprintk(curr_ctx->dev, 2, "Finishing capture buffer fill\n");
635 		curr_ctx->num_processed = 0;
636 		v4l2_m2m_job_finish(vim2m_dev->m2m_dev, curr_ctx->fh.m2m_ctx);
637 	} else {
638 		device_run(curr_ctx);
639 	}
640 }
641 
642 /*
643  * video ioctls
644  */
645 static int vidioc_querycap(struct file *file, void *priv,
646 			   struct v4l2_capability *cap)
647 {
648 	strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
649 	strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
650 	snprintf(cap->bus_info, sizeof(cap->bus_info),
651 		 "platform:%s", MEM2MEM_NAME);
652 	return 0;
653 }
654 
655 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
656 {
657 	int i, num;
658 	struct vim2m_fmt *fmt;
659 
660 	num = 0;
661 
662 	for (i = 0; i < NUM_FORMATS; ++i) {
663 		if (formats[i].types & type) {
664 			/* index-th format of type type found ? */
665 			if (num == f->index)
666 				break;
667 			/*
668 			 * Correct type but haven't reached our index yet,
669 			 * just increment per-type index
670 			 */
671 			++num;
672 		}
673 	}
674 
675 	if (i < NUM_FORMATS) {
676 		/* Format found */
677 		fmt = &formats[i];
678 		f->pixelformat = fmt->fourcc;
679 		return 0;
680 	}
681 
682 	/* Format not found */
683 	return -EINVAL;
684 }
685 
686 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
687 				   struct v4l2_fmtdesc *f)
688 {
689 	return enum_fmt(f, MEM2MEM_CAPTURE);
690 }
691 
692 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
693 				   struct v4l2_fmtdesc *f)
694 {
695 	return enum_fmt(f, MEM2MEM_OUTPUT);
696 }
697 
698 static int vidioc_enum_framesizes(struct file *file, void *priv,
699 				  struct v4l2_frmsizeenum *fsize)
700 {
701 	if (fsize->index != 0)
702 		return -EINVAL;
703 
704 	if (!find_format(fsize->pixel_format))
705 		return -EINVAL;
706 
707 	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
708 	fsize->stepwise.min_width = MIN_W;
709 	fsize->stepwise.min_height = MIN_H;
710 	fsize->stepwise.max_width = MAX_W;
711 	fsize->stepwise.max_height = MAX_H;
712 
713 	get_alignment(fsize->pixel_format,
714 		      &fsize->stepwise.step_width,
715 		      &fsize->stepwise.step_height);
716 	return 0;
717 }
718 
719 static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
720 {
721 	struct vb2_queue *vq;
722 	struct vim2m_q_data *q_data;
723 
724 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
725 	if (!vq)
726 		return -EINVAL;
727 
728 	q_data = get_q_data(ctx, f->type);
729 	if (!q_data)
730 		return -EINVAL;
731 
732 	f->fmt.pix.width	= q_data->width;
733 	f->fmt.pix.height	= q_data->height;
734 	f->fmt.pix.field	= V4L2_FIELD_NONE;
735 	f->fmt.pix.pixelformat	= q_data->fmt->fourcc;
736 	f->fmt.pix.bytesperline	= (q_data->width * q_data->fmt->depth) >> 3;
737 	f->fmt.pix.sizeimage	= q_data->sizeimage;
738 	f->fmt.pix.colorspace	= ctx->colorspace;
739 	f->fmt.pix.xfer_func	= ctx->xfer_func;
740 	f->fmt.pix.ycbcr_enc	= ctx->ycbcr_enc;
741 	f->fmt.pix.quantization	= ctx->quant;
742 
743 	return 0;
744 }
745 
746 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
747 				struct v4l2_format *f)
748 {
749 	return vidioc_g_fmt(file2ctx(file), f);
750 }
751 
752 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
753 				struct v4l2_format *f)
754 {
755 	return vidioc_g_fmt(file2ctx(file), f);
756 }
757 
758 static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt)
759 {
760 	int walign, halign;
761 	/*
762 	 * V4L2 specification specifies the driver corrects the
763 	 * format struct if any of the dimensions is unsupported
764 	 */
765 	if (f->fmt.pix.height < MIN_H)
766 		f->fmt.pix.height = MIN_H;
767 	else if (f->fmt.pix.height > MAX_H)
768 		f->fmt.pix.height = MAX_H;
769 
770 	if (f->fmt.pix.width < MIN_W)
771 		f->fmt.pix.width = MIN_W;
772 	else if (f->fmt.pix.width > MAX_W)
773 		f->fmt.pix.width = MAX_W;
774 
775 	get_alignment(f->fmt.pix.pixelformat, &walign, &halign);
776 	f->fmt.pix.width &= ~(walign - 1);
777 	f->fmt.pix.height &= ~(halign - 1);
778 	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
779 	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
780 	f->fmt.pix.field = V4L2_FIELD_NONE;
781 
782 	return 0;
783 }
784 
785 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
786 				  struct v4l2_format *f)
787 {
788 	struct vim2m_fmt *fmt;
789 	struct vim2m_ctx *ctx = file2ctx(file);
790 
791 	fmt = find_format(f->fmt.pix.pixelformat);
792 	if (!fmt) {
793 		f->fmt.pix.pixelformat = formats[0].fourcc;
794 		fmt = find_format(f->fmt.pix.pixelformat);
795 	}
796 	if (!(fmt->types & MEM2MEM_CAPTURE)) {
797 		v4l2_err(&ctx->dev->v4l2_dev,
798 			 "Fourcc format (0x%08x) invalid.\n",
799 			 f->fmt.pix.pixelformat);
800 		return -EINVAL;
801 	}
802 	f->fmt.pix.colorspace = ctx->colorspace;
803 	f->fmt.pix.xfer_func = ctx->xfer_func;
804 	f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
805 	f->fmt.pix.quantization = ctx->quant;
806 
807 	return vidioc_try_fmt(f, fmt);
808 }
809 
810 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
811 				  struct v4l2_format *f)
812 {
813 	struct vim2m_fmt *fmt;
814 	struct vim2m_ctx *ctx = file2ctx(file);
815 
816 	fmt = find_format(f->fmt.pix.pixelformat);
817 	if (!fmt) {
818 		f->fmt.pix.pixelformat = formats[0].fourcc;
819 		fmt = find_format(f->fmt.pix.pixelformat);
820 	}
821 	if (!(fmt->types & MEM2MEM_OUTPUT)) {
822 		v4l2_err(&ctx->dev->v4l2_dev,
823 			 "Fourcc format (0x%08x) invalid.\n",
824 			 f->fmt.pix.pixelformat);
825 		return -EINVAL;
826 	}
827 	if (!f->fmt.pix.colorspace)
828 		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
829 
830 	return vidioc_try_fmt(f, fmt);
831 }
832 
833 static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
834 {
835 	struct vim2m_q_data *q_data;
836 	struct vb2_queue *vq;
837 
838 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
839 	if (!vq)
840 		return -EINVAL;
841 
842 	q_data = get_q_data(ctx, f->type);
843 	if (!q_data)
844 		return -EINVAL;
845 
846 	if (vb2_is_busy(vq)) {
847 		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
848 		return -EBUSY;
849 	}
850 
851 	q_data->fmt		= find_format(f->fmt.pix.pixelformat);
852 	q_data->width		= f->fmt.pix.width;
853 	q_data->height		= f->fmt.pix.height;
854 	q_data->sizeimage	= q_data->width * q_data->height
855 				* q_data->fmt->depth >> 3;
856 
857 	dprintk(ctx->dev, 1,
858 		"Format for type %s: %dx%d (%d bpp), fmt: %c%c%c%c\n",
859 		type_name(f->type), q_data->width, q_data->height,
860 		q_data->fmt->depth,
861 		(q_data->fmt->fourcc & 0xff),
862 		(q_data->fmt->fourcc >>  8) & 0xff,
863 		(q_data->fmt->fourcc >> 16) & 0xff,
864 		(q_data->fmt->fourcc >> 24) & 0xff);
865 
866 	return 0;
867 }
868 
869 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
870 				struct v4l2_format *f)
871 {
872 	int ret;
873 
874 	ret = vidioc_try_fmt_vid_cap(file, priv, f);
875 	if (ret)
876 		return ret;
877 
878 	return vidioc_s_fmt(file2ctx(file), f);
879 }
880 
881 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
882 				struct v4l2_format *f)
883 {
884 	struct vim2m_ctx *ctx = file2ctx(file);
885 	int ret;
886 
887 	ret = vidioc_try_fmt_vid_out(file, priv, f);
888 	if (ret)
889 		return ret;
890 
891 	ret = vidioc_s_fmt(file2ctx(file), f);
892 	if (!ret) {
893 		ctx->colorspace = f->fmt.pix.colorspace;
894 		ctx->xfer_func = f->fmt.pix.xfer_func;
895 		ctx->ycbcr_enc = f->fmt.pix.ycbcr_enc;
896 		ctx->quant = f->fmt.pix.quantization;
897 	}
898 	return ret;
899 }
900 
901 static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl)
902 {
903 	struct vim2m_ctx *ctx =
904 		container_of(ctrl->handler, struct vim2m_ctx, hdl);
905 
906 	switch (ctrl->id) {
907 	case V4L2_CID_HFLIP:
908 		if (ctrl->val)
909 			ctx->mode |= MEM2MEM_HFLIP;
910 		else
911 			ctx->mode &= ~MEM2MEM_HFLIP;
912 		break;
913 
914 	case V4L2_CID_VFLIP:
915 		if (ctrl->val)
916 			ctx->mode |= MEM2MEM_VFLIP;
917 		else
918 			ctx->mode &= ~MEM2MEM_VFLIP;
919 		break;
920 
921 	case V4L2_CID_TRANS_TIME_MSEC:
922 		ctx->transtime = ctrl->val;
923 		if (ctx->transtime < 1)
924 			ctx->transtime = 1;
925 		break;
926 
927 	case V4L2_CID_TRANS_NUM_BUFS:
928 		ctx->translen = ctrl->val;
929 		break;
930 
931 	default:
932 		v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
933 		return -EINVAL;
934 	}
935 
936 	return 0;
937 }
938 
939 static const struct v4l2_ctrl_ops vim2m_ctrl_ops = {
940 	.s_ctrl = vim2m_s_ctrl,
941 };
942 
943 static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
944 	.vidioc_querycap	= vidioc_querycap,
945 
946 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
947 	.vidioc_enum_framesizes = vidioc_enum_framesizes,
948 	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
949 	.vidioc_try_fmt_vid_cap	= vidioc_try_fmt_vid_cap,
950 	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
951 
952 	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
953 	.vidioc_g_fmt_vid_out	= vidioc_g_fmt_vid_out,
954 	.vidioc_try_fmt_vid_out	= vidioc_try_fmt_vid_out,
955 	.vidioc_s_fmt_vid_out	= vidioc_s_fmt_vid_out,
956 
957 	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
958 	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
959 	.vidioc_qbuf		= v4l2_m2m_ioctl_qbuf,
960 	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
961 	.vidioc_prepare_buf	= v4l2_m2m_ioctl_prepare_buf,
962 	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
963 	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
964 
965 	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
966 	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
967 
968 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
969 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
970 };
971 
972 /*
973  * Queue operations
974  */
975 
976 static int vim2m_queue_setup(struct vb2_queue *vq,
977 			     unsigned int *nbuffers,
978 			     unsigned int *nplanes,
979 			     unsigned int sizes[],
980 			     struct device *alloc_devs[])
981 {
982 	struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);
983 	struct vim2m_q_data *q_data;
984 	unsigned int size, count = *nbuffers;
985 
986 	q_data = get_q_data(ctx, vq->type);
987 	if (!q_data)
988 		return -EINVAL;
989 
990 	size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
991 
992 	while (size * count > MEM2MEM_VID_MEM_LIMIT)
993 		(count)--;
994 	*nbuffers = count;
995 
996 	if (*nplanes)
997 		return sizes[0] < size ? -EINVAL : 0;
998 
999 	*nplanes = 1;
1000 	sizes[0] = size;
1001 
1002 	dprintk(ctx->dev, 1, "%s: get %d buffer(s) of size %d each.\n",
1003 		type_name(vq->type), count, size);
1004 
1005 	return 0;
1006 }
1007 
1008 static int vim2m_buf_out_validate(struct vb2_buffer *vb)
1009 {
1010 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1011 	struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1012 
1013 	if (vbuf->field == V4L2_FIELD_ANY)
1014 		vbuf->field = V4L2_FIELD_NONE;
1015 	if (vbuf->field != V4L2_FIELD_NONE) {
1016 		dprintk(ctx->dev, 1, "%s field isn't supported\n", __func__);
1017 		return -EINVAL;
1018 	}
1019 
1020 	return 0;
1021 }
1022 
1023 static int vim2m_buf_prepare(struct vb2_buffer *vb)
1024 {
1025 	struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1026 	struct vim2m_q_data *q_data;
1027 
1028 	dprintk(ctx->dev, 2, "type: %s\n", type_name(vb->vb2_queue->type));
1029 
1030 	q_data = get_q_data(ctx, vb->vb2_queue->type);
1031 	if (!q_data)
1032 		return -EINVAL;
1033 	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
1034 		dprintk(ctx->dev, 1,
1035 			"%s data will not fit into plane (%lu < %lu)\n",
1036 			__func__, vb2_plane_size(vb, 0),
1037 			(long)q_data->sizeimage);
1038 		return -EINVAL;
1039 	}
1040 
1041 	vb2_set_plane_payload(vb, 0, q_data->sizeimage);
1042 
1043 	return 0;
1044 }
1045 
1046 static void vim2m_buf_queue(struct vb2_buffer *vb)
1047 {
1048 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1049 	struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1050 
1051 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1052 }
1053 
1054 static int vim2m_start_streaming(struct vb2_queue *q, unsigned int count)
1055 {
1056 	struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
1057 	struct vim2m_q_data *q_data = get_q_data(ctx, q->type);
1058 
1059 	if (!q_data)
1060 		return -EINVAL;
1061 
1062 	if (V4L2_TYPE_IS_OUTPUT(q->type))
1063 		ctx->aborting = 0;
1064 
1065 	q_data->sequence = 0;
1066 	return 0;
1067 }
1068 
1069 static void vim2m_stop_streaming(struct vb2_queue *q)
1070 {
1071 	struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
1072 	struct vb2_v4l2_buffer *vbuf;
1073 
1074 	cancel_delayed_work_sync(&ctx->work_run);
1075 
1076 	for (;;) {
1077 		if (V4L2_TYPE_IS_OUTPUT(q->type))
1078 			vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1079 		else
1080 			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1081 		if (!vbuf)
1082 			return;
1083 		v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,
1084 					   &ctx->hdl);
1085 		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
1086 	}
1087 }
1088 
1089 static void vim2m_buf_request_complete(struct vb2_buffer *vb)
1090 {
1091 	struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1092 
1093 	v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);
1094 }
1095 
1096 static const struct vb2_ops vim2m_qops = {
1097 	.queue_setup	 = vim2m_queue_setup,
1098 	.buf_out_validate	 = vim2m_buf_out_validate,
1099 	.buf_prepare	 = vim2m_buf_prepare,
1100 	.buf_queue	 = vim2m_buf_queue,
1101 	.start_streaming = vim2m_start_streaming,
1102 	.stop_streaming  = vim2m_stop_streaming,
1103 	.wait_prepare	 = vb2_ops_wait_prepare,
1104 	.wait_finish	 = vb2_ops_wait_finish,
1105 	.buf_request_complete = vim2m_buf_request_complete,
1106 };
1107 
1108 static int queue_init(void *priv, struct vb2_queue *src_vq,
1109 		      struct vb2_queue *dst_vq)
1110 {
1111 	struct vim2m_ctx *ctx = priv;
1112 	int ret;
1113 
1114 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1115 	src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1116 	src_vq->drv_priv = ctx;
1117 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1118 	src_vq->ops = &vim2m_qops;
1119 	src_vq->mem_ops = &vb2_vmalloc_memops;
1120 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1121 	src_vq->lock = &ctx->vb_mutex;
1122 	src_vq->supports_requests = true;
1123 
1124 	ret = vb2_queue_init(src_vq);
1125 	if (ret)
1126 		return ret;
1127 
1128 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1129 	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1130 	dst_vq->drv_priv = ctx;
1131 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1132 	dst_vq->ops = &vim2m_qops;
1133 	dst_vq->mem_ops = &vb2_vmalloc_memops;
1134 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1135 	dst_vq->lock = &ctx->vb_mutex;
1136 
1137 	return vb2_queue_init(dst_vq);
1138 }
1139 
1140 static struct v4l2_ctrl_config vim2m_ctrl_trans_time_msec = {
1141 	.ops = &vim2m_ctrl_ops,
1142 	.id = V4L2_CID_TRANS_TIME_MSEC,
1143 	.name = "Transaction Time (msec)",
1144 	.type = V4L2_CTRL_TYPE_INTEGER,
1145 	.min = 1,
1146 	.max = 10001,
1147 	.step = 1,
1148 };
1149 
1150 static const struct v4l2_ctrl_config vim2m_ctrl_trans_num_bufs = {
1151 	.ops = &vim2m_ctrl_ops,
1152 	.id = V4L2_CID_TRANS_NUM_BUFS,
1153 	.name = "Buffers Per Transaction",
1154 	.type = V4L2_CTRL_TYPE_INTEGER,
1155 	.def = 1,
1156 	.min = 1,
1157 	.max = MEM2MEM_DEF_NUM_BUFS,
1158 	.step = 1,
1159 };
1160 
1161 /*
1162  * File operations
1163  */
1164 static int vim2m_open(struct file *file)
1165 {
1166 	struct vim2m_dev *dev = video_drvdata(file);
1167 	struct vim2m_ctx *ctx = NULL;
1168 	struct v4l2_ctrl_handler *hdl;
1169 	int rc = 0;
1170 
1171 	if (mutex_lock_interruptible(&dev->dev_mutex))
1172 		return -ERESTARTSYS;
1173 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1174 	if (!ctx) {
1175 		rc = -ENOMEM;
1176 		goto open_unlock;
1177 	}
1178 
1179 	v4l2_fh_init(&ctx->fh, video_devdata(file));
1180 	file->private_data = &ctx->fh;
1181 	ctx->dev = dev;
1182 	hdl = &ctx->hdl;
1183 	v4l2_ctrl_handler_init(hdl, 4);
1184 	v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1185 	v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1186 
1187 	vim2m_ctrl_trans_time_msec.def = default_transtime;
1188 	v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_time_msec, NULL);
1189 	v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_num_bufs, NULL);
1190 	if (hdl->error) {
1191 		rc = hdl->error;
1192 		v4l2_ctrl_handler_free(hdl);
1193 		kfree(ctx);
1194 		goto open_unlock;
1195 	}
1196 	ctx->fh.ctrl_handler = hdl;
1197 	v4l2_ctrl_handler_setup(hdl);
1198 
1199 	ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
1200 	ctx->q_data[V4L2_M2M_SRC].width = 640;
1201 	ctx->q_data[V4L2_M2M_SRC].height = 480;
1202 	ctx->q_data[V4L2_M2M_SRC].sizeimage =
1203 		ctx->q_data[V4L2_M2M_SRC].width *
1204 		ctx->q_data[V4L2_M2M_SRC].height *
1205 		(ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);
1206 	ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
1207 	ctx->colorspace = V4L2_COLORSPACE_REC709;
1208 
1209 	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
1210 
1211 	mutex_init(&ctx->vb_mutex);
1212 	INIT_DELAYED_WORK(&ctx->work_run, device_work);
1213 
1214 	if (IS_ERR(ctx->fh.m2m_ctx)) {
1215 		rc = PTR_ERR(ctx->fh.m2m_ctx);
1216 
1217 		v4l2_ctrl_handler_free(hdl);
1218 		v4l2_fh_exit(&ctx->fh);
1219 		kfree(ctx);
1220 		goto open_unlock;
1221 	}
1222 
1223 	v4l2_fh_add(&ctx->fh);
1224 	atomic_inc(&dev->num_inst);
1225 
1226 	dprintk(dev, 1, "Created instance: %p, m2m_ctx: %p\n",
1227 		ctx, ctx->fh.m2m_ctx);
1228 
1229 open_unlock:
1230 	mutex_unlock(&dev->dev_mutex);
1231 	return rc;
1232 }
1233 
1234 static int vim2m_release(struct file *file)
1235 {
1236 	struct vim2m_dev *dev = video_drvdata(file);
1237 	struct vim2m_ctx *ctx = file2ctx(file);
1238 
1239 	dprintk(dev, 1, "Releasing instance %p\n", ctx);
1240 
1241 	v4l2_fh_del(&ctx->fh);
1242 	v4l2_fh_exit(&ctx->fh);
1243 	v4l2_ctrl_handler_free(&ctx->hdl);
1244 	mutex_lock(&dev->dev_mutex);
1245 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1246 	mutex_unlock(&dev->dev_mutex);
1247 	kfree(ctx);
1248 
1249 	atomic_dec(&dev->num_inst);
1250 
1251 	return 0;
1252 }
1253 
1254 static void vim2m_device_release(struct video_device *vdev)
1255 {
1256 	struct vim2m_dev *dev = container_of(vdev, struct vim2m_dev, vfd);
1257 
1258 	v4l2_device_unregister(&dev->v4l2_dev);
1259 	v4l2_m2m_release(dev->m2m_dev);
1260 #ifdef CONFIG_MEDIA_CONTROLLER
1261 	media_device_cleanup(&dev->mdev);
1262 #endif
1263 	kfree(dev);
1264 }
1265 
1266 static const struct v4l2_file_operations vim2m_fops = {
1267 	.owner		= THIS_MODULE,
1268 	.open		= vim2m_open,
1269 	.release	= vim2m_release,
1270 	.poll		= v4l2_m2m_fop_poll,
1271 	.unlocked_ioctl	= video_ioctl2,
1272 	.mmap		= v4l2_m2m_fop_mmap,
1273 };
1274 
1275 static const struct video_device vim2m_videodev = {
1276 	.name		= MEM2MEM_NAME,
1277 	.vfl_dir	= VFL_DIR_M2M,
1278 	.fops		= &vim2m_fops,
1279 	.ioctl_ops	= &vim2m_ioctl_ops,
1280 	.minor		= -1,
1281 	.release	= vim2m_device_release,
1282 	.device_caps	= V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
1283 };
1284 
1285 static const struct v4l2_m2m_ops m2m_ops = {
1286 	.device_run	= device_run,
1287 	.job_ready	= job_ready,
1288 	.job_abort	= job_abort,
1289 };
1290 
1291 static const struct media_device_ops m2m_media_ops = {
1292 	.req_validate = vb2_request_validate,
1293 	.req_queue = v4l2_m2m_request_queue,
1294 };
1295 
1296 static int vim2m_probe(struct platform_device *pdev)
1297 {
1298 	struct vim2m_dev *dev;
1299 	struct video_device *vfd;
1300 	int ret;
1301 
1302 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1303 	if (!dev)
1304 		return -ENOMEM;
1305 
1306 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1307 	if (ret)
1308 		goto error_free;
1309 
1310 	atomic_set(&dev->num_inst, 0);
1311 	mutex_init(&dev->dev_mutex);
1312 
1313 	dev->vfd = vim2m_videodev;
1314 	vfd = &dev->vfd;
1315 	vfd->lock = &dev->dev_mutex;
1316 	vfd->v4l2_dev = &dev->v4l2_dev;
1317 
1318 	video_set_drvdata(vfd, dev);
1319 	v4l2_info(&dev->v4l2_dev,
1320 		  "Device registered as /dev/video%d\n", vfd->num);
1321 
1322 	platform_set_drvdata(pdev, dev);
1323 
1324 	dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
1325 	if (IS_ERR(dev->m2m_dev)) {
1326 		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
1327 		ret = PTR_ERR(dev->m2m_dev);
1328 		dev->m2m_dev = NULL;
1329 		goto error_dev;
1330 	}
1331 
1332 #ifdef CONFIG_MEDIA_CONTROLLER
1333 	dev->mdev.dev = &pdev->dev;
1334 	strscpy(dev->mdev.model, "vim2m", sizeof(dev->mdev.model));
1335 	strscpy(dev->mdev.bus_info, "platform:vim2m",
1336 		sizeof(dev->mdev.bus_info));
1337 	media_device_init(&dev->mdev);
1338 	dev->mdev.ops = &m2m_media_ops;
1339 	dev->v4l2_dev.mdev = &dev->mdev;
1340 #endif
1341 
1342 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
1343 	if (ret) {
1344 		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1345 		goto error_m2m;
1346 	}
1347 
1348 #ifdef CONFIG_MEDIA_CONTROLLER
1349 	ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd,
1350 						 MEDIA_ENT_F_PROC_VIDEO_SCALER);
1351 	if (ret) {
1352 		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");
1353 		goto error_v4l2;
1354 	}
1355 
1356 	ret = media_device_register(&dev->mdev);
1357 	if (ret) {
1358 		v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");
1359 		goto error_m2m_mc;
1360 	}
1361 #endif
1362 	return 0;
1363 
1364 #ifdef CONFIG_MEDIA_CONTROLLER
1365 error_m2m_mc:
1366 	v4l2_m2m_unregister_media_controller(dev->m2m_dev);
1367 #endif
1368 error_v4l2:
1369 	video_unregister_device(&dev->vfd);
1370 	/* vim2m_device_release called by video_unregister_device to release various objects */
1371 	return ret;
1372 error_m2m:
1373 	v4l2_m2m_release(dev->m2m_dev);
1374 error_dev:
1375 	v4l2_device_unregister(&dev->v4l2_dev);
1376 error_free:
1377 	kfree(dev);
1378 
1379 	return ret;
1380 }
1381 
1382 static void vim2m_remove(struct platform_device *pdev)
1383 {
1384 	struct vim2m_dev *dev = platform_get_drvdata(pdev);
1385 
1386 	v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);
1387 
1388 #ifdef CONFIG_MEDIA_CONTROLLER
1389 	media_device_unregister(&dev->mdev);
1390 	v4l2_m2m_unregister_media_controller(dev->m2m_dev);
1391 #endif
1392 	video_unregister_device(&dev->vfd);
1393 }
1394 
1395 static struct platform_driver vim2m_pdrv = {
1396 	.probe		= vim2m_probe,
1397 	.remove_new	= vim2m_remove,
1398 	.driver		= {
1399 		.name	= MEM2MEM_NAME,
1400 	},
1401 };
1402 
1403 static void __exit vim2m_exit(void)
1404 {
1405 	platform_driver_unregister(&vim2m_pdrv);
1406 	platform_device_unregister(&vim2m_pdev);
1407 }
1408 
1409 static int __init vim2m_init(void)
1410 {
1411 	int ret;
1412 
1413 	ret = platform_device_register(&vim2m_pdev);
1414 	if (ret)
1415 		return ret;
1416 
1417 	ret = platform_driver_register(&vim2m_pdrv);
1418 	if (ret)
1419 		platform_device_unregister(&vim2m_pdev);
1420 
1421 	return ret;
1422 }
1423 
1424 module_init(vim2m_init);
1425 module_exit(vim2m_exit);
1426