1 /*
2 *
3 * SPDX-License-Identifier: GPL-2.0
4 *
5 * Copyright (C) 2011-2018 ARM or its affiliates
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; version 2.
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
12 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
13 * for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17 *
18 */
19
20 #include <linux/videodev2.h>
21 #include <media/videobuf2-core.h>
22 #include <media/videobuf2-vmalloc.h>
23 #include <media/v4l2-device.h>
24 #include <media/videobuf2-memops.h>
25
26 #include "acamera_logger.h"
27 #include "isp-v4l2-common.h"
28 #include "isp-v4l2-stream.h"
29 #include "isp-vb2.h"
30 #include "system_am_sc.h"
31 #include "acamera_autocap.h"
32 /* ----------------------------------------------------------------
33 * VB2 operations
34 */
35 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 8, 0))
isp_vb2_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])36 static int isp_vb2_queue_setup( struct vb2_queue *vq,
37 unsigned int *nbuffers, unsigned int *nplanes,
38 unsigned int sizes[], struct device *alloc_devs[] )
39 #else
40 static int isp_vb2_queue_setup( struct vb2_queue *vq, const struct v4l2_format *fmt,
41 unsigned int *nbuffers, unsigned int *nplanes,
42 unsigned int sizes[], void *alloc_ctxs[] )
43 #endif
44 {
45 static unsigned long cnt = 0;
46 isp_v4l2_stream_t *pstream = vb2_get_drv_priv( vq );
47 struct v4l2_format vfmt;
48 //unsigned int size;
49 int i;
50 LOG( LOG_INFO, "Enter id:%d, cnt: %lu.", pstream->stream_id, cnt++ );
51
52 // get current format
53 if ( isp_v4l2_stream_get_format( pstream, &vfmt ) < 0 ) {
54 LOG( LOG_ERR, "fail to get format from stream" );
55 return -EBUSY;
56 }
57
58 #if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 8, 0))
59 if ( fmt )
60 LOG( LOG_INFO, "fmt: %p, width: %u, height: %u, sizeimage: %u.",
61 fmt, fmt->fmt.pix.width, fmt->fmt.pix.height, fmt->fmt.pix.sizeimage );
62 #endif
63
64 if ( vq->num_buffers + *nbuffers < 3 )
65 *nbuffers = 3 - vq->num_buffers;
66
67 if ( vfmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE ) {
68 *nplanes = 1;
69 sizes[0] = vfmt.fmt.pix.sizeimage;
70 } else if ( vfmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ) {
71 *nplanes = vfmt.fmt.pix_mp.num_planes;
72 for ( i = 0; i < vfmt.fmt.pix_mp.num_planes; i++ ) {
73 sizes[i] = vfmt.fmt.pix_mp.plane_fmt[i].sizeimage;
74 LOG( LOG_INFO, "size: %u", sizes[i] );
75 #if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 8, 0))
76 alloc_ctxs[i] = 0;
77 #endif
78 }
79 } else {
80 LOG( LOG_ERR, "Wrong type: %u", vfmt.type );
81 return -EINVAL;
82 }
83
84 /*
85 * videobuf2-vmalloc allocator is context-less so no need to set
86 * alloc_ctxs array.
87 */
88
89 return 0;
90 }
91
isp_vb2_buf_prepare(struct vb2_buffer * vb)92 static int isp_vb2_buf_prepare( struct vb2_buffer *vb )
93 {
94 unsigned long size;
95 isp_v4l2_stream_t *pstream = vb2_get_drv_priv( vb->vb2_queue );
96 static unsigned long cnt = 0;
97 struct v4l2_format vfmt;
98 int i;
99 LOG( LOG_DEBUG, "Enter id:%d, cnt: %lu.", pstream->stream_id, cnt++ );
100
101 // get current format
102 if ( isp_v4l2_stream_get_format( pstream, &vfmt ) < 0 ) {
103 LOG( LOG_ERR, "fail to get format from stream" );
104 return -EBUSY;
105 }
106
107 if ( vfmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE ) {
108 size = vfmt.fmt.pix.sizeimage;
109
110 if ( vb2_plane_size( vb, 0 ) < size ) {
111 LOG( LOG_ERR, "buffer too small (%lu < %lu)", vb2_plane_size( vb, 0 ), size );
112 return -EINVAL;
113 }
114
115 vb2_set_plane_payload( vb, 0, size );
116 } else if ( vfmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ) {
117 for ( i = 0; i < vfmt.fmt.pix_mp.num_planes; i++ ) {
118 size = vfmt.fmt.pix_mp.plane_fmt[i].sizeimage;
119 if ( vb2_plane_size( vb, i ) < size ) {
120 LOG( LOG_ERR, "i:%d buffer too small (%lu < %lu)", i, vb2_plane_size( vb, 0 ), size );
121 return -EINVAL;
122 }
123 vb2_set_plane_payload( vb, i, size );
124 }
125 }
126
127 return 0;
128 }
129
isp_vb_mmap_cvt(isp_v4l2_stream_t * pstream,tframe_t * frame,isp_v4l2_buffer_t * buf)130 static int isp_vb_mmap_cvt(isp_v4l2_stream_t *pstream, tframe_t *frame, isp_v4l2_buffer_t *buf)
131 {
132 void *p_mem = NULL;
133 void *s_mem = NULL;
134 unsigned int p_size = 0;
135 unsigned int s_size = 0;
136 unsigned int bytesline = 0;
137 struct page *cma_pages = NULL;
138
139 if (frame == NULL || buf == NULL) {
140 LOG(LOG_ERR, "Error input param");
141 return -1;
142 }
143
144 if (pstream->cur_v4l2_fmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
145 p_mem = vb2_plane_vaddr(&buf->vvb.vb2_buf, 0);
146 p_size = PAGE_ALIGN(buf->vvb.vb2_buf.planes[0].length);
147
148 s_mem = vb2_plane_vaddr(&buf->vvb.vb2_buf, 1);
149 s_size = PAGE_ALIGN(buf->vvb.vb2_buf.planes[1].length);
150
151 cma_pages = p_mem;
152 frame->primary.address = page_to_phys(cma_pages);
153 frame->primary.size = p_size;
154 frame->primary.line_offset = pstream->cur_v4l2_fmt.fmt.pix_mp.plane_fmt[0].bytesperline;
155
156 cma_pages = s_mem;
157 frame->secondary.address = page_to_phys(cma_pages);
158 frame->secondary.size = s_size;
159 frame->secondary.line_offset = pstream->cur_v4l2_fmt.fmt.pix_mp.plane_fmt[0].bytesperline;
160 } else if (pstream->cur_v4l2_fmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
161 if ((pstream->cur_v4l2_fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_NV21) ||
162 (pstream->cur_v4l2_fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_NV12)) {
163 p_mem = vb2_plane_vaddr(&buf->vvb.vb2_buf, 0);
164 bytesline = pstream->cur_v4l2_fmt.fmt.pix.bytesperline;
165 p_size = bytesline * pstream->cur_v4l2_fmt.fmt.pix.height;
166 s_size = p_size / 2;
167 cma_pages = p_mem;
168 frame->primary.address = page_to_phys(cma_pages);
169 frame->primary.size = p_size;
170 frame->secondary.address = frame->primary.address + p_size;
171 frame->secondary.size = s_size;
172 } else {
173 p_mem = vb2_plane_vaddr(&buf->vvb.vb2_buf, 0);
174 p_size = PAGE_ALIGN(buf->vvb.vb2_buf.planes[0].length);
175 cma_pages = p_mem;
176 frame->primary.address = page_to_phys(cma_pages);
177 frame->primary.size = p_size;
178 frame->secondary.address = 0;
179 frame->secondary.size = 0;
180 }
181 } else {
182 LOG(LOG_CRIT, "v4l2 bufer format not supported\n");
183 }
184
185 frame->list = (void *)&buf->list;
186 return 0;
187 }
188
189 #ifdef CONFIG_AMLOGIC_ION
190 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 10, 0))
isp_ion_buffer_to_phys(struct ion_buffer * buffer,phys_addr_t * addr,size_t * len)191 void isp_ion_buffer_to_phys(struct ion_buffer *buffer,
192 phys_addr_t *addr, size_t *len)
193 {
194 struct sg_table *sg_table;
195 struct page *page;
196
197 if (buffer) {
198 sg_table = buffer->sg_table;
199 page = sg_page(sg_table->sgl);
200 *addr = PFN_PHYS(page_to_pfn(page));
201 *len = buffer->size;
202 }
203 }
204
isp_ion_share_fd_to_phys(int fd,phys_addr_t * addr,size_t * len)205 int isp_ion_share_fd_to_phys(int fd, phys_addr_t *addr, size_t *len)
206 {
207 struct dma_buf *dmabuf;
208 struct ion_buffer *buffer;
209
210 dmabuf = dma_buf_get(fd);
211 if (IS_ERR_OR_NULL(dmabuf))
212 return PTR_ERR(dmabuf);
213
214 buffer = (struct ion_buffer *)dmabuf->priv;
215 isp_ion_buffer_to_phys(buffer, addr, len);
216 dma_buf_put(dmabuf);
217
218 return 0;
219 }
220 #endif
221
isp_vb_userptr_cvt(isp_v4l2_stream_t * pstream,tframe_t * frame,isp_v4l2_buffer_t * buf)222 static void isp_vb_userptr_cvt(isp_v4l2_stream_t *pstream, tframe_t *frame, isp_v4l2_buffer_t *buf)
223 {
224 int i = 0;
225 int ret = 0;
226 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 10, 0))
227 phys_addr_t addr;
228 #else
229 ion_phys_addr_t addr;
230 #endif
231 size_t size = 0;
232 uint32_t p_len = 0;
233 uint32_t p_off = 0;
234 uint32_t p_addr = 0;
235 struct vb2_cmalloc_buf *cma_buf;
236
237 for (i = 0; i < buf->vvb.vb2_buf.num_planes; i++) {
238 cma_buf = buf->vvb.vb2_buf.planes[i].mem_priv;
239 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 10, 0))
240 ret = isp_ion_share_fd_to_phys((unsigned long)cma_buf->vaddr,
241 &addr, &size);
242 #else
243 ret = meson_ion_share_fd_to_phys(pstream->ion_client, (unsigned long)cma_buf->vaddr,
244 &addr, &size);
245 #endif
246 if (ret < 0) {
247 LOG(LOG_CRIT, "Failed to get phys addr\n");
248 return;
249 }
250
251 p_len = buf->vvb.vb2_buf.planes[i].length;
252 p_addr = addr + p_off;
253
254 switch (i) {
255 case 0:
256 frame->primary.address = p_addr;
257 frame->primary.size = p_len;
258 break;
259 case 1:
260 frame->secondary.address = p_addr;
261 frame->secondary.size = p_len;
262 break;
263 default:
264 LOG(LOG_CRIT, "Failed to support %d planes\n", i + 1);
265 break;
266 }
267
268 LOG(LOG_DEBUG, "idx %u, plane[%d], addr 0x%x, len %u, off %u, size %u",
269 buf->vvb.vb2_buf.index, i, p_addr, p_len, p_off, size);
270
271 p_off += p_len;
272 }
273
274 frame->list = (void *)&buf->list;
275 }
276 #else
isp_vb_userptr_cvt(isp_v4l2_stream_t * pstream,tframe_t * frame,isp_v4l2_buffer_t * buf)277 static void isp_vb_userptr_cvt(isp_v4l2_stream_t *pstream, tframe_t *frame, isp_v4l2_buffer_t *buf)
278 {
279 }
280 #endif
isp_vb_to_tframe(isp_v4l2_stream_t * pstream,tframe_t * frame,isp_v4l2_buffer_t * buf)281 static int isp_vb_to_tframe(isp_v4l2_stream_t *pstream,
282 tframe_t *frame,
283 isp_v4l2_buffer_t *buf)
284 {
285 if (frame == NULL || buf == NULL) {
286 LOG(LOG_ERR, "Error input param");
287 return -1;
288 }
289
290 if (buf->vvb.vb2_buf.memory == VB2_MEMORY_MMAP)
291 isp_vb_mmap_cvt(pstream, frame, buf);
292 else if (buf->vvb.vb2_buf.memory == VB2_MEMORY_USERPTR)
293 isp_vb_userptr_cvt(pstream, frame, buf);
294
295 return 0;
296 }
297
isp_frame_buff_queue(void * stream,isp_v4l2_buffer_t * buf,unsigned int index)298 static void isp_frame_buff_queue(void *stream, isp_v4l2_buffer_t *buf, unsigned int index)
299 {
300 isp_v4l2_stream_t *pstream = NULL;
301 int32_t s_type = -1;
302 uint8_t d_type = 0;
303 uint32_t rtn = 0;
304 tframe_t f_buff;
305 int rc = -1;
306
307 if (stream == NULL || buf == NULL) {
308 LOG(LOG_ERR, "Error input param\n");
309 return;
310 }
311
312 pstream = stream;
313 memset(&f_buff, 0, sizeof(f_buff));
314
315 s_type = pstream->stream_type;
316
317 switch (s_type) {
318 case V4L2_STREAM_TYPE_FR:
319 d_type = dma_fr;
320 break;
321 case V4L2_STREAM_TYPE_DS1:
322 d_type = dma_ds1;
323 break;
324 #if ISP_HAS_DS2
325 case V4L2_STREAM_TYPE_DS2:
326 d_type = dma_ds2;
327 break;
328 #endif
329 default:
330 return;
331 }
332
333 if ((s_type == V4L2_STREAM_TYPE_FR) ||
334 (s_type == V4L2_STREAM_TYPE_DS1)) {
335 rc = isp_vb_to_tframe(pstream, &f_buff, buf);
336 if (rc != 0) {
337 LOG( LOG_ERR, "isp vb to tframe is error.");
338 return;
339 }
340
341 acamera_api_dma_buffer(pstream->ctx_id, d_type, &f_buff, 1, &rtn, index);
342 }
343
344 #if ISP_HAS_DS2
345 if (s_type == V4L2_STREAM_TYPE_DS2) {
346 rc = isp_vb_to_tframe(pstream, &f_buff, buf);
347 if (rc != 0) {
348 LOG( LOG_ERR, "isp vb to tframe is error.");
349 return;
350 }
351 am_sc_api_dma_buffer(&f_buff, index);
352 }
353 #endif
354 }
355
356 #ifdef AUTOWRITE_MODULES_V4L2_API
isp_autocap_buf_queue(isp_v4l2_stream_t * pstream,isp_v4l2_buffer_t * buf)357 void isp_autocap_buf_queue(isp_v4l2_stream_t *pstream, isp_v4l2_buffer_t *buf)
358 {
359 int32_t s_type = -1;
360 uint8_t d_type = 0;
361 tframe_t f_buff;
362
363 f_buff.primary.address = 0;
364
365 isp_vb_to_tframe(pstream, &f_buff, buf);
366
367 s_type = pstream->stream_type;
368
369 switch (s_type) {
370 case V4L2_STREAM_TYPE_FR:
371 d_type = dma_fr;
372 break;
373 case V4L2_STREAM_TYPE_DS1:
374 d_type = dma_ds1;
375 break;
376 #if ISP_HAS_DS2
377 case V4L2_STREAM_TYPE_DS2:
378 d_type = dma_ds2;
379 break;
380 #endif
381 default:
382 return;
383 }
384
385 autocap_pushbuf(pstream->ctx_id, d_type, f_buff, pstream);
386 }
387 #endif
388
isp_vb2_buf_queue(struct vb2_buffer * vb)389 static void isp_vb2_buf_queue( struct vb2_buffer *vb )
390 {
391 isp_v4l2_stream_t *pstream = vb2_get_drv_priv( vb->vb2_queue );
392 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
393 struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer( vb );
394 isp_v4l2_buffer_t *buf = container_of( vvb, isp_v4l2_buffer_t, vvb );
395 #else
396 isp_v4l2_buffer_t *buf = container_of( vb, isp_v4l2_buffer_t, vb );
397 #endif
398 static unsigned long cnt = 0;
399
400 LOG( LOG_DEBUG, "Enter id:%d, cnt: %lu.", pstream->stream_id, cnt++ );
401
402 spin_lock( &pstream->slock );
403 list_add_tail( &buf->list, &pstream->stream_buffer_list );
404 #ifdef AUTOWRITE_MODULES_V4L2_API
405 if(autocap_get_mode(pstream->ctx_id) == AUTOCAP_MODE0)
406 isp_autocap_buf_queue(pstream, buf);
407 #endif
408 isp_frame_buff_queue(pstream, buf, vb->index);
409 spin_unlock( &pstream->slock );
410 }
411
412 static const struct vb2_ops isp_vb2_ops = {
413 .queue_setup = isp_vb2_queue_setup, // called from VIDIOC_REQBUFS
414 .buf_prepare = isp_vb2_buf_prepare,
415 .buf_queue = isp_vb2_buf_queue,
416 .wait_prepare = vb2_ops_wait_prepare,
417 .wait_finish = vb2_ops_wait_finish,
418 };
419
420 /* ----------------------------------------------------------------
421 * VB2 external interface for isp-v4l2
422 */
isp_vb2_queue_init(struct vb2_queue * q,struct mutex * mlock,isp_v4l2_stream_t * pstream,struct device * dev)423 int isp_vb2_queue_init( struct vb2_queue *q, struct mutex *mlock, isp_v4l2_stream_t *pstream, struct device *dev )
424 {
425 #if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 10, 0))
426 #ifdef CONFIG_AMLOGIC_ION
427 char ion_client_name[32];
428 #endif
429 #endif
430 memset( q, 0, sizeof( struct vb2_queue ) );
431
432 /* start creating the vb2 queues */
433
434 //all stream multiplanar
435 q->type = pstream->cur_v4l2_fmt.type;
436
437 if (pstream->stream_id == V4L2_STREAM_TYPE_FR ||
438 pstream->stream_id == V4L2_STREAM_TYPE_DS1) {
439 q->mem_ops = &vb2_cmalloc_memops;
440 }
441 #if ISP_HAS_DS2
442 else if (pstream->stream_id == V4L2_STREAM_TYPE_DS2) {
443 q->mem_ops = &vb2_cmalloc_memops;
444 }
445 #endif
446 else {
447 q->mem_ops = &vb2_vmalloc_memops;
448 }
449
450 #ifdef CONFIG_AMLOGIC_ION
451 q->io_modes = VB2_MMAP | VB2_READ | VB2_USERPTR;
452 #else
453 q->io_modes = VB2_MMAP | VB2_READ;
454 #endif
455 q->drv_priv = pstream;
456 q->buf_struct_size = sizeof( isp_v4l2_buffer_t );
457
458 q->ops = &isp_vb2_ops;
459 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
460 q->min_buffers_needed = 3;
461 q->lock = mlock;
462 #if ( LINUX_VERSION_CODE >= KERNEL_VERSION( 4, 8, 0 ) )
463 q->dev = dev;
464 #endif
465
466 #if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 10, 0))
467 #ifdef CONFIG_AMLOGIC_ION
468 sprintf(ion_client_name, "isp_stream_%d", pstream->stream_id);
469 if (!pstream->ion_client)
470 pstream->ion_client = meson_ion_client_create(-1, ion_client_name);
471 #endif
472 #endif
473
474 return vb2_queue_init( q );
475 }
476
isp_vb2_queue_release(struct vb2_queue * q,isp_v4l2_stream_t * pstream)477 void isp_vb2_queue_release( struct vb2_queue *q, isp_v4l2_stream_t *pstream )
478 {
479 vb2_queue_release( q );
480
481 #if (LINUX_VERSION_CODE < KERNEL_VERSION(4, 10, 0))
482 #ifdef CONFIG_AMLOGIC_ION
483 if (pstream->ion_client) {
484 ion_client_destroy(pstream->ion_client);
485 pstream->ion_client = NULL;
486 }
487 #endif
488 #endif
489 }
490