1 // Copyright (c) 2010 The Chromium OS Authors. All rights reserved.
2 // Use of this source code is governed by a BSD-style license that can be
3 // found in the LICENSE file.
4
5 #include "media_v4l2_device.h"
6
7 #include <assert.h>
8 #include <time.h>
9 #include <sys/stat.h>
10
11 #include <string>
12 #include <utility>
13
14 #define CHECK(a) assert(a)
15 #define MAJOR(dev) (((uint32_t)(dev)) >> 8)
16 #define MINOR(dev) (((uint32_t)(dev)) & 0xff)
17 #define V4L2_VIDEO_CAPTURE_MAJOR 81
18 #define V4L2_VIDEO_CAPTURE_MINOR_MIN 0
19 #define V4L2_VIDEO_CAPTURE_MINOR_MAX 64
20
V4L2Device(const char * dev_name,uint32_t buffers)21 V4L2Device::V4L2Device(const char* dev_name,
22 uint32_t buffers)
23 : dev_name_(dev_name),
24 io_(IO_METHOD_UNDEFINED),
25 fd_(-1),
26 v4l2_buffers_(NULL),
27 num_buffers_(0),
28 min_buffers_(buffers),
29 stopped_(false),
30 initialized_(false) {
31 }
32
~V4L2Device()33 V4L2Device::~V4L2Device() {
34 if (initialized_)
35 UninitDevice();
36 CloseDevice();
37 }
38
OpenDevice()39 bool V4L2Device::OpenDevice() {
40 struct stat st;
41 if (-1 == stat(dev_name_, &st)) {
42 printf("<<< Error: could not find v4l2 device %s: (%d) %s.>>>\n",
43 dev_name_, errno, strerror(errno));
44 return false;
45 }
46
47 if (!S_ISCHR(st.st_mode)) {
48 printf("<<< Error: specified v4l2 device %s is not char device.>>>\n",
49 dev_name_);
50 return false;
51 }
52
53 if (MAJOR(st.st_rdev) != V4L2_VIDEO_CAPTURE_MAJOR
54 || MINOR(st.st_rdev) >= V4L2_VIDEO_CAPTURE_MINOR_MAX) {
55 printf("<<< Error: specified v4l2 device %s is not v4l2 device.>>>\n",
56 dev_name_);
57 return false;
58 }
59
60 fd_ = open(dev_name_, O_RDWR | O_NONBLOCK, 0);
61 if (-1 == fd_) {
62 printf("<<< Error: specified v4l2 device %s could not be opened.>>>\n",
63 dev_name_);
64 return false;
65 }
66
67 v4l2_capability cap;
68 if (!ProbeCaps(&cap))
69 return false;
70
71 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
72 printf("<<< Error: %s does not support video capture.>>>\n", dev_name_);
73 return false;
74 }
75
76 return true;
77 }
78
CloseDevice()79 void V4L2Device::CloseDevice() {
80 if (fd_ != -1)
81 close(fd_);
82 fd_ = -1;
83 }
84
InitDevice(IOMethod io,uint32_t width,uint32_t height,uint32_t pixfmt,float fps,ConstantFramerate constant_framerate,uint32_t num_skip_frames)85 bool V4L2Device::InitDevice(IOMethod io,
86 uint32_t width,
87 uint32_t height,
88 uint32_t pixfmt,
89 float fps,
90 ConstantFramerate constant_framerate,
91 uint32_t num_skip_frames) {
92 io_ = io;
93 // Crop/Format setting could live across session.
94 // We should always initialized them when supported.
95 v4l2_cropcap cropcap;
96 memset(&cropcap, 0, sizeof(cropcap));
97 if (GetCropCap(&cropcap)) {
98 v4l2_crop crop;
99 memset(&crop, 0, sizeof(crop));
100 // Use default capture rectangle.
101 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
102 crop.c = cropcap.defrect;
103 SetCrop(&crop);
104 }
105
106 v4l2_format fmt;
107 if (!GetV4L2Format(&fmt))
108 return false;
109
110 fmt.fmt.pix.width = width;
111 fmt.fmt.pix.height = height;
112 fmt.fmt.pix.pixelformat = pixfmt;
113 fmt.fmt.pix.field = V4L2_FIELD_NONE;
114
115 if (-1 == DoIoctl(VIDIOC_S_FMT, &fmt)) {
116 printf("<<< Error: VIDIOC_S_FMT on %s.>>>\n", dev_name_);
117 return false;
118 }
119
120 v4l2_capability cap;
121 if (!ProbeCaps(&cap))
122 return false;
123
124 switch (io_) {
125 case IO_METHOD_MMAP:
126 case IO_METHOD_USERPTR:
127 if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
128 printf("<<< Error: %s does not support streaming.>>>\n", dev_name_);
129 return false;
130 }
131 break;
132 default:
133 printf("<<< Error: IO method should be defined.>>>\n");
134 return false;
135 }
136
137 v4l2_streamparm param;
138 if (!GetParam(¶m))
139 return false;
140
141 if (param.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
142 if (fps > 0) {
143 SetFrameRate(fps);
144 } else {
145 printf("<<< Error: fps %f should be a positive number.>>>\n", fps);
146 return false;
147 }
148 }
149 float actual_fps = GetFrameRate();
150
151 int32_t constant_framerate_setting;
152 std::string constant_framerate_msg = "";
153 switch (constant_framerate) {
154 case DEFAULT_FRAMERATE_SETTING:
155 constant_framerate_setting = 1;
156 break;
157 case ENABLE_CONSTANT_FRAMERATE:
158 constant_framerate_setting = 0;
159 constant_framerate_msg = " with constant framerate";
160 break;
161 case DISABLE_CONSTANT_FRAMERATE:
162 constant_framerate_setting = 1;
163 constant_framerate_msg = " without constant framerate";
164 break;
165 default:
166 printf("<<< Error: Invalid constant framerate setting: %d. >>>\n",
167 constant_framerate);
168 return false;
169 }
170 SetControl(V4L2_CID_EXPOSURE_AUTO_PRIORITY, constant_framerate_setting);
171
172 printf("actual format for capture %dx%d %c%c%c%c picture at %.2f fps%s\n",
173 fmt.fmt.pix.width, fmt.fmt.pix.height,
174 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
175 (pixfmt >> 16) & 0xff, (pixfmt >> 24 ) & 0xff, actual_fps,
176 constant_framerate_msg.c_str());
177 frame_timestamps_.clear();
178 num_skip_frames_ = num_skip_frames;
179
180 bool ret = false;
181 switch (io_) {
182 case IO_METHOD_MMAP:
183 ret = InitMmapIO();
184 break;
185 case IO_METHOD_USERPTR:
186 ret = InitUserPtrIO(fmt.fmt.pix.sizeimage);
187 break;
188 default:
189 printf("<<< Error: IO method should be defined.>>>\n");
190 return false;
191 }
192 if (ret)
193 initialized_ = true;
194 return ret;
195 }
196
UninitDevice()197 bool V4L2Device::UninitDevice() {
198 v4l2_requestbuffers req;
199 memset(&req, 0, sizeof(req));
200 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
201 switch (io_) {
202 case IO_METHOD_MMAP:
203 for (uint32_t i = 0; i < num_buffers_; ++i)
204 if (-1 == munmap(v4l2_buffers_[i].start, v4l2_buffers_[i].length)) {
205 printf("<<< Error: munmap() on %s failed.>>>\n", dev_name_);
206 return false;
207 }
208
209 req.memory = V4L2_MEMORY_MMAP;
210 if (-1 == DoIoctl(VIDIOC_REQBUFS, &req)) {
211 printf("<<< Error: VIDIOC_REQBUFS for MMAP failed on %s: %s.>>>\n",
212 dev_name_, strerror(errno));
213 return false;
214 }
215 break;
216 case IO_METHOD_USERPTR:
217 req.memory = V4L2_MEMORY_USERPTR;
218 if (-1 == DoIoctl(VIDIOC_REQBUFS, &req)) {
219 printf("<<< Error: VIDIOC_REQBUFS for USERPTR failed on %s.: %s>>>\n",
220 dev_name_, strerror(errno));
221 return false;
222 }
223
224 for (uint32_t i = 0; i < num_buffers_; ++i)
225 free(v4l2_buffers_[i].start);
226 break;
227 default:
228 printf("<<< Error: IO method should be defined.>>>\n");
229 return false;
230 }
231 FreeBuffer();
232 initialized_ = false;
233 return true;
234 }
235
StartCapture()236 bool V4L2Device::StartCapture() {
237 for (uint32_t i = 0; i < num_buffers_; ++i) {
238 if (!EnqueueBuffer(i))
239 return false;
240 }
241 v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
242 if (-1 == DoIoctl(VIDIOC_STREAMON, &type)) {
243 printf("<<< Error: VIDIOC_STREAMON on %s.>>>\n", dev_name_);
244 return false;
245 }
246
247 uint32_t buf_index, data_size;
248 for (size_t i = 0; i < num_skip_frames_; i++) {
249 if (!ReadOneFrame(&buf_index, &data_size))
250 return false;
251 if (!EnqueueBuffer(buf_index))
252 return false;
253 }
254
255 return true;
256 }
257
StopCapture()258 bool V4L2Device::StopCapture() {
259 v4l2_buf_type type;
260 switch (io_) {
261 case IO_METHOD_MMAP:
262 case IO_METHOD_USERPTR:
263 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
264 if (-1 == DoIoctl(VIDIOC_STREAMOFF, &type)) {
265 printf("<<< Error: VIDIOC_STREAMOFF on %s.>>>\n", dev_name_);
266 return false;
267 }
268 break;
269 default:
270 printf("<<< Error: IO method should be defined.>>>\n");
271 return false;
272 }
273 return true;
274 }
275
ProcessImage(const void * p)276 void V4L2Device::ProcessImage(const void* p) {
277 printf(".");
278 fflush(stdout);
279 }
280
281 // Do capture for duration of |time_in_sec|.
Run(uint32_t time_in_sec)282 bool V4L2Device::Run(uint32_t time_in_sec) {
283 stopped_ = false;
284 if (!time_in_sec)
285 return false;
286
287 uint64_t start_in_nanosec = Now();
288 uint32_t buffer_index, data_size;
289 while (!stopped_) {
290 int32_t r = ReadOneFrame(&buffer_index, &data_size);
291 if (r < 0)
292 return false;
293 if (r) {
294 ProcessImage(v4l2_buffers_[buffer_index].start);
295 if (!EnqueueBuffer(buffer_index))
296 return false;
297 }
298 uint64_t end_in_nanosec = Now();
299 if ( end_in_nanosec - start_in_nanosec >= time_in_sec * 1000000000ULL)
300 break;
301 }
302 // All resolutions should have at least 1 fps.
303 float actual_fps = static_cast<float>(GetNumFrames()) / time_in_sec;
304 printf("\n<<< Info: Actual fps is %f on %s.>>>\n", actual_fps, dev_name_);
305 if (actual_fps < 1.0) {
306 printf("<<< Error: The actual fps is too low on %s.>>>\n", dev_name_);
307 return false;
308 }
309 return true;
310 }
311
Stop()312 bool V4L2Device::Stop() {
313 stopped_ = true;
314 return true;
315 }
316
DoIoctl(int32_t request,void * arg)317 int32_t V4L2Device::DoIoctl(int32_t request, void* arg) {
318 int32_t r;
319 do {
320 r = ioctl(fd_, request, arg);
321 } while (-1 == r && EINTR == errno);
322 return r;
323 }
324
325 // return 1 : successful to retrieve a frame from device
326 // return 0 : EAGAIN
327 // negative : error
ReadOneFrame(uint32_t * buffer_index,uint32_t * data_size)328 int32_t V4L2Device::ReadOneFrame(uint32_t* buffer_index, uint32_t* data_size) {
329 fd_set fds;
330 FD_ZERO(&fds);
331 FD_SET(fd_, &fds);
332 timeval tv;
333 tv.tv_sec = 2; // Normal timeout will be 2 seconds.
334 tv.tv_usec = 0;
335 int32_t r = select(fd_ + 1, &fds, NULL, NULL, &tv);
336 if (-1 == r) {
337 if (EINTR == errno) // If interrupted, try again.
338 return 0;
339 printf("<<< Error: select() failed on %s.>>>\n", dev_name_);
340 return -1;
341 }
342 if (0 == r) {
343 printf("<<< Error: select() timeout on %s.>>>\n", dev_name_);
344 return -1;
345 }
346
347 v4l2_buffer buf;
348 memset(&buf, 0, sizeof(buf));
349 switch (io_) {
350 case IO_METHOD_MMAP:
351 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
352 buf.memory = V4L2_MEMORY_MMAP;
353 if (-1 == DoIoctl(VIDIOC_DQBUF, &buf)) {
354 switch (errno) {
355 case EAGAIN:
356 return 0;
357 case EIO:
358 // Could ignore EIO, see spec.
359 // Fall through.
360 default:
361 printf("<<< Error: VIDIOC_DQBUF failed on %s.>>>\n", dev_name_);
362 return -2;
363 }
364 }
365 // We cannot use the timestamp in v4l2_buffer because
366 // 1. The time delta between the first and the second frame may be bigger
367 // because it includes sensor initialization time.
368 // 2. Even if we ignore the first frame timestamp, v4l2_buffer timestamps
369 // on Kevin are totally wrong for unknown reasons.
370 // 3. Kernel version <= 3.18 doesn't have the fix to disable hardware
371 // timestamp. https://patchwork.kernel.org/patch/6874491/
372 frame_timestamps_.push_back(Now());
373 CHECK(buf.index < num_buffers_);
374 // TODO: uvcvideo driver ignores this field. This is negligible,
375 // so disabling this for now until we get a fix into the upstream driver.
376 // CHECK(buf.field == V4L2_FIELD_NONE); // progressive only.
377 break;
378 case IO_METHOD_USERPTR:
379 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
380 buf.memory = V4L2_MEMORY_USERPTR;
381 if (-1 == DoIoctl(VIDIOC_DQBUF, &buf)) {
382 switch (errno) {
383 case EAGAIN:
384 return 0;
385 case EIO:
386 // Could ignore EIO, see spec.
387 // Fall through.
388 default:
389 printf("<<< Error: VIDIOC_DQBUF failed on %s.>>>\n", dev_name_);
390 return -2;
391 }
392 }
393 frame_timestamps_.push_back(Now());
394 CHECK(buf.index < num_buffers_);
395 break;
396 default:
397 printf("<<< Error: IO method should be defined.>>>\n");
398 return -1;
399 }
400 if (buffer_index)
401 *buffer_index = buf.index;
402 if (data_size)
403 *data_size = buf.bytesused;
404 return 1;
405 }
406
EnqueueBuffer(uint32_t buffer_index)407 bool V4L2Device::EnqueueBuffer(uint32_t buffer_index) {
408 v4l2_buffer buf;
409 memset(&buf, 0, sizeof(buf));
410 switch (io_) {
411 case IO_METHOD_MMAP:
412 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
413 buf.memory = V4L2_MEMORY_MMAP;
414 buf.index = buffer_index;
415 if (-1 == DoIoctl(VIDIOC_QBUF, &buf)) {
416 printf("<<< Error: VIDIOC_QBUF failed on %s.>>>\n", dev_name_);
417 return false;
418 }
419 break;
420 case IO_METHOD_USERPTR:
421 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
422 buf.memory = V4L2_MEMORY_USERPTR;
423 buf.index = buffer_index;
424 buf.m.userptr = (unsigned long) v4l2_buffers_[buffer_index].start;
425 buf.length = v4l2_buffers_[buffer_index].length;
426 if (-1 == DoIoctl(VIDIOC_QBUF, &buf)) {
427 printf("<<< Error: VIDIOC_QBUF failed on %s.>>>\n", dev_name_);
428 return false;
429 }
430 break;
431 default:
432 printf("<<< Error: IO method should be defined.>>>\n");
433 return false;
434 }
435 return true;
436 }
437
AllocateBuffer(uint32_t buffer_count)438 bool V4L2Device::AllocateBuffer(uint32_t buffer_count) {
439 v4l2_buffers_ = new Buffer[buffer_count];
440 if (!v4l2_buffers_) {
441 printf("<<< Error: Out of memory.>>>\n");
442 return false;
443 }
444 return true;
445 }
446
FreeBuffer()447 bool V4L2Device::FreeBuffer() {
448 free(v4l2_buffers_);
449 v4l2_buffers_ = NULL;
450 return true;
451 }
452
InitMmapIO()453 bool V4L2Device::InitMmapIO() {
454 v4l2_requestbuffers req;
455 memset(&req, 0, sizeof(req));
456 req.count = min_buffers_;
457 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
458 req.memory = V4L2_MEMORY_MMAP;
459 if (-1 == DoIoctl(VIDIOC_REQBUFS, &req)) {
460 if (EINVAL == errno)
461 printf("<<< Error: mmap() io is not supported on %s.>>>\n", dev_name_);
462 else
463 printf("<<< Error: VIDIOC_REQBUFS for MMAP(%d) failed on %s: %s.>>>\n",
464 min_buffers_, dev_name_, strerror(errno));
465 return false;
466 }
467
468 if (req.count < min_buffers_) {
469 printf("<<< Error: Insufficient buffer memory on %s >>>\n",
470 dev_name_); // TODO(jiesun) :add flexibilities.
471 return false;
472 }
473
474 if (!AllocateBuffer(req.count))
475 return false;
476
477 for (num_buffers_ = 0; num_buffers_ < req.count; ++num_buffers_) {
478 v4l2_buffer buf;
479 memset(&buf, 0, sizeof(buf));
480 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
481 buf.memory = V4L2_MEMORY_MMAP;
482 buf.index = num_buffers_;
483 if (-1 == DoIoctl(VIDIOC_QUERYBUF, &buf)) {
484 printf("<<< Error: VIDIOC_QUERYBUF failed on %s.>>>\n", dev_name_);
485 return false;
486 }
487 v4l2_buffers_[num_buffers_].length = buf.length;
488 v4l2_buffers_[num_buffers_].start =
489 mmap(NULL, // Start anywhere.
490 buf.length,
491 PROT_READ | PROT_WRITE,
492 MAP_SHARED,
493 fd_, buf.m.offset);
494 if (MAP_FAILED == v4l2_buffers_[num_buffers_].start) {
495 printf("<<< Error: mmap() failed on %s.>>>\n", dev_name_);
496 return false;
497 }
498 }
499 return true;
500 }
501
InitUserPtrIO(uint32_t buffer_size)502 bool V4L2Device::InitUserPtrIO(uint32_t buffer_size) {
503 v4l2_requestbuffers req;
504 memset(&req, 0, sizeof(req));
505 req.count = min_buffers_;
506 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
507 req.memory = V4L2_MEMORY_USERPTR;
508
509 // Align up buffer_size to page size boundary.
510 uint32_t page_size = getpagesize();
511 buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
512 if (-1 == DoIoctl(VIDIOC_REQBUFS, &req)) {
513 if (EINVAL == errno)
514 printf("<<< Error: user pointer is not supported on %s.>>>\n", dev_name_);
515 else
516 printf("<<< Error: VIDIOC_REQBUFS for USERPTR(%d) failed on %s: %s.>>>\n",
517 min_buffers_, dev_name_, strerror(errno));
518 return false;
519 }
520
521 if (!AllocateBuffer(4))
522 return false;
523
524 for (num_buffers_ = 0; num_buffers_ < min_buffers_; ++num_buffers_) {
525 v4l2_buffers_[num_buffers_].length = buffer_size;
526 v4l2_buffers_[num_buffers_].start = memalign(page_size, buffer_size);
527 if (!v4l2_buffers_[num_buffers_].start) {
528 printf("<<< Error: Out of memory.>>>\n");
529 return false;
530 }
531 }
532 return true;
533 }
534
EnumInput()535 bool V4L2Device::EnumInput() {
536 v4l2_input input;
537 int32_t index;
538 if (-1 == DoIoctl(VIDIOC_G_INPUT, &index)) {
539 printf("<<< Info: VIDIOC_G_INPUT not supported.>>>\n");
540 return false;
541 }
542
543 for (int32_t i = 0 ; ; ++i) {
544 memset(&input, 0, sizeof(input));
545 input.index = i;
546 if (-1 == DoIoctl(VIDIOC_ENUMINPUT, &input)) {
547 if (i == 0) {
548 printf("<<< Info: VIDIOC_ENUMINPUT not supported.>>>\n");
549 return false;
550 } else {
551 break;
552 }
553 }
554 printf("Current input: %s %s\n", input.name, i == index ? "*" : "");
555 }
556 return true;
557 }
558
EnumStandard()559 bool V4L2Device::EnumStandard() {
560 v4l2_input input;
561 v4l2_standard standard;
562 memset(&input, 0, sizeof(input));
563 if (-1 == DoIoctl(VIDIOC_G_INPUT, &input.index)) {
564 printf("<<< Info: VIDIOC_G_INPUT not supported.>>>\n");
565 return false;
566 }
567
568 if (-1 == DoIoctl(VIDIOC_ENUMINPUT, &input)) {
569 printf("<<< Info: VIDIOC_ENUMINPUT not supported.>>>\n");
570 return false;
571 }
572
573 printf("Current input %s supports:\n", input.name);
574 memset(&standard, 0, sizeof(standard));
575 standard.index = 0;
576 while (0 == DoIoctl(VIDIOC_ENUMSTD, &standard)) {
577 if (standard.id & input.std)
578 printf("%s\n", standard.name);
579 standard.index++;
580 }
581 // EINVAL indicates the end of the enumeration, which cannot be
582 // empty unless this device falls under the USB exception.
583 if (errno != EINVAL || standard.index == 0) {
584 printf("<<< Info: VIDIOC_ENUMSTD not supported.>>>\n");
585 return false;
586 }
587 return true;
588 }
589
EnumControl(bool show_menu)590 bool V4L2Device::EnumControl(bool show_menu) {
591 v4l2_queryctrl query_ctrl;
592 memset(&query_ctrl, 0, sizeof(query_ctrl));
593 // Query V4L2_CID_CAMERA_CLASS_BASE is for V4L2_CID_EXPOSURE_AUTO_PRIORITY.
594 std::vector<std::pair<uint32_t, uint32_t>> query_ctrl_sets;
595 query_ctrl_sets.push_back(std::make_pair(V4L2_CID_BASE, V4L2_CID_LASTP1));
596 query_ctrl_sets.push_back(std::make_pair(V4L2_CID_CAMERA_CLASS_BASE,
597 V4L2_CID_TILT_SPEED));
598
599 for (int i = 0; i < query_ctrl_sets.size(); i++) {
600 for (query_ctrl.id = query_ctrl_sets[i].first;
601 query_ctrl.id < query_ctrl_sets[i].second;
602 ++query_ctrl.id) {
603 if (0 == DoIoctl(VIDIOC_QUERYCTRL, &query_ctrl)) {
604 if (query_ctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
605 printf("Control %s is disabled\n", query_ctrl.name);
606 } else {
607 printf("Control %s is enabled(%d-%d:%d)\n",
608 query_ctrl.name, query_ctrl.minimum,
609 query_ctrl.maximum, query_ctrl.default_value);
610 }
611 if (query_ctrl.type == V4L2_CTRL_TYPE_MENU && show_menu)
612 EnumControlMenu(query_ctrl);
613 } else if (errno != EINVAL) {
614 printf("<<< Info: VIDIOC_query_ctrl not supported.>>>\n");
615 return false;
616 }
617 }
618 }
619
620 for (query_ctrl.id = V4L2_CID_PRIVATE_BASE;; query_ctrl.id++) {
621 if (0 == DoIoctl(VIDIOC_QUERYCTRL, &query_ctrl)) {
622 if (query_ctrl.flags & V4L2_CTRL_FLAG_DISABLED)
623 printf("Private Control %s is disabled\n", query_ctrl.name);
624 else
625 printf("Private Control %s is enabled\n", query_ctrl.name);
626 if (query_ctrl.type == V4L2_CTRL_TYPE_MENU && show_menu)
627 EnumControlMenu(query_ctrl);
628 } else {
629 // Assume private control ids are contiguous.
630 if (errno == EINVAL)
631 break;
632 printf("<<< Info: VIDIOC_query_ctrl not supported.>>>\n");
633 return false;
634 }
635 }
636 return true;
637 }
638
EnumControlMenu(const v4l2_queryctrl & query_ctrl)639 bool V4L2Device::EnumControlMenu(const v4l2_queryctrl& query_ctrl) {
640 v4l2_querymenu query_menu;
641 memset(&query_menu, 0, sizeof(query_menu));
642 printf("\t\tMenu items:\n");
643 query_menu.id = query_ctrl.id;
644 for (query_menu.index = query_ctrl.minimum;
645 query_menu.index <= query_ctrl.maximum;
646 ++query_menu.index) {
647 if (0 == DoIoctl(VIDIOC_QUERYMENU, &query_menu)) {
648 printf("\t\t\t%s\n", query_menu.name);
649 } else {
650 printf("<<< Info: VIDIOC_QUERYMENU not supported.>>>\n");
651 return false;
652 }
653 }
654 return true;
655 }
656
EnumFormat(uint32_t * num_formats,bool show_fmt)657 bool V4L2Device::EnumFormat(uint32_t* num_formats, bool show_fmt) {
658 uint32_t i;
659 for (i = 0; ; ++i) {
660 v4l2_fmtdesc format_desc;
661 memset(&format_desc, 0, sizeof(format_desc));
662 format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
663 format_desc.index = i;
664 if (-1 == DoIoctl(VIDIOC_ENUM_FMT, &format_desc)) {
665 if (i == 0) {
666 printf("<<< Info: VIDIOC_ENUM_FMT not supported.>>>\n");
667 return false;
668 } else {
669 break;
670 }
671 }
672 if (show_fmt)
673 printf("<<< Info supported format #%d: %s (%c%c%c%c) >>>\n",
674 i+1, format_desc.description,
675 (format_desc.pixelformat >> 0) & 0xff,
676 (format_desc.pixelformat >> 8) & 0xff,
677 (format_desc.pixelformat >> 16) & 0xff,
678 (format_desc.pixelformat >> 24) & 0xff);
679 }
680
681 if (num_formats)
682 *num_formats = i;
683 return true;
684 }
685
GetPixelFormat(uint32_t index,uint32_t * pixfmt)686 bool V4L2Device::GetPixelFormat(uint32_t index, uint32_t* pixfmt) {
687 v4l2_fmtdesc format_desc;
688 memset(&format_desc, 0, sizeof(format_desc));
689 format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
690 format_desc.index = index;
691 if (-1 == DoIoctl(VIDIOC_ENUM_FMT, &format_desc))
692 return false;
693 if (pixfmt)
694 *pixfmt = format_desc.pixelformat;
695 return true;
696 }
697
EnumFrameSize(uint32_t pixfmt,uint32_t * num_sizes,bool show_frmsize)698 bool V4L2Device::EnumFrameSize(
699 uint32_t pixfmt, uint32_t* num_sizes, bool show_frmsize) {
700 uint32_t i;
701 for (i = 0; ; ++i) {
702 v4l2_frmsizeenum frmsize_desc;
703 memset(&frmsize_desc, 0, sizeof(frmsize_desc));
704 frmsize_desc.pixel_format = pixfmt;
705 frmsize_desc.index = i;
706 if (-1 == DoIoctl(VIDIOC_ENUM_FRAMESIZES, &frmsize_desc)) {
707 if (i == 0) {
708 printf("<<< Info: VIDIOC_ENUM_FRAMESIZES not supported.>>>\n");
709 return false;
710 } else {
711 break;
712 }
713 }
714 if (show_frmsize) {
715 switch (frmsize_desc.type) {
716 case V4L2_FRMSIZE_TYPE_DISCRETE:
717 printf("<<< Info supported discrete frame size #%d:"
718 " for pixel format(%c%c%c%c): %dx%d >>>\n", i+1,
719 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
720 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
721 frmsize_desc.discrete.width,
722 frmsize_desc.discrete.height);
723 break;
724 case V4L2_FRMSIZE_TYPE_CONTINUOUS:
725 printf("<<< Info supported discrete frame size #%d:"
726 " for pixel format(%c%c%c%c): "
727 " from %dx%d to %dx%d >>>\n", i+1,
728 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
729 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
730 frmsize_desc.stepwise.min_width,
731 frmsize_desc.stepwise.min_height,
732 frmsize_desc.stepwise.max_width,
733 frmsize_desc.stepwise.max_height);
734 break;
735 case V4L2_FRMSIZE_TYPE_STEPWISE:
736 printf("<<< Info supported discrete frame size #%d:"
737 " for pixel format(%c%c%c%c): "
738 " from %dx%d to %dx%d step(%d,%d) >>>\n", i+1,
739 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
740 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
741 frmsize_desc.stepwise.min_width,
742 frmsize_desc.stepwise.min_height,
743 frmsize_desc.stepwise.max_width,
744 frmsize_desc.stepwise.max_height,
745 frmsize_desc.stepwise.step_width,
746 frmsize_desc.stepwise.step_height);
747 break;
748 }
749 }
750 }
751 if (num_sizes)
752 *num_sizes = i;
753 return true;
754 }
755
GetFrameSize(uint32_t index,uint32_t pixfmt,uint32_t * width,uint32_t * height)756 bool V4L2Device::GetFrameSize(
757 uint32_t index, uint32_t pixfmt, uint32_t *width, uint32_t *height) {
758 v4l2_frmsizeenum frmsize_desc;
759 memset(&frmsize_desc, 0, sizeof(frmsize_desc));
760 frmsize_desc.pixel_format = pixfmt;
761 frmsize_desc.index = index;
762 if (-1 == DoIoctl(VIDIOC_ENUM_FRAMESIZES, &frmsize_desc)) {
763 printf("<<< Error: VIDIOC_ENUM_FRAMESIZES not supported.>>>\n");
764 return false;
765 }
766 if (frmsize_desc.type != V4L2_FRMSIZE_TYPE_DISCRETE) {
767 printf("<<< Error: frame size type %d not supported.>>>\n",
768 frmsize_desc.type);
769 return false;
770 }
771
772 if (width && height) {
773 *width = frmsize_desc.discrete.width;
774 *height = frmsize_desc.discrete.height;
775 }
776 return true;
777 }
778
EnumFrameInterval(uint32_t pixfmt,uint32_t width,uint32_t height,uint32_t * num_intervals,bool show_intervals)779 bool V4L2Device::EnumFrameInterval(
780 uint32_t pixfmt, uint32_t width, uint32_t height, uint32_t* num_intervals,
781 bool show_intervals) {
782 uint32_t i;
783 for (i = 0; ; ++i) {
784 v4l2_frmivalenum frm_interval;
785 memset(&frm_interval, 0, sizeof(frm_interval));
786 frm_interval.pixel_format = pixfmt;
787 frm_interval.width = width;
788 frm_interval.height = height;
789 frm_interval.index = i;
790 if (-1 == DoIoctl(VIDIOC_ENUM_FRAMEINTERVALS, &frm_interval)) {
791 if (i == 0) {
792 printf("<<< Error: VIDIOC_ENUM_FRAMEINTERVALS not supported.>>>\n");
793 return false;
794 } else {
795 break;
796 }
797 }
798 if (show_intervals) {
799 switch(frm_interval.type) {
800 case V4L2_FRMIVAL_TYPE_DISCRETE:
801 printf("<<< Info supported discrete frame interval #%d:"
802 " for pixel format(%c%c%c%c): %dx%d: %d/%d >>>\n", i+1,
803 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
804 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
805 width, height, frm_interval.discrete.numerator,
806 frm_interval.discrete.denominator);
807 break;
808 case V4L2_FRMIVAL_TYPE_CONTINUOUS:
809 printf("<<< Info supported continuous frame interval #%d:"
810 " for pixel format(%c%c%c%c): %dx%d:"
811 " from %d/%d to %d/%d >>>\n", i+1,
812 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
813 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
814 width, height,
815 frm_interval.stepwise.min.numerator,
816 frm_interval.stepwise.min.denominator,
817 frm_interval.stepwise.max.numerator,
818 frm_interval.stepwise.max.denominator);
819 break;
820 case V4L2_FRMIVAL_TYPE_STEPWISE:
821 printf("<<< Info supported stepwise frame interval #%d:"
822 " for pixel format(%c%c%c%c): %dx%d:"
823 " from %d/%d to %d/%d step(%d,%d) >>>\n", i+1,
824 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
825 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
826 width, height,
827 frm_interval.stepwise.min.numerator,
828 frm_interval.stepwise.min.denominator,
829 frm_interval.stepwise.max.numerator,
830 frm_interval.stepwise.max.denominator,
831 frm_interval.stepwise.step.numerator,
832 frm_interval.stepwise.step.denominator);
833 break;
834 default:
835 printf("<<< Error: unsupported frame interval type %d: for index %d"
836 " pixel format(%c%c%c%c): %dx%d >>>\n", frm_interval.type,
837 i+1, (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
838 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff, width, height);
839 return false;
840 }
841 }
842 }
843 if (num_intervals)
844 *num_intervals = i;
845 return true;
846 }
847
GetFrameInterval(uint32_t index,uint32_t pixfmt,uint32_t width,uint32_t height,float * frame_rate)848 bool V4L2Device::GetFrameInterval(
849 uint32_t index, uint32_t pixfmt, uint32_t width, uint32_t height,
850 float* frame_rate) {
851 v4l2_frmivalenum frm_interval;
852 memset(&frm_interval, 0, sizeof(frm_interval));
853 frm_interval.pixel_format = pixfmt;
854 frm_interval.width = width;
855 frm_interval.height = height;
856 frm_interval.index = index;
857 if (-1 == DoIoctl(VIDIOC_ENUM_FRAMEINTERVALS, &frm_interval)) {
858 printf("<<< Error: VIDIOC_ENUM_FRAMEINTERVALS not supported.>>>\n");
859 return false;
860 }
861 if (frm_interval.type != V4L2_FRMIVAL_TYPE_DISCRETE) {
862 printf("<<< Error: frame interval type %d not supported.>>>\n",
863 frm_interval.type);
864 return false;
865 }
866
867 if (frame_rate) {
868 *frame_rate = static_cast<float>(frm_interval.discrete.denominator) /
869 frm_interval.discrete.numerator;
870 }
871 return true;
872 }
873
QueryControl(uint32_t id,v4l2_queryctrl * ctrl)874 bool V4L2Device::QueryControl(uint32_t id, v4l2_queryctrl* ctrl) {
875 memset(ctrl, 0, sizeof(*ctrl));
876 ctrl->id = id;
877 if (-1 == DoIoctl(VIDIOC_QUERYCTRL, ctrl)) {
878 if (errno != EINVAL) return false;
879 printf("%d is not supported\n", id);
880 return false;
881 }
882 if (ctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
883 printf("%d is not supported\n", id);
884 return false;
885 }
886 return true;
887 }
888
SetControl(uint32_t id,int32_t value)889 bool V4L2Device::SetControl(uint32_t id, int32_t value) {
890 v4l2_control control;
891 control.id = id;
892 control.value = value;
893 if (-1 == DoIoctl(VIDIOC_S_CTRL, &control)) {
894 printf("<<< Error: VIDIOC_S_CTRL failed. %d>>>\n", errno);
895 return false;
896 }
897 return true;
898 }
899
GetCropCap(v4l2_cropcap * cropcap)900 bool V4L2Device::GetCropCap(v4l2_cropcap* cropcap) {
901 if (-1 == DoIoctl(VIDIOC_CROPCAP, cropcap)) {
902 printf("<<< Warning: VIDIOC_CROPCAP not supported.>>>\n");
903 return false;
904 }
905 return true;
906 }
907
GetCrop(v4l2_crop * crop)908 bool V4L2Device::GetCrop(v4l2_crop* crop) {
909 if (-1 == DoIoctl(VIDIOC_G_CROP, crop)) {
910 printf("<<< Warning: VIDIOC_G_CROP not supported.>>>\n");
911 return false;
912 }
913 printf("crop: %d, %d, %d, %d\n",
914 crop->c.left, crop->c.top,
915 crop->c.width, crop->c.height);
916 return true;
917 }
918
SetCrop(v4l2_crop * crop)919 bool V4L2Device::SetCrop(v4l2_crop* crop) {
920 if (-1 == DoIoctl(VIDIOC_S_CROP, crop)) {
921 printf("<<< Warning: VIDIOC_S_CROP not supported.>>>\n");
922 return false;
923 }
924 return true;
925 }
926
ProbeCaps(v4l2_capability * cap,bool show_caps)927 bool V4L2Device::ProbeCaps(v4l2_capability* cap, bool show_caps) {
928 if (-1 == DoIoctl(VIDIOC_QUERYCAP, cap)) {
929 printf("<<< Error: VIDIOC_QUERYCAP on %s.>>>\n", dev_name_);
930 return false;
931 }
932
933 if (show_caps) {
934 if (cap->capabilities & V4L2_CAP_VIDEO_CAPTURE)
935 printf("<<< Info: %s support video capture interface.>>>\n", dev_name_);
936 if (cap->capabilities & V4L2_CAP_VIDEO_OUTPUT)
937 printf("<<< Info: %s support video output interface.>>>\n", dev_name_);
938 if (cap->capabilities & V4L2_CAP_VIDEO_OVERLAY)
939 printf("<<< Info: %s support video overlay interface.>>>\n", dev_name_);
940 if (cap->capabilities & V4L2_CAP_AUDIO)
941 printf("<<< Info: %s support audio i/o interface.>>>\n", dev_name_);
942
943 if (cap->capabilities & V4L2_CAP_STREAMING)
944 printf("<<< Info: %s support streaming i/o interface.>>>\n", dev_name_);
945 }
946
947 return true;
948 }
949
MapFourCC(const char * fourcc)950 uint32_t V4L2Device::MapFourCC(const char* fourcc) {
951 return v4l2_fourcc(fourcc[0], fourcc[1], fourcc[2], fourcc[3]);
952 }
953
GetParam(v4l2_streamparm * param)954 bool V4L2Device::GetParam(v4l2_streamparm* param) {
955 param->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
956 if (-1 == DoIoctl(VIDIOC_G_PARM, param)) {
957 printf("<<< Warning: VIDIOC_G_PARM not supported.>>>\n");
958 return false;
959 }
960
961 return true;
962 }
963
SetParam(v4l2_streamparm * param)964 bool V4L2Device::SetParam(v4l2_streamparm* param) {
965 if (-1 == DoIoctl(VIDIOC_S_PARM, param)) {
966 printf("<<< Warning: VIDIOC_S_PARM not supported.>>>\n");
967 return false;
968 }
969 return true;
970 }
971
SetFrameRate(float fps)972 bool V4L2Device::SetFrameRate(float fps) {
973 v4l2_streamparm param;
974 if (!GetParam(¶m))
975 return false;
976
977 const int kFrameRatePrecision = 10000;
978 param.parm.capture.timeperframe.numerator = kFrameRatePrecision;
979 param.parm.capture.timeperframe.denominator = fps * kFrameRatePrecision;
980 return SetParam(¶m);
981 }
982
GetFrameRate()983 float V4L2Device::GetFrameRate() {
984 v4l2_streamparm param;
985 if (!GetParam(¶m))
986 return -1;
987 return static_cast<float>(param.parm.capture.timeperframe.denominator) /
988 param.parm.capture.timeperframe.numerator;
989 }
990
GetV4L2Format(v4l2_format * format)991 bool V4L2Device::GetV4L2Format(v4l2_format* format) {
992 memset(format, 0, sizeof(v4l2_format));
993 format->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
994
995 if (-1 == DoIoctl(VIDIOC_G_FMT, format)) {
996 printf("<<< Error: VIDIOC_G_FMT on %s.>>>\n", dev_name_);
997 return false;
998 }
999 return true;
1000 }
1001
Now()1002 uint64_t V4L2Device::Now() {
1003 struct timespec ts;
1004 int res = clock_gettime(CLOCK_MONOTONIC, &ts);
1005 CHECK(res == 0);
1006 return ts.tv_sec * 1000000000ULL + ts.tv_nsec;
1007 }
1008