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#define CHECK(a) assert(a)
12#define MAJOR(dev) (((uint32_t)(dev)) >> 8)
13#define MINOR(dev) (((uint32_t)(dev)) & 0xff)
14#define V4L2_VIDEO_CAPTURE_MAJOR      81
15#define V4L2_VIDEO_CAPTURE_MINOR_MIN  0
16#define V4L2_VIDEO_CAPTURE_MINOR_MAX  64
17
18V4L2Device::V4L2Device(const char* dev_name,
19                       IOMethod io,
20                       uint32_t buffers)
21    : dev_name_(dev_name),
22      io_(io),
23      fd_(-1),
24      v4l2_buffers_(NULL),
25      num_buffers_(0),
26      min_buffers_(buffers),
27      stopped_(false) {
28}
29
30bool V4L2Device::OpenDevice() {
31  struct stat st;
32  if (-1 == stat(dev_name_, &st)) {
33    printf("<<< Error: could not find v4l2 device %s: (%d) %s.>>>\n",
34           dev_name_, errno, strerror(errno));
35    return false;
36  }
37
38  if (!S_ISCHR(st.st_mode)) {
39    printf("<<< Error: specified v4l2 device %s is not char device.>>>\n",
40           dev_name_);
41    return false;
42  }
43
44  if (MAJOR(st.st_rdev) != V4L2_VIDEO_CAPTURE_MAJOR
45      || MINOR(st.st_rdev) >= V4L2_VIDEO_CAPTURE_MINOR_MAX) {
46    printf("<<< Error: specified v4l2 device %s is not v4l2 device.>>>\n",
47           dev_name_);
48    return false;
49  }
50
51  fd_ = open(dev_name_, O_RDWR | O_NONBLOCK, 0);
52  if (-1 == fd_) {
53    printf("<<< Error: specified v4l2 device %s could not be opened.>>>\n",
54           dev_name_);
55    return false;
56  }
57
58  v4l2_capability cap;
59  if (!ProbeCaps(&cap))
60    return false;
61
62  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
63    printf("<<< Error: %s does not support video capture.>>>\n", dev_name_);
64    return false;
65  }
66
67  switch (io_) {
68    case IO_METHOD_READ:
69      if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
70        printf("<<< Error: %s does not support read i/o.>>>\n", dev_name_);
71        return false;
72      }
73      break;
74    case IO_METHOD_MMAP:
75    case IO_METHOD_USERPTR:
76      if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
77        printf("<<< Error: %s does not support streaming.>>>\n", dev_name_);
78        return false;
79      }
80      break;
81  }
82
83  return true;
84}
85
86void V4L2Device::CloseDevice() {
87  if (fd_ != -1)
88    close(fd_);
89  fd_ = -1;
90}
91
92bool V4L2Device::InitDevice(uint32_t width,
93                            uint32_t height,
94                            uint32_t pixfmt,
95                            uint32_t fps) {
96  // Crop/Format setting could live across session.
97  // We should always initialized them when supported.
98  v4l2_cropcap cropcap;
99  memset(&cropcap, 0, sizeof(cropcap));
100  if (GetCropCap(&cropcap)) {
101    v4l2_crop crop;
102    memset(&crop, 0, sizeof(crop));
103    // Use default capture rectangle.
104    crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
105    crop.c = cropcap.defrect;
106    SetCrop(&crop);
107  }
108
109  v4l2_format fmt;
110  memset(&fmt, 0, sizeof(fmt));
111  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
112
113  if (-1 == DoIoctl(VIDIOC_G_FMT, &fmt)) {
114    printf("<<< Error: VIDIOC_G_FMT on %s.>>>\n", dev_name_);
115    return false;
116  }
117
118  fmt.fmt.pix.width = width;
119  fmt.fmt.pix.height = height;
120  fmt.fmt.pix.pixelformat = pixfmt;
121  fmt.fmt.pix.field = V4L2_FIELD_NONE;
122
123  if (-1 == DoIoctl(VIDIOC_S_FMT, &fmt)) {
124    printf("<<< Error: VIDIOC_S_FMT on %s.>>>\n", dev_name_);
125    return false;
126  }
127
128  v4l2_capability cap;
129  if (!ProbeCaps(&cap))
130    return false;
131
132  if (cap.capabilities & V4L2_CAP_TIMEPERFRAME) {
133    if (fps > 0)
134      SetFrameRate(fps);
135    fps = GetFrameRate();
136  } else {
137    // TODO(jiesun): probably we should derive this from VIDIOC_G_STD
138    fps = 30;
139  }
140
141  printf("actual format for capture %dx%d %c%c%c%c picture at %d fps\n",
142         fmt.fmt.pix.width, fmt.fmt.pix.height,
143         (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
144         (pixfmt >> 16) & 0xff, (pixfmt >> 24 ) & 0xff, fps);
145  width_ = fmt.fmt.pix.width;
146  height_ = fmt.fmt.pix.height;
147  pixfmt_ = fmt;
148
149  switch (io_) {
150    case IO_METHOD_READ:
151      return InitReadIO(fmt.fmt.pix.sizeimage);
152    case IO_METHOD_MMAP:
153      return InitMmapIO();
154    case IO_METHOD_USERPTR:
155      return InitUserPtrIO(fmt.fmt.pix.sizeimage);
156  }
157  return false;
158}
159
160bool V4L2Device::UninitDevice() {
161  switch (io_) {
162    case IO_METHOD_READ:
163      // Only one buffer for read() i/o.
164      free(v4l2_buffers_[0].start);
165      break;
166    case IO_METHOD_MMAP:
167      for (uint32_t i = 0; i < num_buffers_; ++i)
168        if (-1 == munmap(v4l2_buffers_[i].start, v4l2_buffers_[i].length)) {
169          printf("<<< Error: munmap() on %s failed.>>>\n", dev_name_);
170          return false;
171        }
172      break;
173    case IO_METHOD_USERPTR:
174      for (uint32_t i = 0; i < num_buffers_; ++i)
175        free(v4l2_buffers_[i].start);
176      break;
177  }
178  FreeBuffer();
179  return true;
180}
181
182bool V4L2Device::StartCapture() {
183  v4l2_buffer buf;
184  uint32_t i;
185  v4l2_buf_type type;
186  switch (io_) {
187    case IO_METHOD_READ:
188      // Nothing to do.
189      break;
190    case IO_METHOD_MMAP:
191      for (i = 0; i < num_buffers_; ++i) {
192        memset(&buf, 0, sizeof(buf));
193        buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
194        buf.memory = V4L2_MEMORY_MMAP;
195        buf.index  = i;
196        if (-1 == DoIoctl(VIDIOC_QBUF, &buf)) {
197          printf("<<< Error: VIDIOC_QBUF on %s.>>>\n", dev_name_);
198          return false;
199        }
200      }
201      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
202      if (-1 == DoIoctl(VIDIOC_STREAMON, &type)) {
203        printf("<<< Error: VIDIOC_STREAMON on %s.>>>\n", dev_name_);
204        return false;
205      }
206      break;
207    case IO_METHOD_USERPTR:
208      for (i = 0; i < num_buffers_; ++i) {
209        memset(&buf, 0, sizeof(buf));
210        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
211        buf.memory = V4L2_MEMORY_USERPTR;
212        buf.index = i;
213        buf.m.userptr = (unsigned long) v4l2_buffers_[i].start;
214        buf.length = v4l2_buffers_[i].length;
215        if (-1 == DoIoctl(VIDIOC_QBUF, &buf)) {
216          printf("<<< Error: VIDIOC_QBUF on %s.>>>\n", dev_name_);
217          return false;
218        }
219      }
220      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
221      if (-1 == DoIoctl(VIDIOC_STREAMON, &type)) {
222        printf("<<< Error: VIDIOC_STREAMON on %s.>>>\n", dev_name_);
223        return false;
224      }
225      break;
226  }
227  return true;
228}
229
230bool V4L2Device::StopCapture() {
231  v4l2_buf_type type;
232  switch (io_) {
233    case IO_METHOD_READ:
234      // Nothing to do.
235      break;
236    case IO_METHOD_MMAP:
237    case IO_METHOD_USERPTR:
238      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
239      if (-1 == DoIoctl(VIDIOC_STREAMOFF, &type)) {
240        printf("<<< Error: VIDIOC_STREAMOFF on %s.>>>\n", dev_name_);
241        return false;
242      }
243      break;
244  }
245  return true;
246}
247
248void V4L2Device::ProcessImage(const void* p) {
249  printf(".");
250  fflush(stdout);
251}
252
253// Do capture for number of |frames| ( when time_in_sec == 0 )
254// or for duration of |time_in_sec|  ( when time_in_sec > 0 ).
255bool V4L2Device::Run(uint32_t frames, uint32_t time_in_sec) {
256  stopped_ = false;
257  if (time_in_sec) // duration setting override the frames setting.
258    frames = 30 * time_in_sec; // Assume maximum fps is 30.
259
260  uint64_t start_in_sec = Now();
261  int32_t timeout = 5;  // Used 5 seconds for initial delay.
262  while (!stopped_ && frames > 0) {
263    fd_set fds;
264    FD_ZERO(&fds);
265    FD_SET(fd_, &fds);
266    timeval tv;
267    tv.tv_sec = timeout;
268    tv.tv_usec = 0;
269    timeout = 2;  // Normal timeout will be 2 seconds.
270    int32_t r = select(fd_ + 1, &fds, NULL, NULL, &tv);
271    if (-1 == r) {
272      if (EINTR == errno)  // If interrupted, continue.
273        continue;
274      printf("<<< Error: select() failed on %s.>>>\n", dev_name_);
275      return false;
276    }
277    if (0 == r) {
278      printf("<<< Error: select() timeout on %s.>>>\n", dev_name_);
279      return false;
280    }
281    r = ReadOneFrame();
282    if (r < 0)
283      return false;
284    if (r)
285      frames--;
286    if (time_in_sec) {
287      uint64_t end_in_sec = Now();
288      if ( end_in_sec - start_in_sec >= time_in_sec )
289        return true;
290    }
291  }
292  return true;
293}
294
295bool V4L2Device::Stop() {
296  stopped_ = true;
297}
298
299int32_t V4L2Device::DoIoctl(int32_t request, void* arg) {
300  int32_t r;
301  do {
302    r = ioctl(fd_, request, arg);
303  } while (-1 == r && EINTR == errno);
304  return r;
305}
306
307// return 1 : successful to retrieve a frame from device
308// return 0 : EAGAIN
309// negative : error
310int32_t V4L2Device::ReadOneFrame() {
311  v4l2_buffer buf;
312  memset(&buf, 0, sizeof(buf));
313  uint32_t i;
314  switch (io_) {
315    case IO_METHOD_READ:
316      if (-1 == read(fd_, v4l2_buffers_[0].start, v4l2_buffers_[0].length)) {
317        switch (errno) {
318          case EAGAIN:
319            return 0;
320          case EIO:
321            // Could ignore EIO, see spec.
322            // Fall through.
323          default:
324            printf("<<< Error: read() failed on %s.>>>\n", dev_name_);
325            return -1;
326        }
327      }
328      ProcessImage(v4l2_buffers_[0].start);
329      break;
330    case IO_METHOD_MMAP:
331      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
332      buf.memory = V4L2_MEMORY_MMAP;
333      if (-1 == DoIoctl(VIDIOC_DQBUF, &buf)) {
334        switch (errno) {
335          case EAGAIN:
336            return 0;
337          case EIO:
338            // Could ignore EIO, see spec.
339            // Fall through.
340          default:
341            printf("<<< Error: VIDIOC_DQBUF failed on %s.>>>\n", dev_name_);
342            return -2;
343        }
344      }
345      CHECK(buf.index < num_buffers_);
346      // TODO: uvcvideo driver ignores this field. This is negligible,
347      // so disabling this for now until we get a fix into the upstream driver.
348      // CHECK(buf.field == V4L2_FIELD_NONE);  // progressive only.
349      ProcessImage(v4l2_buffers_[buf.index].start);
350      if (-1 == DoIoctl(VIDIOC_QBUF, &buf)) {
351        printf("<<< Error: VIDIOC_QBUF failed on %s.>>>\n", dev_name_);
352        return -3;
353      }
354      break;
355    case IO_METHOD_USERPTR:
356      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
357      buf.memory = V4L2_MEMORY_USERPTR;
358      if (-1 == DoIoctl(VIDIOC_DQBUF, &buf)) {
359        switch (errno) {
360          case EAGAIN:
361            return 0;
362          case EIO:
363            // Could ignore EIO, see spec.
364            // Fall through.
365          default:
366            printf("<<< Error: VIDIOC_DQBUF failed on %s.>>>\n", dev_name_);
367            return -2;
368        }
369      }
370      for (i = 0; i < num_buffers_; ++i) {
371        if (buf.m.userptr == (unsigned long) v4l2_buffers_[i].start
372            && buf.length == v4l2_buffers_[i].length)
373          break;
374      }
375      CHECK(i < num_buffers_);
376      ProcessImage(reinterpret_cast<void*>(buf.m.userptr));
377      if (-1 == DoIoctl(VIDIOC_QBUF, &buf)) {
378        printf("<<< Error: VIDIOC_QBUF failed on %s.>>>\n", dev_name_);
379        return -3;
380      }
381      break;
382  }
383  return 1;
384}
385
386bool V4L2Device::AllocateBuffer(uint32_t buffer_count) {
387  v4l2_buffers_ = new Buffer[buffer_count];
388  if (!v4l2_buffers_) {
389    printf("<<< Error: Out of memory.>>>\n");
390    return false;
391  }
392  return true;
393}
394
395bool V4L2Device::FreeBuffer() {
396  free(v4l2_buffers_);
397  v4l2_buffers_ = NULL;
398  return true;
399}
400
401bool V4L2Device::InitReadIO(uint32_t buffer_size) {
402  if (!AllocateBuffer(1))
403    return false;
404  v4l2_buffers_[0].length = buffer_size;
405  v4l2_buffers_[0].start = new uint8_t[buffer_size];
406  if (!v4l2_buffers_[0].start) {
407    printf("<<< Error: Out of memory.>>>\n");
408    return false;
409  }
410  return true;
411}
412
413bool V4L2Device::InitMmapIO() {
414  v4l2_requestbuffers req;
415  memset(&req, 0, sizeof(req));
416  req.count = min_buffers_;
417  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
418  req.memory = V4L2_MEMORY_MMAP;
419  if (-1 == DoIoctl(VIDIOC_REQBUFS, &req)) {
420    if (EINVAL == errno)
421      printf("<<< Error: mmap() io is not supported on %s.>>>\n", dev_name_);
422    else
423      printf("<<< Error: VIDIOC_REQBUFS failed on %s.>>>\n", dev_name_);
424    return false;
425  }
426
427  if (req.count < min_buffers_) {
428    printf("<<< Error: Insufficient buffer memory on %s >>>\n",
429            dev_name_);  // TODO(jiesun) :add flexibilities.
430    return false;
431  }
432
433  if (!AllocateBuffer(req.count))
434    return false;
435
436  for (num_buffers_ = 0; num_buffers_ < req.count; ++num_buffers_) {
437    v4l2_buffer buf;
438    memset(&buf, 0, sizeof(buf));
439    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
440    buf.memory = V4L2_MEMORY_MMAP;
441    buf.index = num_buffers_;
442    if (-1 == DoIoctl(VIDIOC_QUERYBUF, &buf)) {
443      printf("<<< Error: VIDIOC_QUERYBUF failed on %s.>>>\n", dev_name_);
444      return false;
445    }
446    v4l2_buffers_[num_buffers_].length = buf.length;
447    v4l2_buffers_[num_buffers_].start =
448        mmap(NULL,  // Start anywhere.
449             buf.length,
450             PROT_READ | PROT_WRITE,
451             MAP_SHARED,
452             fd_, buf.m.offset);
453    if (MAP_FAILED == v4l2_buffers_[num_buffers_].start) {
454      printf("<<< Error: mmap() failed on %s.>>>\n", dev_name_);
455      return false;
456    }
457  }
458  return true;
459}
460
461bool V4L2Device::InitUserPtrIO(uint32_t buffer_size) {
462  v4l2_requestbuffers req;
463  memset(&req, 0, sizeof(req));
464  req.count = min_buffers_;
465  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
466  req.memory = V4L2_MEMORY_USERPTR;
467
468  // Align up buffer_size to page size boundary.
469  uint32_t page_size = getpagesize();
470  buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
471  if (-1 == DoIoctl(VIDIOC_REQBUFS, &req)) {
472    if (EINVAL == errno)
473      printf("<<< Error: user pointer is not supported on %s.>>>\n", dev_name_);
474    else
475      printf("<<< Error: VIDIOC_REQBUFS failed on %s.>>>\n", dev_name_);
476    return false;
477  }
478
479  if (!AllocateBuffer(4))
480    return false;
481
482  for (num_buffers_ = 0; num_buffers_ < min_buffers_; ++num_buffers_) {
483    v4l2_buffers_[num_buffers_].length = buffer_size;
484    v4l2_buffers_[num_buffers_].start = memalign(page_size, buffer_size);
485    if (!v4l2_buffers_[num_buffers_].start) {
486      printf("<<< Error: Out of memory.>>>\n");
487      return false;
488    }
489  }
490  return true;
491}
492
493bool V4L2Device::EnumInput() {
494  v4l2_input input;
495  int32_t index;
496  if (-1 == DoIoctl(VIDIOC_G_INPUT, &index)) {
497    printf("<<< Info: VIDIOC_G_INPUT not supported.>>>\n");
498    return false;
499  }
500
501  for (int32_t i = 0 ; ; ++i) {
502    memset(&input, 0, sizeof(input));
503    input.index = i;
504    if (-1 == DoIoctl(VIDIOC_ENUMINPUT, &input)) {
505      if (i == 0) {
506        printf("<<< Info: VIDIOC_ENUMINPUT not supported.>>>\n");
507        return false;
508      } else {
509        break;
510      }
511    }
512    printf("Current input: %s %s\n", input.name, i == index ? "*" : "");
513  }
514  return true;
515}
516
517bool V4L2Device::EnumStandard() {
518  v4l2_input input;
519  v4l2_standard standard;
520  memset(&input, 0, sizeof(input));
521  if (-1 == DoIoctl(VIDIOC_G_INPUT, &input.index)) {
522    printf("<<< Info: VIDIOC_G_INPUT not supported.>>>\n");
523    return false;
524  }
525
526  if (-1 == DoIoctl(VIDIOC_ENUMINPUT, &input)) {
527    printf("<<< Info: VIDIOC_ENUMINPUT not supported.>>>\n");
528    return false;
529  }
530
531  printf("Current input %s supports:\n", input.name);
532  memset(&standard, 0, sizeof(standard));
533  standard.index = 0;
534  while (0 == DoIoctl(VIDIOC_ENUMSTD, &standard)) {
535    if (standard.id & input.std)
536      printf("%s\n", standard.name);
537    standard.index++;
538  }
539  // EINVAL indicates the end of the enumeration, which cannot be
540  // empty unless this device falls under the USB exception.
541  if (errno != EINVAL || standard.index == 0) {
542    printf("<<< Info: VIDIOC_ENUMSTD not supported.>>>\n");
543    return false;
544  }
545  return true;
546}
547
548bool V4L2Device::EnumControl(bool show_menu) {
549  v4l2_queryctrl query_ctrl;
550  memset(&query_ctrl, 0, sizeof(query_ctrl));
551  for (query_ctrl.id = V4L2_CID_BASE;
552       query_ctrl.id < V4L2_CID_LASTP1;
553       ++query_ctrl.id) {
554    if (0 == DoIoctl(VIDIOC_QUERYCTRL, &query_ctrl)) {
555      if (query_ctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
556          printf("Control %s is disabled\n", query_ctrl.name);
557      } else {
558          printf("Control %s is enabled(%d-%d:%d)\n",
559                 query_ctrl.name, query_ctrl.minimum,
560                 query_ctrl.maximum, query_ctrl.default_value);
561      }
562      if (query_ctrl.type == V4L2_CTRL_TYPE_MENU && show_menu)
563        EnumControlMenu(query_ctrl);
564    } else if (errno != EINVAL) {
565      printf("<<< Info: VIDIOC_query_ctrl not supported.>>>\n");
566      return false;
567    }
568  }
569
570  for (query_ctrl.id = V4L2_CID_PRIVATE_BASE;; query_ctrl.id++) {
571    if (0 == DoIoctl(VIDIOC_QUERYCTRL, &query_ctrl)) {
572      if (query_ctrl.flags & V4L2_CTRL_FLAG_DISABLED)
573        printf("Private Control %s is disabled\n", query_ctrl.name);
574      else
575        printf("Private Control %s is enabled\n", query_ctrl.name);
576      if (query_ctrl.type == V4L2_CTRL_TYPE_MENU && show_menu)
577        EnumControlMenu(query_ctrl);
578    } else {
579      // Assume private control ids are contiguous.
580      if (errno == EINVAL)
581        break;
582      printf("<<< Info: VIDIOC_query_ctrl not supported.>>>\n");
583      return false;
584    }
585  }
586  return true;
587}
588
589bool V4L2Device::EnumControlMenu(const v4l2_queryctrl& query_ctrl) {
590  v4l2_querymenu query_menu;
591  memset(&query_menu, 0, sizeof(query_menu));
592  printf("\t\tMenu items:\n");
593  query_menu.id = query_ctrl.id;
594  for (query_menu.index = query_ctrl.minimum;
595       query_menu.index <= query_ctrl.maximum;
596       ++query_menu.index) {
597    if (0 == DoIoctl(VIDIOC_QUERYMENU, &query_menu)) {
598      printf("\t\t\t%s\n", query_menu.name);
599    } else {
600      printf("<<< Info: VIDIOC_QUERYMENU not supported.>>>\n");
601      return false;
602    }
603  }
604  return true;
605}
606
607bool V4L2Device::EnumFormat(uint32_t* num_formats, bool show_fmt) {
608  uint32_t i;
609  for (i = 0; ; ++i) {
610    v4l2_fmtdesc format_desc;
611    memset(&format_desc, 0, sizeof(format_desc));
612    format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
613    format_desc.index = i;
614    if (-1 == DoIoctl(VIDIOC_ENUM_FMT, &format_desc)) {
615      if (i == 0) {
616          printf("<<< Info: VIDIOC_ENUM_FMT not supported.>>>\n");
617          return false;
618      } else {
619          break;
620      }
621    }
622    if (show_fmt)
623      printf("<<< Info supported format #%d: %s (%c%c%c%c) >>>\n",
624             i+1, format_desc.description,
625             (format_desc.pixelformat >> 0) & 0xff,
626             (format_desc.pixelformat >> 8) & 0xff,
627             (format_desc.pixelformat >> 16) & 0xff,
628             (format_desc.pixelformat >> 24) & 0xff);
629  }
630
631  if (num_formats)
632    *num_formats = i;
633  return true;
634}
635
636uint32_t V4L2Device::GetPixelFormat(uint32_t index) {
637  v4l2_fmtdesc format_desc;
638  memset(&format_desc, 0, sizeof(format_desc));
639  format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
640  format_desc.index = index;
641  if (-1 == DoIoctl(VIDIOC_ENUM_FMT, &format_desc))
642    return 0xFFFFFFFF;
643  return format_desc.pixelformat;
644}
645
646bool V4L2Device::EnumFrameSize(uint32_t pixfmt, bool show_frmsize) {
647  for (uint32_t i = 0; ; ++i) {
648    v4l2_frmsizeenum frmsize_desc;
649    memset(&frmsize_desc, 0, sizeof(frmsize_desc));
650    frmsize_desc.pixel_format = pixfmt;
651    frmsize_desc.index = i;
652    if (-1 == DoIoctl(VIDIOC_ENUM_FRAMESIZES, &frmsize_desc)) {
653      if (i == 0) {
654        printf("<<< Info: VIDIOC_ENUM_FRAMESIZES not supported.>>>\n");
655        return false;
656      } else {
657        break;
658      }
659    }
660    if (show_frmsize) {
661      switch (frmsize_desc.type) {
662        case V4L2_FRMSIZE_TYPE_DISCRETE:
663          printf("<<< Info supported discrete frame size #%d:"
664                 " for pixel foramt(%c%c%c%c): %dx%d >>>\n", i+1,
665                 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
666                 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
667                 frmsize_desc.discrete.width,
668                 frmsize_desc.discrete.height);
669          break;
670        case V4L2_FRMSIZE_TYPE_CONTINUOUS:
671          printf("<<< Info supported discrete frame size #%d:"
672                 " for pixel foramt(%c%c%c%c): "
673                 " from %dx%d to %dx%d >>>\n", i+1,
674                 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
675                 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
676                 frmsize_desc.stepwise.min_width,
677                 frmsize_desc.stepwise.min_height,
678                 frmsize_desc.stepwise.max_width,
679                 frmsize_desc.stepwise.max_height);
680          break;
681        case V4L2_FRMSIZE_TYPE_STEPWISE:
682          printf("<<< Info supported discrete frame size #%d:"
683                 " for pixel foramt(%c%c%c%c): "
684                 " from %dx%d to %dx%d step(%d,%d) >>>\n", i+1,
685                 (pixfmt >> 0) & 0xff, (pixfmt >> 8) & 0xff,
686                 (pixfmt >> 16) & 0xff, (pixfmt >> 24) & 0xff,
687                 frmsize_desc.stepwise.min_width,
688                 frmsize_desc.stepwise.min_height,
689                 frmsize_desc.stepwise.max_width,
690                 frmsize_desc.stepwise.max_height,
691                 frmsize_desc.stepwise.step_width,
692                 frmsize_desc.stepwise.step_height);
693          break;
694      }
695    }
696  }
697  return true;
698}
699
700bool V4L2Device::QueryControl(uint32_t id, v4l2_queryctrl* ctrl) {
701  memset(ctrl, 0, sizeof(*ctrl));
702  ctrl->id = id;
703  if (-1 == DoIoctl(VIDIOC_QUERYCTRL, ctrl)) {
704    if (errno != EINVAL) return false;
705    printf("%d is not supported\n", id);
706    return false;
707  }
708  if (ctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
709    printf("%d is not supported\n", id);
710    return false;
711  }
712  return true;
713}
714
715bool V4L2Device::SetControl(uint32_t id, int32_t value) {
716  v4l2_control control;
717  control.id = id;
718  control.value = value;
719  if (-1 == DoIoctl(VIDIOC_S_CTRL, &control)) {
720    printf("<<< Info: VIDIOC_S_CTRL failed. %d>>>\n", errno);
721    return false;
722  }
723  return true;
724}
725
726bool V4L2Device::GetCropCap(v4l2_cropcap* cropcap) {
727  if (-1 == DoIoctl(VIDIOC_CROPCAP, cropcap)) {
728    printf("<<< Warning: VIDIOC_CROPCAP not supported.>>>\n");
729    return false;
730  }
731  return true;
732}
733
734bool V4L2Device::GetCrop(v4l2_crop* crop) {
735  if (-1 == DoIoctl(VIDIOC_G_CROP, crop)) {
736    printf("<<< Warning: VIDIOC_G_CROP not supported.>>>\n");
737    return false;
738  }
739  printf("crop: %d, %d, %d, %d\n",
740         crop->c.left, crop->c.top,
741         crop->c.width, crop->c.height);
742  return true;
743}
744
745bool V4L2Device::SetCrop(v4l2_crop* crop) {
746  if (-1 == DoIoctl(VIDIOC_S_CROP, crop)) {
747    printf("<<< Warning: VIDIOC_S_CROP not supported.>>>\n");
748    return false;
749  }
750  return true;
751}
752
753bool V4L2Device::ProbeCaps(v4l2_capability* cap, bool show_caps) {
754  if (-1 == DoIoctl(VIDIOC_QUERYCAP, cap)) {
755    printf("<<< Error: VIDIOC_QUERYCAP on %s.>>>\n", dev_name_);
756    return false;
757  }
758
759  if (show_caps) {
760    if (cap->capabilities & V4L2_CAP_VIDEO_CAPTURE)
761      printf("<<< Info: %s support video capture interface.>>>\n", dev_name_);
762    if (cap->capabilities & V4L2_CAP_VIDEO_OUTPUT)
763      printf("<<< Info: %s support video output interface.>>>\n", dev_name_);
764    if (cap->capabilities & V4L2_CAP_VIDEO_OVERLAY)
765      printf("<<< Info: %s support video overlay interface.>>>\n", dev_name_);
766    if (cap->capabilities & V4L2_CAP_AUDIO)
767      printf("<<< Info: %s support audio i/o interface.>>>\n", dev_name_);
768
769    if (cap->capabilities & V4L2_CAP_READWRITE)
770      printf("<<< Info: %s support read/write interface.>>>\n", dev_name_);
771    if (cap->capabilities & V4L2_CAP_STREAMING)
772      printf("<<< Info: %s support streaming i/o interface.>>>\n", dev_name_);
773    if (cap->capabilities & V4L2_CAP_TIMEPERFRAME)
774      printf("<<< Info: %s support flexible frame period.>>>\n", dev_name_);
775  }
776
777  return true;
778}
779
780uint32_t V4L2Device::MapFourCC(const char* fourcc) {
781  return v4l2_fourcc(fourcc[0], fourcc[1], fourcc[2], fourcc[3]);
782}
783
784bool V4L2Device::GetParam(v4l2_streamparm* param) {
785  param->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
786  if (-1 == DoIoctl(VIDIOC_G_PARM, param)) {
787    printf("<<< Warning: VIDIOC_G_PARM not supported.>>>\n");
788    return false;
789  }
790
791  return true;
792}
793
794bool V4L2Device::SetParam(v4l2_streamparm* param) {
795  if (-1 == DoIoctl(VIDIOC_S_PARM, param)) {
796    printf("<<< Warning: VIDIOC_S_PARM not supported.>>>\n");
797    return false;
798  }
799  return true;
800}
801
802bool V4L2Device::SetFrameRate(uint32_t fps) {
803  v4l2_streamparm param;
804  if (!GetParam(&param))
805    return false;
806  param.parm.capture.timeperframe.numerator = 1;
807  param.parm.capture.timeperframe.denominator = fps;
808  return SetParam(&param);
809}
810
811uint32_t V4L2Device::GetFrameRate() {
812  v4l2_streamparm param;
813  if (!GetParam(&param))
814    return -1;
815  return (param.parm.capture.timeperframe.denominator /
816          param.parm.capture.timeperframe.numerator);
817}
818
819uint64_t V4L2Device::Now() {
820  struct timespec ts;
821  int res = clock_gettime(CLOCK_MONOTONIC, &ts);
822  CHECK(res == 0);
823  return static_cast<uint64_t>(ts.tv_sec);
824}
825