1// SPDX-License-Identifier: GPL-2.0-or-later 2/* 3 * vimc-capture.c Virtual Media Controller Driver 4 * 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 the 24 * queue because the frames are generated from a hardware interruption 25 * and the isr is not allowed to sleep. 26 * Even if it is not necessary a spinlock in the vimc driver, we 27 * use it here as a code reference 28 */ 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 element 47 * the videobuf2 framework will allocate this struct based on 48 * buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of 49 * memory as a vb2_buffer 50 */ 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}; 225 226static void vimc_capture_return_all_buffers(struct vimc_capture_device *vcapture, 227 enum vb2_buffer_state state) 228{ 229 struct vimc_capture_buffer *vbuf, *node; 230 231 spin_lock(&vcapture->qlock); 232 233 list_for_each_entry_safe(vbuf, node, &vcapture->buf_list, list) { 234 list_del(&vbuf->list); 235 vb2_buffer_done(&vbuf->vb2.vb2_buf, state); 236 } 237 238 spin_unlock(&vcapture->qlock); 239} 240 241static int vimc_capture_start_streaming(struct vb2_queue *vq, unsigned int count) 242{ 243 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq); 244 int ret; 245 246 vcapture->sequence = 0; 247 248 /* Start the media pipeline */ 249 ret = video_device_pipeline_start(&vcapture->vdev, &vcapture->stream.pipe); 250 if (ret) { 251 vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED); 252 return ret; 253 } 254 255 ret = vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 1); 256 if (ret) { 257 video_device_pipeline_stop(&vcapture->vdev); 258 vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED); 259 return ret; 260 } 261 262 return 0; 263} 264 265/* 266 * Stop the stream engine. Any remaining buffers in the stream queue are 267 * dequeued and passed on to the vb2 framework marked as STATE_ERROR. 268 */ 269static void vimc_capture_stop_streaming(struct vb2_queue *vq) 270{ 271 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq); 272 273 vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 0); 274 275 /* Stop the media pipeline */ 276 video_device_pipeline_stop(&vcapture->vdev); 277 278 /* Release all active buffers */ 279 vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_ERROR); 280} 281 282static void vimc_capture_buf_queue(struct vb2_buffer *vb2_buf) 283{ 284 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb2_buf->vb2_queue); 285 struct vimc_capture_buffer *buf = container_of(vb2_buf, 286 struct vimc_capture_buffer, 287 vb2.vb2_buf); 288 289 spin_lock(&vcapture->qlock); 290 list_add_tail(&buf->list, &vcapture->buf_list); 291 spin_unlock(&vcapture->qlock); 292} 293 294static int vimc_capture_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers, 295 unsigned int *nplanes, unsigned int sizes[], 296 struct device *alloc_devs[]) 297{ 298 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq); 299 300 if (*nplanes) 301 return sizes[0] < vcapture->format.sizeimage ? -EINVAL : 0; 302 /* We don't support multiplanes for now */ 303 *nplanes = 1; 304 sizes[0] = vcapture->format.sizeimage; 305 306 return 0; 307} 308 309static int vimc_capture_buffer_prepare(struct vb2_buffer *vb) 310{ 311 struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb->vb2_queue); 312 unsigned long size = vcapture->format.sizeimage; 313 314 if (vb2_plane_size(vb, 0) < size) { 315 dev_err(vcapture->ved.dev, "%s: buffer too small (%lu < %lu)\n", 316 vcapture->vdev.name, vb2_plane_size(vb, 0), size); 317 return -EINVAL; 318 } 319 return 0; 320} 321 322static const struct vb2_ops vimc_capture_qops = { 323 .start_streaming = vimc_capture_start_streaming, 324 .stop_streaming = vimc_capture_stop_streaming, 325 .buf_queue = vimc_capture_buf_queue, 326 .queue_setup = vimc_capture_queue_setup, 327 .buf_prepare = vimc_capture_buffer_prepare, 328 /* 329 * Since q->lock is set we can use the standard 330 * vb2_ops_wait_prepare/finish helper functions. 331 */ 332 .wait_prepare = vb2_ops_wait_prepare, 333 .wait_finish = vb2_ops_wait_finish, 334}; 335 336static const struct media_entity_operations vimc_capture_mops = { 337 .link_validate = vimc_vdev_link_validate, 338}; 339 340static void vimc_capture_release(struct vimc_ent_device *ved) 341{ 342 struct vimc_capture_device *vcapture = 343 container_of(ved, struct vimc_capture_device, ved); 344 345 media_entity_cleanup(vcapture->ved.ent); 346 kfree(vcapture); 347} 348 349static void vimc_capture_unregister(struct vimc_ent_device *ved) 350{ 351 struct vimc_capture_device *vcapture = 352 container_of(ved, struct vimc_capture_device, ved); 353 354 vb2_video_unregister_device(&vcapture->vdev); 355} 356 357static void *vimc_capture_process_frame(struct vimc_ent_device *ved, 358 const void *frame) 359{ 360 struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device, 361 ved); 362 struct vimc_capture_buffer *vimc_buf; 363 void *vbuf; 364 365 spin_lock(&vcapture->qlock); 366 367 /* Get the first entry of the list */ 368 vimc_buf = list_first_entry_or_null(&vcapture->buf_list, 369 typeof(*vimc_buf), list); 370 if (!vimc_buf) { 371 spin_unlock(&vcapture->qlock); 372 return ERR_PTR(-EAGAIN); 373 } 374 375 /* Remove this entry from the list */ 376 list_del(&vimc_buf->list); 377 378 spin_unlock(&vcapture->qlock); 379 380 /* Fill the buffer */ 381 vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns(); 382 vimc_buf->vb2.sequence = vcapture->sequence++; 383 vimc_buf->vb2.field = vcapture->format.field; 384 385 vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0); 386 387 memcpy(vbuf, frame, vcapture->format.sizeimage); 388 389 /* Set it as ready */ 390 vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0, 391 vcapture->format.sizeimage); 392 vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE); 393 return NULL; 394} 395 396static struct vimc_ent_device *vimc_capture_add(struct vimc_device *vimc, 397 const char *vcfg_name) 398{ 399 struct v4l2_device *v4l2_dev = &vimc->v4l2_dev; 400 const struct vimc_pix_map *vpix; 401 struct vimc_capture_device *vcapture; 402 struct video_device *vdev; 403 struct vb2_queue *q; 404 int ret; 405 406 /* Allocate the vimc_capture_device struct */ 407 vcapture = kzalloc(sizeof(*vcapture), GFP_KERNEL); 408 if (!vcapture) 409 return ERR_PTR(-ENOMEM); 410 411 /* Initialize the media entity */ 412 vcapture->vdev.entity.name = vcfg_name; 413 vcapture->vdev.entity.function = MEDIA_ENT_F_IO_V4L; 414 vcapture->pad.flags = MEDIA_PAD_FL_SINK; 415 ret = media_entity_pads_init(&vcapture->vdev.entity, 416 1, &vcapture->pad); 417 if (ret) 418 goto err_free_vcapture; 419 420 /* Initialize the lock */ 421 mutex_init(&vcapture->lock); 422 423 /* Initialize the vb2 queue */ 424 q = &vcapture->queue; 425 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 426 q->io_modes = VB2_MMAP | VB2_DMABUF; 427 if (vimc_allocator == VIMC_ALLOCATOR_VMALLOC) 428 q->io_modes |= VB2_USERPTR; 429 q->drv_priv = vcapture; 430 q->buf_struct_size = sizeof(struct vimc_capture_buffer); 431 q->ops = &vimc_capture_qops; 432 q->mem_ops = vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG 433 ? &vb2_dma_contig_memops : &vb2_vmalloc_memops; 434 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 435 q->min_buffers_needed = 2; 436 q->lock = &vcapture->lock; 437 q->dev = v4l2_dev->dev; 438 439 ret = vb2_queue_init(q); 440 if (ret) { 441 dev_err(vimc->mdev.dev, "%s: vb2 queue init failed (err=%d)\n", 442 vcfg_name, ret); 443 goto err_clean_m_ent; 444 } 445 446 /* Initialize buffer list and its lock */ 447 INIT_LIST_HEAD(&vcapture->buf_list); 448 spin_lock_init(&vcapture->qlock); 449 450 /* Set default frame format */ 451 vcapture->format = fmt_default; 452 vpix = vimc_pix_map_by_pixelformat(vcapture->format.pixelformat); 453 vcapture->format.bytesperline = vcapture->format.width * vpix->bpp; 454 vcapture->format.sizeimage = vcapture->format.bytesperline * 455 vcapture->format.height; 456 457 /* Fill the vimc_ent_device struct */ 458 vcapture->ved.ent = &vcapture->vdev.entity; 459 vcapture->ved.process_frame = vimc_capture_process_frame; 460 vcapture->ved.vdev_get_format = vimc_capture_get_format; 461 vcapture->ved.dev = vimc->mdev.dev; 462 463 /* Initialize the video_device struct */ 464 vdev = &vcapture->vdev; 465 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING 466 | V4L2_CAP_IO_MC; 467 vdev->entity.ops = &vimc_capture_mops; 468 vdev->release = video_device_release_empty; 469 vdev->fops = &vimc_capture_fops; 470 vdev->ioctl_ops = &vimc_capture_ioctl_ops; 471 vdev->lock = &vcapture->lock; 472 vdev->queue = q; 473 vdev->v4l2_dev = v4l2_dev; 474 vdev->vfl_dir = VFL_DIR_RX; 475 strscpy(vdev->name, vcfg_name, sizeof(vdev->name)); 476 video_set_drvdata(vdev, &vcapture->ved); 477 478 /* Register the video_device with the v4l2 and the media framework */ 479 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 480 if (ret) { 481 dev_err(vimc->mdev.dev, "%s: video register failed (err=%d)\n", 482 vcapture->vdev.name, ret); 483 goto err_clean_m_ent; 484 } 485 486 return &vcapture->ved; 487 488err_clean_m_ent: 489 media_entity_cleanup(&vcapture->vdev.entity); 490err_free_vcapture: 491 kfree(vcapture); 492 493 return ERR_PTR(ret); 494} 495 496struct vimc_ent_type vimc_capture_type = { 497 .add = vimc_capture_add, 498 .unregister = vimc_capture_unregister, 499 .release = vimc_capture_release 500}; 501