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 
V4L2Device(const char * dev_name,IOMethod io,uint32_t buffers)18 V4L2Device::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 
OpenDevice()30 bool 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 
CloseDevice()86 void V4L2Device::CloseDevice() {
87   if (fd_ != -1)
88     close(fd_);
89   fd_ = -1;
90 }
91 
InitDevice(uint32_t width,uint32_t height,uint32_t pixfmt,uint32_t fps)92 bool 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 
UninitDevice()160 bool 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 
StartCapture()182 bool 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 
StopCapture()230 bool 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 
ProcessImage(const void * p)248 void 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 ).
Run(uint32_t frames,uint32_t time_in_sec)255 bool 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 
Stop()295 bool V4L2Device::Stop() {
296   stopped_ = true;
297 }
298 
DoIoctl(int32_t request,void * arg)299 int32_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
ReadOneFrame()310 int32_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 
AllocateBuffer(uint32_t buffer_count)386 bool 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 
FreeBuffer()395 bool V4L2Device::FreeBuffer() {
396   free(v4l2_buffers_);
397   v4l2_buffers_ = NULL;
398   return true;
399 }
400 
InitReadIO(uint32_t buffer_size)401 bool 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 
InitMmapIO()413 bool 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 
InitUserPtrIO(uint32_t buffer_size)461 bool 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 
EnumInput()493 bool 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 
EnumStandard()517 bool 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 
EnumControl(bool show_menu)548 bool 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 
EnumControlMenu(const v4l2_queryctrl & query_ctrl)589 bool 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 
EnumFormat(uint32_t * num_formats,bool show_fmt)607 bool 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 
GetPixelFormat(uint32_t index)636 uint32_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 
EnumFrameSize(uint32_t pixfmt,bool show_frmsize)646 bool 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 
QueryControl(uint32_t id,v4l2_queryctrl * ctrl)700 bool 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 
SetControl(uint32_t id,int32_t value)715 bool 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 
GetCropCap(v4l2_cropcap * cropcap)726 bool 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 
GetCrop(v4l2_crop * crop)734 bool 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 
SetCrop(v4l2_crop * crop)745 bool 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 
ProbeCaps(v4l2_capability * cap,bool show_caps)753 bool 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 
MapFourCC(const char * fourcc)780 uint32_t V4L2Device::MapFourCC(const char* fourcc) {
781   return v4l2_fourcc(fourcc[0], fourcc[1], fourcc[2], fourcc[3]);
782 }
783 
GetParam(v4l2_streamparm * param)784 bool 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 
SetParam(v4l2_streamparm * param)794 bool 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 
SetFrameRate(uint32_t fps)802 bool 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 
GetFrameRate()811 uint32_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 
Now()819 uint64_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