1 /*
2 * drivers/media/platform/sunxi-vin/vin_test/mplane_image/csi_test_mplane.c
3 *
4 * Copyright (c) 2014 softwinner.
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 */
17
18 /*
19 * zw
20 * for csi & isp test
21 */
22
23 #include <stdio.h>
24 #include <stdlib.h>
25 #include <string.h>
26 #include <assert.h>
27 #include <time.h>
28 #include <signal.h>
29 #include <linux/fb.h>
30 #include <linux/input.h>
31 #include <linux/version.h>
32 #include <getopt.h>
33 #include <fcntl.h>
34 #include <unistd.h>
35 #include <errno.h>
36 #include <malloc.h>
37 #include <signal.h>
38 #include <sys/stat.h>
39 #include <sys/types.h>
40 #include <sys/time.h>
41 #include <sys/mman.h>
42 #include <sys/ioctl.h>
43
44 #include <asm/types.h>
45
46 #include "../sunxi_camera_v2.h"
47 #include "../sunxi_display2.h"
48
49 #define CLEAR(x) (memset(&(x), 0, sizeof(x)))
50 #define ALIGN_4K(x) (((x) + (4095)) & ~(4095))
51 #define ALIGN_16B(x) (((x) + (15)) & ~(15))
52
53 #define display_frame 0
54 #define video_s_ctrl 0
55
56 struct size {
57 int width;
58 int height;
59 };
60 struct buffer {
61 void *start[3];
62 int length[3];
63 };
64
65 typedef enum {
66 TVD_PL_YUV420 = 0,
67 TVD_MB_YUV420 = 1,
68 TVD_PL_YUV422 = 2,
69 } TVD_FMT_T;
70
71 struct disp_screen {
72 int x;
73 int y;
74 int w;
75 int h;
76 };
77
78 struct test_layer_info {
79 int screen_id;
80 int layer_id;
81 int mem_id;
82 disp_layer_config layer_config;
83 int addr_map;
84 int width, height;/* screen size */
85 int dispfh;/* device node handle */
86 int fh;/* picture resource file handle */
87 int mem;
88 int clear;/* is clear layer */
89 char filename[32];
90 int full_screen;
91 unsigned int pixformat;
92 disp_output_type output_type;
93 };
94
95 /**
96 * tvd_dev info
97 */
98 struct tvd_dev {
99 unsigned int ch_id;
100 unsigned int height;
101 unsigned int width;
102 unsigned int interface;
103 unsigned int system;
104 unsigned int row;
105 unsigned int column;
106 unsigned int ch0_en;
107 unsigned int ch1_en;
108 unsigned int ch2_en;
109 unsigned int ch3_en;
110 unsigned int pixformat;
111 struct test_layer_info layer_info;
112 int frame_no_to_grap;
113 FILE *raw_fp;
114 };
115 struct tvd_dev dev;
116
117 static char path_name[20];
118 static char dev_name[20];
119 static int fd = -1;
120 static int isp0_fd = -1;
121 static int isp1_fd = -1;
122
123 struct buffer *buffers;
124 static unsigned int n_buffers;
125
126 struct size input_size;
127
128 unsigned int req_frame_num = 5;
129 unsigned int read_num = 20;
130 unsigned int count;
131 unsigned int nplanes;
132 unsigned int save_flag;
133 int dev_id;
134 unsigned int fps = 30;
135 unsigned int wdr_mode;
136
137 #define ROT_90 0
138
yuv_r90(char * dst,char * src,int width,int height)139 static void yuv_r90(char *dst, char *src, int width, int height)
140 {
141 int i = 0, j = 0;
142
143 for (i = 0; i < width; i++) {
144 for (j = 0; j < height; j++)
145 *(char *)(dst + j + i * height) = *(char *)(src + (height - j - 1) * width + i);
146 }
147 }
148
uv_r90(char * dst,char * src,int width,int height)149 static void uv_r90(char *dst, char *src, int width, int height)
150 {
151 int i = 0, j = 0;
152
153 for (i = 0; i < width/2; i++) {
154 for (j = 0; j < height/2; j++)
155 *(char *)(dst + j * 2 + i * height) = *(char *)(src + (height/2 - j - 1) * width + i * 2);
156 }
157
158 for (i = 0; i < width/2; i++) {
159 for (j = 0; j < height/2; j++)
160 *(char *)(dst + j * 2 + 1 + i * height) = *(char *)(src + (height/2 - j - 1) * width + i * 2 + 1);
161 }
162 }
163
disp_set_addr(int width,int height,struct v4l2_buffer * buf)164 static int disp_set_addr(int width, int height, struct v4l2_buffer *buf)
165
166 {
167 unsigned long arg[6];
168 int ret;
169
170 if (dev.layer_info.pixformat == TVD_PL_YUV420) {
171 /* printf("******YUV420!\n"); */
172 dev.layer_info.layer_config.info.fb.size[0].width = width;
173 dev.layer_info.layer_config.info.fb.size[0].height = height;
174 dev.layer_info.layer_config.info.fb.size[1].width = width / 2;
175 dev.layer_info.layer_config.info.fb.size[1].height = height / 2;
176 dev.layer_info.layer_config.info.fb.size[2].width = width / 2;
177 dev.layer_info.layer_config.info.fb.size[2].height = height / 2;
178 dev.layer_info.layer_config.info.fb.crop.width =
179 (unsigned long long)width << 32;
180 dev.layer_info.layer_config.info.fb.crop.height =
181 (unsigned long long)height << 32;
182
183 dev.layer_info.layer_config.info.fb.addr[0] = buf->m.planes[0].m.mem_offset;
184 dev.layer_info.layer_config.info.fb.addr[1] = buf->m.planes[1].m.mem_offset;
185 dev.layer_info.layer_config.info.fb.addr[2] = buf->m.planes[2].m.mem_offset;
186
187 /* dev.layer_info.layer_config.info.fb.addr[0] = (*addr);
188 dev.layer_info.layer_config.info.fb.addr[1] =
189 (dev.layer_info.layer_config.info.fb.addr[0] + width * height);
190 dev.layer_info.layer_config.info.fb.addr[2] =
191 dev.layer_info.layer_config.info.fb.addr[0] +
192 width * height * 5 / 4;
193 dev.layer_info.layer_config.info.fb.trd_right_addr[0] =
194 (dev.layer_info.layer_config.info.fb.addr[0] +
195 width * height * 3 / 2);
196 dev.layer_info.layer_config.info.fb.trd_right_addr[1] =
197 (dev.layer_info.layer_config.info.fb.addr[0] + width * height);
198 dev.layer_info.layer_config.info.fb.trd_right_addr[2] =
199 (dev.layer_info.layer_config.info.fb.addr[0] +
200 width * height * 5 / 4); */
201 } else {
202 dev.layer_info.layer_config.info.fb.size[0].width = width;
203 dev.layer_info.layer_config.info.fb.size[0].height = height;
204 dev.layer_info.layer_config.info.fb.size[1].width = width / 2;
205 dev.layer_info.layer_config.info.fb.size[1].height = height;
206 dev.layer_info.layer_config.info.fb.size[2].width = width / 2;
207 dev.layer_info.layer_config.info.fb.size[2].height = height;
208 dev.layer_info.layer_config.info.fb.crop.width =
209 (unsigned long long)width << 32;
210 dev.layer_info.layer_config.info.fb.crop.height =
211 (unsigned long long)height << 32;
212
213 dev.layer_info.layer_config.info.fb.addr[0] = buf->m.planes[0].m.mem_offset;
214 dev.layer_info.layer_config.info.fb.addr[1] = buf->m.planes[1].m.mem_offset;
215 dev.layer_info.layer_config.info.fb.addr[2] = buf->m.planes[2].m.mem_offset;
216
217 /* dev.layer_info.layer_config.info.fb.addr[0] = (*addr);
218 dev.layer_info.layer_config.info.fb.addr[1] =
219 (dev.layer_info.layer_config.info.fb.addr[0] + width * height);
220 dev.layer_info.layer_config.info.fb.addr[2] =
221 dev.layer_info.layer_config.info.fb.addr[0] +
222 width * height * 2 / 2;
223 dev.layer_info.layer_config.info.fb.trd_right_addr[0] =
224 (dev.layer_info.layer_config.info.fb.addr[0] +
225 width * height * 2);
226 dev.layer_info.layer_config.info.fb.trd_right_addr[1] =
227 (dev.layer_info.layer_config.info.fb.addr[0] + width * height); */
228 }
229
230 dev.layer_info.layer_config.enable = 1;
231
232 arg[0] = dev.layer_info.screen_id;
233 arg[1] = (unsigned long)&dev.layer_info.layer_config;
234 arg[2] = 1;
235 arg[3] = 0;
236 ret = ioctl(dev.layer_info.dispfh, DISP_LAYER_SET_CONFIG, (void *)arg);
237 if (ret != 0)
238 printf("disp_set_addr fail to set layer info\n");
239
240 return 0;
241 }
242
read_frame(int mode)243 static int read_frame(int mode)
244 {
245 struct v4l2_buffer buf;
246 char fdstr[50];
247 FILE *file_fd = NULL;
248 char *dst = NULL;
249
250 CLEAR(buf);
251 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
252 buf.memory = V4L2_MEMORY_MMAP;
253 buf.length = nplanes;
254 buf.m.planes =
255 (struct v4l2_plane *)calloc(nplanes, sizeof(struct v4l2_plane));
256
257 if (-1 == ioctl(fd, VIDIOC_DQBUF, &buf)) {
258 free(buf.m.planes);
259 printf("VIDIOC_DQBUF failed\n");
260 return -1;
261 }
262
263 assert(buf.index < n_buffers);
264
265 if (save_flag == 0) {
266 if ((count == read_num / 2) || ((count > 0) && (nplanes == 1))) {
267 printf("file length = %d %d %d\n", buffers[buf.index].length[0],
268 buffers[buf.index].length[1],
269 buffers[buf.index].length[2]);
270 printf("file start = %p %p %p\n", buffers[buf.index].start[0],
271 buffers[buf.index].start[1],
272 buffers[buf.index].start[2]);
273
274 switch (nplanes) {
275 case 1:
276 sprintf(fdstr, "%s/fb%d_y%d_%d_%d_%u.bin", path_name, dev_id, mode, input_size.width, input_size.height, count);
277 file_fd = fopen(fdstr, "w");
278 fwrite(buffers[buf.index].start[0], buffers[buf.index].length[0], 1, file_fd);
279 fclose(file_fd);
280 break;
281 case 2:
282 #if ROT_90
283 dst = (char *)malloc(buffers[buf.index].length[0]);
284 yuv_r90(dst, buffers[buf.index].start[0], input_size.width, input_size.height);
285 sprintf(fdstr, "%s/fb%d_y%d_%d_%d.bin", path_name, dev_id, mode, input_size.height, input_size.width);
286 file_fd = fopen(fdstr, "w");
287 fwrite(dst, buffers[buf.index].length[0], 1, file_fd);
288 fclose(file_fd);
289 free(dst);
290
291 dst = (char *)malloc(buffers[buf.index].length[1]);
292 uv_r90(dst, buffers[buf.index].start[1], input_size.width, input_size.height);
293 sprintf(fdstr, "%s/fb%d_uv%d_%d_%d.bin", path_name, dev_id, mode, input_size.height, input_size.width);
294 file_fd = fopen(fdstr, "w");
295 fwrite(dst, buffers[buf.index].length[1], 1, file_fd);
296 fclose(file_fd);
297 free(dst);
298 #else
299 sprintf(fdstr, "%s/fb%d_y%d_%d_%d.bin", path_name, dev_id, mode, input_size.width, input_size.height);
300 file_fd = fopen(fdstr, "w");
301 fwrite(buffers[buf.index].start[0], buffers[buf.index].length[0], 1, file_fd);
302 fclose(file_fd);
303 sprintf(fdstr, "%s/fb%d_uv%d_%d_%d.bin", path_name, dev_id, mode, input_size.width, input_size.height);
304 file_fd = fopen(fdstr, "w");
305 fwrite(buffers[buf.index].start[1], buffers[buf.index].length[1], 1, file_fd);
306 fclose(file_fd);
307 #endif
308 break;
309 case 3:
310 #if ROT_90
311 dst = (char *)malloc(buffers[buf.index].length[0]);
312 yuv_r90(dst, buffers[buf.index].start[0], input_size.width, input_size.height);
313 sprintf(fdstr, "%s/fb%d_y%d_%d_%d.bin", path_name, dev_id, mode, input_size.height, input_size.width);
314 file_fd = fopen(fdstr, "w");
315 fwrite(dst, buffers[buf.index].length[0], 1, file_fd);
316 fclose(file_fd);
317 free(dst);
318
319 dst = (char *)malloc(buffers[buf.index].length[1]);
320 yuv_r90(dst, buffers[buf.index].start[1], input_size.width/2, input_size.height/2);
321 sprintf(fdstr, "%s/fb%d_u%d_%d_%d.bin", path_name, dev_id, mode, input_size.height, input_size.width);
322 file_fd = fopen(fdstr, "w");
323 fwrite(dst, buffers[buf.index].length[1], 1, file_fd);
324 fclose(file_fd);
325 free(dst);
326
327 dst = (char *)malloc(buffers[buf.index].length[2]);
328 yuv_r90(dst, buffers[buf.index].start[2], input_size.width/2, input_size.height/2);
329 sprintf(fdstr, "%s/fb%d_v%d_%d_%d.bin", path_name, dev_id, mode, input_size.height, input_size.width);
330 file_fd = fopen(fdstr, "w");
331 fwrite(dst, buffers[buf.index].length[2], 1, file_fd);
332 fclose(file_fd);
333 free(dst);
334 #else
335 sprintf(fdstr, "%s/fb%d_y%d_%d_%d.bin", path_name, dev_id, mode, input_size.width, input_size.height);
336 file_fd = fopen(fdstr, "w");
337 fwrite(buffers[buf.index].start[0], buffers[buf.index].length[0], 1, file_fd);
338 fclose(file_fd);
339
340 sprintf(fdstr, "%s/fb%d_u%d_%d_%d.bin", path_name, dev_id, mode, input_size.width, input_size.height);
341 file_fd = fopen(fdstr, "w");
342 fwrite(buffers[buf.index].start[1], buffers[buf.index].length[1], 1, file_fd);
343 fclose(file_fd);
344
345 sprintf(fdstr, "%s/fb%d_v%d_%d_%d.bin", path_name, dev_id, mode, input_size.width, input_size.height);
346 file_fd = fopen(fdstr, "w");
347 fwrite(buffers[buf.index].start[2], buffers[buf.index].length[2], 1, file_fd);
348 fclose(file_fd);
349 #endif
350 break;
351 default:
352 break;
353 }
354 }
355 } else if (save_flag == 1) {
356 //if ((count > 0) && (count % 4 == 0)) {
357 if ((count > 0)) {
358 switch (nplanes) {
359 case 1:
360 sprintf(fdstr, "%s/fb%d_yuv%d_%d_%d.bin", path_name, dev_id, mode, input_size.width, input_size.height);
361 file_fd = fopen(fdstr, "ab");
362 fwrite(buffers[buf.index].start[0], buffers[buf.index].length[0], 1, file_fd);
363 fclose(file_fd);
364 break;
365 case 2:
366 sprintf(fdstr, "%s/fb%d_yuv%d_%d_%d.bin", path_name, dev_id, mode, input_size.width, input_size.height);
367 file_fd = fopen(fdstr, "ab");
368 fwrite(buffers[buf.index].start[0], buffers[buf.index].length[0], 1, file_fd);
369 fclose(file_fd);
370 file_fd = fopen(fdstr, "ab");
371 fwrite(buffers[buf.index].start[1], buffers[buf.index].length[1], 1, file_fd);
372 fclose(file_fd);
373 break;
374 case 3:
375 sprintf(fdstr, "%s/fb%d_yuv%d_%d_%d.bin", path_name, dev_id, mode, input_size.width, input_size.height);
376 file_fd = fopen(fdstr, "ab");
377 fwrite(buffers[buf.index].start[0], buffers[buf.index].length[0], 1, file_fd);
378 fclose(file_fd);
379 file_fd = fopen(fdstr, "ab");
380 fwrite(buffers[buf.index].start[1], buffers[buf.index].length[1], 1, file_fd);
381 fclose(file_fd);
382 file_fd = fopen(fdstr, "ab");
383 fwrite(buffers[buf.index].start[2], buffers[buf.index].length[2], 1, file_fd);
384 fclose(file_fd);
385 break;
386 default:
387 break;
388 }
389 }
390 } else if (save_flag == 2) {
391 if (count <= 1)
392 count = read_num;
393 #if display_frame
394 disp_set_addr(input_size.width, input_size.height, &buf);
395 #endif
396 } else {
397 count = 0;
398 }
399
400 if (-1 == ioctl(fd, VIDIOC_QBUF, &buf)) {
401 printf("VIDIOC_QBUF buf.index %d failed\n", buf.index);
402 free(buf.m.planes);
403 return -1;
404 }
405
406 free(buf.m.planes);
407
408 return 0;
409 }
410
get_disp_screen(int w1,int h1,int w2,int h2)411 static struct disp_screen get_disp_screen(int w1, int h1, int w2, int h2)
412 {
413 struct disp_screen screen;
414 float r1, r2;
415
416 r1 = (float)w1/(float)w2;
417 r2 = (float)h1/(float)h2;
418 if (r1 < r2) {
419 screen.w = w2*r1;
420 screen.h = h2*r1;
421 } else {
422 screen.w = w2*r2;
423 screen.h = h2*r2;
424 }
425
426 screen.x = (w1 - screen.w)/2;
427 screen.y = (h1 - screen.h)/2;
428
429 return screen;
430 }
431
disp_disable(void)432 static int disp_disable(void)
433 {
434 #if display_frame
435 int ret;
436 unsigned long arg[6];
437 struct disp_layer_config disp;
438
439 /* release memory && clear layer */
440 arg[0] = 0;
441 arg[1] = 0;
442 arg[2] = 0;
443 arg[3] = 0;
444 ioctl(dev.layer_info.dispfh, DISP_LAYER_DISABLE, (void *)arg);
445
446 /*close channel 0*/
447 memset(&disp, 0, sizeof(disp_layer_config));
448 disp.channel = 0;
449 disp.layer_id = 0;
450 disp.enable = 0;
451 arg[0] = dev.layer_info.screen_id;
452 arg[1] = (unsigned long)&disp;
453 arg[2] = 1;
454 arg[3] = 0;
455 ret = ioctl(dev.layer_info.dispfh, DISP_LAYER_SET_CONFIG, (void *)arg);
456 if (ret != 0)
457 printf("disp_disable:disp_set_addr fail to set layer info\n");
458
459 /*close channel 2*/
460 memset(&disp, 0, sizeof(disp_layer_config));
461 disp.channel = 2;
462 disp.layer_id = 0;
463 disp.enable = 0;
464 arg[0] = dev.layer_info.screen_id;
465 arg[1] = (unsigned long)&disp;
466 arg[2] = 1;
467 arg[3] = 0;
468 ret = ioctl(dev.layer_info.dispfh, DISP_LAYER_SET_CONFIG, (void *)arg);
469 if (ret != 0)
470 printf("disp_disable:disp_set_addr fail to set layer info\n");
471
472 return ret;
473 #else
474 return 0;
475 #endif
476 }
477
disp_init(int width,int height,unsigned int pixformat)478 static int disp_init(int width, int height, unsigned int pixformat)
479 {
480 #if display_frame
481 /* display_handle* disp = (display_handle*)display; */
482 unsigned int arg[6] = {0};
483 int layer_id = 0;
484
485 dev.layer_info.screen_id = 0;
486
487 if (dev.layer_info.screen_id < 0)
488 return 0;
489
490 /* open device /dev/disp */
491 dev.layer_info.dispfh = open("/dev/disp", O_RDWR);
492 if (dev.layer_info.dispfh == -1) {
493 printf("open display device fail!\n");
494 return -1;
495 }
496
497 /* open disp */
498 arg[0] = dev.layer_info.screen_id;
499 arg[1] = 1;
500 arg[2] = 4;
501 ioctl(
502 dev.layer_info.dispfh, DISP_DEVICE_SWITCH, (void *)arg);
503
504 /* get current output type */
505 arg[0] = dev.layer_info.screen_id;
506 dev.layer_info.output_type = (disp_output_type)ioctl(
507 dev.layer_info.dispfh, DISP_GET_OUTPUT_TYPE, (void *)arg);
508 if (dev.layer_info.output_type == DISP_OUTPUT_TYPE_NONE) {
509 printf("the output type is DISP_OUTPUT_TYPE_NONE %d\n",
510 dev.layer_info.output_type);
511 return -1;
512 }
513
514 disp_disable();
515
516 dev.layer_info.pixformat = pixformat;
517 dev.layer_info.layer_config.channel = 0;
518 dev.layer_info.layer_config.layer_id = layer_id;
519 dev.layer_info.layer_config.info.zorder = 1;
520 dev.layer_info.layer_config.info.alpha_mode = 1;
521 dev.layer_info.layer_config.info.alpha_value = 0xff;
522 dev.layer_info.width =
523 ioctl(dev.layer_info.dispfh, DISP_GET_SCN_WIDTH, (void *)arg);
524 dev.layer_info.height =
525 ioctl(dev.layer_info.dispfh, DISP_GET_SCN_HEIGHT, (void *)arg);
526
527 dev.layer_info.layer_config.info.mode = LAYER_MODE_BUFFER;
528
529 if (dev.layer_info.pixformat == TVD_PL_YUV420)
530 dev.layer_info.layer_config.info.fb.format = DISP_FORMAT_YUV420_P; /*DISP_FORMAT_YUV420_P ---- V4L2_PIX_FMT_YUV420M*/
531 //DISP_FORMAT_YUV420_SP_UVUV; /*DISP_FORMAT_YUV420_SP_UVUV ---- V4L2_PIX_FMT_NV12*/
532 else
533 dev.layer_info.layer_config.info.fb.format =
534 DISP_FORMAT_YUV422_SP_VUVU;
535
536 if (dev.layer_info.full_screen == 0 && width < dev.layer_info.width &&
537 height < dev.layer_info.height) {
538 dev.layer_info.layer_config.info.screen_win.x =
539 (dev.layer_info.width - width) / 2;
540 dev.layer_info.layer_config.info.screen_win.y =
541 (dev.layer_info.height - height) / 2;
542 if (!dev.layer_info.layer_config.info.screen_win.width) {
543 dev.layer_info.layer_config.info.screen_win.width = width;
544 dev.layer_info.layer_config.info.screen_win.height =
545 height;
546 }
547 } else {
548 /* struct disp_screen screen; */
549 get_disp_screen(dev.layer_info.width, dev.layer_info.height,
550 width, height);
551 dev.layer_info.layer_config.info.screen_win.x = 0; /* screen.x; */
552 dev.layer_info.layer_config.info.screen_win.y = 0; /* screen.y; */
553 dev.layer_info.layer_config.info.screen_win.width =
554 dev.layer_info.width;
555 dev.layer_info.layer_config.info.screen_win.height =
556 dev.layer_info.height;
557 /* printf("x: %d, y: %d, w: %d, h: %d\n",screen.x,screen.y,screen.w,screen.h); */
558 }
559 #endif
560 return 0;
561 }
562
terminate(int sig_no)563 static void terminate(int sig_no)
564 {
565 printf("Got signal %d, exiting ...\n", sig_no);
566 disp_disable();
567 usleep(20*1000);
568 exit(1);
569 }
570
install_sig_handler(void)571 static void install_sig_handler(void)
572 {
573 signal(SIGBUS, terminate);
574 signal(SIGFPE, terminate);
575 signal(SIGHUP, terminate);
576 signal(SIGILL, terminate);
577 signal(SIGKILL, terminate);
578 signal(SIGINT, terminate);
579 signal(SIGIOT, terminate);
580 signal(SIGPIPE, terminate);
581 signal(SIGQUIT, terminate);
582 signal(SIGSEGV, terminate);
583 signal(SIGSYS, terminate);
584 signal(SIGTERM, terminate);
585 signal(SIGTRAP, terminate);
586 signal(SIGUSR1, terminate);
587 signal(SIGUSR2, terminate);
588 }
589
req_frame_buffers(void)590 static int req_frame_buffers(void)
591 {
592 unsigned int i;
593 struct v4l2_requestbuffers req;
594 struct v4l2_exportbuffer exp;
595
596 CLEAR(req);
597 req.count = req_frame_num;
598 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
599 req.memory = V4L2_MEMORY_MMAP;
600 if (-1 == ioctl(fd, VIDIOC_REQBUFS, &req)) {
601 printf("VIDIOC_REQBUFS error\n");
602 return -1;
603 }
604
605 buffers = calloc(req.count, sizeof(*buffers));
606
607 for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
608 struct v4l2_buffer buf;
609
610 CLEAR(buf);
611 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
612 buf.memory = V4L2_MEMORY_MMAP;
613 buf.index = n_buffers;
614 buf.length = nplanes;
615 buf.m.planes =
616 (struct v4l2_plane *)calloc(nplanes,
617 sizeof(struct v4l2_plane));
618 if (buf.m.planes == NULL) {
619 printf("buf.m.planes calloc failed!\n");
620 return -1;
621 }
622 if (-1 == ioctl(fd, VIDIOC_QUERYBUF, &buf)) {
623 printf("VIDIOC_QUERYBUF error\n");
624 free(buf.m.planes);
625 return -1;
626 }
627
628 for (i = 0; i < nplanes; i++) {
629 buffers[n_buffers].length[i] = buf.m.planes[i].length;
630 buffers[n_buffers].start[i] =
631 mmap(NULL,/* start anywhere */
632 buf.m.planes[i].length,
633 PROT_READ | PROT_WRITE,/* required */
634 MAP_SHARED, /* recommended */
635 fd, buf.m.planes[i].m.mem_offset);
636
637 if (buffers[n_buffers].start[i] == MAP_FAILED) {
638 printf("mmap failed\n");
639 free(buf.m.planes);
640 return -1;
641 }
642 #if 0
643 CLEAR(exp);
644 exp.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
645 exp.index = n_buffers;
646 exp.plane = i;
647 exp.flags = O_CLOEXEC;
648 if (-1 == ioctl(fd, VIDIOC_EXPBUF, &exp)) {
649 printf("VIDIOC_EXPBUF error\n");
650 return -1;
651 }
652 printf("buffer %d plane %d DMABUF fd is %d\n", n_buffers, i, exp.fd);
653 #endif
654 }
655 free(buf.m.planes);
656 }
657
658 for (i = 0; i < n_buffers; ++i) {
659 struct v4l2_buffer buf;
660
661 CLEAR(buf);
662 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
663 buf.memory = V4L2_MEMORY_MMAP;
664 buf.index = i;
665 buf.length = nplanes;
666 buf.m.planes =
667 (struct v4l2_plane *)calloc(nplanes,
668 sizeof(struct v4l2_plane));
669
670 if (-1 == ioctl(fd, VIDIOC_QBUF, &buf)) {
671 printf("VIDIOC_QBUF failed\n");
672 free(buf.m.planes);
673 return -1;
674 }
675 free(buf.m.planes);
676 }
677 return 0;
678 }
679
free_frame_buffers(void)680 static int free_frame_buffers(void)
681 {
682 unsigned int i, j;
683
684 for (i = 0; i < n_buffers; ++i) {
685 for (j = 0; j < nplanes; j++)
686 if (-1 ==
687 munmap(buffers[i].start[j], buffers[i].length[j])) {
688 printf("munmap error");
689 return -1;
690 }
691 }
692 free(buffers);
693 return 0;
694 }
695
subdev_open(int * sub_fd,char * str)696 static int subdev_open(int *sub_fd, char *str)
697 {
698 char subdev[20] = {'\0'};
699 char node[50] = {'\0'};
700 char data[20] = {'\0'};
701 int i, fs = -1;
702
703 for (i = 0; i < 255; i++) {
704 sprintf(node, "/sys/class/video4linux/v4l-subdev%d/name", i);
705 fs = open(node, O_RDONLY/* required */| O_NONBLOCK, 0);
706 if (fs < 0) {
707 printf("open %s falied\n", node);
708 continue;
709 }
710 /*data_length = lseek(fd, 0, SEEK_END);*/
711 lseek(fs, 0L, SEEK_SET);
712 read(fs, data, 20);
713 close(fs);
714 if (!strncmp(str, data, strlen(str))) {
715 sprintf(subdev, "/dev/v4l-subdev%d", i);
716 printf("find %s is %s\n", str, subdev);
717 *sub_fd = open(subdev, O_RDWR | O_NONBLOCK, 0);
718 if (*sub_fd < 0) {
719 printf("open %s falied\n", str);
720 return -1;
721 }
722 printf("open %s fd = %d\n", str, *sub_fd);
723 return 0;
724 }
725 }
726 printf("can not find %s!\n", str);
727 return -1;
728 }
729
camera_init(int sel,int mode)730 static int camera_init(int sel, int mode)
731 {
732 struct v4l2_input inp;
733 struct v4l2_streamparm parms;
734 struct sensor_isp_cfg sensor_isp_cfg;
735
736 fd = open(dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
737
738 if (fd < 0) {
739 printf("open falied\n");
740 return -1;
741 }
742 printf("open %s fd = %d\n", dev_name, fd);
743
744 #ifdef SUBDEV_TEST
745 if (-1 == subdev_open(&isp0_fd, "sunxi_isp.0"))
746 return -1;
747 if (-1 == subdev_open(&isp1_fd, "sunxi_isp.1"))
748 return -1;
749 #endif
750
751 inp.index = sel;
752 if (-1 == ioctl(fd, VIDIOC_S_INPUT, &inp)) {
753 printf("VIDIOC_S_INPUT %d error!\n", sel);
754 return -1;
755 }
756
757 CLEAR(parms);
758 parms.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
759 parms.parm.capture.timeperframe.numerator = 1;
760 parms.parm.capture.timeperframe.denominator = fps;
761 parms.parm.capture.capturemode = V4L2_MODE_VIDEO;
762 /* parms.parm.capture.capturemode = V4L2_MODE_IMAGE; */
763 /*when different video have the same sensor source, 1:use sensor current win, 0:find the nearest win*/
764 parms.parm.capture.reserved[0] = 0;
765 parms.parm.capture.reserved[1] = wdr_mode;/*2:command, 1: wdr, 0: normal*/
766
767 if (-1 == ioctl(fd, VIDIOC_S_PARM, &parms)) {
768 printf("VIDIOC_S_PARM error\n");
769 return -1;
770 }
771 CLEAR(sensor_isp_cfg);
772 sensor_isp_cfg.isp_wdr_mode = wdr_mode;/*2:command, 1: wdr, 0: normal*/
773 if (-1 == ioctl(fd, VIDIOC_SET_SENSOR_ISP_CFG, &sensor_isp_cfg)) {
774 printf("VIDIOC_SET_SENSOR_ISP_CFG error\n");
775 }
776
777 return 0;
778 }
779
camera_fmt_set(int mode)780 static int camera_fmt_set(int mode)
781 {
782 struct v4l2_format fmt;
783
784 CLEAR(fmt);
785 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
786 fmt.fmt.pix_mp.width = input_size.width;
787 fmt.fmt.pix_mp.height = input_size.height;
788 switch (mode) {
789 case 0:
790 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_SBGGR8;
791 break;
792 case 1:
793 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_YUV420M;
794 break;
795 case 2:
796 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_YUV420;
797 break;
798 case 3:
799 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12M;
800 break;
801 case 4:
802 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12;
803 break;
804 case 5:
805 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_SBGGR10;
806 break;
807 case 6:
808 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_SBGGR12;
809 break;
810 case 7:
811 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_LBC_2_5X;
812 break;
813 default:
814 fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_YUV420M;
815 break;
816 }
817 fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
818
819 if (-1 == ioctl(fd, VIDIOC_S_FMT, &fmt)) {
820 printf("VIDIOC_S_FMT error!\n");
821 return -1;
822 }
823
824 if (-1 == ioctl(fd, VIDIOC_G_FMT, &fmt)) {
825 printf("VIDIOC_G_FMT error!\n");
826 return -1;
827 } else {
828 nplanes = fmt.fmt.pix_mp.num_planes;
829 printf("resolution got from sensor = %d*%d num_planes = %d\n",
830 fmt.fmt.pix_mp.width, fmt.fmt.pix_mp.height,
831 fmt.fmt.pix_mp.num_planes);
832 }
833
834 return 0;
835 }
836
video_set_control(int cmd,int value)837 static int video_set_control(int cmd, int value)
838 {
839 struct v4l2_control control;
840
841 control.id = cmd;
842 control.value = value;
843 if (-1 == ioctl(fd, VIDIOC_S_CTRL, &control)) {
844 printf("VIDIOC_S_CTRL failed\n");
845 return -1;
846 }
847 return 0;
848 }
849
video_get_control(int cmd)850 static int video_get_control(int cmd)
851 {
852 struct v4l2_control control;
853
854 control.id = cmd;
855 if (-1 == ioctl(fd, VIDIOC_G_CTRL, &control)) {
856 printf("VIDIOC_G_CTRL failed\n");
857 return -1;
858 }
859 return control.value;
860 }
861
main_test(int sel,int mode)862 static int main_test(int sel, int mode)
863 {
864 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
865 struct v4l2_ext_control ctrls[4];
866 struct v4l2_ext_controls ext_ctrls;
867 struct v4l2_control control;
868 unsigned int pixformat;
869 int ret;
870 int i, j = 0;
871
872 if (-1 == camera_init(sel, mode))
873 return -1;
874 if (-1 == camera_fmt_set(mode))
875 return -1;
876 if (-1 == req_frame_buffers())
877 return -1;
878
879 pixformat = TVD_PL_YUV420;
880 ret = disp_init(input_size.width, input_size.height, pixformat);
881
882 if (-1 == ioctl(fd, VIDIOC_STREAMON, &type)) {
883 printf("VIDIOC_STREAMON failed\n");
884 return -1;
885 } else
886 printf("VIDIOC_STREAMON ok\n");
887
888 count = read_num;
889 while (count-- > 0) {
890 for (;;) {
891 fd_set fds;
892 struct timeval tv;
893 int r;
894
895 FD_ZERO(&fds);
896 FD_SET(fd, &fds);
897
898 tv.tv_sec = 2; /* Timeout. */
899 tv.tv_usec = 0;
900 #if video_s_ctrl
901 if (count%3 == 0) {
902 if (j == 0) {
903 video_set_control(V4L2_CID_VFLIP, 0);
904 video_set_control(V4L2_CID_HFLIP, 0);
905 j = 1;
906 printf("V4L2_CID_VFLIP done, j = %d, count = %d\n", j, count);
907 } else {
908 video_set_control(V4L2_CID_VFLIP, 1);
909 video_set_control(V4L2_CID_HFLIP, 1);
910 j = 0;
911 printf("V4L2_CID_VFLIP no done, j = %d, count = %d\n", j, count);
912 }
913 }
914 #endif
915 #ifdef SUBDEV_TEST
916 for (i = 0; i < 4; i++) {
917 ctrls[i].id = V4L2_CID_R_GAIN + i;
918 ctrls[i].value = count % 256;
919 }
920 memset(&ext_ctrls, 0, sizeof(ext_ctrls));
921 ext_ctrls.ctrl_class = V4L2_CID_R_GAIN;
922 ext_ctrls.count = 4;
923 ext_ctrls.controls = ctrls;
924 ioctl(isp0_fd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);
925
926 for (i = 0; i < 4; i++) {
927 ctrls[i].id = V4L2_CID_AE_WIN_X1 + i;
928 ctrls[i].value = count*16 % 256;
929 }
930 memset(&ext_ctrls, 0, sizeof(ext_ctrls));
931 ext_ctrls.ctrl_class = V4L2_CID_AE_WIN_X1;
932 ext_ctrls.count = 4;
933 ext_ctrls.controls = ctrls;
934 ioctl(isp0_fd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);
935
936 for (i = 0; i < 4; i++) {
937 ctrls[i].id = V4L2_CID_AF_WIN_X1 + i;
938 ctrls[i].value = count*16 % 256;
939 }
940 memset(&ext_ctrls, 0, sizeof(ext_ctrls));
941 ext_ctrls.ctrl_class = V4L2_CID_AF_WIN_X1;
942 ext_ctrls.count = 4;
943 ext_ctrls.controls = ctrls;
944 ioctl(isp0_fd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);
945
946 if (count == read_num / 4) {
947 control.id = V4L2_CID_VFLIP;
948 control.value = 1;
949 if (-1 == ioctl(fd, VIDIOC_S_CTRL, &control)) {
950 printf("VIDIOC_S_CTRL failed\n");
951 return -1;
952 } else
953 printf("VIDIOC_S_CTRL ok\n");
954 }
955
956 if (count == read_num / 2) {
957 control.id = V4L2_CID_HFLIP;
958 control.value = 1;
959 if (-1 == ioctl(fd, VIDIOC_S_CTRL, &control)) {
960 printf("VIDIOC_S_CTRL failed\n");
961 return -1;
962 } else
963 printf("VIDIOC_S_CTRL ok\n");
964 }
965 #endif
966
967 r = select(fd + 1, &fds, NULL, NULL, &tv);
968
969 if (-1 == r) {
970 if (errno == EINTR)
971 continue;
972 printf("select err\n");
973 }
974 if (r == 0) {
975 fprintf(stderr, "select timeout\n");
976 #ifdef TIMEOUT
977 if (-1 == ioctl(fd, VIDIOC_STREAMOFF, &type))
978 printf("VIDIOC_STREAMOFF failed\n");
979 else
980 printf("VIDIOC_STREAMOFF ok\n");
981 free_frame_buffers();
982 return -1;
983 #else
984 continue;
985 #endif
986 }
987
988 if (!read_frame(mode))
989 break;
990 else
991 return -1;
992 }
993 }
994 disp_disable();
995 usleep(20*1000);
996
997 if (-1 == ioctl(fd, VIDIOC_STREAMOFF, &type)) {
998 printf("VIDIOC_STREAMOFF failed\n");
999 return -1;
1000 } else
1001 printf("VIDIOC_STREAMOFF ok\n");
1002
1003 if (-1 == free_frame_buffers())
1004 return -1;
1005 #if SUBDEV_TEST
1006 close(isp0_fd);
1007 close(isp1_fd);
1008 #endif
1009 return 0;
1010 }
1011
main(int argc,char * argv[])1012 int main(int argc, char *argv[])
1013 {
1014 int i, test_cnt = 1;
1015 int sel = 0;
1016 int width = 640;
1017 int height = 480;
1018 int mode = 1;
1019 struct timeval tv1, tv2;
1020 float tv;
1021
1022 install_sig_handler();
1023
1024 CLEAR(dev_name);
1025 CLEAR(path_name);
1026 if (argc == 1) {
1027 sprintf(dev_name, "/dev/video0");
1028 sprintf(path_name, "/mnt/sdcard");
1029 } else if (argc == 3) {
1030 dev_id = atoi(argv[1]);
1031 sprintf(dev_name, "/dev/video%d", dev_id);
1032 sel = atoi(argv[2]);
1033 sprintf(path_name, "/mnt/sdcard");
1034 } else if (argc == 5) {
1035 dev_id = atoi(argv[1]);
1036 sprintf(dev_name, "/dev/video%d", dev_id);
1037 sel = atoi(argv[2]);
1038 width = atoi(argv[3]);
1039 height = atoi(argv[4]);
1040 sprintf(path_name, "/mnt/sdcard");
1041 } else if (argc == 6) {
1042 dev_id = atoi(argv[1]);
1043 sprintf(dev_name, "/dev/video%d", dev_id);
1044 sel = atoi(argv[2]);
1045 width = atoi(argv[3]);
1046 height = atoi(argv[4]);
1047 sprintf(path_name, "%s", argv[5]);
1048 } else if (argc == 7) {
1049 dev_id = atoi(argv[1]);
1050 sprintf(dev_name, "/dev/video%d", dev_id);
1051 sel = atoi(argv[2]);
1052 width = atoi(argv[3]);
1053 height = atoi(argv[4]);
1054 sprintf(path_name, "%s", argv[5]);
1055 mode = atoi(argv[6]);
1056 } else if (argc == 8) {
1057 dev_id = atoi(argv[1]);
1058 sprintf(dev_name, "/dev/video%d", dev_id);
1059 sel = atoi(argv[2]);
1060 width = atoi(argv[3]);
1061 height = atoi(argv[4]);
1062 sprintf(path_name, "%s", argv[5]);
1063 mode = atoi(argv[6]);
1064 test_cnt = atoi(argv[7]);
1065 } else if (argc == 9) {
1066 dev_id = atoi(argv[1]);
1067 sprintf(dev_name, "/dev/video%d", dev_id);
1068 sel = atoi(argv[2]);
1069 width = atoi(argv[3]);
1070 height = atoi(argv[4]);
1071 sprintf(path_name, "%s", argv[5]);
1072 mode = atoi(argv[6]);
1073 test_cnt = atoi(argv[7]);
1074 fps = atoi(argv[8]);
1075 } else if (argc == 10) {
1076 dev_id = atoi(argv[1]);
1077 sprintf(dev_name, "/dev/video%d", dev_id);
1078 sel = atoi(argv[2]);
1079 width = atoi(argv[3]);
1080 height = atoi(argv[4]);
1081 sprintf(path_name, "%s", argv[5]);
1082 mode = atoi(argv[6]);
1083 test_cnt = atoi(argv[7]);
1084 fps = atoi(argv[8]);
1085 wdr_mode = atoi(argv[9]);
1086 } else {
1087 printf("please select the video device: 0-video0 1-video1 ......\n");
1088 scanf("%d", &dev_id);
1089 sprintf(dev_name, "/dev/video%d", dev_id);
1090
1091 printf("please select the camera: 0-dev0 1-dev1 ......\n");
1092 scanf("%d", &sel);
1093
1094 printf("please input the resolution: width height......\n");
1095 scanf("%d %d", &width, &height);
1096
1097 printf("please input the frame saving path......\n");
1098 scanf("%15s", path_name);
1099
1100 printf("please input the test mode: 0~3......\n");
1101 scanf("%d", &mode);
1102
1103 printf("please input the test_cnt: >=1......\n");
1104 scanf("%d", &test_cnt);
1105 }
1106
1107 input_size.width = width;
1108 input_size.height = height;
1109
1110 if (test_cnt < read_num) {
1111 read_num = test_cnt;
1112 save_flag = 0;
1113 test_cnt = 1;
1114 } else if (test_cnt < 1000) {
1115 read_num = test_cnt;
1116 /*if output is raw then save one frame*/
1117 if (mode < 4)
1118 save_flag = 1;
1119 else
1120 save_flag = 0;
1121 test_cnt = 1;
1122 } else if (test_cnt < 10000) {
1123 read_num = test_cnt;
1124 save_flag = 3;
1125 test_cnt = 10;
1126 } else {
1127 read_num = test_cnt;
1128 save_flag = 2;
1129 test_cnt = 1;
1130 }
1131
1132 for (i = 0; i < test_cnt; i++) {
1133 gettimeofday(&tv1, NULL);
1134 if (0 == main_test(sel, mode))
1135 printf("mode %d test done at the %d time!!\n", mode, i);
1136 else
1137 printf("mode %d test failed at the %d time!!\n", mode, i);
1138 close(fd);
1139 gettimeofday(&tv2, NULL);
1140 tv = (float)((tv2.tv_sec - tv1.tv_sec) * 1000000 + tv2.tv_usec - tv1.tv_usec) / 1000000;
1141 printf("time cost %f(s)\n", tv);
1142 }
1143 return 0;
1144 }
1145