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 ret = -EINVAL;
156 goto stop_media_pipeline;
157 }
158
159 config.pixelformat = video->fmt.fmt.pix.pixelformat;
160 config.code = video->mbus_code;
161 config.field = video->fmt.fmt.pix.field;
162 config.width = video->fmt.fmt.pix.width;
163 config.height = video->fmt.fmt.pix.height;
164
165 ret = sun6i_csi_update_config(video->csi, &config);
166 if (ret < 0)
167 goto stop_media_pipeline;
168
169 spin_lock_irqsave(&video->dma_queue_lock, flags);
170
171 buf = list_first_entry(&video->dma_queue,
172 struct sun6i_csi_buffer, list);
173 buf->queued_to_csi = true;
174 sun6i_csi_update_buf_addr(video->csi, buf->dma_addr);
175
176 sun6i_csi_set_stream(video->csi, true);
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195 next_buf = list_next_entry(buf, list);
196 next_buf->queued_to_csi = true;
197 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
198
199 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
200
201 ret = v4l2_subdev_call(subdev, video, s_stream, 1);
202 if (ret && ret != -ENOIOCTLCMD)
203 goto stop_csi_stream;
204
205 return 0;
206
207stop_csi_stream:
208 sun6i_csi_set_stream(video->csi, false);
209stop_media_pipeline:
210 media_pipeline_stop(&video->vdev.entity);
211clear_dma_queue:
212 spin_lock_irqsave(&video->dma_queue_lock, flags);
213 list_for_each_entry(buf, &video->dma_queue, list)
214 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
215 INIT_LIST_HEAD(&video->dma_queue);
216 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
217
218 return ret;
219}
220
221static void sun6i_video_stop_streaming(struct vb2_queue *vq)
222{
223 struct sun6i_video *video = vb2_get_drv_priv(vq);
224 struct v4l2_subdev *subdev;
225 unsigned long flags;
226 struct sun6i_csi_buffer *buf;
227
228 subdev = sun6i_video_remote_subdev(video, NULL);
229 if (subdev)
230 v4l2_subdev_call(subdev, video, s_stream, 0);
231
232 sun6i_csi_set_stream(video->csi, false);
233
234 media_pipeline_stop(&video->vdev.entity);
235
236
237 spin_lock_irqsave(&video->dma_queue_lock, flags);
238 list_for_each_entry(buf, &video->dma_queue, list)
239 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
240 INIT_LIST_HEAD(&video->dma_queue);
241 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
242}
243
244static void sun6i_video_buffer_queue(struct vb2_buffer *vb)
245{
246 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
247 struct sun6i_csi_buffer *buf =
248 container_of(vbuf, struct sun6i_csi_buffer, vb);
249 struct sun6i_video *video = vb2_get_drv_priv(vb->vb2_queue);
250 unsigned long flags;
251
252 spin_lock_irqsave(&video->dma_queue_lock, flags);
253 buf->queued_to_csi = false;
254 list_add_tail(&buf->list, &video->dma_queue);
255 spin_unlock_irqrestore(&video->dma_queue_lock, flags);
256}
257
258void sun6i_video_frame_done(struct sun6i_video *video)
259{
260 struct sun6i_csi_buffer *buf;
261 struct sun6i_csi_buffer *next_buf;
262 struct vb2_v4l2_buffer *vbuf;
263
264 spin_lock(&video->dma_queue_lock);
265
266 buf = list_first_entry(&video->dma_queue,
267 struct sun6i_csi_buffer, list);
268 if (list_is_last(&buf->list, &video->dma_queue)) {
269 dev_dbg(video->csi->dev, "Frame dropped!\n");
270 goto unlock;
271 }
272
273 next_buf = list_next_entry(buf, list);
274
275
276
277
278
279 if (!next_buf->queued_to_csi) {
280 next_buf->queued_to_csi = true;
281 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
282 dev_dbg(video->csi->dev, "Frame dropped!\n");
283 goto unlock;
284 }
285
286 list_del(&buf->list);
287 vbuf = &buf->vb;
288 vbuf->vb2_buf.timestamp = ktime_get_ns();
289 vbuf->sequence = video->sequence;
290 vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
291
292
293 if (!list_is_last(&next_buf->list, &video->dma_queue)) {
294 next_buf = list_next_entry(next_buf, list);
295 next_buf->queued_to_csi = true;
296 sun6i_csi_update_buf_addr(video->csi, next_buf->dma_addr);
297 } else {
298 dev_dbg(video->csi->dev, "Next frame will be dropped!\n");
299 }
300
301unlock:
302 video->sequence++;
303 spin_unlock(&video->dma_queue_lock);
304}
305
306static const struct vb2_ops sun6i_csi_vb2_ops = {
307 .queue_setup = sun6i_video_queue_setup,
308 .wait_prepare = vb2_ops_wait_prepare,
309 .wait_finish = vb2_ops_wait_finish,
310 .buf_prepare = sun6i_video_buffer_prepare,
311 .start_streaming = sun6i_video_start_streaming,
312 .stop_streaming = sun6i_video_stop_streaming,
313 .buf_queue = sun6i_video_buffer_queue,
314};
315
316static int vidioc_querycap(struct file *file, void *priv,
317 struct v4l2_capability *cap)
318{
319 struct sun6i_video *video = video_drvdata(file);
320
321 strscpy(cap->driver, "sun6i-video", sizeof(cap->driver));
322 strscpy(cap->card, video->vdev.name, sizeof(cap->card));
323 snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
324 video->csi->dev->of_node->name);
325
326 return 0;
327}
328
329static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
330 struct v4l2_fmtdesc *f)
331{
332 u32 index = f->index;
333
334 if (index >= ARRAY_SIZE(supported_pixformats))
335 return -EINVAL;
336
337 f->pixelformat = supported_pixformats[index];
338
339 return 0;
340}
341
342static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
343 struct v4l2_format *fmt)
344{
345 struct sun6i_video *video = video_drvdata(file);
346
347 *fmt = video->fmt;
348
349 return 0;
350}
351
352static int sun6i_video_try_fmt(struct sun6i_video *video,
353 struct v4l2_format *f)
354{
355 struct v4l2_pix_format *pixfmt = &f->fmt.pix;
356 int bpp;
357
358 if (!is_pixformat_valid(pixfmt->pixelformat))
359 pixfmt->pixelformat = supported_pixformats[0];
360
361 v4l_bound_align_image(&pixfmt->width, MIN_WIDTH, MAX_WIDTH, 1,
362 &pixfmt->height, MIN_HEIGHT, MAX_WIDTH, 1, 1);
363
364 bpp = sun6i_csi_get_bpp(pixfmt->pixelformat);
365 pixfmt->bytesperline = (pixfmt->width * bpp) >> 3;
366 pixfmt->sizeimage = pixfmt->bytesperline * pixfmt->height;
367
368 if (pixfmt->field == V4L2_FIELD_ANY)
369 pixfmt->field = V4L2_FIELD_NONE;
370
371 pixfmt->colorspace = V4L2_COLORSPACE_RAW;
372 pixfmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
373 pixfmt->quantization = V4L2_QUANTIZATION_DEFAULT;
374 pixfmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
375
376 return 0;
377}
378
379static int sun6i_video_set_fmt(struct sun6i_video *video, struct v4l2_format *f)
380{
381 int ret;
382
383 ret = sun6i_video_try_fmt(video, f);
384 if (ret)
385 return ret;
386
387 video->fmt = *f;
388
389 return 0;
390}
391
392static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
393 struct v4l2_format *f)
394{
395 struct sun6i_video *video = video_drvdata(file);
396
397 if (vb2_is_busy(&video->vb2_vidq))
398 return -EBUSY;
399
400 return sun6i_video_set_fmt(video, f);
401}
402
403static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
404 struct v4l2_format *f)
405{
406 struct sun6i_video *video = video_drvdata(file);
407
408 return sun6i_video_try_fmt(video, f);
409}
410
411static int vidioc_enum_input(struct file *file, void *fh,
412 struct v4l2_input *inp)
413{
414 if (inp->index != 0)
415 return -EINVAL;
416
417 strscpy(inp->name, "camera", sizeof(inp->name));
418 inp->type = V4L2_INPUT_TYPE_CAMERA;
419
420 return 0;
421}
422
423static int vidioc_g_input(struct file *file, void *fh, unsigned int *i)
424{
425 *i = 0;
426
427 return 0;
428}
429
430static int vidioc_s_input(struct file *file, void *fh, unsigned int i)
431{
432 if (i != 0)
433 return -EINVAL;
434
435 return 0;
436}
437
438static const struct v4l2_ioctl_ops sun6i_video_ioctl_ops = {
439 .vidioc_querycap = vidioc_querycap,
440 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
441 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
442 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
443 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
444
445 .vidioc_enum_input = vidioc_enum_input,
446 .vidioc_s_input = vidioc_s_input,
447 .vidioc_g_input = vidioc_g_input,
448
449 .vidioc_reqbufs = vb2_ioctl_reqbufs,
450 .vidioc_querybuf = vb2_ioctl_querybuf,
451 .vidioc_qbuf = vb2_ioctl_qbuf,
452 .vidioc_expbuf = vb2_ioctl_expbuf,
453 .vidioc_dqbuf = vb2_ioctl_dqbuf,
454 .vidioc_create_bufs = vb2_ioctl_create_bufs,
455 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
456 .vidioc_streamon = vb2_ioctl_streamon,
457 .vidioc_streamoff = vb2_ioctl_streamoff,
458
459 .vidioc_log_status = v4l2_ctrl_log_status,
460 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
461 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
462};
463
464
465
466
467static int sun6i_video_open(struct file *file)
468{
469 struct sun6i_video *video = video_drvdata(file);
470 int ret;
471
472 if (mutex_lock_interruptible(&video->lock))
473 return -ERESTARTSYS;
474
475 ret = v4l2_fh_open(file);
476 if (ret < 0)
477 goto unlock;
478
479 ret = v4l2_pipeline_pm_get(&video->vdev.entity);
480 if (ret < 0)
481 goto fh_release;
482
483
484 if (!v4l2_fh_is_singular_file(file)) {
485 ret = -EBUSY;
486 goto unlock;
487 }
488
489 ret = sun6i_csi_set_power(video->csi, true);
490 if (ret < 0)
491 goto fh_release;
492
493 mutex_unlock(&video->lock);
494 return 0;
495
496fh_release:
497 v4l2_fh_release(file);
498unlock:
499 mutex_unlock(&video->lock);
500 return ret;
501}
502
503static int sun6i_video_close(struct file *file)
504{
505 struct sun6i_video *video = video_drvdata(file);
506 bool last_fh;
507
508 mutex_lock(&video->lock);
509
510 last_fh = v4l2_fh_is_singular_file(file);
511
512 _vb2_fop_release(file, NULL);
513
514 v4l2_pipeline_pm_put(&video->vdev.entity);
515
516 if (last_fh)
517 sun6i_csi_set_power(video->csi, false);
518
519 mutex_unlock(&video->lock);
520
521 return 0;
522}
523
524static const struct v4l2_file_operations sun6i_video_fops = {
525 .owner = THIS_MODULE,
526 .open = sun6i_video_open,
527 .release = sun6i_video_close,
528 .unlocked_ioctl = video_ioctl2,
529 .mmap = vb2_fop_mmap,
530 .poll = vb2_fop_poll
531};
532
533
534
535
536static int sun6i_video_link_validate_get_format(struct media_pad *pad,
537 struct v4l2_subdev_format *fmt)
538{
539 if (is_media_entity_v4l2_subdev(pad->entity)) {
540 struct v4l2_subdev *sd =
541 media_entity_to_v4l2_subdev(pad->entity);
542
543 fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
544 fmt->pad = pad->index;
545 return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
546 }
547
548 return -EINVAL;
549}
550
551static int sun6i_video_link_validate(struct media_link *link)
552{
553 struct video_device *vdev = container_of(link->sink->entity,
554 struct video_device, entity);
555 struct sun6i_video *video = video_get_drvdata(vdev);
556 struct v4l2_subdev_format source_fmt;
557 int ret;
558
559 video->mbus_code = 0;
560
561 if (!media_entity_remote_pad(link->sink->entity->pads)) {
562 dev_info(video->csi->dev,
563 "video node %s pad not connected\n", vdev->name);
564 return -ENOLINK;
565 }
566
567 ret = sun6i_video_link_validate_get_format(link->source, &source_fmt);
568 if (ret < 0)
569 return ret;
570
571 if (!sun6i_csi_is_format_supported(video->csi,
572 video->fmt.fmt.pix.pixelformat,
573 source_fmt.format.code)) {
574 dev_err(video->csi->dev,
575 "Unsupported pixformat: 0x%x with mbus code: 0x%x!\n",
576 video->fmt.fmt.pix.pixelformat,
577 source_fmt.format.code);
578 return -EPIPE;
579 }
580
581 if (source_fmt.format.width != video->fmt.fmt.pix.width ||
582 source_fmt.format.height != video->fmt.fmt.pix.height) {
583 dev_err(video->csi->dev,
584 "Wrong width or height %ux%u (%ux%u expected)\n",
585 video->fmt.fmt.pix.width, video->fmt.fmt.pix.height,
586 source_fmt.format.width, source_fmt.format.height);
587 return -EPIPE;
588 }
589
590 video->mbus_code = source_fmt.format.code;
591
592 return 0;
593}
594
595static const struct media_entity_operations sun6i_video_media_ops = {
596 .link_validate = sun6i_video_link_validate
597};
598
599int sun6i_video_init(struct sun6i_video *video, struct sun6i_csi *csi,
600 const char *name)
601{
602 struct video_device *vdev = &video->vdev;
603 struct vb2_queue *vidq = &video->vb2_vidq;
604 struct v4l2_format fmt = { 0 };
605 int ret;
606
607 video->csi = csi;
608
609
610 video->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
611 vdev->entity.ops = &sun6i_video_media_ops;
612 ret = media_entity_pads_init(&vdev->entity, 1, &video->pad);
613 if (ret < 0)
614 return ret;
615
616 mutex_init(&video->lock);
617
618 INIT_LIST_HEAD(&video->dma_queue);
619 spin_lock_init(&video->dma_queue_lock);
620
621 video->sequence = 0;
622
623
624 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
625 fmt.fmt.pix.pixelformat = supported_pixformats[0];
626 fmt.fmt.pix.width = 1280;
627 fmt.fmt.pix.height = 720;
628 fmt.fmt.pix.field = V4L2_FIELD_NONE;
629 sun6i_video_set_fmt(video, &fmt);
630
631
632 vidq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
633 vidq->io_modes = VB2_MMAP | VB2_DMABUF;
634 vidq->drv_priv = video;
635 vidq->buf_struct_size = sizeof(struct sun6i_csi_buffer);
636 vidq->ops = &sun6i_csi_vb2_ops;
637 vidq->mem_ops = &vb2_dma_contig_memops;
638 vidq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
639 vidq->lock = &video->lock;
640
641 vidq->min_buffers_needed = 3;
642 vidq->dev = csi->dev;
643
644 ret = vb2_queue_init(vidq);
645 if (ret) {
646 v4l2_err(&csi->v4l2_dev, "vb2_queue_init failed: %d\n", ret);
647 goto clean_entity;
648 }
649
650
651 strscpy(vdev->name, name, sizeof(vdev->name));
652 vdev->release = video_device_release_empty;
653 vdev->fops = &sun6i_video_fops;
654 vdev->ioctl_ops = &sun6i_video_ioctl_ops;
655 vdev->vfl_type = VFL_TYPE_VIDEO;
656 vdev->vfl_dir = VFL_DIR_RX;
657 vdev->v4l2_dev = &csi->v4l2_dev;
658 vdev->queue = vidq;
659 vdev->lock = &video->lock;
660 vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE;
661 video_set_drvdata(vdev, video);
662
663 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
664 if (ret < 0) {
665 v4l2_err(&csi->v4l2_dev,
666 "video_register_device failed: %d\n", ret);
667 goto clean_entity;
668 }
669
670 return 0;
671
672clean_entity:
673 media_entity_cleanup(&video->vdev.entity);
674 mutex_destroy(&video->lock);
675 return ret;
676}
677
678void sun6i_video_cleanup(struct sun6i_video *video)
679{
680 vb2_video_unregister_device(&video->vdev);
681 media_entity_cleanup(&video->vdev.entity);
682 mutex_destroy(&video->lock);
683}
684
