1
2
3
4
5
6
7
8
9
10
11
12
13
14#include <linux/bitops.h>
15#include <linux/device.h>
16#include <linux/delay.h>
17#include <linux/errno.h>
18#include <linux/kernel.h>
19#include <linux/module.h>
20#include <linux/types.h>
21#include <linux/printk.h>
22#include <linux/pm_runtime.h>
23#include <linux/slab.h>
24#include <linux/videodev2.h>
25
26#include <media/v4l2-device.h>
27#include <media/v4l2-ioctl.h>
28#include <media/videobuf2-v4l2.h>
29#include <media/videobuf2-dma-contig.h>
30#include <media/drv-intf/exynos-fimc.h>
31
32#include "common.h"
33#include "media-dev.h"
34#include "fimc-is.h"
35#include "fimc-isp-video.h"
36#include "fimc-is-param.h"
37
38static int isp_video_capture_queue_setup(struct vb2_queue *vq,
39 unsigned int *num_buffers, unsigned int *num_planes,
40 unsigned int sizes[], struct device *alloc_devs[])
41{
42 struct fimc_isp *isp = vb2_get_drv_priv(vq);
43 struct v4l2_pix_format_mplane *vid_fmt = &isp->video_capture.pixfmt;
44 const struct fimc_fmt *fmt = isp->video_capture.format;
45 unsigned int wh, i;
46
47 wh = vid_fmt->width * vid_fmt->height;
48
49 if (fmt == NULL)
50 return -EINVAL;
51
52 *num_buffers = clamp_t(u32, *num_buffers, FIMC_ISP_REQ_BUFS_MIN,
53 FIMC_ISP_REQ_BUFS_MAX);
54 if (*num_planes) {
55 if (*num_planes != fmt->memplanes)
56 return -EINVAL;
57 for (i = 0; i < *num_planes; i++)
58 if (sizes[i] < (wh * fmt->depth[i]) / 8)
59 return -EINVAL;
60 return 0;
61 }
62
63 *num_planes = fmt->memplanes;
64
65 for (i = 0; i < fmt->memplanes; i++)
66 sizes[i] = (wh * fmt->depth[i]) / 8;
67
68 return 0;
69}
70
71static inline struct param_dma_output *__get_isp_dma2(struct fimc_is *is)
72{
73 return &__get_curr_is_config(is)->isp.dma2_output;
74}
75
76static int isp_video_capture_start_streaming(struct vb2_queue *q,
77 unsigned int count)
78{
79 struct fimc_isp *isp = vb2_get_drv_priv(q);
80 struct fimc_is *is = fimc_isp_to_is(isp);
81 struct param_dma_output *dma = __get_isp_dma2(is);
82 struct fimc_is_video *video = &isp->video_capture;
83 int ret;
84
85 if (!test_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state) ||
86 test_bit(ST_ISP_VID_CAP_STREAMING, &isp->state))
87 return 0;
88
89
90 dma->cmd = DMA_OUTPUT_COMMAND_ENABLE;
91 dma->notify_dma_done = DMA_OUTPUT_NOTIFY_DMA_DONE_ENABLE;
92 dma->buffer_address = is->is_dma_p_region +
93 DMA2_OUTPUT_ADDR_ARRAY_OFFS;
94 dma->buffer_number = video->reqbufs_count;
95 dma->dma_out_mask = video->buf_mask;
96
97 isp_dbg(2, &video->ve.vdev,
98 "buf_count: %d, planes: %d, dma addr table: %#x\n",
99 video->buf_count, video->format->memplanes,
100 dma->buffer_address);
101
102 fimc_is_mem_barrier();
103
104 fimc_is_set_param_bit(is, PARAM_ISP_DMA2_OUTPUT);
105 __fimc_is_hw_update_param(is, PARAM_ISP_DMA2_OUTPUT);
106
107 ret = fimc_is_itf_s_param(is, false);
108 if (ret < 0)
109 return ret;
110
111 ret = fimc_pipeline_call(&video->ve, set_stream, 1);
112 if (ret < 0)
113 return ret;
114
115 set_bit(ST_ISP_VID_CAP_STREAMING, &isp->state);
116 return ret;
117}
118
119static void isp_video_capture_stop_streaming(struct vb2_queue *q)
120{
121 struct fimc_isp *isp = vb2_get_drv_priv(q);
122 struct fimc_is *is = fimc_isp_to_is(isp);
123 struct param_dma_output *dma = __get_isp_dma2(is);
124 int ret;
125
126 ret = fimc_pipeline_call(&isp->video_capture.ve, set_stream, 0);
127 if (ret < 0)
128 return;
129
130 dma->cmd = DMA_OUTPUT_COMMAND_DISABLE;
131 dma->notify_dma_done = DMA_OUTPUT_NOTIFY_DMA_DONE_DISABLE;
132 dma->buffer_number = 0;
133 dma->buffer_address = 0;
134 dma->dma_out_mask = 0;
135
136 fimc_is_set_param_bit(is, PARAM_ISP_DMA2_OUTPUT);
137 __fimc_is_hw_update_param(is, PARAM_ISP_DMA2_OUTPUT);
138
139 ret = fimc_is_itf_s_param(is, false);
140 if (ret < 0)
141 dev_warn(&is->pdev->dev, "%s: DMA stop failed\n", __func__);
142
143 fimc_is_hw_set_isp_buf_mask(is, 0);
144
145 clear_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state);
146 clear_bit(ST_ISP_VID_CAP_STREAMING, &isp->state);
147
148 isp->video_capture.buf_count = 0;
149}
150
151static int isp_video_capture_buffer_prepare(struct vb2_buffer *vb)
152{
153 struct fimc_isp *isp = vb2_get_drv_priv(vb->vb2_queue);
154 struct fimc_is_video *video = &isp->video_capture;
155 int i;
156
157 if (video->format == NULL)
158 return -EINVAL;
159
160 for (i = 0; i < video->format->memplanes; i++) {
161 unsigned long size = video->pixfmt.plane_fmt[i].sizeimage;
162
163 if (vb2_plane_size(vb, i) < size) {
164 v4l2_err(&video->ve.vdev,
165 "User buffer too small (%ld < %ld)\n",
166 vb2_plane_size(vb, i), size);
167 return -EINVAL;
168 }
169 vb2_set_plane_payload(vb, i, size);
170 }
171
172
173 if (test_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state)) {
174 dma_addr_t dma_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
175 int i;
176
177 for (i = 0; i < video->buf_count; i++)
178 if (video->buffers[i]->dma_addr[0] == dma_addr)
179 return 0;
180 return -ENXIO;
181 }
182
183 return 0;
184}
185
186static void isp_video_capture_buffer_queue(struct vb2_buffer *vb)
187{
188 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
189 struct fimc_isp *isp = vb2_get_drv_priv(vb->vb2_queue);
190 struct fimc_is_video *video = &isp->video_capture;
191 struct fimc_is *is = fimc_isp_to_is(isp);
192 struct isp_video_buf *ivb = to_isp_video_buf(vbuf);
193 unsigned long flags;
194 unsigned int i;
195
196 if (test_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state)) {
197 spin_lock_irqsave(&is->slock, flags);
198 video->buf_mask |= BIT(ivb->index);
199 spin_unlock_irqrestore(&is->slock, flags);
200 } else {
201 unsigned int num_planes = video->format->memplanes;
202
203 ivb->index = video->buf_count;
204 video->buffers[ivb->index] = ivb;
205
206 for (i = 0; i < num_planes; i++) {
207 int buf_index = ivb->index * num_planes + i;
208
209 ivb->dma_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
210 is->is_p_region->shared[32 + buf_index] =
211 ivb->dma_addr[i];
212
213 isp_dbg(2, &video->ve.vdev,
214 "dma_buf %d (%d/%d/%d) addr: %pad\n",
215 buf_index, ivb->index, i, vb->index,
216 &ivb->dma_addr[i]);
217 }
218
219 if (++video->buf_count < video->reqbufs_count)
220 return;
221
222 video->buf_mask = (1UL << video->buf_count) - 1;
223 set_bit(ST_ISP_VID_CAP_BUF_PREP, &isp->state);
224 }
225
226 if (!test_bit(ST_ISP_VID_CAP_STREAMING, &isp->state))
227 isp_video_capture_start_streaming(vb->vb2_queue, 0);
228}
229
230
231
232
233
234void fimc_isp_video_irq_handler(struct fimc_is *is)
235{
236 struct fimc_is_video *video = &is->isp.video_capture;
237 struct vb2_v4l2_buffer *vbuf;
238 int buf_index;
239
240
241 if (!test_bit(ST_ISP_VID_CAP_STREAMING, &is->isp.state))
242 return;
243
244 buf_index = (is->i2h_cmd.args[1] - 1) % video->buf_count;
245 vbuf = &video->buffers[buf_index]->vb;
246
247 vbuf->vb2_buf.timestamp = ktime_get_ns();
248 vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
249
250 video->buf_mask &= ~BIT(buf_index);
251 fimc_is_hw_set_isp_buf_mask(is, video->buf_mask);
252}
253
254static const struct vb2_ops isp_video_capture_qops = {
255 .queue_setup = isp_video_capture_queue_setup,
256 .buf_prepare = isp_video_capture_buffer_prepare,
257 .buf_queue = isp_video_capture_buffer_queue,
258 .wait_prepare = vb2_ops_wait_prepare,
259 .wait_finish = vb2_ops_wait_finish,
260 .start_streaming = isp_video_capture_start_streaming,
261 .stop_streaming = isp_video_capture_stop_streaming,
262};
263
264static int isp_video_open(struct file *file)
265{
266 struct fimc_isp *isp = video_drvdata(file);
267 struct exynos_video_entity *ve = &isp->video_capture.ve;
268 struct media_entity *me = &ve->vdev.entity;
269 int ret;
270
271 if (mutex_lock_interruptible(&isp->video_lock))
272 return -ERESTARTSYS;
273
274 ret = v4l2_fh_open(file);
275 if (ret < 0)
276 goto unlock;
277
278 ret = pm_runtime_get_sync(&isp->pdev->dev);
279 if (ret < 0)
280 goto rel_fh;
281
282 if (v4l2_fh_is_singular_file(file)) {
283 mutex_lock(&me->graph_obj.mdev->graph_mutex);
284
285 ret = fimc_pipeline_call(ve, open, me, true);
286
287
288 if (ret == 0)
289 me->use_count++;
290
291 mutex_unlock(&me->graph_obj.mdev->graph_mutex);
292 }
293 if (!ret)
294 goto unlock;
295rel_fh:
296 pm_runtime_put_noidle(&isp->pdev->dev);
297 v4l2_fh_release(file);
298unlock:
299 mutex_unlock(&isp->video_lock);
300 return ret;
301}
302
303static int isp_video_release(struct file *file)
304{
305 struct fimc_isp *isp = video_drvdata(file);
306 struct fimc_is_video *ivc = &isp->video_capture;
307 struct media_entity *entity = &ivc->ve.vdev.entity;
308 struct media_device *mdev = entity->graph_obj.mdev;
309
310 mutex_lock(&isp->video_lock);
311
312 if (v4l2_fh_is_singular_file(file) && ivc->streaming) {
313 media_pipeline_stop(entity);
314 ivc->streaming = 0;
315 }
316
317 _vb2_fop_release(file, NULL);
318
319 if (v4l2_fh_is_singular_file(file)) {
320 fimc_pipeline_call(&ivc->ve, close);
321
322 mutex_lock(&mdev->graph_mutex);
323 entity->use_count--;
324 mutex_unlock(&mdev->graph_mutex);
325 }
326
327 pm_runtime_put(&isp->pdev->dev);
328 mutex_unlock(&isp->video_lock);
329
330 return 0;
331}
332
333static const struct v4l2_file_operations isp_video_fops = {
334 .owner = THIS_MODULE,
335 .open = isp_video_open,
336 .release = isp_video_release,
337 .poll = vb2_fop_poll,
338 .unlocked_ioctl = video_ioctl2,
339 .mmap = vb2_fop_mmap,
340};
341
342
343
344
345static int isp_video_querycap(struct file *file, void *priv,
346 struct v4l2_capability *cap)
347{
348 struct fimc_isp *isp = video_drvdata(file);
349
350 __fimc_vidioc_querycap(&isp->pdev->dev, cap);
351 return 0;
352}
353
354static int isp_video_enum_fmt(struct file *file, void *priv,
355 struct v4l2_fmtdesc *f)
356{
357 const struct fimc_fmt *fmt;
358
359 if (f->index >= FIMC_ISP_NUM_FORMATS)
360 return -EINVAL;
361
362 fmt = fimc_isp_find_format(NULL, NULL, f->index);
363 if (WARN_ON(fmt == NULL))
364 return -EINVAL;
365
366 f->pixelformat = fmt->fourcc;
367
368 return 0;
369}
370
371static int isp_video_g_fmt_mplane(struct file *file, void *fh,
372 struct v4l2_format *f)
373{
374 struct fimc_isp *isp = video_drvdata(file);
375
376 f->fmt.pix_mp = isp->video_capture.pixfmt;
377 return 0;
378}
379
380static void __isp_video_try_fmt(struct fimc_isp *isp,
381 struct v4l2_pix_format_mplane *pixm,
382 const struct fimc_fmt **fmt)
383{
384 const struct fimc_fmt *__fmt;
385
386 __fmt = fimc_isp_find_format(&pixm->pixelformat, NULL, 2);
387
388 if (fmt)
389 *fmt = __fmt;
390
391 pixm->colorspace = V4L2_COLORSPACE_SRGB;
392 pixm->field = V4L2_FIELD_NONE;
393 pixm->num_planes = __fmt->memplanes;
394 pixm->pixelformat = __fmt->fourcc;
395
396
397
398
399 v4l_bound_align_image(&pixm->width, FIMC_ISP_SOURCE_WIDTH_MIN,
400 FIMC_ISP_SOURCE_WIDTH_MAX, 3,
401 &pixm->height, FIMC_ISP_SOURCE_HEIGHT_MIN,
402 FIMC_ISP_SOURCE_HEIGHT_MAX, 0, 0);
403}
404
405static int isp_video_try_fmt_mplane(struct file *file, void *fh,
406 struct v4l2_format *f)
407{
408 struct fimc_isp *isp = video_drvdata(file);
409
410 __isp_video_try_fmt(isp, &f->fmt.pix_mp, NULL);
411 return 0;
412}
413
414static int isp_video_s_fmt_mplane(struct file *file, void *priv,
415 struct v4l2_format *f)
416{
417 struct fimc_isp *isp = video_drvdata(file);
418 struct fimc_is *is = fimc_isp_to_is(isp);
419 struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
420 const struct fimc_fmt *ifmt = NULL;
421 struct param_dma_output *dma = __get_isp_dma2(is);
422
423 __isp_video_try_fmt(isp, pixm, &ifmt);
424
425 if (WARN_ON(ifmt == NULL))
426 return -EINVAL;
427
428 dma->format = DMA_OUTPUT_FORMAT_BAYER;
429 dma->order = DMA_OUTPUT_ORDER_GB_BG;
430 dma->plane = ifmt->memplanes;
431 dma->bitwidth = ifmt->depth[0];
432 dma->width = pixm->width;
433 dma->height = pixm->height;
434
435 fimc_is_mem_barrier();
436
437 isp->video_capture.format = ifmt;
438 isp->video_capture.pixfmt = *pixm;
439
440 return 0;
441}
442
443
444
445
446
447static int isp_video_pipeline_validate(struct fimc_isp *isp)
448{
449 struct v4l2_subdev *sd = &isp->subdev;
450 struct v4l2_subdev_format sink_fmt, src_fmt;
451 struct media_pad *pad;
452 int ret;
453
454 while (1) {
455
456 pad = &sd->entity.pads[0];
457 if (!(pad->flags & MEDIA_PAD_FL_SINK))
458 break;
459 sink_fmt.pad = pad->index;
460 sink_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
461 ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sink_fmt);
462 if (ret < 0 && ret != -ENOIOCTLCMD)
463 return -EPIPE;
464
465
466 pad = media_entity_remote_pad(pad);
467 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
468 break;
469
470 sd = media_entity_to_v4l2_subdev(pad->entity);
471 src_fmt.pad = pad->index;
472 src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
473 ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &src_fmt);
474 if (ret < 0 && ret != -ENOIOCTLCMD)
475 return -EPIPE;
476
477 if (src_fmt.format.width != sink_fmt.format.width ||
478 src_fmt.format.height != sink_fmt.format.height ||
479 src_fmt.format.code != sink_fmt.format.code)
480 return -EPIPE;
481 }
482
483 return 0;
484}
485
486static int isp_video_streamon(struct file *file, void *priv,
487 enum v4l2_buf_type type)
488{
489 struct fimc_isp *isp = video_drvdata(file);
490 struct exynos_video_entity *ve = &isp->video_capture.ve;
491 struct media_entity *me = &ve->vdev.entity;
492 int ret;
493
494 ret = media_pipeline_start(me, &ve->pipe->mp);
495 if (ret < 0)
496 return ret;
497
498 ret = isp_video_pipeline_validate(isp);
499 if (ret < 0)
500 goto p_stop;
501
502 ret = vb2_ioctl_streamon(file, priv, type);
503 if (ret < 0)
504 goto p_stop;
505
506 isp->video_capture.streaming = 1;
507 return 0;
508p_stop:
509 media_pipeline_stop(me);
510 return ret;
511}
512
513static int isp_video_streamoff(struct file *file, void *priv,
514 enum v4l2_buf_type type)
515{
516 struct fimc_isp *isp = video_drvdata(file);
517 struct fimc_is_video *video = &isp->video_capture;
518 int ret;
519
520 ret = vb2_ioctl_streamoff(file, priv, type);
521 if (ret < 0)
522 return ret;
523
524 media_pipeline_stop(&video->ve.vdev.entity);
525 video->streaming = 0;
526 return 0;
527}
528
529static int isp_video_reqbufs(struct file *file, void *priv,
530 struct v4l2_requestbuffers *rb)
531{
532 struct fimc_isp *isp = video_drvdata(file);
533 int ret;
534
535 ret = vb2_ioctl_reqbufs(file, priv, rb);
536 if (ret < 0)
537 return ret;
538
539 if (rb->count && rb->count < FIMC_ISP_REQ_BUFS_MIN) {
540 rb->count = 0;
541 vb2_ioctl_reqbufs(file, priv, rb);
542 ret = -ENOMEM;
543 }
544
545 isp->video_capture.reqbufs_count = rb->count;
546 return ret;
547}
548
549static const struct v4l2_ioctl_ops isp_video_ioctl_ops = {
550 .vidioc_querycap = isp_video_querycap,
551 .vidioc_enum_fmt_vid_cap = isp_video_enum_fmt,
552 .vidioc_try_fmt_vid_cap_mplane = isp_video_try_fmt_mplane,
553 .vidioc_s_fmt_vid_cap_mplane = isp_video_s_fmt_mplane,
554 .vidioc_g_fmt_vid_cap_mplane = isp_video_g_fmt_mplane,
555 .vidioc_reqbufs = isp_video_reqbufs,
556 .vidioc_querybuf = vb2_ioctl_querybuf,
557 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
558 .vidioc_create_bufs = vb2_ioctl_create_bufs,
559 .vidioc_qbuf = vb2_ioctl_qbuf,
560 .vidioc_dqbuf = vb2_ioctl_dqbuf,
561 .vidioc_streamon = isp_video_streamon,
562 .vidioc_streamoff = isp_video_streamoff,
563};
564
565int fimc_isp_video_device_register(struct fimc_isp *isp,
566 struct v4l2_device *v4l2_dev,
567 enum v4l2_buf_type type)
568{
569 struct vb2_queue *q = &isp->video_capture.vb_queue;
570 struct fimc_is_video *iv;
571 struct video_device *vdev;
572 int ret;
573
574 if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
575 iv = &isp->video_capture;
576 else
577 return -ENOSYS;
578
579 mutex_init(&isp->video_lock);
580 INIT_LIST_HEAD(&iv->pending_buf_q);
581 INIT_LIST_HEAD(&iv->active_buf_q);
582 iv->format = fimc_isp_find_format(NULL, NULL, 0);
583 iv->pixfmt.width = IS_DEFAULT_WIDTH;
584 iv->pixfmt.height = IS_DEFAULT_HEIGHT;
585 iv->pixfmt.pixelformat = iv->format->fourcc;
586 iv->pixfmt.colorspace = V4L2_COLORSPACE_SRGB;
587 iv->reqbufs_count = 0;
588
589 memset(q, 0, sizeof(*q));
590 q->type = type;
591 q->io_modes = VB2_MMAP | VB2_USERPTR;
592 q->ops = &isp_video_capture_qops;
593 q->mem_ops = &vb2_dma_contig_memops;
594 q->buf_struct_size = sizeof(struct isp_video_buf);
595 q->drv_priv = isp;
596 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
597 q->lock = &isp->video_lock;
598 q->dev = &isp->pdev->dev;
599
600 ret = vb2_queue_init(q);
601 if (ret < 0)
602 return ret;
603
604 vdev = &iv->ve.vdev;
605 memset(vdev, 0, sizeof(*vdev));
606 strscpy(vdev->name, "fimc-is-isp.capture", sizeof(vdev->name));
607 vdev->queue = q;
608 vdev->fops = &isp_video_fops;
609 vdev->ioctl_ops = &isp_video_ioctl_ops;
610 vdev->v4l2_dev = v4l2_dev;
611 vdev->minor = -1;
612 vdev->release = video_device_release_empty;
613 vdev->lock = &isp->video_lock;
614 vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE_MPLANE;
615
616 iv->pad.flags = MEDIA_PAD_FL_SINK;
617 ret = media_entity_pads_init(&vdev->entity, 1, &iv->pad);
618 if (ret < 0)
619 return ret;
620
621 video_set_drvdata(vdev, isp);
622
623 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
624 if (ret < 0) {
625 media_entity_cleanup(&vdev->entity);
626 return ret;
627 }
628
629 v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
630 vdev->name, video_device_node_name(vdev));
631
632 return 0;
633}
634
635void fimc_isp_video_device_unregister(struct fimc_isp *isp,
636 enum v4l2_buf_type type)
637{
638 struct exynos_video_entity *ve;
639
640 if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
641 ve = &isp->video_capture.ve;
642 else
643 return;
644
645 mutex_lock(&isp->video_lock);
646
647 if (video_is_registered(&ve->vdev)) {
648 video_unregister_device(&ve->vdev);
649 media_entity_cleanup(&ve->vdev.entity);
650 ve->pipe = NULL;
651 }
652
653 mutex_unlock(&isp->video_lock);
654}
655
