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