502 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * vimc-capture.c Virtual Media Controller Driver4 *5 * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>6 */7 8#include <media/v4l2-ioctl.h>9#include <media/videobuf2-core.h>10#include <media/videobuf2-dma-contig.h>11#include <media/videobuf2-vmalloc.h>12 13#include "vimc-common.h"14#include "vimc-streamer.h"15 16struct vimc_capture_device {17 struct vimc_ent_device ved;18 struct video_device vdev;19 struct v4l2_pix_format format;20 struct vb2_queue queue;21 struct list_head buf_list;22 /*23 * NOTE: in a real driver, a spin lock must be used to access the24 * queue because the frames are generated from a hardware interruption25 * and the isr is not allowed to sleep.26 * Even if it is not necessary a spinlock in the vimc driver, we27 * use it here as a code reference28 */29 spinlock_t qlock;30 struct mutex lock;31 u32 sequence;32 struct vimc_stream stream;33 struct media_pad pad;34};35 36static const struct v4l2_pix_format fmt_default = {37 .width = 640,38 .height = 480,39 .pixelformat = V4L2_PIX_FMT_RGB24,40 .field = V4L2_FIELD_NONE,41 .colorspace = V4L2_COLORSPACE_SRGB,42};43 44struct vimc_capture_buffer {45 /*46 * struct vb2_v4l2_buffer must be the first element47 * the videobuf2 framework will allocate this struct based on48 * buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of49 * memory as a vb2_buffer50 */51 struct vb2_v4l2_buffer vb2;52 struct list_head list;53};54 55static int vimc_capture_querycap(struct file *file, void *priv,56 struct v4l2_capability *cap)57{58 strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver));59 strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));60 snprintf(cap->bus_info, sizeof(cap->bus_info),61 "platform:%s", VIMC_PDEV_NAME);62 63 return 0;64}65 66static void vimc_capture_get_format(struct vimc_ent_device *ved,67 struct v4l2_pix_format *fmt)68{69 struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,70 ved);71 72 *fmt = vcapture->format;73}74 75static int vimc_capture_g_fmt_vid_cap(struct file *file, void *priv,76 struct v4l2_format *f)77{78 struct vimc_capture_device *vcapture = video_drvdata(file);79 80 f->fmt.pix = vcapture->format;81 82 return 0;83}84 85static int vimc_capture_try_fmt_vid_cap(struct file *file, void *priv,86 struct v4l2_format *f)87{88 struct v4l2_pix_format *format = &f->fmt.pix;89 const struct vimc_pix_map *vpix;90 91 format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,92 VIMC_FRAME_MAX_WIDTH) & ~1;93 format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,94 VIMC_FRAME_MAX_HEIGHT) & ~1;95 96 /* Don't accept a pixelformat that is not on the table */97 vpix = vimc_pix_map_by_pixelformat(format->pixelformat);98 if (!vpix) {99 format->pixelformat = fmt_default.pixelformat;100 vpix = vimc_pix_map_by_pixelformat(format->pixelformat);101 }102 /* TODO: Add support for custom bytesperline values */103 format->bytesperline = format->width * vpix->bpp;104 format->sizeimage = format->bytesperline * format->height;105 106 if (format->field == V4L2_FIELD_ANY)107 format->field = fmt_default.field;108 109 vimc_colorimetry_clamp(format);110 111 if (format->colorspace == V4L2_COLORSPACE_DEFAULT)112 format->colorspace = fmt_default.colorspace;113 114 return 0;115}116 117static int vimc_capture_s_fmt_vid_cap(struct file *file, void *priv,118 struct v4l2_format *f)119{120 struct vimc_capture_device *vcapture = video_drvdata(file);121 int ret;122 123 /* Do not change the format while stream is on */124 if (vb2_is_busy(&vcapture->queue))125 return -EBUSY;126 127 ret = vimc_capture_try_fmt_vid_cap(file, priv, f);128 if (ret)129 return ret;130 131 dev_dbg(vcapture->ved.dev, "%s: format update: "132 "old:%dx%d (0x%x, %d, %d, %d, %d) "133 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcapture->vdev.name,134 /* old */135 vcapture->format.width, vcapture->format.height,136 vcapture->format.pixelformat, vcapture->format.colorspace,137 vcapture->format.quantization, vcapture->format.xfer_func,138 vcapture->format.ycbcr_enc,139 /* new */140 f->fmt.pix.width, f->fmt.pix.height,141 f->fmt.pix.pixelformat, f->fmt.pix.colorspace,142 f->fmt.pix.quantization, f->fmt.pix.xfer_func,143 f->fmt.pix.ycbcr_enc);144 145 vcapture->format = f->fmt.pix;146 147 return 0;148}149 150static int vimc_capture_enum_fmt_vid_cap(struct file *file, void *priv,151 struct v4l2_fmtdesc *f)152{153 const struct vimc_pix_map *vpix;154 155 if (f->mbus_code) {156 if (f->index > 0)157 return -EINVAL;158 159 vpix = vimc_pix_map_by_code(f->mbus_code);160 } else {161 vpix = vimc_pix_map_by_index(f->index);162 }163 164 if (!vpix)165 return -EINVAL;166 167 f->pixelformat = vpix->pixelformat;168 169 return 0;170}171 172static int vimc_capture_enum_framesizes(struct file *file, void *fh,173 struct v4l2_frmsizeenum *fsize)174{175 const struct vimc_pix_map *vpix;176 177 if (fsize->index)178 return -EINVAL;179 180 /* Only accept code in the pix map table */181 vpix = vimc_pix_map_by_code(fsize->pixel_format);182 if (!vpix)183 return -EINVAL;184 185 fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;186 fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;187 fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;188 fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;189 fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;190 fsize->stepwise.step_width = 1;191 fsize->stepwise.step_height = 1;192 193 return 0;194}195 196static const struct v4l2_file_operations vimc_capture_fops = {197 .owner = THIS_MODULE,198 .open = v4l2_fh_open,199 .release = vb2_fop_release,200 .read = vb2_fop_read,201 .poll = vb2_fop_poll,202 .unlocked_ioctl = video_ioctl2,203 .mmap = vb2_fop_mmap,204};205 206static const struct v4l2_ioctl_ops vimc_capture_ioctl_ops = {207 .vidioc_querycap = vimc_capture_querycap,208 209 .vidioc_g_fmt_vid_cap = vimc_capture_g_fmt_vid_cap,210 .vidioc_s_fmt_vid_cap = vimc_capture_s_fmt_vid_cap,211 .vidioc_try_fmt_vid_cap = vimc_capture_try_fmt_vid_cap,212 .vidioc_enum_fmt_vid_cap = vimc_capture_enum_fmt_vid_cap,213 .vidioc_enum_framesizes = vimc_capture_enum_framesizes,214 215 .vidioc_reqbufs = vb2_ioctl_reqbufs,216 .vidioc_create_bufs = vb2_ioctl_create_bufs,217 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,218 .vidioc_querybuf = vb2_ioctl_querybuf,219 .vidioc_qbuf = vb2_ioctl_qbuf,220 .vidioc_dqbuf = vb2_ioctl_dqbuf,221 .vidioc_expbuf = vb2_ioctl_expbuf,222 .vidioc_streamon = vb2_ioctl_streamon,223 .vidioc_streamoff = vb2_ioctl_streamoff,224 .vidioc_remove_bufs = vb2_ioctl_remove_bufs,225};226 227static void vimc_capture_return_all_buffers(struct vimc_capture_device *vcapture,228 enum vb2_buffer_state state)229{230 struct vimc_capture_buffer *vbuf, *node;231 232 spin_lock(&vcapture->qlock);233 234 list_for_each_entry_safe(vbuf, node, &vcapture->buf_list, list) {235 list_del(&vbuf->list);236 vb2_buffer_done(&vbuf->vb2.vb2_buf, state);237 }238 239 spin_unlock(&vcapture->qlock);240}241 242static int vimc_capture_start_streaming(struct vb2_queue *vq, unsigned int count)243{244 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);245 int ret;246 247 vcapture->sequence = 0;248 249 /* Start the media pipeline */250 ret = video_device_pipeline_start(&vcapture->vdev, &vcapture->stream.pipe);251 if (ret) {252 vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);253 return ret;254 }255 256 ret = vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 1);257 if (ret) {258 video_device_pipeline_stop(&vcapture->vdev);259 vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);260 return ret;261 }262 263 return 0;264}265 266/*267 * Stop the stream engine. Any remaining buffers in the stream queue are268 * dequeued and passed on to the vb2 framework marked as STATE_ERROR.269 */270static void vimc_capture_stop_streaming(struct vb2_queue *vq)271{272 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);273 274 vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 0);275 276 /* Stop the media pipeline */277 video_device_pipeline_stop(&vcapture->vdev);278 279 /* Release all active buffers */280 vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_ERROR);281}282 283static void vimc_capture_buf_queue(struct vb2_buffer *vb2_buf)284{285 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb2_buf->vb2_queue);286 struct vimc_capture_buffer *buf = container_of(vb2_buf,287 struct vimc_capture_buffer,288 vb2.vb2_buf);289 290 spin_lock(&vcapture->qlock);291 list_add_tail(&buf->list, &vcapture->buf_list);292 spin_unlock(&vcapture->qlock);293}294 295static int vimc_capture_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,296 unsigned int *nplanes, unsigned int sizes[],297 struct device *alloc_devs[])298{299 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);300 301 if (*nplanes)302 return sizes[0] < vcapture->format.sizeimage ? -EINVAL : 0;303 /* We don't support multiplanes for now */304 *nplanes = 1;305 sizes[0] = vcapture->format.sizeimage;306 307 return 0;308}309 310static int vimc_capture_buffer_prepare(struct vb2_buffer *vb)311{312 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb->vb2_queue);313 unsigned long size = vcapture->format.sizeimage;314 315 if (vb2_plane_size(vb, 0) < size) {316 dev_err(vcapture->ved.dev, "%s: buffer too small (%lu < %lu)\n",317 vcapture->vdev.name, vb2_plane_size(vb, 0), size);318 return -EINVAL;319 }320 return 0;321}322 323static const struct vb2_ops vimc_capture_qops = {324 .start_streaming = vimc_capture_start_streaming,325 .stop_streaming = vimc_capture_stop_streaming,326 .buf_queue = vimc_capture_buf_queue,327 .queue_setup = vimc_capture_queue_setup,328 .buf_prepare = vimc_capture_buffer_prepare,329 /*330 * Since q->lock is set we can use the standard331 * vb2_ops_wait_prepare/finish helper functions.332 */333 .wait_prepare = vb2_ops_wait_prepare,334 .wait_finish = vb2_ops_wait_finish,335};336 337static const struct media_entity_operations vimc_capture_mops = {338 .link_validate = vimc_vdev_link_validate,339};340 341static void vimc_capture_release(struct vimc_ent_device *ved)342{343 struct vimc_capture_device *vcapture =344 container_of(ved, struct vimc_capture_device, ved);345 346 media_entity_cleanup(vcapture->ved.ent);347 kfree(vcapture);348}349 350static void vimc_capture_unregister(struct vimc_ent_device *ved)351{352 struct vimc_capture_device *vcapture =353 container_of(ved, struct vimc_capture_device, ved);354 355 vb2_video_unregister_device(&vcapture->vdev);356}357 358static void *vimc_capture_process_frame(struct vimc_ent_device *ved,359 const void *frame)360{361 struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,362 ved);363 struct vimc_capture_buffer *vimc_buf;364 void *vbuf;365 366 spin_lock(&vcapture->qlock);367 368 /* Get the first entry of the list */369 vimc_buf = list_first_entry_or_null(&vcapture->buf_list,370 typeof(*vimc_buf), list);371 if (!vimc_buf) {372 spin_unlock(&vcapture->qlock);373 return ERR_PTR(-EAGAIN);374 }375 376 /* Remove this entry from the list */377 list_del(&vimc_buf->list);378 379 spin_unlock(&vcapture->qlock);380 381 /* Fill the buffer */382 vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();383 vimc_buf->vb2.sequence = vcapture->sequence++;384 vimc_buf->vb2.field = vcapture->format.field;385 386 vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);387 388 memcpy(vbuf, frame, vcapture->format.sizeimage);389 390 /* Set it as ready */391 vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,392 vcapture->format.sizeimage);393 vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);394 return NULL;395}396 397static struct vimc_ent_device *vimc_capture_add(struct vimc_device *vimc,398 const char *vcfg_name)399{400 struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;401 const struct vimc_pix_map *vpix;402 struct vimc_capture_device *vcapture;403 struct video_device *vdev;404 struct vb2_queue *q;405 int ret;406 407 /* Allocate the vimc_capture_device struct */408 vcapture = kzalloc(sizeof(*vcapture), GFP_KERNEL);409 if (!vcapture)410 return ERR_PTR(-ENOMEM);411 412 /* Initialize the media entity */413 vcapture->vdev.entity.name = vcfg_name;414 vcapture->vdev.entity.function = MEDIA_ENT_F_IO_V4L;415 vcapture->pad.flags = MEDIA_PAD_FL_SINK;416 ret = media_entity_pads_init(&vcapture->vdev.entity,417 1, &vcapture->pad);418 if (ret)419 goto err_free_vcapture;420 421 /* Initialize the lock */422 mutex_init(&vcapture->lock);423 424 /* Initialize the vb2 queue */425 q = &vcapture->queue;426 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;427 q->io_modes = VB2_MMAP | VB2_DMABUF;428 if (vimc_allocator == VIMC_ALLOCATOR_VMALLOC)429 q->io_modes |= VB2_USERPTR;430 q->drv_priv = vcapture;431 q->buf_struct_size = sizeof(struct vimc_capture_buffer);432 q->ops = &vimc_capture_qops;433 q->mem_ops = vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG434 ? &vb2_dma_contig_memops : &vb2_vmalloc_memops;435 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;436 q->min_reqbufs_allocation = 2;437 q->lock = &vcapture->lock;438 q->dev = v4l2_dev->dev;439 440 ret = vb2_queue_init(q);441 if (ret) {442 dev_err(vimc->mdev.dev, "%s: vb2 queue init failed (err=%d)\n",443 vcfg_name, ret);444 goto err_clean_m_ent;445 }446 447 /* Initialize buffer list and its lock */448 INIT_LIST_HEAD(&vcapture->buf_list);449 spin_lock_init(&vcapture->qlock);450 451 /* Set default frame format */452 vcapture->format = fmt_default;453 vpix = vimc_pix_map_by_pixelformat(vcapture->format.pixelformat);454 vcapture->format.bytesperline = vcapture->format.width * vpix->bpp;455 vcapture->format.sizeimage = vcapture->format.bytesperline *456 vcapture->format.height;457 458 /* Fill the vimc_ent_device struct */459 vcapture->ved.ent = &vcapture->vdev.entity;460 vcapture->ved.process_frame = vimc_capture_process_frame;461 vcapture->ved.vdev_get_format = vimc_capture_get_format;462 vcapture->ved.dev = vimc->mdev.dev;463 464 /* Initialize the video_device struct */465 vdev = &vcapture->vdev;466 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING467 | V4L2_CAP_IO_MC;468 vdev->entity.ops = &vimc_capture_mops;469 vdev->release = video_device_release_empty;470 vdev->fops = &vimc_capture_fops;471 vdev->ioctl_ops = &vimc_capture_ioctl_ops;472 vdev->lock = &vcapture->lock;473 vdev->queue = q;474 vdev->v4l2_dev = v4l2_dev;475 vdev->vfl_dir = VFL_DIR_RX;476 strscpy(vdev->name, vcfg_name, sizeof(vdev->name));477 video_set_drvdata(vdev, &vcapture->ved);478 479 /* Register the video_device with the v4l2 and the media framework */480 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);481 if (ret) {482 dev_err(vimc->mdev.dev, "%s: video register failed (err=%d)\n",483 vcapture->vdev.name, ret);484 goto err_clean_m_ent;485 }486 487 return &vcapture->ved;488 489err_clean_m_ent:490 media_entity_cleanup(&vcapture->vdev.entity);491err_free_vcapture:492 kfree(vcapture);493 494 return ERR_PTR(ret);495}496 497const struct vimc_ent_type vimc_capture_type = {498 .add = vimc_capture_add,499 .unregister = vimc_capture_unregister,500 .release = vimc_capture_release501};502