vim2m.c

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