| 320 | | */</span> |
| 321 | | <span class="cp"> |
| 322 | | #include <byteswap.h> |
| 323 | | #include <stdio.h> |
| 324 | | #include <stdlib.h> |
| 325 | | #include <stdint.h> |
| 326 | | #include <string.h> |
| 327 | | #include <assert.h> |
| 328 | | #include <fcntl.h> </span><span class="cm">/* low-level i/o */</span><span class="cp"> |
| 329 | | #include <unistd.h> |
| 330 | | #include <errno.h> |
| 331 | | #include <sys/types.h> |
| 332 | | #include <sys/mman.h> |
| 333 | | #include <sys/ioctl.h> |
| 334 | | |
| 335 | | #include <linux/videodev2.h> |
| 336 | | |
| 337 | | #define CLEAR(x) memset(&(x), 0, sizeof(x)) |
| 338 | | </span> |
| 339 | | <span class="k">struct</span> buffer <span class="p">{</span> |
| 340 | | <span class="kt">void</span> <span class="o">*</span>start<span class="p">;</span> |
| 341 | | <span class="kt">size_t</span> length<span class="p">;</span> |
| 342 | | <span class="p">};</span> |
| 343 | | |
| 344 | | <span class="k">static</span> <span class="kt">void</span> <span class="nf">errno_exit</span><span class="p">(</span><span class="k">const</span> <span class="kt">char</span> <span class="o">*</span>s<span class="p">)</span> |
| 345 | | <span class="p">{</span> |
| 346 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"%s error %d, %s</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> s<span class="p">,</span> errno<span class="p">,</span> strerror<span class="p">(</span>errno<span class="p">));</span> |
| 347 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 348 | | <span class="p">}</span> |
| 349 | | |
| 350 | | <span class="k">static</span> <span class="kt">int</span> <span class="nf">xioctl</span><span class="p">(</span><span class="kt">int</span> fh<span class="p">,</span> <span class="kt">int</span> request<span class="p">,</span> <span class="kt">void</span> <span class="o">*</span>arg<span class="p">)</span> |
| 351 | | <span class="p">{</span> |
| 352 | | <span class="kt">int</span> r<span class="p">;</span> |
| 353 | | |
| 354 | | <span class="k">do</span> <span class="p">{</span> |
| 355 | | r <span class="o">=</span> ioctl<span class="p">(</span>fh<span class="p">,</span> request<span class="p">,</span> arg<span class="p">);</span> |
| 356 | | <span class="p">}</span> <span class="k">while</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> r <span class="o">&&</span> EINTR <span class="o">==</span> errno<span class="p">);</span> |
| 357 | | |
| 358 | | <span class="k">return</span> r<span class="p">;</span> |
| 359 | | <span class="p">}</span> |
| 360 | | |
| 361 | | <span class="cm">/* 16bit/pixel interleaved YUV */</span> |
| 362 | | <span class="k">static</span> <span class="kt">void</span> <span class="nf">process_image</span><span class="p">(</span><span class="k">const</span> <span class="kt">void</span> <span class="o">*</span>_p<span class="p">,</span> <span class="k">struct</span> v4l2_pix_format <span class="o">*</span>fmt<span class="p">)</span> |
| 363 | | <span class="p">{</span> |
| 364 | | <span class="k">const</span> <span class="kt">uint8_t</span> <span class="o">*</span>p <span class="o">=</span> _p<span class="p">;</span> |
| 365 | | <span class="kt">int8_t</span> u<span class="p">;</span> |
| 366 | | <span class="kt">uint8_t</span> y1<span class="p">;</span> |
| 367 | | <span class="kt">int8_t</span> v<span class="p">;</span> |
| 368 | | <span class="kt">uint8_t</span> y2<span class="p">;</span> |
| 369 | | <span class="kt">int</span> r<span class="p">,</span> g<span class="p">,</span> b<span class="p">,</span> c<span class="p">,</span> d <span class="p">,</span>e<span class="p">;</span> |
| 370 | | <span class="kt">int</span> red<span class="p">,</span> i<span class="p">,</span> x<span class="p">,</span> y<span class="p">;</span> |
| 371 | | <span class="kt">int</span> size <span class="o">=</span> fmt<span class="o">-></span>sizeimage<span class="p">;</span> |
| 372 | | |
| 373 | | printf<span class="p">(</span><span class="s">"Processing Frame: %dx%d %c%c%c%c</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> |
| 374 | | fmt<span class="o">-></span>width<span class="p">,</span> fmt<span class="o">-></span>height<span class="p">,</span> |
| 375 | | <span class="p">(</span>fmt<span class="o">-></span>pixelformat <span class="o">>></span> <span class="mi">0</span><span class="p">)</span> <span class="o">&</span> <span class="mh">0xff</span><span class="p">,</span> |
| 376 | | <span class="p">(</span>fmt<span class="o">-></span>pixelformat <span class="o">>></span> <span class="mi">8</span><span class="p">)</span> <span class="o">&</span> <span class="mh">0xff</span><span class="p">,</span> |
| 377 | | <span class="p">(</span>fmt<span class="o">-></span>pixelformat <span class="o">>></span> <span class="mi">16</span><span class="p">)</span> <span class="o">&</span> <span class="mh">0xff</span><span class="p">,</span> |
| 378 | | <span class="p">(</span>fmt<span class="o">-></span>pixelformat <span class="o">>></span> <span class="mi">24</span><span class="p">)</span> <span class="o">&</span> <span class="mh">0xff</span><span class="p">);</span> |
| 379 | | |
| 380 | | red <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> |
| 381 | | <span class="k">for</span> <span class="p">(</span>i <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> i <span class="o"><</span> size<span class="p">;</span> i <span class="o">+=</span> <span class="mi">4</span><span class="p">)</span> <span class="p">{</span> |
| 382 | | u <span class="o">=</span> p<span class="p">[</span>i<span class="o">+</span><span class="mi">0</span><span class="p">];</span> |
| 383 | | y1 <span class="o">=</span> p<span class="p">[</span>i<span class="o">+</span><span class="mi">1</span><span class="p">];</span> |
| 384 | | v <span class="o">=</span> p<span class="p">[</span>i<span class="o">+</span><span class="mi">2</span><span class="p">];</span> |
| 385 | | y2 <span class="o">=</span> p<span class="p">[</span>i<span class="o">+</span><span class="mi">3</span><span class="p">];</span> |
| 386 | | |
| 387 | | u <span class="o">-=</span> <span class="mi">128</span><span class="p">;</span> |
| 388 | | v <span class="o">-=</span> <span class="mi">128</span><span class="p">;</span> |
| 389 | | |
| 390 | | r <span class="o">=</span> y1 <span class="o">+</span> v <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">2</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">3</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">5</span><span class="p">);</span> |
| 391 | | g <span class="o">=</span> y1 <span class="o">-</span> <span class="p">((</span>u<span class="o">>></span><span class="mi">2</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">4</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">5</span><span class="p">))</span> |
| 392 | | <span class="o">-</span> <span class="p">((</span>v<span class="o">>></span><span class="mi">1</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">3</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">4</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">5</span><span class="p">));</span> |
| 393 | | b <span class="o">=</span> y1 <span class="o">+</span> u <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">1</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">2</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">6</span><span class="p">);</span> |
| 394 | | <span class="k">if</span> <span class="p">(</span>r <span class="o">></span> <span class="mi">100</span> <span class="o">&&</span> g <span class="o"><</span> <span class="mi">60</span> <span class="o">&&</span> b <span class="o"><</span> <span class="mi">60</span><span class="p">)</span> red<span class="o">++</span><span class="p">;</span> |
| 395 | | |
| 396 | | r <span class="o">=</span> y2 <span class="o">+</span> v <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">2</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">3</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">5</span><span class="p">);</span> |
| 397 | | g <span class="o">=</span> y2 <span class="o">-</span> <span class="p">((</span>u<span class="o">>></span><span class="mi">2</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">4</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">5</span><span class="p">))</span> |
| 398 | | <span class="o">-</span> <span class="p">((</span>v<span class="o">>></span><span class="mi">1</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">3</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">4</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>v<span class="o">>></span><span class="mi">5</span><span class="p">));</span> |
| 399 | | b <span class="o">=</span> y2 <span class="o">+</span> u <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">1</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">2</span><span class="p">)</span> <span class="o">+</span> <span class="p">(</span>u<span class="o">>></span><span class="mi">6</span><span class="p">);</span> |
| 400 | | <span class="k">if</span> <span class="p">(</span>r <span class="o">></span> <span class="mi">100</span> <span class="o">&&</span> g <span class="o"><</span> <span class="mi">60</span> <span class="o">&&</span> b <span class="o"><</span> <span class="mi">60</span><span class="p">)</span> red<span class="o">++</span><span class="p">;</span> |
| 401 | | |
| 402 | | <span class="cm">/* describe pixels on first line every 250 pixels |
| | 271 | */ |
| | 272 | |
| | 273 | #include <byteswap.h> |
| | 274 | #include <stdio.h> |
| | 275 | #include <stdlib.h> |
| | 276 | #include <stdint.h> |
| | 277 | #include <string.h> |
| | 278 | #include <assert.h> |
| | 279 | #include <fcntl.h> /* low-level i/o */ |
| | 280 | #include <unistd.h> |
| | 281 | #include <errno.h> |
| | 282 | #include <sys/types.h> |
| | 283 | #include <sys/mman.h> |
| | 284 | #include <sys/ioctl.h> |
| | 285 | |
| | 286 | #include <linux/videodev2.h> |
| | 287 | |
| | 288 | #define CLEAR(x) memset(&(x), 0, sizeof(x)) |
| | 289 | |
| | 290 | struct buffer { |
| | 291 | void *start; |
| | 292 | size_t length; |
| | 293 | }; |
| | 294 | |
| | 295 | static void errno_exit(const char *s) |
| | 296 | { |
| | 297 | fprintf(stderr, "%s error %d, %s\n", s, errno, strerror(errno)); |
| | 298 | exit(EXIT_FAILURE); |
| | 299 | } |
| | 300 | |
| | 301 | static int xioctl(int fh, int request, void *arg) |
| | 302 | { |
| | 303 | int r; |
| | 304 | |
| | 305 | do { |
| | 306 | r = ioctl(fh, request, arg); |
| | 307 | } while (-1 == r && EINTR == errno); |
| | 308 | |
| | 309 | return r; |
| | 310 | } |
| | 311 | |
| | 312 | /* 16bit/pixel interleaved YUV */ |
| | 313 | static void process_image(const void *_p, struct v4l2_pix_format *fmt) |
| | 314 | { |
| | 315 | const uint8_t *p = _p; |
| | 316 | int8_t u; |
| | 317 | uint8_t y1; |
| | 318 | int8_t v; |
| | 319 | uint8_t y2; |
| | 320 | int r, g, b, c, d ,e; |
| | 321 | int red, i, x, y; |
| | 322 | int size = fmt->sizeimage; |
| | 323 | |
| | 324 | printf("Processing Frame: %dx%d %c%c%c%c\n", |
| | 325 | fmt->width, fmt->height, |
| | 326 | (fmt->pixelformat >> 0) & 0xff, |
| | 327 | (fmt->pixelformat >> 8) & 0xff, |
| | 328 | (fmt->pixelformat >> 16) & 0xff, |
| | 329 | (fmt->pixelformat >> 24) & 0xff); |
| | 330 | |
| | 331 | red = 0; |
| | 332 | for (i = 0; i < size; i += 4) { |
| | 333 | u = p[i+0]; |
| | 334 | y1 = p[i+1]; |
| | 335 | v = p[i+2]; |
| | 336 | y2 = p[i+3]; |
| | 337 | |
| | 338 | u -= 128; |
| | 339 | v -= 128; |
| | 340 | |
| | 341 | r = y1 + v + (v>>2) + (v>>3) + (v>>5); |
| | 342 | g = y1 - ((u>>2) + (u>>4) + (u>>5)) |
| | 343 | - ((v>>1) + (v>>3) + (v>>4) + (v>>5)); |
| | 344 | b = y1 + u + (u>>1) + (u>>2) + (u>>6); |
| | 345 | if (r > 100 && g < 60 && b < 60) red++; |
| | 346 | |
| | 347 | r = y2 + v + (v>>2) + (v>>3) + (v>>5); |
| | 348 | g = y2 - ((u>>2) + (u>>4) + (u>>5)) |
| | 349 | - ((v>>1) + (v>>3) + (v>>4) + (v>>5)); |
| | 350 | b = y2 + u + (u>>1) + (u>>2) + (u>>6); |
| | 351 | if (r > 100 && g < 60 && b < 60) red++; |
| | 352 | |
| | 353 | /* describe pixels on first line every 250 pixels |
| 404 | | */</span> |
| 405 | | x <span class="o">=</span> <span class="p">(</span>i<span class="o">>></span><span class="mi">1</span><span class="p">)</span> <span class="o">%</span> fmt<span class="o">-></span>width<span class="p">;</span> |
| 406 | | y <span class="o">=</span> <span class="p">(</span>i<span class="o">>></span><span class="mi">1</span><span class="p">)</span> <span class="o">/</span> fmt<span class="o">-></span>height<span class="p">;</span> |
| 407 | | <span class="k">if</span> <span class="p">(</span>y <span class="o">==</span> <span class="mi">0</span> <span class="o">&&</span> <span class="o">!</span><span class="p">(</span>x <span class="o">%</span> <span class="mi">250</span><span class="p">))</span> <span class="p">{</span> |
| 408 | | printf<span class="p">(</span><span class="s">"[%4d,%4d] YUYV:0x%02x%02x%02x%02x "</span><span class="p">,</span> |
| 409 | | x<span class="p">,</span>y<span class="p">,</span>y1<span class="p">,(</span><span class="kt">uint8_t</span><span class="p">)</span>u<span class="p">,</span>y2<span class="p">,(</span><span class="kt">uint8_t</span><span class="p">)</span>v<span class="p">);</span> |
| 410 | | printf<span class="p">(</span><span class="s">"RGB:0x%02x%02x%02x</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> r<span class="p">,</span>g<span class="p">,</span>b<span class="p">);</span> |
| 411 | | <span class="p">}</span> |
| 412 | | <span class="p">}</span> |
| 413 | | printf<span class="p">(</span><span class="s">"red pixel count=%d</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> red<span class="p">);</span> |
| 414 | | <span class="p">}</span> |
| 415 | | |
| 416 | | <span class="k">static</span> <span class="kt">void</span> <span class="nf">save_frame</span><span class="p">(</span><span class="k">const</span> <span class="kt">char</span> <span class="o">*</span>path<span class="p">,</span> <span class="k">const</span> <span class="kt">void</span> <span class="o">*</span>p<span class="p">,</span> <span class="kt">int</span> size<span class="p">)</span> |
| 417 | | <span class="p">{</span> |
| 418 | | <span class="kt">int</span> fd<span class="p">,</span> rz<span class="p">;</span> |
| 419 | | |
| 420 | | fd <span class="o">=</span> open<span class="p">(</span>path<span class="p">,</span> O_WRONLY <span class="o">|</span> O_CREAT <span class="o">|</span> O_TRUNC<span class="p">,</span> <span class="mo">0755</span><span class="p">);</span> |
| 421 | | <span class="k">if</span> <span class="p">(</span>fd <span class="o"><</span> <span class="mi">0</span><span class="p">)</span> |
| 422 | | perror<span class="p">(</span><span class="s">"open"</span><span class="p">);</span> |
| 423 | | <span class="k">else</span> <span class="p">{</span> |
| 424 | | rz <span class="o">=</span> write<span class="p">(</span>fd<span class="p">,</span> p<span class="p">,</span> size<span class="p">);</span> |
| 425 | | printf<span class="p">(</span><span class="s">"Wrote %d of %d bytes to %s</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> rz<span class="p">,</span> size<span class="p">,</span> path<span class="p">);</span> |
| 426 | | close<span class="p">(</span>fd<span class="p">);</span> |
| 427 | | <span class="p">}</span> |
| 428 | | <span class="p">}</span> |
| 429 | | |
| 430 | | <span class="kt">int</span> <span class="nf">main</span><span class="p">(</span><span class="kt">int</span> argc<span class="p">,</span> <span class="kt">char</span> <span class="o">**</span>argv<span class="p">)</span> |
| 431 | | <span class="p">{</span> |
| 432 | | <span class="k">static</span> <span class="kt">char</span> <span class="o">*</span>dev_name<span class="p">;</span> |
| 433 | | <span class="kt">int</span> width<span class="p">,</span> height<span class="p">;</span> |
| 434 | | <span class="k">static</span> <span class="kt">int</span> fd <span class="o">=</span> <span class="o">-</span><span class="mi">1</span><span class="p">;</span> |
| 435 | | <span class="k">struct</span> stat st<span class="p">;</span> |
| 436 | | <span class="k">struct</span> buffer <span class="o">*</span>buffers<span class="p">;</span> |
| 437 | | <span class="k">static</span> <span class="kt">unsigned</span> <span class="kt">int</span> n_buffers<span class="p">;</span> |
| 438 | | <span class="k">enum</span> v4l2_buf_type type<span class="p">;</span> |
| 439 | | <span class="k">struct</span> v4l2_capability cap<span class="p">;</span> |
| 440 | | <span class="k">struct</span> v4l2_format fmt<span class="p">;</span> |
| 441 | | <span class="k">struct</span> v4l2_requestbuffers req<span class="p">;</span> |
| 442 | | <span class="k">struct</span> v4l2_streamparm parm<span class="p">;</span> |
| 443 | | <span class="k">struct</span> v4l2_input input<span class="p">;</span> |
| 444 | | v4l2_std_id std_id<span class="p">;</span> |
| 445 | | <span class="k">struct</span> v4l2_buffer buf<span class="p">;</span> |
| 446 | | <span class="kt">unsigned</span> <span class="kt">int</span> count<span class="p">;</span> |
| 447 | | <span class="kt">unsigned</span> <span class="kt">int</span> i<span class="p">;</span> |
| 448 | | <span class="kt">char</span> filename<span class="p">[</span><span class="mi">32</span><span class="p">];</span> |
| 449 | | |
| 450 | | <span class="cm">/* parse args */</span> |
| 451 | | <span class="k">if</span> <span class="p">(</span>argc <span class="o"><</span> <span class="mi">5</span><span class="p">)</span> <span class="p">{</span> |
| 452 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"usage: %s <device> <width> <height> <count></span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> |
| 453 | | argv<span class="p">[</span><span class="mi">0</span><span class="p">]);</span> |
| 454 | | exit<span class="p">(</span><span class="mi">1</span><span class="p">);</span> |
| 455 | | <span class="p">}</span> |
| 456 | | dev_name <span class="o">=</span> argv<span class="p">[</span><span class="mi">1</span><span class="p">];</span> |
| 457 | | width <span class="o">=</span> atoi<span class="p">(</span>argv<span class="p">[</span><span class="mi">2</span><span class="p">]);</span> |
| 458 | | height <span class="o">=</span> atoi<span class="p">(</span>argv<span class="p">[</span><span class="mi">3</span><span class="p">]);</span> |
| 459 | | count <span class="o">=</span> atoi<span class="p">(</span>argv<span class="p">[</span><span class="mi">4</span><span class="p">]);</span> |
| 460 | | |
| 461 | | <span class="cm">/* open device */</span> |
| 462 | | fd <span class="o">=</span> open<span class="p">(</span>dev_name<span class="p">,</span> O_RDWR <span class="o">|</span> O_NONBLOCK<span class="p">,</span> <span class="mi">0</span><span class="p">);</span> |
| 463 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> fd<span class="p">)</span> <span class="p">{</span> |
| 464 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"Cannot open '%s': %d, %s</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> |
| 465 | | dev_name<span class="p">,</span> errno<span class="p">,</span> strerror<span class="p">(</span>errno<span class="p">));</span> |
| 466 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 467 | | <span class="p">}</span> |
| 468 | | |
| 469 | | <span class="cm">/* get standard (wait for it to be locked onto a signal) */</span> |
| 470 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_G_STD<span class="p">,</span> <span class="o">&</span>std_id<span class="p">))</span> |
| 471 | | perror<span class="p">(</span><span class="s">"VIDIOC_G_STD"</span><span class="p">);</span> |
| 472 | | <span class="k">for</span> <span class="p">(</span>i <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> std_id <span class="o">==</span> V4L2_STD_ALL <span class="o">&&</span> i <span class="o"><</span> <span class="mi">10</span><span class="p">;</span> i<span class="o">++</span><span class="p">)</span> <span class="p">{</span> |
| 473 | | usleep<span class="p">(</span><span class="mi">100000</span><span class="p">);</span> |
| 474 | | xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_G_STD<span class="p">,</span> <span class="o">&</span>std_id<span class="p">);</span> |
| 475 | | <span class="p">}</span> |
| 476 | | <span class="cm">/* set the standard to the detected standard (this is critical for autodetect) */</span> |
| 477 | | <span class="k">if</span> <span class="p">(</span>std_id <span class="o">!=</span> V4L2_STD_UNKNOWN<span class="p">)</span> <span class="p">{</span> |
| 478 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_S_STD<span class="p">,</span> <span class="o">&</span>std_id<span class="p">))</span> |
| 479 | | perror<span class="p">(</span><span class="s">"VIDIOC_S_STD"</span><span class="p">);</span> |
| 480 | | <span class="k">if</span> <span class="p">(</span>std_id <span class="o">&</span> V4L2_STD_NTSC<span class="p">)</span> |
| 481 | | printf<span class="p">(</span><span class="s">"found NTSC TV decoder</span><span class="se">\n</span><span class="s">"</span><span class="p">);</span> |
| 482 | | <span class="k">if</span> <span class="p">(</span>std_id <span class="o">&</span> V4L2_STD_SECAM<span class="p">)</span> |
| 483 | | printf<span class="p">(</span><span class="s">"found SECAM TV decoder</span><span class="se">\n</span><span class="s">"</span><span class="p">);</span> |
| 484 | | <span class="k">if</span> <span class="p">(</span>std_id <span class="o">&</span> V4L2_STD_PAL<span class="p">)</span> |
| 485 | | printf<span class="p">(</span><span class="s">"found PAL TV decoder</span><span class="se">\n</span><span class="s">"</span><span class="p">);</span> |
| 486 | | <span class="p">}</span> |
| 487 | | |
| 488 | | <span class="cm">/* ensure device has video capture capability */</span> |
| 489 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_QUERYCAP<span class="p">,</span> <span class="o">&</span>cap<span class="p">))</span> <span class="p">{</span> |
| 490 | | <span class="k">if</span> <span class="p">(</span>EINVAL <span class="o">==</span> errno<span class="p">)</span> <span class="p">{</span> |
| 491 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"%s is no V4L2 device</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> |
| 492 | | dev_name<span class="p">);</span> |
| 493 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 494 | | <span class="p">}</span> <span class="k">else</span> <span class="p">{</span> |
| 495 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_QUERYCAP"</span><span class="p">);</span> |
| 496 | | <span class="p">}</span> |
| 497 | | <span class="p">}</span> |
| 498 | | <span class="k">if</span> <span class="p">(</span><span class="o">!</span><span class="p">(</span>cap<span class="p">.</span>capabilities <span class="o">&</span> V4L2_CAP_VIDEO_CAPTURE<span class="p">))</span> <span class="p">{</span> |
| 499 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"%s is no video capture device</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> |
| 500 | | dev_name<span class="p">);</span> |
| 501 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 502 | | <span class="p">}</span> |
| 503 | | <span class="k">if</span> <span class="p">(</span><span class="o">!</span><span class="p">(</span>cap<span class="p">.</span>capabilities <span class="o">&</span> V4L2_CAP_STREAMING<span class="p">))</span> <span class="p">{</span> |
| 504 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"%s does not support streaming i/o</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> |
| 505 | | dev_name<span class="p">);</span> |
| 506 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 507 | | <span class="p">}</span> |
| 508 | | |
| 509 | | <span class="cm">/* set video input */</span> |
| 510 | | CLEAR<span class="p">(</span>input<span class="p">);</span> |
| 511 | | input<span class="p">.</span>index <span class="o">=</span> <span class="mi">1</span><span class="p">;</span> <span class="cm">/* IMX6 v4l2 driver: input1 is CSI<->MEM */</span> |
| 512 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_S_INPUT<span class="p">,</span> <span class="o">&</span>input<span class="p">))</span> |
| 513 | | perror<span class="p">(</span><span class="s">"VIDIOC_S_INPUT"</span><span class="p">);</span> |
| 514 | | |
| 515 | | <span class="cm">/* set framerate */</span> |
| 516 | | CLEAR<span class="p">(</span>parm<span class="p">);</span> |
| 517 | | parm<span class="p">.</span>type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 518 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_S_PARM<span class="p">,</span> <span class="o">&</span>parm<span class="p">))</span> |
| 519 | | perror<span class="p">(</span><span class="s">"VIDIOC_S_PARM"</span><span class="p">);</span> |
| 520 | | |
| 521 | | <span class="cm">/* get framerate */</span> |
| 522 | | CLEAR<span class="p">(</span>parm<span class="p">);</span> |
| 523 | | parm<span class="p">.</span>type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 524 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_G_PARM<span class="p">,</span> <span class="o">&</span>parm<span class="p">))</span> |
| 525 | | perror<span class="p">(</span><span class="s">"VIDIOC_G_PARM"</span><span class="p">);</span> |
| 526 | | |
| 527 | | <span class="cm">/* set format */</span> |
| 528 | | CLEAR<span class="p">(</span>fmt<span class="p">);</span> |
| 529 | | fmt<span class="p">.</span>type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 530 | | fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>width <span class="o">=</span> width<span class="p">;</span> |
| 531 | | fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>height <span class="o">=</span> height<span class="p">;</span> |
| 532 | | fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>pixelformat <span class="o">=</span> V4L2_PIX_FMT_UYVY<span class="p">;</span> |
| 533 | | fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>field <span class="o">=</span> V4L2_FIELD_ANY<span class="p">;</span> |
| 534 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_S_FMT<span class="p">,</span> <span class="o">&</span>fmt<span class="p">))</span> |
| 535 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_S_FMT"</span><span class="p">);</span> |
| 536 | | |
| 537 | | <span class="cm">/* get and display format */</span> |
| 538 | | CLEAR<span class="p">(</span>fmt<span class="p">);</span> |
| 539 | | fmt<span class="p">.</span>type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 540 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_G_FMT<span class="p">,</span> <span class="o">&</span>fmt<span class="p">))</span> |
| 541 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_G_FMT"</span><span class="p">);</span> |
| 542 | | printf<span class="p">(</span><span class="s">"%s: %dx%d %c%c%c%c %2.2ffps</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> dev_name<span class="p">,</span> |
| 543 | | fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>width<span class="p">,</span> fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>height<span class="p">,</span> |
| 544 | | <span class="p">(</span>fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>pixelformat <span class="o">>></span> <span class="mi">0</span><span class="p">)</span> <span class="o">&</span> <span class="mh">0xff</span><span class="p">,</span> |
| 545 | | <span class="p">(</span>fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>pixelformat <span class="o">>></span> <span class="mi">8</span><span class="p">)</span> <span class="o">&</span> <span class="mh">0xff</span><span class="p">,</span> |
| 546 | | <span class="p">(</span>fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>pixelformat <span class="o">>></span> <span class="mi">16</span><span class="p">)</span> <span class="o">&</span> <span class="mh">0xff</span><span class="p">,</span> |
| 547 | | <span class="p">(</span>fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">.</span>pixelformat <span class="o">>></span> <span class="mi">24</span><span class="p">)</span> <span class="o">&</span> <span class="mh">0xff</span><span class="p">,</span> |
| 548 | | <span class="p">(</span><span class="kt">float</span><span class="p">)</span>parm<span class="p">.</span>parm<span class="p">.</span>capture<span class="p">.</span>timeperframe<span class="p">.</span>denominator <span class="o">/</span> |
| 549 | | <span class="p">(</span><span class="kt">float</span><span class="p">)</span>parm<span class="p">.</span>parm<span class="p">.</span>capture<span class="p">.</span>timeperframe<span class="p">.</span>numerator |
| 550 | | <span class="p">);</span> |
| 551 | | |
| 552 | | <span class="cm">/* request buffers */</span> |
| 553 | | CLEAR<span class="p">(</span>req<span class="p">);</span> |
| 554 | | req<span class="p">.</span>count <span class="o">=</span> <span class="mi">4</span><span class="p">;</span> |
| 555 | | req<span class="p">.</span>type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 556 | | req<span class="p">.</span>memory <span class="o">=</span> V4L2_MEMORY_MMAP<span class="p">;</span> |
| 557 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_REQBUFS<span class="p">,</span> <span class="o">&</span>req<span class="p">))</span> <span class="p">{</span> |
| 558 | | <span class="k">if</span> <span class="p">(</span>EINVAL <span class="o">==</span> errno<span class="p">)</span> <span class="p">{</span> |
| 559 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"%s does not support "</span> |
| 560 | | <span class="s">"memory mapping</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> dev_name<span class="p">);</span> |
| 561 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 562 | | <span class="p">}</span> <span class="k">else</span> <span class="p">{</span> |
| 563 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_REQBUFS"</span><span class="p">);</span> |
| 564 | | <span class="p">}</span> |
| 565 | | <span class="p">}</span> |
| 566 | | <span class="k">if</span> <span class="p">(</span>req<span class="p">.</span>count <span class="o"><</span> <span class="mi">2</span><span class="p">)</span> <span class="p">{</span> |
| 567 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"Insufficient buffer memory on %s</span><span class="se">\n</span><span class="s">"</span><span class="p">,</span> |
| 568 | | dev_name<span class="p">);</span> |
| 569 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 570 | | <span class="p">}</span> |
| 571 | | |
| 572 | | <span class="cm">/* allocate buffers */</span> |
| 573 | | buffers <span class="o">=</span> calloc<span class="p">(</span>req<span class="p">.</span>count<span class="p">,</span> <span class="k">sizeof</span><span class="p">(</span><span class="o">*</span>buffers<span class="p">));</span> |
| 574 | | <span class="k">if</span> <span class="p">(</span><span class="o">!</span>buffers<span class="p">)</span> <span class="p">{</span> |
| 575 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"Out of memory</span><span class="se">\n</span><span class="s">"</span><span class="p">);</span> |
| 576 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 577 | | <span class="p">}</span> |
| 578 | | |
| 579 | | <span class="cm">/* mmap buffers */</span> |
| 580 | | <span class="k">for</span> <span class="p">(</span>n_buffers <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> n_buffers <span class="o"><</span> req<span class="p">.</span>count<span class="p">;</span> <span class="o">++</span>n_buffers<span class="p">)</span> <span class="p">{</span> |
| 581 | | <span class="k">struct</span> v4l2_buffer buf<span class="p">;</span> |
| 582 | | |
| 583 | | CLEAR<span class="p">(</span>buf<span class="p">);</span> |
| 584 | | |
| 585 | | buf<span class="p">.</span>type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 586 | | buf<span class="p">.</span>memory <span class="o">=</span> V4L2_MEMORY_MMAP<span class="p">;</span> |
| 587 | | buf<span class="p">.</span>index <span class="o">=</span> n_buffers<span class="p">;</span> |
| 588 | | |
| 589 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_QUERYBUF<span class="p">,</span> <span class="o">&</span>buf<span class="p">))</span> |
| 590 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_QUERYBUF"</span><span class="p">);</span> |
| 591 | | |
| 592 | | buffers<span class="p">[</span>n_buffers<span class="p">].</span>length <span class="o">=</span> buf<span class="p">.</span>length<span class="p">;</span> |
| 593 | | buffers<span class="p">[</span>n_buffers<span class="p">].</span>start <span class="o">=</span> |
| 594 | | mmap<span class="p">(</span><span class="nb">NULL</span> <span class="cm">/* start anywhere */</span><span class="p">,</span> |
| 595 | | buf<span class="p">.</span>length<span class="p">,</span> |
| 596 | | PROT_READ <span class="o">|</span> PROT_WRITE <span class="cm">/* required */</span><span class="p">,</span> |
| 597 | | MAP_SHARED <span class="cm">/* recommended */</span><span class="p">,</span> |
| 598 | | fd<span class="p">,</span> buf<span class="p">.</span>m<span class="p">.</span>offset<span class="p">);</span> |
| 599 | | |
| 600 | | <span class="k">if</span> <span class="p">(</span>MAP_FAILED <span class="o">==</span> buffers<span class="p">[</span>n_buffers<span class="p">].</span>start<span class="p">)</span> |
| 601 | | errno_exit<span class="p">(</span><span class="s">"mmap"</span><span class="p">);</span> |
| 602 | | <span class="p">}</span> |
| 603 | | |
| 604 | | <span class="cm">/* queue buffers */</span> |
| 605 | | <span class="k">for</span> <span class="p">(</span>i <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> i <span class="o"><</span> n_buffers<span class="p">;</span> <span class="o">++</span>i<span class="p">)</span> <span class="p">{</span> |
| 606 | | <span class="k">struct</span> v4l2_buffer buf<span class="p">;</span> |
| 607 | | |
| 608 | | CLEAR<span class="p">(</span>buf<span class="p">);</span> |
| 609 | | buf<span class="p">.</span>type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 610 | | buf<span class="p">.</span>memory <span class="o">=</span> V4L2_MEMORY_MMAP<span class="p">;</span> |
| 611 | | buf<span class="p">.</span>index <span class="o">=</span> i<span class="p">;</span> |
| 612 | | |
| 613 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_QBUF<span class="p">,</span> <span class="o">&</span>buf<span class="p">))</span> |
| 614 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_QBUF"</span><span class="p">);</span> |
| 615 | | <span class="p">}</span> |
| 616 | | |
| 617 | | <span class="cm">/* start capture */</span> |
| 618 | | type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 619 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_STREAMON<span class="p">,</span> <span class="o">&</span>type<span class="p">))</span> |
| 620 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_STREAMON"</span><span class="p">);</span> |
| 621 | | |
| 622 | | <span class="cm">/* capture frame(s) (we throw away first incomplete frame ) */</span> |
| 623 | | <span class="k">for</span> <span class="p">(</span>i <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> i <span class="o"><</span> count <span class="o">+</span> <span class="mi">1</span><span class="p">;</span> i<span class="o">++</span><span class="p">)</span> <span class="p">{</span> |
| 624 | | <span class="k">for</span> <span class="p">(;;)</span> <span class="p">{</span> |
| 625 | | fd_set fds<span class="p">;</span> |
| 626 | | <span class="k">struct</span> timeval tv<span class="p">;</span> |
| 627 | | <span class="kt">int</span> r<span class="p">;</span> |
| 628 | | |
| 629 | | FD_ZERO<span class="p">(</span><span class="o">&</span>fds<span class="p">);</span> |
| 630 | | FD_SET<span class="p">(</span>fd<span class="p">,</span> <span class="o">&</span>fds<span class="p">);</span> |
| 631 | | |
| 632 | | <span class="cm">/* Timeout. */</span> |
| 633 | | tv<span class="p">.</span>tv_sec <span class="o">=</span> <span class="mi">2</span><span class="p">;</span> |
| 634 | | tv<span class="p">.</span>tv_usec <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> |
| 635 | | |
| 636 | | r <span class="o">=</span> select<span class="p">(</span>fd <span class="o">+</span> <span class="mi">1</span><span class="p">,</span> <span class="o">&</span>fds<span class="p">,</span> <span class="nb">NULL</span><span class="p">,</span> <span class="nb">NULL</span><span class="p">,</span> <span class="o">&</span>tv<span class="p">);</span> |
| 637 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> r<span class="p">)</span> <span class="p">{</span> |
| 638 | | <span class="k">if</span> <span class="p">(</span>EINTR <span class="o">==</span> errno<span class="p">)</span> |
| 639 | | <span class="k">continue</span><span class="p">;</span> |
| 640 | | errno_exit<span class="p">(</span><span class="s">"select"</span><span class="p">);</span> |
| 641 | | <span class="p">}</span> |
| 642 | | <span class="k">if</span> <span class="p">(</span><span class="mi">0</span> <span class="o">==</span> r<span class="p">)</span> <span class="p">{</span> |
| 643 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"select timeout</span><span class="se">\n</span><span class="s">"</span><span class="p">);</span> |
| 644 | | exit<span class="p">(</span>EXIT_FAILURE<span class="p">);</span> |
| 645 | | <span class="p">}</span> |
| 646 | | |
| 647 | | <span class="cm">/* dequeue captured buffer */</span> |
| 648 | | CLEAR<span class="p">(</span>buf<span class="p">);</span> |
| 649 | | buf<span class="p">.</span>type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 650 | | buf<span class="p">.</span>memory <span class="o">=</span> V4L2_MEMORY_MMAP<span class="p">;</span> |
| 651 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_DQBUF<span class="p">,</span> <span class="o">&</span>buf<span class="p">))</span> <span class="p">{</span> |
| 652 | | <span class="k">if</span> <span class="p">(</span>errno <span class="o">==</span> EAGAIN<span class="p">)</span> |
| 653 | | <span class="k">continue</span><span class="p">;</span> |
| 654 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_DQBUF"</span><span class="p">);</span> |
| 655 | | <span class="p">}</span> |
| 656 | | assert<span class="p">(</span>buf<span class="p">.</span>index <span class="o"><</span> n_buffers<span class="p">);</span> |
| 657 | | |
| 658 | | <span class="cm">/* skip first image as it may not be sync'd */</span> |
| 659 | | <span class="k">if</span> <span class="p">(</span>i <span class="o">></span> <span class="mi">0</span><span class="p">)</span> <span class="p">{</span> |
| 660 | | process_image<span class="p">(</span>buffers<span class="p">[</span>buf<span class="p">.</span>index<span class="p">].</span>start<span class="p">,</span> |
| 661 | | <span class="o">&</span>fmt<span class="p">.</span>fmt<span class="p">.</span>pix<span class="p">);</span> |
| 662 | | sprintf<span class="p">(</span>filename<span class="p">,</span> <span class="s">"frame%d.raw"</span><span class="p">,</span> i<span class="p">);</span> |
| 663 | | save_frame<span class="p">(</span>filename<span class="p">,</span> |
| 664 | | buffers<span class="p">[</span>buf<span class="p">.</span>index<span class="p">].</span>start<span class="p">,</span> |
| 665 | | buf<span class="p">.</span>bytesused<span class="p">);</span> |
| 666 | | <span class="p">}</span> |
| 667 | | |
| 668 | | <span class="cm">/* queue buffer */</span> |
| 669 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_QBUF<span class="p">,</span> <span class="o">&</span>buf<span class="p">))</span> |
| 670 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_QBUF"</span><span class="p">);</span> |
| 671 | | |
| 672 | | <span class="k">break</span><span class="p">;</span> |
| 673 | | <span class="p">}</span> |
| 674 | | <span class="p">}</span> |
| 675 | | |
| 676 | | <span class="cm">/* stop capture */</span> |
| 677 | | type <span class="o">=</span> V4L2_BUF_TYPE_VIDEO_CAPTURE<span class="p">;</span> |
| 678 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> xioctl<span class="p">(</span>fd<span class="p">,</span> VIDIOC_STREAMOFF<span class="p">,</span> <span class="o">&</span>type<span class="p">))</span> |
| 679 | | errno_exit<span class="p">(</span><span class="s">"VIDIOC_STREAMOFF"</span><span class="p">);</span> |
| 680 | | |
| 681 | | <span class="cm">/* unmap and free buffers */</span> |
| 682 | | <span class="k">for</span> <span class="p">(</span>i <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> i <span class="o"><</span> n_buffers<span class="p">;</span> <span class="o">++</span>i<span class="p">)</span> |
| 683 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> munmap<span class="p">(</span>buffers<span class="p">[</span>i<span class="p">].</span>start<span class="p">,</span> buffers<span class="p">[</span>i<span class="p">].</span>length<span class="p">))</span> |
| 684 | | errno_exit<span class="p">(</span><span class="s">"munmap"</span><span class="p">);</span> |
| 685 | | free<span class="p">(</span>buffers<span class="p">);</span> |
| 686 | | |
| 687 | | <span class="cm">/* close device */</span> |
| 688 | | <span class="k">if</span> <span class="p">(</span><span class="o">-</span><span class="mi">1</span> <span class="o">==</span> close<span class="p">(</span>fd<span class="p">))</span> |
| 689 | | errno_exit<span class="p">(</span><span class="s">"close"</span><span class="p">);</span> |
| 690 | | |
| 691 | | fprintf<span class="p">(</span>stderr<span class="p">,</span> <span class="s">"</span><span class="se">\n</span><span class="s">"</span><span class="p">);</span> |
| 692 | | <span class="k">return</span> <span class="mi">0</span><span class="p">;</span> |
| 693 | | <span class="p">}</span> |
| 694 | | </pre></div></li></ul><p> |
| | 355 | */ |
| | 356 | x = (i>>1) % fmt->width; |
| | 357 | y = (i>>1) / fmt->height; |
| | 358 | if (y == 0 && !(x % 250)) { |
| | 359 | printf("[%4d,%4d] YUYV:0x%02x%02x%02x%02x ", |
| | 360 | x,y,y1,(uint8_t)u,y2,(uint8_t)v); |
| | 361 | printf("RGB:0x%02x%02x%02x\n", r,g,b); |
| | 362 | } |
| | 363 | } |
| | 364 | printf("red pixel count=%d\n", red); |
| | 365 | } |
| | 366 | |
| | 367 | static void save_frame(const char *path, const void *p, int size) |
| | 368 | { |
| | 369 | int fd, rz; |
| | 370 | |
| | 371 | fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0755); |
| | 372 | if (fd < 0) |
| | 373 | perror("open"); |
| | 374 | else { |
| | 375 | rz = write(fd, p, size); |
| | 376 | printf("Wrote %d of %d bytes to %s\n", rz, size, path); |
| | 377 | close(fd); |
| | 378 | } |
| | 379 | } |
| | 380 | |
| | 381 | int main(int argc, char **argv) |
| | 382 | { |
| | 383 | static char *dev_name; |
| | 384 | int width, height; |
| | 385 | static int fd = -1; |
| | 386 | struct stat st; |
| | 387 | struct buffer *buffers; |
| | 388 | static unsigned int n_buffers; |
| | 389 | enum v4l2_buf_type type; |
| | 390 | struct v4l2_capability cap; |
| | 391 | struct v4l2_format fmt; |
| | 392 | struct v4l2_requestbuffers req; |
| | 393 | struct v4l2_streamparm parm; |
| | 394 | struct v4l2_input input; |
| | 395 | v4l2_std_id std_id; |
| | 396 | struct v4l2_buffer buf; |
| | 397 | unsigned int count; |
| | 398 | unsigned int i; |
| | 399 | char filename[32]; |
| | 400 | |
| | 401 | /* parse args */ |
| | 402 | if (argc < 5) { |
| | 403 | fprintf(stderr, "usage: %s <device> <width> <height> <count>\n", |
| | 404 | argv[0]); |
| | 405 | exit(1); |
| | 406 | } |
| | 407 | dev_name = argv[1]; |
| | 408 | width = atoi(argv[2]); |
| | 409 | height = atoi(argv[3]); |
| | 410 | count = atoi(argv[4]); |
| | 411 | |
| | 412 | /* open device */ |
| | 413 | fd = open(dev_name, O_RDWR | O_NONBLOCK, 0); |
| | 414 | if (-1 == fd) { |
| | 415 | fprintf(stderr, "Cannot open '%s': %d, %s\n", |
| | 416 | dev_name, errno, strerror(errno)); |
| | 417 | exit(EXIT_FAILURE); |
| | 418 | } |
| | 419 | |
| | 420 | /* get standard (wait for it to be locked onto a signal) */ |
| | 421 | if (-1 == xioctl(fd, VIDIOC_G_STD, &std_id)) |
| | 422 | perror("VIDIOC_G_STD"); |
| | 423 | for (i = 0; std_id == V4L2_STD_ALL && i < 10; i++) { |
| | 424 | usleep(100000); |
| | 425 | xioctl(fd, VIDIOC_G_STD, &std_id); |
| | 426 | } |
| | 427 | /* set the standard to the detected standard (this is critical for autodetect) */ |
| | 428 | if (std_id != V4L2_STD_UNKNOWN) { |
| | 429 | if (-1 == xioctl(fd, VIDIOC_S_STD, &std_id)) |
| | 430 | perror("VIDIOC_S_STD"); |
| | 431 | if (std_id & V4L2_STD_NTSC) |
| | 432 | printf("found NTSC TV decoder\n"); |
| | 433 | if (std_id & V4L2_STD_SECAM) |
| | 434 | printf("found SECAM TV decoder\n"); |
| | 435 | if (std_id & V4L2_STD_PAL) |
| | 436 | printf("found PAL TV decoder\n"); |
| | 437 | } |
| | 438 | |
| | 439 | /* ensure device has video capture capability */ |
| | 440 | if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) { |
| | 441 | if (EINVAL == errno) { |
| | 442 | fprintf(stderr, "%s is no V4L2 device\n", |
| | 443 | dev_name); |
| | 444 | exit(EXIT_FAILURE); |
| | 445 | } else { |
| | 446 | errno_exit("VIDIOC_QUERYCAP"); |
| | 447 | } |
| | 448 | } |
| | 449 | if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { |
| | 450 | fprintf(stderr, "%s is no video capture device\n", |
| | 451 | dev_name); |
| | 452 | exit(EXIT_FAILURE); |
| | 453 | } |
| | 454 | if (!(cap.capabilities & V4L2_CAP_STREAMING)) { |
| | 455 | fprintf(stderr, "%s does not support streaming i/o\n", |
| | 456 | dev_name); |
| | 457 | exit(EXIT_FAILURE); |
| | 458 | } |
| | 459 | |
| | 460 | /* set video input */ |
| | 461 | CLEAR(input); |
| | 462 | input.index = 1; /* IMX6 v4l2 driver: input1 is CSI<->MEM */ |
| | 463 | if (-1 == xioctl(fd, VIDIOC_S_INPUT, &input)) |
| | 464 | perror("VIDIOC_S_INPUT"); |
| | 465 | |
| | 466 | /* set framerate */ |
| | 467 | CLEAR(parm); |
| | 468 | parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 469 | if (-1 == xioctl(fd, VIDIOC_S_PARM, &parm)) |
| | 470 | perror("VIDIOC_S_PARM"); |
| | 471 | |
| | 472 | /* get framerate */ |
| | 473 | CLEAR(parm); |
| | 474 | parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 475 | if (-1 == xioctl(fd, VIDIOC_G_PARM, &parm)) |
| | 476 | perror("VIDIOC_G_PARM"); |
| | 477 | |
| | 478 | /* set format */ |
| | 479 | CLEAR(fmt); |
| | 480 | fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 481 | fmt.fmt.pix.width = width; |
| | 482 | fmt.fmt.pix.height = height; |
| | 483 | fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY; |
| | 484 | fmt.fmt.pix.field = V4L2_FIELD_ANY; |
| | 485 | if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt)) |
| | 486 | errno_exit("VIDIOC_S_FMT"); |
| | 487 | |
| | 488 | /* get and display format */ |
| | 489 | CLEAR(fmt); |
| | 490 | fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 491 | if (-1 == xioctl(fd, VIDIOC_G_FMT, &fmt)) |
| | 492 | errno_exit("VIDIOC_G_FMT"); |
| | 493 | printf("%s: %dx%d %c%c%c%c %2.2ffps\n", dev_name, |
| | 494 | fmt.fmt.pix.width, fmt.fmt.pix.height, |
| | 495 | (fmt.fmt.pix.pixelformat >> 0) & 0xff, |
| | 496 | (fmt.fmt.pix.pixelformat >> 8) & 0xff, |
| | 497 | (fmt.fmt.pix.pixelformat >> 16) & 0xff, |
| | 498 | (fmt.fmt.pix.pixelformat >> 24) & 0xff, |
| | 499 | (float)parm.parm.capture.timeperframe.denominator / |
| | 500 | (float)parm.parm.capture.timeperframe.numerator |
| | 501 | ); |
| | 502 | |
| | 503 | /* request buffers */ |
| | 504 | CLEAR(req); |
| | 505 | req.count = 4; |
| | 506 | req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 507 | req.memory = V4L2_MEMORY_MMAP; |
| | 508 | if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) { |
| | 509 | if (EINVAL == errno) { |
| | 510 | fprintf(stderr, "%s does not support " |
| | 511 | "memory mapping\n", dev_name); |
| | 512 | exit(EXIT_FAILURE); |
| | 513 | } else { |
| | 514 | errno_exit("VIDIOC_REQBUFS"); |
| | 515 | } |
| | 516 | } |
| | 517 | if (req.count < 2) { |
| | 518 | fprintf(stderr, "Insufficient buffer memory on %s\n", |
| | 519 | dev_name); |
| | 520 | exit(EXIT_FAILURE); |
| | 521 | } |
| | 522 | |
| | 523 | /* allocate buffers */ |
| | 524 | buffers = calloc(req.count, sizeof(*buffers)); |
| | 525 | if (!buffers) { |
| | 526 | fprintf(stderr, "Out of memory\n"); |
| | 527 | exit(EXIT_FAILURE); |
| | 528 | } |
| | 529 | |
| | 530 | /* mmap buffers */ |
| | 531 | for (n_buffers = 0; n_buffers < req.count; ++n_buffers) { |
| | 532 | struct v4l2_buffer buf; |
| | 533 | |
| | 534 | CLEAR(buf); |
| | 535 | |
| | 536 | buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 537 | buf.memory = V4L2_MEMORY_MMAP; |
| | 538 | buf.index = n_buffers; |
| | 539 | |
| | 540 | if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf)) |
| | 541 | errno_exit("VIDIOC_QUERYBUF"); |
| | 542 | |
| | 543 | buffers[n_buffers].length = buf.length; |
| | 544 | buffers[n_buffers].start = |
| | 545 | mmap(NULL /* start anywhere */, |
| | 546 | buf.length, |
| | 547 | PROT_READ | PROT_WRITE /* required */, |
| | 548 | MAP_SHARED /* recommended */, |
| | 549 | fd, buf.m.offset); |
| | 550 | |
| | 551 | if (MAP_FAILED == buffers[n_buffers].start) |
| | 552 | errno_exit("mmap"); |
| | 553 | } |
| | 554 | |
| | 555 | /* queue buffers */ |
| | 556 | for (i = 0; i < n_buffers; ++i) { |
| | 557 | struct v4l2_buffer buf; |
| | 558 | |
| | 559 | CLEAR(buf); |
| | 560 | buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 561 | buf.memory = V4L2_MEMORY_MMAP; |
| | 562 | buf.index = i; |
| | 563 | |
| | 564 | if (-1 == xioctl(fd, VIDIOC_QBUF, &buf)) |
| | 565 | errno_exit("VIDIOC_QBUF"); |
| | 566 | } |
| | 567 | |
| | 568 | /* start capture */ |
| | 569 | type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 570 | if (-1 == xioctl(fd, VIDIOC_STREAMON, &type)) |
| | 571 | errno_exit("VIDIOC_STREAMON"); |
| | 572 | |
| | 573 | /* capture frame(s) (we throw away first incomplete frame ) */ |
| | 574 | for (i = 0; i < count + 1; i++) { |
| | 575 | for (;;) { |
| | 576 | fd_set fds; |
| | 577 | struct timeval tv; |
| | 578 | int r; |
| | 579 | |
| | 580 | FD_ZERO(&fds); |
| | 581 | FD_SET(fd, &fds); |
| | 582 | |
| | 583 | /* Timeout. */ |
| | 584 | tv.tv_sec = 2; |
| | 585 | tv.tv_usec = 0; |
| | 586 | |
| | 587 | r = select(fd + 1, &fds, NULL, NULL, &tv); |
| | 588 | if (-1 == r) { |
| | 589 | if (EINTR == errno) |
| | 590 | continue; |
| | 591 | errno_exit("select"); |
| | 592 | } |
| | 593 | if (0 == r) { |
| | 594 | fprintf(stderr, "select timeout\n"); |
| | 595 | exit(EXIT_FAILURE); |
| | 596 | } |
| | 597 | |
| | 598 | /* dequeue captured buffer */ |
| | 599 | CLEAR(buf); |
| | 600 | buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 601 | buf.memory = V4L2_MEMORY_MMAP; |
| | 602 | if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf)) { |
| | 603 | if (errno == EAGAIN) |
| | 604 | continue; |
| | 605 | errno_exit("VIDIOC_DQBUF"); |
| | 606 | } |
| | 607 | assert(buf.index < n_buffers); |
| | 608 | |
| | 609 | /* skip first image as it may not be sync'd */ |
| | 610 | if (i > 0) { |
| | 611 | process_image(buffers[buf.index].start, |
| | 612 | &fmt.fmt.pix); |
| | 613 | sprintf(filename, "frame%d.raw", i); |
| | 614 | save_frame(filename, |
| | 615 | buffers[buf.index].start, |
| | 616 | buf.bytesused); |
| | 617 | } |
| | 618 | |
| | 619 | /* queue buffer */ |
| | 620 | if (-1 == xioctl(fd, VIDIOC_QBUF, &buf)) |
| | 621 | errno_exit("VIDIOC_QBUF"); |
| | 622 | |
| | 623 | break; |
| | 624 | } |
| | 625 | } |
| | 626 | |
| | 627 | /* stop capture */ |
| | 628 | type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
| | 629 | if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type)) |
| | 630 | errno_exit("VIDIOC_STREAMOFF"); |
| | 631 | |
| | 632 | /* unmap and free buffers */ |
| | 633 | for (i = 0; i < n_buffers; ++i) |
| | 634 | if (-1 == munmap(buffers[i].start, buffers[i].length)) |
| | 635 | errno_exit("munmap"); |
| | 636 | free(buffers); |
| | 637 | |
| | 638 | /* close device */ |
| | 639 | if (-1 == close(fd)) |
| | 640 | errno_exit("close"); |
| | 641 | |
| | 642 | fprintf(stderr, "\n"); |
| | 643 | return 0; |
| | 644 | } |
| | 645 | }}} |
| | 646 | |