1
2
3
4
5
6
7
8#include <linux/component.h>
9#include <linux/module.h>
10#include <linux/mod_devicetable.h>
11#include <linux/platform_device.h>
12#include <media/v4l2-ioctl.h>
13#include <media/videobuf2-core.h>
14#include <media/videobuf2-vmalloc.h>
15
16#include "vimc-common.h"
17#include "vimc-streamer.h"
18
19#define VIMC_CAP_DRV_NAME "vimc-capture"
20
21static const u32 vimc_cap_supported_pixfmt[] = {
22 V4L2_PIX_FMT_BGR24,
23 V4L2_PIX_FMT_RGB24,
24 V4L2_PIX_FMT_ARGB32,
25 V4L2_PIX_FMT_SBGGR8,
26 V4L2_PIX_FMT_SGBRG8,
27 V4L2_PIX_FMT_SGRBG8,
28 V4L2_PIX_FMT_SRGGB8,
29 V4L2_PIX_FMT_SBGGR10,
30 V4L2_PIX_FMT_SGBRG10,
31 V4L2_PIX_FMT_SGRBG10,
32 V4L2_PIX_FMT_SRGGB10,
33 V4L2_PIX_FMT_SBGGR10ALAW8,
34 V4L2_PIX_FMT_SGBRG10ALAW8,
35 V4L2_PIX_FMT_SGRBG10ALAW8,
36 V4L2_PIX_FMT_SRGGB10ALAW8,
37 V4L2_PIX_FMT_SBGGR10DPCM8,
38 V4L2_PIX_FMT_SGBRG10DPCM8,
39 V4L2_PIX_FMT_SGRBG10DPCM8,
40 V4L2_PIX_FMT_SRGGB10DPCM8,
41 V4L2_PIX_FMT_SBGGR12,
42 V4L2_PIX_FMT_SGBRG12,
43 V4L2_PIX_FMT_SGRBG12,
44 V4L2_PIX_FMT_SRGGB12,
45};
46
47struct vimc_cap_device {
48 struct vimc_ent_device ved;
49 struct video_device vdev;
50 struct device *dev;
51 struct v4l2_pix_format format;
52 struct vb2_queue queue;
53 struct list_head buf_list;
54
55
56
57
58
59
60
61 spinlock_t qlock;
62 struct mutex lock;
63 u32 sequence;
64 struct vimc_stream stream;
65};
66
67static const struct v4l2_pix_format fmt_default = {
68 .width = 640,
69 .height = 480,
70 .pixelformat = V4L2_PIX_FMT_RGB24,
71 .field = V4L2_FIELD_NONE,
72 .colorspace = V4L2_COLORSPACE_DEFAULT,
73};
74
75struct vimc_cap_buffer {
76
77
78
79
80
81
82 struct vb2_v4l2_buffer vb2;
83 struct list_head list;
84};
85
86static int vimc_cap_querycap(struct file *file, void *priv,
87 struct v4l2_capability *cap)
88{
89 strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver));
90 strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
91 snprintf(cap->bus_info, sizeof(cap->bus_info),
92 "platform:%s", VIMC_PDEV_NAME);
93
94 return 0;
95}
96
97static void vimc_cap_get_format(struct vimc_ent_device *ved,
98 struct v4l2_pix_format *fmt)
99{
100 struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
101 ved);
102
103 *fmt = vcap->format;
104}
105
106static int vimc_cap_g_fmt_vid_cap(struct file *file, void *priv,
107 struct v4l2_format *f)
108{
109 struct vimc_cap_device *vcap = video_drvdata(file);
110
111 f->fmt.pix = vcap->format;
112
113 return 0;
114}
115
116static int vimc_cap_try_fmt_vid_cap(struct file *file, void *priv,
117 struct v4l2_format *f)
118{
119 struct v4l2_pix_format *format = &f->fmt.pix;
120
121 format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,
122 VIMC_FRAME_MAX_WIDTH) & ~1;
123 format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,
124 VIMC_FRAME_MAX_HEIGHT) & ~1;
125
126 vimc_colorimetry_clamp(format);
127
128 if (format->field == V4L2_FIELD_ANY)
129 format->field = fmt_default.field;
130
131
132
133
134 if (!v4l2_format_info(format->pixelformat))
135 format->pixelformat = fmt_default.pixelformat;
136
137 return v4l2_fill_pixfmt(format, format->pixelformat,
138 format->width, format->height);
139}
140
141static int vimc_cap_s_fmt_vid_cap(struct file *file, void *priv,
142 struct v4l2_format *f)
143{
144 struct vimc_cap_device *vcap = video_drvdata(file);
145 int ret;
146
147
148 if (vb2_is_busy(&vcap->queue))
149 return -EBUSY;
150
151 ret = vimc_cap_try_fmt_vid_cap(file, priv, f);
152 if (ret)
153 return ret;
154
155 dev_dbg(vcap->dev, "%s: format update: "
156 "old:%dx%d (0x%x, %d, %d, %d, %d) "
157 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcap->vdev.name,
158
159 vcap->format.width, vcap->format.height,
160 vcap->format.pixelformat, vcap->format.colorspace,
161 vcap->format.quantization, vcap->format.xfer_func,
162 vcap->format.ycbcr_enc,
163
164 f->fmt.pix.width, f->fmt.pix.height,
165 f->fmt.pix.pixelformat, f->fmt.pix.colorspace,
166 f->fmt.pix.quantization, f->fmt.pix.xfer_func,
167 f->fmt.pix.ycbcr_enc);
168
169 vcap->format = f->fmt.pix;
170
171 return 0;
172}
173
174static int vimc_cap_enum_fmt_vid_cap(struct file *file, void *priv,
175 struct v4l2_fmtdesc *f)
176{
177 if (f->index >= ARRAY_SIZE(vimc_cap_supported_pixfmt))
178 return -EINVAL;
179
180 f->pixelformat = vimc_cap_supported_pixfmt[f->index];
181
182 return 0;
183}
184
185static bool vimc_cap_is_pixfmt_supported(u32 pixelformat)
186{
187 unsigned int i;
188
189 for (i = 0; i < ARRAY_SIZE(vimc_cap_supported_pixfmt); i++)
190 if (vimc_cap_supported_pixfmt[i] == pixelformat)
191 return true;
192 return false;
193}
194
195static int vimc_cap_enum_framesizes(struct file *file, void *fh,
196 struct v4l2_frmsizeenum *fsize)
197{
198 if (fsize->index)
199 return -EINVAL;
200
201 if (!vimc_cap_is_pixfmt_supported(fsize->pixel_format))
202 return -EINVAL;
203
204 fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
205 fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;
206 fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;
207 fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;
208 fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;
209 fsize->stepwise.step_width = 1;
210 fsize->stepwise.step_height = 1;
211
212 return 0;
213}
214
215static const struct v4l2_file_operations vimc_cap_fops = {
216 .owner = THIS_MODULE,
217 .open = v4l2_fh_open,
218 .release = vb2_fop_release,
219 .read = vb2_fop_read,
220 .poll = vb2_fop_poll,
221 .unlocked_ioctl = video_ioctl2,
222 .mmap = vb2_fop_mmap,
223};
224
225static const struct v4l2_ioctl_ops vimc_cap_ioctl_ops = {
226 .vidioc_querycap = vimc_cap_querycap,
227
228 .vidioc_g_fmt_vid_cap = vimc_cap_g_fmt_vid_cap,
229 .vidioc_s_fmt_vid_cap = vimc_cap_s_fmt_vid_cap,
230 .vidioc_try_fmt_vid_cap = vimc_cap_try_fmt_vid_cap,
231 .vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap,
232 .vidioc_enum_framesizes = vimc_cap_enum_framesizes,
233
234 .vidioc_reqbufs = vb2_ioctl_reqbufs,
235 .vidioc_create_bufs = vb2_ioctl_create_bufs,
236 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
237 .vidioc_querybuf = vb2_ioctl_querybuf,
238 .vidioc_qbuf = vb2_ioctl_qbuf,
239 .vidioc_dqbuf = vb2_ioctl_dqbuf,
240 .vidioc_expbuf = vb2_ioctl_expbuf,
241 .vidioc_streamon = vb2_ioctl_streamon,
242 .vidioc_streamoff = vb2_ioctl_streamoff,
243};
244
245static void vimc_cap_return_all_buffers(struct vimc_cap_device *vcap,
246 enum vb2_buffer_state state)
247{
248 struct vimc_cap_buffer *vbuf, *node;
249
250 spin_lock(&vcap->qlock);
251
252 list_for_each_entry_safe(vbuf, node, &vcap->buf_list, list) {
253 list_del(&vbuf->list);
254 vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
255 }
256
257 spin_unlock(&vcap->qlock);
258}
259
260static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
261{
262 struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
263 struct media_entity *entity = &vcap->vdev.entity;
264 int ret;
265
266 vcap->sequence = 0;
267
268
269 ret = media_pipeline_start(entity, &vcap->stream.pipe);
270 if (ret) {
271 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
272 return ret;
273 }
274
275 vcap->stream.producer_pixfmt = vcap->format.pixelformat;
276 ret = vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 1);
277 if (ret) {
278 media_pipeline_stop(entity);
279 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
280 return ret;
281 }
282
283 return 0;
284}
285
286
287
288
289
290static void vimc_cap_stop_streaming(struct vb2_queue *vq)
291{
292 struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
293
294 vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 0);
295
296
297 media_pipeline_stop(&vcap->vdev.entity);
298
299
300 vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_ERROR);
301}
302
303static void vimc_cap_buf_queue(struct vb2_buffer *vb2_buf)
304{
305 struct vimc_cap_device *vcap = vb2_get_drv_priv(vb2_buf->vb2_queue);
306 struct vimc_cap_buffer *buf = container_of(vb2_buf,
307 struct vimc_cap_buffer,
308 vb2.vb2_buf);
309
310 spin_lock(&vcap->qlock);
311 list_add_tail(&buf->list, &vcap->buf_list);
312 spin_unlock(&vcap->qlock);
313}
314
315static int vimc_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
316 unsigned int *nplanes, unsigned int sizes[],
317 struct device *alloc_devs[])
318{
319 struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
320
321 if (*nplanes)
322 return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0;
323
324 *nplanes = 1;
325 sizes[0] = vcap->format.sizeimage;
326
327 return 0;
328}
329
330static int vimc_cap_buffer_prepare(struct vb2_buffer *vb)
331{
332 struct vimc_cap_device *vcap = vb2_get_drv_priv(vb->vb2_queue);
333 unsigned long size = vcap->format.sizeimage;
334
335 if (vb2_plane_size(vb, 0) < size) {
336 dev_err(vcap->dev, "%s: buffer too small (%lu < %lu)\n",
337 vcap->vdev.name, vb2_plane_size(vb, 0), size);
338 return -EINVAL;
339 }
340 return 0;
341}
342
343static const struct vb2_ops vimc_cap_qops = {
344 .start_streaming = vimc_cap_start_streaming,
345 .stop_streaming = vimc_cap_stop_streaming,
346 .buf_queue = vimc_cap_buf_queue,
347 .queue_setup = vimc_cap_queue_setup,
348 .buf_prepare = vimc_cap_buffer_prepare,
349
350
351
352
353 .wait_prepare = vb2_ops_wait_prepare,
354 .wait_finish = vb2_ops_wait_finish,
355};
356
357static const struct media_entity_operations vimc_cap_mops = {
358 .link_validate = vimc_link_validate,
359};
360
361static void vimc_cap_release(struct video_device *vdev)
362{
363 struct vimc_cap_device *vcap =
364 container_of(vdev, struct vimc_cap_device, vdev);
365
366 vimc_pads_cleanup(vcap->ved.pads);
367 kfree(vcap);
368}
369
370static void vimc_cap_comp_unbind(struct device *comp, struct device *master,
371 void *master_data)
372{
373 struct vimc_ent_device *ved = dev_get_drvdata(comp);
374 struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
375 ved);
376
377 vb2_queue_release(&vcap->queue);
378 media_entity_cleanup(ved->ent);
379 video_unregister_device(&vcap->vdev);
380}
381
382static void *vimc_cap_process_frame(struct vimc_ent_device *ved,
383 const void *frame)
384{
385 struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
386 ved);
387 struct vimc_cap_buffer *vimc_buf;
388 void *vbuf;
389
390 spin_lock(&vcap->qlock);
391
392
393 vimc_buf = list_first_entry_or_null(&vcap->buf_list,
394 typeof(*vimc_buf), list);
395 if (!vimc_buf) {
396 spin_unlock(&vcap->qlock);
397 return ERR_PTR(-EAGAIN);
398 }
399
400
401 list_del(&vimc_buf->list);
402
403 spin_unlock(&vcap->qlock);
404
405
406 vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
407 vimc_buf->vb2.sequence = vcap->sequence++;
408 vimc_buf->vb2.field = vcap->format.field;
409
410 vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);
411
412 memcpy(vbuf, frame, vcap->format.sizeimage);
413
414
415 vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
416 vcap->format.sizeimage);
417 vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
418 return NULL;
419}
420
421static int vimc_cap_comp_bind(struct device *comp, struct device *master,
422 void *master_data)
423{
424 struct v4l2_device *v4l2_dev = master_data;
425 struct vimc_platform_data *pdata = comp->platform_data;
426 struct vimc_cap_device *vcap;
427 struct video_device *vdev;
428 struct vb2_queue *q;
429 int ret;
430
431
432 vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
433 if (!vcap)
434 return -ENOMEM;
435
436
437 vcap->ved.pads =
438 vimc_pads_init(1, (const unsigned long[1]) {MEDIA_PAD_FL_SINK});
439 if (IS_ERR(vcap->ved.pads)) {
440 ret = PTR_ERR(vcap->ved.pads);
441 goto err_free_vcap;
442 }
443
444
445 vcap->vdev.entity.name = pdata->entity_name;
446 vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
447 ret = media_entity_pads_init(&vcap->vdev.entity,
448 1, vcap->ved.pads);
449 if (ret)
450 goto err_clean_pads;
451
452
453 mutex_init(&vcap->lock);
454
455
456 q = &vcap->queue;
457 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
458 q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_USERPTR;
459 q->drv_priv = vcap;
460 q->buf_struct_size = sizeof(struct vimc_cap_buffer);
461 q->ops = &vimc_cap_qops;
462 q->mem_ops = &vb2_vmalloc_memops;
463 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
464 q->min_buffers_needed = 2;
465 q->lock = &vcap->lock;
466
467 ret = vb2_queue_init(q);
468 if (ret) {
469 dev_err(comp, "%s: vb2 queue init failed (err=%d)\n",
470 pdata->entity_name, ret);
471 goto err_clean_m_ent;
472 }
473
474
475 INIT_LIST_HEAD(&vcap->buf_list);
476 spin_lock_init(&vcap->qlock);
477
478
479 vcap->format = fmt_default;
480 v4l2_fill_pixfmt(&vcap->format, vcap->format.pixelformat,
481 vcap->format.width, vcap->format.height);
482
483
484 vcap->ved.ent = &vcap->vdev.entity;
485 vcap->ved.process_frame = vimc_cap_process_frame;
486 vcap->ved.vdev_get_format = vimc_cap_get_format;
487 dev_set_drvdata(comp, &vcap->ved);
488 vcap->dev = comp;
489
490
491 vdev = &vcap->vdev;
492 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
493 vdev->entity.ops = &vimc_cap_mops;
494 vdev->release = vimc_cap_release;
495 vdev->fops = &vimc_cap_fops;
496 vdev->ioctl_ops = &vimc_cap_ioctl_ops;
497 vdev->lock = &vcap->lock;
498 vdev->queue = q;
499 vdev->v4l2_dev = v4l2_dev;
500 vdev->vfl_dir = VFL_DIR_RX;
501 strscpy(vdev->name, pdata->entity_name, sizeof(vdev->name));
502 video_set_drvdata(vdev, &vcap->ved);
503
504
505 ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
506 if (ret) {
507 dev_err(comp, "%s: video register failed (err=%d)\n",
508 vcap->vdev.name, ret);
509 goto err_release_queue;
510 }
511
512 return 0;
513
514err_release_queue:
515 vb2_queue_release(q);
516err_clean_m_ent:
517 media_entity_cleanup(&vcap->vdev.entity);
518err_clean_pads:
519 vimc_pads_cleanup(vcap->ved.pads);
520err_free_vcap:
521 kfree(vcap);
522
523 return ret;
524}
525
526static const struct component_ops vimc_cap_comp_ops = {
527 .bind = vimc_cap_comp_bind,
528 .unbind = vimc_cap_comp_unbind,
529};
530
531static int vimc_cap_probe(struct platform_device *pdev)
532{
533 return component_add(&pdev->dev, &vimc_cap_comp_ops);
534}
535
536static int vimc_cap_remove(struct platform_device *pdev)
537{
538 component_del(&pdev->dev, &vimc_cap_comp_ops);
539
540 return 0;
541}
542
543static const struct platform_device_id vimc_cap_driver_ids[] = {
544 {
545 .name = VIMC_CAP_DRV_NAME,
546 },
547 { }
548};
549
550static struct platform_driver vimc_cap_pdrv = {
551 .probe = vimc_cap_probe,
552 .remove = vimc_cap_remove,
553 .id_table = vimc_cap_driver_ids,
554 .driver = {
555 .name = VIMC_CAP_DRV_NAME,
556 },
557};
558
559module_platform_driver(vimc_cap_pdrv);
560
561MODULE_DEVICE_TABLE(platform, vimc_cap_driver_ids);
562
563MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Capture");
564MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
565MODULE_LICENSE("GPL");
566