cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

vim2m.c (34139B)


      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 videobuf 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
     30MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
     31MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
     32MODULE_LICENSE("GPL");
     33MODULE_VERSION("0.2");
     34MODULE_ALIAS("mem2mem_testdev");
     35
     36static unsigned int debug;
     37module_param(debug, uint, 0644);
     38MODULE_PARM_DESC(debug, "debug level");
     39
     40/* Default transaction time in msec */
     41static unsigned int default_transtime = 40; /* Max 25 fps */
     42module_param(default_transtime, uint, 0644);
     43MODULE_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
     76static void vim2m_dev_release(struct device *dev)
     77{}
     78
     79static struct platform_device vim2m_pdev = {
     80	.name		= MEM2MEM_NAME,
     81	.dev.release	= vim2m_dev_release,
     82};
     83
     84struct vim2m_fmt {
     85	u32	fourcc;
     86	int	depth;
     87	/* Types the format can be used for */
     88	u32     types;
     89};
     90
     91static 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 */
    134struct 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
    142enum {
    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
    150static 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
    167static 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
    185struct 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
    198struct 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
    230static inline struct vim2m_ctx *file2ctx(struct file *file)
    231{
    232	return container_of(file->private_data, struct vim2m_ctx, fh);
    233}
    234
    235static 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
    248static 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
    263static 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
    281static 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
    439static 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 */
    565static 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
    578static 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 */
    592static 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
    614static 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 */
    645static 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
    655static 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
    686static 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
    692static 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
    698static 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
    719static 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
    746static 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
    752static 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
    758static 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
    785static 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
    810static 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
    833static 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
    869static 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
    881static 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
    901static 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
    939static const struct v4l2_ctrl_ops vim2m_ctrl_ops = {
    940	.s_ctrl = vim2m_s_ctrl,
    941};
    942
    943static 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
    976static 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
   1008static 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
   1023static 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
   1046static 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
   1054static 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
   1069static 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
   1089static 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
   1096static 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
   1108static 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
   1140static 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
   1150static 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 */
   1164static 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
   1229open_unlock:
   1230	mutex_unlock(&dev->dev_mutex);
   1231	return rc;
   1232}
   1233
   1234static 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
   1254static 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
   1266static 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
   1275static 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
   1285static const struct v4l2_m2m_ops m2m_ops = {
   1286	.device_run	= device_run,
   1287	.job_ready	= job_ready,
   1288	.job_abort	= job_abort,
   1289};
   1290
   1291static const struct media_device_ops m2m_media_ops = {
   1292	.req_validate = vb2_request_validate,
   1293	.req_queue = v4l2_m2m_request_queue,
   1294};
   1295
   1296static 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
   1365error_m2m_mc:
   1366	v4l2_m2m_unregister_media_controller(dev->m2m_dev);
   1367#endif
   1368error_v4l2:
   1369	video_unregister_device(&dev->vfd);
   1370	/* vim2m_device_release called by video_unregister_device to release various objects */
   1371	return ret;
   1372error_m2m:
   1373	v4l2_m2m_release(dev->m2m_dev);
   1374error_dev:
   1375	v4l2_device_unregister(&dev->v4l2_dev);
   1376error_free:
   1377	kfree(dev);
   1378
   1379	return ret;
   1380}
   1381
   1382static int 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	return 0;
   1395}
   1396
   1397static struct platform_driver vim2m_pdrv = {
   1398	.probe		= vim2m_probe,
   1399	.remove		= vim2m_remove,
   1400	.driver		= {
   1401		.name	= MEM2MEM_NAME,
   1402	},
   1403};
   1404
   1405static void __exit vim2m_exit(void)
   1406{
   1407	platform_driver_unregister(&vim2m_pdrv);
   1408	platform_device_unregister(&vim2m_pdev);
   1409}
   1410
   1411static int __init vim2m_init(void)
   1412{
   1413	int ret;
   1414
   1415	ret = platform_device_register(&vim2m_pdev);
   1416	if (ret)
   1417		return ret;
   1418
   1419	ret = platform_driver_register(&vim2m_pdrv);
   1420	if (ret)
   1421		platform_device_unregister(&vim2m_pdev);
   1422
   1423	return ret;
   1424}
   1425
   1426module_init(vim2m_init);
   1427module_exit(vim2m_exit);