virtualization: Add virtio-video driver as external module.

This driver should conform WIP spec v3 [1] with some updates for spec v4
[2], and, some unspecified features such as VIRTIO_VIDEO_DEVICE_CAMERA.

Imported from internal OpenSynergy's revision:

  bcc33b6b9e0156b381a70c54d2df02c57b63d270

Kernel was configured with necessary features for this driver:

  enable MEDIA_SUPPORT
  disable MEDIA_SUBDRV_AUTOSELECT
  enable MEDIA_PLATFORM_SUPPORT
  enable VIDEO_VIRTIO

Keep driver as an external module to simplify future updates.

[1]: https://lists.oasis-open.org/archives/virtio-dev/202002/msg00002.html
[2]: https://lists.oasis-open.org/archives/virtio-dev/202006/msg00072.html

Bug-AGL: SPEC-4148
Change-Id: Iea339194b22443f67b3e2ffddca84118357a2f15
Signed-off-by: Vasyl Vavrychuk <vasyl.vavrychuk@opensynergy.com>

1 files changed, 1282 insertions, 0 deletions

diff --git a/meta-egvirt/recipes-kernel/kernel-module-virtio-video/files/virtio_video_device.c b/meta-egvirt/recipes-kernel/kernel-module-virtio-video/files/virtio_video_device.c
new file mode 100644
index 00000000..2e1f90ae
--- /dev/null
+++ b/meta-egvirt/recipes-kernel/kernel-module-virtio-video/files/virtio_video_device.c
@@ -0,0 +1,1282 @@
+// SPDX-License-Identifier: GPL-2.0+
+/* Driver for virtio video device.
+ *
+ * Copyright 2020 OpenSynergy GmbH.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/version.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "virtio_video.h"
+
+enum video_stream_state virtio_video_state(struct virtio_video_stream *stream)
+{
+	return atomic_read(&stream->state);
+}
+
+void virtio_video_state_reset(struct virtio_video_stream *stream)
+{
+	atomic_set(&stream->state, STREAM_STATE_IDLE);
+}
+
+void virtio_video_state_update(struct virtio_video_stream *stream,
+			       enum video_stream_state new_state)
+{
+	enum video_stream_state prev_state;
+
+	do {
+	    prev_state = atomic_read(&stream->state);
+	    if (prev_state == STREAM_STATE_ERROR)
+		    return;
+	} while (atomic_cmpxchg(&stream->state, prev_state, new_state) !=
+		 prev_state);
+}
+
+int virtio_video_pending_buf_list_empty(struct virtio_video_device *vvd)
+{
+	int ret = 0;
+
+	if (vvd->is_m2m_dev) {
+		v4l2_err(&vvd->v4l2_dev, "Unexpected call for m2m device!\n");
+		return -EPERM;
+	}
+
+	spin_lock(&vvd->pending_buf_list_lock);
+	if (list_empty(&vvd->pending_buf_list))
+		ret = 1;
+	spin_unlock(&vvd->pending_buf_list_lock);
+
+	return ret;
+}
+
+int virtio_video_pending_buf_list_pop(struct virtio_video_device *vvd,
+				      struct virtio_video_buffer **virtio_vb)
+{
+	struct virtio_video_buffer *retbuf;
+
+	if (vvd->is_m2m_dev) {
+		v4l2_err(&vvd->v4l2_dev, "Unexpected call for m2m device!\n");
+		return -EPERM;
+	}
+
+	spin_lock(&vvd->pending_buf_list_lock);
+	if (list_empty(&vvd->pending_buf_list)) {
+		spin_unlock(&vvd->pending_buf_list_lock);
+		return -EAGAIN;
+	}
+
+	retbuf = list_first_entry(&vvd->pending_buf_list,
+				  struct virtio_video_buffer, list);
+	spin_unlock(&vvd->pending_buf_list_lock);
+
+	*virtio_vb = retbuf;
+	return 0;
+}
+
+int virtio_video_pending_buf_list_add(struct virtio_video_device *vvd,
+				      struct virtio_video_buffer *virtio_vb)
+{
+	if (vvd->is_m2m_dev) {
+		v4l2_err(&vvd->v4l2_dev, "Unexpected call for m2m device!\n");
+		return -EPERM;
+	}
+
+	spin_lock(&vvd->pending_buf_list_lock);
+	list_add_tail(&virtio_vb->list, &vvd->pending_buf_list);
+	spin_unlock(&vvd->pending_buf_list_lock);
+
+	return 0;
+}
+
+int virtio_video_pending_buf_list_del(struct virtio_video_device *vvd,
+				      struct virtio_video_buffer *virtio_vb)
+{
+	struct virtio_video_buffer *vb, *vb_tmp;
+	int ret = -EINVAL;
+
+	if (vvd->is_m2m_dev) {
+		v4l2_err(&vvd->v4l2_dev, "Unexpected call for m2m device!\n");
+		return -EPERM;
+	}
+
+	spin_lock(&vvd->pending_buf_list_lock);
+	if (list_empty(&vvd->pending_buf_list)) {
+		spin_unlock(&vvd->pending_buf_list_lock);
+		return -EAGAIN;
+	}
+
+	list_for_each_entry_safe(vb, vb_tmp, &vvd->pending_buf_list, list) {
+		if (vb->resource_id == virtio_vb->resource_id) {
+			list_del(&vb->list);
+			ret = 0;
+			break;
+		}
+	}
+	spin_unlock(&vvd->pending_buf_list_lock);
+
+	return ret;
+}
+
+int virtio_video_queue_setup(struct vb2_queue *vq, unsigned int *num_buffers,
+			     unsigned int *num_planes, unsigned int sizes[],
+			     struct device *alloc_devs[])
+{
+	int i;
+	struct virtio_video_stream *stream = vb2_get_drv_priv(vq);
+	struct video_format_info *p_info;
+
+	if (virtio_video_state(stream) == STREAM_STATE_ERROR)
+		return -EIO;
+
+	if (*num_planes)
+		return 0;
+
+	if (V4L2_TYPE_IS_OUTPUT(vq->type))
+		p_info = &stream->in_info;
+	else
+		p_info = &stream->out_info;
+
+	*num_planes = p_info->num_planes;
+
+	for (i = 0; i < p_info->num_planes; i++)
+		sizes[i] = p_info->plane_format[i].plane_size;
+
+	return 0;
+}
+
+static unsigned int
+build_virtio_video_sglist_contig(struct virtio_video_resource_sg_list *sgl,
+			         struct vb2_buffer *vb, unsigned int plane)
+{
+	sgl->entries[0].addr = cpu_to_le64(vb2_dma_contig_plane_dma_addr(vb, plane));
+	sgl->entries[0].length = cpu_to_le32(vb->planes[plane].length);
+
+	sgl->num_entries = 1;
+
+	return VIRTIO_VIDEO_RESOURCE_SG_SIZE(1);
+}
+
+static unsigned int
+build_virtio_video_sglist(struct virtio_video_resource_sg_list *sgl,
+			  struct vb2_buffer *vb, unsigned int plane,
+			  bool has_iommu)
+{
+	int i;
+	struct scatterlist *sg;
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, plane);
+
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		sgl->entries[i].addr = cpu_to_le64(has_iommu
+							? sg_dma_address(sg)
+							: sg_phys(sg));
+		sgl->entries[i].length = cpu_to_le32(sg->length);
+	}
+
+	sgl->num_entries = sgt->nents;
+
+	return VIRTIO_VIDEO_RESOURCE_SG_SIZE(sgt->nents);
+}
+
+int virtio_video_buf_init(struct vb2_buffer *vb)
+{
+	int ret = 0;
+	void *buf;
+	size_t buf_size = 0;
+	struct virtio_video_resource_sg_list *sg_list;
+	unsigned int i, offset = 0, resource_id, nents = 0;
+	struct vb2_queue *vq = vb->vb2_queue;
+	enum v4l2_buf_type queue_type = vq->type;
+	struct virtio_video_stream *stream = vb2_get_drv_priv(vq);
+	struct virtio_video_buffer *virtio_vb = to_virtio_vb(vb);
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+
+	if (vvd->supp_non_contig) {
+		for (i = 0; i < vb->num_planes; i++) {
+			nents = vb2_dma_sg_plane_desc(vb, i)->nents;
+			buf_size += VIRTIO_VIDEO_RESOURCE_SG_SIZE(nents);
+		}
+
+		buf = kcalloc(1, buf_size, GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
+
+		for (i = 0; i < vb->num_planes; i++) {
+			sg_list = buf + offset;
+			offset += build_virtio_video_sglist(sg_list, vb, i,
+							    vvd->has_iommu);
+		}
+	} else {
+		buf_size = vb->num_planes * VIRTIO_VIDEO_RESOURCE_SG_SIZE(nents);
+
+		buf = kcalloc(1, buf_size, GFP_KERNEL);
+		if (!buf)
+			return -ENOMEM;
+
+		for (i = 0; i < vb->num_planes; i++) {
+			sg_list = buf + offset;
+			offset += build_virtio_video_sglist_contig(sg_list,
+								   vb, i);
+		}
+	}
+
+	virtio_video_resource_id_get(vvd, &resource_id);
+
+	ret = virtio_video_cmd_resource_attach(vvd, stream->stream_id,
+					       resource_id,
+					       to_virtio_queue_type(queue_type),
+					       buf, buf_size);
+	if (ret) {
+		virtio_video_resource_id_put(vvd, resource_id);
+		kfree(buf);
+		return ret;
+	}
+
+	virtio_vb->queued = false;
+	virtio_vb->resource_id = resource_id;
+
+	return 0;
+}
+
+void virtio_video_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct virtio_video_stream *stream = vb2_get_drv_priv(vb->vb2_queue);
+	struct virtio_video_buffer *virtio_vb = to_virtio_vb(vb);
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+
+	virtio_video_resource_id_put(vvd, virtio_vb->resource_id);
+}
+
+void virtio_video_buf_queue(struct vb2_buffer *vb)
+{
+	int i, ret;
+	struct virtio_video_buffer *virtio_vb;
+	uint32_t data_size[VB2_MAX_PLANES] = {0};
+	struct virtio_video_stream *stream = vb2_get_drv_priv(vb->vb2_queue);
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+
+	for (i = 0; i < vb->num_planes; ++i)
+		data_size[i] = vb->planes[i].bytesused;
+
+	virtio_vb = to_virtio_vb(vb);
+
+	if (!vvd->is_m2m_dev)
+		virtio_video_pending_buf_list_add(vvd, virtio_vb);
+
+	ret = virtio_video_cmd_resource_queue(vvd, stream->stream_id,
+					      virtio_vb, data_size,
+					      vb->num_planes,
+					      to_virtio_queue_type(vb->type));
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to queue buffer\n");
+		return;
+	}
+
+	virtio_vb->queued = true;
+}
+
+int virtio_video_qbuf(struct file *file, void *priv,
+		      struct v4l2_buffer *buf)
+{
+	struct virtio_video_stream *stream = file2stream(file);
+	struct virtio_video_device *vvd = video_drvdata(file);
+
+	if (virtio_video_state(stream) == STREAM_STATE_ERROR)
+		return -EIO;
+
+	if (vvd->is_m2m_dev)
+		return v4l2_m2m_ioctl_qbuf(file, priv, buf);
+
+	return vb2_ioctl_qbuf(file, priv, buf);
+}
+
+int virtio_video_dqbuf(struct file *file, void *priv,
+		       struct v4l2_buffer *buf)
+{
+	struct virtio_video_stream *stream = file2stream(file);
+	struct virtio_video_device *vvd = video_drvdata(file);
+
+	if (virtio_video_state(stream) == STREAM_STATE_ERROR)
+		return -EIO;
+
+	if (vvd->is_m2m_dev)
+		return v4l2_m2m_ioctl_dqbuf(file, priv, buf);
+
+	return vb2_ioctl_dqbuf(file, priv, buf);
+}
+
+int virtio_video_querycap(struct file *file, void *fh,
+			  struct v4l2_capability *cap)
+{
+	struct video_device *video_dev = video_devdata(file);
+	struct virtio_video_device *vvd = video_drvdata(file);
+
+	if (strscpy(cap->driver, DRIVER_NAME, sizeof(cap->driver)) < 0)
+		v4l2_err(&vvd->v4l2_dev, "failed to copy driver name\n");
+	if (strscpy(cap->card, video_dev->name, sizeof(cap->card)) < 0)
+		v4l2_err(&vvd->v4l2_dev, "failed to copy card name\n");
+
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "virtio:%s",
+		 video_dev->name);
+
+	cap->device_caps = video_dev->device_caps;
+	return 0;
+}
+
+int virtio_video_enum_framesizes(struct file *file, void *fh,
+				 struct v4l2_frmsizeenum *f)
+{
+	struct virtio_video_stream *stream = file2stream(file);
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+	struct video_format *fmt;
+	struct video_format_frame *frm;
+	struct virtio_video_format_frame *frame;
+	int idx = f->index;
+
+	fmt = virtio_video_find_video_format(&vvd->input_fmt_list,
+					     f->pixel_format);
+	if (fmt == NULL)
+		fmt = virtio_video_find_video_format(&vvd->output_fmt_list,
+						     f->pixel_format);
+	if (fmt == NULL)
+		return -EINVAL;
+
+	if (idx >= fmt->desc.num_frames)
+		return -EINVAL;
+
+	frm = &fmt->frames[idx];
+	frame = &frm->frame;
+
+	if (frame->width.min == frame->width.max &&
+	    frame->height.min == frame->height.max) {
+		f->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		f->discrete.width = frame->width.min;
+		f->discrete.height = frame->height.min;
+		return 0;
+	}
+
+	f->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+	f->stepwise.min_width = frame->width.min;
+	f->stepwise.max_width = frame->width.max;
+	f->stepwise.min_height = frame->height.min;
+	f->stepwise.max_height = frame->height.max;
+	f->stepwise.step_width = frame->width.step;
+	f->stepwise.step_height = frame->height.step;
+	return 0;
+}
+
+static bool in_stepped_interval(struct virtio_video_format_range range,
+				uint32_t point)
+{
+	if (point < range.min || point > range.max)
+		return false;
+
+	if (range.step == 0 && range.min == range.max && range.min == point)
+		return true;
+
+	if (range.step != 0 && (point - range.min) % range.step == 0)
+		return true;
+
+	return false;
+}
+
+int virtio_video_enum_framemintervals(struct file *file, void *fh,
+				      struct v4l2_frmivalenum *f)
+{
+	struct virtio_video_stream *stream = file2stream(file);
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+	struct video_format *fmt;
+	struct video_format_frame *frm;
+	struct virtio_video_format_frame *frame;
+	struct virtio_video_format_range *frate;
+	int idx = f->index;
+	int f_idx;
+
+	fmt = virtio_video_find_video_format(&vvd->input_fmt_list,
+					     f->pixel_format);
+	if (fmt == NULL)
+		fmt = virtio_video_find_video_format(&vvd->output_fmt_list,
+						     f->pixel_format);
+	if (fmt == NULL)
+		return -EINVAL;
+
+	for (f_idx = 0; f_idx <= fmt->desc.num_frames; f_idx++) {
+		frm = &fmt->frames[f_idx];
+		frame = &frm->frame;
+		if (in_stepped_interval(frame->width, f->width) &&
+		    in_stepped_interval(frame->height, f->height))
+			break;
+	}
+
+	if (frame == NULL || f->index >= frame->num_rates)
+		return -EINVAL;
+
+	frate = &frm->frame_rates[idx];
+	if (frate->max == frate->min) {
+		f->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+		f->discrete.numerator = 1;
+		f->discrete.denominator = frate->max;
+	} else {
+		f->stepwise.min.numerator = 1;
+		f->stepwise.min.denominator = frate->max;
+		f->stepwise.max.numerator = 1;
+		f->stepwise.max.denominator = frate->min;
+		f->stepwise.step.numerator = 1;
+		f->stepwise.step.denominator = frate->step;
+		if (frate->step == 1)
+			f->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
+		else
+			f->type = V4L2_FRMIVAL_TYPE_STEPWISE;
+	}
+	return 0;
+}
+
+int virtio_video_stream_get_params(struct virtio_video_device *vvd,
+				   struct virtio_video_stream *stream)
+{
+	int ret;
+
+	if (vvd->is_m2m_dev) {
+		ret = virtio_video_cmd_get_params(vvd, stream,
+						VIRTIO_VIDEO_QUEUE_TYPE_INPUT);
+		if (ret) {
+			v4l2_err(&vvd->v4l2_dev,
+				 "failed to get stream in params\n");
+			goto err_get_parms;
+		}
+	}
+
+	ret = virtio_video_cmd_get_params(vvd, stream,
+					  VIRTIO_VIDEO_QUEUE_TYPE_OUTPUT);
+	if (ret)
+		v4l2_err(&vvd->v4l2_dev, "failed to get stream out params\n");
+
+err_get_parms:
+	return ret;
+}
+
+int virtio_video_stream_get_controls(struct virtio_video_device *vvd,
+				     struct virtio_video_stream *stream)
+{
+	int ret;
+
+	ret = virtio_video_cmd_get_control(vvd, stream,
+					   VIRTIO_VIDEO_CONTROL_PROFILE);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to get stream profile\n");
+		goto err_get_ctrl;
+	}
+
+	ret = virtio_video_cmd_get_control(vvd, stream,
+					   VIRTIO_VIDEO_CONTROL_LEVEL);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to get stream level\n");
+		goto err_get_ctrl;
+	}
+
+	ret = virtio_video_cmd_get_control(vvd, stream,
+					   VIRTIO_VIDEO_CONTROL_BITRATE);
+	if (ret)
+		v4l2_err(&vvd->v4l2_dev, "failed to get stream bitrate\n");
+
+err_get_ctrl:
+	return ret;
+}
+
+int virtio_video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	struct video_format_info *info;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct virtio_video_stream *stream = file2stream(file);
+
+	if (!V4L2_TYPE_IS_OUTPUT(f->type))
+		info = &stream->out_info;
+	else
+		info = &stream->in_info;
+
+	virtio_video_format_from_info(info, pix_mp);
+
+	return 0;
+}
+
+int virtio_video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+	int i, ret;
+	struct virtio_video_stream *stream = file2stream(file);
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+	struct video_format_info info;
+	struct video_format_info *p_info;
+	uint32_t queue;
+
+	ret = virtio_video_try_fmt(stream, f);
+	if (ret)
+		return ret;
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		virtio_video_format_fill_default_info(&info, &stream->in_info);
+		queue = VIRTIO_VIDEO_QUEUE_TYPE_INPUT;
+	} else {
+		virtio_video_format_fill_default_info(&info, &stream->out_info);
+		queue = VIRTIO_VIDEO_QUEUE_TYPE_OUTPUT;
+	}
+
+	info.frame_width = pix_mp->width;
+	info.frame_height = pix_mp->height;
+	info.num_planes = pix_mp->num_planes;
+	info.fourcc_format = pix_mp->pixelformat;
+
+	for (i = 0; i < info.num_planes; i++) {
+		info.plane_format[i].stride =
+					 pix_mp->plane_fmt[i].bytesperline;
+		info.plane_format[i].plane_size =
+					 pix_mp->plane_fmt[i].sizeimage;
+	}
+
+	virtio_video_cmd_set_params(vvd, stream, &info, queue);
+	virtio_video_stream_get_params(vvd, stream);
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		p_info = &stream->in_info;
+	else
+		p_info = &stream->out_info;
+
+	virtio_video_format_from_info(p_info, pix_mp);
+
+	return 0;
+}
+
+int virtio_video_g_selection(struct file *file, void *fh,
+			 struct v4l2_selection *sel)
+{
+	struct video_format_info *info;
+	struct virtio_video_stream *stream = file2stream(file);
+	struct virtio_video_device *vvd = video_drvdata(file);
+
+	switch (vvd->type) {
+	case VIRTIO_VIDEO_DEVICE_ENCODER:
+		if (!V4L2_TYPE_IS_OUTPUT(sel->type))
+			return -EINVAL;
+		info = &stream->in_info;
+		break;
+	case VIRTIO_VIDEO_DEVICE_DECODER:
+	case VIRTIO_VIDEO_DEVICE_CAMERA:
+		if (V4L2_TYPE_IS_OUTPUT(sel->type))
+			return -EINVAL;
+		info = &stream->out_info;
+		break;
+	default:
+		v4l2_err(&vvd->v4l2_dev, "unsupported device type\n");
+		return -EINVAL;
+	}
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		sel->r.width = info->frame_width;
+		sel->r.height = info->frame_height;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		sel->r.top = info->crop.top;
+		sel->r.left = info->crop.left;
+		sel->r.width = info->frame_width;
+		sel->r.height = info->frame_height;
+		break;
+	default:
+		v4l2_dbg(1, vvd->debug, &vvd->v4l2_dev,
+			 "unsupported/invalid selection target: %d\n",
+			 sel->target);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int virtio_video_try_fmt(struct virtio_video_stream *stream,
+			 struct v4l2_format *f)
+{
+	int i, idx = 0;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+	struct video_format *fmt;
+	bool found = false;
+	struct video_format_frame *frm;
+	struct virtio_video_format_frame *frame;
+
+	if (virtio_video_state(stream) == STREAM_STATE_ERROR)
+		return -EIO;
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		fmt = virtio_video_find_video_format(&vvd->input_fmt_list,
+						     pix_mp->pixelformat);
+	else
+		fmt = virtio_video_find_video_format(&vvd->output_fmt_list,
+						     pix_mp->pixelformat);
+
+	if (!fmt) {
+		if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+			virtio_video_format_from_info(&stream->out_info,
+						      pix_mp);
+		else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+			virtio_video_format_from_info(&stream->in_info,
+						      pix_mp);
+		else
+			return -EINVAL;
+		return 0;
+	}
+
+	for (i = 0; i < fmt->desc.num_frames && !found; i++) {
+		frm = &fmt->frames[i];
+		frame = &frm->frame;
+		if (!within_range(frame->width.min, pix_mp->width,
+				  frame->width.max))
+			continue;
+
+		if (!within_range(frame->height.min, pix_mp->height,
+				  frame->height.max))
+			continue;
+		idx = i;
+		/*
+		 * Try to find a more suitable frame size. Go with the current
+		 * one otherwise.
+		 */
+		if (needs_alignment(pix_mp->width, frame->width.step))
+			continue;
+
+		if (needs_alignment(pix_mp->height, frame->height.step))
+			continue;
+
+		stream->current_frame = frm;
+		found = true;
+	}
+
+	if (!found) {
+		frm = &fmt->frames[idx];
+		if (!frm)
+			return -EINVAL;
+
+		frame = &frm->frame;
+		pix_mp->width = clamp(pix_mp->width, frame->width.min,
+				      frame->width.max);
+		if (frame->width.step != 0)
+			pix_mp->width = ALIGN(pix_mp->width, frame->width.step);
+
+		pix_mp->height = clamp(pix_mp->height, frame->height.min,
+				       frame->height.max);
+		if (frame->height.step != 0)
+			pix_mp->height = ALIGN(pix_mp->height,
+					       frame->height.step);
+		stream->current_frame = frm;
+	}
+
+	return 0;
+}
+
+static int virtio_video_queue_free(struct virtio_video_device *vvd,
+				   struct virtio_video_stream *stream,
+				   enum v4l2_buf_type type)
+{
+	int ret;
+	enum virtio_video_queue_type queue_type = to_virtio_queue_type(type);
+
+	ret = virtio_video_cmd_queue_detach_resources(vvd, stream, queue_type);
+	if (ret) {
+		v4l2_warn(&vvd->v4l2_dev,
+			  "failed to destroy resources\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+int virtio_video_reqbufs(struct file *file, void *priv,
+			 struct v4l2_requestbuffers *rb)
+{
+	int ret;
+	struct virtio_video_stream *stream = file2stream(file);
+	struct v4l2_m2m_ctx *m2m_ctx = stream->fh.m2m_ctx;
+	struct virtio_video_device *vvd = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+	struct vb2_queue *vq;
+
+	if (vvd->is_m2m_dev)
+		vq = v4l2_m2m_get_vq(m2m_ctx, rb->type);
+	else
+		vq = vdev->queue;
+
+	if (rb->count == 0) {
+		ret = virtio_video_queue_free(vvd, stream, vq->type);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (vvd->is_m2m_dev)
+		return v4l2_m2m_reqbufs(file, m2m_ctx, rb);
+	else
+		return vb2_ioctl_reqbufs(file, priv, rb);
+}
+
+int virtio_video_subscribe_event(struct v4l2_fh *fh,
+				 const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
+	default:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	}
+}
+
+void virtio_video_queue_eos_event(struct virtio_video_stream *stream)
+{
+	static const struct v4l2_event eos_event = {
+		.type = V4L2_EVENT_EOS
+	};
+
+	v4l2_event_queue_fh(&stream->fh, &eos_event);
+}
+
+void virtio_video_queue_res_chg_event(struct virtio_video_stream *stream)
+{
+	static const struct v4l2_event ev_src_ch = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes =
+			V4L2_EVENT_SRC_CH_RESOLUTION,
+	};
+
+	v4l2_event_queue_fh(&stream->fh, &ev_src_ch);
+}
+
+void virtio_video_handle_error(struct virtio_video_stream *stream)
+{
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+
+	if (vvd->is_m2m_dev)
+		virtio_video_queue_release_buffers
+			(stream, VIRTIO_VIDEO_QUEUE_TYPE_INPUT);
+
+	virtio_video_queue_release_buffers
+		(stream, VIRTIO_VIDEO_QUEUE_TYPE_OUTPUT);
+}
+
+int virtio_video_queue_release_buffers(struct virtio_video_stream *stream,
+				       enum virtio_video_queue_type queue_type)
+{
+	int ret;
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+	struct vb2_v4l2_buffer *v4l2_vb;
+	struct virtio_video_buffer *vvb;
+
+	ret = virtio_video_cmd_queue_clear(vvd, stream, queue_type);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to clear queue\n");
+		return ret;
+	}
+
+	if (!vvd->is_m2m_dev) {
+		while (!virtio_video_pending_buf_list_pop(vvd, &vvb) && vvb) {
+			v4l2_vb = &vvb->v4l2_m2m_vb.vb;
+			v4l2_m2m_buf_done(v4l2_vb, VB2_BUF_STATE_ERROR);
+		}
+		return 0;
+	}
+
+	for (;;) {
+		if (queue_type == VIRTIO_VIDEO_QUEUE_TYPE_INPUT)
+			v4l2_vb = v4l2_m2m_src_buf_remove(stream->fh.m2m_ctx);
+		else
+			v4l2_vb = v4l2_m2m_dst_buf_remove(stream->fh.m2m_ctx);
+		if (!v4l2_vb)
+			break;
+		v4l2_m2m_buf_done(v4l2_vb, VB2_BUF_STATE_ERROR);
+	}
+
+	return 0;
+}
+
+void virtio_video_buf_done(struct virtio_video_buffer *virtio_vb,
+			   uint32_t flags, uint64_t timestamp,
+			   uint32_t data_sizes[])
+{
+	int i;
+	enum vb2_buffer_state done_state = VB2_BUF_STATE_DONE;
+	struct vb2_v4l2_buffer *v4l2_vb = &virtio_vb->v4l2_m2m_vb.vb;
+	struct vb2_buffer *vb = &v4l2_vb->vb2_buf;
+	struct vb2_queue *vb2_queue = vb->vb2_queue;
+	struct virtio_video_stream *stream = vb2_get_drv_priv(vb2_queue);
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+	struct video_format_info *p_info;
+
+	virtio_vb->queued = false;
+
+	if (flags & VIRTIO_VIDEO_DEQUEUE_FLAG_ERR)
+		done_state = VB2_BUF_STATE_ERROR;
+
+	if (flags & VIRTIO_VIDEO_DEQUEUE_FLAG_KEY_FRAME)
+		v4l2_vb->flags |= V4L2_BUF_FLAG_KEYFRAME;
+
+	if (flags & VIRTIO_VIDEO_DEQUEUE_FLAG_BFRAME)
+		v4l2_vb->flags |= V4L2_BUF_FLAG_BFRAME;
+
+	if (flags & VIRTIO_VIDEO_DEQUEUE_FLAG_PFRAME)
+		v4l2_vb->flags |= V4L2_BUF_FLAG_PFRAME;
+
+	if (flags & VIRTIO_VIDEO_DEQUEUE_FLAG_EOS) {
+		v4l2_vb->flags |= V4L2_BUF_FLAG_LAST;
+		virtio_video_state_update(stream, STREAM_STATE_STOPPED);
+		virtio_video_queue_eos_event(stream);
+	}
+
+	if ((flags & VIRTIO_VIDEO_DEQUEUE_FLAG_ERR) ||
+	    (flags & VIRTIO_VIDEO_DEQUEUE_FLAG_EOS)) {
+		vb->planes[0].bytesused = 0;
+
+		if (!vvd->is_m2m_dev)
+			virtio_video_pending_buf_list_del(vvd, virtio_vb);
+
+		v4l2_m2m_buf_done(v4l2_vb, done_state);
+		return;
+	}
+
+	if (!V4L2_TYPE_IS_OUTPUT(vb2_queue->type)) {
+		switch (vvd->type) {
+		case VIRTIO_VIDEO_DEVICE_ENCODER:
+			for (i = 0; i < vb->num_planes; i++)
+				vb->planes[i].bytesused =
+					le32_to_cpu(data_sizes[i]);
+			break;
+		case VIRTIO_VIDEO_DEVICE_CAMERA:
+		case VIRTIO_VIDEO_DEVICE_DECODER:
+			p_info = &stream->out_info;
+			for (i = 0; i < p_info->num_planes; i++)
+				vb->planes[i].bytesused =
+					p_info->plane_format[i].plane_size;
+			break;
+		}
+
+		vb->timestamp = timestamp;
+	}
+
+	if (!vvd->is_m2m_dev)
+		virtio_video_pending_buf_list_del(vvd, virtio_vb);
+
+	v4l2_m2m_buf_done(v4l2_vb, done_state);
+}
+
+static int virtio_video_set_device_busy(struct virtio_video_device *vvd)
+{
+	struct video_device *vd = &vvd->video_dev;
+	int ret = 0;
+
+	/* Multiple open is allowed for m2m device */
+	if (vvd->is_m2m_dev)
+		return 0;
+
+	mutex_lock(vd->lock);
+
+	if (vvd->device_busy)
+		ret = -EBUSY;
+	else
+		vvd->device_busy = true;
+
+	mutex_unlock(vd->lock);
+
+	return ret;
+}
+
+static void virtio_video_clear_device_busy(struct virtio_video_device *vvd,
+					   struct mutex *lock)
+{
+	/* Nothing to do for m2m device */
+	if (vvd->is_m2m_dev)
+		return;
+
+	if (lock)
+		mutex_lock(lock);
+
+	vvd->device_busy = false;
+
+	if (lock)
+		mutex_unlock(lock);
+}
+
+static int virtio_video_device_open(struct file *file)
+{
+	int ret;
+	uint32_t stream_id;
+	char name[TASK_COMM_LEN];
+	struct virtio_video_stream *stream;
+	struct video_format *default_fmt;
+	enum virtio_video_format format;
+	struct video_device *video_dev = video_devdata(file);
+	struct virtio_video_device *vvd = video_drvdata(file);
+
+	ret = virtio_video_set_device_busy(vvd);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "device already in use.\n");
+		return ret;
+	}
+
+	default_fmt = list_first_entry_or_null(vvd->ops->get_fmt_list(vvd),
+					       struct video_format,
+					       formats_list_entry);
+	if (!default_fmt) {
+		v4l2_err(&vvd->v4l2_dev, "device failed to start\n");
+		ret = -EIO;
+		goto err;
+	}
+
+	stream = kzalloc(sizeof(*stream), GFP_KERNEL);
+	if (!stream) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	get_task_comm(name, current);
+	format = virtio_video_v4l2_format_to_virtio(default_fmt->desc.format);
+	virtio_video_stream_id_get(vvd, stream, &stream_id);
+	ret = virtio_video_cmd_stream_create(vvd, stream_id, format, name);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to create stream\n");
+		goto err_stream_create;
+	}
+
+	stream->video_dev = video_dev;
+	stream->stream_id = stream_id;
+
+	virtio_video_state_reset(stream);
+
+	ret = virtio_video_stream_get_params(vvd, stream);
+	if (ret)
+		goto err_stream_create;
+
+	if (format >= VIRTIO_VIDEO_FORMAT_CODED_MIN &&
+	    format <= VIRTIO_VIDEO_FORMAT_CODED_MAX) {
+		ret = virtio_video_stream_get_controls(vvd, stream);
+		if (ret)
+			goto err_stream_create;
+	}
+
+	mutex_init(&stream->vq_mutex);
+	v4l2_fh_init(&stream->fh, video_dev);
+	stream->fh.ctrl_handler = &stream->ctrl_handler;
+
+	if (vvd->is_m2m_dev) {
+		stream->fh.m2m_ctx = v4l2_m2m_ctx_init(vvd->m2m_dev, stream,
+						       vvd->ops->init_queues);
+		if (IS_ERR(stream->fh.m2m_ctx)) {
+			ret = PTR_ERR(stream->fh.m2m_ctx);
+			goto err_init_ctx;
+		}
+
+		v4l2_m2m_set_src_buffered(stream->fh.m2m_ctx, true);
+		v4l2_m2m_set_dst_buffered(stream->fh.m2m_ctx, true);
+	} else {
+		vvd->ops->init_queues(stream, NULL, &vvd->vb2_output_queue);
+		/* Video dev queue is required for vb2 ioctl wrappers */
+		video_dev->queue = &vvd->vb2_output_queue;
+	}
+
+	file->private_data = &stream->fh;
+	v4l2_fh_add(&stream->fh);
+
+	if (vvd->ops->init_ctrls) {
+		ret = vvd->ops->init_ctrls(stream);
+		if (ret) {
+			v4l2_err(&vvd->v4l2_dev, "failed to init controls\n");
+			goto err_init_ctrls;
+		}
+	}
+	return 0;
+
+err_init_ctrls:
+	v4l2_fh_del(&stream->fh);
+	mutex_lock(video_dev->lock);
+	if (vvd->is_m2m_dev)
+		v4l2_m2m_ctx_release(stream->fh.m2m_ctx);
+	mutex_unlock(video_dev->lock);
+err_init_ctx:
+	v4l2_fh_exit(&stream->fh);
+err_stream_create:
+	virtio_video_stream_id_put(vvd, stream_id);
+	kfree(stream);
+err:
+	virtio_video_clear_device_busy(vvd, video_dev->lock);
+	return ret;
+}
+
+static int virtio_video_device_release(struct file *file)
+{
+	struct virtio_video_stream *stream = file2stream(file);
+	struct video_device *video_dev = video_devdata(file);
+	struct virtio_video_device *vvd = video_drvdata(file);
+
+	mutex_lock(video_dev->lock);
+
+	v4l2_fh_del(&stream->fh);
+	if (vvd->is_m2m_dev) {
+		v4l2_m2m_ctx_release(stream->fh.m2m_ctx);
+	} else if (file->private_data == video_dev->queue->owner) {
+		vb2_queue_release(&vvd->vb2_output_queue);
+		video_dev->queue->owner = NULL;
+	}
+
+	v4l2_fh_exit(&stream->fh);
+
+	virtio_video_cmd_stream_destroy(vvd, stream->stream_id);
+	virtio_video_stream_id_put(vvd, stream->stream_id);
+
+	kfree(stream);
+
+	/* Mutex already locked here, passing NULL */
+	virtio_video_clear_device_busy(vvd, NULL);
+
+	mutex_unlock(video_dev->lock);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations virtio_video_device_m2m_fops = {
+	.owner		= THIS_MODULE,
+	.open		= virtio_video_device_open,
+	.release	= virtio_video_device_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static const struct v4l2_file_operations virtio_video_device_fops = {
+	.owner		= THIS_MODULE,
+	.open		= virtio_video_device_open,
+	.release	= virtio_video_device_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= vb2_fop_mmap,
+};
+
+static void virtio_video_device_run(void *priv)
+{
+
+}
+
+static void virtio_video_device_job_abort(void *priv)
+{
+	struct virtio_video_stream *stream = priv;
+	struct virtio_video_device *vvd = to_virtio_vd(stream->video_dev);
+
+	v4l2_m2m_job_finish(vvd->m2m_dev, stream->fh.m2m_ctx);
+}
+
+static const struct v4l2_m2m_ops virtio_video_device_m2m_ops = {
+	.device_run	= virtio_video_device_run,
+	.job_abort	= virtio_video_device_job_abort,
+};
+
+static int virtio_video_device_register(struct virtio_video_device *vvd)
+{
+	int ret;
+	struct video_device *vd;
+
+	vd = &vvd->video_dev;
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(5,7,0)
+	ret = video_register_device(vd, VFL_TYPE_VIDEO, vvd->vid_dev_nr);
+#else
+	ret = video_register_device(vd, VFL_TYPE_GRABBER, vvd->vid_dev_nr);
+#endif
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to register video device\n");
+		return ret;
+	}
+
+	v4l2_info(&vvd->v4l2_dev, "Device '%s' registered as /dev/video%d\n",
+		  vd->name, vd->num);
+
+	return 0;
+}
+
+static void virtio_video_device_unregister(struct virtio_video_device *vvd)
+{
+	video_unregister_device(&vvd->video_dev);
+}
+
+static int
+virtio_video_query_capability(struct virtio_video_device *vvd,
+			      void *resp_buf,
+			      enum virtio_video_queue_type queue_type)
+{
+	int ret;
+	int resp_size = vvd->max_caps_len;
+
+	ret = virtio_video_cmd_query_capability(vvd, resp_buf, resp_size,
+						queue_type);
+	if (ret)
+		v4l2_err(&vvd->v4l2_dev, "failed to query capability\n");
+
+	return ret;
+}
+
+int virtio_video_device_init(struct virtio_video_device *vvd)
+{
+	int ret;
+	int vfl_dir;
+	u32 dev_caps;
+	struct video_device *vd;
+	struct v4l2_m2m_dev *m2m_dev;
+	const struct v4l2_file_operations *fops;
+	void *input_resp_buf, *output_resp_buf;
+
+	output_resp_buf = kzalloc(vvd->max_caps_len, GFP_KERNEL);
+	if (!output_resp_buf)
+		return -ENOMEM;
+
+	ret = virtio_video_query_capability(vvd, output_resp_buf,
+					    VIRTIO_VIDEO_QUEUE_TYPE_OUTPUT);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to get output caps\n");
+		goto err_output_cap;
+	}
+
+	if (vvd->is_m2m_dev) {
+		input_resp_buf = kzalloc(vvd->max_caps_len, GFP_KERNEL);
+		if (!input_resp_buf) {
+			ret = -ENOMEM;
+			goto err_input_buf;
+		}
+
+		ret = virtio_video_query_capability(vvd, input_resp_buf,
+						VIRTIO_VIDEO_QUEUE_TYPE_INPUT);
+		if (ret) {
+			v4l2_err(&vvd->v4l2_dev, "failed to get input caps\n");
+			goto err_input_cap;
+		}
+
+		m2m_dev = v4l2_m2m_init(&virtio_video_device_m2m_ops);
+		if (IS_ERR(m2m_dev)) {
+			v4l2_err(&vvd->v4l2_dev, "failed to init m2m device\n");
+			ret = PTR_ERR(m2m_dev);
+			goto err_m2m_dev;
+		}
+		vfl_dir = VFL_DIR_M2M;
+		fops = &virtio_video_device_m2m_fops;
+		dev_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE;
+	} else {
+		input_resp_buf = NULL;
+		m2m_dev = NULL;
+		vfl_dir = VFL_DIR_RX;
+		fops = &virtio_video_device_fops;
+		dev_caps = V4L2_CAP_STREAMING;
+		if (vvd->is_mplane_cam)
+			dev_caps |= V4L2_CAP_VIDEO_CAPTURE_MPLANE;
+		else
+			dev_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	}
+
+	vvd->m2m_dev = m2m_dev;
+	mutex_init(&vvd->video_dev_mutex);
+	vd = &vvd->video_dev;
+	vd->lock = &vvd->video_dev_mutex;
+	vd->v4l2_dev = &vvd->v4l2_dev;
+	vd->vfl_dir = vfl_dir;
+	vd->ioctl_ops = NULL;
+	vd->fops = fops;
+	vd->device_caps = dev_caps;
+	vd->release = video_device_release_empty;
+
+	/* Use the selection API instead */
+	v4l2_disable_ioctl(vd, VIDIOC_CROPCAP);
+	v4l2_disable_ioctl(vd, VIDIOC_G_CROP);
+
+	video_set_drvdata(vd, vvd);
+
+	INIT_LIST_HEAD(&vvd->input_fmt_list);
+	INIT_LIST_HEAD(&vvd->output_fmt_list);
+	INIT_LIST_HEAD(&vvd->controls_fmt_list);
+	INIT_LIST_HEAD(&vvd->pending_buf_list);
+
+	vvd->num_output_fmts = 0;
+	vvd->num_input_fmts = 0;
+
+	switch (vvd->type) {
+	case VIRTIO_VIDEO_DEVICE_CAMERA:
+		virtio_video_cam_init(vvd);
+		break;
+	case VIRTIO_VIDEO_DEVICE_ENCODER:
+		virtio_video_enc_init(vvd);
+		break;
+	case VIRTIO_VIDEO_DEVICE_DECODER:
+	default:
+		virtio_video_dec_init(vvd);
+		break;
+	}
+
+	ret = virtio_video_parse_virtio_capabilities(vvd, input_resp_buf,
+						     output_resp_buf);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to parse a function\n");
+		goto parse_cap_err;
+	}
+
+	ret = virtio_video_parse_virtio_control(vvd);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev, "failed to query controls\n");
+		goto parse_ctrl_err;
+	}
+
+	ret = virtio_video_device_register(vvd);
+	if (ret) {
+		v4l2_err(&vvd->v4l2_dev,
+			 "failed to init virtio video device\n");
+		goto register_err;
+	}
+
+	goto out_cleanup;
+
+register_err:
+	virtio_video_clean_control(vvd);
+parse_ctrl_err:
+	virtio_video_clean_capability(vvd);
+parse_cap_err:
+	if (vvd->is_m2m_dev)
+		v4l2_m2m_release(vvd->m2m_dev);
+err_m2m_dev:
+err_input_cap:
+out_cleanup:
+	if (vvd->is_m2m_dev)
+		kfree(input_resp_buf);
+err_input_buf:
+err_output_cap:
+	kfree(output_resp_buf);
+
+	return ret;
+}
+
+void virtio_video_device_deinit(struct virtio_video_device *vvd)
+{
+	vvd->commandq.ready = false;
+	vvd->eventq.ready = false;
+
+	virtio_video_device_unregister(vvd);
+	if (vvd->is_m2m_dev)
+		v4l2_m2m_release(vvd->m2m_dev);
+	virtio_video_clean_control(vvd);
+	virtio_video_clean_capability(vvd);
+}