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/version.h>
21 #include <linux/device.h>
22 #include <linux/kthread.h>
23 #include <linux/slab.h>
24 #include <linux/freezer.h>
25 #include <linux/random.h>
26 #include <asm/div64.h>
27 #include <linux/sched.h>
28
29 #include <linux/videodev2.h>
30 #include <media/videobuf2-core.h>
31 #include <media/videobuf2-vmalloc.h>
32 #include <media/v4l2-device.h>
33 #include <media/v4l2-ctrls.h>
34 #include <linux/dma-mapping.h>
35 #include <linux/of_reserved_mem.h>
36 #if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 0, 0))
37 #include <linux/dma-contiguous.h>
38 #else
39 #include <linux/dma-map-ops.h>
40 #endif
41
42 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(5, 0, 0))
43 #include <linux/ktime.h>
44 #include <linux/time64.h>
45 #include <linux/timekeeping.h>
46 #endif
47
48 #include "acamera_firmware_api.h"
49 #include "acamera_firmware_config.h"
50
51 #if defined( ISP_HAS_METADATA_FSM )
52 #include "metadata_api.h"
53 #endif
54
55 #include "acamera_logger.h"
56
57 #include "isp-v4l2-common.h"
58 #include "isp-v4l2.h"
59 #include "fw-interface.h"
60
61 #include "isp-v4l2-stream.h"
62
63 #define CHECK_METADATA_ID 0
64
65 #define ISP_FW_FRAME_BUF_INVALID 0 /* buffer data is invalid */
66 #define ISP_FW_FRAME_BUF_VALID 1 /* buffer data is valid */
67
68 #define CMA_ALLOC_SIZE 64
69 /* metadata size */
70 #if defined( ISP_HAS_METADATA_FSM )
71 #define ISP_V4L2_METADATA_SIZE sizeof( firmware_metadata_t )
72 #else
73 #define ISP_V4L2_METADATA_SIZE 4096
74 #endif
75
76 /* max size */
77 #define ISP_V4L2_MAX_WIDTH 4096
78 #define ISP_V4L2_MAX_HEIGHT 3456
79
80 /* default size & format */
81 #define ISP_DEFAULT_FORMAT V4L2_PIX_FMT_RGB24
82 //V4L2_PIX_FMT_RGB32
83
84 typedef struct _isp_v4l2_fmt {
85 const char *name;
86 uint32_t fourcc;
87 uint8_t depth;
88 bool is_yuv;
89 uint8_t planes;
90 } isp_v4l2_fmt_t;
91
92 static isp_v4l2_fmt_t isp_v4l2_supported_formats[] =
93 {
94 {
95 .name = "ARGB32",
96 .fourcc = V4L2_PIX_FMT_RGB32,
97 .depth = 32,
98 .is_yuv = false,
99 .planes = 1,
100 },
101 {
102 .name = "ARGB24",
103 .fourcc = V4L2_PIX_FMT_RGB24,
104 .depth = 24,
105 .is_yuv = false,
106 .planes = 2/*1*/,
107 },
108 {
109 .name = "NV12",
110 .fourcc = V4L2_PIX_FMT_NV12,
111 .depth = 8,
112 .is_yuv = true,
113 .planes = 2,
114 },
115 {
116 .name = "NV21",
117 .fourcc = V4L2_PIX_FMT_NV21,
118 .depth = 8,
119 .is_yuv = true,
120 .planes = 2,
121 },
122 {
123 .name = "AYUV",
124 .fourcc = V4L2_PIX_FMT_YUV444,
125 .depth = 32,
126 .is_yuv = true,
127 .planes = 1,
128 },
129 {
130 .name = "YUV420",
131 .fourcc = V4L2_PIX_FMT_YUV420,
132 .depth = 16,
133 .is_yuv = true,
134 .planes = 1,
135 },
136 {
137 .name = "YUY2",
138 .fourcc = V4L2_PIX_FMT_YUYV,
139 .depth = 16,
140 .is_yuv = true,
141 .planes = 1,
142 },
143 {
144 .name = "UYVY",
145 .fourcc = V4L2_PIX_FMT_UYVY,
146 .depth = 16,
147 .is_yuv = true,
148 .planes = 1,
149 },
150 {
151 .name = "GREY",
152 .fourcc = V4L2_PIX_FMT_GREY,
153 .depth = 8,
154 .is_yuv = true,
155 .planes = 1,
156 },
157 {
158 .name = "RAW 16",
159 .fourcc = V4L2_PIX_FMT_SBGGR16,
160 .depth = 16,
161 .is_yuv = false,
162 .planes = 1,
163 },
164 {
165 .name = "RAW 10",
166 .fourcc = V4L2_PIX_FMT_SRGGB10,
167 .depth = 16,
168 .is_yuv = false,
169 .planes = 1,
170 },
171
172 };
173
174 static isp_v4l2_fmt_t ext_supported_formats[] =
175 {
176 {
177 .name = "ARGB30",
178 .fourcc = ISP_V4L2_PIX_FMT_ARGB2101010,
179 .depth = 32,
180 .is_yuv = false,
181 .planes = 1,
182 },
183 #if ISP_HAS_META_CB
184 {
185 .name = "META",
186 .fourcc = ISP_V4L2_PIX_FMT_META,
187 .depth = 8,
188 .is_yuv = false,
189 .planes = 1,
190 },
191 #endif
192 };
193
194 extern uint8_t *isp_kaddr;
195 extern resource_size_t isp_paddr;
196
197 extern void cache_flush(uint32_t buf_start, uint32_t buf_size);
198
199 #if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 0, 0))
200 extern struct timespec64 normal_fftt;
201 #else
202 extern struct timespec64 normal_fftt;
203 #endif
204
205 uint8_t g_fftt = 1;
206
207 /* ----------------------------------------------------------------
208 * temporal frame sync before DDR access is available
209 */
210 #if V4L2_FRAME_ID_SYNC
211 #define SYNC_FLAG_META ( 1 << 0 )
212 #define SYNC_FLAG_RAW ( 1 << 1 )
213 #define SYNC_FLAG_FR ( 1 << 2 )
214 #define SYNC_FLAG_DS1 ( 1 << 3 )
215 #define SYNC_FLAG_DS2 ( 1 << 4 )
216 static spinlock_t sync_slock;
217 static uint32_t sync_started = 0;
218 static uint32_t sync_flag = 0;
219 static uint32_t sync_frame_id = 0;
220 static uint32_t sync_highest_id = 0;
221 static uint32_t sync_prev_ctx_id[V4L2_STREAM_TYPE_MAX] = {
222 0,
223 };
224 static uint32_t sync_done_meta_cnt = 0;
225
sync_frame(int stream_type,uint32_t ctx_num,uint32_t fid,uint32_t flag)226 int sync_frame( int stream_type, uint32_t ctx_num, uint32_t fid, uint32_t flag )
227 {
228 unsigned long sflags;
229 int rc = -1;
230
231 spin_lock_irqsave( &sync_slock, sflags );
232 if ( sync_prev_ctx_id[stream_type] != ctx_num ) {
233 sync_highest_id = 0;
234 sync_prev_ctx_id[stream_type] = ctx_num;
235 }
236
237 if ( sync_flag > 0 ) {
238 LOG( LOG_DEBUG, "[Stream#%d] Sync_flag on - sync_highest_id (%d), New fid (%d) / sync_frame_id (%d). sync flag: %x, flag: %x",
239 stream_type, sync_highest_id, fid, sync_frame_id, sync_flag, flag);
240
241 if ( fid > sync_highest_id )
242 sync_highest_id = fid;
243
244 if ( fid != sync_frame_id || sync_flag & flag ) {
245 if (fid != sync_frame_id)
246 rc = -2;
247 else
248 rc = -3;
249
250 #if ISP_HAS_META_CB
251 if ( stream_type == V4L2_STREAM_TYPE_META && fid > sync_frame_id ) {
252 sync_flag &= ~flag;
253 /* FIXME: check if need reset the sync_done_meta_cnt*/
254 sync_done_meta_cnt = 0;
255 if (!sync_flag) {
256 sync_flag |= flag;
257 sync_highest_id = fid;
258 sync_frame_id = fid;
259 rc = 0;
260 }
261 }
262 #endif
263 spin_unlock_irqrestore( &sync_slock, sflags );
264 return rc;
265 }
266 sync_flag |= flag;
267 #if ISP_HAS_META_CB
268 if (sync_done_meta_cnt > 0) {
269 sync_flag &= ~SYNC_FLAG_META;
270 sync_done_meta_cnt--;
271 }
272 if (sync_flag & SYNC_FLAG_META) {
273 LOG( LOG_ERR, "[Stream#%d] Sync meta not done - sync_highest_id (%d), New fid (%d) / sync_frame_id (%d). sync flag: %x, flag: %x",
274 stream_type, sync_highest_id, fid, sync_frame_id, sync_flag, flag);
275 }
276 #endif
277 } else {
278 if ( fid > sync_highest_id ) {
279 LOG( LOG_DEBUG, "[Stream#%d] New synced frame started with fid %d", stream_type, fid );
280 sync_highest_id = fid;
281 sync_frame_id = fid;
282 sync_flag |= flag;
283 } else {
284 LOG( LOG_DEBUG, "[Stream#%d] returning since new fid is not the highest, (id = %d, highest = %d) ",
285 stream_type, fid, sync_highest_id );
286 spin_unlock_irqrestore( &sync_slock, sflags );
287 return rc;
288 }
289 }
290 spin_unlock_irqrestore( &sync_slock, sflags );
291
292 return 0;
293 }
294 #endif
295
296
297 /* ----------------------------------------------------------------
298 * Stream callback interface
299 */
300 #if ISP_HAS_META_CB
callback_meta(uint32_t ctx_num,const void * fw_metadata)301 void callback_meta( uint32_t ctx_num, const void *fw_metadata )
302 {
303 isp_v4l2_stream_t *pstream = NULL;
304 isp_v4l2_buffer_t *pbuf = NULL;
305 void *vb2_buf = NULL;
306 uint32_t frame_id = 0;
307 int rc;
308 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
309 struct vb2_v4l2_buffer *vvb;
310 #endif
311 struct vb2_buffer *vb;
312 unsigned int buf_index;
313 unsigned int buf_type;
314
315 //add atomic started here to prevent raise condition with closing
316 if ( fw_metadata == NULL ) {
317 LOG( LOG_DEBUG, "metadata is null (ctx %d)", ctx_num );
318 return;
319 }
320
321 /* find stream pointer */
322 rc = isp_v4l2_find_stream( &pstream, ctx_num, V4L2_STREAM_TYPE_META );
323 if ( rc < 0 ) {
324 LOG( LOG_DEBUG, "can't find stream on ctx %d (errno = %d)", ctx_num, rc );
325 return;
326 }
327
328 /* check if stream is on */
329 if ( !pstream->stream_started ) {
330 LOG( LOG_DEBUG, "[Stream#%d] stream META is not started yet on ctx %d", pstream->stream_id, ctx_num );
331 return;
332 }
333
334 /* filter redundant frame id */
335 frame_id = *(uint32_t *)fw_metadata;
336 #if CHECK_METADATA_ID
337 if ( pstream->last_frame_id == frame_id ) {
338 LOG( LOG_ERR, "[Stream#%d] Redundant frame ID %d on ctx#%d", pstream->stream_id, frame_id, ctx_num );
339 return;
340 }
341 #endif
342 pstream->last_frame_id = frame_id;
343
344 #if V4L2_FRAME_ID_SYNC
345 rc = sync_frame( pstream->stream_type, ctx_num, frame_id, SYNC_FLAG_META );
346 if ( rc < 0 ) {
347 LOG( LOG_DEBUG, "sync_frame on ctx %d (errno = %d)", ctx_num, rc );
348 return;
349 }
350 #endif
351
352 /* get buffer from vb2 queue */
353 spin_lock( &pstream->slock );
354 if ( !list_empty( &pstream->stream_buffer_list ) ) {
355 pbuf = list_entry( pstream->stream_buffer_list.next, isp_v4l2_buffer_t, list );
356 list_del( &pbuf->list );
357 }
358 spin_unlock( &pstream->slock );
359 if ( !pbuf ) {
360 #if V4L2_FRAME_ID_SYNC
361 LOG( LOG_ERR, "[Stream#%d] No active buffer in queue !", pstream->stream_id );
362 #endif
363 return;
364 }
365 if ( atomic_inc_and_test( &pstream->running ) ) {
366 LOG( LOG_ERR, "[Stream#%d] Already deinited stream !", pstream->stream_id );
367 return;
368 }
369
370 /* fill buffer */
371 pstream->fw_frame_seq_count++;
372
373 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
374 vvb = &pbuf->vvb;
375 vb = &vvb->vb2_buf;
376
377 buf_index = vb->index;
378 buf_type = vb->type;
379
380 vvb->field = V4L2_FIELD_NONE;
381 /* update frame id */
382 vvb->sequence = *(uint32_t *)fw_metadata;
383 #else
384 vb = &pbuf->vb;
385
386 buf_index = vb->v4l2_buf.index;
387 buf_type = vb->v4l2_buf.type;
388
389 vb->v4l2_buf.field = V4L2_FIELD_NONE;
390 /* update frame id */
391 vb->v4l2_buf.sequence = *(uint32_t *)fw_metadata;
392 #endif
393
394 vb2_buf = vb2_plane_vaddr( vb, 0 );
395 //
396 //mutiplanar?
397 if ( buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE )
398 memcpy( vb2_buf, fw_metadata, pstream->cur_v4l2_fmt.fmt.pix_mp.plane_fmt[0].sizeimage );
399 else
400 memcpy( vb2_buf, fw_metadata, pstream->cur_v4l2_fmt.fmt.pix.sizeimage );
401
402
403 /* Put buffer back to vb2 queue */
404 vb2_buffer_done( vb, VB2_BUF_STATE_DONE );
405
406 #if V4L2_FRAME_ID_SYNC
407 {
408 unsigned long sflags;
409
410 spin_lock_irqsave( &sync_slock, sflags );
411 sync_done_meta_cnt++;
412 spin_unlock_irqrestore( &sync_slock, sflags );
413 }
414 #endif
415
416 /* Notify buffer ready */
417 isp_v4l2_notify_event( pstream->ctx_id, pstream->stream_id, V4L2_EVENT_ACAMERA_FRAME_READY );
418
419 LOG( LOG_DEBUG, "[Stream#%d] vid_cap buffer %d done (size=%d, m.w=%d, m.h=%d)",
420 pstream->stream_id, buf_index,
421 pstream->cur_v4l2_fmt.fmt.pix.sizeimage,
422 ( (firmware_metadata_t *)vb2_buf )->sensor_width, ( (firmware_metadata_t *)vb2_buf )->sensor_height );
423
424 atomic_set( &pstream->running, 0 );
425 }
426 #endif
427
428 #if ISP_HAS_RAW_CB
callback_raw(uint32_t ctx_num,aframe_t * aframe,const metadata_t * metadata,uint8_t exposures_num)429 void callback_raw( uint32_t ctx_num, aframe_t *aframe, const metadata_t *metadata, uint8_t exposures_num )
430 {
431 isp_v4l2_stream_t *pstream = NULL;
432 struct isp_fw_frame_mgr *frame_mgr;
433 unsigned long flags;
434 int i, rc;
435
436 LOG( LOG_DEBUG, "[Stream#2] v4l2 callback_raw called" );
437
438 if ( !metadata ) {
439 LOG( LOG_ERR, "callback_raw: metadata is NULL" );
440 return;
441 }
442
443 /* find stream pointer */
444 rc = isp_v4l2_find_stream( &pstream, ctx_num, V4L2_STREAM_TYPE_RAW );
445 if ( rc < 0 ) {
446 LOG( LOG_DEBUG, "can't find stream on ctx %d (errno = %d)", ctx_num, rc );
447 return;
448 }
449
450 /* check if stream is on */
451 if ( !pstream->stream_started ) {
452 LOG( LOG_CRIT, "[Stream#%d] stream RAW is not started yet on ctx %d", pstream->stream_id, ctx_num );
453 return;
454 }
455
456 #if CHECK_METADATA_ID
457 /* filter redundant frame id */
458 if ( pstream->last_frame_id == metadata->frame_id ) {
459 LOG( LOG_ERR, "[Stream#%d] Redundant frame ID %d on ctx#%d", pstream->stream_id, metadata->frame_id, ctx_num );
460 return;
461 }
462 pstream->last_frame_id = metadata->frame_id;
463 #endif
464
465 #if V4L2_FRAME_ID_SYNC
466 if ( sync_frame( pstream->stream_type, ctx_num, metadata->frame_id, SYNC_FLAG_RAW ) < 0 ) {
467 LOG( LOG_CRIT, "[Stream#2] stream RAW sync_frame error" );
468 return;
469 }
470 #endif
471
472 frame_mgr = &pstream->frame_mgr;
473 int wake_up = 0;
474
475 spin_lock_irqsave( &frame_mgr->frame_slock, flags );
476 if ( ISP_FW_FRAME_BUF_INVALID == frame_mgr->frame_buffer.state ) {
477 /* lock buffer from firmware */
478
479 /* save current frame */
480
481 for ( i = 0; i < exposures_num; i++ ) {
482 frame_mgr->frame_buffer.addr[i] = aframe[i].address;
483 LOG( LOG_DEBUG, "[Stream#2] v4l2 addresses:0x%x", aframe[i].address );
484 aframe[i].status = dma_buf_purge;
485 }
486 if ( pstream->stream_common->sensor_info.preset[pstream->stream_common->sensor_info.preset_cur].exposures[pstream->stream_common->sensor_info.preset[pstream->stream_common->sensor_info.preset_cur].fps_cur] > exposures_num ) {
487 LOG( LOG_CRIT, "V4L2 Raw exposures expecting %d got %d.", pstream->stream_common->sensor_info.preset[pstream->stream_common->sensor_info.preset_cur].exposures[pstream->stream_common->sensor_info.preset[pstream->stream_common->sensor_info.preset_cur].fps_cur], exposures_num );
488 }
489 frame_mgr->frame_buffer.meta = *metadata;
490 frame_mgr->frame_buffer.state = ISP_FW_FRAME_BUF_VALID;
491
492 frame_mgr->frame_buffer.tframe.primary = *aframe;
493
494 /* wake up thread */
495 wake_up = 1;
496 }
497 spin_unlock_irqrestore( &frame_mgr->frame_slock, flags );
498
499 /* wake up the kernel thread to copy the frame data */
500 if ( wake_up )
501 wake_up_interruptible( &frame_mgr->frame_wq );
502
503 if ( metadata )
504 LOG( LOG_DEBUG, "metadata: width: %u, height: %u, line_size: %u, frame_number: %u.",
505 metadata->width, metadata->height, metadata->line_size, metadata->frame_number );
506 }
507 #endif
508
callback_fr(uint32_t ctx_num,tframe_t * tframe,const metadata_t * metadata)509 void callback_fr( uint32_t ctx_num, tframe_t *tframe, const metadata_t *metadata )
510 {
511 isp_v4l2_stream_t *pstream = NULL;
512 struct isp_fw_frame_mgr *frame_mgr;
513 unsigned long flags;
514 int rc;
515
516 //if ( !metadata ) {
517 // LOG( LOG_ERR, "callback_fr: metadata is NULL" );
518 // return;
519 //}
520
521 /* find stream pointer */
522 rc = isp_v4l2_find_stream( &pstream, ctx_num, V4L2_STREAM_TYPE_FR );
523 if ( rc < 0 ) {
524 LOG( LOG_DEBUG, "can't find stream on ctx %d (errno = %d)", ctx_num, rc );
525 return;
526 }
527
528 /* check if stream is on */
529 if ( !pstream->stream_started ) {
530 LOG( LOG_DEBUG, "[Stream#%d] stream FR is not started yet on ctx %d", pstream->stream_id, ctx_num );
531 return;
532 }
533
534 #if CHECK_METADATA_ID
535 /* filter redundant frame id */
536 if ( pstream->last_frame_id == metadata->frame_id ) {
537 LOG( LOG_ERR, "[Stream#%d] Redundant frame ID %d on ctx#%d", pstream->stream_id, metadata->frame_id, ctx_num );
538 return;
539 }
540 pstream->last_frame_id = metadata->frame_id;
541 #endif
542
543 #if V4L2_FRAME_ID_SYNC
544 rc = sync_frame( pstream->stream_type, ctx_num, metadata->frame_id, SYNC_FLAG_FR );
545 if ( rc < 0 ) {
546 LOG( LOG_DEBUG, "sync_frame on ctx %d (errno = %d)", ctx_num, rc );
547 return;
548 }
549 #endif
550
551 frame_mgr = &pstream->frame_mgr;
552 int wake_up = 0;
553
554 spin_lock_irqsave( &frame_mgr->frame_slock, flags );
555 if ( ISP_FW_FRAME_BUF_INVALID == frame_mgr->frame_buffer.state ) {
556 /* save current frame */
557 //only 2 planes are possible
558 frame_mgr->frame_buffer.addr[0] = tframe->primary.address;
559 frame_mgr->frame_buffer.addr[1] = tframe->secondary.address;
560 if(metadata)
561 frame_mgr->frame_buffer.meta = *metadata;
562 frame_mgr->frame_buffer.state = ISP_FW_FRAME_BUF_VALID;
563 frame_mgr->frame_buffer.tframe = *tframe;
564 /* lock buffer from firmware */
565 tframe->primary.status = dma_buf_purge;
566 tframe->secondary.status = dma_buf_purge;
567
568 /* wake up thread */
569 wake_up = 1;
570 }
571 spin_unlock_irqrestore( &frame_mgr->frame_slock, flags );
572
573 /* wake up the kernel thread to copy the frame data */
574 if ( wake_up )
575 wake_up_interruptible( &frame_mgr->frame_wq );
576
577 if ( metadata )
578 LOG( LOG_DEBUG, "metadata: width: %u, height: %u, line_size: %u, frame_number: %u.",
579 metadata->width, metadata->height, metadata->line_size, metadata->frame_number );
580
581 #if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 0, 0))
582 if( g_fftt ) {
583 do_gettimeofday(&normal_fftt);
584 g_fftt = 0;
585 }
586 #else
587 if( g_fftt ) {
588 ktime_get_real_ts64(&normal_fftt);
589 g_fftt = 0;
590 }
591 #endif
592 }
593
594 // Callback from DS1 output pipe
callback_ds1(uint32_t ctx_num,tframe_t * tframe,const metadata_t * metadata)595 void callback_ds1( uint32_t ctx_num, tframe_t *tframe, const metadata_t *metadata )
596 {
597 #if ISP_HAS_DS1
598 isp_v4l2_stream_t *pstream = NULL;
599 struct isp_fw_frame_mgr *frame_mgr;
600 unsigned long flags;
601 int rc;
602
603 if ( !metadata ) {
604 LOG( LOG_ERR, "callback_ds1: metadata is NULL" );
605 return;
606 }
607
608 /* find stream pointer */
609 rc = isp_v4l2_find_stream( &pstream, ctx_num, V4L2_STREAM_TYPE_DS1 );
610 if ( rc < 0 ) {
611 LOG( LOG_DEBUG, "can't find stream on ctx %d (errno = %d)", ctx_num, rc );
612 return;
613 }
614
615 /* check if stream is on */
616 if ( !pstream->stream_started ) {
617 LOG( LOG_DEBUG, "[Stream#%d] stream DS1 is not started yet on ctx %d", pstream->stream_id, ctx_num );
618 return;
619 }
620
621 #if CHECK_METADATA_ID
622 /* filter redundant frame id */
623 if ( pstream->last_frame_id == metadata->frame_id ) {
624 LOG( LOG_ERR, "[Stream#%d] Redundant frame ID %d on ctx#%d", pstream->stream_id, metadata->frame_id, ctx_num );
625 return;
626 }
627 pstream->last_frame_id = metadata->frame_id;
628 #endif
629
630 #if V4L2_FRAME_ID_SYNC
631 rc = sync_frame( pstream->stream_type, ctx_num, metadata->frame_id, SYNC_FLAG_DS1 );
632 if ( rc < 0 ) {
633 LOG( LOG_DEBUG, "sync_frame on ctx %d (errno = %d)", ctx_num, rc );
634 return;
635 }
636 #endif
637
638 frame_mgr = &pstream->frame_mgr;
639 int wake_up = 0;
640
641 spin_lock_irqsave( &frame_mgr->frame_slock, flags );
642 if ( ISP_FW_FRAME_BUF_INVALID == frame_mgr->frame_buffer.state ) {
643 /* save current frame */
644 //only 2 planes are possible
645 frame_mgr->frame_buffer.addr[0] = tframe->primary.address;
646 frame_mgr->frame_buffer.addr[1] = tframe->secondary.address;
647 if(metadata)
648 frame_mgr->frame_buffer.meta = *metadata;
649 frame_mgr->frame_buffer.state = ISP_FW_FRAME_BUF_VALID;
650 frame_mgr->frame_buffer.tframe = *tframe;
651
652 /* lock buffer from firmware */
653 tframe->primary.status = dma_buf_purge;
654 tframe->secondary.status = dma_buf_purge;
655
656 /* wake up thread */
657 wake_up = 1;
658 }
659 spin_unlock_irqrestore( &frame_mgr->frame_slock, flags );
660
661 /* wake up the kernel thread to copy the frame data */
662 if ( wake_up )
663 wake_up_interruptible( &frame_mgr->frame_wq );
664
665 if ( metadata )
666 LOG( LOG_DEBUG, "metadata: width: %u, height: %u, line_size: %u, frame_number: %u.",
667 metadata->width, metadata->height, metadata->line_size, metadata->frame_number );
668 #endif
669 }
670
671 // Callback from DS2 output pipe
callback_ds2(uint32_t ctx_num,tframe_t * tframe,const metadata_t * metadata)672 void callback_ds2( uint32_t ctx_num, tframe_t *tframe, const metadata_t *metadata )
673 {
674 #if ISP_HAS_DS2
675 isp_v4l2_stream_t *pstream = NULL;
676 struct isp_fw_frame_mgr *frame_mgr;
677 unsigned long flags;
678 int rc;
679
680 if ( !metadata ) {
681 LOG( LOG_ERR, "callback_ds2: metadata is NULL" );
682 return;
683 }
684
685 /* find stream pointer */
686 rc = isp_v4l2_find_stream( &pstream, ctx_num, V4L2_STREAM_TYPE_DS2 );
687 if ( rc < 0 ) {
688 LOG( LOG_DEBUG, "can't find stream on ctx %d (errno = %d)", ctx_num, rc );
689 return;
690 }
691
692 /* check if stream is on */
693 if ( !pstream->stream_started ) {
694 LOG( LOG_DEBUG, "[Stream#%d] stream DS2 is not started yet on ctx %d", pstream->stream_id, ctx_num );
695 return;
696 }
697
698 #if CHECK_METADATA_ID
699 /* filter redundant frame id */
700 if ( pstream->last_frame_id == metadata->frame_id ) {
701 LOG( LOG_ERR, "[Stream#%d] Redundant frame ID %d on ctx#%d", pstream->stream_id, metadata->frame_id, ctx_num );
702 //return;
703 }
704 pstream->last_frame_id = metadata->frame_id;
705 #endif
706
707 #if 0
708 #if V4L2_FRAME_ID_SYNC
709 if ( sync_frame( pstream->stream_type, ctx_num, metadata->frame_id, SYNC_FLAG_DS2 ) < 0 ) {
710 LOG(LOG_ERR, "callback_ds2 sync frame failed");
711 return;
712 }
713 #endif
714 #endif
715 frame_mgr = &pstream->frame_mgr;
716 int wake_up = 0;
717
718 spin_lock_irqsave( &frame_mgr->frame_slock, flags );
719 if ( ISP_FW_FRAME_BUF_INVALID == frame_mgr->frame_buffer.state ) {
720 /* save current frame */
721 //only 2 planes are possible
722 frame_mgr->frame_buffer.addr[0] = tframe->primary.address;
723 frame_mgr->frame_buffer.addr[1] = tframe->secondary.address;
724 if(metadata)
725 frame_mgr->frame_buffer.meta = *metadata;
726 frame_mgr->frame_buffer.state = ISP_FW_FRAME_BUF_VALID;
727 frame_mgr->frame_buffer.tframe = *tframe;
728
729 /* lock buffer from firmware */
730 tframe->primary.status = dma_buf_purge;
731 tframe->secondary.status = dma_buf_purge;
732
733 /* wake up thread */
734 wake_up = 1;
735 }
736 spin_unlock_irqrestore( &frame_mgr->frame_slock, flags );
737
738 /* wake up the kernel thread to copy the frame data */
739 if ( wake_up )
740 wake_up_interruptible( &frame_mgr->frame_wq );
741
742 if ( metadata )
743 LOG( LOG_DEBUG, "metadata: width: %u, height: %u, line_size: %u, frame_number: %u.",
744 metadata->width, metadata->height, metadata->line_size, metadata->frame_number );
745 #endif
746
747 }
748
749
750 /* ----------------------------------------------------------------
751 * Stream control interface
752 */
753 /* DDR ioremap parameters
754 */
755 /* sensor static informations */
756 static isp_v4l2_stream_common g_stream_common[FIRMWARE_CONTEXT_NUMBER];
757
isp_v4l2_stream_init_static_resources(struct platform_device * pdev,uint32_t ctx_id)758 int isp_v4l2_stream_init_static_resources(struct platform_device *pdev, uint32_t ctx_id)
759 {
760 isp_v4l2_stream_common *sc = &( g_stream_common[ctx_id] );
761 int i;
762 /* initialize stream common field */
763 memset( sc, 0, sizeof( isp_v4l2_stream_common ) );
764 fw_intf_isp_get_sensor_info( ctx_id, &sc->sensor_info );
765 sc->snapshot_sizes.frmsize_num = sc->sensor_info.preset_num;
766 for ( i = 0; i < sc->sensor_info.preset_num; i++ ) {
767 sc->snapshot_sizes.frmsize[i].width = sc->sensor_info.preset[i].width;
768 sc->snapshot_sizes.frmsize[i].height = sc->sensor_info.preset[i].height;
769 }
770
771 return 0;
772 }
773
isp_v4l2_stream_deinit_static_resources(struct platform_device * pdev)774 void isp_v4l2_stream_deinit_static_resources( struct platform_device *pdev )
775 {
776 return;
777 }
778
isp_v4l2_stream_init(isp_v4l2_stream_t ** ppstream,int stream_id,int ctx_id)779 int isp_v4l2_stream_init( isp_v4l2_stream_t **ppstream, int stream_id, int ctx_id )
780 {
781 isp_v4l2_stream_t *new_stream = NULL;
782 //int current_sensor_preset;
783 LOG( LOG_DEBUG, "[Stream#%d] Initializing stream ...", stream_id );
784
785 /* allocate isp_v4l2_stream_t */
786 new_stream = kzalloc( sizeof( isp_v4l2_stream_t ), GFP_KERNEL );
787 if ( new_stream == NULL ) {
788 LOG( LOG_ERR, "[Stream#%d] Failed to allocate isp_v4l2_stream_t.", stream_id );
789 return -ENOMEM;
790 }
791
792 /* set default format */
793
794 //all stream multiplanar
795 new_stream->cur_v4l2_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
796
797 /* set input stream info */
798 new_stream->stream_common = &( g_stream_common[ctx_id] );
799
800 /* init control fields */
801 new_stream->ctx_id = ctx_id;
802 new_stream->stream_id = stream_id;
803 new_stream->stream_type = V4L2_STREAM_TYPE_MAX;
804 new_stream->stream_started = 0;
805 new_stream->last_frame_id = 0xFFFFFFFF;
806
807 //format new stream to default isp settings
808 isp_v4l2_stream_try_format( new_stream, &( new_stream->cur_v4l2_fmt ) );
809
810 /* init list */
811 INIT_LIST_HEAD( &new_stream->stream_buffer_list );
812 INIT_LIST_HEAD( &new_stream->stream_buffer_list_busy );
813
814 /* init locks */
815 spin_lock_init( &new_stream->slock );
816 spin_lock_init( &new_stream->frame_mgr.frame_slock );
817
818 #if V4L2_FRAME_ID_SYNC
819 if ( !sync_started ) {
820 spin_lock_init( &sync_slock );
821 sync_started = 1;
822 }
823 #endif
824
825 /* initialize waitqueue for frame manager */
826 init_waitqueue_head( &new_stream->frame_mgr.frame_wq );
827
828 /* return stream private ptr to caller */
829 *ppstream = new_stream;
830
831 return 0;
832 }
833
isp_v4l2_stream_deinit(isp_v4l2_stream_t * pstream,int stream_on_count)834 void isp_v4l2_stream_deinit( isp_v4l2_stream_t *pstream, int stream_on_count )
835 {
836 if ( !pstream ) {
837 LOG( LOG_ERR, "Null stream passed" );
838 return;
839 }
840
841 LOG( LOG_INFO, "[Stream#%d] Deinitializing stream ...", pstream->stream_id );
842
843 /* do stream-off first if it's on */
844 isp_v4l2_stream_off( pstream , stream_on_count);
845
846 /* release fw_info */
847 if ( pstream ) {
848 #if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 0, 0))
849 kzfree( pstream );
850 #else
851 kfree_sensitive( pstream );
852 #endif
853 pstream = NULL;
854 }
855 }
856
isp_v4l2_stream_fill_buf(isp_v4l2_stream_t * pstream,isp_v4l2_buffer_t * buf,uint32_t * hw_buf_offset)857 void isp_v4l2_stream_fill_buf( isp_v4l2_stream_t *pstream, isp_v4l2_buffer_t *buf, uint32_t *hw_buf_offset )
858 {
859 unsigned int img_frame_size;
860 #if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 0, 0))
861 struct timeval begin, end;
862 #else
863 struct timespec64 begin, end;
864 #endif
865 unsigned long interval_val;
866 void *vbuf;
867 uint8_t *ddr_mem = NULL;
868 uint32_t map_size = 0;
869 resource_size_t paddr = 0;
870 int i;
871 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
872 struct vb2_v4l2_buffer *vvb;
873 #endif
874 struct vb2_buffer *vb;
875
876 if ( !pstream ) {
877 LOG( LOG_ERR, "ISP FW not inited yet" );
878 return;
879 }
880
881
882 LOG( LOG_DEBUG, "[Stream#%d] Enter: pstream->fw_frame_seq_count: %u.", pstream->stream_id, pstream->fw_frame_seq_count );
883
884 pstream->fw_frame_seq_count++;
885
886 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
887 vvb = &buf->vvb;
888 vb = &vvb->vb2_buf;
889
890 vvb->field = V4L2_FIELD_NONE;
891 #else
892 vb = &buf->vb;
893
894 vb->v4l2_buf.field = V4L2_FIELD_NONE;
895 #endif
896
897 #if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 0, 0))
898 do_gettimeofday( &begin );
899 #else
900 ktime_get_real_ts64(&begin);
901 #endif
902
903 ddr_mem = (uint8_t *)isp_kaddr;
904 paddr = isp_paddr;
905 map_size = ISP_DDR_SIZE;
906 #if ISP_HAS_RAW_CB
907 #if JUNO_DIRECT_DDR_ACCESS
908 if ( pstream->stream_type != V4L2_STREAM_TYPE_RAW ) {
909 ddr_mem = juno_ddr_mem;
910 map_size = JUNO_DDR_SIZE;
911 }
912 #endif
913 #endif
914
915
916 #if ISP_HAS_RAW_CB
917 /* stop DMA_FE to prevent tearing */
918 if ( pstream->stream_type == V4L2_STREAM_TYPE_RAW ) {
919 fw_intf_stream_pause( pstream->stream_type, 1 );
920 }
921 #endif
922
923 if ( ddr_mem ) {
924
925 if ( pstream->cur_v4l2_fmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ) {
926 for ( i = 0; i < pstream->cur_v4l2_fmt.fmt.pix_mp.num_planes; i++ ) {
927 vbuf = vb2_plane_vaddr( vb, i );
928 img_frame_size = pstream->cur_v4l2_fmt.fmt.pix_mp.plane_fmt[i].sizeimage;
929 memcpy( vbuf, ddr_mem + hw_buf_offset[i] - paddr, img_frame_size );
930 }
931 } else if ( pstream->cur_v4l2_fmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE ) {
932 vbuf = vb2_plane_vaddr( vb, 0 );
933 img_frame_size = pstream->cur_v4l2_fmt.fmt.pix.sizeimage;
934 memcpy( vbuf, ddr_mem + hw_buf_offset[0] - paddr, img_frame_size );
935 } else {
936 LOG( LOG_ERR, "v4l2 bufer format not supported" );
937 }
938 } else {
939 LOG( LOG_ERR, "[Stream#%d] Error: ddr_mem is NULL.", pstream->stream_id );
940 }
941
942 #if ISP_HAS_RAW_CB
943 /* stop DMA_FE to prevent tearing */
944 if ( pstream->stream_type == V4L2_STREAM_TYPE_RAW ) {
945 fw_intf_stream_pause( pstream->stream_type, 0 );
946 }
947 #endif
948
949 #if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 0, 0))
950 do_gettimeofday( &end );
951 #else
952 ktime_get_real_ts64(&end);
953 #endif
954
955 /* Get milliseconds interval */
956 interval_val = ( end.tv_sec - begin.tv_sec ) * 1000;
957 #if (LINUX_VERSION_CODE < KERNEL_VERSION(5, 0, 0))
958 interval_val += ( ( end.tv_usec - begin.tv_usec ) / 1000 );
959 #else
960 interval_val += ( ( end.tv_nsec - begin.tv_nsec ) / 1000000 );
961 #endif
962 LOG( LOG_DEBUG, "[Stream#%d] copy time is: %lu.", pstream->stream_id, interval_val );
963
964 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
965 vb->timestamp = ktime_get_ns();
966 #else
967 v4l2_get_timestamp( &vb->v4l2_buf.timestamp );
968 #endif
969 }
970
isp_v4l2_stream_copy_thread(void * data)971 static int isp_v4l2_stream_copy_thread( void *data )
972 {
973 isp_v4l2_stream_t *pstream = data;
974 isp_fw_frame_mgr_t *frame_mgr;
975 unsigned long flags;
976
977 metadata_t meta;
978 tframe_t tframe;
979 unsigned int idx_tmp = 0;
980 isp_v4l2_buffer_t *pbuf = NULL;
981 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
982 struct vb2_v4l2_buffer *vvb;
983 #endif
984 struct vb2_buffer *vb;
985 unsigned int buf_index;
986 void *s_list = NULL;
987 void *t_list = NULL;
988
989 if ( !pstream ) {
990 LOG( LOG_ERR, "Null stream passed" );
991 return -EINVAL;
992 }
993
994 frame_mgr = &pstream->frame_mgr;
995 set_freezable();
996
997 for ( ;; ) {
998 try_to_freeze();
999
1000 if ( kthread_should_stop() )
1001 break;
1002
1003 /* wait for new frame to come */
1004 if ( wait_event_interruptible_timeout( frame_mgr->frame_wq,
1005 (ISP_FW_FRAME_BUF_VALID == frame_mgr->frame_buffer.state),
1006 msecs_to_jiffies( 10 ) ) < 0 ) {
1007 LOG( LOG_ERR, "[Stream#%d] Error: wait_event return < 0", pstream->stream_id );
1008 continue;
1009 }
1010
1011 /* get a new frame from ISP FW */
1012 spin_lock_irqsave( &frame_mgr->frame_slock, flags );
1013 /* get current frame only its state is VALID */
1014 if ( ISP_FW_FRAME_BUF_VALID == frame_mgr->frame_buffer.state ) {
1015 meta = frame_mgr->frame_buffer.meta;
1016 tframe = frame_mgr->frame_buffer.tframe;
1017 frame_mgr->frame_buffer.state = ISP_FW_FRAME_BUF_INVALID;
1018 } else {
1019 spin_unlock_irqrestore( &frame_mgr->frame_slock, flags );
1020 continue;
1021 }
1022 spin_unlock_irqrestore( &frame_mgr->frame_slock, flags );
1023
1024 /* try to get an active buffer from vb2 queue */
1025 pbuf = NULL;
1026 spin_lock( &pstream->slock );
1027 t_list = tframe.list;
1028
1029 s_list = NULL;
1030
1031 list_for_each_entry(pbuf, &pstream->stream_buffer_list, list) {
1032 s_list = (void *)&pbuf->list;
1033
1034 if (s_list == t_list) {
1035 list_del_init(s_list);
1036 break;
1037 }
1038 }
1039
1040 if ((s_list != t_list) || (t_list == NULL) || (s_list == NULL)) {
1041 LOG(LOG_ERR, "[Stream#%d] Failed to find vb2 buffer on stream buffer list, s_list:%p, t_list:%p", pstream->stream_id, s_list, t_list);
1042 spin_unlock( &pstream->slock );
1043 #if V4L2_FRAME_ID_SYNC
1044 {
1045 unsigned long sflags;
1046
1047 spin_lock_irqsave( &sync_slock, sflags );
1048 #if ISP_HAS_RAW_CB
1049 if ( pstream->stream_type == V4L2_STREAM_TYPE_RAW ) {
1050 LOG( LOG_DEBUG, "[Stream#%d] releasing RAW sync flag", pstream->stream_id );
1051 sync_flag &= ~SYNC_FLAG_RAW;
1052 } else
1053 #endif
1054 #if ISP_HAS_DS1
1055 if ( pstream->stream_type == V4L2_STREAM_TYPE_DS1 ) {
1056 LOG( LOG_DEBUG, "[Stream#%d] releasing DS1 sync flag", pstream->stream_id );
1057 sync_flag &= ~SYNC_FLAG_DS1;
1058 } else
1059 #endif
1060 #if ISP_HAS_DS2
1061 if ( pstream->stream_type == V4L2_STREAM_TYPE_DS2 ) {
1062 LOG( LOG_DEBUG, "[Stream#%d] releasing DS2 sync flag", pstream->stream_id );
1063 sync_flag &= ~SYNC_FLAG_DS2;
1064 } else
1065 #endif
1066 #if ISP_HAS_META_CB
1067 if (pstream->stream_type == V4L2_STREAM_TYPE_META) {
1068 LOG( LOG_DEBUG, "[Stream#%d] releasing Meta sync flag", pstream->stream_id );
1069 sync_flag &= ~SYNC_FLAG_META;
1070 sync_done_meta_cnt = 0;
1071 } else
1072 #endif
1073 if ( pstream->stream_type == V4L2_STREAM_TYPE_FR ) {
1074 LOG( LOG_DEBUG, "[Stream#%d] releasing FR sync flag", pstream->stream_id );
1075 sync_flag &= ~SYNC_FLAG_FR;
1076 }
1077 spin_unlock_irqrestore( &sync_slock, sflags );
1078 }
1079 #endif
1080 continue;
1081 }
1082 spin_unlock( &pstream->slock );
1083
1084 vvb = &pbuf->vvb;
1085 vb = &vvb->vb2_buf;
1086
1087 buf_index = vb->index;
1088
1089 vvb->sequence = meta.frame_id;
1090 vvb->field = V4L2_FIELD_NONE;
1091 vb->timestamp = ktime_get_ns();
1092 pstream->fw_frame_seq_count++;
1093
1094 cache_flush(tframe.primary.address, tframe.primary.size + tframe.secondary.size);
1095 /* Fill buffer */
1096 LOG( LOG_DEBUG, "[Stream#%d] filled buffer %d with frame_buf_idx: %d.",
1097 pstream->stream_id, buf_index, idx_tmp );
1098
1099 /* Put buffer back to vb2 queue */
1100 vb2_buffer_done( vb, VB2_BUF_STATE_DONE );
1101
1102 /* Notify buffer ready */
1103 isp_v4l2_notify_event( pstream->ctx_id, pstream->stream_id, V4L2_EVENT_ACAMERA_FRAME_READY );
1104
1105 #if V4L2_FRAME_ID_SYNC
1106 {
1107 unsigned long sflags;
1108
1109 spin_lock_irqsave( &sync_slock, sflags );
1110 #if ISP_HAS_RAW_CB
1111 if ( pstream->stream_type == V4L2_STREAM_TYPE_RAW ) {
1112 LOG( LOG_DEBUG, "[Stream#%d] releasing RAW sync flag", pstream->stream_id );
1113 sync_flag &= ~SYNC_FLAG_RAW;
1114 } else
1115 #endif
1116 #if ISP_HAS_DS1
1117 if ( pstream->stream_type == V4L2_STREAM_TYPE_DS1 ) {
1118 LOG( LOG_DEBUG, "[Stream#%d] releasing DS1 sync flag", pstream->stream_id );
1119 sync_flag &= ~SYNC_FLAG_DS1;
1120 } else
1121 #endif
1122 #if ISP_HAS_DS2
1123 if ( pstream->stream_type == V4L2_STREAM_TYPE_DS2 ) {
1124 LOG( LOG_DEBUG, "[Stream#%d] releasing DS2 sync flag", pstream->stream_id );
1125 sync_flag &= ~SYNC_FLAG_DS2;
1126 } else
1127 #endif
1128 #if ISP_HAS_META_CB
1129 if (pstream->stream_type == V4L2_STREAM_TYPE_META) {
1130 LOG( LOG_DEBUG, "[Stream#%d] releasing Meta sync flag", pstream->stream_id );
1131 sync_flag &= ~SYNC_FLAG_META;
1132 sync_done_meta_cnt = 0;
1133 } else
1134 #endif
1135 if ( pstream->stream_type == V4L2_STREAM_TYPE_FR ) {
1136 LOG( LOG_DEBUG, "[Stream#%d] releasing FR sync flag", pstream->stream_id );
1137 sync_flag &= ~SYNC_FLAG_FR;
1138 }
1139 spin_unlock_irqrestore( &sync_slock, sflags );
1140 }
1141 #endif
1142
1143 }
1144
1145 /* Notify stream off */
1146 isp_v4l2_notify_event( pstream->ctx_id, pstream->stream_id, V4L2_EVENT_ACAMERA_STREAM_OFF );
1147
1148 LOG( LOG_CRIT, "[Stream#%d] Thread Exit", pstream->stream_id );
1149
1150 return 0;
1151 }
1152
isp_v4l2_stream_on(isp_v4l2_stream_t * pstream)1153 int isp_v4l2_stream_on( isp_v4l2_stream_t *pstream )
1154 {
1155 if ( !pstream ) {
1156 LOG( LOG_ERR, "Null stream passed" );
1157 return -EINVAL;
1158 }
1159
1160 LOG( LOG_DEBUG, "[Stream#%d] called", pstream->stream_id );
1161
1162 #if V4L2_FRAME_ID_SYNC
1163 {
1164 unsigned long sflags;
1165
1166 spin_lock_irqsave( &sync_slock, sflags );
1167 #if ISP_HAS_RAW_CB
1168 if ( pstream->stream_type == V4L2_STREAM_TYPE_RAW ) {
1169 LOG( LOG_DEBUG, "[Stream#%d] releasing RAW sync flag", pstream->stream_id );
1170 sync_flag &= ~SYNC_FLAG_RAW;
1171 } else
1172 #endif
1173 #if ISP_HAS_DS1
1174 if (pstream->stream_type == V4L2_STREAM_TYPE_DS1) {
1175 LOG( LOG_DEBUG, "[Stream#%d] releasing DS1 sync flag", pstream->stream_id );
1176 sync_flag &= ~SYNC_FLAG_DS1;
1177 } else
1178 #endif
1179 #if ISP_HAS_DS2
1180 if (pstream->stream_type == V4L2_STREAM_TYPE_DS2) {
1181 LOG( LOG_DEBUG, "[Stream#%d] releasing DS2 sync flag", pstream->stream_id );
1182 sync_flag &= ~SYNC_FLAG_DS2;
1183 } else
1184 #endif
1185 #if ISP_HAS_META_CB
1186 if (pstream->stream_type == V4L2_STREAM_TYPE_META) {
1187 LOG( LOG_DEBUG, "[Stream#%d] releasing Meta sync flag", pstream->stream_id );
1188 sync_flag &= ~SYNC_FLAG_META;
1189 sync_done_meta_cnt = 0;
1190 } else
1191 #endif
1192 if (pstream->stream_type == V4L2_STREAM_TYPE_FR) {
1193 LOG( LOG_DEBUG, "[Stream#%d] releasing FR sync flag", pstream->stream_id );
1194 sync_flag &= ~SYNC_FLAG_FR;
1195 }
1196 spin_unlock_irqrestore( &sync_slock, sflags );
1197 }
1198 #endif
1199
1200 /* for now, we need memcpy */
1201 #if ISP_HAS_META_CB
1202 if ( pstream->stream_type != V4L2_STREAM_TYPE_META )
1203 #endif
1204 {
1205 /* Resets frame counters */
1206 pstream->fw_frame_seq_count = 0;
1207
1208 /* launch copy thread */
1209 pstream->kthread_stream = kthread_run( isp_v4l2_stream_copy_thread, pstream, "isp-stream-%d", pstream->stream_id );
1210 if ( IS_ERR( pstream->kthread_stream ) ) {
1211 LOG( LOG_ERR, "[Stream#%d] create kernel_thread() failed", pstream->stream_id );
1212 return PTR_ERR( pstream->kthread_stream );
1213 }
1214 }
1215 #if ISP_HAS_META_CB
1216 else { //metadata has no thread
1217 atomic_set( &pstream->running, 0 );
1218 }
1219 #endif
1220
1221 /* hardware stream on */
1222 if ( fw_intf_stream_start( pstream->ctx_id, pstream->stream_type ) < 0 ) {
1223 LOG( LOG_ERR, "[Stream#%d] fw_intf_stream_start failed", pstream->stream_id );
1224 return -1;
1225 }
1226
1227 /* control fields update */
1228 pstream->stream_started = 1;
1229
1230 return 0;
1231 }
1232
isp_v4l2_stream_off(isp_v4l2_stream_t * pstream,int stream_on_count)1233 void isp_v4l2_stream_off( isp_v4l2_stream_t *pstream, int stream_on_count )
1234 {
1235 isp_v4l2_buffer_t *buf;
1236 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
1237 struct vb2_v4l2_buffer *vvb;
1238 #endif
1239 struct vb2_buffer *vb;
1240 unsigned int buf_index;
1241
1242 if ( !pstream ) {
1243 LOG( LOG_ERR, "Null stream passed" );
1244 return;
1245 }
1246
1247 fw_intf_stream_stop( pstream->ctx_id, pstream->stream_type, stream_on_count );
1248
1249 /* shutdown control thread */
1250 if ( pstream->kthread_stream != NULL ) {
1251 kthread_stop( pstream->kthread_stream );
1252 pstream->kthread_stream = NULL;
1253 }
1254 #if ISP_HAS_META_CB
1255 else if ( pstream->stream_type == V4L2_STREAM_TYPE_META ) {
1256 while ( atomic_read( &pstream->running ) > 0 ) { //metadata has no thread
1257 schedule();
1258 }
1259 atomic_set( &pstream->running, -1 );
1260 }
1261 #endif
1262 /* Release all active buffers */
1263 spin_lock( &pstream->slock );
1264 while ( !list_empty( &pstream->stream_buffer_list ) ) {
1265 buf = list_entry( pstream->stream_buffer_list.next,
1266 isp_v4l2_buffer_t, list );
1267 list_del( &buf->list );
1268
1269 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0))
1270 vvb = &buf->vvb;
1271 vb = &vvb->vb2_buf;
1272
1273 buf_index = vb->index;
1274 #else
1275 vb = &buf->vb;
1276
1277 buf_index = vb->v4l2_buf.index;
1278 #endif
1279
1280 vb2_buffer_done( vb, VB2_BUF_STATE_ERROR );
1281 }
1282 spin_unlock( &pstream->slock );
1283
1284 // control fields update
1285
1286 pstream->stream_started = 0;
1287 }
1288
1289
1290 /* ----------------------------------------------------------------
1291 * Stream configuration interface
1292 */
isp_v4l2_stream_find_format(uint32_t pixelformat)1293 static isp_v4l2_fmt_t *isp_v4l2_stream_find_format( uint32_t pixelformat )
1294 {
1295 isp_v4l2_fmt_t *fmt;
1296 unsigned int i;
1297
1298 for ( i = 0; i < ARRAY_SIZE( isp_v4l2_supported_formats ); i++ ) {
1299 fmt = &isp_v4l2_supported_formats[i];
1300
1301 if ( fmt->fourcc == pixelformat )
1302 return fmt;
1303 }
1304
1305 return NULL;
1306 }
1307
isp_v4l2_stream_enum_framesizes(isp_v4l2_stream_t * pstream,struct v4l2_frmsizeenum * fsize)1308 int isp_v4l2_stream_enum_framesizes( isp_v4l2_stream_t *pstream, struct v4l2_frmsizeenum *fsize )
1309 {
1310 if ( !isp_v4l2_stream_find_format( fsize->pixel_format ) )
1311 return -EINVAL;
1312
1313 /* check index */
1314 if ( fsize->index >= pstream->stream_common->snapshot_sizes.frmsize_num ) {
1315 LOG( LOG_DEBUG, "[Stream#%d] index (%d) should be smaller than %lu.",
1316 pstream->stream_id, fsize->index, pstream->stream_common->snapshot_sizes.frmsize_num );
1317 return -EINVAL;
1318 }
1319
1320
1321 /* return framesize */
1322 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1323 fsize->discrete = pstream->stream_common->snapshot_sizes.frmsize[fsize->index];
1324
1325 return 0;
1326 }
1327
isp_v4l2_stream_enum_format(isp_v4l2_stream_t * pstream,struct v4l2_fmtdesc * f)1328 int isp_v4l2_stream_enum_format( isp_v4l2_stream_t *pstream, struct v4l2_fmtdesc *f )
1329 {
1330 const isp_v4l2_fmt_t *fmt;
1331 int desc_size = 0;
1332
1333 /* check index */
1334 if ( f->index >= ARRAY_SIZE( isp_v4l2_supported_formats ) ) {
1335 LOG( LOG_DEBUG, "[Stream#%d] index (%d) should be smaller than %lu.",
1336 pstream->stream_id, f->index, ARRAY_SIZE( isp_v4l2_supported_formats ) );
1337 return -EINVAL;
1338 }
1339
1340 /* get format from index */
1341 fmt = &isp_v4l2_supported_formats[f->index];
1342
1343 /* check description length */
1344 if ( sizeof( fmt->name ) > sizeof( f->description ) )
1345 desc_size = sizeof( f->description );
1346 else
1347 desc_size = sizeof( fmt->name );
1348
1349 /* reset flag */
1350 f->flags = 0;
1351
1352 /* copy description */
1353 strlcpy( f->description, fmt->name, desc_size );
1354
1355 /* copy format code */
1356 f->pixelformat = fmt->fourcc;
1357
1358 return 0;
1359 }
1360
isp_v4l2_stream_try_format(isp_v4l2_stream_t * pstream,struct v4l2_format * f)1361 int isp_v4l2_stream_try_format( isp_v4l2_stream_t *pstream, struct v4l2_format *f )
1362 {
1363 isp_v4l2_fmt_t *tfmt;
1364 int i;
1365 u32 offset;
1366
1367 LOG( LOG_DEBUG, "[Stream#%d] try fmt type: %u, pixelformat: 0x%x, width: %u, height: %u.\n",
1368 pstream->stream_id, f->type, f->fmt.pix_mp.pixelformat, f->fmt.pix_mp.width, f->fmt.pix_mp.height );
1369
1370 #if ISP_HAS_META_CB
1371 if ((pstream->stream_id == V4L2_STREAM_TYPE_META)
1372 && (f->fmt.pix_mp.pixelformat != ISP_V4L2_PIX_FMT_META)) {
1373 LOG(LOG_ERR, "[Stream#%d] pixel format for meta port must be 0x%08x, not 0x%08x. Correct it!!",
1374 pstream->stream_id, ISP_V4L2_PIX_FMT_META, f->fmt.pix_mp.pixelformat);
1375 f->fmt.pix_mp.pixelformat = ISP_V4L2_PIX_FMT_META;
1376 }
1377 #endif
1378
1379 /* check format and modify */
1380 if (f->fmt.pix_mp.pixelformat == ISP_V4L2_PIX_FMT_META) {
1381 tfmt = &ext_supported_formats[1];
1382 } else {
1383 tfmt = isp_v4l2_stream_find_format( f->fmt.pix_mp.pixelformat );
1384 if ( !tfmt ) {
1385 LOG( LOG_CRIT, "qqqq [Stream#%d] format 0x%08x is not supported, setting default format 0x%08x.\n",
1386 pstream->stream_id, f->fmt.pix.pixelformat, ISP_DEFAULT_FORMAT );
1387
1388 f->fmt.pix_mp.pixelformat = ISP_DEFAULT_FORMAT;
1389 tfmt = isp_v4l2_stream_find_format( f->fmt.pix_mp.pixelformat );
1390 }
1391 }
1392 //corect the exposure number here
1393 if ( tfmt->fourcc == V4L2_PIX_FMT_SBGGR16 ) {
1394 uint32_t spreset = 0, exposures_preset, rev_val;
1395
1396 if ( fw_intf_is_isp_started() ) {
1397 acamera_command( pstream->ctx_id, TSENSOR, SENSOR_PRESET, 0, COMMAND_GET, &spreset );
1398 acamera_command( pstream->ctx_id, TSENSOR, SENSOR_INFO_PRESET, spreset, COMMAND_SET, &rev_val );
1399 acamera_command( pstream->ctx_id, TSENSOR, SENSOR_INFO_EXPOSURES, 0, COMMAND_GET, &exposures_preset );
1400
1401 LOG( LOG_DEBUG, "[Stream#%d] Changing the number of planes according preset %d to exposures %d=>%d.\n", pstream->stream_id, spreset, tfmt->planes, exposures_preset );
1402 tfmt->planes = exposures_preset;
1403 } else {
1404 tfmt->planes = 1;
1405 }
1406 }
1407 /* adjust width, height for META stream */
1408 #if ISP_HAS_META_CB
1409 if ( f->fmt.pix.pixelformat == ISP_V4L2_PIX_FMT_META ) {
1410 f->fmt.pix.width = ISP_V4L2_METADATA_SIZE;
1411 f->fmt.pix.height = 1;
1412 } else
1413 #endif
1414 {
1415 if ( f->fmt.pix.width == 0 || f->fmt.pix.height == 0 ) {
1416 if ( fw_intf_is_isp_started() ) {
1417 uint32_t width_cur, height_cur;
1418 acamera_command( pstream->ctx_id, TSENSOR, SENSOR_WIDTH, 0, COMMAND_GET, &width_cur );
1419 acamera_command( pstream->ctx_id, TSENSOR, SENSOR_HEIGHT, 0, COMMAND_GET, &height_cur );
1420 f->fmt.pix.width = width_cur;
1421 f->fmt.pix.height = height_cur;
1422 } else {
1423 f->fmt.pix.width = 1920;
1424 f->fmt.pix.height = 1080;
1425 }
1426 }
1427 v4l_bound_align_image( &f->fmt.pix.width, 48, ISP_V4L2_MAX_WIDTH, 2,
1428 &f->fmt.pix.height, 32, ISP_V4L2_MAX_HEIGHT, 0, 0 );
1429 }
1430
1431 f->fmt.pix.field = V4L2_FIELD_NONE;
1432
1433
1434 //all stream multiplanar
1435 if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1436 f->fmt.pix_mp.num_planes = tfmt->planes;
1437 f->fmt.pix_mp.colorspace = ( tfmt->is_yuv ) ? V4L2_COLORSPACE_SMPTE170M : V4L2_COLORSPACE_SRGB;
1438 for ( i = 0; i < tfmt->planes; i++ ) {
1439 if ( (tfmt->fourcc == V4L2_PIX_FMT_NV12 || tfmt->fourcc == V4L2_PIX_FMT_NV21) && i == 1) {
1440 f->fmt.pix_mp.plane_fmt[i].bytesperline = ( ( ( f->fmt.pix_mp.width * tfmt->depth / 8 ) + 127 ) >> 7 ) << 7; // for padding
1441 f->fmt.pix_mp.plane_fmt[i].sizeimage = f->fmt.pix_mp.height * f->fmt.pix_mp.plane_fmt[i].bytesperline / 2;
1442 } else {
1443 f->fmt.pix_mp.plane_fmt[i].bytesperline = ( ( ( f->fmt.pix_mp.width * tfmt->depth / 8 ) + 127 ) >> 7 ) << 7; // for padding
1444 f->fmt.pix_mp.plane_fmt[i].sizeimage = f->fmt.pix_mp.height * f->fmt.pix_mp.plane_fmt[i].bytesperline;
1445 }
1446 memset( f->fmt.pix_mp.plane_fmt[i].reserved, 0, sizeof( f->fmt.pix_mp.plane_fmt[i].reserved ) );
1447 memset( f->fmt.pix_mp.reserved, 0, sizeof( f->fmt.pix_mp.reserved ) );
1448 }
1449 } else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
1450 f->fmt.pix.bytesperline = (((f->fmt.pix.width * tfmt->depth / 8) + 127) >> 7 ) << 7;
1451 if ((tfmt->fourcc == V4L2_PIX_FMT_NV12) || (tfmt->fourcc == V4L2_PIX_FMT_NV21)) {
1452 offset = (f->fmt.pix.height * f->fmt.pix.bytesperline) / 2;
1453 f->fmt.pix.sizeimage = (f->fmt.pix.height * f->fmt.pix.bytesperline) + offset;
1454 } else
1455 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
1456 }
1457
1458 return 0;
1459 }
1460
isp_v4l2_stream_get_format(isp_v4l2_stream_t * pstream,struct v4l2_format * f)1461 int isp_v4l2_stream_get_format( isp_v4l2_stream_t *pstream, struct v4l2_format *f )
1462 {
1463 if ( !pstream ) {
1464 LOG( LOG_ERR, "Null stream passed" );
1465 return -EINVAL;
1466 }
1467
1468 *f = pstream->cur_v4l2_fmt;
1469
1470 LOG( LOG_DEBUG, "[Stream#%d] - GET fmt - width: %4u, height: %4u, format: 0x%x.",
1471 pstream->stream_id,
1472 f->fmt.pix_mp.width,
1473 f->fmt.pix_mp.height,
1474 f->fmt.pix_mp.pixelformat );
1475
1476 if ( f->fmt.pix_mp.width == 0 || f->fmt.pix_mp.height == 0 || f->fmt.pix_mp.pixelformat == 0 ) { //not formatted yet
1477 LOG( LOG_CRIT, "Compliance error, uninitialized format" );
1478 }
1479
1480 return 0;
1481 }
1482
isp_v4l2_stream_set_format(isp_v4l2_stream_t * pstream,struct v4l2_format * f)1483 int isp_v4l2_stream_set_format( isp_v4l2_stream_t *pstream, struct v4l2_format *f )
1484 {
1485 int rc = 0;
1486
1487 if ( !pstream ) {
1488 LOG( LOG_ERR, "Null stream passed" );
1489 return -EINVAL;
1490 }
1491
1492 LOG( LOG_INFO, "[Stream#%d] - SET fmt - width: %4u, height: %4u, format: 0x%x.",
1493 pstream->stream_id,
1494 f->fmt.pix_mp.width,
1495 f->fmt.pix_mp.height,
1496 f->fmt.pix_mp.pixelformat );
1497
1498
1499 /* try format first */
1500 isp_v4l2_stream_try_format( pstream, f );
1501
1502 pstream->vb2_q->type = f->type;
1503 if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
1504 pstream->vb2_q->is_multiplanar = 0;
1505 else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1506 pstream->vb2_q->is_multiplanar = 1;
1507 else {
1508 LOG( LOG_CRIT, "not support video buffer type" );
1509 return -EINVAL;
1510 }
1511
1512 /* set stream type*/
1513 switch ( f->fmt.pix.pixelformat ) {
1514 case V4L2_PIX_FMT_RGB32:
1515 case ISP_V4L2_PIX_FMT_ARGB2101010:
1516 case V4L2_PIX_FMT_RGB24:
1517 case V4L2_PIX_FMT_NV12:
1518 case V4L2_PIX_FMT_SBGGR16:
1519 case V4L2_PIX_FMT_YUV444:
1520 case V4L2_PIX_FMT_YUYV:
1521 case V4L2_PIX_FMT_UYVY:
1522 case V4L2_PIX_FMT_GREY:
1523 case V4L2_PIX_FMT_NV21:
1524 /*add um*/
1525 case V4L2_PIX_FMT_YUV420:
1526 case V4L2_PIX_FMT_SBGGR10:
1527 pstream->stream_type = pstream->stream_id;
1528 break;
1529 #if ISP_HAS_META_CB
1530 case ISP_V4L2_PIX_FMT_META:
1531 pstream->stream_type = V4L2_STREAM_TYPE_META;
1532 break;
1533 #endif
1534 default:
1535 LOG( LOG_ERR, "Shouldn't be here after try_format()." );
1536 return -EINVAL;
1537 }
1538
1539 /* update resolution */
1540 rc = fw_intf_stream_set_resolution( pstream->ctx_id, &pstream->stream_common->sensor_info,
1541 pstream->stream_type, &( f->fmt.pix_mp.width ), &( f->fmt.pix_mp.height ) );
1542 if ( rc < 0 ) {
1543 LOG( LOG_CRIT, "set resolution failed ! (rc = %d)", rc );
1544 return rc;
1545 }
1546
1547 LOG( LOG_INFO, "[Stream#%d] Current preset:%d Exposures for this settings %d",
1548 pstream->stream_id,
1549 pstream->stream_common->sensor_info.preset_cur,
1550 pstream->stream_common->sensor_info.preset[pstream->stream_common->sensor_info.preset_cur].exposures[pstream->stream_common->sensor_info.preset[pstream->stream_common->sensor_info.preset_cur].fps_cur] );
1551
1552 /* update format */
1553 rc = fw_intf_stream_set_output_format( pstream->ctx_id, pstream->stream_type, f->fmt.pix_mp.pixelformat );
1554 if ( rc < 0 ) {
1555 LOG( LOG_CRIT, "set format failed ! (rc = %d)", rc );
1556 return rc;
1557 }
1558
1559 /* update format field */
1560 pstream->cur_v4l2_fmt = *f;
1561
1562
1563 LOG( LOG_INFO, "[Stream#%d] - New fmt - width: %4u, height: %4u, format: 0x%x, type: %5u. ",
1564 pstream->stream_id,
1565 pstream->cur_v4l2_fmt.fmt.pix_mp.width,
1566 pstream->cur_v4l2_fmt.fmt.pix_mp.height,
1567 pstream->cur_v4l2_fmt.fmt.pix_mp.pixelformat,
1568 pstream->cur_v4l2_fmt.type );
1569
1570 return 0;
1571 }
1572
isp_v4l2_get_gcd(uint32_t width,uint32_t height)1573 static uint32_t isp_v4l2_get_gcd(uint32_t width, uint32_t height)
1574 {
1575 uint32_t gcd = 0;
1576 uint32_t tmp = 0;
1577 uint32_t wd = 0;
1578 uint32_t ht = 0;
1579
1580 if (width == 0 || height == 0) {
1581 LOG(LOG_ERR, "Error input param");
1582 return gcd;
1583 }
1584
1585 wd = width;
1586 ht = height;
1587
1588 while (ht != 0) {
1589 tmp = wd % ht;
1590 wd = ht;
1591 ht = tmp;
1592 }
1593
1594 gcd = wd;
1595
1596 return gcd;
1597 }
1598
isp_v4l2_get_crop_type(isp_v4l2_stream_t * pstream)1599 static int isp_v4l2_get_crop_type(isp_v4l2_stream_t *pstream)
1600 {
1601 int s_type = -1;
1602
1603 if (pstream == NULL) {
1604 LOG(LOG_ERR, "Error input param");
1605 return s_type;
1606 }
1607
1608 switch (pstream->stream_type) {
1609 case V4L2_STREAM_TYPE_FR:
1610 s_type = CROP_FR;
1611 break;
1612 case V4L2_STREAM_TYPE_DS1:
1613 s_type = CROP_DS;
1614 break;
1615 default:
1616 LOG(LOG_ERR, "Error: this stream cannot surport crop");
1617 break;
1618 }
1619
1620 return s_type;
1621 }
1622
isp_v4l2_get_cropcap(isp_v4l2_stream_t * pstream,struct v4l2_cropcap * cap)1623 int isp_v4l2_get_cropcap(isp_v4l2_stream_t *pstream,
1624 struct v4l2_cropcap *cap)
1625 {
1626 int ret = -1;
1627 uint32_t width_cur = 0;
1628 uint32_t height_cur = 0;
1629 uint32_t gcd = 0;
1630 uint32_t ret_val = 0;
1631
1632 acamera_command( pstream->ctx_id, TSENSOR, SENSOR_WIDTH, 0, COMMAND_GET, &width_cur );
1633 acamera_command( pstream->ctx_id, TSENSOR, SENSOR_HEIGHT, 0, COMMAND_GET, &height_cur );
1634
1635 if (width_cur == 0 || height_cur == 0) {
1636 LOG(LOG_ERR, "Error sensor current resolution");
1637 return ret;
1638 }
1639
1640 cap->bounds.top = 0;
1641 cap->bounds.left = 0;
1642 cap->bounds.width = width_cur;
1643 cap->bounds.height = height_cur;
1644
1645 cap->defrect.top = 0;
1646 cap->defrect.left = 0;
1647 if (pstream->stream_type == V4L2_STREAM_TYPE_DS1) {
1648 acamera_command( pstream->ctx_id, TIMAGE, IMAGE_RESIZE_TYPE_ID,
1649 SCALER, COMMAND_SET, &ret_val );
1650 acamera_command(pstream->ctx_id, TIMAGE, IMAGE_RESIZE_WIDTH_ID, 0,
1651 COMMAND_GET, &ret_val);
1652 cap->defrect.width = ret_val;
1653
1654 acamera_command(pstream->ctx_id, TIMAGE, IMAGE_RESIZE_HEIGHT_ID, 0,
1655 COMMAND_GET, &ret_val);
1656 cap->defrect.height = ret_val;
1657 } else {
1658 cap->defrect.width = width_cur;
1659 cap->defrect.height = height_cur;
1660 }
1661 gcd = isp_v4l2_get_gcd(width_cur, height_cur);
1662 if (gcd == 0) {
1663 LOG(LOG_ERR, "Error get gcd");
1664 return 0;
1665 }
1666
1667 cap->pixelaspect.numerator = width_cur / gcd;
1668 cap->pixelaspect.denominator = height_cur / gcd;
1669
1670 return 0;
1671 }
1672
isp_v4l2_set_crop(isp_v4l2_stream_t * pstream,const struct v4l2_crop * crop)1673 int isp_v4l2_set_crop(isp_v4l2_stream_t *pstream,
1674 const struct v4l2_crop *crop)
1675 {
1676 uint8_t result = 0;
1677 uint32_t ret_val = 0;
1678 int s_type = 0;
1679 uint32_t width = 0;
1680 uint32_t height = 0;
1681 uint32_t x_off = 0;
1682 uint32_t y_off = 0;
1683
1684 if (pstream == NULL || crop == NULL) {
1685 LOG(LOG_ERR, "Error input param");
1686 return -1;
1687 }
1688
1689 s_type = isp_v4l2_get_crop_type(pstream);
1690
1691 if (s_type < 0) {
1692 LOG(LOG_ERR, "Error get stream type");
1693 return s_type;
1694 }
1695
1696 x_off = crop->c.left & 0xffff;
1697 y_off = crop->c.top & 0xffff;
1698 width = crop->c.width & 0xffff;
1699 height = crop->c.height & 0xffff;
1700
1701 result = acamera_command( pstream->ctx_id, TIMAGE, IMAGE_RESIZE_TYPE_ID,
1702 s_type, COMMAND_SET, &ret_val );
1703 if (result) {
1704 LOG( LOG_CRIT, "Failed to set resize_type, ret_value: %d.", result );
1705 return result;
1706 }
1707
1708 result = acamera_command(pstream->ctx_id, TIMAGE, IMAGE_CROP_XOFFSET_ID,
1709 x_off, COMMAND_SET, &ret_val);
1710 if (result) {
1711 LOG(LOG_CRIT, "Failed to set x offset, ret_value: %d.", result);
1712 return result;
1713 }
1714
1715 result = acamera_command(pstream->ctx_id, TIMAGE, IMAGE_RESIZE_WIDTH_ID,
1716 width, COMMAND_SET, &ret_val);
1717 if ( result ) {
1718 LOG( LOG_CRIT, "Failed to set resize_width, ret_value: %d.", result );
1719 return result;
1720 }
1721
1722 result = acamera_command(pstream->ctx_id, TIMAGE, IMAGE_CROP_YOFFSET_ID,
1723 y_off, COMMAND_SET, &ret_val);
1724 if (result) {
1725 LOG(LOG_CRIT, "Failed to set y offset, ret_value: %d.", result);
1726 return result;
1727 }
1728
1729 result = acamera_command( pstream->ctx_id, TIMAGE, IMAGE_RESIZE_HEIGHT_ID,
1730 height, COMMAND_SET, &ret_val );
1731 if (result) {
1732 LOG( LOG_CRIT, "Failed to set resize_height, ret_value: %d.", result );
1733 return result;
1734 }
1735
1736 result = acamera_command( pstream->ctx_id, TIMAGE, IMAGE_RESIZE_ENABLE_ID,
1737 RUN, COMMAND_SET, &ret_val);
1738 if (result) {
1739 LOG(LOG_CRIT, "Failed to set resize_enable, ret_value: %d.", result);
1740 return result;
1741 }
1742
1743 if (pstream->stream_type == V4L2_STREAM_TYPE_FR) {
1744 acamera_command( pstream->ctx_id, TAML_SCALER, SCALER_SRC_WIDTH,
1745 width, COMMAND_SET, &ret_val );
1746 acamera_command( pstream->ctx_id, TAML_SCALER, SCALER_SRC_HEIGHT,
1747 height, COMMAND_SET, &ret_val );
1748 }
1749
1750 return result;
1751 }
1752
isp_v4l2_get_crop(isp_v4l2_stream_t * pstream,struct v4l2_crop * crop)1753 int isp_v4l2_get_crop(isp_v4l2_stream_t *pstream,
1754 struct v4l2_crop *crop)
1755 {
1756 uint8_t result = 0;
1757 uint32_t ret_val = 0;
1758 int s_type = 0;
1759
1760 if (pstream == NULL || crop == NULL) {
1761 LOG(LOG_ERR, "Error input param");
1762 return -1;
1763 }
1764
1765 s_type = isp_v4l2_get_crop_type(pstream);
1766
1767 if (s_type < 0) {
1768 LOG(LOG_ERR, "Error get stream type");
1769 return s_type;
1770 }
1771
1772 result = acamera_command( pstream->ctx_id, TIMAGE, IMAGE_RESIZE_TYPE_ID,
1773 s_type, COMMAND_SET, &ret_val );
1774 if (result) {
1775 LOG( LOG_CRIT, "Failed to get resize_type, ret_value: %d.", result );
1776 return result;
1777 }
1778
1779 result = acamera_command(pstream->ctx_id, TIMAGE, IMAGE_CROP_XOFFSET_ID,
1780 0, COMMAND_GET, &ret_val);
1781 if (result) {
1782 LOG(LOG_CRIT, "Failed to get x offset, ret_value: %d.", result);
1783 return result;
1784 }
1785 crop->c.left = ret_val;
1786
1787 result = acamera_command(pstream->ctx_id, TIMAGE, IMAGE_CROP_YOFFSET_ID,
1788 0, COMMAND_GET, &ret_val);
1789 if (result) {
1790 LOG(LOG_CRIT, "Failed to get x offset, ret_value: %d.", result);
1791 return result;
1792 }
1793 crop->c.top = ret_val;
1794
1795 result = acamera_command(pstream->ctx_id, TIMAGE, IMAGE_RESIZE_WIDTH_ID,
1796 0, COMMAND_GET, &ret_val);
1797 if (result) {
1798 LOG(LOG_CRIT, "Failed to get x offset, ret_value: %d.", result);
1799 return result;
1800 }
1801 crop->c.width = ret_val;
1802
1803 result = acamera_command(pstream->ctx_id, TIMAGE, IMAGE_RESIZE_HEIGHT_ID,
1804 0, COMMAND_GET, &ret_val);
1805 if (result) {
1806 LOG(LOG_CRIT, "Failed to get x offset, ret_value: %d.", result);
1807 return result;
1808 }
1809 crop->c.height = ret_val;
1810
1811 return result;
1812 }
1813
1814
isp_v4l2_stream_enum_frameintervals(isp_v4l2_stream_t * pstream,struct v4l2_frmivalenum * fival)1815 int isp_v4l2_stream_enum_frameintervals( isp_v4l2_stream_t *pstream, struct v4l2_frmivalenum *fival )
1816 {
1817 if ( !isp_v4l2_stream_find_format( fival->pixel_format ) ) {
1818 return -EINVAL;
1819 }
1820
1821 /* check index */
1822 if ( fival->index >= pstream->stream_common->snapshot_sizes.frmsize_num ) {
1823 LOG( LOG_INFO, "[Stream#%d] index (%d) should be smaller than %u.",
1824 pstream->stream_id, fival->index, pstream->stream_common->snapshot_sizes.frmsize_num );
1825 return -EINVAL;
1826 }
1827
1828 /* return framesize */
1829 fival->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1830 fival->discrete.numerator = 1;
1831 fival->discrete.denominator = 30;
1832
1833 return 0;
1834 }
1835
1836