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(buf: cap->bus_info, size: sizeof(cap->bus_info),
61 fmt: "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(pixelformat: format->pixelformat);
98 if (!vpix) {
99 format->pixelformat = fmt_default.pixelformat;
100 vpix = vimc_pix_map_by_pixelformat(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(q: &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(code: f->mbus_code);
160 } else {
161 vpix = vimc_pix_map_by_index(i: 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(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(lock: &vcapture->qlock);
232
233 list_for_each_entry_safe(vbuf, node, &vcapture->buf_list, list) {
234 list_del(entry: &vbuf->list);
235 vb2_buffer_done(vb: &vbuf->vb2.vb2_buf, state);
236 }
237
238 spin_unlock(lock: &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(q: vq);
244 int ret;
245
246 vcapture->sequence = 0;
247
248 /* Start the media pipeline */
249 ret = video_device_pipeline_start(vdev: &vcapture->vdev, pipe: &vcapture->stream.pipe);
250 if (ret) {
251 vimc_capture_return_all_buffers(vcapture, state: VB2_BUF_STATE_QUEUED);
252 return ret;
253 }
254
255 ret = vimc_streamer_s_stream(stream: &vcapture->stream, ved: &vcapture->ved, enable: 1);
256 if (ret) {
257 video_device_pipeline_stop(vdev: &vcapture->vdev);
258 vimc_capture_return_all_buffers(vcapture, state: 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(q: vq);
272
273 vimc_streamer_s_stream(stream: &vcapture->stream, ved: &vcapture->ved, enable: 0);
274
275 /* Stop the media pipeline */
276 video_device_pipeline_stop(vdev: &vcapture->vdev);
277
278 /* Release all active buffers */
279 vimc_capture_return_all_buffers(vcapture, state: 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(q: 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(lock: &vcapture->qlock);
290 list_add_tail(new: &buf->list, head: &vcapture->buf_list);
291 spin_unlock(lock: &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(q: 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(q: vb->vb2_queue);
312 unsigned long size = vcapture->format.sizeimage;
313
314 if (vb2_plane_size(vb, plane_no: 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(entity: vcapture->ved.ent);
346 kfree(objp: 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(vdev: &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(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(lock: &vcapture->qlock);
372 return ERR_PTR(error: -EAGAIN);
373 }
374
375 /* Remove this entry from the list */
376 list_del(entry: &vimc_buf->list);
377
378 spin_unlock(lock: &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(vb: &vimc_buf->vb2.vb2_buf, plane_no: 0);
386
387 memcpy(vbuf, frame, vcapture->format.sizeimage);
388
389 /* Set it as ready */
390 vb2_set_plane_payload(vb: &vimc_buf->vb2.vb2_buf, plane_no: 0,
391 size: vcapture->format.sizeimage);
392 vb2_buffer_done(vb: &vimc_buf->vb2.vb2_buf, state: 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(size: sizeof(*vcapture), GFP_KERNEL);
408 if (!vcapture)
409 return ERR_PTR(error: -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(entity: &vcapture->vdev.entity,
416 num_pads: 1, pads: &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_queued_buffers = 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(list: &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(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, data: &vcapture->ved);
477
478 /* Register the video_device with the v4l2 and the media framework */
479 ret = video_register_device(vdev, type: VFL_TYPE_VIDEO, nr: -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(entity: &vcapture->vdev.entity);
490err_free_vcapture:
491 kfree(objp: vcapture);
492
493 return ERR_PTR(error: 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

source code of linux/drivers/media/test-drivers/vimc/vimc-capture.c