00001 /* 00002 * V4L2 video capture example 00003 * 00004 * This program can be used and distributed without restrictions. 00005 */ 00006 00007 #include <stdio.h> 00008 00009 #include "capture.h" 00010 00011 int RGB_Y_tab[256]; 00012 int B_U_tab[256]; 00013 int G_U_tab[256]; 00014 int G_V_tab[256]; 00015 int R_V_tab[256]; 00016 const int BITS_OUT = 16; 00017 char* dev_name = "/dev/video0"; 00018 io_method io = IO_METHOD_MMAP; 00019 //io_method io = IO_METHOD_READ; 00020 int fd = -1; 00021 buffer* buffers = NULL; 00022 unsigned int n_buffers = 0; 00023 //int width = 160, height = 120; 00024 int width = 320, height = 240; 00025 frame* currentFrame = NULL; 00026 00027 void errno_exit(const char *s) 00028 { 00029 printf ("%s error %d, %s\n", 00030 s, errno, strerror (errno)); 00031 exit (0); 00032 } 00033 00034 int xioctl(int fd, int request, void *arg) 00035 { 00036 int r; 00037 00038 do r = ioctl (fd, request, arg); 00039 while (-1 == r && EINTR == errno); 00040 00041 return r; 00042 } 00043 00044 void colorspace_init() 00045 { 00046 const int VIDEOYUV_Y_OFFSET = 16; 00047 const int VIDEOYUV_Y_RANGE = 219; // Y range = [16,235] 00048 const int VIDEOYUV_UV_OFFSET = 128; 00049 const int VIDEOYUV_UV_RANGE = 224; // UV range = [16,240] 00050 00051 const double JPEGYUV_RGB_Y = 1.0; 00052 const double JPEGYUV_R_V = 1.402; 00053 const double JPEGYUV_G_U = -0.34414; 00054 const double JPEGYUV_G_V = -0.71414; 00055 const double JPEGYUV_B_U = 1.772; 00056 00057 const double VIDEOYUV_RGB_Y = JPEGYUV_RGB_Y * (255.0/VIDEOYUV_Y_RANGE); 00058 const double VIDEOYUV_R_V = JPEGYUV_R_V * (255.0/VIDEOYUV_UV_RANGE); 00059 const double VIDEOYUV_G_U = JPEGYUV_G_U * (255.0/VIDEOYUV_UV_RANGE); 00060 const double VIDEOYUV_G_V = JPEGYUV_G_V * (255.0/VIDEOYUV_UV_RANGE); 00061 const double VIDEOYUV_B_U = JPEGYUV_B_U * (255.0/VIDEOYUV_UV_RANGE); 00062 00063 00064 // so that we get proper rounding in fixed-point integer arithmetic: 00065 const int half = 00066 BITS_OUT > 0 00067 ? 1<<(BITS_OUT-1) 00068 : 0; 00069 00070 const int scale = 1<<BITS_OUT; 00071 00072 int i; 00073 for (i = 0; i < 256; i++) 00074 { 00075 const int y = i-VIDEOYUV_Y_OFFSET; 00076 const int uv = i-VIDEOYUV_UV_OFFSET; 00077 00078 RGB_Y_tab[i] = half+(int)(0.5 + y * VIDEOYUV_RGB_Y * scale); 00079 R_V_tab[i] = (int)(0.5 + uv * VIDEOYUV_R_V * scale); 00080 // FIXME should probably have -0.5 instead of +0.5 here and 00081 // flip the sign of G_U_tab and G_V_tab: 00082 G_U_tab[i] = (int)(0.5 - uv * VIDEOYUV_G_U * scale); 00083 G_V_tab[i] = (int)(0.5 - uv * VIDEOYUV_G_V * scale); 00084 B_U_tab[i] = (int)(0.5 + uv * VIDEOYUV_B_U * scale); 00085 } 00086 } 00087 00088 00089 void yuv422_to_rgb24_c(unsigned char* dst, 00090 const int w, const int h, 00091 const unsigned char* yuv422ptr, 00092 const int byteswap) 00093 { 00094 int i, j; 00095 if (byteswap) 00096 for (j = 0; j < h; ++j) 00097 for (i = 0; i < w; i += 2) 00098 { 00099 // we have 2 luminance pixels per chroma pair 00100 00101 const unsigned char y1 = yuv422ptr[0]; 00102 const unsigned char u = yuv422ptr[1]; 00103 const unsigned char y2 = yuv422ptr[2]; 00104 const unsigned char v = yuv422ptr[3]; 00105 00106 yuv422ptr += 4; 00107 00108 const int r_v = R_V_tab[v]; 00109 const int g_uv = - G_U_tab[u] - G_V_tab[v]; 00110 const int b_u = B_U_tab[u]; 00111 00112 // first luminance pixel: 00113 const int rgb_y1 = RGB_Y_tab[y1]; 00114 00115 *dst++ = (rgb_y1 + r_v) >> BITS_OUT; 00116 *dst++ = (rgb_y1 + g_uv) >> BITS_OUT; 00117 *dst++ = (rgb_y1 + b_u) >> BITS_OUT; 00118 00119 // second luminance pixel: 00120 const int rgb_y2 = RGB_Y_tab[y2]; 00121 00122 *dst++ = (rgb_y2 + r_v) >> BITS_OUT; 00123 *dst++ = (rgb_y2 + g_uv) >> BITS_OUT; 00124 *dst++ = (rgb_y2 + b_u) >> BITS_OUT; 00125 } 00126 00127 else // no unsigned charswap 00128 for ( j = 0; j < h; ++j) 00129 for ( i = 0; i < w; i += 2) 00130 { 00131 // we have 2 luminance pixels per chroma pair 00132 00133 const unsigned char y1 = yuv422ptr[1]; 00134 const unsigned char u = yuv422ptr[0]; 00135 const unsigned char y2 = yuv422ptr[3]; 00136 const unsigned char v = yuv422ptr[2]; 00137 00138 yuv422ptr += 4; 00139 00140 const int r_v = R_V_tab[v]; 00141 const int g_uv = - G_U_tab[u] - G_V_tab[v]; 00142 const int b_u = B_U_tab[u]; 00143 00144 // first luminance pixel: 00145 const int rgb_y1 = RGB_Y_tab[y1]; 00146 00147 *dst++ = (rgb_y1 + r_v) >> BITS_OUT; 00148 *dst++ = (rgb_y1 + g_uv) >> BITS_OUT; 00149 *dst++ = (rgb_y1 + b_u) >> BITS_OUT; 00150 00151 // second luminance pixel: 00152 const int rgb_y2 = RGB_Y_tab[y2]; 00153 00154 *dst++ = (rgb_y2 + r_v) >> BITS_OUT; 00155 *dst++ = (rgb_y2 + g_uv) >> BITS_OUT; 00156 *dst++ = (rgb_y2 + b_u) >> BITS_OUT; 00157 } 00158 } 00159 00160 unsigned char clamp(int d) 00161 { 00162 int r = d; 00163 if (r > 255) r = 255; 00164 if (r < 0) r = 0; 00165 00166 return r; 00167 } 00168 00169 void yv12_to_rgb24_c(unsigned char* dst, 00170 int dst_stride, 00171 const unsigned char* y_src, 00172 const unsigned char* u_src, 00173 const unsigned char* v_src, 00174 int y_stride, 00175 int uv_stride, 00176 int width, 00177 int height) 00178 { 00179 00180 if (width & 1) 00181 printf("width must be even\n"); 00182 00183 if (height & 1) 00184 printf("height must be even\n"); 00185 00186 const int dst_dif = 6 * dst_stride - 3 * width; 00187 int y_dif = 2 * y_stride - width; 00188 00189 unsigned char* dst2 = dst + 3 * dst_stride; 00190 const unsigned char* y_src2 = y_src + y_stride; 00191 00192 if (height < 0) { /* flip image? */ 00193 height = -height; 00194 y_src += (height - 1) * y_stride; 00195 y_src2 = y_src - y_stride; 00196 u_src += (height / 2 - 1) * uv_stride; 00197 v_src += (height / 2 - 1) * uv_stride; 00198 y_dif = -width - 2 * y_stride; 00199 uv_stride = -uv_stride; 00200 } 00201 00202 int x,y; 00203 for (y = height / 2; y; y--) { 00204 for (x = 0; x < width / 2; x++) { 00205 const int u = u_src[x]; 00206 const int v = v_src[x]; 00207 00208 const int r_v = R_V_tab[v]; 00209 const int g_uv = - G_U_tab[u] - G_V_tab[v]; 00210 const int b_u = B_U_tab[u]; 00211 00212 { 00213 const int rgb_y = RGB_Y_tab[*y_src]; 00214 const int r = (rgb_y + r_v) >> BITS_OUT; 00215 const int g = (rgb_y + g_uv) >> BITS_OUT; 00216 const int b = (rgb_y + b_u) >> BITS_OUT; 00217 dst[0] = clamp(r); 00218 dst[1] = clamp(g); 00219 dst[2] = clamp(b); 00220 y_src++; 00221 } 00222 { 00223 const int rgb_y = RGB_Y_tab[*y_src]; 00224 const int r = (rgb_y + r_v) >> BITS_OUT; 00225 const int g = (rgb_y + g_uv) >> BITS_OUT; 00226 const int b = (rgb_y + b_u) >> BITS_OUT; 00227 dst[3] = clamp(r); 00228 dst[4] = clamp(g); 00229 dst[5] = clamp(b); 00230 y_src++; 00231 } 00232 { 00233 const int rgb_y = RGB_Y_tab[*y_src2]; 00234 const int r = (rgb_y + r_v) >> BITS_OUT; 00235 const int g = (rgb_y + g_uv) >> BITS_OUT; 00236 const int b = (rgb_y + b_u) >> BITS_OUT; 00237 dst2[0] = clamp(r); 00238 dst2[1] = clamp(g); 00239 dst2[2] = clamp(b); 00240 y_src2++; 00241 } 00242 { 00243 const int rgb_y = RGB_Y_tab[*y_src2]; 00244 const int r = (rgb_y + r_v) >> BITS_OUT; 00245 const int g = (rgb_y + g_uv) >> BITS_OUT; 00246 const int b = (rgb_y + b_u) >> BITS_OUT; 00247 dst2[3] = clamp(r); 00248 dst2[4] = clamp(g); 00249 dst2[5] = clamp(b); 00250 y_src2++; 00251 } 00252 00253 dst += 6; 00254 dst2 += 6; 00255 } 00256 00257 dst += dst_dif; 00258 dst2 += dst_dif; 00259 00260 y_src += y_dif; 00261 y_src2 += y_dif; 00262 00263 u_src += uv_stride; 00264 v_src += uv_stride; 00265 } 00266 } 00267 00268 frame* process_image(const void *p, const int len, const bool color) 00269 { 00270 00271 const int w2 = (width+1)/2; 00272 const int h2 = (height+1)/2; 00273 00274 yv12_to_rgb24_c(currentFrame->data, 00275 width, // dst_stride , 00276 (const unsigned char*)p, 00277 (const unsigned char*)p + width*height, 00278 (const unsigned char*)p + width*height + w2*h2, 00279 width, // y_stride , 00280 w2, // uv_stride , 00281 width, // image width , 00282 height); // image height 00283 00284 if (!color) 00285 { 00286 const unsigned char* sPtr = currentFrame->data; 00287 for(int i=0; i<width*height; i++) 00288 { 00289 currentFrame->lumData[i] = (*sPtr + *(sPtr+1) + *(sPtr+2))/3; 00290 sPtr += 3; 00291 } 00292 } 00293 00294 return currentFrame; 00295 } 00296 00297 int read_frame(void) 00298 { 00299 struct v4l2_buffer buf; 00300 unsigned int i; 00301 00302 switch (io) { 00303 case IO_METHOD_READ: 00304 if (-1 == (i = read (fd, buffers[0].start, buffers[0].length))) { 00305 switch (errno) { 00306 case EAGAIN: 00307 return 0; 00308 00309 case EIO: 00310 /* Could ignore EIO, see spec. */ 00311 00312 /* fall through */ 00313 00314 default: 00315 errno_exit ("read"); 00316 } 00317 } 00318 00319 process_image (buffers[0].start, buffers[0].length); 00320 00321 break; 00322 00323 case IO_METHOD_MMAP: 00324 CLEAR (buf); 00325 00326 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00327 buf.memory = V4L2_MEMORY_MMAP; 00328 00329 // wait until buffer 'itsCurrentFrame' has been fully captured: 00330 if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) { 00331 switch (errno) { 00332 case EAGAIN: 00333 return 0; 00334 00335 case EIO: 00336 /* Could ignore EIO, see spec. */ 00337 00338 /* fall through */ 00339 00340 default: 00341 errno_exit ("VIDIOC_DQBUF"); 00342 } 00343 } 00344 00345 assert (buf.index < n_buffers); 00346 00347 process_image (buffers[buf.index].start,buffers[buf.index].length ); 00348 00349 // get ready for capture of that frame again (for later): 00350 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf)) 00351 errno_exit ("VIDIOC_QBUF"); 00352 00353 break; 00354 00355 case IO_METHOD_USERPTR: 00356 CLEAR (buf); 00357 00358 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00359 buf.memory = V4L2_MEMORY_USERPTR; 00360 00361 if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) { 00362 switch (errno) { 00363 case EAGAIN: 00364 return 0; 00365 00366 case EIO: 00367 /* Could ignore EIO, see spec. */ 00368 00369 /* fall through */ 00370 00371 default: 00372 errno_exit ("VIDIOC_DQBUF"); 00373 } 00374 } 00375 00376 for (i = 0; i < n_buffers; ++i) 00377 if (buf.m.userptr == (unsigned long) buffers[i].start 00378 && buf.length == buffers[i].length) 00379 break; 00380 00381 assert (i < n_buffers); 00382 00383 process_image ((void *) buf.m.userptr, 0); 00384 00385 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf)) 00386 errno_exit ("VIDIOC_QBUF"); 00387 00388 break; 00389 } 00390 00391 return 1; 00392 } 00393 00394 frame* get_frame(bool color) 00395 { 00396 struct v4l2_buffer buf; 00397 unsigned int i; 00398 00399 switch (io) { 00400 case IO_METHOD_READ: 00401 if (-1 == (i = read (fd, buffers[0].start, buffers[0].length))) { 00402 switch (errno) { 00403 case EAGAIN: 00404 return 0; 00405 00406 case EIO: 00407 /* Could ignore EIO, see spec. */ 00408 00409 /* fall through */ 00410 00411 default: 00412 errno_exit ("read"); 00413 } 00414 } 00415 00416 return process_image (buffers[0].start, buffers[0].length, color); 00417 00418 break; 00419 00420 case IO_METHOD_MMAP: 00421 CLEAR (buf); 00422 00423 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00424 buf.memory = V4L2_MEMORY_MMAP; 00425 00426 // wait until buffer 'itsCurrentFrame' has been fully captured: 00427 if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) { 00428 switch (errno) { 00429 case EAGAIN: 00430 return 0; 00431 00432 case EIO: 00433 /* Could ignore EIO, see spec. */ 00434 return 0; 00435 00436 /* fall through */ 00437 00438 default: 00439 errno_exit ("VIDIOC_DQBUF error"); 00440 } 00441 } 00442 00443 assert (buf.index < n_buffers); 00444 00445 00446 // get ready for capture of that frame again (for later): 00447 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf)) 00448 errno_exit ("VIDIOC_QBUF"); 00449 00450 return process_image (buffers[buf.index].start,buffers[buf.index].length, color ); 00451 break; 00452 00453 case IO_METHOD_USERPTR: 00454 CLEAR (buf); 00455 00456 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00457 buf.memory = V4L2_MEMORY_USERPTR; 00458 00459 if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) { 00460 switch (errno) { 00461 case EAGAIN: 00462 return 0; 00463 00464 case EIO: 00465 /* Could ignore EIO, see spec. */ 00466 00467 /* fall through */ 00468 00469 default: 00470 errno_exit ("VIDIOC_DQBUF"); 00471 } 00472 } 00473 00474 for (i = 0; i < n_buffers; ++i) 00475 if (buf.m.userptr == (unsigned long) buffers[i].start 00476 && buf.length == buffers[i].length) 00477 break; 00478 00479 assert (i < n_buffers); 00480 00481 return process_image ((void *) buf.m.userptr, 0, color); 00482 00483 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf)) 00484 errno_exit ("VIDIOC_QBUF"); 00485 00486 break; 00487 } 00488 00489 return NULL; 00490 } 00491 00492 void mainloop(void) 00493 { 00494 unsigned int count; 00495 00496 count = 100; 00497 00498 while (count-- > 0) { 00499 for (;;) { 00500 fd_set fds; 00501 struct timeval tv; 00502 int r; 00503 00504 FD_ZERO (&fds); 00505 FD_SET (fd, &fds); 00506 00507 /* Timeout. */ 00508 tv.tv_sec = 2; 00509 tv.tv_usec = 0; 00510 00511 r = select (fd + 1, &fds, NULL, NULL, &tv); 00512 00513 if (-1 == r) { 00514 if (EINTR == errno) 00515 continue; 00516 00517 errno_exit ("select"); 00518 } 00519 00520 if (0 == r) { 00521 fprintf (stderr, "select timeout\n"); 00522 exit (EXIT_FAILURE); 00523 } 00524 00525 if (read_frame ()) 00526 break; 00527 00528 /* EAGAIN - continue select loop. */ 00529 } 00530 } 00531 } 00532 00533 void stop_capturing(void) 00534 { 00535 enum v4l2_buf_type type; 00536 00537 switch (io) { 00538 case IO_METHOD_READ: 00539 /* Nothing to do. */ 00540 break; 00541 00542 case IO_METHOD_MMAP: 00543 case IO_METHOD_USERPTR: 00544 type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00545 00546 if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type)) 00547 errno_exit ("VIDIOC_STREAMOFF"); 00548 00549 break; 00550 } 00551 } 00552 00553 void start_capturing(void) 00554 { 00555 unsigned int i; 00556 enum v4l2_buf_type type; 00557 00558 currentFrame = new frame; 00559 currentFrame->data = new unsigned char[width*height*3]; 00560 currentFrame->lumData = new unsigned char[width*height]; 00561 currentFrame->width = width; 00562 currentFrame->height = height; 00563 currentFrame->pixType=0; 00564 00565 00566 switch (io) { 00567 case IO_METHOD_READ: 00568 /* Nothing to do. */ 00569 break; 00570 00571 case IO_METHOD_MMAP: 00572 for (i = 0; i < n_buffers; ++i) { 00573 struct v4l2_buffer buf; 00574 00575 CLEAR (buf); 00576 00577 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00578 buf.memory = V4L2_MEMORY_MMAP; 00579 buf.index = i; 00580 00581 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf)) 00582 errno_exit ("VIDIOC_QBUF"); 00583 } 00584 00585 type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00586 00587 if (-1 == xioctl (fd, VIDIOC_STREAMON, &type)) 00588 errno_exit ("VIDIOC_STREAMON"); 00589 00590 break; 00591 00592 case IO_METHOD_USERPTR: 00593 for (i = 0; i < n_buffers; ++i) { 00594 struct v4l2_buffer buf; 00595 00596 CLEAR (buf); 00597 00598 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00599 buf.memory = V4L2_MEMORY_USERPTR; 00600 buf.index = i; 00601 buf.m.userptr = (unsigned long) buffers[i].start; 00602 buf.length = buffers[i].length; 00603 00604 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf)) 00605 errno_exit ("VIDIOC_QBUF"); 00606 } 00607 00608 type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00609 00610 if (-1 == xioctl (fd, VIDIOC_STREAMON, &type)) 00611 errno_exit ("VIDIOC_STREAMON"); 00612 00613 break; 00614 } 00615 } 00616 00617 void uninit_device(void) 00618 { 00619 unsigned int i; 00620 00621 switch (io) { 00622 case IO_METHOD_READ: 00623 free (buffers[0].start); 00624 break; 00625 00626 case IO_METHOD_MMAP: 00627 for (i = 0; i < n_buffers; ++i) 00628 if (-1 == munmap (buffers[i].start, buffers[i].length)) 00629 errno_exit ("munmap"); 00630 break; 00631 00632 case IO_METHOD_USERPTR: 00633 for (i = 0; i < n_buffers; ++i) 00634 free (buffers[i].start); 00635 break; 00636 } 00637 00638 if (currentFrame != NULL) 00639 { 00640 if (currentFrame->data != NULL) 00641 free(currentFrame->data); 00642 if (currentFrame->lumData != NULL) 00643 free(currentFrame->lumData); 00644 free(currentFrame); 00645 } 00646 00647 free (buffers); 00648 } 00649 00650 void init_read(unsigned int buffer_size) 00651 { 00652 buffers = (struct buffer*)calloc (1, sizeof (*buffers)); 00653 00654 if (!buffers) { 00655 fprintf (stderr, "Out of memory\n"); 00656 exit (EXIT_FAILURE); 00657 } 00658 00659 buffers[0].length = buffer_size; 00660 buffers[0].start = malloc (buffer_size); 00661 00662 if (!buffers[0].start) { 00663 fprintf (stderr, "Out of memory\n"); 00664 exit (EXIT_FAILURE); 00665 } 00666 } 00667 00668 void init_mmap(void) 00669 { 00670 struct v4l2_requestbuffers req; 00671 00672 CLEAR (req); 00673 00674 req.count = 1; 00675 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00676 req.memory = V4L2_MEMORY_MMAP; 00677 00678 if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) { 00679 if (EINVAL == errno) { 00680 fprintf (stderr, "%s does not support " 00681 "memory mapping\n", dev_name); 00682 exit (EXIT_FAILURE); 00683 } else { 00684 errno_exit ("VIDIOC_REQBUFS"); 00685 } 00686 } 00687 00688 if (req.count < 2) { 00689 fprintf (stderr, "Insufficient buffer memory on %s\n", 00690 dev_name); 00691 exit (EXIT_FAILURE); 00692 } 00693 00694 buffers = (struct buffer*)calloc (req.count, sizeof (*buffers)); 00695 00696 if (!buffers) { 00697 fprintf (stderr, "Out of memory\n"); 00698 exit (EXIT_FAILURE); 00699 } 00700 00701 for (n_buffers = 0; n_buffers < req.count; ++n_buffers) { 00702 struct v4l2_buffer buf; 00703 00704 CLEAR (buf); 00705 00706 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00707 buf.memory = V4L2_MEMORY_MMAP; 00708 buf.index = n_buffers; 00709 00710 if (-1 == xioctl (fd, VIDIOC_QUERYBUF, &buf)) 00711 errno_exit ("VIDIOC_QUERYBUF"); 00712 00713 buffers[n_buffers].length = buf.length; 00714 buffers[n_buffers].start = 00715 mmap (NULL /* start anywhere */, 00716 buf.length, 00717 PROT_READ | PROT_WRITE /* required */, 00718 MAP_SHARED /* recommended */, 00719 fd, buf.m.offset); 00720 00721 if (MAP_FAILED == buffers[n_buffers].start) 00722 errno_exit ("mmap"); 00723 } 00724 } 00725 00726 void init_userp(unsigned int buffer_size) 00727 { 00728 struct v4l2_requestbuffers req; 00729 00730 CLEAR (req); 00731 00732 req.count = 4; 00733 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00734 req.memory = V4L2_MEMORY_USERPTR; 00735 00736 if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) { 00737 if (EINVAL == errno) { 00738 fprintf (stderr, "%s does not support " 00739 "user pointer i/o\n", dev_name); 00740 exit (EXIT_FAILURE); 00741 } else { 00742 errno_exit ("VIDIOC_REQBUFS"); 00743 } 00744 } 00745 00746 buffers = (struct buffer*)calloc (4, sizeof (*buffers)); 00747 00748 if (!buffers) { 00749 fprintf (stderr, "Out of memory\n"); 00750 exit (EXIT_FAILURE); 00751 } 00752 00753 for (n_buffers = 0; n_buffers < 4; ++n_buffers) { 00754 buffers[n_buffers].length = buffer_size; 00755 buffers[n_buffers].start = malloc (buffer_size); 00756 00757 if (!buffers[n_buffers].start) { 00758 fprintf (stderr, "Out of memory\n"); 00759 exit (EXIT_FAILURE); 00760 } 00761 } 00762 } 00763 00764 void init_device(int pix_fmt) 00765 { 00766 struct v4l2_capability cap; 00767 struct v4l2_cropcap cropcap; 00768 struct v4l2_crop crop; 00769 struct v4l2_format fmt; 00770 unsigned int min; 00771 00772 colorspace_init(); 00773 00774 printf("Settting to pixformat %i\n", pix_fmt); 00775 if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap)) { 00776 if (EINVAL == errno) { 00777 fprintf (stderr, "%s is no V4L2 device\n", 00778 dev_name); 00779 exit (EXIT_FAILURE); 00780 } else { 00781 errno_exit ("VIDIOC_QUERYCAP"); 00782 } 00783 } 00784 00785 printf("V4L2 kernel driver: %s\n", cap.driver); 00786 printf("FrameGrabber board name is: %s\n", cap.card); 00787 printf("FrameGrabber board bus info: %s\n", cap.bus_info); 00788 if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) 00789 printf(" > Supports video capture\n"); 00790 else printf("Not a video capture device.\n"); 00791 if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT) 00792 printf(" > Supports video output\n"); 00793 if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY) 00794 printf(" > Supports video overlay\n"); 00795 if (cap.capabilities & V4L2_CAP_VBI_CAPTURE) 00796 printf(" > Supports raw VBI capture\n"); 00797 if (cap.capabilities & V4L2_CAP_VBI_OUTPUT) 00798 printf(" > Supports raw VBI output\n"); 00799 00800 // NOTE: older versions of V4L2 don't know about sliced VBI: 00801 #ifdef V4L2_CAP_SLICED_VBI_CAPTURE 00802 if (cap.capabilities & V4L2_CAP_SLICED_VBI_CAPTURE) 00803 printf(" > Supports sliced VBI capture\n"); 00804 #endif 00805 #ifdef V4L2_CAP_SLICED_VBI_OUTPUT 00806 if (cap.capabilities & V4L2_CAP_SLICED_VBI_OUTPUT) 00807 printf(" > Supports sliced VBI_OUTPUT\n"); 00808 #endif 00809 if (cap.capabilities & V4L2_CAP_RDS_CAPTURE) 00810 printf(" > Supports RDS capture\n"); 00811 if (cap.capabilities & V4L2_CAP_TUNER) 00812 printf(" > Has an RF tuner and/or modulator\n"); 00813 if (cap.capabilities & V4L2_CAP_AUDIO) 00814 printf(" > Supports audio input and/or output\n"); 00815 if (cap.capabilities & V4L2_CAP_READWRITE) 00816 printf(" > Supports read/write I/O method\n"); 00817 if (cap.capabilities & V4L2_CAP_ASYNCIO) 00818 printf(" > Supports asynchronous I/O method\n"); 00819 if (cap.capabilities & V4L2_CAP_STREAMING) 00820 printf(" > Supports streaming I/O (MMAP) method\n"); 00821 00822 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00823 00824 // List available controls and their current settings: 00825 struct v4l2_queryctrl ctrl; 00826 int cid; 00827 for (cid = V4L2_CID_BASE; cid < V4L2_CID_LASTP1; cid ++) 00828 { 00829 memset(&ctrl, 0, sizeof(ctrl)); 00830 ctrl.id = cid; 00831 00832 // is that control supported? 00833 if (xioctl(fd, VIDIOC_QUERYCTRL, &ctrl) == 0 && 00834 ctrl.type != 0) 00835 { 00836 // first of all, if we have a ModelParam for it, let's 00837 // attempt to set the current value: 00838 struct v4l2_control control; control.id = cid; 00839 switch(cid) 00840 { 00841 case V4L2_CID_BRIGHTNESS: 00842 control.value = 64; 00843 if (xioctl(fd, VIDIOC_S_CTRL, &control) == -1) 00844 printf("Failed to set brightness to %d (VIDIOC_S_CTRL)\n", 00845 control.value); 00846 break; 00847 case V4L2_CID_CONTRAST: 00848 control.value = 0; 00849 if (xioctl(fd, VIDIOC_S_CTRL, &control) == -1) 00850 printf("Failed to set contrast to %d (VIDIOC_S_CTRL)\n", 00851 control.value); 00852 break; 00853 case V4L2_CID_WHITENESS: // NOTE this is = V4L2_CID_GAMMA 00854 // NOTE: bttv driver supports saturation but not whiteness 00855 // NOTE: macbook supports gamma 00856 /*control.value = 50; 00857 if (xioctl(fd, VIDIOC_S_CTRL, &control) == -1) 00858 printf("Failed to set whiteness to %d (VIDIOC_S_CTRL)\n", 00859 control.value);*/ 00860 break; 00861 case V4L2_CID_SATURATION: 00862 control.value = 0; 00863 if (xioctl(fd, VIDIOC_S_CTRL, &control) == -1) 00864 printf("Failed to set saturation to %d (VIDIOC_S_CTRL)\n", 00865 control.value); 00866 break; 00867 case V4L2_CID_HUE: 00868 control.value = 32768; 00869 if (xioctl(fd, VIDIOC_S_CTRL, &control) == -1) 00870 printf("Failed to set hue to %d (VIDIOC_S_CTRL)\n", 00871 control.value); 00872 break; 00873 00874 default: break; // Ignore other controls 00875 } 00876 00877 // ok, now get the current value, silently ignoring errors: 00878 int val = -1; 00879 if (xioctl(fd, VIDIOC_G_CTRL, &control) == 0) 00880 val = control.value; 00881 00882 switch(ctrl.type) 00883 { 00884 case V4L2_CTRL_TYPE_INTEGER: 00885 printf("CONTROL: %s=%d (Def=%d, Rng=[%d .. %d])%s\n", 00886 ctrl.name, val, ctrl.default_value, 00887 ctrl.minimum, ctrl.maximum, 00888 ctrl.flags & V4L2_CTRL_FLAG_DISABLED ? 00889 " (DISABLED)" : ""); 00890 break; 00891 00892 case V4L2_CTRL_TYPE_BOOLEAN: 00893 printf("CONTROL: %s=%s (Def=%s)%s\n", 00894 ctrl.name, val ? "true" : "false", 00895 ctrl.default_value ? "true" : "false", 00896 ctrl.flags & V4L2_CTRL_FLAG_DISABLED ? 00897 " (DISABLED)" : ""); 00898 break; 00899 00900 case V4L2_CTRL_TYPE_MENU: 00901 printf("CONTROL: %s=[menu type not yet implemented]%s\n", 00902 ctrl.name, 00903 ctrl.flags & V4L2_CTRL_FLAG_DISABLED ? 00904 " (DISABLED)" : ""); 00905 break; 00906 case V4L2_CTRL_TYPE_BUTTON: 00907 printf("CONTROL: %s=[button]%s\n", 00908 ctrl.name, 00909 ctrl.flags & V4L2_CTRL_FLAG_DISABLED ? 00910 " (DISABLED)" : ""); 00911 break; 00912 00913 default: break; 00914 } 00915 } 00916 } 00917 00918 // list available channels: 00919 int i; 00920 for (i = 0; ; i++) 00921 { 00922 struct v4l2_input inp; 00923 memset(&inp, 0, sizeof(inp)); 00924 inp.index = i; 00925 if (xioctl(fd, VIDIOC_ENUMINPUT, &inp) != 0) break; 00926 printf("Video input %d is '%s'\n", i, inp.name); 00927 } 00928 00929 00930 00931 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00932 00933 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { 00934 fprintf (stderr, "%s is no video capture device\n", 00935 dev_name); 00936 exit (EXIT_FAILURE); 00937 } 00938 00939 switch (io) { 00940 case IO_METHOD_READ: 00941 if (!(cap.capabilities & V4L2_CAP_READWRITE)) { 00942 fprintf (stderr, "%s does not support read i/o\n", 00943 dev_name); 00944 exit (EXIT_FAILURE); 00945 } 00946 00947 break; 00948 00949 case IO_METHOD_MMAP: 00950 case IO_METHOD_USERPTR: 00951 if (!(cap.capabilities & V4L2_CAP_STREAMING)) { 00952 fprintf (stderr, "%s does not support streaming i/o\n", 00953 dev_name); 00954 exit (EXIT_FAILURE); 00955 } 00956 00957 break; 00958 } 00959 00960 00961 /* Select video input, video standard and tune here. */ 00962 int channel = 0; 00963 if (xioctl(fd, VIDIOC_S_INPUT, &channel) < 0) 00964 printf("Cannot select video input %d\n", channel); 00965 else 00966 printf("Selected video input %d\n", channel); 00967 00968 00969 00970 // Reset cropping parameters and get resolution capabilities. NOTE: 00971 // just open()'ing the device does not reset it, according to the 00972 // unix toolchain philosophy. Hence, although here we do not provide 00973 // support for cropping, we still need to ensure that it is properly 00974 // reset: 00975 CLEAR (cropcap); 00976 cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00977 00978 if (0 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) { 00979 printf("Video capture bounds: (%d, %d) -> (%d, %d)\n", 00980 cropcap.bounds.left, cropcap.bounds.top, 00981 cropcap.bounds.left + cropcap.bounds.width - 1, 00982 cropcap.bounds.top + cropcap.bounds.height - 1); 00983 printf("Video default capture rectangle: (%d, %d) -> (%d, %d)\n", 00984 cropcap.defrect.left, cropcap.defrect.top, 00985 cropcap.defrect.left + cropcap.defrect.width - 1, 00986 cropcap.defrect.top + cropcap.defrect.height - 1); 00987 00988 } else { 00989 printf("Failed to get cropping capabilities -- IGNORED.\n"); 00990 } 00991 00992 memset (&crop, 0, sizeof(crop)); 00993 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00994 crop.c = cropcap.defrect; 00995 if (xioctl(fd, VIDIOC_S_CROP, &crop) == -1) 00996 printf("Failed to reset cropping settings -- IGNORED.\n"); 00997 00998 00999 01000 // list available video formats and see which ones would work with 01001 // the requested resolution: 01002 CLEAR (fmt); 01003 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01004 fmt.fmt.pix.width = width; 01005 fmt.fmt.pix.height = height; 01006 fmt.fmt.pix.field = V4L2_FIELD_INTERLACED; 01007 int fmt_to_use = -1; // in case user selected "auto" format 01008 01009 for (i = 0; ; i ++) 01010 { 01011 struct v4l2_fmtdesc fdesc; 01012 memset(&fdesc, 0, sizeof(fdesc)); 01013 fdesc.index = i; 01014 fdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01015 if (xioctl(fd, VIDIOC_ENUM_FMT, &fdesc) != 0) break; 01016 unsigned int f; 01017 for (f = 0; f < (unsigned int)IROBOT_VIDFMT_AUTO; f ++) 01018 if (v4l2format[f] == fdesc.pixelformat) break; 01019 01020 // try to see whether that would work with our given image dims: 01021 fmt.fmt.pix.pixelformat = fdesc.pixelformat; 01022 int worked = 0; 01023 if (xioctl(fd, VIDIOC_TRY_FMT, &fmt) == 0) 01024 { 01025 worked = 1; 01026 if (fmt_to_use == -1) fmt_to_use = fdesc.pixelformat; 01027 } 01028 printf("Video Format: Use '%i index %i' for '%s (Fourcc: %c%c%c%c)'%s%s\n", 01029 f == IROBOT_VIDFMT_AUTO ? -1 : f, 01030 fdesc.index, 01031 fdesc.description, 01032 ((char*)(&fdesc.pixelformat))[0], 01033 ((char*)(&fdesc.pixelformat))[1], 01034 ((char*)(&fdesc.pixelformat))[2], 01035 ((char*)(&fdesc.pixelformat))[3], 01036 fdesc.flags & V4L2_FMT_FLAG_COMPRESSED ? " (COMPRESSED)" : "", 01037 worked ? " [OK]" : " [FAILED TO SET]"); 01038 } 01039 printf("Using format %i\n", fmt_to_use); 01040 01041 // Get the frame time, according to current video standard: 01042 struct v4l2_streamparm sparm; 01043 memset(&sparm, 0, sizeof(sparm)); 01044 sparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01045 if (xioctl(fd, VIDIOC_G_PARM, &sparm) == -1) 01046 { 01047 printf("Cannot get video standard params - ASSUMING NTSC\n"); 01048 } 01049 else 01050 printf("Capture at %.2f fps\n", 01051 (float)sparm.parm.capture.timeperframe.numerator/ 01052 (float)sparm.parm.capture.timeperframe.denominator); 01053 01054 //fmt.fmt.pix.pixelformat = fmt_to_use; 01055 fmt.fmt.pix.pixelformat = v4l2format[11]; 01056 01057 if (xioctl(fd, VIDIOC_S_FMT, &fmt) == -1) 01058 printf("Cannot set requested video mode/resolution %i [%dx%d], probably unsupported by hardware.", 01059 fmt_to_use, width, height); 01060 01061 if (xioctl(fd, VIDIOC_G_FMT, &fmt) == 0) 01062 printf("Video mode/resolution set to %i [%dx%d]\n", 01063 fmt_to_use, 01064 fmt.fmt.pix.width, fmt.fmt.pix.height); 01065 01066 if ((int)fmt.fmt.pix.width != width || 01067 (int)fmt.fmt.pix.height != height) 01068 printf("Hardware suggests changing input dims to %dx%d instead of " 01069 "requested %dx%d\n", 01070 fmt.fmt.pix.width, fmt.fmt.pix.height, 01071 width, height); 01072 01073 width = fmt.fmt.pix.width; 01074 height = fmt.fmt.pix.height; 01075 01076 01077 /* Note VIDIOC_S_FMT may change width and height. */ 01078 01079 switch (io) { 01080 case IO_METHOD_READ: 01081 init_read (fmt.fmt.pix.sizeimage); 01082 break; 01083 01084 case IO_METHOD_MMAP: 01085 printf("Using MMap\n"); 01086 init_mmap (); 01087 break; 01088 01089 case IO_METHOD_USERPTR: 01090 init_userp (fmt.fmt.pix.sizeimage); 01091 break; 01092 } 01093 } 01094 01095 void close_device(void) 01096 { 01097 if (-1 == close (fd)) 01098 errno_exit ("close"); 01099 01100 fd = -1; 01101 } 01102 01103 void open_device(void) 01104 { 01105 struct stat st; 01106 01107 if (-1 == stat (dev_name, &st)) { 01108 fprintf (stderr, "Cannot identify '%s': %d, %s\n", 01109 dev_name, errno, strerror (errno)); 01110 exit (EXIT_FAILURE); 01111 } 01112 01113 if (!S_ISCHR (st.st_mode)) { 01114 fprintf (stderr, "%s is no device\n", dev_name); 01115 exit (EXIT_FAILURE); 01116 } 01117 01118 fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0); 01119 01120 if (-1 == fd) { 01121 fprintf (stderr, "Cannot open '%s': %d, %s\n", 01122 dev_name, errno, strerror (errno)); 01123 exit (EXIT_FAILURE); 01124 } 01125 }