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_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 | |
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_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 | |
55 | static 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 | |
66 | static 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 | |
75 | static 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 | |
85 | static 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 | |
117 | static 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 | |
150 | static 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 | |
172 | static 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 | |
196 | static 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 | |
206 | static 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 | |
226 | static 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 | |
241 | static 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 | */ |
269 | static 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 | |
282 | static 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 | |
294 | static 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 | |
309 | static 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 | |
322 | static 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 | |
336 | static const struct media_entity_operations vimc_capture_mops = { |
337 | .link_validate = vimc_vdev_link_validate, |
338 | }; |
339 | |
340 | static 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 | |
349 | static 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 | |
357 | static 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 | |
396 | static 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 | |
488 | err_clean_m_ent: |
489 | media_entity_cleanup(entity: &vcapture->vdev.entity); |
490 | err_free_vcapture: |
491 | kfree(objp: vcapture); |
492 | |
493 | return ERR_PTR(error: ret); |
494 | } |
495 | |
496 | struct vimc_ent_type vimc_capture_type = { |
497 | .add = vimc_capture_add, |
498 | .unregister = vimc_capture_unregister, |
499 | .release = vimc_capture_release |
500 | }; |
501 | |