1
2
3
4
5
6
7
8#include <linux/of.h>
9
10#include <media/v4l2-device.h>
11#include <media/v4l2-event.h>
12#include <media/v4l2-ioctl.h>
13#include <media/v4l2-mc.h>
14#include <media/videobuf2-dma-contig.h>
15#include <media/videobuf2-v4l2.h>
16
17#include "sun6i_csi.h"
18#include "sun6i_video.h"
19
20
21#define MIN_WIDTH (32)
22#define MIN_HEIGHT (32)
23#define MAX_WIDTH (4800)
24#define MAX_HEIGHT (4800)
25
26struct sun6i_csi_buffer {
27 struct vb2_v4l2_buffer vb;
28 struct list_head list;
29
30 dma_addr_t dma_addr;
31 bool queued_to_csi;
32};
33
34static const u32 supported_pixformats[] = {
35 V4L2_PIX_FMT_SBGGR8,
36 V4L2_PIX_FMT_SGBRG8,
37 V4L2_PIX_FMT_SGRBG8,
38 V4L2_PIX_FMT_SRGGB8,
39 V4L2_PIX_FMT_SBGGR10,
40 V4L2_PIX_FMT_SGBRG10,
41 V4L2_PIX_FMT_SGRBG10,
42 V4L2_PIX_FMT_SRGGB10,
43 V4L2_PIX_FMT_SBGGR12,
44 V4L2_PIX_FMT_SGBRG12,
45 V4L2_PIX_FMT_SGRBG12,
46 V4L2_PIX_FMT_SRGGB12,
47 V4L2_PIX_FMT_YUYV,
48 V4L2_PIX_FMT_YVYU,
49 V4L2_PIX_FMT_UYVY,
50 V4L2_PIX_FMT_VYUY,
51 V4L2_PIX_FMT_HM12,
52 V4L2_PIX_FMT_NV12,
53 V4L2_PIX_FMT_NV21,
54 V4L2_PIX_FMT_YUV420,
55 V4L2_PIX_FMT_YVU420,
56 V4L2_PIX_FMT_NV16,
57 V4L2_PIX_FMT_NV61,
58 V4L2_PIX_FMT_YUV422P,
59 V4L2_PIX_FMT_RGB565,
60 V4L2_PIX_FMT_RGB565X,
61 V4L2_PIX_FMT_JPEG,
62};
63
64static bool is_pixformat_valid(unsigned int pixformat)
65{
66 unsigned int i;
67
68 for (i = 0; i < ARRAY_SIZE(supported_pixformats); i++)
69 if (supported_pixformats[i] == pixformat)
70 return true;
71
72 return false;
73}
74
75static struct v4l2_subdev *
76sun6i_video_remote_subdev(struct sun6i_video *video, u32 *pad)
77{
78 struct media_pad *remote;
79
80 remote = media_entity_remote_pad(&video->pad);
81
82 if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
83 return NULL;
84
85 if (pad)
86 *pad = remote->index;
87
88 return media_entity_to_v4l2_subdev(remote->entity);
89}
90
91static int sun6i_video_queue_setup(struct vb2_queue *vq,
92 unsigned int *nbuffers,
93 unsigned int *nplanes,
94 unsigned int sizes[],
95 struct device *alloc_devs[])
96{
97 struct sun6i_video *video = vb2_get_drv_priv(vq);
98 unsigned int size = video->fmt.fmt.pix.sizeimage;
99
100 if (*nplanes)
101 return sizes[0] < size ? -EINVAL : 0;
102
103 *nplanes = 1;
104 sizes[0] = size;
105
106 return 0;
107}
108
109static int sun6i_video_buffer_prepare(struct vb2_buffer *vb)
110{
111 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
112 struct sun6i_csi_buffer *buf =
113 container_of(vbuf, struct sun6i_csi_buffer, vb);
114 struct sun6i_video *video = vb2_get_drv_priv(vb->vb2_queue);
115 unsigned long size = video->fmt.fmt.pix.sizeimage;
116
117 if (vb2_plane_size(vb, 0) < size) {
118 v4l2_err(video->vdev.v4l2_dev, "buffer too small (%lu < %lu)\n",
119 vb2_plane_size(vb, 0), size);
120 return -EINVAL;
121 }
122
123 vb2_set_plane_payload(vb, 0, size);
124
125 buf->dma_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
126
127 vbuf->field = video->fmt.fmt.pix.field;
128
129 return 0;
130}
131
132static int sun6i_video_start_streaming(struct vb2_queue *vq, unsigned int count)
133{
134 struct sun6i_video *video = vb2_get_drv_priv(vq);
135 struct sun6i_csi_buffer *buf;
136 struct sun6i_csi_buffer *next_buf;
137 struct sun6i_csi_config config;
138 struct v4l2_subdev *subdev;
139 unsigned long flags;
140 int ret;
141
142 video->sequence = 0;
143
144 ret = media_pipeline_start(&video->vdev.entity, &video->vdev.pipe);
145 if (ret < 0)
146 goto clear_dma_queue;
147
148 if (video->mbus_code == 0) {
149 ret = -EINVAL;
150 goto stop_media_pipeline;
151 }
152
153 subdev = sun6i_video_remote_subdev(video, NULL);
154 if (!subdev)
155 goto stop_media_pipeline;
156
157 config.pixelformat = video->fmt.fmt.pix.pixelformat;
158 config.code = video->mbus_code;
159 config.field = video->fmt.fmt.pix.field;
160 config.width = video->fmt.fmt.pix.width;
161 config.height = video->fmt.fmt.pix.height;
162
163 ret = sun6i_csi_update_config(video->csi, &config);
164 if (ret < 0)
165 goto stop_media_pipeline;
166
167 spin_lock_irqsave(&video->dma_queue_lock, flags);
168
169 buf = list_first_entry(&video->dma_queue,
170 struct sun6i_csi_buffer, list);
171 buf->queued_to_csi = true;
172 sun6i_csi_update_buf_addr(video->csi, buf->dma_addr);
173
174 sun6i_csi_set_stream(video->csi, true);
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193 next_buf = list_next_entry(buf, list);
194 next_buf->queued_to_csi = true;
195 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
196
197 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
198
199 ret = v4l2_subdev_call(subdev, video, s_stream, 1);
200 if (ret && ret != -ENOIOCTLCMD)
201 goto stop_csi_stream;
202
203 return 0;
204
205stop_csi_stream:
206 sun6i_csi_set_stream(video->csi, false);
207stop_media_pipeline:
208 media_pipeline_stop(&video->vdev.entity);
209clear_dma_queue:
210 spin_lock_irqsave(&video->dma_queue_lock, flags);
211 list_for_each_entry(buf, &video->dma_queue, list)
212 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
213 INIT_LIST_HEAD(&video->dma_queue);
214 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
215
216 return ret;
217}
218
219static void sun6i_video_stop_streaming(struct vb2_queue *vq)
220{
221 struct sun6i_video *video = vb2_get_drv_priv(vq);
222 struct v4l2_subdev *subdev;
223 unsigned long flags;
224 struct sun6i_csi_buffer *buf;
225
226 subdev = sun6i_video_remote_subdev(video, NULL);
227 if (subdev)
228 v4l2_subdev_call(subdev, video, s_stream, 0);
229
230 sun6i_csi_set_stream(video->csi, false);
231
232 media_pipeline_stop(&video->vdev.entity);
233
234
235 spin_lock_irqsave(&video->dma_queue_lock, flags);
236 list_for_each_entry(buf, &video->dma_queue, list)
237 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
238 INIT_LIST_HEAD(&video->dma_queue);
239 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
240}
241
242static void sun6i_video_buffer_queue(struct vb2_buffer *vb)
243{
244 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
245 struct sun6i_csi_buffer *buf =
246 container_of(vbuf, struct sun6i_csi_buffer, vb);
247 struct sun6i_video *video = vb2_get_drv_priv(vb->vb2_queue);
248 unsigned long flags;
249
250 spin_lock_irqsave(&video->dma_queue_lock, flags);
251 buf->queued_to_csi = false;
252 list_add_tail(&buf->list, &video->dma_queue);
253 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
254}
255
256void sun6i_video_frame_done(struct sun6i_video *video)
257{
258 struct sun6i_csi_buffer *buf;
259 struct sun6i_csi_buffer *next_buf;
260 struct vb2_v4l2_buffer *vbuf;
261
262 spin_lock(&video->dma_queue_lock);
263
264 buf = list_first_entry(&video->dma_queue,
265 struct sun6i_csi_buffer, list);
266 if (list_is_last(&buf->list, &video->dma_queue)) {
267 dev_dbg(video->csi->dev, "Frame dropped!\n");
268 goto unlock;
269 }
270
271 next_buf = list_next_entry(buf, list);
272
273
274
275
276
277 if (!next_buf->queued_to_csi) {
278 next_buf->queued_to_csi = true;
279 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
280 dev_dbg(video->csi->dev, "Frame dropped!\n");
281 goto unlock;
282 }
283
284 list_del(&buf->list);
285 vbuf = &buf->vb;
286 vbuf->vb2_buf.timestamp = ktime_get_ns();
287 vbuf->sequence = video->sequence;
288 vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
289
290
291 if (!list_is_last(&next_buf->list, &video->dma_queue)) {
292 next_buf = list_next_entry(next_buf, list);
293 next_buf->queued_to_csi = true;
294 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
295 } else {
296 dev_dbg(video->csi->dev, "Next frame will be dropped!\n");
297 }
298
299unlock:
300 video->sequence++;
301 spin_unlock(&video->dma_queue_lock);
302}
303
304static const struct vb2_ops sun6i_csi_vb2_ops = {
305 .queue_setup = sun6i_video_queue_setup,
306 .wait_prepare = vb2_ops_wait_prepare,
307 .wait_finish = vb2_ops_wait_finish,
308 .buf_prepare = sun6i_video_buffer_prepare,
309 .start_streaming = sun6i_video_start_streaming,
310 .stop_streaming = sun6i_video_stop_streaming,
311 .buf_queue = sun6i_video_buffer_queue,
312};
313
314static int vidioc_querycap(struct file *file, void *priv,
315 struct v4l2_capability *cap)
316{
317 struct sun6i_video *video = video_drvdata(file);
318
319 strscpy(cap->driver, "sun6i-video", sizeof(cap->driver));
320 strscpy(cap->card, video->vdev.name, sizeof(cap->card));
321 snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
322 video->csi->dev->of_node->name);
323
324 return 0;
325}
326
327static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
328 struct v4l2_fmtdesc *f)
329{
330 u32 index = f->index;
331
332 if (index >= ARRAY_SIZE(supported_pixformats))
333 return -EINVAL;
334
335 f->pixelformat = supported_pixformats[index];
336
337 return 0;
338}
339
340static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
341 struct v4l2_format *fmt)
342{
343 struct sun6i_video *video = video_drvdata(file);
344
345 *fmt = video->fmt;
346
347 return 0;
348}
349
350static int sun6i_video_try_fmt(struct sun6i_video *video,
351 struct v4l2_format *f)
352{
353 struct v4l2_pix_format *pixfmt = &f->fmt.pix;
354 int bpp;
355
356 if (!is_pixformat_valid(pixfmt->pixelformat))
357 pixfmt->pixelformat = supported_pixformats[0];
358
359 v4l_bound_align_image(&pixfmt->width, MIN_WIDTH, MAX_WIDTH, 1,
360 &pixfmt->height, MIN_HEIGHT, MAX_WIDTH, 1, 1);
361
362 bpp = sun6i_csi_get_bpp(pixfmt->pixelformat);
363 pixfmt->bytesperline = (pixfmt->width * bpp) >> 3;
364 pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
365
366 if (pixfmt->field == V4L2_FIELD_ANY)
367 pixfmt->field = V4L2_FIELD_NONE;
368
369 pixfmt->colorspace = V4L2_COLORSPACE_RAW;
370 pixfmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
371 pixfmt->quantization = V4L2_QUANTIZATION_DEFAULT;
372 pixfmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
373
374 return 0;
375}
376
377static int sun6i_video_set_fmt(struct sun6i_video *video, struct v4l2_format *f)
378{
379 int ret;
380
381 ret = sun6i_video_try_fmt(video, f);
382 if (ret)
383 return ret;
384
385 video->fmt = *f;
386
387 return 0;
388}
389
390static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
391 struct v4l2_format *f)
392{
393 struct sun6i_video *video = video_drvdata(file);
394
395 if (vb2_is_busy(&video->vb2_vidq))
396 return -EBUSY;
397
398 return sun6i_video_set_fmt(video, f);
399}
400
401static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
402 struct v4l2_format *f)
403{
404 struct sun6i_video *video = video_drvdata(file);
405
406 return sun6i_video_try_fmt(video, f);
407}
408
409static int vidioc_enum_input(struct file *file, void *fh,
410 struct v4l2_input *inp)
411{
412 if (inp->index != 0)
413 return -EINVAL;
414
415 strscpy(inp->name, "camera", sizeof(inp->name));
416 inp->type = V4L2_INPUT_TYPE_CAMERA;
417
418 return 0;
419}
420
421static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
422{
423 *i = 0;
424
425 return 0;
426}
427
428static int vidioc_s_input(struct file *file, void *fh, unsigned int i)
429{
430 if (i != 0)
431 return -EINVAL;
432
433 return 0;
434}
435
436static const struct v4l2_ioctl_ops sun6i_video_ioctl_ops = {
437 .vidioc_querycap = vidioc_querycap,
438 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
439 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
440 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
441 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
442
443 .vidioc_enum_input = vidioc_enum_input,
444 .vidioc_s_input = vidioc_s_input,
445 .vidioc_g_input = vidioc_g_input,
446
447 .vidioc_reqbufs = vb2_ioctl_reqbufs,
448 .vidioc_querybuf = vb2_ioctl_querybuf,
449 .vidioc_qbuf = vb2_ioctl_qbuf,
450 .vidioc_expbuf = vb2_ioctl_expbuf,
451 .vidioc_dqbuf = vb2_ioctl_dqbuf,
452 .vidioc_create_bufs = vb2_ioctl_create_bufs,
453 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
454 .vidioc_streamon = vb2_ioctl_streamon,
455 .vidioc_streamoff = vb2_ioctl_streamoff,
456
457 .vidioc_log_status = v4l2_ctrl_log_status,
458 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
459 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
460};
461
462
463
464
465static int sun6i_video_open(struct file *file)
466{
467 struct sun6i_video *video = video_drvdata(file);
468 int ret;
469
470 if (mutex_lock_interruptible(&video->lock))
471 return -ERESTARTSYS;
472
473 ret = v4l2_fh_open(file);
474 if (ret < 0)
475 goto unlock;
476
477 ret = v4l2_pipeline_pm_get(&video->vdev.entity);
478 if (ret < 0)
479 goto fh_release;
480
481
482 if (!v4l2_fh_is_singular_file(file))
483 goto unlock;
484
485 ret = sun6i_csi_set_power(video->csi, true);
486 if (ret < 0)
487 goto fh_release;
488
489 mutex_unlock(&video->lock);
490 return 0;
491
492fh_release:
493 v4l2_fh_release(file);
494unlock:
495 mutex_unlock(&video->lock);
496 return ret;
497}
498
499static int sun6i_video_close(struct file *file)
500{
501 struct sun6i_video *video = video_drvdata(file);
502 bool last_fh;
503
504 mutex_lock(&video->lock);
505
506 last_fh = v4l2_fh_is_singular_file(file);
507
508 _vb2_fop_release(file, NULL);
509
510 v4l2_pipeline_pm_put(&video->vdev.entity);
511
512 if (last_fh)
513 sun6i_csi_set_power(video->csi, false);
514
515 mutex_unlock(&video->lock);
516
517 return 0;
518}
519
520static const struct v4l2_file_operations sun6i_video_fops = {
521 .owner = THIS_MODULE,
522 .open = sun6i_video_open,
523 .release = sun6i_video_close,
524 .unlocked_ioctl = video_ioctl2,
525 .mmap = vb2_fop_mmap,
526 .poll = vb2_fop_poll
527};
528
529
530
531
532static int sun6i_video_link_validate_get_format(struct media_pad *pad,
533 struct v4l2_subdev_format *fmt)
534{
535 if (is_media_entity_v4l2_subdev(pad->entity)) {
536 struct v4l2_subdev *sd =
537 media_entity_to_v4l2_subdev(pad->entity);
538
539 fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
540 fmt->pad = pad->index;
541 return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
542 }
543
544 return -EINVAL;
545}
546
547static int sun6i_video_link_validate(struct media_link *link)
548{
549 struct video_device *vdev = container_of(link->sink->entity,
550 struct video_device, entity);
551 struct sun6i_video *video = video_get_drvdata(vdev);
552 struct v4l2_subdev_format source_fmt;
553 int ret;
554
555 video->mbus_code = 0;
556
557 if (!media_entity_remote_pad(link->sink->entity->pads)) {
558 dev_info(video->csi->dev,
559 "video node %s pad not connected\n", vdev->name);
560 return -ENOLINK;
561 }
562
563 ret = sun6i_video_link_validate_get_format(link->source, &source_fmt);
564 if (ret < 0)
565 return ret;
566
567 if (!sun6i_csi_is_format_supported(video->csi,
568 video->fmt.fmt.pix.pixelformat,
569 source_fmt.format.code)) {
570 dev_err(video->csi->dev,
571 "Unsupported pixformat: 0x%x with mbus code: 0x%x!\n",
572 video->fmt.fmt.pix.pixelformat,
573 source_fmt.format.code);
574 return -EPIPE;
575 }
576
577 if (source_fmt.format.width != video->fmt.fmt.pix.width ||
578 source_fmt.format.height != video->fmt.fmt.pix.height) {
579 dev_err(video->csi->dev,
580 "Wrong width or height %ux%u (%ux%u expected)\n",
581 video->fmt.fmt.pix.width, video->fmt.fmt.pix.height,
582 source_fmt.format.width, source_fmt.format.height);
583 return -EPIPE;
584 }
585
586 video->mbus_code = source_fmt.format.code;
587
588 return 0;
589}
590
591static const struct media_entity_operations sun6i_video_media_ops = {
592 .link_validate = sun6i_video_link_validate
593};
594
595int sun6i_video_init(struct sun6i_video *video, struct sun6i_csi *csi,
596 const char *name)
597{
598 struct video_device *vdev = &video->vdev;
599 struct vb2_queue *vidq = &video->vb2_vidq;
600 struct v4l2_format fmt = { 0 };
601 int ret;
602
603 video->csi = csi;
604
605
606 video->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
607 vdev->entity.ops = &sun6i_video_media_ops;
608 ret = media_entity_pads_init(&vdev->entity, 1, &video->pad);
609 if (ret < 0)
610 return ret;
611
612 mutex_init(&video->lock);
613
614 INIT_LIST_HEAD(&video->dma_queue);
615 spin_lock_init(&video->dma_queue_lock);
616
617 video->sequence = 0;
618
619
620 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
621 fmt.fmt.pix.pixelformat = supported_pixformats[0];
622 fmt.fmt.pix.width = 1280;
623 fmt.fmt.pix.height = 720;
624 fmt.fmt.pix.field = V4L2_FIELD_NONE;
625 sun6i_video_set_fmt(video, &fmt);
626
627
628 vidq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
629 vidq->io_modes = VB2_MMAP | VB2_DMABUF;
630 vidq->drv_priv = video;
631 vidq->buf_struct_size = sizeof(struct sun6i_csi_buffer);
632 vidq->ops = &sun6i_csi_vb2_ops;
633 vidq->mem_ops = &vb2_dma_contig_memops;
634 vidq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
635 vidq->lock = &video->lock;
636
637 vidq->min_buffers_needed = 3;
638 vidq->dev = csi->dev;
639
640 ret = vb2_queue_init(vidq);
641 if (ret) {
642 v4l2_err(&csi->v4l2_dev, "vb2_queue_init failed: %d\n", ret);
643 goto clean_entity;
644 }
645
646
647 strscpy(vdev->name, name, sizeof(vdev->name));
648 vdev->release = video_device_release_empty;
649 vdev->fops = &sun6i_video_fops;
650 vdev->ioctl_ops = &sun6i_video_ioctl_ops;
651 vdev->vfl_type = VFL_TYPE_VIDEO;
652 vdev->vfl_dir = VFL_DIR_RX;
653 vdev->v4l2_dev = &csi->v4l2_dev;
654 vdev->queue = vidq;
655 vdev->lock = &video->lock;
656 vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
657 video_set_drvdata(vdev, video);
658
659 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
660 if (ret < 0) {
661 v4l2_err(&csi->v4l2_dev,
662 "video_register_device failed: %d\n", ret);
663 goto release_vb2;
664 }
665
666 return 0;
667
668release_vb2:
669 vb2_queue_release(&video->vb2_vidq);
670clean_entity:
671 media_entity_cleanup(&video->vdev.entity);
672 mutex_destroy(&video->lock);
673 return ret;
674}
675
676void sun6i_video_cleanup(struct sun6i_video *video)
677{
678 video_unregister_device(&video->vdev);
679 media_entity_cleanup(&video->vdev.entity);
680 vb2_queue_release(&video->vb2_vidq);
681 mutex_destroy(&video->lock);
682}
683
