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 16 struct vimc_cap_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 36 static 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 44 struct vimc_cap_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 55 static int vimc_cap_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 66 static void vimc_cap_get_format(struct vimc_ent_device *ved, 67 struct v4l2_pix_format *fmt) 68 { 69 struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device, 70 ved); 71 72 *fmt = vcap->format; 73 } 74 75 static int vimc_cap_g_fmt_vid_cap(struct file *file, void *priv, 76 struct v4l2_format *f) 77 { 78 struct vimc_cap_device *vcap = video_drvdata(file); 79 80 f->fmt.pix = vcap->format; 81 82 return 0; 83 } 84 85 static int vimc_cap_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 117 static int vimc_cap_s_fmt_vid_cap(struct file *file, void *priv, 118 struct v4l2_format *f) 119 { 120 struct vimc_cap_device *vcap = video_drvdata(file); 121 int ret; 122 123 /* Do not change the format while stream is on */ 124 if (vb2_is_busy(&vcap->queue)) 125 return -EBUSY; 126 127 ret = vimc_cap_try_fmt_vid_cap(file, priv, f); 128 if (ret) 129 return ret; 130 131 dev_dbg(vcap->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", vcap->vdev.name, 134 /* old */ 135 vcap->format.width, vcap->format.height, 136 vcap->format.pixelformat, vcap->format.colorspace, 137 vcap->format.quantization, vcap->format.xfer_func, 138 vcap->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 vcap->format = f->fmt.pix; 146 147 return 0; 148 } 149 150 static int vimc_cap_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 172 static int vimc_cap_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 196 static const struct v4l2_file_operations vimc_cap_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 206 static const struct v4l2_ioctl_ops vimc_cap_ioctl_ops = { 207 .vidioc_querycap = vimc_cap_querycap, 208 209 .vidioc_g_fmt_vid_cap = vimc_cap_g_fmt_vid_cap, 210 .vidioc_s_fmt_vid_cap = vimc_cap_s_fmt_vid_cap, 211 .vidioc_try_fmt_vid_cap = vimc_cap_try_fmt_vid_cap, 212 .vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap, 213 .vidioc_enum_framesizes = vimc_cap_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 226 static void vimc_cap_return_all_buffers(struct vimc_cap_device *vcap, 227 enum vb2_buffer_state state) 228 { 229 struct vimc_cap_buffer *vbuf, *node; 230 231 spin_lock(&vcap->qlock); 232 233 list_for_each_entry_safe(vbuf, node, &vcap->buf_list, list) { 234 list_del(&vbuf->list); 235 vb2_buffer_done(&vbuf->vb2.vb2_buf, state); 236 } 237 238 spin_unlock(&vcap->qlock); 239 } 240 241 static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count) 242 { 243 struct vimc_cap_device *vcap = vb2_get_drv_priv(vq); 244 struct media_entity *entity = &vcap->vdev.entity; 245 int ret; 246 247 vcap->sequence = 0; 248 249 /* Start the media pipeline */ 250 ret = media_pipeline_start(entity, &vcap->stream.pipe); 251 if (ret) { 252 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED); 253 return ret; 254 } 255 256 ret = vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 1); 257 if (ret) { 258 media_pipeline_stop(entity); 259 vimc_cap_return_all_buffers(vcap, 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 are 268 * dequeued and passed on to the vb2 framework marked as STATE_ERROR. 269 */ 270 static void vimc_cap_stop_streaming(struct vb2_queue *vq) 271 { 272 struct vimc_cap_device *vcap = vb2_get_drv_priv(vq); 273 274 vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 0); 275 276 /* Stop the media pipeline */ 277 media_pipeline_stop(&vcap->vdev.entity); 278 279 /* Release all active buffers */ 280 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_ERROR); 281 } 282 283 static void vimc_cap_buf_queue(struct vb2_buffer *vb2_buf) 284 { 285 struct vimc_cap_device *vcap = vb2_get_drv_priv(vb2_buf->vb2_queue); 286 struct vimc_cap_buffer *buf = container_of(vb2_buf, 287 struct vimc_cap_buffer, 288 vb2.vb2_buf); 289 290 spin_lock(&vcap->qlock); 291 list_add_tail(&buf->list, &vcap->buf_list); 292 spin_unlock(&vcap->qlock); 293 } 294 295 static int vimc_cap_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_cap_device *vcap = vb2_get_drv_priv(vq); 300 301 if (*nplanes) 302 return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0; 303 /* We don't support multiplanes for now */ 304 *nplanes = 1; 305 sizes[0] = vcap->format.sizeimage; 306 307 return 0; 308 } 309 310 static int vimc_cap_buffer_prepare(struct vb2_buffer *vb) 311 { 312 struct vimc_cap_device *vcap = vb2_get_drv_priv(vb->vb2_queue); 313 unsigned long size = vcap->format.sizeimage; 314 315 if (vb2_plane_size(vb, 0) < size) { 316 dev_err(vcap->ved.dev, "%s: buffer too small (%lu < %lu)\n", 317 vcap->vdev.name, vb2_plane_size(vb, 0), size); 318 return -EINVAL; 319 } 320 return 0; 321 } 322 323 static const struct vb2_ops vimc_cap_qops = { 324 .start_streaming = vimc_cap_start_streaming, 325 .stop_streaming = vimc_cap_stop_streaming, 326 .buf_queue = vimc_cap_buf_queue, 327 .queue_setup = vimc_cap_queue_setup, 328 .buf_prepare = vimc_cap_buffer_prepare, 329 /* 330 * Since q->lock is set we can use the standard 331 * vb2_ops_wait_prepare/finish helper functions. 332 */ 333 .wait_prepare = vb2_ops_wait_prepare, 334 .wait_finish = vb2_ops_wait_finish, 335 }; 336 337 static const struct media_entity_operations vimc_cap_mops = { 338 .link_validate = vimc_vdev_link_validate, 339 }; 340 341 static void vimc_cap_release(struct vimc_ent_device *ved) 342 { 343 struct vimc_cap_device *vcap = 344 container_of(ved, struct vimc_cap_device, ved); 345 346 media_entity_cleanup(vcap->ved.ent); 347 kfree(vcap); 348 } 349 350 static void vimc_cap_unregister(struct vimc_ent_device *ved) 351 { 352 struct vimc_cap_device *vcap = 353 container_of(ved, struct vimc_cap_device, ved); 354 355 vb2_video_unregister_device(&vcap->vdev); 356 } 357 358 static void *vimc_cap_process_frame(struct vimc_ent_device *ved, 359 const void *frame) 360 { 361 struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device, 362 ved); 363 struct vimc_cap_buffer *vimc_buf; 364 void *vbuf; 365 366 spin_lock(&vcap->qlock); 367 368 /* Get the first entry of the list */ 369 vimc_buf = list_first_entry_or_null(&vcap->buf_list, 370 typeof(*vimc_buf), list); 371 if (!vimc_buf) { 372 spin_unlock(&vcap->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(&vcap->qlock); 380 381 /* Fill the buffer */ 382 vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns(); 383 vimc_buf->vb2.sequence = vcap->sequence++; 384 vimc_buf->vb2.field = vcap->format.field; 385 386 vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0); 387 388 memcpy(vbuf, frame, vcap->format.sizeimage); 389 390 /* Set it as ready */ 391 vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0, 392 vcap->format.sizeimage); 393 vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE); 394 return NULL; 395 } 396 397 static struct vimc_ent_device *vimc_cap_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_cap_device *vcap; 403 struct video_device *vdev; 404 struct vb2_queue *q; 405 int ret; 406 407 /* Allocate the vimc_cap_device struct */ 408 vcap = kzalloc(sizeof(*vcap), GFP_KERNEL); 409 if (!vcap) 410 return ERR_PTR(-ENOMEM); 411 412 /* Initialize the media entity */ 413 vcap->vdev.entity.name = vcfg_name; 414 vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L; 415 vcap->pad.flags = MEDIA_PAD_FL_SINK; 416 ret = media_entity_pads_init(&vcap->vdev.entity, 417 1, &vcap->pad); 418 if (ret) 419 goto err_free_vcap; 420 421 /* Initialize the lock */ 422 mutex_init(&vcap->lock); 423 424 /* Initialize the vb2 queue */ 425 q = &vcap->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 = vcap; 431 q->buf_struct_size = sizeof(struct vimc_cap_buffer); 432 q->ops = &vimc_cap_qops; 433 q->mem_ops = vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG 434 ? &vb2_dma_contig_memops : &vb2_vmalloc_memops; 435 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 436 q->min_buffers_needed = 2; 437 q->lock = &vcap->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(&vcap->buf_list); 449 spin_lock_init(&vcap->qlock); 450 451 /* Set default frame format */ 452 vcap->format = fmt_default; 453 vpix = vimc_pix_map_by_pixelformat(vcap->format.pixelformat); 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.ent = &vcap->vdev.entity; 460 vcap->ved.process_frame = vimc_cap_process_frame; 461 vcap->ved.vdev_get_format = vimc_cap_get_format; 462 vcap->ved.dev = vimc->mdev.dev; 463 464 /* Initialize the video_device struct */ 465 vdev = &vcap->vdev; 466 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING 467 | V4L2_CAP_IO_MC; 468 vdev->entity.ops = &vimc_cap_mops; 469 vdev->release = video_device_release_empty; 470 vdev->fops = &vimc_cap_fops; 471 vdev->ioctl_ops = &vimc_cap_ioctl_ops; 472 vdev->lock = &vcap->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, &vcap->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 vcap->vdev.name, ret); 484 goto err_clean_m_ent; 485 } 486 487 return &vcap->ved; 488 489 err_clean_m_ent: 490 media_entity_cleanup(&vcap->vdev.entity); 491 err_free_vcap: 492 kfree(vcap); 493 494 return ERR_PTR(ret); 495 } 496 497 struct vimc_ent_type vimc_cap_type = { 498 .add = vimc_cap_add, 499 .unregister = vimc_cap_unregister, 500 .release = vimc_cap_release 501 }; 502