1 /*
2 V4L2 pixfmt test
3
4 Copyright (C) 2007, 2008 Michael H. Schimek <mschimek@gmx.at>
5
6 This program is free software; you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation; either version 2 of the License, or
9 (at your option) any later version.
10
11 This program is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU General Public License for more details.
15
16 You should have received a copy of the GNU General Public License
17 along with this program; if not, write to the Free Software
18 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 */
20
21 #define _GNU_SOURCE 1
22
23 #include <config.h>
24
25 #include <stdio.h>
26 #include <stdlib.h>
27 #include <stdbool.h>
28 #include <stdarg.h>
29 #include <string.h>
30 #include <inttypes.h>
31 #include <assert.h>
32
33 #include <getopt.h> /* getopt_long() */
34
35 #include <fcntl.h> /* low-level i/o */
36 #include <unistd.h>
37 #include <errno.h>
38 #include <sys/stat.h>
39 #include <sys/types.h>
40 #include <sys/time.h>
41 #include <sys/mman.h>
42 #include <sys/ioctl.h>
43
44 #include <asm/types.h> /* for videodev2.h */
45
46 #include <linux/videodev2.h>
47
48 #include <X11/Xlib.h>
49 #include <X11/keysym.h>
50 #include <X11/Xutil.h>
51
52 #undef MAX
53 #define MAX(x, y) ({ \
54 __typeof__ (x) _x = (x); \
55 __typeof__ (y) _y = (y); \
56 (void)(&_x == &_y); /* alert when type mismatch */ \
57 (_x > _y) ? _x : _y; \
58 })
59
60 #define N_ELEMENTS(array) (sizeof (array) / sizeof ((array)[0]))
61 #define CLEAR(var) memset (&(var), 0, sizeof (var))
62
63 typedef enum {
64 /* Packed RGB formats. */
65
66 /* in memory */
67 BGRA8888_LE = 1, /* bbbbbbbb gggggggg rrrrrrrr aaaaaaaa */
68 BGRA8888_BE, /* aaaaaaaa rrrrrrrr gggggggg bbbbbbbb */
69 RGBA8888_LE, /* rrrrrrrr gggggggg bbbbbbbb aaaaaaaa */
70 RGBA8888_BE, /* aaaaaaaa bbbbbbbb gggggggg rrrrrrrr */
71
72 BGR888_LE, /* bbbbbbbb gggggggg rrrrrrrr */
73 BGR888_BE, /* rrrrrrrr gggggggg bbbbbbbb */
74
75 BGR565_LE, /* gggbbbbb rrrrrggg */
76 BGR565_BE, /* rrrrrggg gggbbbbb */
77 RGB565_LE, /* gggrrrrr bbbbbggg */
78 RGB565_BE, /* bbbbbggg gggrrrrr */
79
80 BGRA5551_LE, /* gggbbbbb arrrrrgg */
81 BGRA5551_BE, /* arrrrrgg gggbbbbb */
82 RGBA5551_LE, /* gggrrrrr abbbbbgg */
83 RGBA5551_BE, /* abbbbbgg gggrrrrr */
84
85 ABGR1555_LE, /* ggbbbbba rrrrrggg */
86 ABGR1555_BE, /* rrrrrggg ggbbbbba */
87 ARGB1555_LE, /* ggrrrrra bbbbbggg */
88 ARGB1555_BE, /* bbbbbggg ggrrrrra */
89
90 BGRA4444_LE, /* ggggbbbb aaaarrrr */
91 BGRA4444_BE, /* aaaarrrr ggggbbbb */
92 RGBA4444_LE, /* ggggrrrr aaaabbbb */
93 RGBA4444_BE, /* aaaabbbb ggggrrrr */
94
95 ABGR4444_LE, /* bbbbaaaa rrrrgggg */
96 ABGR4444_BE, /* rrrrgggg bbbbaaaa */
97 ARGB4444_LE, /* rrrraaaa bbbbgggg */
98 ARGB4444_BE, /* bbbbgggg rrrraaaa */
99
100 BGR233, /* rrrgggbb */
101 RGB332, /* bbgggrrr */
102
103 /* Bayer formats. */
104
105 BGGR8, /* bbbbbbbb gggggggg */
106 /* gggggggg rrrrrrrr */
107 GBRG8, /* gggggggg bbbbbbbb */
108 /* rrrrrrrr gggggggg */
109 RGGB8, /* rrrrrrrr gggggggg */
110 /* gggggggg bbbbbbbb */
111 GRBG8, /* gggggggg rrrrrrrr */
112 /* bbbbbbbb gggggggg */
113
114 BGGR16, /* b7...b0 b15...b8 g7...g0 g15...g8 */
115 /* g7...g0 g15...g8 r7...r0 r15...r8 */
116 GBRG16, /* g7...g0 g15...g8 b7...b0 b15...b8 */
117 /* r7...r0 r15...r8 g7...g0 g15...g8 */
118 RGGB16, /* r7...r0 r15...r8 g7...g0 g15...g8 */
119 /* g7...g0 g15...g8 b7...b0 b15...b8 */
120 GRBG16, /* g7...g0 g15...g8 r7...r0 r15...r8 */
121 /* b7...b0 b15...b8 g7...g0 g15...g8 */
122 } pixfmt;
123
124 /* A pixfmt set would be nicer, but I doubt all
125 YUV and RGB formats will fit in 64 bits. */
126 typedef enum {
127 PACKED_RGB = (1 << 0),
128 BAYER = (1 << 1)
129 } pixfmt_class;
130
131 typedef enum {
132 LE = 1,
133 BE
134 } byte_order;
135
136 typedef struct {
137 /* Our name for this format. */
138 const char * name;
139
140 /* V4L2's name "V4L2_PIX_FMT_..." or NULL. */
141 const char * v4l2_fourcc_name;
142
143 /* Our ID for this format. */
144 pixfmt pixfmt;
145
146 /* Same pixfmt with opposite byte order.
147 Applies only to packed RGB formats. */
148 pixfmt pixfmt_opposite_byte_order;
149
150 /* Same pixfmt with red and blue bits swapped.
151 Applies only to RGB formats. */
152 pixfmt pixfmt_swap_red_blue;
153
154 /* Same pixfmt with alpha bits at the other end.
155 Applies only to packed RGB formats. */
156 pixfmt pixfmt_opposite_alpha;
157
158 pixfmt_class pixfmt_class;
159
160 /* V4L2's FOURCC or 0. */
161 uint32_t v4l2_fourcc;
162
163 /* LE or BE. Applies only to packed RGB formats. */
164 byte_order byte_order;
165
166 /* Applies only to RGB formats. */
167 uint8_t bits_per_pixel;
168
169 /* Number of blue, green and red bits per pixel.
170 Applies only to RGB formats. */
171 uint8_t color_depth;
172
173 /* Blue, green, red, alpha bit masks.
174 Applies only to packed RGB formats. */
175 uint32_t mask[4];
176
177 /* Number of blue, green, red, alpha bits.
178 Applies only to packed RGB formats. */
179 uint8_t n_bits[4];
180
181 /* Number of zero bits above the blue, green, red, alpha MSB.
182 E.g. 0x80001234 -> 0, 0x00000001 -> 31, 0 -> 32.
183 Applies only to packed RGB formats. */
184 uint8_t shr[4];
185
186 } pixel_format;
187
188 /* Population count in 32 bit constant, e.g. 0x70F -> 7. */
189 #define PC32b(m) ((m) - (((m) >> 1) & 0x55555555))
190 #define PC32a(m) ((PC32b (m) & 0x33333333) + ((PC32b (m) >> 2) & 0x33333333))
191 #define PC32(m) ((((uint64_t)((PC32a (m) & 0x0F0F0F0F) \
192 + ((PC32a (m) >> 4) & 0x0F0F0F0F)) \
193 * 0x01010101) >> 24) & 0xFF)
194
195 /* Find first set bit in 32 bit constant, see man 3 ffs(). */
196 #define FFS2(m) ((m) & 0x2 ? 2 : (m))
197 #define FFS4(m) ((m) & 0xC ? 2 + FFS2 ((m) >> 2) : FFS2 (m))
198 #define FFS8(m) ((m) & 0xF0 ? 4 + FFS4 ((m) >> 4) : FFS4 (m))
199 #define FFS16(m) ((m) & 0xFF00 ? 8 + FFS8 ((m) >> 8) : FFS8 (m))
200 #define FFS32(m) ((m) & 0xFFFF0000 ? 16 + FFS16 ((m) >> 16) : FFS16 (m))
201
202 #define PF_RGB(tn, vn, pf, pfxbo, pfxrb, pfxa, vpf, bo, b, g, r, a) \
203 [pf] = { \
204 .name = tn, \
205 .v4l2_fourcc_name = (0 == vpf) ? NULL : vn, \
206 .pixfmt = pf, \
207 .pixfmt_opposite_byte_order = pfxbo, \
208 .pixfmt_swap_red_blue = pfxrb, \
209 .pixfmt_opposite_alpha = pfxa, \
210 .pixfmt_class = PACKED_RGB, \
211 .v4l2_fourcc = vpf, \
212 .byte_order = bo, \
213 .bits_per_pixel = PC32 ((b) | (g) | (r) | (a)), \
214 .color_depth = PC32 ((b) | (g) | (r)), \
215 .mask = { b, g, r, a }, \
216 .n_bits = { PC32 (b), PC32 (g), PC32 (r), PC32 (a) }, \
217 .shr = { 32 - FFS32 (b), 32 - FFS32 (g), \
218 32 - FFS32 (r), 32 - FFS32 (a) } \
219 }
220
221 #define PF_RGB8(pf, pfxrb, vpf, b, g, r, a) \
222 PF_RGB (# pf, # vpf, pf, pf, pfxrb, 0, vpf, LE, b, g, r, a)
223
224 #define PF_RGB16(fmt, bo, pfxrb, pfxa, vpf, b, g, r, a) \
225 PF_RGB (# fmt "_" # bo, # vpf, \
226 fmt ## _ ## bo, \
227 (bo == LE) ? fmt ## _ ## BE : fmt ## _ ## LE, \
228 pfxrb, pfxa, vpf, bo, b, g, r, a)
229
230 #define PF_RGB24 PF_RGB16
231 #define PF_RGB32 PF_RGB16
232
233 #define PF_BAYER(pf, pfxrb, bpp, vpf) \
234 [pf] = { \
235 .name = # pf, \
236 .v4l2_fourcc_name = (0 == vpf) ? NULL : # vpf, \
237 .pixfmt = pf, \
238 .pixfmt_opposite_byte_order = pf, \
239 .pixfmt_swap_red_blue = pfxrb, \
240 .pixfmt_opposite_alpha = pf, \
241 .pixfmt_class = BAYER, \
242 .v4l2_fourcc = vpf, \
243 .byte_order = LE, \
244 .bits_per_pixel = bpp, \
245 .color_depth = bpp * 3 /* sort of */ \
246 }
247
248 static const pixel_format
249 pixel_formats [] = {
250 PF_RGB32 (BGRA8888, LE, RGBA8888_LE, RGBA8888_BE,
251 V4L2_PIX_FMT_BGR32, 0xFF, 0xFF00, 0xFF0000, 0xFF000000),
252 PF_RGB32 (BGRA8888, BE, RGBA8888_BE, RGBA8888_LE,
253 V4L2_PIX_FMT_RGB32, 0xFF, 0xFF00, 0xFF0000, 0xFF000000),
254 PF_RGB32 (RGBA8888, LE, BGRA8888_LE, BGRA8888_BE,
255 0, 0xFF0000, 0xFF00, 0xFF, 0xFF000000),
256 PF_RGB32 (RGBA8888, BE, BGRA8888_BE, BGRA8888_LE,
257 0, 0xFF0000, 0xFF00, 0xFF, 0xFF000000),
258
259 PF_RGB24 (BGR888, LE, BGR888_BE, 0,
260 V4L2_PIX_FMT_BGR24, 0xFF, 0xFF00, 0xFF0000, 0),
261 PF_RGB24 (BGR888, BE, BGR888_LE, 0,
262 V4L2_PIX_FMT_RGB24, 0xFF, 0xFF00, 0xFF0000, 0),
263
264 PF_RGB16 (BGR565, LE, RGB565_LE, 0,
265 V4L2_PIX_FMT_RGB565, 0x001F, 0x07E0, 0xF800, 0),
266 PF_RGB16 (BGR565, BE, RGB565_BE, 0,
267 V4L2_PIX_FMT_RGB565X, 0x001F, 0x07E0, 0xF800, 0),
268 PF_RGB16 (RGB565, LE, BGR565_LE, 0, 0, 0xF800, 0x07E0, 0x001F, 0),
269 PF_RGB16 (RGB565, BE, BGR565_BE, 0, 0, 0xF800, 0x07E0, 0x001F, 0),
270
271 PF_RGB16 (BGRA5551, LE, RGBA5551_LE, ABGR1555_LE,
272 V4L2_PIX_FMT_RGB555, 0x001F, 0x03E0, 0x7C00, 0x8000),
273 PF_RGB16 (BGRA5551, BE, RGBA5551_BE, ABGR1555_BE,
274 V4L2_PIX_FMT_RGB555X, 0x001F, 0x03E0, 0x7C00, 0x8000),
275 PF_RGB16 (RGBA5551, LE, BGRA5551_LE, ARGB1555_LE,
276 0, 0x7C00, 0x03E0, 0x001F, 0x8000),
277 PF_RGB16 (RGBA5551, BE, BGRA5551_BE, ARGB1555_BE,
278 0, 0x7C00, 0x03E0, 0x001F, 0x8000),
279
280 PF_RGB16 (ABGR1555, LE, ARGB1555_LE, BGRA5551_LE,
281 0, 0x003E, 0x07C0, 0xF800, 0x0001),
282 PF_RGB16 (ABGR1555, BE, ARGB1555_BE, BGRA5551_BE,
283 0, 0x003E, 0x07C0, 0xF800, 0x0001),
284 PF_RGB16 (ARGB1555, LE, ABGR1555_LE, RGBA5551_LE,
285 0, 0xF800, 0x07C0, 0x003E, 0x0001),
286 PF_RGB16 (ARGB1555, BE, ABGR1555_BE, RGBA5551_BE,
287 0, 0xF800, 0x07C0, 0x003E, 0x0001),
288
289 PF_RGB16 (BGRA4444, LE, RGBA4444_LE, ABGR4444_LE,
290 V4L2_PIX_FMT_RGB444, 0x000F, 0x00F0, 0x0F00, 0xF000),
291 PF_RGB16 (BGRA4444, BE, RGBA4444_BE, ABGR4444_BE,
292 0, 0x000F, 0x00F0, 0x0F00, 0xF000),
293 PF_RGB16 (RGBA4444, LE, BGRA4444_LE, ARGB4444_LE,
294 0, 0x0F00, 0x00F0, 0x000F, 0xF000),
295 PF_RGB16 (RGBA4444, BE, BGRA4444_BE, ARGB4444_BE,
296 0, 0x0F00, 0x00F0, 0x000F, 0xF000),
297
298 PF_RGB16 (ABGR4444, LE, ARGB4444_LE, BGRA4444_LE,
299 0, 0x00F0, 0x0F00, 0xF000, 0x000F),
300 PF_RGB16 (ABGR4444, BE, ARGB4444_BE, BGRA4444_BE,
301 0, 0x00F0, 0x0F00, 0xF000, 0x000F),
302 PF_RGB16 (ARGB4444, LE, ABGR4444_LE, RGBA4444_LE,
303 0, 0xF000, 0x0F00, 0x00F0, 0x000F),
304 PF_RGB16 (ARGB4444, BE, ABGR4444_BE, RGBA4444_BE,
305 0, 0xF000, 0x0F00, 0x00F0, 0x000F),
306
307 PF_RGB8 (BGR233, RGB332,
308 V4L2_PIX_FMT_RGB332, 0x03, 0x1C, 0xE0, 0),
309 PF_RGB8 (RGB332, BGR233,
310 0, 0xE0, 0x1C, 0x03, 0),
311
312 PF_BAYER (BGGR8, RGGB8, 8, V4L2_PIX_FMT_SBGGR8),
313 PF_BAYER (RGGB8, BGGR8, 8, 0),
314 PF_BAYER (GBRG8, GRBG8, 8, 0),
315 PF_BAYER (GRBG8, GBRG8, 8, 0),
316
317 PF_BAYER (BGGR16, RGGB16, 16, V4L2_PIX_FMT_SBGGR16),
318 PF_BAYER (RGGB16, BGGR16, 16, 0),
319 PF_BAYER (GBRG16, GRBG16, 16, 0),
320 PF_BAYER (GRBG16, GBRG16, 16, 0),
321 };
322
323 static const pixel_format *
find_v4l2_fourcc(uint32_t fourcc)324 find_v4l2_fourcc (uint32_t fourcc)
325 {
326 const pixel_format *pf;
327
328 for (pf = pixel_formats;
329 pf < pixel_formats + N_ELEMENTS (pixel_formats); ++pf) {
330 if (fourcc == pf->v4l2_fourcc)
331 return pf;
332 }
333
334 return NULL;
335 }
336
337 static const pixel_format *
next_converter(const pixel_format * pf)338 next_converter (const pixel_format * pf)
339 {
340 const pixel_format *next_pf;
341
342 if (NULL == pf)
343 pf = pixel_formats;
344 else
345 pf = pixel_formats + pf->pixfmt;
346
347 next_pf = pf;
348
349 for (;;) {
350 if (++next_pf >= pixel_formats + N_ELEMENTS (pixel_formats))
351 next_pf = pixel_formats;
352
353 if (next_pf == pf)
354 break;
355
356 if (0 == next_pf->pixfmt)
357 continue;
358
359 if (pf->pixfmt_class == next_pf->pixfmt_class
360 && pf->bits_per_pixel == next_pf->bits_per_pixel)
361 break;
362 }
363
364 return next_pf;
365 }
366
367 typedef enum {
368 IO_METHOD_READ = 1,
369 IO_METHOD_MMAP,
370 } io_methods;
371
372 typedef struct {
373 void * start;
374 size_t length;
375 } io_buffer;
376
377 static const char * my_name;
378
379 static const char * dev_name = "/dev/video";
380
381 static int dev_fd;
382 static v4l2_std_id std_id;
383 static io_methods io_method;
384 static struct v4l2_format fmt;
385 static io_buffer * buffers;
386 static unsigned int n_buffers;
387
388 static Display * display;
389 static int screen;
390 static Window window;
391 static GC gc;
392 static Atom xa_delete_window;
393
394 static XImage * ximage;
395 static const pixel_format * ximage_pf;
396
397 static void
error_exit(const char * templ,...)398 error_exit (const char * templ,
399 ...)
400 {
401 va_list ap;
402
403 fprintf (stderr, "%s: ", my_name);
404 va_start (ap, templ);
405 vfprintf (stderr, templ, ap);
406 va_end (ap);
407
408 exit (EXIT_FAILURE);
409 }
410
411 static void
errno_exit(const char * s)412 errno_exit (const char * s)
413 {
414 error_exit ("%s error %d, %s\n",
415 s, errno, strerror (errno));
416 }
417
418 static void
write_rgb_pixel(uint8_t * dst,const pixel_format * dst_pf,unsigned int b,unsigned int g,unsigned int r,unsigned int depth)419 write_rgb_pixel (uint8_t * dst,
420 const pixel_format * dst_pf,
421 unsigned int b,
422 unsigned int g,
423 unsigned int r,
424 unsigned int depth)
425 {
426 unsigned int dst_pixel;
427 unsigned int shl;
428
429 shl = 32 - depth;
430 dst_pixel = ((b << shl) >> dst_pf->shr[0]) & dst_pf->mask[0];
431 dst_pixel |= ((g << shl) >> dst_pf->shr[1]) & dst_pf->mask[1];
432 dst_pixel |= ((r << shl) >> dst_pf->shr[2]) & dst_pf->mask[2];
433
434 switch (dst_pf->byte_order * 256 + dst_pf->bits_per_pixel) {
435 case LE * 256 + 32:
436 dst[3] = dst_pixel >> 24;
437 /* fall through */
438 case LE * 256 + 24:
439 dst[2] = dst_pixel >> 16;
440 /* fall through */
441 case LE * 256 + 16:
442 dst[1] = dst_pixel >> 8;
443 /* fall through */
444 case LE * 256 + 8:
445 dst[0] = dst_pixel;
446 break;
447
448 case BE * 256 + 32:
449 *dst++ = dst_pixel >> 24;
450 /* fall through */
451 case BE * 256 + 24:
452 *dst++ = dst_pixel >> 16;
453 /* fall through */
454 case BE * 256 + 16:
455 *dst++ = dst_pixel >> 8;
456 /* fall through */
457 case BE * 256 + 8:
458 *dst = dst_pixel;
459 break;
460
461 default:
462 assert (0);
463 break;
464 }
465 }
466
467 static void
convert_bayer8_image(uint8_t * dst,const pixel_format * dst_pf,unsigned long dst_bpl,const uint8_t * src,const pixel_format * src_pf,unsigned long src_bpl,unsigned int width,unsigned int height)468 convert_bayer8_image (uint8_t * dst,
469 const pixel_format * dst_pf,
470 unsigned long dst_bpl,
471 const uint8_t * src,
472 const pixel_format * src_pf,
473 unsigned long src_bpl,
474 unsigned int width,
475 unsigned int height)
476 {
477 unsigned long dst_padding;
478 unsigned int tile;
479 unsigned int y;
480
481 assert (PACKED_RGB == dst_pf->pixfmt_class);
482 assert (BAYER == src_pf->pixfmt_class);
483
484 assert (width >= 2 && 0 == (width & 1));
485 assert (height >= 2 && 0 == (height & 1));
486
487 dst_padding = dst_bpl - width * (dst_pf->bits_per_pixel >> 3);
488 assert ((long) dst_padding >= 0);
489
490 switch (src_pf->pixfmt) {
491 case BGGR8:
492 tile = 0;
493 break;
494
495 case GBRG8:
496 tile = 1;
497 break;
498
499 case RGGB8:
500 tile = 2;
501 break;
502
503 case GRBG8:
504 tile = 3;
505 break;
506
507 default:
508 assert (0);
509 break;
510 }
511
512 for (y = 0; y < height; ++y) {
513 const uint8_t *srcm;
514 const uint8_t *srcp;
515 unsigned int x;
516
517 srcm = srcp = src - src_bpl;
518
519 if (0 == y)
520 srcm += src_bpl * 2;
521
522 if (y != height - 1)
523 srcp += src_bpl * 2;
524
525 for (x = 0; x < width; ++x) {
526 int xm, xp;
527
528 xm = (((0 == x) - 1) | 1) + x;
529 xp = (((x != width - 1) - 1) | 1) + x;
530
531 switch (tile) {
532 case 0: /* BG
533 GR */
534 write_rgb_pixel (dst, dst_pf,
535 /* b */ src[x],
536 /* g */ (src[xm] +
537 src[xp] +
538 srcm[x] +
539 srcp[x] + 2) >> 2,
540 /* r */ (srcm[xm] +
541 srcm[xp] +
542 srcp[xm] +
543 srcp[xp] + 2) >> 2,
544 /* depth */ 8);
545 break;
546
547 case 1: /* GB
548 RG */
549 write_rgb_pixel (dst, dst_pf,
550 /* b */ (src[xm] +
551 src[xp] + 1) >> 1,
552 /* g */ src[x],
553 /* r */ (srcm[x] +
554 srcp[x] + 1) >> 1,
555 /* depth */ 8);
556 break;
557
558 case 2: /* GR
559 BG */
560 write_rgb_pixel (dst, dst_pf,
561 /* b */ (srcm[x] +
562 srcp[x] + 1) >> 1,
563 /* g */ src[x],
564 /* r */ (src[xm] +
565 src[xp] + 1) >> 1,
566 /* depth */ 8);
567 break;
568
569 case 3: /* RG
570 GB */
571 write_rgb_pixel (dst, dst_pf,
572 /* b */ (srcm[xm] +
573 srcm[xp] +
574 srcp[xm] +
575 srcp[xp] + 2) >> 2,
576 /* g */ (src[xm] +
577 src[xp] +
578 srcm[x] +
579 srcp[x] + 2) >> 2,
580 /* r */ src[x],
581 /* depth */ 8);
582 break;
583
584 default:
585 assert (0);
586 break;
587 }
588
589 tile ^= 1;
590
591 dst += dst_pf->bits_per_pixel >> 3;
592 }
593
594 tile ^= 2;
595
596 dst += dst_padding;
597 src += src_bpl;
598 }
599 }
600
601 static void
convert_bayer16_image(uint8_t * dst,const pixel_format * dst_pf,unsigned long dst_bpl,const uint16_t * src,const pixel_format * src_pf,unsigned long src_bpl,unsigned int width,unsigned int height)602 convert_bayer16_image (uint8_t * dst,
603 const pixel_format * dst_pf,
604 unsigned long dst_bpl,
605 const uint16_t * src,
606 const pixel_format * src_pf,
607 unsigned long src_bpl,
608 unsigned int width,
609 unsigned int height)
610 {
611 unsigned long dst_padding;
612 unsigned int tile;
613 unsigned int y;
614
615 assert (PACKED_RGB == dst_pf->pixfmt_class);
616 assert (BAYER == src_pf->pixfmt_class);
617
618 assert (width >= 2 && 0 == (width & 1));
619 assert (height >= 2 && 0 == (height & 1));
620
621 dst_padding = dst_bpl - width * (dst_pf->bits_per_pixel >> 3);
622 assert ((long) dst_padding >= 0);
623
624 switch (src_pf->pixfmt) {
625 case BGGR16:
626 tile = 0;
627 break;
628
629 case GBRG16:
630 tile = 1;
631 break;
632
633 case RGGB16:
634 tile = 2;
635 break;
636
637 case GRBG16:
638 tile = 3;
639 break;
640
641 default:
642 assert (0);
643 break;
644 }
645
646 for (y = 0; y < height; ++y) {
647 const uint16_t *srcm;
648 const uint16_t *srcp;
649 unsigned int x;
650
651 srcm = srcp = (const uint16_t *)
652 ((char *) src - src_bpl);
653
654 if (0 == y)
655 srcm = (const uint16_t *)
656 ((char *) srcm + src_bpl * 2);
657
658 if (y != height - 1)
659 srcp = (const uint16_t *)
660 ((char *) srcp + src_bpl * 2);
661
662 for (x = 0; x < width; ++x) {
663 int xm, xp;
664
665 xm = (((0 == x) - 1) | 1) + x;
666 xp = (((x != width - 1) - 1) | 1) + x;
667
668 switch (tile) {
669 case 0: /* BG
670 GR */
671 write_rgb_pixel (dst, dst_pf,
672 /* b */ src[x],
673 /* g */ (src[xm] +
674 src[xp] +
675 srcm[x] +
676 srcp[x] + 2) >> 2,
677 /* r */ (srcm[xm] +
678 srcm[xp] +
679 srcp[xm] +
680 srcp[xp] + 2) >> 2,
681 /* depth */ 10);
682 break;
683
684 case 1: /* GB
685 RG */
686 write_rgb_pixel (dst, dst_pf,
687 /* b */ (src[xm] +
688 src[xp] + 1) >> 1,
689 /* g */ src[x],
690 /* r */ (srcm[x] +
691 srcp[x] + 1) >> 1,
692 /* depth */ 10);
693 break;
694
695 case 2: /* GR
696 BG */
697 write_rgb_pixel (dst, dst_pf,
698 /* b */ (srcm[x] +
699 srcp[x] + 1) >> 1,
700 /* g */ src[x],
701 /* r */ (src[xm] +
702 src[xp] + 1) >> 1,
703 /* depth */ 10);
704 break;
705
706 case 3: /* RG
707 GB */
708 write_rgb_pixel (dst, dst_pf,
709 /* b */ (srcm[xm] +
710 srcm[xp] +
711 srcp[xm] +
712 srcp[xp] + 2) >> 2,
713 /* g */ (src[xm] +
714 src[xp] +
715 srcm[x] +
716 srcp[x] + 2) >> 2,
717 /* r */ src[x],
718 /* depth */ 10);
719 break;
720
721 default:
722 assert (0);
723 break;
724 }
725
726 tile ^= 1;
727
728 dst += dst_pf->bits_per_pixel >> 3;
729 }
730
731 tile ^= 2;
732
733 dst += dst_padding;
734 src = (const uint16_t *)((char *) src + src_bpl);
735 }
736 }
737
738 static void
convert_packed_rgb_pixel(uint8_t * dst,const pixel_format * dst_pf,const uint8_t * src,const pixel_format * src_pf)739 convert_packed_rgb_pixel (uint8_t * dst,
740 const pixel_format * dst_pf,
741 const uint8_t * src,
742 const pixel_format * src_pf)
743 {
744 uint32_t dst_pixel;
745 uint32_t src_pixel;
746 unsigned int i;
747
748 src_pixel = 0;
749
750 switch (src_pf->byte_order * 256 + src_pf->bits_per_pixel) {
751 case LE * 256 + 32:
752 src_pixel = src[3] << 24;
753 /* fall through */
754 case LE * 256 + 24:
755 src_pixel |= src[2] << 16;
756 case LE * 256 + 16:
757 src_pixel |= src[1] << 8;
758 case LE * 256 + 8:
759 src_pixel |= src[0];
760 break;
761
762 case BE * 256 + 32:
763 src_pixel = *src++ << 24;
764 case BE * 256 + 24:
765 src_pixel |= *src++ << 16;
766 case BE * 256 + 16:
767 src_pixel |= *src++ << 8;
768 case BE * 256 + 8:
769 src_pixel |= *src;
770 break;
771
772 default:
773 assert (0);
774 break;
775 }
776
777 dst_pixel = 0;
778
779 for (i = 0; i < 3; ++i) {
780 unsigned int c;
781
782 c = (src_pixel & src_pf->mask[i]) << src_pf->shr[i];
783
784 /* XXX Check if CPU supports only signed right shift. */
785 c |= c >> src_pf->n_bits[i];
786 c |= c >> (src_pf->n_bits[i] * 2);
787
788 dst_pixel |= (c >> dst_pf->shr[i]) & dst_pf->mask[i];
789 }
790
791 switch (dst_pf->byte_order * 256 + dst_pf->bits_per_pixel) {
792 case LE * 256 + 32:
793 dst[3] = dst_pixel >> 24;
794 /* fall through */
795 case LE * 256 + 24:
796 dst[2] = dst_pixel >> 16;
797 case LE * 256 + 16:
798 dst[1] = dst_pixel >> 8;
799 case LE * 256 + 8:
800 dst[0] = dst_pixel;
801 break;
802
803 case BE * 256 + 32:
804 *dst++ = dst_pixel >> 24;
805 case BE * 256 + 24:
806 *dst++ = dst_pixel >> 16;
807 case BE * 256 + 16:
808 *dst++ = dst_pixel >> 8;
809 case BE * 256 + 8:
810 *dst = dst_pixel;
811 break;
812
813 default:
814 assert (0);
815 break;
816 }
817 }
818
819 static void
convert_rgb_image(uint8_t * dst,const pixel_format * dst_pf,unsigned long dst_bpl,const uint8_t * src,const pixel_format * src_pf,unsigned long src_bpl,unsigned int width,unsigned int height)820 convert_rgb_image (uint8_t * dst,
821 const pixel_format * dst_pf,
822 unsigned long dst_bpl,
823 const uint8_t * src,
824 const pixel_format * src_pf,
825 unsigned long src_bpl,
826 unsigned int width,
827 unsigned int height)
828 {
829 unsigned long dst_padding;
830 unsigned long src_padding;
831
832 assert (PACKED_RGB == dst_pf->pixfmt_class);
833
834 if (BAYER == src_pf->pixfmt_class) {
835 if (8 == src_pf->bits_per_pixel) {
836 convert_bayer8_image (dst, dst_pf, dst_bpl,
837 src, src_pf, src_bpl,
838 width, height);
839 } else {
840 convert_bayer16_image (dst, dst_pf, dst_bpl,
841 (const uint16_t *) src,
842 src_pf, src_bpl,
843 width, height);
844 }
845 return;
846 }
847
848 assert (width > 0);
849 assert (height > 0);
850
851 dst_padding = dst_bpl - width * (dst_pf->bits_per_pixel >> 3);
852 src_padding = src_bpl - width * (src_pf->bits_per_pixel >> 3);
853
854 assert ((long)(dst_padding | src_padding) >= 0);
855
856 do {
857 unsigned int count = width;
858
859 do {
860 convert_packed_rgb_pixel (dst, dst_pf, src, src_pf);
861
862 dst += dst_pf->bits_per_pixel >> 3;
863 src += src_pf->bits_per_pixel >> 3;
864 } while (--count > 0);
865
866 dst += dst_padding;
867 src += src_padding;
868 } while (--height > 0);
869 }
870
871 typedef enum {
872 NEXT_FORMAT = 1,
873 NEXT_CONVERTER
874 } my_event;
875
876 static my_event
x_event(void)877 x_event (void)
878 {
879 while (XPending (display)) {
880 XEvent event;
881 int key;
882
883 XNextEvent (display, &event);
884
885 switch (event.type) {
886 case KeyPress:
887 key = XLookupKeysym (&event.xkey, 0);
888
889 switch (key) {
890 case 'n':
891 return NEXT_FORMAT;
892
893 case 'c':
894 if (event.xkey.state & ControlMask)
895 exit (EXIT_SUCCESS);
896 return NEXT_CONVERTER;
897
898 case 'q':
899 exit (EXIT_SUCCESS);
900
901 default:
902 break;
903 }
904
905 break;
906
907 case ClientMessage:
908 /* We requested only delete_window messages. */
909 exit (EXIT_SUCCESS);
910
911 default:
912 break;
913 }
914 }
915
916 return 0;
917 }
918
919 static XImage *
create_ximage(const pixel_format ** pf,unsigned int width,unsigned int height)920 create_ximage (const pixel_format ** pf,
921 unsigned int width,
922 unsigned int height)
923 {
924 XImage *xi;
925 unsigned int image_size;
926 unsigned int i;
927
928 assert (NULL != display);
929
930 xi = XCreateImage (display,
931 DefaultVisual (display, screen),
932 DefaultDepth (display, screen),
933 ZPixmap,
934 /* offset */ 0,
935 /* data */ NULL,
936 width,
937 height,
938 /* bitmap_pad (n/a) */ 8,
939 /* bytes_per_line: auto */ 0);
940 if (NULL == xi) {
941 error_exit ("Cannot allocate XImage.\n");
942 }
943
944 for (i = 0; i < N_ELEMENTS (pixel_formats); ++i) {
945 if (PACKED_RGB != pixel_formats[i].pixfmt_class)
946 continue;
947 if ((LSBFirst == xi->byte_order)
948 != (LE == pixel_formats[i].byte_order))
949 continue;
950 if (xi->bits_per_pixel
951 != pixel_formats[i].bits_per_pixel)
952 continue;
953 if (xi->blue_mask != pixel_formats[i].mask[0])
954 continue;
955 if (xi->green_mask != pixel_formats[i].mask[1])
956 continue;
957 if (xi->red_mask != pixel_formats[i].mask[2])
958 continue;
959 break;
960 }
961
962 if (i >= N_ELEMENTS (pixel_formats)) {
963 error_exit ("Unknown XImage pixel format "
964 "(bpp=%u %s b=0x%08x g=0x%08x r=0x%08x).\n",
965 xi->bits_per_pixel,
966 (LSBFirst == xi->byte_order) ?
967 "LSBFirst" : "MSBFirst",
968 xi->blue_mask,
969 xi->green_mask,
970 xi->red_mask);
971 }
972
973 if (NULL != pf)
974 *pf = pixel_formats + i;
975
976 image_size = (xi->bytes_per_line * xi->height);
977
978 xi->data = malloc (image_size);
979 if (NULL == xi->data) {
980 error_exit ("Cannot allocate XImage data (%u bytes).\n",
981 image_size);
982 exit (EXIT_FAILURE);
983 }
984
985 return xi;
986 }
987
988 static void
resize_window(unsigned int image_width,unsigned int image_height,unsigned int text_width,unsigned int text_height)989 resize_window (unsigned int image_width,
990 unsigned int image_height,
991 unsigned int text_width,
992 unsigned int text_height)
993 {
994 assert (0 != window);
995
996 XResizeWindow (display, window,
997 MAX (image_width, text_width),
998 image_height + text_height);
999
1000 if (NULL != ximage) {
1001 free (ximage->data);
1002 ximage->data = NULL;
1003
1004 XDestroyImage (ximage);
1005 }
1006
1007 ximage = create_ximage (&ximage_pf, image_width, image_height);
1008 }
1009
1010 static const char *
pixel_format_bit_string(const pixel_format * pf)1011 pixel_format_bit_string (const pixel_format * pf)
1012 {
1013 static char buf[64];
1014 char *d;
1015 unsigned int i;
1016
1017 if (PACKED_RGB != pf->pixfmt_class)
1018 return NULL;
1019
1020 d = buf;
1021
1022 for (i = 0; i < pf->bits_per_pixel; i += 8) {
1023 unsigned int ii;
1024 int j;
1025
1026 if (0 != i)
1027 *d++ = ' ';
1028
1029 ii = i;
1030 if (BE == pf->byte_order)
1031 ii = pf->bits_per_pixel - i - 8;
1032
1033 for (j = 7; j >= 0; --j) {
1034 unsigned int k;
1035
1036 for (k = 0; k < 4; ++k) {
1037 if (pf->mask[k] & (1 << (ii + j))) {
1038 *d++ = "bgra"[k];
1039 break;
1040 }
1041 }
1042 }
1043 }
1044
1045 *d = 0;
1046
1047 return buf;
1048 }
1049
1050 static void
display_image(const uint8_t * image,uint32_t v4l2_fourcc,const pixel_format * image_pf,unsigned long image_bpl,unsigned int image_width,unsigned int image_height)1051 display_image (const uint8_t * image,
1052 uint32_t v4l2_fourcc,
1053 const pixel_format * image_pf,
1054 unsigned long image_bpl,
1055 unsigned int image_width,
1056 unsigned int image_height)
1057 {
1058 XWindowAttributes wa;
1059 XFontStruct *font;
1060 unsigned int text_height;
1061 XTextItem xti;
1062 const char *v4l2_fourcc_name;
1063 unsigned int i;
1064
1065 assert (NULL != ximage);
1066
1067 if (!XGetWindowAttributes (display, window, &wa)) {
1068 error_exit ("Cannot determine current X11 window size.\n");
1069 }
1070
1071 font = XQueryFont (display, XGContextFromGC (gc));
1072 text_height = font->max_bounds.ascent + font->max_bounds.descent;
1073
1074 if (image_width > (unsigned int) ximage->width
1075 || image_width != (unsigned int) wa.width
1076 || image_height > (unsigned int) ximage->height
1077 || image_height + text_height != (unsigned int) wa.height) {
1078 resize_window (image_width,
1079 image_height,
1080 /* text_width */ image_width,
1081 text_height);
1082 }
1083
1084 convert_rgb_image ((uint8_t *) ximage->data,
1085 ximage_pf,
1086 ximage->bytes_per_line,
1087 image,
1088 image_pf,
1089 image_bpl,
1090 image_width,
1091 image_height);
1092
1093 XPutImage (display,
1094 window,
1095 gc,
1096 ximage,
1097 /* src_x */ 0,
1098 /* src_y */ 0,
1099 /* dst_x */ 0,
1100 /* dst_y */ 0,
1101 /* width */ image_width,
1102 /* height */ image_height);
1103
1104
1105 XSetForeground (display, gc, XBlackPixel (display, screen));
1106
1107 XFillRectangle (display,
1108 window,
1109 gc,
1110 /* x */ 0,
1111 /* y */ image_height,
1112 wa.width,
1113 text_height);
1114
1115 XSetForeground (display, gc, XWhitePixel (display, screen));
1116
1117 v4l2_fourcc_name = "?";
1118
1119 for (i = 0; i < N_ELEMENTS (pixel_formats); ++i) {
1120 if (v4l2_fourcc == pixel_formats[i].v4l2_fourcc) {
1121 v4l2_fourcc_name = pixel_formats[i].v4l2_fourcc_name;
1122 break;
1123 }
1124 }
1125
1126
1127 CLEAR (xti);
1128
1129 if (PACKED_RGB == image_pf->pixfmt_class) {
1130 xti.nchars = asprintf (&xti.chars,
1131 "Format %s, converter %s (%s)",
1132 v4l2_fourcc_name,
1133 image_pf->name,
1134 pixel_format_bit_string (image_pf));
1135 } else {
1136 xti.nchars = asprintf (&xti.chars,
1137 "Format %s, converter %s",
1138 v4l2_fourcc_name,
1139 image_pf->name);
1140 }
1141
1142 if (xti.nchars < 0) {
1143 error_exit ("Cannot allocate text buffer.\n");
1144 }
1145
1146 XDrawText (display, window, gc,
1147 /* x */ 4,
1148 /* y */ image_height + font->max_bounds.ascent,
1149 &xti,
1150 /* n_items */ 1);
1151
1152 free (xti.chars);
1153
1154 XFreeFontInfo (/* names */ NULL, font, 1);
1155 }
1156
1157 static void
open_window(unsigned int width,unsigned int height)1158 open_window (unsigned int width,
1159 unsigned int height)
1160 {
1161 GC default_gc;
1162 XFontStruct *font;
1163 unsigned int text_height;
1164
1165 display = XOpenDisplay (NULL);
1166 if (NULL == display) {
1167 error_exit ("Cannot open X11 display.\n");
1168 }
1169
1170 screen = DefaultScreen (display);
1171
1172 default_gc = XDefaultGC (display, screen);
1173 font = XQueryFont (display, XGContextFromGC (default_gc));
1174 text_height = font->max_bounds.ascent + font->max_bounds.descent;
1175
1176 window = XCreateSimpleWindow (display,
1177 RootWindow (display, screen),
1178 /* x */ 0,
1179 /* y */ 0,
1180 width,
1181 height + text_height,
1182 /* border width */ 2,
1183 /* foreground */
1184 XWhitePixel (display, screen),
1185 /* background */
1186 XBlackPixel (display, screen));
1187 if (0 == window) {
1188 error_exit ("Cannot open X11 window.\n");
1189 }
1190
1191 gc = XCreateGC (display, window,
1192 /* valuemask */ 0,
1193 /* values */ NULL);
1194
1195 XSetFunction (display, gc, GXcopy);
1196 XSetFillStyle (display, gc, FillSolid);
1197
1198 ximage = create_ximage (&ximage_pf, width, height);
1199
1200 XSelectInput (display, window,
1201 (KeyPressMask |
1202 ExposureMask |
1203 StructureNotifyMask));
1204
1205 xa_delete_window = XInternAtom (display, "WM_DELETE_WINDOW",
1206 /* only_if_exists */ False);
1207
1208 XSetWMProtocols (display, window, &xa_delete_window, /* count */ 1);
1209
1210 XStoreName (display, window,
1211 "V4L2 Pixfmt Test - "
1212 "Press [n] for next format, [c] for next converter");
1213
1214 XMapWindow (display, window);
1215
1216 XSync (display, /* discard all events */ False);
1217 }
1218
1219 static int
xioctl(int fd,unsigned long int request,void * arg)1220 xioctl (int fd,
1221 unsigned long int request,
1222 void * arg)
1223 {
1224 int r;
1225
1226 do r = ioctl (fd, request, arg);
1227 while (-1 == r && EINTR == errno);
1228
1229 return r;
1230 }
1231
1232 static bool
read_and_display_frame(const pixel_format * conv_pf)1233 read_and_display_frame (const pixel_format * conv_pf)
1234 {
1235 struct v4l2_buffer buf;
1236
1237 switch (io_method) {
1238 case IO_METHOD_READ:
1239 if (-1 == read (dev_fd, buffers[0].start,
1240 buffers[0].length)) {
1241 switch (errno) {
1242 case EAGAIN:
1243 return false;
1244
1245 default:
1246 errno_exit ("read");
1247 }
1248 }
1249
1250 display_image (buffers[0].start,
1251 fmt.fmt.pix.pixelformat,
1252 conv_pf,
1253 fmt.fmt.pix.bytesperline,
1254 fmt.fmt.pix.width,
1255 fmt.fmt.pix.height);
1256
1257 break;
1258
1259 case IO_METHOD_MMAP:
1260 CLEAR (buf);
1261
1262 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1263 buf.memory = V4L2_MEMORY_MMAP;
1264
1265 if (-1 == xioctl (dev_fd, VIDIOC_DQBUF, &buf)) {
1266 switch (errno) {
1267 case EAGAIN:
1268 return false;
1269
1270 case EIO:
1271 /* Could ignore EIO, see spec. */
1272
1273 /* fall through */
1274
1275 default:
1276 errno_exit ("VIDIOC_DQBUF");
1277 }
1278 }
1279
1280 assert (buf.index < n_buffers);
1281
1282 display_image (buffers[buf.index].start,
1283 fmt.fmt.pix.pixelformat,
1284 conv_pf,
1285 fmt.fmt.pix.bytesperline,
1286 fmt.fmt.pix.width,
1287 fmt.fmt.pix.height);
1288
1289 if (-1 == xioctl (dev_fd, VIDIOC_QBUF, &buf))
1290 errno_exit ("VIDIOC_QBUF");
1291
1292 break;
1293 }
1294
1295 return true;
1296 }
1297
1298 static void
wait_for_next_frame(void)1299 wait_for_next_frame (void)
1300 {
1301 for (;;) {
1302 struct timeval timeout;
1303 fd_set fds;
1304 int r;
1305
1306 FD_ZERO (&fds);
1307 FD_SET (dev_fd, &fds);
1308
1309 timeout.tv_sec = 2;
1310 timeout.tv_usec = 0;
1311
1312 r = select (dev_fd + 1, &fds, NULL, NULL, &timeout);
1313 if (-1 == r) {
1314 if (EINTR == errno)
1315 continue;
1316
1317 errno_exit ("select");
1318 } else if (0 == r) {
1319 error_exit ("select timeout.\n");
1320 } else {
1321 break;
1322 }
1323 }
1324 }
1325
1326 static void
flush_capture_queue(void)1327 flush_capture_queue (void)
1328 {
1329 struct v4l2_buffer buf;
1330
1331 for (;;) {
1332 switch (io_method) {
1333 case IO_METHOD_READ:
1334 /* Nothing to do. */
1335 return;
1336
1337 case IO_METHOD_MMAP:
1338 CLEAR (buf);
1339
1340 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1341 buf.memory = V4L2_MEMORY_MMAP;
1342
1343 if (-1 == xioctl (dev_fd, VIDIOC_DQBUF, &buf)) {
1344 switch (errno) {
1345 case EAGAIN:
1346 return;
1347
1348 default:
1349 errno_exit ("VIDIOC_DQBUF");
1350 }
1351 }
1352
1353 if (-1 == xioctl (dev_fd, VIDIOC_QBUF, &buf))
1354 errno_exit ("VIDIOC_QBUF");
1355
1356 break;
1357
1358 default:
1359 assert (0);
1360 break;
1361 }
1362 }
1363 }
1364
1365 static void
capture_loop(void)1366 capture_loop (void)
1367 {
1368 const pixel_format *conv_pf;
1369
1370 conv_pf = find_v4l2_fourcc (fmt.fmt.pix.pixelformat);
1371 assert (NULL != conv_pf);
1372
1373 for (;;) {
1374 /* Remove images from the capture queue if
1375 we can't display them fast enough. */
1376 flush_capture_queue ();
1377
1378 do {
1379 wait_for_next_frame ();
1380 } while (!read_and_display_frame (conv_pf));
1381
1382 switch (x_event ()) {
1383 case NEXT_CONVERTER:
1384 conv_pf = next_converter (conv_pf);
1385 break;
1386
1387 case NEXT_FORMAT:
1388 return;
1389
1390 default:
1391 break;
1392 }
1393 }
1394 }
1395
1396 static void
stop_capturing(void)1397 stop_capturing (void)
1398 {
1399 enum v4l2_buf_type type;
1400
1401 switch (io_method) {
1402 case IO_METHOD_READ:
1403 /* Nothing to do. */
1404 break;
1405
1406 case IO_METHOD_MMAP:
1407 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1408
1409 if (-1 == xioctl (dev_fd, VIDIOC_STREAMOFF, &type))
1410 errno_exit ("VIDIOC_STREAMOFF");
1411
1412 break;
1413 }
1414 }
1415
1416 static void
start_capturing(void)1417 start_capturing (void)
1418 {
1419 unsigned int i;
1420 enum v4l2_buf_type type;
1421
1422 switch (io_method) {
1423 case IO_METHOD_READ:
1424 /* Nothing to do. */
1425 break;
1426
1427 case IO_METHOD_MMAP:
1428 for (i = 0; i < n_buffers; ++i) {
1429 struct v4l2_buffer buf;
1430
1431 CLEAR (buf);
1432
1433 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1434 buf.memory = V4L2_MEMORY_MMAP;
1435 buf.index = i;
1436
1437 if (-1 == xioctl (dev_fd, VIDIOC_QBUF, &buf))
1438 errno_exit ("VIDIOC_QBUF");
1439 }
1440
1441 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1442
1443 if (-1 == xioctl (dev_fd, VIDIOC_STREAMON, &type))
1444 errno_exit ("VIDIOC_STREAMON");
1445
1446 break;
1447
1448 default:
1449 assert (0);
1450 break;
1451 }
1452 }
1453
1454 static void
free_io_buffers(void)1455 free_io_buffers (void)
1456 {
1457 unsigned int i;
1458
1459 switch (io_method) {
1460 case IO_METHOD_READ:
1461 free (buffers[0].start);
1462 break;
1463
1464 case IO_METHOD_MMAP:
1465 for (i = 0; i < n_buffers; ++i) {
1466 if (-1 == munmap (buffers[i].start,
1467 buffers[i].length)) {
1468 errno_exit ("munmap");
1469 }
1470 }
1471
1472 break;
1473
1474 default:
1475 assert (0);
1476 break;
1477 }
1478
1479 free (buffers);
1480 buffers = NULL;
1481 }
1482
1483 static void
init_read_io(unsigned int buffer_size)1484 init_read_io (unsigned int buffer_size)
1485 {
1486 buffers = calloc (1, sizeof (*buffers));
1487
1488 if (NULL == buffers) {
1489 error_exit ("Cannot allocate capture buffer (%u bytes).\n",
1490 sizeof (*buffers));
1491 }
1492
1493 buffers[0].length = buffer_size;
1494 buffers[0].start = malloc (buffer_size);
1495
1496 if (NULL == buffers[0].start) {
1497 error_exit ("Cannot allocate capture buffer (%u bytes).\n",
1498 buffer_size);
1499 }
1500 }
1501
1502 static void
init_mmap_io(void)1503 init_mmap_io (void)
1504 {
1505 struct v4l2_requestbuffers req;
1506
1507 CLEAR (req);
1508
1509 req.count = 4;
1510 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1511 req.memory = V4L2_MEMORY_MMAP;
1512
1513 if (-1 == xioctl (dev_fd, VIDIOC_REQBUFS, &req)) {
1514 if (EINVAL == errno) {
1515 error_exit ("%s does not support "
1516 "memory mapping.\n", dev_name);
1517 } else {
1518 errno_exit ("VIDIOC_REQBUFS");
1519 }
1520 }
1521
1522 if (req.count < 2) {
1523 error_exit ("Insufficient buffer memory on %s.\n",
1524 dev_name);
1525 }
1526
1527 buffers = calloc (req.count, sizeof (*buffers));
1528 if (NULL == buffers) {
1529 error_exit ("Cannot allocate capture buffer (%u bytes).\n",
1530 req.count * sizeof (*buffers));
1531 }
1532
1533 for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
1534 struct v4l2_buffer buf;
1535
1536 CLEAR (buf);
1537
1538 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1539 buf.memory = V4L2_MEMORY_MMAP;
1540 buf.index = n_buffers;
1541
1542 if (-1 == xioctl (dev_fd, VIDIOC_QUERYBUF, &buf))
1543 errno_exit ("VIDIOC_QUERYBUF");
1544
1545 buffers[n_buffers].length = buf.length;
1546 buffers[n_buffers].start =
1547 mmap (NULL /* start anywhere */,
1548 buf.length,
1549 PROT_READ | PROT_WRITE /* required */,
1550 MAP_SHARED /* recommended */,
1551 dev_fd, buf.m.offset);
1552
1553 if (MAP_FAILED == buffers[n_buffers].start)
1554 errno_exit ("mmap");
1555 }
1556 }
1557
1558 static void
mainloop(void)1559 mainloop (void)
1560 {
1561 bool checked_formats[N_ELEMENTS (pixel_formats)];
1562
1563 CLEAR (checked_formats);
1564
1565 for (;;) {
1566 const pixel_format *pf;
1567 const pixel_format *actual_pf;
1568 unsigned int width;
1569 unsigned int height;
1570 unsigned int min_bpl;
1571 unsigned int min_size;
1572 unsigned int i;
1573
1574 for (i = 0; i < N_ELEMENTS (pixel_formats); ++i) {
1575 if (checked_formats[i])
1576 continue;
1577 checked_formats[i] = true;
1578 if (0 != pixel_formats[i].v4l2_fourcc)
1579 break;
1580 }
1581
1582 if (i >= N_ELEMENTS (pixel_formats))
1583 return; /* all done */
1584
1585 pf = pixel_formats + i;
1586
1587 CLEAR (fmt);
1588
1589 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1590
1591 height = 480;
1592 if (std_id & V4L2_STD_625_50)
1593 height = 576;
1594
1595 width = height * 4 / 3;
1596
1597 fmt.fmt.pix.width = width;
1598 fmt.fmt.pix.height = height;
1599
1600 fmt.fmt.pix.pixelformat = pf->v4l2_fourcc;
1601
1602 fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
1603
1604 if (-1 == xioctl (dev_fd, VIDIOC_S_FMT, &fmt)) {
1605 if (EINVAL != errno) {
1606 errno_exit ("VIDIOC_S_FMT");
1607 }
1608
1609 fprintf (stderr, "Format %s %ux%u "
1610 "not supported by driver.\n",
1611 pf->v4l2_fourcc_name,
1612 width,
1613 height);
1614
1615 continue;
1616 }
1617
1618 actual_pf = find_v4l2_fourcc (fmt.fmt.pix.pixelformat);
1619 if (0 == actual_pf) {
1620 fprintf (stderr,
1621 "Requested pixelformat %s, driver "
1622 "returned unknown pixelformat 0x%08x.\n",
1623 pf->v4l2_fourcc_name,
1624 fmt.fmt.pix.pixelformat);
1625 continue;
1626 } else if (pf != actual_pf) {
1627 /* Some drivers change pixelformat. */
1628 checked_formats[actual_pf->pixfmt] = true;
1629 pf = actual_pf;
1630 }
1631
1632 min_bpl = (fmt.fmt.pix.width * pf->bits_per_pixel) >> 3;
1633
1634 if (fmt.fmt.pix.bytesperline < min_bpl) {
1635 error_exit ("Driver returned fmt.pix.pixelformat=%s "
1636 "width=%u height=%u bytesperline=%u. "
1637 "Expected bytesperline >= %u.\n",
1638 pf->v4l2_fourcc_name,
1639 fmt.fmt.pix.width,
1640 fmt.fmt.pix.height,
1641 fmt.fmt.pix.bytesperline,
1642 min_bpl);
1643 continue;
1644 }
1645
1646 min_size = (fmt.fmt.pix.height - 1)
1647 * MAX (min_bpl, fmt.fmt.pix.bytesperline)
1648 + min_bpl;
1649
1650 if (fmt.fmt.pix.sizeimage < min_size) {
1651 error_exit ("Driver returned fmt.pix.pixelformat=%s "
1652 "width=%u height=%u bytesperline=%u "
1653 "size=%u. Expected size >= %u.\n",
1654 pf->v4l2_fourcc_name,
1655 fmt.fmt.pix.width,
1656 fmt.fmt.pix.height,
1657 fmt.fmt.pix.bytesperline,
1658 fmt.fmt.pix.sizeimage,
1659 min_size);
1660 continue;
1661 }
1662
1663 if (0 == window) {
1664 open_window (fmt.fmt.pix.width,
1665 fmt.fmt.pix.height);
1666 }
1667
1668 switch (io_method) {
1669 case IO_METHOD_READ:
1670 init_read_io (fmt.fmt.pix.sizeimage);
1671 break;
1672
1673 case IO_METHOD_MMAP:
1674 init_mmap_io ();
1675 break;
1676 }
1677
1678 start_capturing ();
1679
1680 capture_loop ();
1681
1682 stop_capturing ();
1683
1684 free_io_buffers ();
1685 }
1686 }
1687
1688 static void
init_device(void)1689 init_device (void)
1690 {
1691 struct v4l2_capability cap;
1692 struct v4l2_cropcap cropcap;
1693 struct v4l2_crop crop;
1694
1695 if (-1 == xioctl (dev_fd, VIDIOC_QUERYCAP, &cap)) {
1696 if (EINVAL == errno) {
1697 error_exit ("%s is not a V4L2 device.\n");
1698 } else {
1699 errno_exit ("VIDIOC_QUERYCAP");
1700 }
1701 }
1702
1703 if (io_method == 0) {
1704 if (cap.capabilities & V4L2_CAP_STREAMING) {
1705 io_method = IO_METHOD_MMAP;
1706 } else if (cap.capabilities & V4L2_CAP_READWRITE) {
1707 io_method = IO_METHOD_READ;
1708 } else {
1709 error_exit ("%s does not support reading or "
1710 "streaming.\n");
1711 }
1712 }
1713
1714 switch (io_method) {
1715 case IO_METHOD_READ:
1716 if (0 == (cap.capabilities & V4L2_CAP_READWRITE)) {
1717 error_exit ("%s does not support read i/o.\n");
1718 }
1719
1720 break;
1721
1722 case IO_METHOD_MMAP:
1723 if (0 == (cap.capabilities & V4L2_CAP_STREAMING)) {
1724 error_exit ("%s does not support streaming i/o.\n");
1725 }
1726
1727 break;
1728
1729 default:
1730 assert (0);
1731 break;
1732 }
1733
1734 CLEAR (cropcap);
1735
1736 cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1737
1738 if (0 == xioctl (dev_fd, VIDIOC_CROPCAP, &cropcap)) {
1739 crop.type = cropcap.type;
1740 crop.c = cropcap.defrect; /* reset to default */
1741
1742 /* Errors ignored. */
1743 xioctl (dev_fd, VIDIOC_S_CROP, &crop);
1744 } else {
1745 /* Errors ignored. */
1746 }
1747
1748 /* Webcams may not support any standard at all, see
1749 http://v4l2spec.bytesex.org/spec/x448.htm for details */
1750 if (-1 == xioctl (dev_fd, VIDIOC_G_STD, &std_id))
1751 std_id = 0;
1752 }
1753
1754 static void
open_device(void)1755 open_device (void)
1756 {
1757 struct stat st;
1758
1759 if (-1 == stat (dev_name, &st)) {
1760 error_exit ("Cannot identify '%s'. %s.\n",
1761 dev_name, strerror (errno));
1762 }
1763
1764 if (!S_ISCHR (st.st_mode)) {
1765 error_exit ("%s is not a device file.\n", dev_name);
1766 }
1767
1768 dev_fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
1769 if (-1 == dev_fd) {
1770 error_exit ("Cannot open %s. %s.\n",
1771 dev_name, strerror (errno));
1772 }
1773 }
1774
1775 static void
self_test(void)1776 self_test (void)
1777 {
1778 const pixel_format *pf;
1779
1780 assert (0 == pixel_formats[0].pixfmt);
1781 assert (N_ELEMENTS (pixel_formats) > 0);
1782
1783 for (pf = pixel_formats + 1;
1784 pf < pixel_formats + N_ELEMENTS (pixel_formats); ++pf) {
1785 const pixel_format *pf2;
1786 unsigned int i;
1787
1788 #define pf_assert(expr) \
1789 do { \
1790 if (!(expr)) { \
1791 error_exit ("Assertion %s failed in " \
1792 "pixel_format[%d = %s].\n", \
1793 #expr, (int)(pf - pixel_formats), \
1794 pf->name ? pf->name : "?"); \
1795 } \
1796 } while (0)
1797
1798 pf_assert (0 != pf->pixfmt);
1799 pf_assert (NULL != pf->name);
1800
1801 pf_assert ((0 == pf->v4l2_fourcc)
1802 == (NULL == pf->v4l2_fourcc_name));
1803
1804 pf_assert (0 != pf->pixfmt_swap_red_blue);
1805
1806 pf_assert (LE == pf->byte_order
1807 || BE == pf->byte_order);
1808
1809 pf_assert (PACKED_RGB == pf->pixfmt_class
1810 || BAYER == pf->pixfmt_class);
1811
1812 if (PACKED_RGB == pf->pixfmt_class) {
1813 pf_assert (pf->color_depth == (pf->n_bits[0] +
1814 pf->n_bits[1] +
1815 pf->n_bits[2]));
1816
1817 pf_assert (pf->bits_per_pixel == (pf->n_bits[0] +
1818 pf->n_bits[1] +
1819 pf->n_bits[2] +
1820 pf->n_bits[3]));
1821
1822 pf_assert (0 != pf->pixfmt_opposite_byte_order);
1823
1824 if (0 != pf->mask[3]) /* has alpha */
1825 pf_assert (0 != pf->pixfmt_opposite_alpha);
1826 else
1827 pf_assert (0 == pf->pixfmt_opposite_alpha);
1828
1829 for (i = 0; i < N_ELEMENTS (pf->mask); ++i) {
1830 pf_assert (pf->n_bits[i] + pf->shr[i] <= 32);
1831 pf_assert (pf->mask[i]
1832 == (((1u << pf->n_bits[i]) - 1)
1833 << (32 - pf->n_bits[i]
1834 - pf->shr[i])));
1835 }
1836 }
1837
1838 for (pf2 = pf + 1;
1839 pf2 < pixel_formats + N_ELEMENTS (pixel_formats);
1840 ++pf2) {
1841 if (pf->pixfmt == pf2->pixfmt
1842 || 0 == strcmp (pf->name, pf2->name)) {
1843 error_exit ("Assertion failure: pixfmt "
1844 "%u (%s) twice in "
1845 "pixel_formats[] table.\n",
1846 pf->pixfmt, pf->name);
1847 }
1848
1849 if (0 != pf->v4l2_fourcc
1850 && 0 != pf2->v4l2_fourcc
1851 && (pf->v4l2_fourcc == pf2->v4l2_fourcc
1852 || 0 == strcmp (pf->v4l2_fourcc_name,
1853 pf2->v4l2_fourcc_name))) {
1854 error_exit ("Assertion failure: V4L2 "
1855 "fourcc 0x%08x (%s) twice in "
1856 "pixel_formats[] table.\n",
1857 pf->v4l2_fourcc,
1858 pf->v4l2_fourcc_name);
1859 }
1860 }
1861
1862 #undef pf_assert
1863
1864 }
1865
1866 /* XXX Should also test the converters here. */
1867 }
1868
1869 static void
usage(FILE * fp,int argc,char ** argv)1870 usage (FILE * fp,
1871 int argc,
1872 char ** argv)
1873 {
1874 fprintf (fp, "\
1875 V4L2 pixfmt test " V4L_UTILS_VERSION "\n\
1876 Copyright (C) 2007 Michael H. Schimek\n\
1877 This program is licensed under GPL 2 or later. NO WARRANTIES.\n\n\
1878 Usage: %s [options]\n\n\
1879 Options:\n\
1880 -d | --device name Video device name [%s]\n\
1881 -h | --help Print this message\n\
1882 -m | --mmap Use memory mapped buffers (auto)\n\
1883 -r | --read Use read() calls (auto)\n\
1884 ",
1885 my_name, dev_name);
1886 }
1887
1888 static const char short_options [] = "d:hmr";
1889
1890 static const struct option
1891 long_options [] = {
1892 { "device", required_argument, NULL, 'd' },
1893 { "help", no_argument, NULL, 'h' },
1894 { "mmap", no_argument, NULL, 'm' },
1895 { "read", no_argument, NULL, 'r' },
1896 { "usage", no_argument, NULL, 'h' },
1897 { 0, 0, 0, 0 }
1898 };
1899
1900 int
main(int argc,char ** argv)1901 main (int argc,
1902 char ** argv)
1903 {
1904 my_name = argv[0];
1905
1906 self_test ();
1907
1908 for (;;) {
1909 int opt_index;
1910 int c;
1911
1912 c = getopt_long (argc, argv,
1913 short_options, long_options,
1914 &opt_index);
1915
1916 if (-1 == c)
1917 break;
1918
1919 switch (c) {
1920 case 0: /* getopt_long() flag */
1921 break;
1922
1923 case 'd':
1924 dev_name = optarg;
1925 break;
1926
1927 case 'h':
1928 usage (stdout, argc, argv);
1929 exit (EXIT_SUCCESS);
1930
1931 case 'm':
1932 io_method = IO_METHOD_MMAP;
1933 break;
1934
1935 case 'r':
1936 io_method = IO_METHOD_READ;
1937 break;
1938
1939 default:
1940 usage (stderr, argc, argv);
1941 exit (EXIT_FAILURE);
1942 }
1943 }
1944
1945 open_device ();
1946
1947 init_device ();
1948
1949 mainloop ();
1950
1951 exit (EXIT_SUCCESS);
1952
1953 return 0;
1954 }
1955