matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019-2022, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief V4L2 frameserver implementation
6 * @author Pete Black <pblack@collabora.com>
7 * @author Jakob Bornecrantz <jakob@collabora.com>
8 * @ingroup drv_v4l2
9 */
10
11#include "os/os_time.h"
12
13#include "util/u_var.h"
14#include "util/u_sink.h"
15#include "util/u_misc.h"
16#include "util/u_debug.h"
17#include "util/u_format.h"
18#include "util/u_logging.h"
19#include "util/u_trace_marker.h"
20
21#include "v4l2_interface.h"
22#include "v4l2_driver.h"
23
24#include <stdio.h>
25#include <unistd.h>
26#include <pthread.h>
27
28#include <linux/videodev2.h>
29#include <linux/v4l2-common.h>
30#include <sys/ioctl.h>
31#include <sys/mman.h>
32#include <fcntl.h>
33
34
35/*
36 *
37 * Defines.
38 *
39 */
40
41
42#define V_CONTROL_GET(VID, CONTROL) \
43 do { \
44 int _value = 0; \
45 if (v4l2_control_get(VID, V4L2_CID_##CONTROL, &_value) != 0) { \
46 V4L2_ERROR(VID, "failed to get V4L2_CID_" #CONTROL); \
47 } else { \
48 V4L2_DEBUG(VID, "V4L2_CID_" #CONTROL " = %i", _value); \
49 } \
50 } while (false);
51
52#define V_CONTROL_SET(VID, CONTROL, VALUE) \
53 do { \
54 if (v4l2_control_set(VID, V4L2_CID_##CONTROL, VALUE) != 0) { \
55 V4L2_ERROR(VID, "failed to set V4L2_CID_" #CONTROL); \
56 } \
57 } while (false);
58
59DEBUG_GET_ONCE_LOG_OPTION(v4l2_log, "V4L2_LOG", U_LOGGING_WARN)
60DEBUG_GET_ONCE_NUM_OPTION(v4l2_exposure_absolute, "V4L2_EXPOSURE_ABSOLUTE", 10)
61
62/*!
63 * Streaming thread entrypoint
64 */
65static void *
66v4l2_fs_mainloop(void *ptr);
67
68static void
69dump_controls(struct v4l2_fs *vid);
70
71static void
72dump_contron_name(uint32_t id);
73
74
75/*
76 *
77 * Misc helper functions
78 *
79 */
80
81static size_t
82align_up(size_t size, size_t align)
83{
84 if ((size % align) == 0) {
85 return size;
86 }
87
88 return size + (align - (size % align));
89}
90
91static void
92v4l2_free_frame(struct xrt_frame *xf)
93{
94 struct v4l2_frame *vf = (struct v4l2_frame *)xf;
95 struct v4l2_fs *vid = (struct v4l2_fs *)xf->owner;
96
97 vid->used_frames--;
98
99 if (!vid->is_running) {
100 return;
101 }
102
103 if (ioctl(vid->fd, VIDIOC_QBUF, &vf->v_buf) < 0) {
104 V4L2_ERROR(vid, "error: Requeue failed!");
105 vid->is_running = false;
106 }
107}
108
109XRT_MAYBE_UNUSED static int
110v4l2_control_get(struct v4l2_fs *vid, uint32_t id, int *out_value)
111{
112 struct v4l2_control control = {0};
113 int ret;
114
115 control.id = id;
116 ret = ioctl(vid->fd, VIDIOC_G_CTRL, &control);
117 if (ret != 0) {
118 return ret;
119 }
120
121 *out_value = control.value;
122 return 0;
123}
124
125static int
126v4l2_control_set(struct v4l2_fs *vid, uint32_t id, int value)
127{
128 struct v4l2_control control = {0};
129 int ret;
130
131 control.id = id;
132 control.value = value;
133 ret = ioctl(vid->fd, VIDIOC_S_CTRL, &control);
134 if (ret != 0) {
135 return ret;
136 }
137
138 return 0;
139}
140
141static void
142v4l2_add_control_state(struct v4l2_fs *vid, int control, struct v4l2_state_want want[2], int force, const char *name)
143{
144 struct v4l2_control_state *state = &vid->states[vid->num_states++];
145
146 state->id = control;
147 state->name = name;
148 state->want[0] = want[0];
149 state->want[1] = want[1];
150 state->force = force;
151}
152
153static int
154v4l2_query_cap_and_validate(struct v4l2_fs *vid)
155{
156 int ret;
157
158 /*
159 * Regular caps.
160 */
161 struct v4l2_capability cap;
162 ret = ioctl(vid->fd, VIDIOC_QUERYCAP, &cap);
163 if (ret != 0) {
164 V4L2_ERROR(vid, "error: Failed to get v4l2 cap.");
165 return ret;
166 }
167
168 char *card = (char *)cap.card;
169 snprintf(vid->base.name, sizeof(vid->base.name), "%s", card);
170
171 V4L2_DEBUG(vid, "V4L2 device: '%s'", vid->base.name);
172
173 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
174 // not a video device
175 V4L2_ERROR(vid, "error: Is not a capture device.");
176 return -1;
177 }
178 if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
179 // cannot stream
180 V4L2_ERROR(vid, "error: Cannot stream!");
181 return -1;
182 }
183 if (cap.capabilities & V4L2_CAP_EXT_PIX_FORMAT) {
184 // need to query for extended format info
185 vid->has.extended_format = true;
186 }
187
188 /*
189 * Stream capture caps.
190 */
191 struct v4l2_streamparm stream = {0};
192 stream.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
193
194 ret = ioctl(vid->fd, VIDIOC_G_PARM, &stream);
195 if (ret != 0) {
196 V4L2_ERROR(vid, "error: Failed to get v4l2 stream param.");
197 return ret;
198 }
199
200 if (stream.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
201 // Does this device support setting the timeperframe interval.
202 vid->has.timeperframe = true;
203 } else {
204 V4L2_DEBUG(vid, "warning: No V4L2_CAP_TIMEPERFRAME");
205 }
206
207 // Log controls.
208 if (vid->log_level <= U_LOGGING_DEBUG) {
209 dump_controls(vid);
210 }
211
212 /*
213 * Find quirks
214 */
215 vid->quirks.ps4_cam = strcmp(card, "USB Camera-OV580: USB Camera-OV") == 0;
216
217#define ADD(CONTROL, WANT1, WANT2, WANT3, WANT4, NAME) \
218 do { \
219 struct v4l2_state_want want[2] = {{WANT1, WANT2}, {WANT3, WANT4}}; \
220 v4l2_add_control_state(vid, V4L2_CID_##CONTROL, want, 2, NAME); \
221 } while (false)
222
223 if (vid->quirks.ps4_cam) {
224 // The experimented best controls to best track things.
225 ADD(GAIN, //
226 true, 0, false, 0, //
227 "gain");
228 ADD(AUTO_WHITE_BALANCE, //
229 true, 0, true, 1, //
230 "auto_white_balance");
231 ADD(WHITE_BALANCE_TEMPERATURE, //
232 true, 3900, false, 0, //
233 "white_balance_temperature");
234 ADD(EXPOSURE_AUTO, //
235 true, 2, true, 0, //
236 "exposure_auto");
237 long num = debug_get_num_option_v4l2_exposure_absolute();
238 ADD(EXPOSURE_ABSOLUTE, //
239 true, num, false, 0, //
240 "exposure_absolute");
241 }
242
243 if (strcmp(card, "3D USB Camera: 3D USB Camera") == 0) {
244 // The experimented best controls to best track things.
245 ADD(AUTO_WHITE_BALANCE, //
246 true, 0, true, 1, //
247 "auto_white_balance");
248 ADD(WHITE_BALANCE_TEMPERATURE, //
249 true, 6500, false, 0, //
250 "white_balance_temperature");
251 ADD(EXPOSURE_AUTO, //
252 true, 1, true, 3, //
253 "exposure_auto");
254 ADD(EXPOSURE_ABSOLUTE, //
255 true, 10, false, 0, //
256 "exposure_absolute");
257 }
258
259 // Done
260 return 0;
261}
262
263static int
264v4l2_try_userptr(struct v4l2_fs *vid, struct v4l2_requestbuffers *v_bufrequest)
265{
266 v_bufrequest->memory = V4L2_MEMORY_USERPTR;
267 if (ioctl(vid->fd, VIDIOC_REQBUFS, v_bufrequest) == 0) {
268 vid->capture.userptr = true;
269 return 0;
270 }
271
272 V4L2_DEBUG(vid, "info: Driver does not handle userptr buffers.");
273 return -1;
274}
275
276static int
277v4l2_try_mmap(struct v4l2_fs *vid, struct v4l2_requestbuffers *v_bufrequest)
278{
279 v_bufrequest->memory = V4L2_MEMORY_MMAP;
280 if (ioctl(vid->fd, VIDIOC_REQBUFS, v_bufrequest) == 0) {
281 vid->capture.mmap = true;
282 return 0;
283 }
284
285 V4L2_DEBUG(vid, "info: Driver does not mmap userptr buffers.");
286 return -1;
287}
288
289static int
290v4l2_setup_mmap_buffer(struct v4l2_fs *vid, struct v4l2_frame *vf, struct v4l2_buffer *v_buf)
291{
292 void *ptr = mmap(0, v_buf->length, PROT_READ, MAP_SHARED, vid->fd, v_buf->m.offset);
293 if (ptr == MAP_FAILED) {
294 V4L2_ERROR(vid, "error: Call to mmap failed!");
295 return -1;
296 }
297
298 vf->mem = ptr;
299
300 return 0;
301}
302
303static int
304v4l2_setup_userptr_buffer(struct v4l2_fs *vid, struct v4l2_frame *vf, struct v4l2_buffer *v_buf)
305{
306 // align this to a memory page, v4l2 likes it that way
307 long sz = sysconf(_SC_PAGESIZE);
308 size_t size = align_up(v_buf->length, (size_t)sz);
309
310 void *ptr = aligned_alloc(sz, size);
311 if (ptr == NULL) {
312 V4L2_ERROR(vid, "error: Could not alloc page-aligned memory!");
313 return -1;
314 }
315
316 vf->mem = ptr;
317 v_buf->m.userptr = (intptr_t)ptr;
318
319 return 0;
320}
321
322
323/*
324 *
325 * Mode adding functions.
326 *
327 */
328
329static struct v4l2_source_descriptor *
330v4l2_add_descriptor(struct v4l2_fs *vid)
331{
332 uint32_t index = vid->num_descriptors++;
333 U_ARRAY_REALLOC_OR_FREE(vid->descriptors, struct v4l2_source_descriptor, vid->num_descriptors);
334
335 struct v4l2_source_descriptor *desc = &vid->descriptors[index];
336 U_ZERO(desc);
337
338 return desc;
339}
340
341static void
342v4l2_list_modes_interval(struct v4l2_fs *vid,
343 const struct v4l2_fmtdesc *fmt,
344 const struct v4l2_frmsizeenum *size,
345 const struct v4l2_frmivalenum *interval)
346{
347 if (interval->discrete.denominator % interval->discrete.numerator == 0) {
348 int fps = interval->discrete.denominator / interval->discrete.numerator;
349
350 V4L2_DEBUG(vid, "#%i %dx%d@%i", vid->num_descriptors, interval->width, interval->height, fps);
351 } else {
352 double fps = (double)interval->discrete.denominator / (double)interval->discrete.numerator;
353
354 V4L2_DEBUG(vid, "#%i %dx%d@%f", vid->num_descriptors, interval->width, interval->height, fps);
355 }
356}
357
358static void
359v4l2_list_modes_size(struct v4l2_fs *vid, const struct v4l2_fmtdesc *fmt, const struct v4l2_frmsizeenum *size)
360{
361 if (size->type != V4L2_FRMSIZE_TYPE_DISCRETE) {
362 V4L2_DEBUG(vid, "warning: Skipping non discrete frame size.");
363 return;
364 }
365
366 struct v4l2_frmivalenum interval;
367 U_ZERO(&interval);
368 interval.pixel_format = size->pixel_format;
369 interval.width = size->discrete.width;
370 interval.height = size->discrete.height;
371
372 // Since we don't keep track of the interval
373 // we only make sure there is at least one.
374 while (ioctl(vid->fd, VIDIOC_ENUM_FRAMEINTERVALS, &interval) == 0) {
375 v4l2_list_modes_interval(vid, fmt, size, &interval);
376 interval.index++;
377 }
378
379 // We didn't find any frame intervals.
380 if (interval.index == 0) {
381 return;
382 }
383
384 enum xrt_format format = (enum xrt_format)0;
385 switch (interval.pixel_format) {
386 case V4L2_PIX_FMT_YUYV: format = XRT_FORMAT_YUYV422; break;
387 case V4L2_PIX_FMT_UYVY: format = XRT_FORMAT_UYVY422; break;
388 case V4L2_PIX_FMT_MJPEG: format = XRT_FORMAT_MJPEG; break;
389 case V4L2_PIX_FMT_SGRBG8: format = XRT_FORMAT_BAYER_GR8; break;
390 default: V4L2_ERROR(vid, "error: Format not supported."); return;
391 }
392
393 // Allocate new descriptor.
394 struct v4l2_source_descriptor *desc = v4l2_add_descriptor(vid);
395
396 // Fill out the stream variables.
397 desc->stream.width = interval.width;
398 desc->stream.height = interval.height;
399 desc->stream.format = interval.pixel_format;
400 snprintf(desc->format_name, sizeof(desc->format_name), "%s", fmt->description);
401
402 if (u_format_is_blocks(format)) {
403 u_format_size_for_dimensions(format, interval.width, interval.height, &desc->stream.stride,
404 &desc->stream.size);
405 }
406
407 // Fill out the out sink variables.
408 desc->base.stereo_format = XRT_STEREO_FORMAT_NONE;
409 desc->base.format = format;
410 desc->base.width = desc->stream.width;
411 desc->base.height = desc->stream.height;
412}
413
414static void
415v4l2_list_modes_fmt(struct v4l2_fs *vid, const struct v4l2_fmtdesc *fmt)
416{
417 V4L2_DEBUG(vid, "format: %s %08x %d", fmt->description, fmt->pixelformat, fmt->type);
418
419 switch (fmt->pixelformat) {
420 case V4L2_PIX_FMT_YUYV: break;
421 case V4L2_PIX_FMT_UYVY: break;
422 case V4L2_PIX_FMT_MJPEG: break;
423 case V4L2_PIX_FMT_SGRBG8: break;
424 default: V4L2_ERROR(vid, "error: Unknown pixelformat '%s' '%08x'", fmt->description, fmt->pixelformat); return;
425 }
426
427 struct v4l2_frmsizeenum size = {0};
428 size.pixel_format = fmt->pixelformat;
429
430 while (ioctl(vid->fd, VIDIOC_ENUM_FRAMESIZES, &size) == 0) {
431 v4l2_list_modes_size(vid, fmt, &size);
432 size.index++;
433 }
434}
435
436static void
437v4l2_list_modes(struct v4l2_fs *vid)
438{
439 struct v4l2_fmtdesc desc;
440 U_ZERO(&desc);
441 desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
442
443 while (ioctl(vid->fd, VIDIOC_ENUM_FMT, &desc) == 0) {
444 v4l2_list_modes_fmt(vid, &desc);
445 desc.index++;
446 }
447}
448
449static void
450v4l2_set_control_if_diff(struct v4l2_fs *vid, struct v4l2_control_state *state)
451{
452 int value = 0;
453 int ret = 0;
454
455 struct v4l2_state_want *want = &state->want[vid->capture_type];
456 if (!want->active) {
457 return;
458 }
459
460 ret = v4l2_control_get(vid, state->id, &value);
461 if (ret != 0) {
462 return;
463 }
464
465 if (value == want->value && state->force <= 0) {
466 return;
467 }
468
469#if 0
470 dump_contron_name(state->id);
471
472 U_LOG_E(" ret: %i, want: %i, was: %i, force: %i", ret,
473 state->want, value, state->force);
474#endif
475
476 ret = v4l2_control_set(vid, state->id, want->value);
477 if (ret != 0) {
478 fprintf(stderr, "Failed to set ");
479 dump_contron_name(state->id);
480 fprintf(stderr, "\n");
481 return;
482 }
483
484 if (state->force > 0) {
485 state->force--;
486 }
487}
488
489static void
490v4l2_update_controls(struct v4l2_fs *vid)
491{
492 for (size_t i = 0; i < vid->num_states; i++) {
493 v4l2_set_control_if_diff(vid, &vid->states[i]);
494 }
495}
496
497
498bool
499v4l2_fs_setup_format(struct v4l2_fs *vid)
500{
501 if (vid->fd == -1) {
502 V4L2_ERROR(vid, "error: Device not opened!");
503 return false;
504 }
505
506
507 struct v4l2_source_descriptor *desc = &vid->descriptors[vid->selected];
508
509 struct v4l2_format v_format;
510 U_ZERO(&v_format);
511 v_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
512 v_format.fmt.pix.width = desc->stream.width;
513 v_format.fmt.pix.height = desc->stream.height;
514 v_format.fmt.pix.pixelformat = desc->stream.format;
515 v_format.fmt.pix.field = V4L2_FIELD_ANY;
516 if (vid->has.extended_format) {
517 v_format.fmt.pix.priv = V4L2_PIX_FMT_PRIV_MAGIC;
518 }
519
520 if (ioctl(vid->fd, VIDIOC_S_FMT, &v_format) < 0) {
521 V4L2_ERROR(vid, "Could not set up format!");
522 return false;
523 }
524 return true;
525}
526
527
528/*
529 *
530 * Exported functions.
531 *
532 */
533
534static bool
535v4l2_fs_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count)
536{
537 struct v4l2_fs *vid = v4l2_fs(xfs);
538 if (vid->num_descriptors == 0) {
539 return false;
540 }
541
542 struct xrt_fs_mode *modes = U_TYPED_ARRAY_CALLOC(struct xrt_fs_mode, vid->num_descriptors);
543 if (modes == NULL) {
544 return false;
545 }
546
547 for (uint32_t i = 0; i < vid->num_descriptors; i++) {
548 modes[i] = vid->descriptors[i].base;
549 }
550
551 *out_modes = modes;
552 *out_count = vid->num_descriptors;
553
554 return true;
555}
556
557static bool
558v4l2_fs_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp)
559{
560 // struct v4l2_fs *vid = v4l2_fs(xfs);
561 //! @todo
562 return false;
563}
564
565static bool
566v4l2_fs_stream_start(struct xrt_fs *xfs,
567 struct xrt_frame_sink *xs,
568 enum xrt_fs_capture_type capture_type,
569 uint32_t descriptor_index)
570{
571 struct v4l2_fs *vid = v4l2_fs(xfs);
572
573 if (descriptor_index >= vid->num_descriptors) {
574 V4L2_ERROR(vid, "error Invalid descriptor_index (%i >= %i)", descriptor_index, vid->num_descriptors);
575 return false;
576 }
577 vid->selected = descriptor_index;
578
579 vid->sink = xs;
580 vid->is_running = true;
581 vid->capture_type = capture_type;
582
583 if (!v4l2_fs_setup_format(vid)) {
584 vid->is_running = false;
585 return false;
586 }
587
588 if (pthread_create(&vid->stream_thread, NULL, v4l2_fs_mainloop, xfs)) {
589 vid->is_running = false;
590 V4L2_ERROR(vid, "error: Could not create thread");
591 return false;
592 }
593
594 V4L2_TRACE(vid, "info: Started!");
595
596 // we're off to the races!
597 return true;
598}
599
600static bool
601v4l2_fs_stream_stop(struct xrt_fs *xfs)
602{
603 struct v4l2_fs *vid = v4l2_fs(xfs);
604
605 if (!vid->is_running) {
606 return true;
607 }
608
609 vid->is_running = false;
610 pthread_join(vid->stream_thread, NULL);
611
612 return true;
613}
614
615static bool
616v4l2_fs_is_running(struct xrt_fs *xfs)
617{
618 struct v4l2_fs *vid = v4l2_fs(xfs);
619
620 return vid->is_running;
621}
622
623static void
624v4l2_fs_destroy(struct v4l2_fs *vid)
625{
626 // Make sure that the stream is stopped.
627 v4l2_fs_stream_stop(&vid->base);
628
629 // Stop the variable tracking.
630 u_var_remove_root(vid);
631 u_sink_debug_destroy(&vid->usd);
632
633 if (vid->descriptors != NULL) {
634 free(vid->descriptors);
635 vid->descriptors = NULL;
636 vid->num_descriptors = 0;
637 }
638
639 vid->capture.mmap = false;
640 if (vid->capture.userptr) {
641 vid->capture.userptr = false;
642 for (uint32_t i = 0; i < NUM_V4L2_BUFFERS; i++) {
643 free(vid->frames[i].mem);
644 vid->frames[i].mem = NULL;
645 }
646 }
647
648 if (vid->fd >= 0) {
649 close(vid->fd);
650 vid->fd = -1;
651 }
652
653 free(vid);
654}
655
656static void
657v4l2_fs_node_break_apart(struct xrt_frame_node *node)
658{
659 struct v4l2_fs *vid = container_of(node, struct v4l2_fs, node);
660 v4l2_fs_stream_stop(&vid->base);
661}
662
663static void
664v4l2_fs_node_destroy(struct xrt_frame_node *node)
665{
666 struct v4l2_fs *vid = container_of(node, struct v4l2_fs, node);
667 v4l2_fs_destroy(vid);
668}
669
670struct xrt_fs *
671v4l2_fs_create(struct xrt_frame_context *xfctx,
672 const char *path,
673 const char *product,
674 const char *manufacturer,
675 const char *serial)
676{
677 struct v4l2_fs *vid = U_TYPED_CALLOC(struct v4l2_fs);
678 vid->base.enumerate_modes = v4l2_fs_enumerate_modes;
679 vid->base.configure_capture = v4l2_fs_configure_capture;
680 vid->base.stream_start = v4l2_fs_stream_start;
681 vid->base.stream_stop = v4l2_fs_stream_stop;
682 vid->base.is_running = v4l2_fs_is_running;
683 vid->node.break_apart = v4l2_fs_node_break_apart;
684 vid->node.destroy = v4l2_fs_node_destroy;
685 vid->log_level = debug_get_log_option_v4l2_log();
686 vid->fd = -1;
687
688 snprintf(vid->base.product, sizeof(vid->base.product), "%s", product);
689 snprintf(vid->base.manufacturer, sizeof(vid->base.manufacturer), "%s", manufacturer);
690 snprintf(vid->base.serial, sizeof(vid->base.serial), "%s", serial);
691
692 int fd = open(path, O_RDWR, 0);
693 if (fd < 0) {
694 V4L2_ERROR(vid, "Cannot open '%s'", path);
695 free(vid);
696 return NULL;
697 }
698
699 vid->fd = fd;
700
701 int ret = v4l2_query_cap_and_validate(vid);
702 if (ret != 0) {
703 v4l2_fs_destroy(vid);
704 vid = NULL;
705 return NULL;
706 }
707
708 // It's now safe to add it to the context.
709 xrt_frame_context_add(xfctx, &vid->node);
710
711 // Start the variable tracking after we know what device we have.
712 u_sink_debug_init(&vid->usd);
713 u_var_add_root(vid, "V4L2 Frameserver", true);
714 u_var_add_ro_text(vid, vid->base.name, "Card");
715 u_var_add_log_level(vid, &vid->log_level, "Log Level");
716 for (size_t i = 0; i < vid->num_states; i++) {
717 u_var_add_i32(vid, &vid->states[i].want[0].value, vid->states[i].name);
718 }
719 u_var_add_sink_debug(vid, &vid->usd, "Output");
720
721 v4l2_list_modes(vid);
722
723 return &(vid->base);
724}
725
726void *
727v4l2_fs_mainloop(void *ptr)
728{
729 U_TRACE_SET_THREAD_NAME("V4L2");
730
731 struct xrt_fs *xfs = (struct xrt_fs *)ptr;
732 struct v4l2_fs *vid = v4l2_fs(xfs);
733
734 V4L2_DEBUG(vid, "info: Thread enter!");
735
736 if (vid->fd == -1) {
737 V4L2_ERROR(vid, "error: Device not opened!");
738 return NULL;
739 }
740
741 struct v4l2_source_descriptor *desc = &vid->descriptors[vid->selected];
742
743 // set up our buffers - prefer userptr (client alloc) vs mmap (kernel
744 // alloc)
745 // TODO: using buffer caps may be better than 'fallthrough to mmap'
746 struct v4l2_requestbuffers v_bufrequest;
747 U_ZERO(&v_bufrequest);
748 v_bufrequest.count = NUM_V4L2_BUFFERS;
749 v_bufrequest.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
750
751 if (v4l2_try_userptr(vid, &v_bufrequest) != 0 && v4l2_try_mmap(vid, &v_bufrequest) != 0) {
752 V4L2_ERROR(vid, "error: Driver does not support mmap or userptr.");
753 return NULL;
754 }
755
756
757 for (uint32_t i = 0; i < NUM_V4L2_BUFFERS; i++) {
758 struct v4l2_frame *vf = &vid->frames[i];
759 struct v4l2_buffer *v_buf = &vf->v_buf;
760
761 vf->base.owner = vid;
762 vf->base.destroy = v4l2_free_frame;
763
764 v_buf->index = i;
765 v_buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
766 v_buf->memory = v_bufrequest.memory;
767
768 if (ioctl(vid->fd, VIDIOC_QUERYBUF, v_buf) < 0) {
769 V4L2_ERROR(vid, "error: Could not query buffers!");
770 return NULL;
771 }
772
773 if (vid->capture.userptr && v4l2_setup_userptr_buffer(vid, vf, v_buf) != 0) {
774 return NULL;
775 }
776 if (vid->capture.mmap && v4l2_setup_mmap_buffer(vid, vf, v_buf) != 0) {
777 return NULL;
778 }
779
780 // Silence valgrind.
781 memset(vf->mem, 0, v_buf->length);
782
783 // Queue this buffer
784 if (ioctl(vid->fd, VIDIOC_QBUF, v_buf) < 0) {
785 V4L2_ERROR(vid, "error: queueing buffer failed!");
786 return NULL;
787 }
788 }
789
790 int start_capture = V4L2_BUF_TYPE_VIDEO_CAPTURE;
791 if (ioctl(vid->fd, VIDIOC_STREAMON, &start_capture) < 0) {
792 V4L2_ERROR(vid, "error: Could not start capture!");
793 return NULL;
794 }
795
796 /*
797 * Need to set these after we have started the stream.
798 */
799 v4l2_update_controls(vid);
800
801 struct v4l2_buffer v_buf = {0};
802 v_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
803 v_buf.memory = v_bufrequest.memory;
804
805 while (vid->is_running) {
806 if (vid->used_frames == NUM_V4L2_BUFFERS) {
807 V4L2_ERROR(vid, "No frames left");
808 }
809
810 if (ioctl(vid->fd, VIDIOC_DQBUF, &v_buf) < 0) {
811 V4L2_ERROR(vid, "error: Dequeue failed!");
812 vid->is_running = false;
813 break;
814 }
815
816 v4l2_update_controls(vid);
817
818 SINK_TRACE_IDENT(v4l2_fs_frame);
819
820 V4L2_TRACE(vid, "Got frame #%u, index %i. Used frames are %i", v_buf.sequence, v_buf.index,
821 vid->used_frames);
822
823 struct v4l2_frame *vf = &vid->frames[v_buf.index];
824 struct xrt_frame *xf = NULL;
825
826 xrt_frame_reference(&xf, &vf->base);
827 uint8_t *data = (uint8_t *)vf->mem;
828
829 //! @todo Sequence number and timestamp.
830 xf->width = desc->base.width;
831 xf->height = desc->base.height;
832 xf->format = desc->base.format;
833 xf->stereo_format = desc->base.stereo_format;
834
835 xf->data = data + desc->offset;
836 xf->stride = desc->stream.stride;
837 xf->size = v_buf.bytesused - desc->offset;
838 xf->source_id = vid->base.source_id;
839 xf->source_sequence = v_buf.sequence;
840
841 if ((v_buf.flags & V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC) != 0) {
842 xf->timestamp = os_timeval_to_ns(&v_buf.timestamp);
843 xf->source_timestamp = xf->timestamp;
844 }
845
846 vid->sink->push_frame(vid->sink, xf);
847
848 // Checks if active.
849 u_sink_debug_push_frame(&vid->usd, xf);
850
851 vid->used_frames++;
852
853 // The frame is requeued as soon as the refcount reaches zero,
854 // this can be done safely from another thread.
855 xrt_frame_reference(&xf, NULL);
856 }
857
858 V4L2_DEBUG(vid, "info: Thread leave!");
859
860 return NULL;
861}
862
863
864/*
865 *
866 * Helper debug functions.
867 *
868 */
869
870static void
871dump_integer(struct v4l2_fs *vid, struct v4l2_queryctrl *queryctrl)
872{
873 U_LOG_D(" Type: Integer");
874 U_LOG_D(" min: %i, max: %i, step: %i.", queryctrl->minimum, queryctrl->maximum, queryctrl->step);
875}
876
877static void
878dump_menu(struct v4l2_fs *vid, uint32_t id, uint32_t min, uint32_t max)
879{
880 U_LOG_D(" Menu items:");
881
882 struct v4l2_querymenu querymenu = {0};
883 querymenu.id = id;
884
885 for (querymenu.index = min; querymenu.index <= max; querymenu.index++) {
886 if (0 != ioctl(vid->fd, VIDIOC_QUERYMENU, &querymenu)) {
887 U_LOG_D(" %i", querymenu.index);
888 continue;
889 }
890 U_LOG_D(" %i: %s", querymenu.index, querymenu.name);
891 }
892}
893
894static void
895dump_contron_name(uint32_t id)
896{
897 const char *str = "ERROR";
898 switch (id) {
899#define CASE(CONTROL) \
900 case V4L2_CID_##CONTROL: str = "V4L2_CID_" #CONTROL; break
901 CASE(BRIGHTNESS);
902 CASE(CONTRAST);
903 CASE(SATURATION);
904 CASE(HUE);
905 CASE(AUDIO_VOLUME);
906 CASE(AUDIO_BALANCE);
907 CASE(AUDIO_BASS);
908 CASE(AUDIO_TREBLE);
909 CASE(AUDIO_MUTE);
910 CASE(AUDIO_LOUDNESS);
911 CASE(BLACK_LEVEL);
912 CASE(AUTO_WHITE_BALANCE);
913 CASE(DO_WHITE_BALANCE);
914 CASE(RED_BALANCE);
915 CASE(BLUE_BALANCE);
916 CASE(GAMMA);
917
918 CASE(EXPOSURE);
919 CASE(AUTOGAIN);
920 CASE(GAIN);
921#ifdef V4L2_CID_DIGITAL_GAIN
922 CASE(DIGITAL_GAIN);
923#endif
924 CASE(ANALOGUE_GAIN);
925 CASE(HFLIP);
926 CASE(VFLIP);
927 CASE(POWER_LINE_FREQUENCY);
928 CASE(POWER_LINE_FREQUENCY_DISABLED);
929 CASE(POWER_LINE_FREQUENCY_50HZ);
930 CASE(POWER_LINE_FREQUENCY_60HZ);
931 CASE(POWER_LINE_FREQUENCY_AUTO);
932 CASE(HUE_AUTO);
933 CASE(WHITE_BALANCE_TEMPERATURE);
934 CASE(SHARPNESS);
935 CASE(BACKLIGHT_COMPENSATION);
936 CASE(CHROMA_AGC);
937 CASE(CHROMA_GAIN);
938 CASE(COLOR_KILLER);
939 CASE(COLORFX);
940 CASE(COLORFX_CBCR);
941 CASE(AUTOBRIGHTNESS);
942 CASE(ROTATE);
943 CASE(BG_COLOR);
944 CASE(ILLUMINATORS_1);
945 CASE(ILLUMINATORS_2);
946 CASE(MIN_BUFFERS_FOR_CAPTURE);
947 CASE(MIN_BUFFERS_FOR_OUTPUT);
948 CASE(ALPHA_COMPONENT);
949
950 // Camera controls
951 CASE(EXPOSURE_AUTO);
952 CASE(EXPOSURE_ABSOLUTE);
953 CASE(EXPOSURE_AUTO_PRIORITY);
954 CASE(AUTO_EXPOSURE_BIAS);
955 CASE(PAN_RELATIVE);
956 CASE(TILT_RELATIVE);
957 CASE(PAN_RESET);
958 CASE(TILT_RESET);
959 CASE(PAN_ABSOLUTE);
960 CASE(TILT_ABSOLUTE);
961 CASE(FOCUS_ABSOLUTE);
962 CASE(FOCUS_RELATIVE);
963 CASE(FOCUS_AUTO);
964 CASE(ZOOM_ABSOLUTE);
965 CASE(ZOOM_RELATIVE);
966 CASE(ZOOM_CONTINUOUS);
967 CASE(PRIVACY);
968 CASE(IRIS_ABSOLUTE);
969 CASE(IRIS_RELATIVE);
970#undef CASE
971 default: fprintf(stderr, "0x%08x", id); return;
972 }
973 fprintf(stderr, "%s", str);
974}
975
976static void
977dump_controls(struct v4l2_fs *vid)
978{
979 struct v4l2_queryctrl queryctrl = {0};
980
981 queryctrl.id = V4L2_CTRL_FLAG_NEXT_CTRL;
982 while (0 == ioctl(vid->fd, VIDIOC_QUERYCTRL, &queryctrl)) {
983 fprintf(stderr, "Control ");
984 dump_contron_name(queryctrl.id);
985 fprintf(stderr, " '%s'", queryctrl.name);
986
987#define V_CHECK(FLAG) \
988 do { \
989 if (queryctrl.flags & V4L2_CTRL_FLAG_##FLAG) { \
990 fprintf(stderr, ", " #FLAG); \
991 } \
992 } while (false)
993
994 V_CHECK(DISABLED);
995 V_CHECK(GRABBED);
996 V_CHECK(READ_ONLY);
997 V_CHECK(UPDATE);
998 V_CHECK(INACTIVE);
999 V_CHECK(SLIDER);
1000 V_CHECK(WRITE_ONLY);
1001 V_CHECK(VOLATILE);
1002 V_CHECK(HAS_PAYLOAD);
1003 V_CHECK(EXECUTE_ON_WRITE);
1004#ifdef V4L2_CTRL_FLAG_MODIFY_LAYOUT
1005 V_CHECK(MODIFY_LAYOUT);
1006#endif
1007#undef V_CHECK
1008
1009 U_LOG_E(" ");
1010
1011 switch (queryctrl.type) {
1012 case V4L2_CTRL_TYPE_BOOLEAN: U_LOG_D(" Type: Boolean"); break;
1013 case V4L2_CTRL_TYPE_INTEGER: dump_integer(vid, &queryctrl); break;
1014 case V4L2_CTRL_TYPE_INTEGER64: U_LOG_D(" Type: Integer64"); break;
1015 case V4L2_CTRL_TYPE_BUTTON: U_LOG_D(" Type: Buttons"); break;
1016 case V4L2_CTRL_TYPE_MENU: dump_menu(vid, queryctrl.id, queryctrl.minimum, queryctrl.maximum); break;
1017 case V4L2_CTRL_TYPE_STRING: U_LOG_D(" Type: String"); break;
1018 default: U_LOG_D(" Type: Unknown"); break;
1019 }
1020
1021 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
1022 continue;
1023 }
1024
1025 queryctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
1026 }
1027}