drivers/media/platform/vimc/vimc-capture.c

   1 /*
   2  * vimc-capture.c Virtual Media Controller Driver
   3  *
   4  * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  */
  17
  18 #include <media/v4l2-ioctl.h>
  19 #include <media/videobuf2-core.h>
  20 #include <media/videobuf2-vmalloc.h>
  21
  22 #include "vimc-capture.h"
  23
  24 struct vimc_cap_device {
  25         struct vimc_ent_device ved;
  26         struct video_device vdev;
  27         struct v4l2_pix_format format;
  28         struct vb2_queue queue;
  29         struct list_head buf_list;
  30         /*
  31          * NOTE: in a real driver, a spin lock must be used to access the
  32          * queue because the frames are generated from a hardware interruption
  33          * and the isr is not allowed to sleep.
  34          * Even if it is not necessary a spinlock in the vimc driver, we
  35          * use it here as a code reference
  36          */
  37         spinlock_t qlock;
  38         struct mutex lock;
  39         u32 sequence;
  40         struct media_pipeline pipe;
  41 };
  42
  43 struct vimc_cap_buffer {
  44         /*
  45          * struct vb2_v4l2_buffer must be the first element
  46          * the videobuf2 framework will allocate this struct based on
  47          * buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of
  48          * memory as a vb2_buffer
  49          */
  50         struct vb2_v4l2_buffer vb2;
  51         struct list_head list;
  52 };
  53
  54 static int vimc_cap_querycap(struct file *file, void *priv,
  55                              struct v4l2_capability *cap)
  56 {
  57         struct vimc_cap_device *vcap = video_drvdata(file);
  58
  59         strlcpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
  60         strlcpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
  61         snprintf(cap->bus_info, sizeof(cap->bus_info),
  62                  "platform:%s", vcap->vdev.v4l2_dev->name);
  63
  64         return 0;
  65 }
  66
  67 static int vimc_cap_fmt_vid_cap(struct file *file, void *priv,
  68                                   struct v4l2_format *f)
  69 {
  70         struct vimc_cap_device *vcap = video_drvdata(file);
  71
  72         f->fmt.pix = vcap->format;
  73
  74         return 0;
  75 }
  76
  77 static int vimc_cap_enum_fmt_vid_cap(struct file *file, void *priv,
  78                                      struct v4l2_fmtdesc *f)
  79 {
  80         struct vimc_cap_device *vcap = video_drvdata(file);
  81
  82         if (f->index > 0)
  83                 return -EINVAL;
  84
  85         /* We only support one format for now */
  86         f->pixelformat = vcap->format.pixelformat;
  87
  88         return 0;
  89 }
  90
  91 static const struct v4l2_file_operations vimc_cap_fops = {
  92         .owner          = THIS_MODULE,
  93         .open           = v4l2_fh_open,
  94         .release        = vb2_fop_release,
  95         .read           = vb2_fop_read,
  96         .poll           = vb2_fop_poll,
  97         .unlocked_ioctl = video_ioctl2,
  98         .mmap           = vb2_fop_mmap,
  99 };
 100
 101 static const struct v4l2_ioctl_ops vimc_cap_ioctl_ops = {
 102         .vidioc_querycap = vimc_cap_querycap,
 103
 104         .vidioc_g_fmt_vid_cap = vimc_cap_fmt_vid_cap,
 105         .vidioc_s_fmt_vid_cap = vimc_cap_fmt_vid_cap,
 106         .vidioc_try_fmt_vid_cap = vimc_cap_fmt_vid_cap,
 107         .vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap,
 108
 109         .vidioc_reqbufs = vb2_ioctl_reqbufs,
 110         .vidioc_create_bufs = vb2_ioctl_create_bufs,
 111         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
 112         .vidioc_querybuf = vb2_ioctl_querybuf,
 113         .vidioc_qbuf = vb2_ioctl_qbuf,
 114         .vidioc_dqbuf = vb2_ioctl_dqbuf,
 115         .vidioc_expbuf = vb2_ioctl_expbuf,
 116         .vidioc_streamon = vb2_ioctl_streamon,
 117         .vidioc_streamoff = vb2_ioctl_streamoff,
 118 };
 119
 120 static void vimc_cap_return_all_buffers(struct vimc_cap_device *vcap,
 121                                         enum vb2_buffer_state state)
 122 {
 123         struct vimc_cap_buffer *vbuf, *node;
 124
 125         spin_lock(&vcap->qlock);
 126
 127         list_for_each_entry_safe(vbuf, node, &vcap->buf_list, list) {
 128                 list_del(&vbuf->list);
 129                 vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
 130         }
 131
 132         spin_unlock(&vcap->qlock);
 133 }
 134
 135 static int vimc_cap_pipeline_s_stream(struct vimc_cap_device *vcap, int enable)
 136 {
 137         struct v4l2_subdev *sd;
 138         struct media_pad *pad;
 139         int ret;
 140
 141         /* Start the stream in the subdevice direct connected */
 142         pad = media_entity_remote_pad(&vcap->vdev.entity.pads[0]);
 143
 144         /*
 145          * if it is a raw node from vimc-core, there is nothing to activate
 146          * TODO: remove this when there are no more raw nodes in the
 147          * core and return error instead
 148          */
 149         if (pad->entity->obj_type == MEDIA_ENTITY_TYPE_BASE)
 150                 return 0;
 151
 152         sd = media_entity_to_v4l2_subdev(pad->entity);
 153         ret = v4l2_subdev_call(sd, video, s_stream, enable);
 154         if (ret && ret != -ENOIOCTLCMD)
 155                 return ret;
 156
 157         return 0;
 158 }
 159
 160 static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
 161 {
 162         struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
 163         struct media_entity *entity = &vcap->vdev.entity;
 164         int ret;
 165
 166         vcap->sequence = 0;
 167
 168         /* Start the media pipeline */
 169         ret = media_pipeline_start(entity, &vcap->pipe);
 170         if (ret) {
 171                 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
 172                 return ret;
 173         }
 174
 175         /* Enable streaming from the pipe */
 176         ret = vimc_cap_pipeline_s_stream(vcap, 1);
 177         if (ret) {
 178                 media_pipeline_stop(entity);
 179                 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
 180                 return ret;
 181         }
 182
 183         return 0;
 184 }
 185
 186 /*
 187  * Stop the stream engine. Any remaining buffers in the stream queue are
 188  * dequeued and passed on to the vb2 framework marked as STATE_ERROR.
 189  */
 190 static void vimc_cap_stop_streaming(struct vb2_queue *vq)
 191 {
 192         struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
 193
 194         /* Disable streaming from the pipe */
 195         vimc_cap_pipeline_s_stream(vcap, 0);
 196
 197         /* Stop the media pipeline */
 198         media_pipeline_stop(&vcap->vdev.entity);
 199
 200         /* Release all active buffers */
 201         vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_ERROR);
 202 }
 203
 204 static void vimc_cap_buf_queue(struct vb2_buffer *vb2_buf)
 205 {
 206         struct vimc_cap_device *vcap = vb2_get_drv_priv(vb2_buf->vb2_queue);
 207         struct vimc_cap_buffer *buf = container_of(vb2_buf,
 208                                                    struct vimc_cap_buffer,
 209                                                    vb2.vb2_buf);
 210
 211         spin_lock(&vcap->qlock);
 212         list_add_tail(&buf->list, &vcap->buf_list);
 213         spin_unlock(&vcap->qlock);
 214 }
 215
 216 static int vimc_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
 217                                 unsigned int *nplanes, unsigned int sizes[],
 218                                 struct device *alloc_devs[])
 219 {
 220         struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
 221
 222         if (*nplanes)
 223                 return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0;
 224         /* We don't support multiplanes for now */
 225         *nplanes = 1;
 226         sizes[0] = vcap->format.sizeimage;
 227
 228         return 0;
 229 }
 230
 231 static int vimc_cap_buffer_prepare(struct vb2_buffer *vb)
 232 {
 233         struct vimc_cap_device *vcap = vb2_get_drv_priv(vb->vb2_queue);
 234         unsigned long size = vcap->format.sizeimage;
 235
 236         if (vb2_plane_size(vb, 0) < size) {
 237                 dev_err(vcap->vdev.v4l2_dev->dev,
 238                         "%s: buffer too small (%lu < %lu)\n",
 239                         vcap->vdev.name, vb2_plane_size(vb, 0), size);
 240                 return -EINVAL;
 241         }
 242         return 0;
 243 }
 244
 245 static const struct vb2_ops vimc_cap_qops = {
 246         .start_streaming        = vimc_cap_start_streaming,
 247         .stop_streaming         = vimc_cap_stop_streaming,
 248         .buf_queue              = vimc_cap_buf_queue,
 249         .queue_setup            = vimc_cap_queue_setup,
 250         .buf_prepare            = vimc_cap_buffer_prepare,
 251         /*
 252          * Since q->lock is set we can use the standard
 253          * vb2_ops_wait_prepare/finish helper functions.
 254          */
 255         .wait_prepare           = vb2_ops_wait_prepare,
 256         .wait_finish            = vb2_ops_wait_finish,
 257 };
 258
 259 /*
 260  * NOTE: this function is a copy of v4l2_subdev_link_validate_get_format
 261  * maybe the v4l2 function should be public
 262  */
 263 static int vimc_cap_v4l2_subdev_link_validate_get_format(struct media_pad *pad,
 264                                                 struct v4l2_subdev_format *fmt)
 265 {
 266         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(pad->entity);
 267
 268         fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
 269         fmt->pad = pad->index;
 270
 271         return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
 272 }
 273
 274 static int vimc_cap_link_validate(struct media_link *link)
 275 {
 276         struct v4l2_subdev_format source_fmt;
 277         const struct vimc_pix_map *vpix;
 278         struct vimc_cap_device *vcap = container_of(link->sink->entity,
 279                                                     struct vimc_cap_device,
 280                                                     vdev.entity);
 281         struct v4l2_pix_format *sink_fmt = &vcap->format;
 282         int ret;
 283
 284         /*
 285          * if it is a raw node from vimc-core, ignore the link for now
 286          * TODO: remove this when there are no more raw nodes in the
 287          * core and return error instead
 288          */
 289         if (link->source->entity->obj_type == MEDIA_ENTITY_TYPE_BASE)
 290                 return 0;
 291
 292         /* Get the the format of the subdev */
 293         ret = vimc_cap_v4l2_subdev_link_validate_get_format(link->source,
 294                                                             &source_fmt);
 295         if (ret)
 296                 return ret;
 297
 298         dev_dbg(vcap->vdev.v4l2_dev->dev,
 299                 "%s: link validate formats src:%dx%d %d sink:%dx%d %d\n",
 300                 vcap->vdev.name,
 301                 source_fmt.format.width, source_fmt.format.height,
 302                 source_fmt.format.code,
 303                 sink_fmt->width, sink_fmt->height,
 304                 sink_fmt->pixelformat);
 305
 306         /* The width, height and code must match. */
 307         vpix = vimc_pix_map_by_pixelformat(sink_fmt->pixelformat);
 308         if (source_fmt.format.width != sink_fmt->width
 309             || source_fmt.format.height != sink_fmt->height
 310             || vpix->code != source_fmt.format.code)
 311                 return -EPIPE;
 312
 313         /*
 314          * The field order must match, or the sink field order must be NONE
 315          * to support interlaced hardware connected to bridges that support
 316          * progressive formats only.
 317          */
 318         if (source_fmt.format.field != sink_fmt->field &&
 319             sink_fmt->field != V4L2_FIELD_NONE)
 320                 return -EPIPE;
 321
 322         return 0;
 323 }
 324
 325 static const struct media_entity_operations vimc_cap_mops = {
 326         .link_validate          = vimc_cap_link_validate,
 327 };
 328
 329 static void vimc_cap_destroy(struct vimc_ent_device *ved)
 330 {
 331         struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
 332                                                     ved);
 333
 334         vb2_queue_release(&vcap->queue);
 335         media_entity_cleanup(ved->ent);
 336         video_unregister_device(&vcap->vdev);
 337         vimc_pads_cleanup(vcap->ved.pads);
 338         kfree(vcap);
 339 }
 340
 341 static void vimc_cap_process_frame(struct vimc_ent_device *ved,
 342                                    struct media_pad *sink, const void *frame)
 343 {
 344         struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
 345                                                     ved);
 346         struct vimc_cap_buffer *vimc_buf;
 347         void *vbuf;
 348
 349         spin_lock(&vcap->qlock);
 350
 351         /* Get the first entry of the list */
 352         vimc_buf = list_first_entry_or_null(&vcap->buf_list,
 353                                             typeof(*vimc_buf), list);
 354         if (!vimc_buf) {
 355                 spin_unlock(&vcap->qlock);
 356                 return;
 357         }
 358
 359         /* Remove this entry from the list */
 360         list_del(&vimc_buf->list);
 361
 362         spin_unlock(&vcap->qlock);
 363
 364         /* Fill the buffer */
 365         vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
 366         vimc_buf->vb2.sequence = vcap->sequence++;
 367         vimc_buf->vb2.field = vcap->format.field;
 368
 369         vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);
 370
 371         memcpy(vbuf, frame, vcap->format.sizeimage);
 372
 373         /* Set it as ready */
 374         vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
 375                               vcap->format.sizeimage);
 376         vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
 377 }
 378
 379 struct vimc_ent_device *vimc_cap_create(struct v4l2_device *v4l2_dev,
 380                                         const char *const name,
 381                                         u16 num_pads,
 382                                         const unsigned long *pads_flag)
 383 {
 384         const struct vimc_pix_map *vpix;
 385         struct vimc_cap_device *vcap;
 386         struct video_device *vdev;
 387         struct vb2_queue *q;
 388         int ret;
 389
 390         /*
 391          * Check entity configuration params
 392          * NOTE: we only support a single sink pad
 393          */
 394         if (!name || num_pads != 1 || !pads_flag ||
 395             !(pads_flag[0] & MEDIA_PAD_FL_SINK))
 396                 return ERR_PTR(-EINVAL);
 397
 398         /* Allocate the vimc_cap_device struct */
 399         vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
 400         if (!vcap)
 401                 return ERR_PTR(-ENOMEM);
 402
 403         /* Allocate the pads */
 404         vcap->ved.pads = vimc_pads_init(num_pads, pads_flag);
 405         if (IS_ERR(vcap->ved.pads)) {
 406                 ret = PTR_ERR(vcap->ved.pads);
 407                 goto err_free_vcap;
 408         }
 409
 410         /* Initialize the media entity */
 411         vcap->vdev.entity.name = name;
 412         vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
 413         ret = media_entity_pads_init(&vcap->vdev.entity,
 414                                      num_pads, vcap->ved.pads);
 415         if (ret)
 416                 goto err_clean_pads;
 417
 418         /* Initialize the lock */
 419         mutex_init(&vcap->lock);
 420
 421         /* Initialize the vb2 queue */
 422         q = &vcap->queue;
 423         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 424         q->io_modes = VB2_MMAP | VB2_DMABUF;
 425         q->drv_priv = vcap;
 426         q->buf_struct_size = sizeof(struct vimc_cap_buffer);
 427         q->ops = &vimc_cap_qops;
 428         q->mem_ops = &vb2_vmalloc_memops;
 429         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 430         q->min_buffers_needed = 2;
 431         q->lock = &vcap->lock;
 432
 433         ret = vb2_queue_init(q);
 434         if (ret) {
 435                 dev_err(vcap->vdev.v4l2_dev->dev,
 436                         "%s: vb2 queue init failed (err=%d)\n",
 437                         vcap->vdev.name, ret);
 438                 goto err_clean_m_ent;
 439         }
 440
 441         /* Initialize buffer list and its lock */
 442         INIT_LIST_HEAD(&vcap->buf_list);
 443         spin_lock_init(&vcap->qlock);
 444
 445         /* Set the frame format (this is hardcoded for now) */
 446         vcap->format.width = 640;
 447         vcap->format.height = 480;
 448         vcap->format.pixelformat = V4L2_PIX_FMT_RGB24;
 449         vcap->format.field = V4L2_FIELD_NONE;
 450         vcap->format.colorspace = V4L2_COLORSPACE_SRGB;
 451
 452         vpix = vimc_pix_map_by_pixelformat(vcap->format.pixelformat);
 453
 454         vcap->format.bytesperline = vcap->format.width * vpix->bpp;
 455         vcap->format.sizeimage = vcap->format.bytesperline *
 456                                  vcap->format.height;
 457
 458         /* Fill the vimc_ent_device struct */
 459         vcap->ved.destroy = vimc_cap_destroy;
 460         vcap->ved.ent = &vcap->vdev.entity;
 461         vcap->ved.process_frame = vimc_cap_process_frame;
 462
 463         /* Initialize the video_device struct */
 464         vdev = &vcap->vdev;
 465         vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 466         vdev->entity.ops = &vimc_cap_mops;
 467         vdev->release = video_device_release_empty;
 468         vdev->fops = &vimc_cap_fops;
 469         vdev->ioctl_ops = &vimc_cap_ioctl_ops;
 470         vdev->lock = &vcap->lock;
 471         vdev->queue = q;
 472         vdev->v4l2_dev = v4l2_dev;
 473         vdev->vfl_dir = VFL_DIR_RX;
 474         strlcpy(vdev->name, name, sizeof(vdev->name));
 475         video_set_drvdata(vdev, &vcap->ved);
 476
 477         /* Register the video_device with the v4l2 and the media framework */
 478         ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
 479         if (ret) {
 480                 dev_err(vcap->vdev.v4l2_dev->dev,
 481                         "%s: video register failed (err=%d)\n",
 482                         vcap->vdev.name, ret);
 483                 goto err_release_queue;
 484         }
 485
 486         return &vcap->ved;
 487
 488 err_release_queue:
 489         vb2_queue_release(q);
 490 err_clean_m_ent:
 491         media_entity_cleanup(&vcap->vdev.entity);
 492 err_clean_pads:
 493         vimc_pads_cleanup(vcap->ved.pads);
 494 err_free_vcap:
 495         kfree(vcap);
 496
 497         return ERR_PTR(ret);
 498 }