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