diff options
Diffstat (limited to 'meta-egvirt/recipes-kernel/kernel-module-virtio-video/files/virtio_video_device.c')
| -rw-r--r-- | meta-egvirt/recipes-kernel/kernel-module-virtio-video/files/virtio_video_device.c | 1282 |
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); +} |