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-vmalloc.h> 11 12 #include "vimc-common.h" 13 #include "vimc-streamer.h" 14 15 struct vimc_cap_device { 16 struct vimc_ent_device ved; 17 struct video_device vdev; 18 struct v4l2_pix_format format; 19 struct vb2_queue queue; 20 struct list_head buf_list; 21 /* 22 * NOTE: in a real driver, a spin lock must be used to access the 23 * queue because the frames are generated from a hardware interruption 24 * and the isr is not allowed to sleep. 25 * Even if it is not necessary a spinlock in the vimc driver, we 26 * use it here as a code reference 27 */ 28 spinlock_t qlock; 29 struct mutex lock; 30 u32 sequence; 31 struct vimc_stream stream; 32 struct media_pad pad; 33 }; 34 35 static const struct v4l2_pix_format fmt_default = { 36 .width = 640, 37 .height = 480, 38 .pixelformat = V4L2_PIX_FMT_RGB24, 39 .field = V4L2_FIELD_NONE, 40 .colorspace = V4L2_COLORSPACE_SRGB, 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 strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver)); 58 strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card)); 59 snprintf(cap->bus_info, sizeof(cap->bus_info), 60 "platform:%s", VIMC_PDEV_NAME); 61 62 return 0; 63 } 64 65 static void vimc_cap_get_format(struct vimc_ent_device *ved, 66 struct v4l2_pix_format *fmt) 67 { 68 struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device, 69 ved); 70 71 *fmt = vcap->format; 72 } 73 74 static int vimc_cap_g_fmt_vid_cap(struct file *file, void *priv, 75 struct v4l2_format *f) 76 { 77 struct vimc_cap_device *vcap = video_drvdata(file); 78 79 f->fmt.pix = vcap->format; 80 81 return 0; 82 } 83 84 static int vimc_cap_try_fmt_vid_cap(struct file *file, void *priv, 85 struct v4l2_format *f) 86 { 87 struct v4l2_pix_format *format = &f->fmt.pix; 88 const struct vimc_pix_map *vpix; 89 90 format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH, 91 VIMC_FRAME_MAX_WIDTH) & ~1; 92 format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT, 93 VIMC_FRAME_MAX_HEIGHT) & ~1; 94 95 /* Don't accept a pixelformat that is not on the table */ 96 vpix = vimc_pix_map_by_pixelformat(format->pixelformat); 97 if (!vpix) { 98 format->pixelformat = fmt_default.pixelformat; 99 vpix = vimc_pix_map_by_pixelformat(format->pixelformat); 100 } 101 /* TODO: Add support for custom bytesperline values */ 102 format->bytesperline = format->width * vpix->bpp; 103 format->sizeimage = format->bytesperline * format->height; 104 105 if (format->field == V4L2_FIELD_ANY) 106 format->field = fmt_default.field; 107 108 vimc_colorimetry_clamp(format); 109 110 if (format->colorspace == V4L2_COLORSPACE_DEFAULT) 111 format->colorspace = fmt_default.colorspace; 112 113 return 0; 114 } 115 116 static int vimc_cap_s_fmt_vid_cap(struct file *file, void *priv, 117 struct v4l2_format *f) 118 { 119 struct vimc_cap_device *vcap = video_drvdata(file); 120 int ret; 121 122 /* Do not change the format while stream is on */ 123 if (vb2_is_busy(&vcap->queue)) 124 return -EBUSY; 125 126 ret = vimc_cap_try_fmt_vid_cap(file, priv, f); 127 if (ret) 128 return ret; 129 130 dev_dbg(vcap->ved.dev, "%s: format update: " 131 "old:%dx%d (0x%x, %d, %d, %d, %d) " 132 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcap->vdev.name, 133 /* old */ 134 vcap->format.width, vcap->format.height, 135 vcap->format.pixelformat, vcap->format.colorspace, 136 vcap->format.quantization, vcap->format.xfer_func, 137 vcap->format.ycbcr_enc, 138 /* new */ 139 f->fmt.pix.width, f->fmt.pix.height, 140 f->fmt.pix.pixelformat, f->fmt.pix.colorspace, 141 f->fmt.pix.quantization, f->fmt.pix.xfer_func, 142 f->fmt.pix.ycbcr_enc); 143 144 vcap->format = f->fmt.pix; 145 146 return 0; 147 } 148 149 static int vimc_cap_enum_fmt_vid_cap(struct file *file, void *priv, 150 struct v4l2_fmtdesc *f) 151 { 152 const struct vimc_pix_map *vpix; 153 154 if (f->mbus_code) { 155 if (f->index > 0) 156 return -EINVAL; 157 158 vpix = vimc_pix_map_by_code(f->mbus_code); 159 } else { 160 vpix = vimc_pix_map_by_index(f->index); 161 } 162 163 if (!vpix) 164 return -EINVAL; 165 166 f->pixelformat = vpix->pixelformat; 167 168 return 0; 169 } 170 171 static int vimc_cap_enum_framesizes(struct file *file, void *fh, 172 struct v4l2_frmsizeenum *fsize) 173 { 174 const struct vimc_pix_map *vpix; 175 176 if (fsize->index) 177 return -EINVAL; 178 179 /* Only accept code in the pix map table */ 180 vpix = vimc_pix_map_by_code(fsize->pixel_format); 181 if (!vpix) 182 return -EINVAL; 183 184 fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS; 185 fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH; 186 fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH; 187 fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT; 188 fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT; 189 fsize->stepwise.step_width = 1; 190 fsize->stepwise.step_height = 1; 191 192 return 0; 193 } 194 195 static const struct v4l2_file_operations vimc_cap_fops = { 196 .owner = THIS_MODULE, 197 .open = v4l2_fh_open, 198 .release = vb2_fop_release, 199 .read = vb2_fop_read, 200 .poll = vb2_fop_poll, 201 .unlocked_ioctl = video_ioctl2, 202 .mmap = vb2_fop_mmap, 203 }; 204 205 static const struct v4l2_ioctl_ops vimc_cap_ioctl_ops = { 206 .vidioc_querycap = vimc_cap_querycap, 207 208 .vidioc_g_fmt_vid_cap = vimc_cap_g_fmt_vid_cap, 209 .vidioc_s_fmt_vid_cap = vimc_cap_s_fmt_vid_cap, 210 .vidioc_try_fmt_vid_cap = vimc_cap_try_fmt_vid_cap, 211 .vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap, 212 .vidioc_enum_framesizes = vimc_cap_enum_framesizes, 213 214 .vidioc_reqbufs = vb2_ioctl_reqbufs, 215 .vidioc_create_bufs = vb2_ioctl_create_bufs, 216 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 217 .vidioc_querybuf = vb2_ioctl_querybuf, 218 .vidioc_qbuf = vb2_ioctl_qbuf, 219 .vidioc_dqbuf = vb2_ioctl_dqbuf, 220 .vidioc_expbuf = vb2_ioctl_expbuf, 221 .vidioc_streamon = vb2_ioctl_streamon, 222 .vidioc_streamoff = vb2_ioctl_streamoff, 223 }; 224 225 static void vimc_cap_return_all_buffers(struct vimc_cap_device *vcap, 226 enum vb2_buffer_state state) 227 { 228 struct vimc_cap_buffer *vbuf, *node; 229 230 spin_lock(&vcap->qlock); 231 232 list_for_each_entry_safe(vbuf, node, &vcap->buf_list, list) { 233 list_del(&vbuf->list); 234 vb2_buffer_done(&vbuf->vb2.vb2_buf, state); 235 } 236 237 spin_unlock(&vcap->qlock); 238 } 239 240 static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count) 241 { 242 struct vimc_cap_device *vcap = vb2_get_drv_priv(vq); 243 struct media_entity *entity = &vcap->vdev.entity; 244 int ret; 245 246 vcap->sequence = 0; 247 248 /* Start the media pipeline */ 249 ret = media_pipeline_start(entity, &vcap->stream.pipe); 250 if (ret) { 251 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED); 252 return ret; 253 } 254 255 ret = vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 1); 256 if (ret) { 257 media_pipeline_stop(entity); 258 vimc_cap_return_all_buffers(vcap, 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 */ 269 static void vimc_cap_stop_streaming(struct vb2_queue *vq) 270 { 271 struct vimc_cap_device *vcap = vb2_get_drv_priv(vq); 272 273 vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 0); 274 275 /* Stop the media pipeline */ 276 media_pipeline_stop(&vcap->vdev.entity); 277 278 /* Release all active buffers */ 279 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_ERROR); 280 } 281 282 static void vimc_cap_buf_queue(struct vb2_buffer *vb2_buf) 283 { 284 struct vimc_cap_device *vcap = vb2_get_drv_priv(vb2_buf->vb2_queue); 285 struct vimc_cap_buffer *buf = container_of(vb2_buf, 286 struct vimc_cap_buffer, 287 vb2.vb2_buf); 288 289 spin_lock(&vcap->qlock); 290 list_add_tail(&buf->list, &vcap->buf_list); 291 spin_unlock(&vcap->qlock); 292 } 293 294 static int vimc_cap_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_cap_device *vcap = vb2_get_drv_priv(vq); 299 300 if (*nplanes) 301 return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0; 302 /* We don't support multiplanes for now */ 303 *nplanes = 1; 304 sizes[0] = vcap->format.sizeimage; 305 306 return 0; 307 } 308 309 static int vimc_cap_buffer_prepare(struct vb2_buffer *vb) 310 { 311 struct vimc_cap_device *vcap = vb2_get_drv_priv(vb->vb2_queue); 312 unsigned long size = vcap->format.sizeimage; 313 314 if (vb2_plane_size(vb, 0) < size) { 315 dev_err(vcap->ved.dev, "%s: buffer too small (%lu < %lu)\n", 316 vcap->vdev.name, vb2_plane_size(vb, 0), size); 317 return -EINVAL; 318 } 319 return 0; 320 } 321 322 static const struct vb2_ops vimc_cap_qops = { 323 .start_streaming = vimc_cap_start_streaming, 324 .stop_streaming = vimc_cap_stop_streaming, 325 .buf_queue = vimc_cap_buf_queue, 326 .queue_setup = vimc_cap_queue_setup, 327 .buf_prepare = vimc_cap_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 336 static const struct media_entity_operations vimc_cap_mops = { 337 .link_validate = vimc_vdev_link_validate, 338 }; 339 340 static void vimc_cap_release(struct vimc_ent_device *ved) 341 { 342 struct vimc_cap_device *vcap = 343 container_of(ved, struct vimc_cap_device, ved); 344 345 media_entity_cleanup(vcap->ved.ent); 346 kfree(vcap); 347 } 348 349 static void vimc_cap_unregister(struct vimc_ent_device *ved) 350 { 351 struct vimc_cap_device *vcap = 352 container_of(ved, struct vimc_cap_device, ved); 353 354 vb2_video_unregister_device(&vcap->vdev); 355 } 356 357 static void *vimc_cap_process_frame(struct vimc_ent_device *ved, 358 const void *frame) 359 { 360 struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device, 361 ved); 362 struct vimc_cap_buffer *vimc_buf; 363 void *vbuf; 364 365 spin_lock(&vcap->qlock); 366 367 /* Get the first entry of the list */ 368 vimc_buf = list_first_entry_or_null(&vcap->buf_list, 369 typeof(*vimc_buf), list); 370 if (!vimc_buf) { 371 spin_unlock(&vcap->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(&vcap->qlock); 379 380 /* Fill the buffer */ 381 vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns(); 382 vimc_buf->vb2.sequence = vcap->sequence++; 383 vimc_buf->vb2.field = vcap->format.field; 384 385 vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0); 386 387 memcpy(vbuf, frame, vcap->format.sizeimage); 388 389 /* Set it as ready */ 390 vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0, 391 vcap->format.sizeimage); 392 vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE); 393 return NULL; 394 } 395 396 static struct vimc_ent_device *vimc_cap_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_cap_device *vcap; 402 struct video_device *vdev; 403 struct vb2_queue *q; 404 int ret; 405 406 /* Allocate the vimc_cap_device struct */ 407 vcap = kzalloc(sizeof(*vcap), GFP_KERNEL); 408 if (!vcap) 409 return ERR_PTR(-ENOMEM); 410 411 /* Initialize the media entity */ 412 vcap->vdev.entity.name = vcfg_name; 413 vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L; 414 vcap->pad.flags = MEDIA_PAD_FL_SINK; 415 ret = media_entity_pads_init(&vcap->vdev.entity, 416 1, &vcap->pad); 417 if (ret) 418 goto err_free_vcap; 419 420 /* Initialize the lock */ 421 mutex_init(&vcap->lock); 422 423 /* Initialize the vb2 queue */ 424 q = &vcap->queue; 425 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 426 q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_USERPTR; 427 q->drv_priv = vcap; 428 q->buf_struct_size = sizeof(struct vimc_cap_buffer); 429 q->ops = &vimc_cap_qops; 430 q->mem_ops = &vb2_vmalloc_memops; 431 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 432 q->min_buffers_needed = 2; 433 q->lock = &vcap->lock; 434 435 ret = vb2_queue_init(q); 436 if (ret) { 437 dev_err(vimc->mdev.dev, "%s: vb2 queue init failed (err=%d)\n", 438 vcfg_name, ret); 439 goto err_clean_m_ent; 440 } 441 442 /* Initialize buffer list and its lock */ 443 INIT_LIST_HEAD(&vcap->buf_list); 444 spin_lock_init(&vcap->qlock); 445 446 /* Set default frame format */ 447 vcap->format = fmt_default; 448 vpix = vimc_pix_map_by_pixelformat(vcap->format.pixelformat); 449 vcap->format.bytesperline = vcap->format.width * vpix->bpp; 450 vcap->format.sizeimage = vcap->format.bytesperline * 451 vcap->format.height; 452 453 /* Fill the vimc_ent_device struct */ 454 vcap->ved.ent = &vcap->vdev.entity; 455 vcap->ved.process_frame = vimc_cap_process_frame; 456 vcap->ved.vdev_get_format = vimc_cap_get_format; 457 vcap->ved.dev = vimc->mdev.dev; 458 459 /* Initialize the video_device struct */ 460 vdev = &vcap->vdev; 461 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING 462 | V4L2_CAP_IO_MC; 463 vdev->entity.ops = &vimc_cap_mops; 464 vdev->release = video_device_release_empty; 465 vdev->fops = &vimc_cap_fops; 466 vdev->ioctl_ops = &vimc_cap_ioctl_ops; 467 vdev->lock = &vcap->lock; 468 vdev->queue = q; 469 vdev->v4l2_dev = v4l2_dev; 470 vdev->vfl_dir = VFL_DIR_RX; 471 strscpy(vdev->name, vcfg_name, sizeof(vdev->name)); 472 video_set_drvdata(vdev, &vcap->ved); 473 474 /* Register the video_device with the v4l2 and the media framework */ 475 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 476 if (ret) { 477 dev_err(vimc->mdev.dev, "%s: video register failed (err=%d)\n", 478 vcap->vdev.name, ret); 479 goto err_clean_m_ent; 480 } 481 482 return &vcap->ved; 483 484 err_clean_m_ent: 485 media_entity_cleanup(&vcap->vdev.entity); 486 err_free_vcap: 487 kfree(vcap); 488 489 return ERR_PTR(ret); 490 } 491 492 struct vimc_ent_type vimc_cap_type = { 493 .add = vimc_cap_add, 494 .unregister = vimc_cap_unregister, 495 .release = vimc_cap_release 496 }; 497