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 | |