backend-v4l2.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
4 #include "metadata.h"
5 #include "backend-v4l2.h"
6 #include "backend-hid.h"
7 #include "backend.h"
8 #include "types.h"
9 #if defined(USING_UDEV)
10 #include "udev-device-watcher.h"
11 #else
12 #include "../polling-device-watcher.h"
13 #endif
14 #include "usb/usb-enumerator.h"
15 #include "usb/usb-device.h"
16 
17 #include <cassert>
18 #include <cstdlib>
19 #include <cstdio>
20 #include <cstring>
21 
22 #include <algorithm>
23 #include <functional>
24 #include <string>
25 #include <sstream>
26 #include <fstream>
27 #include <regex>
28 #include <thread>
29 #include <utility> // for pair
30 #include <chrono>
31 #include <thread>
32 #include <atomic>
33 #include <iomanip> // std::put_time
34 
35 #include <dirent.h>
36 #include <fcntl.h>
37 #include <unistd.h>
38 #include <limits.h>
39 #include <cmath>
40 #include <errno.h>
41 #include <sys/stat.h>
42 #include <sys/mman.h>
43 #include <sys/ioctl.h>
44 #include <sys/sysmacros.h> // minor(...), major(...)
45 #include <linux/usb/video.h>
46 #include <linux/media.h>
47 #include <linux/uvcvideo.h>
48 #include <linux/videodev2.h>
49 #include <regex>
50 #include <list>
51 
52 #include <cstddef> // offsetof
53 
54 #include <sys/signalfd.h>
55 #include <signal.h>
56 #pragma GCC diagnostic ignored "-Woverflow"
57 
58 const size_t MAX_DEV_PARENT_DIR = 10;
60 
61 #include "../tm2/tm-boot.h"
62 //D457 Dev. TODO -shall be refactored into the kernel headers.
63 constexpr uint32_t RS_STREAM_CONFIG_0 = 0x4000;
64 constexpr uint32_t RS_CAMERA_CID_BASE = (V4L2_CTRL_CLASS_CAMERA | RS_STREAM_CONFIG_0);
80 
81 //const uint32_t RS_CAMERA_GENERIC_XU = (RS_CAMERA_CID_BASE+15); // RS_CAMERA_CID_HWMC duplicate??
82 constexpr uint32_t RS_CAMERA_CID_LASER_POWER_MODE = (RS_CAMERA_CID_BASE+16); // RS_CAMERA_CID_LASER_POWER duplicate ???
84 constexpr uint32_t RS_CAMERA_CID_LASER_POWER_LEVEL = (RS_CAMERA_CID_BASE+18); // RS_CAMERA_CID_MANUAL_LASER_POWER ??
86 constexpr uint32_t RS_CAMERA_CID_WHITE_BALANCE_MODE = (RS_CAMERA_CID_BASE+20); // Similar to RS_CAMERA_CID_EWB ??
88 constexpr uint32_t RS_CAMERA_CID_EMITTER_FREQUENCY = (RS_CAMERA_CID_BASE+22); // [MIPI - Select projector frequency values: 0->57[KHZ], 1->91[KHZ]
90 
91 /* refe4rence for kernel 4.9 to be removed
92 #define UVC_CID_GENERIC_XU (V4L2_CID_PRIVATE_BASE+15)
93 #define UVC_CID_LASER_POWER_MODE (V4L2_CID_PRIVATE_BASE+16)
94 #define UVC_CID_MANUAL_EXPOSURE (V4L2_CID_PRIVATE_BASE+17)
95 #define UVC_CID_LASER_POWER_LEVEL (V4L2_CID_PRIVATE_BASE+18)
96 #define UVC_CID_EXPOSURE_MODE (V4L2_CID_PRIVATE_BASE+19)
97 #define UVC_CID_WHITE_BALANCE_MODE (V4L2_CID_PRIVATE_BASE+20)
98 #define UVC_CID_PRESET (V4L2_CID_PRIVATE_BASE+21)
99 UVC_CID_MANUAL_EXPOSURE
100 */
101 #ifdef ANDROID
102 
103 // https://android.googlesource.com/platform/bionic/+/master/libc/include/bits/lockf.h
104 #define F_ULOCK 0
105 #define F_LOCK 1
106 #define F_TLOCK 2
107 #define F_TEST 3
108 
109 // https://android.googlesource.com/platform/bionic/+/master/libc/bionic/lockf.cpp
110 int lockf64(int fd, int cmd, off64_t length)
111 {
112  // Translate POSIX lockf into fcntl.
113  struct flock64 fl;
114  memset(&fl, 0, sizeof(fl));
115  fl.l_whence = SEEK_CUR;
116  fl.l_start = 0;
117  fl.l_len = length;
118 
119  if (cmd == F_ULOCK) {
120  fl.l_type = F_UNLCK;
121  cmd = F_SETLK64;
122  return fcntl(fd, F_SETLK64, &fl);
123  }
124 
125  if (cmd == F_LOCK) {
126  fl.l_type = F_WRLCK;
127  return fcntl(fd, F_SETLKW64, &fl);
128  }
129 
130  if (cmd == F_TLOCK) {
131  fl.l_type = F_WRLCK;
132  return fcntl(fd, F_SETLK64, &fl);
133  }
134  if (cmd == F_TEST) {
135  fl.l_type = F_RDLCK;
136  if (fcntl(fd, F_GETLK64, &fl) == -1) return -1;
137  if (fl.l_type == F_UNLCK || fl.l_pid == getpid()) return 0;
138  errno = EACCES;
139  return -1;
140  }
141 
142  errno = EINVAL;
143  return -1;
144 }
145 
146 int lockf(int fd, int cmd, off_t length)
147 {
148  return lockf64(fd, cmd, length);
149 }
150 #endif
151 
152 namespace librealsense
153 {
154  namespace platform
155  {
156  std::recursive_mutex named_mutex::_init_mutex;
157  std::map<std::string, std::recursive_mutex> named_mutex::_dev_mutex;
158  std::map<std::string, int> named_mutex::_dev_mutex_cnt;
159 
160  named_mutex::named_mutex(const std::string& device_path, unsigned timeout)
161  : _device_path(device_path),
162  _timeout(timeout), // TODO: try to lock with timeout
163  _fildes(-1),
164  _object_lock_counter(0)
165  {
166  _init_mutex.lock();
167  _dev_mutex[_device_path]; // insert a mutex for _device_path
168  if (_dev_mutex_cnt.find(_device_path) == _dev_mutex_cnt.end())
169  {
171  }
172  _init_mutex.unlock();
173  }
174 
176  {
177  unlock();
178  }
179 
181  {
182  std::lock_guard<std::mutex> lock(_mutex);
183  acquire();
184  }
185 
187  {
188  std::lock_guard<std::mutex> lock(_mutex);
189  release();
190  }
191 
193  {
194  std::lock_guard<std::mutex> lock(_mutex);
195  if (-1 == _fildes)
196  {
197  _fildes = open(_device_path.c_str(), O_RDWR, 0); //TODO: check
198  if(_fildes < 0)
199  return false;
200  }
201 
202  auto ret = lockf(_fildes, F_TLOCK, 0);
203  if (ret != 0)
204  return false;
205 
206  return true;
207  }
208 
210  {
211  _dev_mutex[_device_path].lock();
212  _dev_mutex_cnt[_device_path] += 1; //Advance counters even if throws because catch calls release()
214  if (_dev_mutex_cnt[_device_path] == 1)
215  {
216  if (-1 == _fildes)
217  {
218  _fildes = open(_device_path.c_str(), O_RDWR, 0); //TODO: check
219  if(0 > _fildes)
220  {
221  release();
222  throw linux_backend_exception(to_string() << __FUNCTION__ << ": Cannot open '" << _device_path);
223  }
224  }
225 
226  auto ret = lockf(_fildes, F_LOCK, 0);
227  if (0 != ret)
228  {
229  release();
230  throw linux_backend_exception(to_string() << __FUNCTION__ << ": Acquire failed");
231  }
232  }
233  }
234 
236  {
238  if (_object_lock_counter < 0)
239  {
241  return;
242  }
244  std::string err_msg;
245  if (_dev_mutex_cnt[_device_path] < 0)
246  {
248  throw linux_backend_exception(to_string() << "Error: _dev_mutex_cnt[" << _device_path << "] < 0");
249  }
250 
251  if ((_dev_mutex_cnt[_device_path] == 0) && (-1 != _fildes))
252  {
253  auto ret = lockf(_fildes, F_ULOCK, 0);
254  if (0 != ret)
255  err_msg = to_string() << "lockf(...) failed";
256  else
257  {
258  ret = close(_fildes);
259  if (0 != ret)
260  err_msg = to_string() << "close(...) failed";
261  else
262  _fildes = -1;
263  }
264  }
265  _dev_mutex[_device_path].unlock();
266 
267  if (!err_msg.empty())
268  throw linux_backend_exception(err_msg);
269 
270  }
271 
272  static int xioctl(int fh, unsigned long request, void *arg)
273  {
274  int r=0;
275  do {
276  r = ioctl(fh, request, arg);
277  } while (r < 0 && errno == EINTR);
278  return r;
279  }
280 
281  buffer::buffer(int fd, v4l2_buf_type type, bool use_memory_map, uint32_t index)
282  : _type(type), _use_memory_map(use_memory_map), _index(index)
283  {
284  v4l2_buffer buf = {};
285  buf.type = _type;
286  buf.memory = use_memory_map ? V4L2_MEMORY_MMAP : V4L2_MEMORY_USERPTR;
287  buf.index = index;
288  if(xioctl(fd, VIDIOC_QUERYBUF, &buf) < 0)
289  throw linux_backend_exception("xioctl(VIDIOC_QUERYBUF) failed");
290 
291  // Prior to kernel 4.16 metadata payload was attached to the end of the video payload
292  uint8_t md_extra = (V4L2_BUF_TYPE_VIDEO_CAPTURE==type) ? MAX_META_DATA_SIZE : 0;
293  _original_length = buf.length;
294  _length = _original_length + md_extra;
295 
296  if (use_memory_map)
297  {
298  _start = static_cast<uint8_t*>(mmap(nullptr, _original_length,
299  PROT_READ | PROT_WRITE, MAP_SHARED,
300  fd, buf.m.offset));
301  if(_start == MAP_FAILED)
302  throw linux_backend_exception("mmap failed");
303  }
304  else
305  {
306  //_length += (V4L2_BUF_TYPE_VIDEO_CAPTURE==type) ? MAX_META_DATA_SIZE : 0;
307  _start = static_cast<uint8_t*>(malloc( _length));
308  if (!_start) throw linux_backend_exception("User_p allocation failed!");
309  memset(_start, 0, _length);
310  }
311  }
312 
314  {
315  v4l2_buffer buf = {};
316  buf.type = _type;
317  buf.memory = _use_memory_map ? V4L2_MEMORY_MMAP : V4L2_MEMORY_USERPTR;
318  buf.index = _index;
319  buf.length = _length;
320 
321  if ( !_use_memory_map )
322  {
323  buf.m.userptr = reinterpret_cast<unsigned long>(_start);
324  }
325  if(xioctl(fd, VIDIOC_QBUF, &buf) < 0)
326  throw linux_backend_exception("xioctl(VIDIOC_QBUF) failed");
327  else
328  LOG_DEBUG_V4L("prepare_for_streaming fd " << std::dec << fd);
329  }
330 
332  {
333  if (_use_memory_map)
334  {
335  if(munmap(_start, _original_length) < 0)
336  linux_backend_exception("munmap");
337  }
338  else
339  {
340  free(_start);
341  }
342  }
343 
344  void buffer::attach_buffer(const v4l2_buffer& buf)
345  {
346  std::lock_guard<std::mutex> lock(_mutex);
347  _buf = buf;
348  _must_enqueue = true;
349  }
350 
352  {
353  std::lock_guard<std::mutex> lock(_mutex);
354  _must_enqueue = false;
355  }
356 
357  void buffer::request_next_frame(int fd, bool force)
358  {
359  std::lock_guard<std::mutex> lock(_mutex);
360 
361  if (_must_enqueue || force)
362  {
363  if (!_use_memory_map)
364  {
365  auto metadata_offset = get_full_length() - MAX_META_DATA_SIZE;
366  memset((byte*)(get_frame_start()) + metadata_offset, 0, MAX_META_DATA_SIZE);
367  }
368 
369  LOG_DEBUG_V4L("Enqueue buf " << std::dec << _buf.index << " for fd " << fd);
370  if (xioctl(fd, VIDIOC_QBUF, &_buf) < 0)
371  {
372  LOG_ERROR("xioctl(VIDIOC_QBUF) failed when requesting new frame! fd: " << fd << " error: " << strerror(errno));
373  }
374 
375  _must_enqueue = false;
376  }
377  }
378 
380  int file_desc,
381  v4l2_buffer v4l_buf,
382  std::shared_ptr<platform::buffer> data_buf
383  )
384  {
385  if (e_max_kernel_buf_type <=buf_type)
386  throw linux_backend_exception("invalid kernel buffer type request");
387 
388  // D457 development - changed from 1 to 0
389  if (file_desc < 0)
390  {
391  // QBUF to be performed by a 3rd party
392  this->buffers.at(buf_type)._managed = true;
393  }
394  else
395  {
396  buffers.at(buf_type)._file_desc = file_desc;
397  buffers.at(buf_type)._managed = false;
398  buffers.at(buf_type)._data_buf = data_buf;
399  buffers.at(buf_type)._dq_buf = v4l_buf;
400  }
401  }
402 
404  {
405  for (auto& buf : buffers)
406  {
407  LOG_DEBUG_V4L("request_next_frame with fd = " << buf._file_desc);
408  if (buf._data_buf && (buf._file_desc >= 0))
409  buf._data_buf->request_next_frame(buf._file_desc);
410  };
411  _md_start = nullptr;
412  _md_size = 0;
413  }
414 
416  {
417  void* md_start = nullptr;
418  auto md_size = 0;
419 
420  if (buffers.at(e_video_buf)._file_desc >=0)
421  {
422  static const int d4xx_md_size = 248;
423  auto buffer = buffers.at(e_video_buf)._data_buf;
424  auto dq = buffers.at(e_video_buf)._dq_buf;
425  auto fr_payload_size = buffer->get_length_frame_only();
426 
427  // For compressed data assume D4XX metadata struct
428  // TODO - devise SKU-agnostic heuristics
429  auto md_appendix_sz = 0L;
430  if (compressed && (dq.bytesused < fr_payload_size))
431  md_appendix_sz = d4xx_md_size;
432  else
433  md_appendix_sz = long(dq.bytesused) - fr_payload_size;
434 
435  if (md_appendix_sz >0 )
436  {
437  md_start = buffer->get_frame_start() + dq.bytesused - md_appendix_sz;
438  md_size = (*(static_cast<uint8_t*>(md_start)));
439  int md_flags = (*(static_cast<uint8_t*>(md_start)+1));
440  // Use heuristics for metadata validation
441  if ((md_appendix_sz != md_size) || (!val_in_range(md_flags, {0x8e, 0x8f})))
442  {
443  md_size = 0;
444  md_start=nullptr;
445  }
446  }
447  }
448 
449  if (nullptr == md_start)
450  {
451  LOG_DEBUG("Could not parse metadata");
452  }
453  set_md_attributes(static_cast<uint8_t>(md_size),md_start);
454  }
455 
457  {
458  if ((buffers[e_video_buf]._file_desc > 0) && (buffers[e_metadata_buf]._file_desc > 0))
459  {
460  if (buffers[e_video_buf]._dq_buf.sequence != buffers[e_metadata_buf]._dq_buf.sequence)
461  {
462  LOG_ERROR("Non-sequential Video and Metadata v4l buffers - video seq = " << buffers[e_video_buf]._dq_buf.sequence << ", md seq = " << buffers[e_metadata_buf]._dq_buf.sequence);
463  return false;
464  }
465  }
466  return true;
467  }
468 
470  {
471  return (buffers[e_metadata_buf]._file_desc > 0);
472  }
473 
474  // retrieve the USB specification attributed to a specific USB device.
475  // This functionality is required to find the USB connection type for UVC device
476  // Note that the input parameter is passed by value
478  {
479  usb_spec res{usb_undefined};
480 
481  char usb_actual_path[PATH_MAX] = {0};
482  if (realpath(path.c_str(), usb_actual_path) != nullptr)
483  {
484  path = std::string(usb_actual_path);
485  std::string val;
486  if(!(std::ifstream(path + "/version") >> val))
487  throw linux_backend_exception("Failed to read usb version specification");
488 
489  auto kvp = std::find_if(usb_spec_names.begin(),usb_spec_names.end(),
490  [&val](const std::pair<usb_spec, std::string>& kpv){ return (std::string::npos !=val.find(kpv.second));});
491  if (kvp != std::end(usb_spec_names))
492  res = kvp->first;
493  }
494  return res;
495  }
496 
497  // Retrieve device video capabilities to discriminate video capturing and metadata nodes
498  static uint32_t get_dev_capabilities(const std::string dev_name)
499  {
500  // RAII to handle exceptions
501  std::unique_ptr<int, std::function<void(int*)> > fd(
502  new int (open(dev_name.c_str(), O_RDWR | O_NONBLOCK, 0)),
503  [](int* d){ if (d && (*d)) {::close(*d); } delete d; });
504 
505  if(*fd < 0)
506  throw linux_backend_exception(to_string() << __FUNCTION__ << ": Cannot open '" << dev_name);
507 
508  v4l2_capability cap = {};
509  if(xioctl(*fd, VIDIOC_QUERYCAP, &cap) < 0)
510  {
511  if(errno == EINVAL)
512  throw linux_backend_exception(to_string() << __FUNCTION__ << " " << dev_name << " is no V4L2 device");
513  else
514  throw linux_backend_exception(to_string() <<__FUNCTION__ << " xioctl(VIDIOC_QUERYCAP) failed");
515  }
516 
517  return cap.device_caps;
518  }
519 
520  void stream_ctl_on(int fd, v4l2_buf_type type=V4L2_BUF_TYPE_VIDEO_CAPTURE)
521  {
522  if(xioctl(fd, VIDIOC_STREAMON, &type) < 0)
523  throw linux_backend_exception(to_string() << "xioctl(VIDIOC_STREAMON) failed for buf_type=" << type);
524  }
525 
526  void stream_off(int fd, v4l2_buf_type type=V4L2_BUF_TYPE_VIDEO_CAPTURE)
527  {
528  if(xioctl(fd, VIDIOC_STREAMOFF, &type) < 0)
529  throw linux_backend_exception(to_string() << "xioctl(VIDIOC_STREAMOFF) failed for buf_type=" << type);
530  }
531 
532  void req_io_buff(int fd, uint32_t count, std::string dev_name,
533  v4l2_memory mem_type, v4l2_buf_type type)
534  {
535  struct v4l2_requestbuffers req = { count, type, mem_type, {}};
536 
537  if(xioctl(fd, VIDIOC_REQBUFS, &req) < 0)
538  {
539  if(errno == EINVAL)
540  LOG_ERROR(dev_name + " does not support memory mapping");
541  else
542  return;
543  //D457 - fails on close (when num = 0)
544  //throw linux_backend_exception("xioctl(VIDIOC_REQBUFS) failed");
545  }
546  }
547 
548  std::vector<std::string> v4l_uvc_device::get_video_paths()
549  {
550  std::vector<std::string> video_paths;
551  // Enumerate all subdevices present on the system
552  DIR * dir = opendir("/sys/class/video4linux");
553  if(!dir)
554  {
555  LOG_INFO("Cannot access /sys/class/video4linux");
556  return video_paths;
557  }
558  while (dirent * entry = readdir(dir))
559  {
560  std::string name = entry->d_name;
561  if(name == "." || name == "..") continue;
562 
563  // Resolve a pathname to ignore virtual video devices and sub-devices
564  static const std::regex video_dev_pattern("(\\/video\\d+)$");
565 
566  std::string path = "/sys/class/video4linux/" + name;
567  std::string real_path{};
568  char buff[PATH_MAX] = {0};
569  if (realpath(path.c_str(), buff) != nullptr)
570  {
571  real_path = std::string(buff);
572  if (real_path.find("virtual") != std::string::npos)
573  continue;
574  if (!std::regex_search(real_path, video_dev_pattern))
575  {
576  //LOG_INFO("Skipping Video4Linux entry " << real_path << " - not a device");
577  continue;
578  }
579  }
580  video_paths.push_back(real_path);
581  }
582  closedir(dir);
583 
584 
585  // UVC nodes shall be traversed in ascending order for metadata nodes assignment ("dev/video1, Video2..
586  // Replace lexicographic with numeric sort to ensure "video2" is listed before "video11"
587  std::sort(video_paths.begin(), video_paths.end(),
588  [](const std::string& first, const std::string& second)
589  {
590  // getting videoXX
591  std::string first_video = first.substr(first.find_last_of('/') + 1);
592  std::string second_video = second.substr(second.find_last_of('/') + 1);
593 
594  // getting the index XX from videoXX
595  std::stringstream first_index(first_video.substr(first_video.find_first_of("0123456789")));
596  std::stringstream second_index(second_video.substr(second_video.find_first_of("0123456789")));
597  int left_id = 0, right_id = 0;
598  first_index >> left_id;
599  second_index >> right_id;
600  return left_id < right_id;
601  });
602  return video_paths;
603  }
604 
605  bool v4l_uvc_device::is_usb_path_valid(const std::string& usb_video_path, const std::string& dev_name,
606  std::string& busnum, std::string& devnum, std::string& devpath)
607  {
608  struct stat st = {};
609  if(stat(dev_name.c_str(), &st) < 0)
610  {
611  throw linux_backend_exception(to_string() << "Cannot identify '" << dev_name);
612  }
613  if(!S_ISCHR(st.st_mode))
614  throw linux_backend_exception(dev_name + " is no device");
615 
616  // Search directory and up to three parent directories to find busnum/devnum
617  auto valid_path = false;
618  std::ostringstream ss;
619  ss << "/sys/dev/char/" << major(st.st_rdev) << ":" << minor(st.st_rdev) << "/device/";
620 
621  auto char_dev_path = ss.str();
622 
623  for(auto i=0U; i < MAX_DEV_PARENT_DIR; ++i)
624  {
625  if(std::ifstream(char_dev_path + "busnum") >> busnum)
626  {
627  if(std::ifstream(char_dev_path + "devnum") >> devnum)
628  {
629  if(std::ifstream(char_dev_path + "devpath") >> devpath)
630  {
631  valid_path = true;
632  break;
633  }
634  }
635  }
636  char_dev_path += "../";
637  }
638  return valid_path;
639  }
640 
641  uvc_device_info v4l_uvc_device::get_info_from_usb_device_path(const std::string& video_path, const std::string& name)
642  {
643  std::string busnum, devnum, devpath;
644  auto dev_name = "/dev/" + name;
645 
646  if (!is_usb_path_valid(video_path, dev_name, busnum, devnum, devpath))
647  {
648 #ifndef RS2_USE_CUDA
649  /* On the Jetson TX, the camera module is CSI & I2C and does not report as this code expects
650  Patch suggested by JetsonHacks: https://github.com/jetsonhacks/buildLibrealsense2TX */
651  LOG_INFO("Failed to read busnum/devnum. Device Path: " << ("/sys/class/video4linux/" + name));
652 #endif
653  throw linux_backend_exception("Failed to read busnum/devnum of usb device");
654  }
655 
656  LOG_INFO("Enumerating UVC " << name << " realpath=" << video_path);
657  uint16_t vid{}, pid{}, mi{};
658  usb_spec usb_specification(usb_undefined);
659 
660  std::string modalias;
661  if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/modalias") >> modalias))
662  throw linux_backend_exception("Failed to read modalias");
663  if(modalias.size() < 14 || modalias.substr(0,5) != "usb:v" || modalias[9] != 'p')
664  throw linux_backend_exception("Not a usb format modalias");
665  if(!(std::istringstream(modalias.substr(5,4)) >> std::hex >> vid))
666  throw linux_backend_exception("Failed to read vendor ID");
667  if(!(std::istringstream(modalias.substr(10,4)) >> std::hex >> pid))
668  throw linux_backend_exception("Failed to read product ID");
669  if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/bInterfaceNumber") >> std::hex >> mi))
670  throw linux_backend_exception("Failed to read interface number");
671 
672  // Find the USB specification (USB2/3) type from the underlying device
673  // Use device mapping obtained in previous step to traverse node tree
674  // and extract the required descriptors
675  // Traverse from
676  // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/3-6:1.0/video4linux/video0
677  // to
678  // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/version
679  usb_specification = get_usb_connection_type(video_path + "/../../../");
680 
682  info.pid = pid;
683  info.vid = vid;
684  info.mi = mi;
685  info.id = dev_name;
686  info.device_path = video_path;
687  info.unique_id = busnum + "-" + devpath + "-" + devnum;
688  info.conn_spec = usb_specification;
689  info.uvc_capabilities = get_dev_capabilities(dev_name);
690 
691  return info;
692  }
693 
694  void v4l_uvc_device::get_mipi_device_info(const std::string& dev_name,
695  std::string& bus_info, std::string& card)
696  {
697  struct v4l2_capability vcap;
698  int fd = open(dev_name.c_str(), O_RDWR);
699  if (fd < 0)
700  throw linux_backend_exception("Mipi device capability could not be grabbed");
701  int err = ioctl(fd, VIDIOC_QUERYCAP, &vcap);
702  if (err)
703  {
704  struct media_device_info mdi;
705 
706  err = ioctl(fd, MEDIA_IOC_DEVICE_INFO, &mdi);
707  if (!err)
708  {
709  if (mdi.bus_info[0])
710  bus_info = mdi.bus_info;
711  else
712  bus_info = std::string("platform:") + mdi.driver;
713 
714  if (mdi.model[0])
715  card = mdi.model;
716  else
717  card = mdi.driver;
718  }
719  }
720  else
721  {
722  bus_info = reinterpret_cast<const char *>(vcap.bus_info);
723  card = reinterpret_cast<const char *>(vcap.card);
724  }
725  ::close(fd);
726  }
727 
728  uvc_device_info v4l_uvc_device::get_info_from_mipi_device_path(const std::string& video_path, const std::string& name)
729  {
730  uint16_t vid{}, pid{}, mi{};
731  usb_spec usb_specification(usb_undefined);
732  std::string bus_info, card;
733 
734  auto dev_name = "/dev/" + name;
735 
736  get_mipi_device_info(dev_name, bus_info, card);
737 
738  // the following 2 lines need to be changed in order to enable multiple mipi devices support
739  // or maybe another field in the info structure - TBD
740  vid = 0x8086;
741  pid = 0xABCD; // D457 dev
742 
743  static std::regex video_dev_index("\\d+$");
744  std::smatch match;
745  uint8_t ind{};
746  if (std::regex_search(name, match, video_dev_index))
747  {
748  ind = static_cast<uint8_t>(std::stoi(match[0]));
749  }
750  else
751  {
752  LOG_WARNING("Unresolved Video4Linux device pattern: " << name << ", device is skipped");
753  throw linux_backend_exception("Unresolved Video4Linux device, device is skipped");
754  }
755 
756  // D457 exposes (assuming first_video_index = 0):
757  // - video0 for Depth and video1 for Depth's md.
758  // - video2 for RGB and video3 for RGB's md.
759  // - video4 for IR stream. IR's md is currently not available.
760  // - video5 for IMU (accel or gyro TBD)
761  // next several lines permit to use D457 even if a usb device has already "taken" the video0,1,2 (for example)
762  // further development is needed to permit use of several mipi devices
763  static int first_video_index = ind;
764  ind = (ind - first_video_index) % 6; // offset from first mipi video node and assume 6 nodes per mipi camera
765  if (ind == 0 || ind == 2 || ind == 4)
766  mi = 0;
767  else if (ind == 1 | ind == 3)
768  mi = 3;
769  else if (ind == 5)
770  mi = 4;
771  else
772  {
773  LOG_WARNING("Unresolved Video4Linux device mi, device is skipped");
774  throw linux_backend_exception("Unresolved Video4Linux device, device is skipped");
775  }
776 
778  info.pid = pid;
779  info.vid = vid;
780  info.mi = mi;
781  info.id = dev_name;
782  info.device_path = video_path;
783  // unique id for MIPI:
784  // it cannot be generated as in usb, because the params busnum, devpath and devnum
785  // are not available via mipi
786  // TODO - find a way to assign unique id for mipi
787  // maybe using bus_info and card params (see above in this method)
788  info.unique_id = bus_info; // use bus_info as per camera unique id for mipi
789  info.conn_spec = usb_specification;
790  info.uvc_capabilities = get_dev_capabilities(dev_name);
791 
792  return info;
793  }
794 
795  bool v4l_uvc_device::is_usb_device_path(const std::string& video_path)
796  {
797  static const std::regex uvc_pattern("(\\/usb\\d+\\/)\\w+"); // Locate UVC device path pattern ../usbX/...
798  return std::regex_search(video_path, uvc_pattern);
799  }
800 
802  std::function<void(const uvc_device_info&,
803  const std::string&)> action)
804  {
805  std::vector<std::string> video_paths = get_video_paths();
806 
807  // Collect UVC nodes info to bundle metadata and video
808  typedef std::pair<uvc_device_info,std::string> node_info;
809  std::vector<node_info> uvc_nodes,uvc_devices;
810 
811  for(auto&& video_path : video_paths)
812  {
813  // following line grabs video0 from
814  auto name = video_path.substr(video_path.find_last_of('/') + 1);
815 
816  try
817  {
819  if (is_usb_device_path(video_path))
820  {
821  info = get_info_from_usb_device_path(video_path, name);
822  }
823  else //video4linux devices that are not USB devices
824  {
825  info = get_info_from_mipi_device_path(video_path, name);
826  }
827 
828  auto dev_name = "/dev/" + name;
829  uvc_nodes.emplace_back(info, dev_name);
830  }
831  catch(const std::exception & e)
832  {
833  LOG_INFO("Not a USB video device: " << e.what());
834  }
835  }
836 
837  // Matching video and metadata nodes
838  // Assume uvc_nodes is already sorted according to videoXX (video0, then video1...)
839  // Assume for each metadata node with index N there is a origin streaming node with index (N-1)
840  for (auto&& cur_node : uvc_nodes)
841  {
842  try
843  {
844  if (!(cur_node.first.uvc_capabilities & V4L2_CAP_META_CAPTURE))
845  uvc_devices.emplace_back(cur_node);
846  else
847  {
848  if (uvc_devices.empty())
849  {
850  LOG_ERROR("UVC meta-node with no video streaming node encountered: " << std::string(cur_node.first));
851  continue;
852  }
853 
854  // Update the preceding uvc item with metadata node info
855  auto uvc_node = uvc_devices.back();
856 
857  if (uvc_node.first.uvc_capabilities & V4L2_CAP_META_CAPTURE)
858  {
859  LOG_ERROR("Consequtive UVC meta-nodes encountered: " << std::string(uvc_node.first) << " and " << std::string(cur_node.first) );
860  continue;
861  }
862 
863  if (uvc_node.first.has_metadata_node)
864  {
865  LOG_ERROR( "Metadata node for uvc device: " << std::string(uvc_node.first) << " was previously assigned ");
866  continue;
867  }
868 
869  // modify the last element with metadata node info
870  uvc_node.first.has_metadata_node = true;
871  uvc_node.first.metadata_node_id = cur_node.first.id;
872  uvc_devices.back() = uvc_node;
873  }
874  }
875  catch(const std::exception & e)
876  {
877  LOG_ERROR("Failed to map meta-node " << std::string(cur_node.first) <<", error encountered: " << e.what());
878  }
879  }
880 
881  try
882  {
883  // Dispatch registration for enumerated uvc devices
884  for (auto&& dev : uvc_devices)
885  action(dev.first, dev.second);
886  }
887  catch(const std::exception & e)
888  {
889  LOG_ERROR("Registration of UVC device failed: " << e.what());
890  }
891  }
892 
894  {
895  bool is_kpi_violated = false;
896  long int timestamp_usec = static_cast<long int> (timestamp.tv_sec * 1000000 + timestamp.tv_usec);
897 
898  // checking if the current profile is already in the drops_per_stream container
899  auto it = std::find_if(drops_per_stream.begin(), drops_per_stream.end(),
900  [profile](std::pair<stream_profile, std::deque<long int>>& sp_deq)
901  {return profile == sp_deq.first; });
902 
903  // if the profile is already in the drops_per_stream container,
904  // checking kpi with the new partial frame caught
905  if (it != drops_per_stream.end())
906  {
907  // setting the kpi checking to be done on the last 30 seconds
908  int time_limit = 30;
909 
910  // max number of drops that can be received in the time_limit, without violation of the kpi
911  int max_num_of_drops = profile.fps * _kpi_frames_drops_pct * time_limit;
912 
913  auto& queue_for_profile = it->second;
914  // removing too old timestamps of partial frames
915  while (queue_for_profile.size() > 0)
916  {
917  auto delta_ts_usec = timestamp_usec - queue_for_profile.front();
918  if (delta_ts_usec > (time_limit * 1000000))
919  {
920  queue_for_profile.pop_front();
921  }
922  else
923  break; // correct because the frames are added chronologically
924  }
925  // checking kpi violation
926  if (queue_for_profile.size() >= max_num_of_drops)
927  {
928  is_kpi_violated = true;
929  queue_for_profile.clear();
930  }
931  else
932  queue_for_profile.push_back(timestamp_usec);
933  }
934  else
935  {
936  // adding the the current partial frame's profile and timestamp to the container
937  std::deque<long int> deque_to_add;
938  deque_to_add.push_back(timestamp_usec);
939  drops_per_stream.push_back(std::make_pair(profile, deque_to_add));
940  }
941  return is_kpi_violated;
942  }
943 
945  : _name(""), _info(),
946  _is_capturing(false),
947  _is_alive(true),
948  _is_started(false),
949  _thread(nullptr),
950  _named_mtx(nullptr),
951  _use_memory_map(use_memory_map),
952  _fd(-1),
953  _stop_pipe_fd{},
954  _buf_dispatch(use_memory_map),
956  {
957  foreach_uvc_device([&info, this](const uvc_device_info& i, const std::string& name)
958  {
959  if (i == info)
960  {
961  _name = name;
962  _info = i;
965  }
966  });
967  if (_name == "")
968  throw linux_backend_exception("device is no longer connected!");
969 
970  _named_mtx = std::unique_ptr<named_mutex>(new named_mutex(_name, 5000));
971  }
972 
974  {
975  _is_capturing = false;
976  if (_thread && _thread->joinable()) _thread->join();
977  for (auto&& fd : _fds)
978  {
979  try { if (fd) ::close(fd);} catch (...) {}
980  }
981  }
982 
984  {
985  if(!_is_capturing && !_callback)
986  {
987  v4l2_fmtdesc pixel_format = {};
988  pixel_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
989 
990  while (ioctl(_fd, VIDIOC_ENUM_FMT, &pixel_format) == 0)
991  {
992  v4l2_frmsizeenum frame_size = {};
993  frame_size.pixel_format = pixel_format.pixelformat;
994 
995  uint32_t fourcc = (const big_endian<int> &)pixel_format.pixelformat;
996 
997  if (pixel_format.pixelformat == 0)
998  {
999  // Microsoft Depth GUIDs for R400 series are not yet recognized
1000  // by the Linux kernel, but they do not require a patch, since there
1001  // are "backup" Z16 and Y8 formats in place
1002  static const std::set<std::string> pending_formats = {
1003  "00000050-0000-0010-8000-00aa003",
1004  "00000032-0000-0010-8000-00aa003",
1005  };
1006 
1007  if (std::find(pending_formats.begin(),
1008  pending_formats.end(),
1009  (const char*)pixel_format.description) ==
1010  pending_formats.end())
1011  {
1012  const std::string s(to_string() << "!" << pixel_format.description);
1013  std::regex rgx("!([0-9a-f]+)-.*");
1014  std::smatch match;
1015 
1016  if (std::regex_search(s.begin(), s.end(), match, rgx))
1017  {
1018  std::stringstream ss;
1019  ss << match[1];
1020  int id;
1021  ss >> std::hex >> id;
1022  fourcc = (const big_endian<int> &)id;
1023 
1024  if (fourcc == profile.format)
1025  {
1026  throw linux_backend_exception(to_string() << "The requested pixel format '" << fourcc_to_string(id)
1027  << "' is not natively supported by the running Linux kernel and likely requires a patch");
1028  }
1029  }
1030  }
1031  }
1032  ++pixel_format.index;
1033  }
1034 
1035  set_format(profile);
1036 
1037  v4l2_streamparm parm = {};
1038  parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1039  if(xioctl(_fd, VIDIOC_G_PARM, &parm) < 0)
1040  throw linux_backend_exception("xioctl(VIDIOC_G_PARM) failed");
1041 
1042  parm.parm.capture.timeperframe.numerator = 1;
1043  parm.parm.capture.timeperframe.denominator = profile.fps;
1044  if(xioctl(_fd, VIDIOC_S_PARM, &parm) < 0)
1045  throw linux_backend_exception("xioctl(VIDIOC_S_PARM) failed");
1046 
1047  // Init memory mapped IO
1048  negotiate_kernel_buffers(static_cast<size_t>(buffers));
1049  allocate_io_buffers(static_cast<size_t>(buffers));
1050 
1051  _profile = profile;
1052  _callback = callback;
1053  }
1054  else
1055  {
1056  throw wrong_api_call_sequence_exception("Device already streaming!");
1057  }
1058  }
1059 
1060  void v4l_uvc_device::stream_on(std::function<void(const notification& n)> error_handler)
1061  {
1062  if(!_is_capturing)
1063  {
1064  _error_handler = error_handler;
1065 
1066  // Start capturing
1068 
1069  // Synchronise stream requests for meta and video data.
1070  streamon();
1071 
1072  _is_capturing = true;
1073  _thread = std::unique_ptr<std::thread>(new std::thread([this](){ capture_loop(); }));
1074 
1075  // Starting the video/metadata syncer
1077  }
1078  }
1079 
1081  {
1082  for (auto&& buf : _buffers) buf->prepare_for_streaming(_fd);
1083  }
1084 
1086  {
1087  _is_capturing = false;
1088  _is_started = false;
1089 
1090  // Stop nn-demand frames polling
1091  signal_stop();
1092 
1093  _thread->join();
1094  _thread.reset();
1095 
1096  // Notify kernel
1097  streamoff();
1098  }
1099 
1101  {
1102  _is_started = true;
1103  }
1104 
1106  {
1107  _is_started = false;
1108  }
1109 
1111  {
1112  if(_is_capturing)
1113  {
1115  }
1116 
1117  if (_callback)
1118  {
1119  // Release allocated buffers
1121 
1122  // Release IO
1124 
1125  _callback = nullptr;
1126  }
1127  }
1128 
1130  {
1131  uint32_t device_fourcc = id;
1132  char fourcc_buff[sizeof(device_fourcc)+1];
1133  librealsense::copy(fourcc_buff, &device_fourcc, sizeof(device_fourcc));
1134  fourcc_buff[sizeof(device_fourcc)] = 0;
1135  return fourcc_buff;
1136  }
1137 
1139  {
1141  char buff[1]={};
1142  if (write(_stop_pipe_fd[1], buff, 1) < 0)
1143  {
1144  throw linux_backend_exception("Could not signal video capture thread to stop. Error write to pipe.");
1145  }
1146  }
1147 
1148  std::string time_in_HH_MM_SS_MMM()
1149  {
1150  using namespace std::chrono;
1151 
1152  // get current time
1153  auto now = system_clock::now();
1154 
1155  // get number of milliseconds for the current second
1156  // (remainder after division into seconds)
1157  auto ms = duration_cast<milliseconds>(now.time_since_epoch()) % 1000;
1158 
1159  // convert to std::time_t in order to convert to std::tm (broken time)
1160  auto timer = system_clock::to_time_t(now);
1161 
1162  // convert to broken time
1163  std::tm bt = *std::localtime(&timer);
1164 
1165  std::ostringstream oss;
1166 
1167  oss << std::put_time(&bt, "%H:%M:%S"); // HH:MM:SS
1168  oss << '.' << std::setfill('0') << std::setw(3) << ms.count();
1169  return oss.str();
1170  }
1171 
1173  {
1174  fd_set fds{};
1175  FD_ZERO(&fds);
1176  for (auto fd : _fds)
1177  {
1178  FD_SET(fd, &fds);
1179  }
1180 
1181  struct timespec mono_time;
1182  int ret = clock_gettime(CLOCK_MONOTONIC, &mono_time);
1183  if (ret) throw linux_backend_exception("could not query time!");
1184 
1185  struct timeval expiration_time = { mono_time.tv_sec + 5, mono_time.tv_nsec / 1000 };
1186  int val = 0;
1187 
1188  auto realtime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
1189  auto time_since_epoch = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
1190  LOG_DEBUG_V4L("Select initiated at " << time_in_HH_MM_SS_MMM() << ", mono time " << time_since_epoch << ", host time " << realtime );
1191  do {
1192  struct timeval remaining;
1193  ret = clock_gettime(CLOCK_MONOTONIC, &mono_time);
1194  if (ret) throw linux_backend_exception("could not query time!");
1195 
1196  struct timeval current_time = { mono_time.tv_sec, mono_time.tv_nsec / 1000 };
1197  timersub(&expiration_time, &current_time, &remaining);
1198  if (timercmp(&current_time, &expiration_time, <)) {
1199  val = select(_max_fd + 1, &fds, nullptr, nullptr, &remaining);
1200  }
1201  else {
1202  val = 0;
1203  LOG_DEBUG_V4L("Select timeouted");
1204  }
1205 
1206  if (val< 0)
1207  LOG_DEBUG_V4L("Select interrupted, val = " << val << ", error = " << errno);
1208  } while (val < 0 && errno == EINTR);
1209 
1210  LOG_DEBUG_V4L("Select done, val = " << val << " at " << time_in_HH_MM_SS_MMM());
1211  if(val < 0)
1212  {
1213  _is_capturing = false;
1214  _is_started = false;
1215 
1216  // Notify kernel
1217  streamoff();
1218  }
1219  else
1220  {
1221  if(val > 0)
1222  {
1223  if(FD_ISSET(_stop_pipe_fd[0], &fds) || FD_ISSET(_stop_pipe_fd[1], &fds))
1224  {
1225  if(!_is_capturing)
1226  {
1227  LOG_INFO("V4L stream is closed");
1228  return;
1229  }
1230  else
1231  {
1232  LOG_ERROR("Stop pipe was signalled during streaming");
1233  return;
1234  }
1235  }
1236  else // Check and acquire data buffers from kernel
1237  {
1238  bool md_extracted = false;
1239  bool keep_md = false;
1240  bool wa_applied = false;
1241  buffers_mgr buf_mgr(_use_memory_map);
1243  {
1244  buf_mgr = _buf_dispatch; // Handle over MD buffer from the previous cycle
1245  md_extracted = true;
1246  wa_applied = true;
1247  _buf_dispatch.set_md_attributes(0,nullptr);
1248  }
1249 
1250  // Relax the required frame size for compressed formats, i.e. MJPG, Z16H
1251  bool compressed_format = val_in_range(_profile.format, { 0x4d4a5047U , 0x5a313648U});
1252 
1253  // METADATA STREAM
1254  // Read metadata. Metadata node performs a blocking call to ensure video and metadata sync
1255  acquire_metadata(buf_mgr,fds,compressed_format);
1256  md_extracted = true;
1257 
1258  if (wa_applied)
1259  {
1260  auto fn = *(uint32_t*)((char*)(buf_mgr.metadata_start())+28);
1261  LOG_DEBUG_V4L("Extracting md buff, fn = " << fn);
1262  }
1263 
1264  // VIDEO STREAM
1265  if(FD_ISSET(_fd, &fds))
1266  {
1267  FD_CLR(_fd,&fds);
1268  v4l2_buffer buf = {};
1269  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1270  buf.memory = _use_memory_map ? V4L2_MEMORY_MMAP : V4L2_MEMORY_USERPTR;
1271  if(xioctl(_fd, VIDIOC_DQBUF, &buf) < 0)
1272  {
1273  LOG_DEBUG_V4L("Dequeued empty buf for fd " << std::dec << _fd);
1274  }
1275  LOG_DEBUG_V4L("Dequeued buf " << std::dec << buf.index << " for fd " << _fd << " seq " << buf.sequence);
1276 
1277  auto buffer = _buffers[buf.index];
1278  buf_mgr.handle_buffer(e_video_buf, _fd, buf, buffer);
1279 
1280  if (_is_started)
1281  {
1282  if(buf.bytesused == 0)
1283  {
1284  LOG_DEBUG_V4L("Empty video frame arrived, index " << buf.index);
1285  return;
1286  }
1287 
1288  // Drop partial and overflow frames (assumes D4XX metadata only)
1289  bool partial_frame = (!compressed_format && (buf.bytesused < buffer->get_full_length() - MAX_META_DATA_SIZE));
1290  bool overflow_frame = (buf.bytesused == buffer->get_length_frame_only() + MAX_META_DATA_SIZE);
1291  if (partial_frame || overflow_frame)
1292  {
1293  auto percentage = (100 * buf.bytesused) / buffer->get_full_length();
1294  std::stringstream s;
1295  if (partial_frame)
1296  {
1297  s << "Incomplete video frame detected!\nSize " << buf.bytesused
1298  << " out of " << buffer->get_full_length() << " bytes (" << percentage << "%)";
1299  if (overflow_frame)
1300  {
1301  s << ". Overflow detected: payload size " << buffer->get_length_frame_only();
1302  LOG_ERROR("Corrupted UVC frame data, underflow and overflow reported:\n" << s.str().c_str());
1303  }
1304  }
1305  else
1306  {
1307  if (overflow_frame)
1308  s << "overflow video frame detected!\nSize " << buf.bytesused
1309  << ", payload size " << buffer->get_length_frame_only();
1310  }
1311  LOG_DEBUG("Incomplete frame received: " << s.str()); // Ev -try1
1312  bool kpi_violated = _frame_drop_monitor.update_and_check_kpi(_profile, buf.timestamp);
1313  if (kpi_violated)
1314  {
1316  _error_handler(n);
1317  }
1318 
1319  // Check if metadata was already allocated
1320  if (buf_mgr.metadata_size())
1321  {
1322  LOG_WARNING("Metadata was present when partial frame arrived, mark md as extracted");
1323  md_extracted = true;
1324  LOG_DEBUG_V4L("Discarding md due to invalid video payload");
1325  auto md_buf = buf_mgr.get_buffers().at(e_metadata_buf);
1326  md_buf._data_buf->request_next_frame(md_buf._file_desc,true);
1327  }
1328  }
1329  else
1330  {
1331  if (!_info.has_metadata_node)
1332  {
1333  if(has_metadata())
1334  {
1335  auto timestamp = (double)buf.timestamp.tv_sec*1000.f + (double)buf.timestamp.tv_usec/1000.f;
1337 
1338  // Read metadata. Metadata node performs a blocking call to ensure video and metadata sync
1339  acquire_metadata(buf_mgr,fds,compressed_format);
1340  md_extracted = true;
1341 
1342  if (wa_applied)
1343  {
1344  auto fn = *(uint32_t*)((char*)(buf_mgr.metadata_start())+28);
1345  LOG_DEBUG_V4L("Extracting md buff, fn = " << fn);
1346  }
1347 
1348  auto frame_sz = buf_mgr.md_node_present() ? buf.bytesused :
1349  std::min(buf.bytesused - buf_mgr.metadata_size(), buffer->get_length_frame_only());
1350  frame_object fo{ frame_sz, buf_mgr.metadata_size(),
1352 
1353  buffer->attach_buffer(buf);
1354  buf_mgr.handle_buffer(e_video_buf,-1); // transfer new buffer request to the frame callback
1355 
1356  if (buf_mgr.verify_vd_md_sync())
1357  {
1358  //Invoke user callback and enqueue next frame
1359  _callback(_profile, fo, [buf_mgr]() mutable {
1360  buf_mgr.request_next_frame();
1361  });
1362  }
1363  else
1364  {
1365  LOG_WARNING("Video frame dropped, video and metadata buffers inconsistency");
1366  }
1367  }
1368  else // when metadata is not enabled at all, streaming only video
1369  {
1370  auto timestamp = (double)buf.timestamp.tv_sec * 1000.f + (double)buf.timestamp.tv_usec / 1000.f;
1372 
1373  LOG_DEBUG_V4L("no metadata streamed");
1374  if (buf_mgr.verify_vd_md_sync())
1375  {
1376  buffer->attach_buffer(buf);
1377  buf_mgr.handle_buffer(e_video_buf, -1); // transfer new buffer request to the frame callback
1378 
1379 
1380  auto frame_sz = buf_mgr.md_node_present() ? buf.bytesused :
1381  std::min(buf.bytesused - buf_mgr.metadata_size(),
1383 
1384  uint8_t md_size = buf_mgr.metadata_size();
1385  void* md_start = buf_mgr.metadata_start();
1386 
1387  // D457 development - hid over uvc - md size for IMU is 64
1388  metadata_hid_raw meta_data{};
1389  if (md_size == 0 && buffer->get_length_frame_only() <= 64)
1390  {
1391  // Populate HID IMU data - Header
1392  populate_imu_data(meta_data, buffer->get_frame_start(), md_size, &md_start);
1393  }
1394 
1395  frame_object fo{ frame_sz, md_size,
1396  buffer->get_frame_start(), md_start, timestamp };
1397 
1398  //Invoke user callback and enqueue next frame
1399  _callback(_profile, fo, [buf_mgr]() mutable {
1400  buf_mgr.request_next_frame();
1401  });
1402  }
1403  else
1404  {
1405  LOG_WARNING("Video frame dropped, video and metadata buffers inconsistency");
1406  }
1407  }
1408  }
1409  else
1410  {
1411  // saving video buffer to syncer
1412  _video_md_syncer.push_video({std::make_shared<v4l2_buffer>(buf), _fd, buf.index});
1413  buf_mgr.handle_buffer(e_video_buf, -1);
1414  }
1415  }
1416  }
1417  else
1418  {
1419  LOG_DEBUG_V4L("Video frame arrived in idle mode."); // TODO - verification
1420  }
1421  }
1422  else
1423  {
1424  if (_is_started)
1425  keep_md = true;
1426  LOG_DEBUG("FD_ISSET: no data on video node sink");
1427  }
1428 
1429  // pulling synchronized video and metadata and uploading them to user's callback
1431  }
1432  }
1433  else // (val==0)
1434  {
1435  LOG_WARNING("Frames didn't arrived within 5 seconds");
1436  librealsense::notification n = {RS2_NOTIFICATION_CATEGORY_FRAMES_TIMEOUT, 0, RS2_LOG_SEVERITY_WARN, "Frames didn't arrived within 5 seconds"};
1437 
1438  _error_handler(n);
1439  }
1440  }
1441  }
1442 
1443  void v4l_uvc_device::populate_imu_data(metadata_hid_raw& meta_data, uint8_t* frame_start, uint8_t& md_size, void** md_start) const
1444  {
1447  meta_data.header.timestamp = *(reinterpret_cast<uint64_t *>(frame_start + offsetof(hid_mipi_data, hwTs)));
1448  // Payload:
1451 
1452  md_size = sizeof(metadata_hid_raw);
1453  *md_start = &meta_data;
1454  }
1455 
1456 
1458  {
1459  // uploading to user's callback
1460  std::shared_ptr<v4l2_buffer> video_v4l2_buffer;
1461  std::shared_ptr<v4l2_buffer> md_v4l2_buffer;
1462 
1464  {
1465  int video_fd = -1, md_fd = -1;
1466  if (_video_md_syncer.pull_video_with_metadata(video_v4l2_buffer, md_v4l2_buffer, video_fd, md_fd))
1467  {
1468  // Preparing video buffer
1469  auto video_buffer = get_video_buffer(video_v4l2_buffer->index);
1470  video_buffer->attach_buffer(*video_v4l2_buffer);
1471 
1472  // happens when the video did not arrive on
1473  // the current polling iteration (was taken from the syncer's video queue)
1474  if (buf_mgr.get_buffers()[e_video_buf]._file_desc == -1)
1475  {
1476  buf_mgr.handle_buffer(e_video_buf, video_fd, *video_v4l2_buffer, video_buffer);
1477  }
1478  buf_mgr.handle_buffer(e_video_buf, -1); // transfer new buffer request to the frame callback
1479 
1480  // Preparing metadata buffer
1481  auto metadata_buffer = get_md_buffer(md_v4l2_buffer->index);
1482  set_metadata_attributes(buf_mgr, md_v4l2_buffer->bytesused, metadata_buffer->get_frame_start());
1483  metadata_buffer->attach_buffer(*md_v4l2_buffer);
1484 
1485  if (buf_mgr.get_buffers()[e_metadata_buf]._file_desc == -1)
1486  {
1487  buf_mgr.handle_buffer(e_metadata_buf, md_fd, *md_v4l2_buffer, metadata_buffer);
1488  }
1489  buf_mgr.handle_buffer(e_metadata_buf, -1); // transfer new buffer request to the frame callback
1490 
1491  auto frame_sz = buf_mgr.md_node_present() ? video_v4l2_buffer->bytesused :
1492  std::min(video_v4l2_buffer->bytesused - buf_mgr.metadata_size(),
1493  video_buffer->get_length_frame_only());
1494 
1495  auto timestamp = (double)video_v4l2_buffer->timestamp.tv_sec * 1000.f + (double)video_v4l2_buffer->timestamp.tv_usec / 1000.f;
1496  timestamp = monotonic_to_realtime(timestamp);
1497 
1498  // D457 work - to work with "normal camera", use frame_sz as the first input to the following frame_object:
1499  //frame_object fo{ buf.bytesused - MAX_META_DATA_SIZE, buf_mgr.metadata_size(),
1500  frame_object fo{ frame_sz, buf_mgr.metadata_size(),
1501  video_buffer->get_frame_start(), buf_mgr.metadata_start(), timestamp };
1502 
1503  //Invoke user callback and enqueue next frame
1504  _callback(_profile, fo, [buf_mgr]() mutable {
1505  buf_mgr.request_next_frame();
1506  });
1507  }
1508  else
1509  {
1510  LOG_DEBUG("video_md_syncer - synchronized video and md could not be pulled");
1511  }
1512  }
1513  }
1514 
1515  void v4l_uvc_device::set_metadata_attributes(buffers_mgr& buf_mgr, __u32 bytesused, uint8_t* md_start)
1516  {
1517  size_t uvc_md_start_offset = sizeof(uvc_meta_buffer::ns) + sizeof(uvc_meta_buffer::sof);
1518  buf_mgr.set_md_attributes(bytesused - uvc_md_start_offset,
1519  md_start + uvc_md_start_offset);
1520  }
1521 
1522  void v4l_mipi_device::set_metadata_attributes(buffers_mgr& buf_mgr, __u32 bytesused, uint8_t* md_start)
1523  {
1524  buf_mgr.set_md_attributes(bytesused, md_start);
1525  }
1526 
1527 
1528  void v4l_uvc_device::acquire_metadata(buffers_mgr& buf_mgr,fd_set &, bool compressed_format)
1529  {
1530  if (has_metadata())
1531  buf_mgr.set_md_from_video_node(compressed_format);
1532  else
1533  buf_mgr.set_md_attributes(0, nullptr);
1534  }
1535 
1537  {
1538  if (state == D0 && _state == D3)
1539  {
1541  }
1542  if (state == D3 && _state == D0)
1543  {
1544  close(_profile);
1546  }
1547  _state = state;
1548  }
1549 
1550  bool v4l_uvc_device::set_xu(const extension_unit& xu, uint8_t control, const uint8_t* data, int size)
1551  {
1552  uvc_xu_control_query q = {static_cast<uint8_t>(xu.unit), control, UVC_SET_CUR,
1553  static_cast<uint16_t>(size), const_cast<uint8_t *>(data)};
1554  if(xioctl(_fd, UVCIOC_CTRL_QUERY, &q) < 0)
1555  {
1556  if (errno == EIO || errno == EAGAIN) // TODO: Log?
1557  return false;
1558 
1559  throw linux_backend_exception("set_xu(...). xioctl(UVCIOC_CTRL_QUERY) failed");
1560  }
1561 
1562  return true;
1563  }
1564  bool v4l_uvc_device::get_xu(const extension_unit& xu, uint8_t control, uint8_t* data, int size) const
1565  {
1566  memset(data, 0, size);
1567  uvc_xu_control_query q = {static_cast<uint8_t>(xu.unit), control, UVC_GET_CUR,
1568  static_cast<uint16_t>(size), const_cast<uint8_t *>(data)};
1569  if(xioctl(_fd, UVCIOC_CTRL_QUERY, &q) < 0)
1570  {
1571  if (errno == EIO || errno == EAGAIN || errno == EBUSY) // TODO: Log?
1572  return false;
1573 
1574  throw linux_backend_exception("get_xu(...). xioctl(UVCIOC_CTRL_QUERY) failed");
1575  }
1576 
1577  return true;
1578  }
1580  {
1581  control_range result{};
1582  __u16 size = 0;
1583  //__u32 value = 0; // all of the real sense extended controls are up to 4 bytes
1584  // checking return value for UVC_GET_LEN and allocating
1585  // appropriately might be better
1586  //__u8 * data = (__u8 *)&value;
1587  // MS XU controls are partially supported only
1588  struct uvc_xu_control_query xquery = {};
1589  memset(&xquery, 0, sizeof(xquery));
1590  xquery.query = UVC_GET_LEN;
1591  xquery.size = 2; // size seems to always be 2 for the LEN query, but
1592  //doesn't seem to be documented. Use result for size
1593  //in all future queries of the same control number
1594  xquery.selector = control;
1595  xquery.unit = xu.unit;
1596  xquery.data = (__u8 *)&size;
1597 
1598  if(-1 == ioctl(_fd,UVCIOC_CTRL_QUERY,&xquery)){
1599  throw linux_backend_exception("xioctl(UVC_GET_LEN) failed");
1600  }
1601 
1602  assert(size<=len);
1603 
1604  std::vector<uint8_t> buf;
1605  auto buf_size = std::max((size_t)len,sizeof(__u32));
1606  buf.resize(buf_size);
1607 
1608  xquery.query = UVC_GET_MIN;
1609  xquery.size = size;
1610  xquery.selector = control;
1611  xquery.unit = xu.unit;
1612  xquery.data = buf.data();
1613  if(-1 == ioctl(_fd,UVCIOC_CTRL_QUERY,&xquery)){
1614  throw linux_backend_exception("xioctl(UVC_GET_MIN) failed");
1615  }
1616  result.min.resize(buf_size);
1617  std::copy(buf.begin(), buf.end(), result.min.begin());
1618 
1619  xquery.query = UVC_GET_MAX;
1620  xquery.size = size;
1621  xquery.selector = control;
1622  xquery.unit = xu.unit;
1623  xquery.data = buf.data();
1624  if(-1 == ioctl(_fd,UVCIOC_CTRL_QUERY,&xquery)){
1625  throw linux_backend_exception("xioctl(UVC_GET_MAX) failed");
1626  }
1627  result.max.resize(buf_size);
1628  std::copy(buf.begin(), buf.end(), result.max.begin());
1629 
1630  xquery.query = UVC_GET_DEF;
1631  xquery.size = size;
1632  xquery.selector = control;
1633  xquery.unit = xu.unit;
1634  xquery.data = buf.data();
1635  if(-1 == ioctl(_fd,UVCIOC_CTRL_QUERY,&xquery)){
1636  throw linux_backend_exception("xioctl(UVC_GET_DEF) failed");
1637  }
1638  result.def.resize(buf_size);
1639  std::copy(buf.begin(), buf.end(), result.def.begin());
1640 
1641  xquery.query = UVC_GET_RES;
1642  xquery.size = size;
1643  xquery.selector = control;
1644  xquery.unit = xu.unit;
1645  xquery.data = buf.data();
1646  if(-1 == ioctl(_fd,UVCIOC_CTRL_QUERY,&xquery)){
1647  throw linux_backend_exception("xioctl(UVC_GET_CUR) failed");
1648  }
1649  result.step.resize(buf_size);
1650  std::copy(buf.begin(), buf.end(), result.step.begin());
1651 
1652  return result;
1653  }
1654 
1656  {
1657  struct v4l2_control control = {get_cid(opt), 0};
1658  if (xioctl(_fd, VIDIOC_G_CTRL, &control) < 0)
1659  {
1660  if (errno == EIO || errno == EAGAIN) // TODO: Log?
1661  return false;
1662 
1663  throw linux_backend_exception("xioctl(VIDIOC_G_CTRL) failed");
1664  }
1665 
1666  if (RS2_OPTION_ENABLE_AUTO_EXPOSURE==opt) { control.value = (V4L2_EXPOSURE_MANUAL==control.value) ? 0 : 1; }
1667  value = control.value;
1668 
1669  return true;
1670  }
1671 
1673  {
1674  struct v4l2_control control = {get_cid(opt), value};
1675  if (RS2_OPTION_ENABLE_AUTO_EXPOSURE==opt) { control.value = value ? V4L2_EXPOSURE_APERTURE_PRIORITY : V4L2_EXPOSURE_MANUAL; }
1676 
1677  // We chose not to protect the subscribe / unsubscribe with mutex due to performance reasons,
1678  // we prefer returning on timeout (and let the retry mechanism try again if exist) than blocking the main thread on every set command
1679 
1680 
1681  // RAII to handle unsubscribe in case of exceptions
1682  std::unique_ptr< uint32_t, std::function< void( uint32_t * ) > > unsubscriber(
1683  new uint32_t( control.id ),
1684  [this]( uint32_t * id ) {
1685  if (id)
1686  {
1687  // `unsubscribe_from_ctrl_event()` may throw so we first release the memory allocated and than call it.
1688  auto local_id = *id;
1689  delete id;
1690  unsubscribe_from_ctrl_event( local_id );
1691  }
1692  } );
1693 
1694  subscribe_to_ctrl_event(control.id);
1695 
1696  // Set value
1697  if (xioctl(_fd, VIDIOC_S_CTRL, &control) < 0)
1698  {
1699  if (errno == EIO || errno == EAGAIN) // TODO: Log?
1700  return false;
1701 
1702  throw linux_backend_exception("xioctl(VIDIOC_S_CTRL) failed");
1703  }
1704 
1706  return false;
1707 
1708  return true;
1709  }
1710 
1712  {
1713  // Auto controls range is trimed to {0,1} range
1715  {
1716  static const int32_t min = 0, max = 1, step = 1, def = 1;
1717  control_range range(min, max, step, def);
1718 
1719  return range;
1720  }
1721 
1722  struct v4l2_queryctrl query = {};
1723  query.id = get_cid(option);
1724  if (xioctl(_fd, VIDIOC_QUERYCTRL, &query) < 0)
1725  {
1726  // Some controls (exposure, auto exposure, auto hue) do not seem to work on V4L2
1727  // Instead of throwing an error, return an empty range. This will cause this control to be omitted on our UI sample.
1728  // TODO: Figure out what can be done about these options and make this work
1729  query.minimum = query.maximum = 0;
1730  }
1731 
1732  control_range range(query.minimum, query.maximum, query.step, query.default_value);
1733 
1734  return range;
1735  }
1736 
1737  std::vector<stream_profile> v4l_uvc_device::get_profiles() const
1738  {
1739  std::vector<stream_profile> results;
1740 
1741  // Retrieve the caps one by one, first get pixel format, then sizes, then
1742  // frame rates. See http://linuxtv.org/downloads/v4l-dvb-apis for reference.
1743  v4l2_fmtdesc pixel_format = {};
1744  pixel_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1745  while (ioctl(_fd, VIDIOC_ENUM_FMT, &pixel_format) == 0)
1746  {
1747  v4l2_frmsizeenum frame_size = {};
1748  frame_size.pixel_format = pixel_format.pixelformat;
1749 
1750  uint32_t fourcc = (const big_endian<int> &)pixel_format.pixelformat;
1751 
1752  if (pixel_format.pixelformat == 0)
1753  {
1754  // Microsoft Depth GUIDs for R400 series are not yet recognized
1755  // by the Linux kernel, but they do not require a patch, since there
1756  // are "backup" Z16 and Y8 formats in place
1757  std::set<std::string> known_problematic_formats = {
1758  "00000050-0000-0010-8000-00aa003",
1759  "00000032-0000-0010-8000-00aa003",
1760  };
1761 
1762  if (std::find(known_problematic_formats.begin(),
1763  known_problematic_formats.end(),
1764  (const char*)pixel_format.description) ==
1765  known_problematic_formats.end())
1766  {
1767  const std::string s(to_string() << "!" << pixel_format.description);
1768  std::regex rgx("!([0-9a-f]+)-.*");
1769  std::smatch match;
1770 
1771  if (std::regex_search(s.begin(), s.end(), match, rgx))
1772  {
1773  std::stringstream ss;
1774  ss << match[1];
1775  int id;
1776  ss >> std::hex >> id;
1777  fourcc = (const big_endian<int> &)id;
1778 
1779  auto format_str = fourcc_to_string(id);
1780  LOG_WARNING("Pixel format " << pixel_format.description << " likely requires patch for fourcc code " << format_str << "!");
1781  }
1782  }
1783  }
1784  else
1785  {
1786  LOG_DEBUG("Recognized pixel-format " << pixel_format.description);
1787  }
1788 
1789  while (ioctl(_fd, VIDIOC_ENUM_FRAMESIZES, &frame_size) == 0)
1790  {
1791  v4l2_frmivalenum frame_interval = {};
1792  frame_interval.pixel_format = pixel_format.pixelformat;
1793  frame_interval.width = frame_size.discrete.width;
1794  frame_interval.height = frame_size.discrete.height;
1795  while (ioctl(_fd, VIDIOC_ENUM_FRAMEINTERVALS, &frame_interval) == 0)
1796  {
1797  if (frame_interval.type == V4L2_FRMIVAL_TYPE_DISCRETE)
1798  {
1799  if (frame_interval.discrete.numerator != 0)
1800  {
1801  auto fps =
1802  static_cast<float>(frame_interval.discrete.denominator) /
1803  static_cast<float>(frame_interval.discrete.numerator);
1804 
1805  stream_profile p{};
1806  p.format = fourcc;
1807  p.width = frame_size.discrete.width;
1808  p.height = frame_size.discrete.height;
1809  p.fps = fps;
1810  if (fourcc != 0) results.push_back(p);
1811  }
1812  }
1813 
1814  ++frame_interval.index;
1815  }
1816 
1817  ++frame_size.index;
1818  }
1819 
1820  ++pixel_format.index;
1821  }
1822  return results;
1823  }
1824 
1826  {
1827  _named_mtx->lock();
1828  }
1830  {
1831  _named_mtx->unlock();
1832  }
1833 
1835  {
1836  switch(option)
1837  {
1838  case RS2_OPTION_BACKLIGHT_COMPENSATION: return V4L2_CID_BACKLIGHT_COMPENSATION;
1839  case RS2_OPTION_BRIGHTNESS: return V4L2_CID_BRIGHTNESS;
1840  case RS2_OPTION_CONTRAST: return V4L2_CID_CONTRAST;
1841  case RS2_OPTION_EXPOSURE: return V4L2_CID_EXPOSURE_ABSOLUTE; // Is this actually valid? I'm getting a lot of VIDIOC error 22s...
1842  case RS2_OPTION_GAIN: return V4L2_CID_GAIN;
1843  case RS2_OPTION_GAMMA: return V4L2_CID_GAMMA;
1844  case RS2_OPTION_HUE: return V4L2_CID_HUE;
1845  case RS2_OPTION_SATURATION: return V4L2_CID_SATURATION;
1846  case RS2_OPTION_SHARPNESS: return V4L2_CID_SHARPNESS;
1847  case RS2_OPTION_WHITE_BALANCE: return V4L2_CID_WHITE_BALANCE_TEMPERATURE;
1848  case RS2_OPTION_ENABLE_AUTO_EXPOSURE: return V4L2_CID_EXPOSURE_AUTO; // Automatic gain/exposure control
1849  case RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE: return V4L2_CID_AUTO_WHITE_BALANCE;
1850  case RS2_OPTION_POWER_LINE_FREQUENCY : return V4L2_CID_POWER_LINE_FREQUENCY;
1851  case RS2_OPTION_AUTO_EXPOSURE_PRIORITY: return V4L2_CID_EXPOSURE_AUTO_PRIORITY;
1852  default: throw linux_backend_exception(to_string() << "no v4l2 cid for option " << option);
1853  }
1854  }
1855 
1857  {
1858  try
1859  {
1860  while(_is_capturing)
1861  {
1862  poll();
1863  }
1864  }
1865  catch (const std::exception& ex)
1866  {
1867  LOG_ERROR(ex.what());
1868 
1870 
1871  _error_handler(n);
1872  }
1873  }
1874 
1876  {
1877  return !_use_memory_map;
1878  }
1879 
1881  {
1882  stream_ctl_on(_fd);
1883  }
1884 
1886  {
1887  stream_off(_fd);
1888  }
1889 
1891  {
1892  req_io_buff(_fd, num, _name,
1893  _use_memory_map ? V4L2_MEMORY_MMAP : V4L2_MEMORY_USERPTR,
1894  V4L2_BUF_TYPE_VIDEO_CAPTURE);
1895  }
1896 
1898  {
1899  if (buffers)
1900  {
1901  for(size_t i = 0; i < buffers; ++i)
1902  {
1903  _buffers.push_back(std::make_shared<buffer>(_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, _use_memory_map, i));
1904  }
1905  }
1906  else
1907  {
1908  for(size_t i = 0; i < _buffers.size(); i++)
1909  {
1910  _buffers[i]->detach_buffer();
1911  }
1912  _buffers.resize(0);
1913  }
1914  }
1915 
1917  {
1918  _fd = open(_name.c_str(), O_RDWR | O_NONBLOCK, 0);
1919  if(_fd < 0)
1920  throw linux_backend_exception(to_string() <<__FUNCTION__ << " Cannot open '" << _name);
1921 
1922  if (pipe(_stop_pipe_fd) < 0)
1923  throw linux_backend_exception(to_string() <<__FUNCTION__ << " Cannot create pipe!");
1924 
1925  if (_fds.size())
1926  throw linux_backend_exception(to_string() <<__FUNCTION__ << " Device descriptor is already allocated");
1927 
1928  _fds.insert(_fds.end(),{_fd,_stop_pipe_fd[0],_stop_pipe_fd[1]});
1929  _max_fd = *std::max_element(_fds.begin(),_fds.end());
1930 
1931  v4l2_capability cap = {};
1932  if(xioctl(_fd, VIDIOC_QUERYCAP, &cap) < 0)
1933  {
1934  if(errno == EINVAL)
1935  throw linux_backend_exception(_name + " is not V4L2 device");
1936  else
1937  throw linux_backend_exception("xioctl(VIDIOC_QUERYCAP) failed");
1938  }
1939  if(!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
1940  throw linux_backend_exception(_name + " is no video capture device");
1941 
1942  if(!(cap.capabilities & V4L2_CAP_STREAMING))
1943  throw linux_backend_exception(_name + " does not support streaming I/O");
1944 
1945  // Select video input, video standard and tune here.
1946  v4l2_cropcap cropcap = {};
1947  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1948  if(xioctl(_fd, VIDIOC_CROPCAP, &cropcap) == 0)
1949  {
1950  v4l2_crop crop = {};
1951  crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1952  crop.c = cropcap.defrect; // reset to default
1953  if(xioctl(_fd, VIDIOC_S_CROP, &crop) < 0)
1954  {
1955  switch (errno)
1956  {
1957  case EINVAL: break; // Cropping not supported
1958  default: break; // Errors ignored
1959  }
1960  }
1961  } else {} // Errors ignored
1962  }
1963 
1965  {
1966  if(::close(_fd) < 0)
1967  throw linux_backend_exception("v4l_uvc_device: close(_fd) failed");
1968 
1969  if(::close(_stop_pipe_fd[0]) < 0)
1970  throw linux_backend_exception("v4l_uvc_device: close(_stop_pipe_fd[0]) failed");
1971  if(::close(_stop_pipe_fd[1]) < 0)
1972  throw linux_backend_exception("v4l_uvc_device: close(_stop_pipe_fd[1]) failed");
1973 
1974  _fd = 0;
1975  _stop_pipe_fd[0] = _stop_pipe_fd[1] = 0;
1976  _fds.clear();
1977  }
1978 
1980  {
1981  v4l2_format fmt = {};
1982  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1983  fmt.fmt.pix.width = profile.width;
1984  fmt.fmt.pix.height = profile.height;
1985  fmt.fmt.pix.pixelformat = (const big_endian<int> &)profile.format;
1986  fmt.fmt.pix.field = V4L2_FIELD_NONE;
1987  if(xioctl(_fd, VIDIOC_S_FMT, &fmt) < 0)
1988  {
1989  throw linux_backend_exception(to_string() << "xioctl(VIDIOC_S_FMT) failed, errno=" << errno);
1990  }
1991  else
1992  LOG_INFO("Video node was successfully configured to " << fourcc_to_string(fmt.fmt.pix.pixelformat) << " format" <<", fd " << std::dec << _fd);
1993 
1994  LOG_INFO("Trying to configure fourcc " << fourcc_to_string(fmt.fmt.pix.pixelformat));
1995  }
1996 
1998  {
1999  struct v4l2_event_subscription event_subscription ;
2000  event_subscription.flags = V4L2_EVENT_SUB_FL_ALLOW_FEEDBACK;
2001  event_subscription.type = V4L2_EVENT_CTRL;
2002  event_subscription.id = control_id;
2003  memset(event_subscription.reserved,0, sizeof(event_subscription.reserved));
2004  if (xioctl(_fd, VIDIOC_SUBSCRIBE_EVENT, &event_subscription) < 0)
2005  {
2006  throw linux_backend_exception(to_string() << "xioctl(VIDIOC_SUBSCRIBE_EVENT) with control_id = " << control_id << " failed");
2007  }
2008  }
2009 
2011  {
2012  struct v4l2_event_subscription event_subscription ;
2013  event_subscription.flags = V4L2_EVENT_SUB_FL_ALLOW_FEEDBACK;
2014  event_subscription.type = V4L2_EVENT_CTRL;
2015  event_subscription.id = control_id;
2016  memset(event_subscription.reserved,0, sizeof(event_subscription.reserved));
2017  if (xioctl(_fd, VIDIOC_UNSUBSCRIBE_EVENT, &event_subscription) < 0)
2018  {
2019  throw linux_backend_exception(to_string() << "xioctl(VIDIOC_UNSUBSCRIBE_EVENT) with control_id = " << control_id << " failed");
2020  }
2021  }
2022 
2023 
2025  {
2026  struct v4l2_event event;
2027  memset(&event, 0 , sizeof(event));
2028 
2029  // Poll registered events and verify that set control event raised (wait max of 10 * 2 = 20 [ms])
2030  static int MAX_POLL_RETRIES = 10;
2031  for ( int i = 0 ; i < MAX_POLL_RETRIES && event.type != V4L2_EVENT_CTRL ; i++)
2032  {
2033  if(xioctl(_fd, VIDIOC_DQEVENT, &event) < 0)
2034  {
2035  std::this_thread::sleep_for(std::chrono::milliseconds(2));
2036  }
2037  }
2038 
2039  return event.type == V4L2_EVENT_CTRL;
2040  }
2041 
2043  v4l_uvc_device(info,use_memory_map),
2044  _md_fd(0),
2045  _md_name(info.metadata_node_id)
2046  {
2047  }
2048 
2050  {
2051  }
2052 
2054  {
2055  if (_md_fd != -1)
2056  {
2057  // D457 development - added for mipi device, for IR because no metadata there
2058  // Metadata stream shall be configured first to allow sync with video node
2060  }
2061 
2062  // Invoke UVC streaming request
2064 
2065  }
2066 
2068  {
2070  if (_md_fd != -1)
2071  {
2072  // D457 development - added for mipi device, for IR because no metadata there
2074  }
2075  }
2076 
2078  {
2080 
2081  if (_md_fd == -1)
2082  {
2083  // D457 development - added for mipi device, for IR because no metadata there
2084  return;
2085  }
2086  req_io_buff(_md_fd, num, _name,
2087  _use_memory_map ? V4L2_MEMORY_MMAP : V4L2_MEMORY_USERPTR,
2089  }
2090 
2092  {
2094 
2095  if (buffers)
2096  {
2097  for(size_t i = 0; i < buffers; ++i)
2098  {
2099  // D457 development - added for mipi device, for IR because no metadata there
2100  if (_md_fd == -1)
2101  continue;
2102  _md_buffers.push_back(std::make_shared<buffer>(_md_fd, LOCAL_V4L2_BUF_TYPE_META_CAPTURE, _use_memory_map, i));
2103  }
2104  }
2105  else
2106  {
2107  for(size_t i = 0; i < _md_buffers.size(); i++)
2108  {
2109  _md_buffers[i]->detach_buffer();
2110  }
2111  _md_buffers.resize(0);
2112  }
2113  }
2114 
2116  {
2118 
2119  if (_md_fd>0)
2120  throw linux_backend_exception(to_string() << _md_name << " descriptor is already opened");
2121 
2122  _md_fd = open(_md_name.c_str(), O_RDWR | O_NONBLOCK, 0);
2123  if(_md_fd < 0)
2124  {
2125  // D457 development - added for mipi device, for IR because no metadata there
2126  return;
2127  throw linux_backend_exception(to_string() << "Cannot open '" << _md_name);
2128  }
2129 
2130  //The minimal video/metadata nodes syncer will be implemented by using two blocking calls:
2131  // 1. Obtain video node data.
2132  // 2. Obtain metadata
2133  // To revert to multiplexing mode uncomment the next line
2134  _fds.push_back(_md_fd);
2135  _max_fd = *std::max_element(_fds.begin(),_fds.end());
2136 
2137  v4l2_capability cap = {};
2138  if(xioctl(_md_fd, VIDIOC_QUERYCAP, &cap) < 0)
2139  {
2140  if(errno == EINVAL)
2141  throw linux_backend_exception(_md_name + " is no V4L2 device");
2142  else
2143  throw linux_backend_exception(_md_name + " xioctl(VIDIOC_QUERYCAP) for metadata failed");
2144  }
2145 
2146  if(!(cap.capabilities & V4L2_CAP_META_CAPTURE))
2147  throw linux_backend_exception(_md_name + " is not metadata capture device");
2148 
2149  if(!(cap.capabilities & V4L2_CAP_STREAMING))
2150  throw linux_backend_exception(_md_name + " does not support metadata streaming I/O");
2151  }
2152 
2153 
2155  {
2157 
2158  if(::close(_md_fd) < 0)
2159  {
2160  // D457 development - added for mipi device, for IR because no metadata there
2161  return;
2162  throw linux_backend_exception("v4l_uvc_meta_device: close(_md_fd) failed");
2163  }
2164 
2165  _md_fd = 0;
2166  }
2167 
2169  {
2170  // Select video node streaming format
2171  v4l_uvc_device::set_format(profile);
2172 
2173  // Configure metadata node stream format
2174  v4l2_format fmt{ };
2176 
2177  if (xioctl(_md_fd, VIDIOC_G_FMT, &fmt))
2178  {
2179  // D457 development - added for mipi device, for IR because no metadata there
2180  return;
2181  throw linux_backend_exception(_md_name + " ioctl(VIDIOC_G_FMT) for metadata node failed");
2182  }
2183 
2185  throw linux_backend_exception("ioctl(VIDIOC_G_FMT): " + _md_name + " node is not metadata capture");
2186 
2187  bool success = false;
2188  for (const uint32_t& request : { V4L2_META_FMT_D4XX, V4L2_META_FMT_UVC})
2189  {
2190  // Configure metadata format - try d4xx, then fallback to currently retrieve UVC default header of 12 bytes
2191  memcpy(fmt.fmt.raw_data,&request,sizeof(request));
2192 
2193  if(xioctl(_md_fd, VIDIOC_S_FMT, &fmt) >= 0)
2194  {
2195  LOG_INFO("Metadata node was successfully configured to " << fourcc_to_string(request) << " format" <<", fd " << std::dec <<_md_fd);
2196  success =true;
2197  break;
2198  }
2199  else
2200  {
2201  LOG_WARNING("Metadata node configuration failed for " << fourcc_to_string(request));
2202  }
2203  }
2204 
2205  if (!success)
2206  throw linux_backend_exception(_md_name + " ioctl(VIDIOC_S_FMT) for metadata node failed");
2207 
2208  }
2209 
2211  {
2212  if (_md_fd != -1)
2213  {
2214  // D457 development - added for mipi device, for IR because no metadata there
2215  // Meta node to be initialized first to enforce initial sync
2216  for (auto&& buf : _md_buffers) buf->prepare_for_streaming(_md_fd);
2217  }
2218 
2219  // Request streaming for video node
2221  }
2222 
2223  // Retrieve metadata from a dedicated UVC node. For kernels 4.16+
2224  void v4l_uvc_meta_device::acquire_metadata(buffers_mgr & buf_mgr,fd_set &fds, bool)
2225  {
2226  //Use non-blocking metadata node polling
2227  if(_md_fd > 0 && FD_ISSET(_md_fd, &fds))
2228  {
2229  // In scenario if [md+vid] ->[md] ->[md,vid] the third md should not be retrieved but wait for next select
2230  if (buf_mgr.metadata_size())
2231  {
2232  LOG_WARNING("Metadata override requested but avoided skipped");
2233  // D457 wa - return removed
2234  //return;
2235  // In scenario: {vid[i]} ->{md[i]} ->{md[i+1],vid[i+1]}:
2236  // - vid[i] will be uploaded to user callback without metadata (for now)
2237  // - md[i] will be dropped
2238  // - vid[i+1] and md[i+1] will be uploaded together - back to stable stream
2239  auto md_buf = buf_mgr.get_buffers().at(e_metadata_buf);
2240  md_buf._data_buf->request_next_frame(md_buf._file_desc,true);
2241  }
2242  FD_CLR(_md_fd,&fds);
2243 
2244  v4l2_buffer buf{};
2246  buf.memory = _use_memory_map ? V4L2_MEMORY_MMAP : V4L2_MEMORY_USERPTR;
2247 
2248  // W/O multiplexing this will create a blocking call for metadata node
2249  if(xioctl(_md_fd, VIDIOC_DQBUF, &buf) < 0)
2250  {
2251  LOG_DEBUG_V4L("Dequeued empty buf for md fd " << std::dec << _md_fd);
2252  }
2253 
2254  //V4l debugging message
2255  auto mdbuf = _md_buffers[buf.index]->get_frame_start();
2256  auto hwts = *(uint32_t*)((mdbuf+2));
2257  auto fn = *(uint32_t*)((mdbuf+38));
2258  LOG_DEBUG_V4L("Dequeued md buf " << std::dec << buf.index << " for fd " << _md_fd << " seq " << buf.sequence
2259  << " fn " << fn << " hw ts " << hwts
2260  << " v4lbuf ts usec " << buf.timestamp.tv_usec);
2261 
2262  auto buffer = _md_buffers[buf.index];
2264 
2265  // pushing metadata buffer to syncer
2266  _video_md_syncer.push_metadata({std::make_shared<v4l2_buffer>(buf), _md_fd, buf.index});
2267  buf_mgr.handle_buffer(e_metadata_buf, -1);
2268  }
2269  }
2270 
2272  v4l_uvc_meta_device(info,use_memory_map)
2273  {}
2274 
2276  {}
2277 
2278  // D457 controls map- temporal solution to bypass backend interface with actual codes
2279  // DS5 depth XU identifiers
2290  const uint8_t RS_LED_PWR = 0xE;
2291  const uint8_t RS_EMITTER_FREQUENCY = 0x10; // Match to DS5_EMITTER_FREQUENCY
2292 
2293 
2295  {
2296  v4l2_ext_control control{get_cid(opt), 0, 0, 0};
2297  // Extract the control group from the underlying control query
2298  v4l2_ext_controls ctrls_block { control.id&0xffff0000, 1, 0, 0, 0, &control};
2299 
2300  if (xioctl(_fd, VIDIOC_G_EXT_CTRLS, &ctrls_block) < 0)
2301  {
2302  if (errno == EIO || errno == EAGAIN) // TODO: Log?
2303  return false;
2304 
2305  throw linux_backend_exception("xioctl(VIDIOC_G_EXT_CTRLS) failed");
2306  }
2307 
2309  control.value = (V4L2_EXPOSURE_MANUAL==control.value) ? 0 : 1;
2310  value = control.value;
2311 
2312  return true;
2313  }
2314 
2316  {
2317  v4l2_ext_control control{get_cid(opt), 0, 0, value};
2319  control.value = value ? V4L2_EXPOSURE_APERTURE_PRIORITY : V4L2_EXPOSURE_MANUAL;
2320 
2321  // Extract the control group from the underlying control query
2322  v4l2_ext_controls ctrls_block{ control.id & 0xffff0000, 1, 0, 0, 0, &control };
2323  if (xioctl(_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block) < 0)
2324  {
2325  if (errno == EIO || errno == EAGAIN) // TODO: Log?
2326  return false;
2327 
2328  throw linux_backend_exception("xioctl(VIDIOC_S_EXT_CTRLS) failed");
2329  }
2330 
2331  return true;
2332  }
2333 
2334  bool v4l_mipi_device::set_xu(const extension_unit& xu, uint8_t control, const uint8_t* data, int size)
2335  {
2336  v4l2_ext_control xctrl{xu_to_cid(xu,control), uint32_t(size), 0, 0};
2337  switch (size)
2338  {
2339  case 1: xctrl.value = *(reinterpret_cast<const uint8_t*>(data)); break;
2340  case 2: xctrl.value = *reinterpret_cast<const uint16_t*>(data); break; // TODO check signed/unsigned
2341  case 4: xctrl.value = *reinterpret_cast<const int32_t*>(data); break;
2342  case 8: xctrl.value64 = *reinterpret_cast<const int64_t*>(data); break;
2343  default:
2344  xctrl.p_u8 = const_cast<uint8_t*>(data); // TODO aggregate initialization with union
2345  }
2346 
2347  if (control == RS_ENABLE_AUTO_EXPOSURE)
2348  xctrl.value = xctrl.value ? V4L2_EXPOSURE_APERTURE_PRIORITY : V4L2_EXPOSURE_MANUAL;
2349 
2350  // Extract the control group from the underlying control query
2351  v4l2_ext_controls ctrls_block { xctrl.id&0xffff0000, 1, 0, 0, 0, &xctrl };
2352 
2353  int retVal = xioctl(_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block);
2354  if (retVal < 0)
2355  {
2356  if (errno == EIO || errno == EAGAIN) // TODO: Log?
2357  return false;
2358 
2359  throw linux_backend_exception("xioctl(VIDIOC_S_EXT_CTRLS) failed");
2360  }
2361  return true;
2362  }
2363 
2364  bool v4l_mipi_device::get_xu(const extension_unit& xu, uint8_t control, uint8_t* data, int size) const
2365  {
2366  v4l2_ext_control xctrl{xu_to_cid(xu,control), uint32_t(size), 0, 0};
2367  xctrl.p_u8 = data;
2368 
2369  v4l2_ext_controls ext {xctrl.id & 0xffff0000, 1, 0, 0, 0, &xctrl};
2370 
2371  // the ioctl fails once when performing send and receive right after it
2372  // it succeeds on the second time
2373  int tries = 2;
2374  while(tries--)
2375  {
2376  int ret = xioctl(_fd, VIDIOC_G_EXT_CTRLS, &ext);
2377  if (ret < 0)
2378  {
2379  // exception is thrown if the ioctl fails twice
2380  continue;
2381  }
2382 
2383  if (control == RS_ENABLE_AUTO_EXPOSURE)
2384  xctrl.value = (V4L2_EXPOSURE_MANUAL == xctrl.value) ? 0 : 1;
2385 
2386  // used to parse the data when only a value is returned (e.g. laser power),
2387  // and not a pointer to a buffer of data (e.g. gvd)
2388  if (size < sizeof(__s64))
2389  memcpy(data,(void*)(&xctrl.value), size);
2390 
2391  return true;
2392  }
2393 
2394  // sending error on ioctl failure
2395  if (errno == EIO || errno == EAGAIN) // TODO: Log?
2396  return false;
2397  throw linux_backend_exception("xioctl(VIDIOC_G_EXT_CTRLS) failed");
2398  }
2399 
2401  {
2402  v4l2_query_ext_ctrl xctrl_query{};
2403  xctrl_query.id = xu_to_cid(xu,control);
2404 
2405  if(0 > ioctl(_fd,VIDIOC_QUERY_EXT_CTRL,&xctrl_query)){
2406  throw linux_backend_exception(to_string() << "xioctl(VIDIOC_QUERY_EXT_CTRL) failed, errno=" << errno);
2407  }
2408 
2409  if ((xctrl_query.elems !=1 ) ||
2410  (xctrl_query.minimum < std::numeric_limits<int32_t>::min()) ||
2411  (xctrl_query.maximum > std::numeric_limits<int32_t>::max()))
2412  throw linux_backend_exception(to_string() << "Mipi Control range for " << xctrl_query.name
2413  << " is not compliant with backend interface: [min,max,default,step]:\n"
2414  << xctrl_query.minimum << ", " << xctrl_query.maximum << ", "
2415  << xctrl_query.default_value << ", " << xctrl_query.step
2416  << "\n Elements = " << xctrl_query.elems);
2417 
2418  if (control == RS_ENABLE_AUTO_EXPOSURE)
2419  return {0, 1, 1, 1};
2420  return { static_cast<int32_t>(xctrl_query.minimum), static_cast<int32_t>(xctrl_query.maximum),
2421  static_cast<int32_t>(xctrl_query.step), static_cast<int32_t>(xctrl_query.default_value)};
2422  }
2423 
2425  {
2426  return v4l_uvc_device::get_pu_range(option);
2427  }
2428 
2430  {
2431  switch(option)
2432  {
2433  case RS2_OPTION_BACKLIGHT_COMPENSATION: return V4L2_CID_BACKLIGHT_COMPENSATION;
2434  case RS2_OPTION_BRIGHTNESS: return V4L2_CID_BRIGHTNESS;
2435  case RS2_OPTION_CONTRAST: return V4L2_CID_CONTRAST;
2436  case RS2_OPTION_EXPOSURE: return V4L2_CID_EXPOSURE_ABSOLUTE; // Is this actually valid? I'm getting a lot of VIDIOC error 22s...
2437  case RS2_OPTION_GAIN: return V4L2_CTRL_CLASS_IMAGE_SOURCE | 0x903; // v4l2-ctl --list-ctrls -d /dev/video0
2438  case RS2_OPTION_GAMMA: return V4L2_CID_GAMMA;
2439  case RS2_OPTION_HUE: return V4L2_CID_HUE;
2440  case RS2_OPTION_LASER_POWER: return V4L2_CID_EXPOSURE_ABSOLUTE;
2441  case RS2_OPTION_EMITTER_ENABLED: return V4L2_CID_EXPOSURE_AUTO;
2442 // case RS2_OPTION_SATURATION: return V4L2_CID_SATURATION;
2443 // case RS2_OPTION_SHARPNESS: return V4L2_CID_SHARPNESS;
2444 // case RS2_OPTION_WHITE_BALANCE: return V4L2_CID_WHITE_BALANCE_TEMPERATURE;
2445  case RS2_OPTION_ENABLE_AUTO_EXPOSURE: return V4L2_CID_EXPOSURE_AUTO; // Automatic gain/exposure control
2446 // case RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE: return V4L2_CID_AUTO_WHITE_BALANCE;
2447 // case RS2_OPTION_POWER_LINE_FREQUENCY : return V4L2_CID_POWER_LINE_FREQUENCY;
2448 // case RS2_OPTION_AUTO_EXPOSURE_PRIORITY: return V4L2_CID_EXPOSURE_AUTO_PRIORITY;
2449  default: throw linux_backend_exception(to_string() << "no v4l2 mipi mapping cid for option " << option);
2450  }
2451  }
2452 
2453  // D457 controls map - temporal solution to bypass backend interface with actual codes
2455  {
2456  if (0==xu.subdevice)
2457  {
2458  switch(control)
2459  {
2460  case RS_HWMONITOR: return RS_CAMERA_CID_HWMC;
2462  case RS_EXPOSURE: return V4L2_CID_EXPOSURE_ABSOLUTE;//RS_CAMERA_CID_MANUAL_EXPOSURE; V4L2_CID_EXPOSURE_ABSOLUTE
2465  case RS_ENABLE_AUTO_EXPOSURE: return V4L2_CID_EXPOSURE_AUTO; //RS_CAMERA_CID_EXPOSURE_MODE;
2468  // D457 Missing functionality
2469  //case RS_ERROR_REPORTING: TBD;
2470  //case RS_EXT_TRIGGER: TBD;
2471  //case RS_ASIC_AND_PROJECTOR_TEMPERATURES: TBD;
2472  //case RS_LED_PWR: TBD;
2473 
2474  default: throw linux_backend_exception(to_string() << "no v4l2 mipi cid for XU depth control " << std::dec << int(control));
2475  }
2476  }
2477  else
2478  throw linux_backend_exception(to_string() << "MIPI Controls mapping is for Depth XU only, requested for subdevice " << xu.subdevice);
2479  }
2480 
2481  std::shared_ptr<uvc_device> v4l_backend::create_uvc_device(uvc_device_info info) const
2482  {
2483  bool mipi_device = 0xABCD == info.pid; // D457 development. Not for upstream
2484  auto v4l_uvc_dev = mipi_device ? std::make_shared<v4l_mipi_device>(info) :
2485  ((!info.has_metadata_node) ? std::make_shared<v4l_uvc_device>(info) :
2486  std::make_shared<v4l_uvc_meta_device>(info));
2487 
2488  return std::make_shared<platform::retry_controls_work_around>(v4l_uvc_dev);
2489  }
2490 
2491  std::vector<uvc_device_info> v4l_backend::query_uvc_devices() const
2492  {
2493  std::vector<uvc_device_info> uvc_nodes;
2495  [&uvc_nodes](const uvc_device_info& i, const std::string&)
2496  {
2497  uvc_nodes.push_back(i);
2498  });
2499 
2500  return uvc_nodes;
2501  }
2502 
2503  std::shared_ptr<command_transfer> v4l_backend::create_usb_device(usb_device_info info) const
2504  {
2506  if(dev)
2507  return std::make_shared<platform::command_transfer_usb>(dev);
2508  return nullptr;
2509  }
2510 
2511  std::vector<usb_device_info> v4l_backend::query_usb_devices() const
2512  {
2513  auto device_infos = usb_enumerator::query_devices_info();
2514  // Give the device a chance to restart, if we don't catch
2515  // it, the watcher will find it later.
2516  if(tm_boot(device_infos)) {
2517  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
2518  device_infos = usb_enumerator::query_devices_info();
2519  }
2520  return device_infos;
2521  }
2522 
2523  std::shared_ptr<hid_device> v4l_backend::create_hid_device(hid_device_info info) const
2524  {
2525  return std::make_shared<v4l_hid_device>(info);
2526  }
2527 
2528  std::vector<hid_device_info> v4l_backend::query_hid_devices() const
2529  {
2530  std::vector<hid_device_info> results;
2531  v4l_hid_device::foreach_hid_device([&](const hid_device_info& hid_dev_info){
2532  results.push_back(hid_dev_info);
2533  });
2534  return results;
2535  }
2536  std::shared_ptr<time_service> v4l_backend::create_time_service() const
2537  {
2538  return std::make_shared<os_time_service>();
2539  }
2540 
2541  std::shared_ptr<device_watcher> v4l_backend::create_device_watcher() const
2542  {
2543 #if defined(USING_UDEV)
2544  return std::make_shared< udev_device_watcher >( this );
2545 #else
2546  return std::make_shared< polling_device_watcher >( this );
2547 #endif
2548  }
2549 
2550  std::shared_ptr<backend> create_backend()
2551  {
2552  return std::make_shared<v4l_backend>();
2553  }
2554 
2556  {
2557  std::lock_guard<std::mutex> lock(_syncer_mutex);
2558  if(!_is_ready)
2559  {
2560  LOG_DEBUG_V4L("video_md_syncer - push_video called but syncer not ready");
2561  return;
2562  }
2563  _video_queue.push(video_buffer);
2564  LOG_DEBUG_V4L("video_md_syncer - video pushed with sequence " << video_buffer._v4l2_buf->sequence << ", buf " << video_buffer._buffer_index);
2565 
2566  // remove old video_buffer
2567  if (_video_queue.size() > 2)
2568  {
2569  // Enqueue of video buffer before throwing its content away
2570  enqueue_front_buffer_before_throwing_it(_video_queue);
2571  }
2572  }
2573 
2575  {
2576  std::lock_guard<std::mutex> lock(_syncer_mutex);
2577  if(!_is_ready)
2578  {
2579  LOG_DEBUG_V4L("video_md_syncer - push_metadata called but syncer not ready");
2580  return;
2581  }
2582  // override front buffer if it has the same sequence that the new buffer - happens with metadata sequence 0
2583  if (_md_queue.size() > 0 && _md_queue.front()._v4l2_buf->sequence == md_buffer._v4l2_buf->sequence)
2584  {
2585  LOG_DEBUG_V4L("video_md_syncer - calling enqueue_front_buffer_before_throwing_it - md buf " << md_buffer._buffer_index << " and md buf " << _md_queue.front()._buffer_index << " have same sequence");
2586  enqueue_front_buffer_before_throwing_it(_md_queue);
2587  }
2588  _md_queue.push(md_buffer);
2589  LOG_DEBUG_V4L("video_md_syncer - md pushed with sequence " << md_buffer._v4l2_buf->sequence << ", buf " << md_buffer._buffer_index);
2590  LOG_DEBUG_V4L("video_md_syncer - md queue size = " << _md_queue.size());
2591 
2592  // remove old md_buffer
2593  if (_md_queue.size() > 2)
2594  {
2595  LOG_DEBUG_V4L("video_md_syncer - calling enqueue_front_buffer_before_throwing_it - md queue size is: " << _md_queue.size());
2596  // Enqueue of md buffer before throwing its content away
2597  enqueue_front_buffer_before_throwing_it(_md_queue);
2598  }
2599  }
2600 
2601  bool v4l2_video_md_syncer::pull_video_with_metadata(std::shared_ptr<v4l2_buffer>& video_buffer, std::shared_ptr<v4l2_buffer>& md_buffer,
2602  int& video_fd, int& md_fd)
2603  {
2604  std::lock_guard<std::mutex> lock(_syncer_mutex);
2605  if(!_is_ready)
2606  {
2607  LOG_DEBUG_V4L("video_md_syncer - pull_video_with_metadata called but syncer not ready");
2608  return false;
2609  }
2610  if (_video_queue.empty())
2611  {
2612  LOG_DEBUG_V4L("video_md_syncer - video queue is empty");
2613  return false;
2614  }
2615 
2616  if (_md_queue.empty())
2617  {
2618  LOG_DEBUG_V4L("video_md_syncer - md queue is empty");
2619  return false;
2620  }
2621 
2622  sync_buffer video_candidate = _video_queue.front();
2623  sync_buffer md_candidate = _md_queue.front();
2624 
2625  // set video and md file descriptors
2626  video_fd = video_candidate._fd;
2627  md_fd = md_candidate._fd;
2628 
2629  // sync is ok if latest video and md have the same sequence
2630  if (video_candidate._v4l2_buf->sequence == md_candidate._v4l2_buf->sequence)
2631  {
2632  video_buffer = video_candidate._v4l2_buf;
2633  md_buffer = md_candidate._v4l2_buf;
2634  // removing from queues
2635  _video_queue.pop();
2636  _md_queue.pop();
2637  LOG_DEBUG_V4L("video_md_syncer - video and md pulled with sequence " << video_candidate._v4l2_buf->sequence);
2638  return true;
2639  }
2640 
2641  LOG_DEBUG_V4L("video_md_syncer - video_candidate seq " << video_candidate._v4l2_buf->sequence << ", md_candidate seq " << md_candidate._v4l2_buf->sequence);
2642 
2643  if (video_candidate._v4l2_buf->sequence > md_candidate._v4l2_buf->sequence && _md_queue.size() > 1)
2644  {
2645  // Enqueue of md buffer before throwing its content away
2646  enqueue_buffer_before_throwing_it(md_candidate);
2647  _md_queue.pop();
2648 
2649  // checking remaining metadata buffer in queue
2650  auto alternative_md_candidate = _md_queue.front();
2651  // sync is ok if latest video and md have the same sequence
2652  if (video_candidate._v4l2_buf->sequence == alternative_md_candidate._v4l2_buf->sequence)
2653  {
2654  video_buffer = video_candidate._v4l2_buf;
2655  md_buffer = alternative_md_candidate._v4l2_buf;
2656  // removing from queues
2657  _video_queue.pop();
2658  _md_queue.pop();
2659  LOG_DEBUG_V4L("video_md_syncer - video and md pulled with sequence " << video_candidate._v4l2_buf->sequence);
2660  return true;
2661  }
2662  }
2663  if (video_candidate._v4l2_buf->sequence < md_candidate._v4l2_buf->sequence && _video_queue.size() > 1)
2664  {
2665  // Enqueue of md buffer before throwing its content away
2666  enqueue_buffer_before_throwing_it(video_candidate);
2667  _video_queue.pop();
2668 
2669  // checking remaining video buffer in queue
2670  auto alternative_video_candidate = _video_queue.front();
2671  // sync is ok if latest video and md have the same sequence
2672  if (alternative_video_candidate._v4l2_buf->sequence == md_candidate._v4l2_buf->sequence)
2673  {
2674  video_buffer = alternative_video_candidate._v4l2_buf;
2675  md_buffer = md_candidate._v4l2_buf;
2676  // removing from queues
2677  _video_queue.pop();
2678  _md_queue.pop();
2679  LOG_DEBUG_V4L("video_md_syncer - video and md pulled with sequence " << md_candidate._v4l2_buf->sequence);
2680  return true;
2681  }
2682  }
2683  return false;
2684  }
2685 
2687  {
2688  // Enqueue of buffer before throwing its content away
2689  LOG_DEBUG_V4L("video_md_syncer - Enqueue buf " << std::dec << sb._buffer_index << " for fd " << sb._fd << " before dropping it");
2690  if (xioctl(sb._fd, VIDIOC_QBUF, sb._v4l2_buf.get()) < 0)
2691  {
2692  LOG_ERROR("xioctl(VIDIOC_QBUF) failed when requesting new frame! fd: " << sb._fd << " error: " << strerror(errno));
2693  }
2694  }
2695 
2696  void v4l2_video_md_syncer::enqueue_front_buffer_before_throwing_it(std::queue<sync_buffer>& sync_queue)
2697  {
2698  // Enqueue of buffer before throwing its content away
2699  LOG_DEBUG_V4L("video_md_syncer - Enqueue buf " << std::dec << sync_queue.front()._buffer_index << " for fd " << sync_queue.front()._fd << " before dropping it");
2700  if (xioctl(sync_queue.front()._fd, VIDIOC_QBUF, sync_queue.front()._v4l2_buf.get()) < 0)
2701  {
2702  LOG_ERROR("xioctl(VIDIOC_QBUF) failed when requesting new frame! fd: " << sync_queue.front()._fd << " error: " << strerror(errno));
2703  }
2704  sync_queue.pop();
2705  }
2706 
2707 
2709  {
2710  _is_ready = false;
2711  flush_queues();
2712  }
2713 
2715  {
2716  // Empty queues
2717  LOG_DEBUG_V4L("video_md_syncer - flush video and md queues");
2718  std::lock_guard<std::mutex> lock(_syncer_mutex);
2719  while(!_video_queue.empty())
2720  {
2721  _video_queue.pop();
2722  }
2723  while(!_md_queue.empty())
2724  {
2725  _md_queue.pop();
2726  }
2727  LOG_DEBUG_V4L("video_md_syncer - flush video and md queues done - mq_q size = " << _md_queue.size() << ", video_q size = " << _video_queue.size());
2728  }
2729  }
2730 }
virtual uint32_t get_cid(rs2_option option) const
void enqueue_buffer_before_throwing_it(const sync_buffer &sb) const
static const textual_icon lock
Definition: model-views.h:219
const uint8_t RS_EXT_TRIGGER
constexpr uint32_t RS_CAMERA_CID_LASER_POWER_MODE
#define LOG_ERROR(...)
Definition: easyloggingpp.h:58
const uint8_t RS_EMITTER_FREQUENCY
void set_format(stream_profile profile)
GLuint GLuint end
bool get_xu(const extension_unit &xu, uint8_t control, uint8_t *data, int size) const override
virtual void acquire_metadata(buffers_mgr &buf_mgr, fd_set &fds, bool compressed_format=false) override
static bool is_usb_device_path(const std::string &video_path)
string action
Definition: devices.py:737
std::istringstream istringstream
Definition: Arg.h:43
static rs_usb_device create_usb_device(const usb_device_info &info)
buffer(int fd, v4l2_buf_type type, bool use_memory_map, uint32_t index)
GLuint const GLchar * name
virtual void negotiate_kernel_buffers(size_t num) const override
std::vector< usb_device_info > query_usb_devices() const override
GLuint GLfloat * val
const uint8_t RS_HARDWARE_PRESET
uint32_t xu_to_cid(const extension_unit &xu, uint8_t control) const
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
virtual void set_format(stream_profile profile) override
GLdouble s
static std::vector< usb_device_info > query_devices_info()
metadata_hid_raw - HID metadata structure layout populated by backend
Definition: src/metadata.h:872
uint32_t get_full_length() const
Definition: backend-v4l2.h:135
constexpr uint32_t RS_CAMERA_CID_LASER_POWER_LEVEL
std::vector< std::shared_ptr< buffer > > _md_buffers
Definition: backend-v4l2.h:454
#define V4L2_META_FMT_D4XX
Definition: backend-v4l2.h:61
bool get_pu(rs2_option opt, int32_t &value) const override
constexpr uint32_t RS_CAMERA_DEPTH_CALIBRATION_TABLE_SET
bool val_in_range(const T &val, const std::initializer_list< T > &list)
Definition: src/types.h:175
uint32_t get_length_frame_only() const
Definition: backend-v4l2.h:136
virtual void set_metadata_attributes(buffers_mgr &buf_mgr, __u32 bytesused, uint8_t *md_start)
GLfloat value
#define LOG_WARNING(...)
Definition: easyloggingpp.h:57
constexpr uint32_t RS_CAMERA_CID_MANUAL_EXPOSURE
bool set_xu(const extension_unit &xu, uint8_t control, const uint8_t *data, int size) override
std::shared_ptr< hid_device > create_hid_device(hid_device_info info) const override
#define V4L2_CAP_META_CAPTURE
Definition: backend-v4l2.h:55
#define V4L2_META_FMT_UVC
Definition: backend-v4l2.h:52
void set_power_state(power_state state) override
const uint8_t MAX_META_DATA_SIZE
Definition: backend.h:34
constexpr uint32_t RS_CAMERA_CID_LASER_POWER
constexpr uint32_t RS_CAMERA_DEPTH_CALIBRATION_TABLE_GET
static void foreach_uvc_device(std::function< void(const uvc_device_info &, const std::string &)> action)
std::array< kernel_buf_guard, e_max_kernel_buf_type > & get_buffers()
Definition: backend-v4l2.h:223
static void foreach_hid_device(std::function< void(const hid_device_info &)> action)
void set_metadata_attributes(buffers_mgr &buf_mgr, __u32 bytesused, uint8_t *md_start) override
control_range get_xu_range(const extension_unit &xu, uint8_t control, int len) const override
unsigned short uint16_t
Definition: stdint.h:79
static const std::map< usb_spec, std::string > usb_spec_names
Definition: usb-types.h:124
void close(stream_profile) override
d
Definition: rmse.py:171
constexpr uint32_t RS_CAMERA_CID_AE_SETPOINT_GET
static uvc_device_info get_info_from_mipi_device_path(const std::string &video_path, const std::string &name)
constexpr uint32_t RS_CAMERA_CID_AE_ROI_GET
Definition: arg_fwd.hpp:23
unsigned char uint8_t
Definition: stdint.h:78
#define LOG_DEBUG(...)
Definition: easyloggingpp.h:55
e
Definition: rmse.py:177
std::shared_ptr< uvc_device > create_uvc_device(uvc_device_info info) const override
virtual bool has_metadata() const override
bool tm_boot(const std::vector< usb_device_info > &devices)
Definition: tm-boot.h:48
void sort(sort_type m_sort_type, const std::string &in, const std::string &out)
std::vector< hid_device_info > query_hid_devices() const override
void stream_on(std::function< void(const notification &n)> error_handler) override
std::vector< stream_profile > get_profiles() const override
std::shared_ptr< backend > create_backend()
GLenum GLuint id
constexpr uint32_t RS_CAMERA_CID_WHITE_BALANCE_MODE
void set_md_attributes(uint8_t md_size, void *md_start)
Definition: backend-v4l2.h:217
GLuint index
void request_next_frame(int fd, bool force=false)
const size_t MAX_DEV_PARENT_DIR
void upload_video_and_metadata_from_syncer(buffers_mgr &buf_mgr)
GLenum GLuint GLenum GLsizei const GLchar * buf
control_range get_pu_range(rs2_option option) const override
Definition: parser.hpp:157
def info(name, value, persistent=False)
Definition: test.py:327
constexpr uint32_t RS_CAMERA_CID_ERB
static bool is_usb_path_valid(const std::string &usb_video_path, const std::string &dev_name, std::string &busnum, std::string &devnum, std::string &devpath)
static uvc_device_info get_info_from_usb_device_path(const std::string &video_path, const std::string &name)
constexpr uint8_t metadata_imu_report_size
Definition: src/metadata.h:848
constexpr uint32_t RS_CAMERA_CID_FW_VERSION
static void get_mipi_device_info(const std::string &dev_name, std::string &bus_info, std::string &card)
GLsizeiptr size
virtual void acquire_metadata(buffers_mgr &buf_mgr, fd_set &fds, bool compressed_format=false)
const uint8_t RS_ERROR_REPORTING
std::string fourcc_to_string(uint32_t id) const
GLdouble GLdouble r
static std::map< std::string, int > _dev_mutex_cnt
Definition: backend-v4l2.h:114
double p[GRIDW][GRIDH]
Definition: wave.c:109
bool get_pu(rs2_option opt, int32_t &value) const override
constexpr uint32_t RS_CAMERA_CID_PRESET
std::shared_ptr< command_transfer > create_usb_device(usb_device_info info) const override
void populate_imu_data(metadata_hid_raw &meta_data, uint8_t *frame_start, uint8_t &md_size, void **md_start) const
#define assert(condition)
Definition: lz4.c:245
virtual void unmap_device_descriptor() override
void req_io_buff(int fd, uint32_t count, std::string dev_name, v4l2_memory mem_type, v4l2_buf_type type)
virtual uint32_t get_cid(rs2_option option) const
void handle_buffer(supported_kernel_buf_types buf_type, int file_desc, v4l2_buffer buf=v4l2_buffer(), std::shared_ptr< platform::buffer > data_buf=nullptr)
unsigned int uint32_t
Definition: stdint.h:80
static uint32_t get_dev_capabilities(const std::string dev_name)
virtual void map_device_descriptor() override
control_range get_xu_range(const extension_unit &xu, uint8_t control, int len) const override
std::shared_ptr< time_service > create_time_service() const override
constexpr auto LOCAL_V4L2_BUF_TYPE_META_CAPTURE
Definition: backend-v4l2.h:72
void stream_off(int fd, v4l2_buf_type type=V4L2_BUF_TYPE_VIDEO_CAPTURE)
def find(dir, mask)
Definition: file.py:45
def callback(frame)
Definition: t265_stereo.py:91
const uint8_t RS_ASIC_AND_PROJECTOR_TEMPERATURES
unsigned __int64 uint64_t
Definition: stdint.h:90
#define LOG_DEBUG_V4L(...)
Definition: backend-v4l2.h:68
bool set_pu(rs2_option opt, int32_t value) override
static int xioctl(int fh, unsigned long request, void *arg)
constexpr uint32_t RS_CAMERA_CID_HWMC_LEGACY
const uint8_t RS_ENABLE_AUTO_EXPOSURE
GLint first
void stream_ctl_on(int fd, v4l2_buf_type type=V4L2_BUF_TYPE_VIDEO_CAPTURE)
bool set_xu(const extension_unit &xu, uint8_t control, const uint8_t *data, int size) override
constexpr uint32_t RS_STREAM_CONFIG_0
void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override
constexpr uint32_t RS_CAMERA_CID_EWB
bool set_pu(rs2_option opt, int32_t value) override
void enqueue_front_buffer_before_throwing_it(std::queue< sync_buffer > &sync_queue)
const GLuint * buffers
constexpr uint32_t RS_CAMERA_CID_AE_ROI_SET
bool get_xu(const extension_unit &xu, uint8_t control, uint8_t *data, int size) const override
constexpr uint32_t RS_CAMERA_CID_MANUAL_LASER_POWER
virtual bool is_metadata_streamed() const
Definition: backend-v4l2.h:398
std::function< void(const notification &n)> _error_handler
Definition: backend.h:386
std::function< void(stream_profile, frame_object, std::function< void()>)> frame_callback
Definition: backend.h:184
double monotonic_to_realtime(double monotonic)
Definition: backend.cpp:26
v4l_mipi_device(const uvc_device_info &info, bool use_memory_map=true)
bool pull_video_with_metadata(std::shared_ptr< v4l2_buffer > &video_buffer, std::shared_ptr< v4l2_buffer > &md_buffer, int &video_fd, int &md_fd)
LOG_INFO("Log message using LOG_INFO()")
static std::recursive_mutex _init_mutex
Definition: backend-v4l2.h:112
void attach_buffer(const v4l2_buffer &buf)
named_mutex(const std::string &device_path, unsigned timeout)
constexpr uint32_t RS_CAMERA_COEFF_CALIBRATION_TABLE_SET
unsigned char byte
Definition: src/types.h:40
void unsubscribe_from_ctrl_event(uint32_t control_id)
GLdouble GLdouble GLdouble q
const uint8_t RS_ENABLE_AUTO_WHITE_BALANCE
constexpr uint32_t RS_CAMERA_CID_EMITTER_FREQUENCY
GLbitfield GLuint64 timeout
static auto it
GLenum type
std::unique_ptr< named_mutex > _named_mtx
Definition: backend-v4l2.h:415
virtual void allocate_io_buffers(size_t num) override
int min(int a, int b)
Definition: lz4s.c:73
signed __int64 int64_t
Definition: stdint.h:89
void set_md_from_video_node(bool compressed)
void subscribe_to_ctrl_event(uint32_t control_id)
control_range get_pu_range(rs2_option option) const override
GLint GLsizei count
static std::map< std::string, std::recursive_mutex > _dev_mutex
Definition: backend-v4l2.h:113
const uint8_t RS_HWMONITOR
const uint8_t RS_LASER_POWER
virtual std::shared_ptr< buffer > get_md_buffer(__u32 index) const
Definition: backend-v4l2.h:400
bool update_and_check_kpi(const stream_profile &profile, const timeval &timestamp)
const double DEFAULT_KPI_FRAME_DROPS_PERCENTAGE
constexpr uint32_t RS_CAMERA_COEFF_CALIBRATION_TABLE_GET
void push_video(const sync_buffer &video_buffer)
std::string time_in_HH_MM_SS_MMM()
virtual void prepare_capture_buffers() override
int stoi(const std::string &value)
platform::hid_header header
Definition: src/metadata.h:874
GLsizei range
int i
v4l_uvc_meta_device(const uvc_device_info &info, bool use_memory_map=false)
virtual void streamoff() const override
#define offsetof(STRUCTURE, FIELD)
Definition: sqlite3.c:11372
GLenum GLuint GLenum GLsizei length
constexpr uint32_t RS_CAMERA_CID_AE_SETPOINT_SET
static std::vector< std::string > get_video_paths()
signed int int32_t
Definition: stdint.h:77
constexpr uint32_t RS_CAMERA_CID_HWMC
const uint8_t RS_DEPTH_EMITTER_ENABLED
void push_metadata(const sync_buffer &md_buffer)
constexpr uint32_t RS_CAMERA_CID_BASE
v4l_uvc_device(const uvc_device_info &info, bool use_memory_map=false)
constexpr uint32_t RS_CAMERA_CID_GVD
_timeout
Definition: libci.py:89
virtual void streamon() const override
std::vector< std::shared_ptr< buffer > > _buffers
Definition: backend-v4l2.h:408
GLboolean * data
constexpr uint8_t hid_header_size
Definition: backend.h:172
constexpr uint32_t RS_CAMERA_CID_EXPOSURE_MODE
uint8_t * get_frame_start() const
Definition: backend-v4l2.h:138
std::vector< uvc_device_info > query_uvc_devices() const override
std::shared_ptr< device_watcher > create_device_watcher() const override
virtual std::shared_ptr< buffer > get_video_buffer(__u32 index) const
Definition: backend-v4l2.h:399
Definition: parser.hpp:153
void copy(void *dst, void const *src, size_t size)
Definition: types.cpp:255
std::unique_ptr< std::thread > _thread
Definition: backend-v4l2.h:414
static usb_spec get_usb_connection_type(std::string path)
virtual void stop_data_capture() override
std::string to_string(T value)


librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Thu Dec 22 2022 03:41:42