1
2
3
4
5
6
7
8
9
10
11
12
13
14
15#include <linux/dma/xilinx_dma.h>
16#include <linux/lcm.h>
17#include <linux/list.h>
18#include <linux/module.h>
19#include <linux/of.h>
20#include <linux/slab.h>
21
22#include <media/v4l2-dev.h>
23#include <media/v4l2-fh.h>
24#include <media/v4l2-ioctl.h>
25#include <media/videobuf2-v4l2.h>
26#include <media/videobuf2-dma-contig.h>
27
28#include "xilinx-dma.h"
29#include "xilinx-vip.h"
30#include "xilinx-vipp.h"
31
32#define XVIP_DMA_DEF_FORMAT V4L2_PIX_FMT_YUYV
33#define XVIP_DMA_DEF_WIDTH 1920
34#define XVIP_DMA_DEF_HEIGHT 1080
35
36
37#define XVIP_DMA_MIN_WIDTH 1U
38#define XVIP_DMA_MAX_WIDTH 65535U
39#define XVIP_DMA_MIN_HEIGHT 1U
40#define XVIP_DMA_MAX_HEIGHT 8191U
41
42
43
44
45
46static struct v4l2_subdev *
47xvip_dma_remote_subdev(struct media_pad *local, u32 *pad)
48{
49 struct media_pad *remote;
50
51 remote = media_entity_remote_pad(local);
52 if (remote == NULL ||
53 media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
54 return NULL;
55
56 if (pad)
57 *pad = remote->index;
58
59 return media_entity_to_v4l2_subdev(remote->entity);
60}
61
62static int xvip_dma_verify_format(struct xvip_dma *dma)
63{
64 struct v4l2_subdev_format fmt;
65 struct v4l2_subdev *subdev;
66 int ret;
67
68 subdev = xvip_dma_remote_subdev(&dma->pad, &fmt.pad);
69 if (subdev == NULL)
70 return -EPIPE;
71
72 fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
73 ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
74 if (ret < 0)
75 return ret == -ENOIOCTLCMD ? -EINVAL : ret;
76
77 if (dma->fmtinfo->code != fmt.format.code ||
78 dma->format.height != fmt.format.height ||
79 dma->format.width != fmt.format.width ||
80 dma->format.colorspace != fmt.format.colorspace)
81 return -EINVAL;
82
83 return 0;
84}
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101static int xvip_pipeline_start_stop(struct xvip_pipeline *pipe, bool start)
102{
103 struct xvip_dma *dma = pipe->output;
104 struct media_entity *entity;
105 struct media_pad *pad;
106 struct v4l2_subdev *subdev;
107 int ret;
108
109 entity = &dma->video.entity;
110 while (1) {
111 pad = &entity->pads[0];
112 if (!(pad->flags & MEDIA_PAD_FL_SINK))
113 break;
114
115 pad = media_entity_remote_pad(pad);
116 if (pad == NULL ||
117 media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
118 break;
119
120 entity = pad->entity;
121 subdev = media_entity_to_v4l2_subdev(entity);
122
123 ret = v4l2_subdev_call(subdev, video, s_stream, start);
124 if (start && ret < 0 && ret != -ENOIOCTLCMD)
125 return ret;
126 }
127
128 return 0;
129}
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156static int xvip_pipeline_set_stream(struct xvip_pipeline *pipe, bool on)
157{
158 int ret = 0;
159
160 mutex_lock(&pipe->lock);
161
162 if (on) {
163 if (pipe->stream_count == pipe->num_dmas - 1) {
164 ret = xvip_pipeline_start_stop(pipe, true);
165 if (ret < 0)
166 goto done;
167 }
168 pipe->stream_count++;
169 } else {
170 if (--pipe->stream_count == 0)
171 xvip_pipeline_start_stop(pipe, false);
172 }
173
174done:
175 mutex_unlock(&pipe->lock);
176 return ret;
177}
178
179static int xvip_pipeline_validate(struct xvip_pipeline *pipe,
180 struct xvip_dma *start)
181{
182 struct media_entity_graph graph;
183 struct media_entity *entity = &start->video.entity;
184 struct media_device *mdev = entity->parent;
185 unsigned int num_inputs = 0;
186 unsigned int num_outputs = 0;
187
188 mutex_lock(&mdev->graph_mutex);
189
190
191 media_entity_graph_walk_start(&graph, entity);
192
193 while ((entity = media_entity_graph_walk_next(&graph))) {
194 struct xvip_dma *dma;
195
196 if (entity->type != MEDIA_ENT_T_DEVNODE_V4L)
197 continue;
198
199 dma = to_xvip_dma(media_entity_to_video_device(entity));
200
201 if (dma->pad.flags & MEDIA_PAD_FL_SINK) {
202 pipe->output = dma;
203 num_outputs++;
204 } else {
205 num_inputs++;
206 }
207 }
208
209 mutex_unlock(&mdev->graph_mutex);
210
211
212 if (num_outputs != 1 || num_inputs > 1)
213 return -EPIPE;
214
215 pipe->num_dmas = num_inputs + num_outputs;
216
217 return 0;
218}
219
220static void __xvip_pipeline_cleanup(struct xvip_pipeline *pipe)
221{
222 pipe->num_dmas = 0;
223 pipe->output = NULL;
224}
225
226
227
228
229
230
231
232static void xvip_pipeline_cleanup(struct xvip_pipeline *pipe)
233{
234 mutex_lock(&pipe->lock);
235
236
237 if (--pipe->use_count == 0)
238 __xvip_pipeline_cleanup(pipe);
239
240 mutex_unlock(&pipe->lock);
241}
242
243
244
245
246
247
248
249
250
251
252
253static int xvip_pipeline_prepare(struct xvip_pipeline *pipe,
254 struct xvip_dma *dma)
255{
256 int ret;
257
258 mutex_lock(&pipe->lock);
259
260
261 if (pipe->use_count == 0) {
262 ret = xvip_pipeline_validate(pipe, dma);
263 if (ret < 0) {
264 __xvip_pipeline_cleanup(pipe);
265 goto done;
266 }
267 }
268
269 pipe->use_count++;
270 ret = 0;
271
272done:
273 mutex_unlock(&pipe->lock);
274 return ret;
275}
276
277
278
279
280
281
282
283
284
285
286
287struct xvip_dma_buffer {
288 struct vb2_v4l2_buffer buf;
289 struct list_head queue;
290 struct xvip_dma *dma;
291};
292
293#define to_xvip_dma_buffer(vb) container_of(vb, struct xvip_dma_buffer, buf)
294
295static void xvip_dma_complete(void *param)
296{
297 struct xvip_dma_buffer *buf = param;
298 struct xvip_dma *dma = buf->dma;
299
300 spin_lock(&dma->queued_lock);
301 list_del(&buf->queue);
302 spin_unlock(&dma->queued_lock);
303
304 buf->buf.field = V4L2_FIELD_NONE;
305 buf->buf.sequence = dma->sequence++;
306 v4l2_get_timestamp(&buf->buf.timestamp);
307 vb2_set_plane_payload(&buf->buf.vb2_buf, 0, dma->format.sizeimage);
308 vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_DONE);
309}
310
311static int
312xvip_dma_queue_setup(struct vb2_queue *vq, const void *parg,
313 unsigned int *nbuffers, unsigned int *nplanes,
314 unsigned int sizes[], void *alloc_ctxs[])
315{
316 const struct v4l2_format *fmt = parg;
317 struct xvip_dma *dma = vb2_get_drv_priv(vq);
318
319
320 if (fmt && fmt->fmt.pix.sizeimage < dma->format.sizeimage)
321 return -EINVAL;
322
323 *nplanes = 1;
324
325 sizes[0] = fmt ? fmt->fmt.pix.sizeimage : dma->format.sizeimage;
326 alloc_ctxs[0] = dma->alloc_ctx;
327
328 return 0;
329}
330
331static int xvip_dma_buffer_prepare(struct vb2_buffer *vb)
332{
333 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
334 struct xvip_dma *dma = vb2_get_drv_priv(vb->vb2_queue);
335 struct xvip_dma_buffer *buf = to_xvip_dma_buffer(vbuf);
336
337 buf->dma = dma;
338
339 return 0;
340}
341
342static void xvip_dma_buffer_queue(struct vb2_buffer *vb)
343{
344 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
345 struct xvip_dma *dma = vb2_get_drv_priv(vb->vb2_queue);
346 struct xvip_dma_buffer *buf = to_xvip_dma_buffer(vbuf);
347 struct dma_async_tx_descriptor *desc;
348 dma_addr_t addr = vb2_dma_contig_plane_dma_addr(vb, 0);
349 u32 flags;
350
351 if (dma->queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
352 flags = DMA_PREP_INTERRUPT | DMA_CTRL_ACK;
353 dma->xt.dir = DMA_DEV_TO_MEM;
354 dma->xt.src_sgl = false;
355 dma->xt.dst_sgl = true;
356 dma->xt.dst_start = addr;
357 } else {
358 flags = DMA_PREP_INTERRUPT | DMA_CTRL_ACK;
359 dma->xt.dir = DMA_MEM_TO_DEV;
360 dma->xt.src_sgl = true;
361 dma->xt.dst_sgl = false;
362 dma->xt.src_start = addr;
363 }
364
365 dma->xt.frame_size = 1;
366 dma->sgl[0].size = dma->format.width * dma->fmtinfo->bpp;
367 dma->sgl[0].icg = dma->format.bytesperline - dma->sgl[0].size;
368 dma->xt.numf = dma->format.height;
369
370 desc = dmaengine_prep_interleaved_dma(dma->dma, &dma->xt, flags);
371 if (!desc) {
372 dev_err(dma->xdev->dev, "Failed to prepare DMA transfer\n");
373 vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_ERROR);
374 return;
375 }
376 desc->callback = xvip_dma_complete;
377 desc->callback_param = buf;
378
379 spin_lock_irq(&dma->queued_lock);
380 list_add_tail(&buf->queue, &dma->queued_bufs);
381 spin_unlock_irq(&dma->queued_lock);
382
383 dmaengine_submit(desc);
384
385 if (vb2_is_streaming(&dma->queue))
386 dma_async_issue_pending(dma->dma);
387}
388
389static int xvip_dma_start_streaming(struct vb2_queue *vq, unsigned int count)
390{
391 struct xvip_dma *dma = vb2_get_drv_priv(vq);
392 struct xvip_dma_buffer *buf, *nbuf;
393 struct xvip_pipeline *pipe;
394 int ret;
395
396 dma->sequence = 0;
397
398
399
400
401
402
403
404
405 pipe = dma->video.entity.pipe
406 ? to_xvip_pipeline(&dma->video.entity) : &dma->pipe;
407
408 ret = media_entity_pipeline_start(&dma->video.entity, &pipe->pipe);
409 if (ret < 0)
410 goto error;
411
412
413
414
415 ret = xvip_dma_verify_format(dma);
416 if (ret < 0)
417 goto error_stop;
418
419 ret = xvip_pipeline_prepare(pipe, dma);
420 if (ret < 0)
421 goto error_stop;
422
423
424
425
426 dma_async_issue_pending(dma->dma);
427
428
429 xvip_pipeline_set_stream(pipe, true);
430
431 return 0;
432
433error_stop:
434 media_entity_pipeline_stop(&dma->video.entity);
435
436error:
437
438 spin_lock_irq(&dma->queued_lock);
439 list_for_each_entry_safe(buf, nbuf, &dma->queued_bufs, queue) {
440 vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_QUEUED);
441 list_del(&buf->queue);
442 }
443 spin_unlock_irq(&dma->queued_lock);
444
445 return ret;
446}
447
448static void xvip_dma_stop_streaming(struct vb2_queue *vq)
449{
450 struct xvip_dma *dma = vb2_get_drv_priv(vq);
451 struct xvip_pipeline *pipe = to_xvip_pipeline(&dma->video.entity);
452 struct xvip_dma_buffer *buf, *nbuf;
453
454
455 xvip_pipeline_set_stream(pipe, false);
456
457
458 dmaengine_terminate_all(dma->dma);
459
460
461 xvip_pipeline_cleanup(pipe);
462 media_entity_pipeline_stop(&dma->video.entity);
463
464
465 spin_lock_irq(&dma->queued_lock);
466 list_for_each_entry_safe(buf, nbuf, &dma->queued_bufs, queue) {
467 vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_ERROR);
468 list_del(&buf->queue);
469 }
470 spin_unlock_irq(&dma->queued_lock);
471}
472
473static struct vb2_ops xvip_dma_queue_qops = {
474 .queue_setup = xvip_dma_queue_setup,
475 .buf_prepare = xvip_dma_buffer_prepare,
476 .buf_queue = xvip_dma_buffer_queue,
477 .wait_prepare = vb2_ops_wait_prepare,
478 .wait_finish = vb2_ops_wait_finish,
479 .start_streaming = xvip_dma_start_streaming,
480 .stop_streaming = xvip_dma_stop_streaming,
481};
482
483
484
485
486
487static int
488xvip_dma_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
489{
490 struct v4l2_fh *vfh = file->private_data;
491 struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
492
493 cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING
494 | dma->xdev->v4l2_caps;
495
496 if (dma->queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
497 cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
498 else
499 cap->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
500
501 strlcpy(cap->driver, "xilinx-vipp", sizeof(cap->driver));
502 strlcpy(cap->card, dma->video.name, sizeof(cap->card));
503 snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s:%u",
504 dma->xdev->dev->of_node->name, dma->port);
505
506 return 0;
507}
508
509
510
511
512
513
514static int
515xvip_dma_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
516{
517 struct v4l2_fh *vfh = file->private_data;
518 struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
519
520 if (f->index > 0)
521 return -EINVAL;
522
523 f->pixelformat = dma->format.pixelformat;
524 strlcpy(f->description, dma->fmtinfo->description,
525 sizeof(f->description));
526
527 return 0;
528}
529
530static int
531xvip_dma_get_format(struct file *file, void *fh, struct v4l2_format *format)
532{
533 struct v4l2_fh *vfh = file->private_data;
534 struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
535
536 format->fmt.pix = dma->format;
537
538 return 0;
539}
540
541static void
542__xvip_dma_try_format(struct xvip_dma *dma, struct v4l2_pix_format *pix,
543 const struct xvip_video_format **fmtinfo)
544{
545 const struct xvip_video_format *info;
546 unsigned int min_width;
547 unsigned int max_width;
548 unsigned int min_bpl;
549 unsigned int max_bpl;
550 unsigned int width;
551 unsigned int align;
552 unsigned int bpl;
553
554
555
556
557 info = xvip_get_format_by_fourcc(pix->pixelformat);
558 if (IS_ERR(info))
559 info = xvip_get_format_by_fourcc(XVIP_DMA_DEF_FORMAT);
560
561 pix->pixelformat = info->fourcc;
562 pix->field = V4L2_FIELD_NONE;
563
564
565
566
567
568 align = lcm(dma->align, info->bpp);
569 min_width = roundup(XVIP_DMA_MIN_WIDTH, align);
570 max_width = rounddown(XVIP_DMA_MAX_WIDTH, align);
571 width = rounddown(pix->width * info->bpp, align);
572
573 pix->width = clamp(width, min_width, max_width) / info->bpp;
574 pix->height = clamp(pix->height, XVIP_DMA_MIN_HEIGHT,
575 XVIP_DMA_MAX_HEIGHT);
576
577
578
579
580
581 min_bpl = pix->width * info->bpp;
582 max_bpl = rounddown(XVIP_DMA_MAX_WIDTH, dma->align);
583 bpl = rounddown(pix->bytesperline, dma->align);
584
585 pix->bytesperline = clamp(bpl, min_bpl, max_bpl);
586 pix->sizeimage = pix->bytesperline * pix->height;
587
588 if (fmtinfo)
589 *fmtinfo = info;
590}
591
592static int
593xvip_dma_try_format(struct file *file, void *fh, struct v4l2_format *format)
594{
595 struct v4l2_fh *vfh = file->private_data;
596 struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
597
598 __xvip_dma_try_format(dma, &format->fmt.pix, NULL);
599 return 0;
600}
601
602static int
603xvip_dma_set_format(struct file *file, void *fh, struct v4l2_format *format)
604{
605 struct v4l2_fh *vfh = file->private_data;
606 struct xvip_dma *dma = to_xvip_dma(vfh->vdev);
607 const struct xvip_video_format *info;
608
609 __xvip_dma_try_format(dma, &format->fmt.pix, &info);
610
611 if (vb2_is_busy(&dma->queue))
612 return -EBUSY;
613
614 dma->format = format->fmt.pix;
615 dma->fmtinfo = info;
616
617 return 0;
618}
619
620static const struct v4l2_ioctl_ops xvip_dma_ioctl_ops = {
621 .vidioc_querycap = xvip_dma_querycap,
622 .vidioc_enum_fmt_vid_cap = xvip_dma_enum_format,
623 .vidioc_g_fmt_vid_cap = xvip_dma_get_format,
624 .vidioc_g_fmt_vid_out = xvip_dma_get_format,
625 .vidioc_s_fmt_vid_cap = xvip_dma_set_format,
626 .vidioc_s_fmt_vid_out = xvip_dma_set_format,
627 .vidioc_try_fmt_vid_cap = xvip_dma_try_format,
628 .vidioc_try_fmt_vid_out = xvip_dma_try_format,
629 .vidioc_reqbufs = vb2_ioctl_reqbufs,
630 .vidioc_querybuf = vb2_ioctl_querybuf,
631 .vidioc_qbuf = vb2_ioctl_qbuf,
632 .vidioc_dqbuf = vb2_ioctl_dqbuf,
633 .vidioc_create_bufs = vb2_ioctl_create_bufs,
634 .vidioc_expbuf = vb2_ioctl_expbuf,
635 .vidioc_streamon = vb2_ioctl_streamon,
636 .vidioc_streamoff = vb2_ioctl_streamoff,
637};
638
639
640
641
642
643static const struct v4l2_file_operations xvip_dma_fops = {
644 .owner = THIS_MODULE,
645 .unlocked_ioctl = video_ioctl2,
646 .open = v4l2_fh_open,
647 .release = vb2_fop_release,
648 .poll = vb2_fop_poll,
649 .mmap = vb2_fop_mmap,
650};
651
652
653
654
655
656int xvip_dma_init(struct xvip_composite_device *xdev, struct xvip_dma *dma,
657 enum v4l2_buf_type type, unsigned int port)
658{
659 char name[16];
660 int ret;
661
662 dma->xdev = xdev;
663 dma->port = port;
664 mutex_init(&dma->lock);
665 mutex_init(&dma->pipe.lock);
666 INIT_LIST_HEAD(&dma->queued_bufs);
667 spin_lock_init(&dma->queued_lock);
668
669 dma->fmtinfo = xvip_get_format_by_fourcc(XVIP_DMA_DEF_FORMAT);
670 dma->format.pixelformat = dma->fmtinfo->fourcc;
671 dma->format.colorspace = V4L2_COLORSPACE_SRGB;
672 dma->format.field = V4L2_FIELD_NONE;
673 dma->format.width = XVIP_DMA_DEF_WIDTH;
674 dma->format.height = XVIP_DMA_DEF_HEIGHT;
675 dma->format.bytesperline = dma->format.width * dma->fmtinfo->bpp;
676 dma->format.sizeimage = dma->format.bytesperline * dma->format.height;
677
678
679 dma->pad.flags = type == V4L2_BUF_TYPE_VIDEO_CAPTURE
680 ? MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
681
682 ret = media_entity_init(&dma->video.entity, 1, &dma->pad, 0);
683 if (ret < 0)
684 goto error;
685
686
687 dma->video.fops = &xvip_dma_fops;
688 dma->video.v4l2_dev = &xdev->v4l2_dev;
689 dma->video.queue = &dma->queue;
690 snprintf(dma->video.name, sizeof(dma->video.name), "%s %s %u",
691 xdev->dev->of_node->name,
692 type == V4L2_BUF_TYPE_VIDEO_CAPTURE ? "output" : "input",
693 port);
694 dma->video.vfl_type = VFL_TYPE_GRABBER;
695 dma->video.vfl_dir = type == V4L2_BUF_TYPE_VIDEO_CAPTURE
696 ? VFL_DIR_RX : VFL_DIR_TX;
697 dma->video.release = video_device_release_empty;
698 dma->video.ioctl_ops = &xvip_dma_ioctl_ops;
699 dma->video.lock = &dma->lock;
700
701 video_set_drvdata(&dma->video, dma);
702
703
704 dma->alloc_ctx = vb2_dma_contig_init_ctx(dma->xdev->dev);
705 if (IS_ERR(dma->alloc_ctx)) {
706 ret = PTR_ERR(dma->alloc_ctx);
707 goto error;
708 }
709
710
711
712
713
714
715
716
717 dma->queue.type = type;
718 dma->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
719 dma->queue.lock = &dma->lock;
720 dma->queue.drv_priv = dma;
721 dma->queue.buf_struct_size = sizeof(struct xvip_dma_buffer);
722 dma->queue.ops = &xvip_dma_queue_qops;
723 dma->queue.mem_ops = &vb2_dma_contig_memops;
724 dma->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
725 | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
726 ret = vb2_queue_init(&dma->queue);
727 if (ret < 0) {
728 dev_err(dma->xdev->dev, "failed to initialize VB2 queue\n");
729 goto error;
730 }
731
732
733 snprintf(name, sizeof(name), "port%u", port);
734 dma->dma = dma_request_slave_channel(dma->xdev->dev, name);
735 if (dma->dma == NULL) {
736 dev_err(dma->xdev->dev, "no VDMA channel found\n");
737 ret = -ENODEV;
738 goto error;
739 }
740
741 dma->align = 1 << dma->dma->device->copy_align;
742
743 ret = video_register_device(&dma->video, VFL_TYPE_GRABBER, -1);
744 if (ret < 0) {
745 dev_err(dma->xdev->dev, "failed to register video device\n");
746 goto error;
747 }
748
749 return 0;
750
751error:
752 xvip_dma_cleanup(dma);
753 return ret;
754}
755
756void xvip_dma_cleanup(struct xvip_dma *dma)
757{
758 if (video_is_registered(&dma->video))
759 video_unregister_device(&dma->video);
760
761 if (dma->dma)
762 dma_release_channel(dma->dma);
763
764 if (!IS_ERR_OR_NULL(dma->alloc_ctx))
765 vb2_dma_contig_cleanup_ctx(dma->alloc_ctx);
766
767 media_entity_cleanup(&dma->video.entity);
768
769 mutex_destroy(&dma->lock);
770 mutex_destroy(&dma->pipe.lock);
771}
772