test-drivers/vimc/vimc-capture.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * vimc-capture.c Virtual Media Controller Driver
0004  *
0005  * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
0006  */
0007
0008 #include <media/v4l2-ioctl.h>
0009 #include <media/videobuf2-core.h>
0010 #include <media/videobuf2-dma-contig.h>
0011 #include <media/videobuf2-vmalloc.h>
0012
0013 #include "vimc-common.h"
0014 #include "vimc-streamer.h"
0015
0016 struct vimc_capture_device {
0017     struct vimc_ent_device ved;
0018     struct video_device vdev;
0019     struct v4l2_pix_format format;
0020     struct vb2_queue queue;
0021     struct list_head buf_list;
0022     /*
0023      * NOTE: in a real driver, a spin lock must be used to access the
0024      * queue because the frames are generated from a hardware interruption
0025      * and the isr is not allowed to sleep.
0026      * Even if it is not necessary a spinlock in the vimc driver, we
0027      * use it here as a code reference
0028      */
0029     spinlock_t qlock;
0030     struct mutex lock;
0031     u32 sequence;
0032     struct vimc_stream stream;
0033     struct media_pad pad;
0034 };
0035
0036 static const struct v4l2_pix_format fmt_default = {
0037     .width = 640,
0038     .height = 480,
0039     .pixelformat = V4L2_PIX_FMT_RGB24,
0040     .field = V4L2_FIELD_NONE,
0041     .colorspace = V4L2_COLORSPACE_SRGB,
0042 };
0043
0044 struct vimc_capture_buffer {
0045     /*
0046      * struct vb2_v4l2_buffer must be the first element
0047      * the videobuf2 framework will allocate this struct based on
0048      * buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of
0049      * memory as a vb2_buffer
0050      */
0051     struct vb2_v4l2_buffer vb2;
0052     struct list_head list;
0053 };
0054
0055 static int vimc_capture_querycap(struct file *file, void *priv,
0056                  struct v4l2_capability *cap)
0057 {
0058     strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver));
0059     strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
0060     snprintf(cap->bus_info, sizeof(cap->bus_info),
0061          "platform:%s", VIMC_PDEV_NAME);
0062
0063     return 0;
0064 }
0065
0066 static void vimc_capture_get_format(struct vimc_ent_device *ved,
0067                 struct v4l2_pix_format *fmt)
0068 {
0069     struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,
0070                             ved);
0071
0072     *fmt = vcapture->format;
0073 }
0074
0075 static int vimc_capture_g_fmt_vid_cap(struct file *file, void *priv,
0076                   struct v4l2_format *f)
0077 {
0078     struct vimc_capture_device *vcapture = video_drvdata(file);
0079
0080     f->fmt.pix = vcapture->format;
0081
0082     return 0;
0083 }
0084
0085 static int vimc_capture_try_fmt_vid_cap(struct file *file, void *priv,
0086                     struct v4l2_format *f)
0087 {
0088     struct v4l2_pix_format *format = &f->fmt.pix;
0089     const struct vimc_pix_map *vpix;
0090
0091     format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,
0092                 VIMC_FRAME_MAX_WIDTH) & ~1;
0093     format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,
0094                  VIMC_FRAME_MAX_HEIGHT) & ~1;
0095
0096     /* Don't accept a pixelformat that is not on the table */
0097     vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
0098     if (!vpix) {
0099         format->pixelformat = fmt_default.pixelformat;
0100         vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
0101     }
0102     /* TODO: Add support for custom bytesperline values */
0103     format->bytesperline = format->width * vpix->bpp;
0104     format->sizeimage = format->bytesperline * format->height;
0105
0106     if (format->field == V4L2_FIELD_ANY)
0107         format->field = fmt_default.field;
0108
0109     vimc_colorimetry_clamp(format);
0110
0111     if (format->colorspace == V4L2_COLORSPACE_DEFAULT)
0112         format->colorspace = fmt_default.colorspace;
0113
0114     return 0;
0115 }
0116
0117 static int vimc_capture_s_fmt_vid_cap(struct file *file, void *priv,
0118                   struct v4l2_format *f)
0119 {
0120     struct vimc_capture_device *vcapture = video_drvdata(file);
0121     int ret;
0122
0123     /* Do not change the format while stream is on */
0124     if (vb2_is_busy(&vcapture->queue))
0125         return -EBUSY;
0126
0127     ret = vimc_capture_try_fmt_vid_cap(file, priv, f);
0128     if (ret)
0129         return ret;
0130
0131     dev_dbg(vcapture->ved.dev, "%s: format update: "
0132         "old:%dx%d (0x%x, %d, %d, %d, %d) "
0133         "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcapture->vdev.name,
0134         /* old */
0135         vcapture->format.width, vcapture->format.height,
0136         vcapture->format.pixelformat, vcapture->format.colorspace,
0137         vcapture->format.quantization, vcapture->format.xfer_func,
0138         vcapture->format.ycbcr_enc,
0139         /* new */
0140         f->fmt.pix.width, f->fmt.pix.height,
0141         f->fmt.pix.pixelformat, f->fmt.pix.colorspace,
0142         f->fmt.pix.quantization, f->fmt.pix.xfer_func,
0143         f->fmt.pix.ycbcr_enc);
0144
0145     vcapture->format = f->fmt.pix;
0146
0147     return 0;
0148 }
0149
0150 static int vimc_capture_enum_fmt_vid_cap(struct file *file, void *priv,
0151                      struct v4l2_fmtdesc *f)
0152 {
0153     const struct vimc_pix_map *vpix;
0154
0155     if (f->mbus_code) {
0156         if (f->index > 0)
0157             return -EINVAL;
0158
0159         vpix = vimc_pix_map_by_code(f->mbus_code);
0160     } else {
0161         vpix = vimc_pix_map_by_index(f->index);
0162     }
0163
0164     if (!vpix)
0165         return -EINVAL;
0166
0167     f->pixelformat = vpix->pixelformat;
0168
0169     return 0;
0170 }
0171
0172 static int vimc_capture_enum_framesizes(struct file *file, void *fh,
0173                     struct v4l2_frmsizeenum *fsize)
0174 {
0175     const struct vimc_pix_map *vpix;
0176
0177     if (fsize->index)
0178         return -EINVAL;
0179
0180     /* Only accept code in the pix map table */
0181     vpix = vimc_pix_map_by_code(fsize->pixel_format);
0182     if (!vpix)
0183         return -EINVAL;
0184
0185     fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
0186     fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;
0187     fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;
0188     fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;
0189     fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;
0190     fsize->stepwise.step_width = 1;
0191     fsize->stepwise.step_height = 1;
0192
0193     return 0;
0194 }
0195
0196 static const struct v4l2_file_operations vimc_capture_fops = {
0197     .owner      = THIS_MODULE,
0198     .open       = v4l2_fh_open,
0199     .release    = vb2_fop_release,
0200     .read           = vb2_fop_read,
0201     .poll       = vb2_fop_poll,
0202     .unlocked_ioctl = video_ioctl2,
0203     .mmap           = vb2_fop_mmap,
0204 };
0205
0206 static const struct v4l2_ioctl_ops vimc_capture_ioctl_ops = {
0207     .vidioc_querycap = vimc_capture_querycap,
0208
0209     .vidioc_g_fmt_vid_cap = vimc_capture_g_fmt_vid_cap,
0210     .vidioc_s_fmt_vid_cap = vimc_capture_s_fmt_vid_cap,
0211     .vidioc_try_fmt_vid_cap = vimc_capture_try_fmt_vid_cap,
0212     .vidioc_enum_fmt_vid_cap = vimc_capture_enum_fmt_vid_cap,
0213     .vidioc_enum_framesizes = vimc_capture_enum_framesizes,
0214
0215     .vidioc_reqbufs = vb2_ioctl_reqbufs,
0216     .vidioc_create_bufs = vb2_ioctl_create_bufs,
0217     .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
0218     .vidioc_querybuf = vb2_ioctl_querybuf,
0219     .vidioc_qbuf = vb2_ioctl_qbuf,
0220     .vidioc_dqbuf = vb2_ioctl_dqbuf,
0221     .vidioc_expbuf = vb2_ioctl_expbuf,
0222     .vidioc_streamon = vb2_ioctl_streamon,
0223     .vidioc_streamoff = vb2_ioctl_streamoff,
0224 };
0225
0226 static void vimc_capture_return_all_buffers(struct vimc_capture_device *vcapture,
0227                     enum vb2_buffer_state state)
0228 {
0229     struct vimc_capture_buffer *vbuf, *node;
0230
0231     spin_lock(&vcapture->qlock);
0232
0233     list_for_each_entry_safe(vbuf, node, &vcapture->buf_list, list) {
0234         list_del(&vbuf->list);
0235         vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
0236     }
0237
0238     spin_unlock(&vcapture->qlock);
0239 }
0240
0241 static int vimc_capture_start_streaming(struct vb2_queue *vq, unsigned int count)
0242 {
0243     struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
0244     struct media_entity *entity = &vcapture->vdev.entity;
0245     int ret;
0246
0247     vcapture->sequence = 0;
0248
0249     /* Start the media pipeline */
0250     ret = media_pipeline_start(entity, &vcapture->stream.pipe);
0251     if (ret) {
0252         vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);
0253         return ret;
0254     }
0255
0256     ret = vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 1);
0257     if (ret) {
0258         media_pipeline_stop(entity);
0259         vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);
0260         return ret;
0261     }
0262
0263     return 0;
0264 }
0265
0266 /*
0267  * Stop the stream engine. Any remaining buffers in the stream queue are
0268  * dequeued and passed on to the vb2 framework marked as STATE_ERROR.
0269  */
0270 static void vimc_capture_stop_streaming(struct vb2_queue *vq)
0271 {
0272     struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
0273
0274     vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 0);
0275
0276     /* Stop the media pipeline */
0277     media_pipeline_stop(&vcapture->vdev.entity);
0278
0279     /* Release all active buffers */
0280     vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_ERROR);
0281 }
0282
0283 static void vimc_capture_buf_queue(struct vb2_buffer *vb2_buf)
0284 {
0285     struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb2_buf->vb2_queue);
0286     struct vimc_capture_buffer *buf = container_of(vb2_buf,
0287                            struct vimc_capture_buffer,
0288                            vb2.vb2_buf);
0289
0290     spin_lock(&vcapture->qlock);
0291     list_add_tail(&buf->list, &vcapture->buf_list);
0292     spin_unlock(&vcapture->qlock);
0293 }
0294
0295 static int vimc_capture_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
0296                 unsigned int *nplanes, unsigned int sizes[],
0297                 struct device *alloc_devs[])
0298 {
0299     struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
0300
0301     if (*nplanes)
0302         return sizes[0] < vcapture->format.sizeimage ? -EINVAL : 0;
0303     /* We don't support multiplanes for now */
0304     *nplanes = 1;
0305     sizes[0] = vcapture->format.sizeimage;
0306
0307     return 0;
0308 }
0309
0310 static int vimc_capture_buffer_prepare(struct vb2_buffer *vb)
0311 {
0312     struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb->vb2_queue);
0313     unsigned long size = vcapture->format.sizeimage;
0314
0315     if (vb2_plane_size(vb, 0) < size) {
0316         dev_err(vcapture->ved.dev, "%s: buffer too small (%lu < %lu)\n",
0317             vcapture->vdev.name, vb2_plane_size(vb, 0), size);
0318         return -EINVAL;
0319     }
0320     return 0;
0321 }
0322
0323 static const struct vb2_ops vimc_capture_qops = {
0324     .start_streaming    = vimc_capture_start_streaming,
0325     .stop_streaming     = vimc_capture_stop_streaming,
0326     .buf_queue      = vimc_capture_buf_queue,
0327     .queue_setup        = vimc_capture_queue_setup,
0328     .buf_prepare        = vimc_capture_buffer_prepare,
0329     /*
0330      * Since q->lock is set we can use the standard
0331      * vb2_ops_wait_prepare/finish helper functions.
0332      */
0333     .wait_prepare       = vb2_ops_wait_prepare,
0334     .wait_finish        = vb2_ops_wait_finish,
0335 };
0336
0337 static const struct media_entity_operations vimc_capture_mops = {
0338     .link_validate      = vimc_vdev_link_validate,
0339 };
0340
0341 static void vimc_capture_release(struct vimc_ent_device *ved)
0342 {
0343     struct vimc_capture_device *vcapture =
0344         container_of(ved, struct vimc_capture_device, ved);
0345
0346     media_entity_cleanup(vcapture->ved.ent);
0347     kfree(vcapture);
0348 }
0349
0350 static void vimc_capture_unregister(struct vimc_ent_device *ved)
0351 {
0352     struct vimc_capture_device *vcapture =
0353         container_of(ved, struct vimc_capture_device, ved);
0354
0355     vb2_video_unregister_device(&vcapture->vdev);
0356 }
0357
0358 static void *vimc_capture_process_frame(struct vimc_ent_device *ved,
0359                     const void *frame)
0360 {
0361     struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,
0362                             ved);
0363     struct vimc_capture_buffer *vimc_buf;
0364     void *vbuf;
0365
0366     spin_lock(&vcapture->qlock);
0367
0368     /* Get the first entry of the list */
0369     vimc_buf = list_first_entry_or_null(&vcapture->buf_list,
0370                         typeof(*vimc_buf), list);
0371     if (!vimc_buf) {
0372         spin_unlock(&vcapture->qlock);
0373         return ERR_PTR(-EAGAIN);
0374     }
0375
0376     /* Remove this entry from the list */
0377     list_del(&vimc_buf->list);
0378
0379     spin_unlock(&vcapture->qlock);
0380
0381     /* Fill the buffer */
0382     vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
0383     vimc_buf->vb2.sequence = vcapture->sequence++;
0384     vimc_buf->vb2.field = vcapture->format.field;
0385
0386     vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);
0387
0388     memcpy(vbuf, frame, vcapture->format.sizeimage);
0389
0390     /* Set it as ready */
0391     vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
0392                   vcapture->format.sizeimage);
0393     vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
0394     return NULL;
0395 }
0396
0397 static struct vimc_ent_device *vimc_capture_add(struct vimc_device *vimc,
0398                         const char *vcfg_name)
0399 {
0400     struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
0401     const struct vimc_pix_map *vpix;
0402     struct vimc_capture_device *vcapture;
0403     struct video_device *vdev;
0404     struct vb2_queue *q;
0405     int ret;
0406
0407     /* Allocate the vimc_capture_device struct */
0408     vcapture = kzalloc(sizeof(*vcapture), GFP_KERNEL);
0409     if (!vcapture)
0410         return ERR_PTR(-ENOMEM);
0411
0412     /* Initialize the media entity */
0413     vcapture->vdev.entity.name = vcfg_name;
0414     vcapture->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
0415     vcapture->pad.flags = MEDIA_PAD_FL_SINK;
0416     ret = media_entity_pads_init(&vcapture->vdev.entity,
0417                      1, &vcapture->pad);
0418     if (ret)
0419         goto err_free_vcapture;
0420
0421     /* Initialize the lock */
0422     mutex_init(&vcapture->lock);
0423
0424     /* Initialize the vb2 queue */
0425     q = &vcapture->queue;
0426     q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
0427     q->io_modes = VB2_MMAP | VB2_DMABUF;
0428     if (vimc_allocator == VIMC_ALLOCATOR_VMALLOC)
0429         q->io_modes |= VB2_USERPTR;
0430     q->drv_priv = vcapture;
0431     q->buf_struct_size = sizeof(struct vimc_capture_buffer);
0432     q->ops = &vimc_capture_qops;
0433     q->mem_ops = vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG
0434            ? &vb2_dma_contig_memops : &vb2_vmalloc_memops;
0435     q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
0436     q->min_buffers_needed = 2;
0437     q->lock = &vcapture->lock;
0438     q->dev = v4l2_dev->dev;
0439
0440     ret = vb2_queue_init(q);
0441     if (ret) {
0442         dev_err(vimc->mdev.dev, "%s: vb2 queue init failed (err=%d)\n",
0443             vcfg_name, ret);
0444         goto err_clean_m_ent;
0445     }
0446
0447     /* Initialize buffer list and its lock */
0448     INIT_LIST_HEAD(&vcapture->buf_list);
0449     spin_lock_init(&vcapture->qlock);
0450
0451     /* Set default frame format */
0452     vcapture->format = fmt_default;
0453     vpix = vimc_pix_map_by_pixelformat(vcapture->format.pixelformat);
0454     vcapture->format.bytesperline = vcapture->format.width * vpix->bpp;
0455     vcapture->format.sizeimage = vcapture->format.bytesperline *
0456                  vcapture->format.height;
0457
0458     /* Fill the vimc_ent_device struct */
0459     vcapture->ved.ent = &vcapture->vdev.entity;
0460     vcapture->ved.process_frame = vimc_capture_process_frame;
0461     vcapture->ved.vdev_get_format = vimc_capture_get_format;
0462     vcapture->ved.dev = vimc->mdev.dev;
0463
0464     /* Initialize the video_device struct */
0465     vdev = &vcapture->vdev;
0466     vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
0467               | V4L2_CAP_IO_MC;
0468     vdev->entity.ops = &vimc_capture_mops;
0469     vdev->release = video_device_release_empty;
0470     vdev->fops = &vimc_capture_fops;
0471     vdev->ioctl_ops = &vimc_capture_ioctl_ops;
0472     vdev->lock = &vcapture->lock;
0473     vdev->queue = q;
0474     vdev->v4l2_dev = v4l2_dev;
0475     vdev->vfl_dir = VFL_DIR_RX;
0476     strscpy(vdev->name, vcfg_name, sizeof(vdev->name));
0477     video_set_drvdata(vdev, &vcapture->ved);
0478
0479     /* Register the video_device with the v4l2 and the media framework */
0480     ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
0481     if (ret) {
0482         dev_err(vimc->mdev.dev, "%s: video register failed (err=%d)\n",
0483             vcapture->vdev.name, ret);
0484         goto err_clean_m_ent;
0485     }
0486
0487     return &vcapture->ved;
0488
0489 err_clean_m_ent:
0490     media_entity_cleanup(&vcapture->vdev.entity);
0491 err_free_vcapture:
0492     kfree(vcapture);
0493
0494     return ERR_PTR(ret);
0495 }
0496
0497 struct vimc_ent_type vimc_capture_type = {
0498     .add = vimc_capture_add,
0499     .unregister = vimc_capture_unregister,
0500     .release = vimc_capture_release
0501 };