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