backend-v4l2.h
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 #pragma once
5 
6 #include "backend.h"
7 #include "types.h"
8 
9 #include <cassert>
10 #include <cstdlib>
11 #include <cstdio>
12 #include <cstring>
13 
14 #include <algorithm>
15 #include <array>
16 #include <functional>
17 #include <string>
18 #include <sstream>
19 #include <fstream>
20 #include <regex>
21 #include <thread>
22 #include <utility> // for pair
23 #include <chrono>
24 #include <thread>
25 #include <atomic>
26 
27 #include <dirent.h>
28 #include <fcntl.h>
29 #include <unistd.h>
30 #include <limits.h>
31 #include <cmath>
32 #include <errno.h>
33 #include <sys/stat.h>
34 #include <sys/mman.h>
35 #include <sys/ioctl.h>
36 #include <linux/usb/video.h>
37 #include <linux/uvcvideo.h>
38 #include <linux/videodev2.h>
39 #include <regex>
40 #include <list>
41 
42 // Metadata streaming nodes are available with kernels 4.16+
43 #ifdef V4L2_META_FMT_UVC
44 constexpr bool metadata_node = true;
45 #else
46 #pragma message ( "\nLibrealsense notification: V4L2_META_FMT_UVC was not defined, adding metadata constructs")
47 
48 constexpr bool metadata_node = false;
49 
50 // Providing missing parts from videodev2.h
51 // V4L2_META_FMT_UVC >> V4L2_CAP_META_CAPTURE is also defined, but the opposite does not hold
52 #define V4L2_META_FMT_UVC v4l2_fourcc('U', 'V', 'C', 'H') /* UVC Payload Header */
53 
54 #ifndef V4L2_CAP_META_CAPTURE
55 #define V4L2_CAP_META_CAPTURE 0x00800000 /* Specified in kernel header v4.16 */
56 #endif // V4L2_CAP_META_CAPTURE
57 
58 #endif // V4L2_META_FMT_UVC
59 
60 #ifndef V4L2_META_FMT_D4XX
61 #define V4L2_META_FMT_D4XX v4l2_fourcc('D', '4', 'X', 'X') /* D400 Payload Header metadata */
62 #endif
63 
64 //#define DEBUG_V4L
65 #ifdef DEBUG_V4L
66 #define LOG_DEBUG_V4L(...) do { CLOG(DEBUG ,"librealsense") << __VA_ARGS__; } while(false)
67 #else
68 #define LOG_DEBUG_V4L(...)
69 #endif //DEBUG_V4L
70 
71 // Use local definition of buf type to resolve for kernel versions
72 constexpr auto LOCAL_V4L2_BUF_TYPE_META_CAPTURE = (v4l2_buf_type)(13);
73 
74 #pragma pack(push, 1)
75 // The struct definition is identical to uvc_meta_buf defined uvcvideo.h/ kernel 4.16 headers, and is provided to allow for cross-kernel compilation
77  __u64 ns; // system timestamp of the payload in nanoseconds
78  __u16 sof; // USB Frame Number
79  __u8 length; // length of the payload metadata header
80  __u8 flags; // payload header flags
81  __u8* buf; //device-specific metadata payload data
82 };
83 #pragma pack(pop)
84 
85 
86 namespace librealsense
87 {
88  namespace platform
89  {
91  {
92  public:
93  named_mutex(const std::string& device_path, unsigned timeout);
94 
95  named_mutex(const named_mutex&) = delete;
96 
97  ~named_mutex();
98 
99  void lock();
100 
101  void unlock();
102 
103  bool try_lock();
104 
105  private:
106  void acquire();
107  void release();
108 
111  int _fildes;
112  static std::recursive_mutex _init_mutex;
113  static std::map<std::string, std::recursive_mutex> _dev_mutex;
114  static std::map<std::string, int> _dev_mutex_cnt;
116  std::mutex _mutex;
117  };
118  static int xioctl(int fh, unsigned long request, void *arg);
119 
120  class buffer
121  {
122  public:
123  buffer(int fd, v4l2_buf_type type, bool use_memory_map, uint32_t index);
124 
125  void prepare_for_streaming(int fd);
126 
127  ~buffer();
128 
129  void attach_buffer(const v4l2_buffer& buf);
130 
131  void detach_buffer();
132 
133  void request_next_frame(int fd, bool force=false);
134 
135  uint32_t get_full_length() const { return _length; }
136  uint32_t get_length_frame_only() const { return _original_length; }
137 
138  uint8_t* get_frame_start() const { return _start; }
139 
140  bool use_memory_map() const { return _use_memory_map; }
141 
142  private:
143  v4l2_buf_type _type;
149  v4l2_buffer _buf;
150  std::mutex _mutex;
151  bool _must_enqueue = false;
152  };
153 
155  {
159  };
160 
161 
162  // RAII handling of kernel buffers interchanges
164  {
165  public:
166  buffers_mgr(bool memory_mapped_buf) :
167  _md_start(nullptr),
168  _md_size(0),
169  _mmap_bufs(memory_mapped_buf)
170  {}
171 
173 
174  void request_next_frame();
175  void handle_buffer(supported_kernel_buf_types buf_type, int file_desc,
176  v4l2_buffer buf= v4l2_buffer(),
177  std::shared_ptr<platform::buffer> data_buf=nullptr);
178 
179  uint8_t metadata_size() const { return _md_size; }
180  void* metadata_start() const { return _md_start; }
181 
182  void set_md_attributes(uint8_t md_size, void* md_start)
183  { _md_start = md_start; _md_size = md_size; }
184  void set_md_from_video_node(bool compressed);
185  bool verify_vd_md_sync() const;
186  bool md_node_present() const;
187 
188  //Debug Evgeni
189  // RAII for buffer exchange with kernel
191  {
193  {
194  if (_data_buf && (!_managed))
195  {
196  if (_file_desc > 0)
197  {
198  if (xioctl(_file_desc, (int)VIDIOC_QBUF, &_dq_buf) < 0)
199  {
200  LOG_DEBUG_V4L("xioctl(VIDIOC_QBUF) guard failed for fd " << std::dec << _file_desc);
201  if (xioctl(_file_desc, (int)VIDIOC_DQBUF, &_dq_buf) >= 0)
202  {
203  LOG_DEBUG_V4L("xioctl(VIDIOC_QBUF) Re-enqueue succeeded for fd " << std::dec << _file_desc);
204  if (xioctl(_file_desc, (int)VIDIOC_QBUF, &_dq_buf) < 0)
205  LOG_DEBUG_V4L("xioctl(VIDIOC_QBUF) re-deque failed for fd " << std::dec << _file_desc);
206  else
207  LOG_DEBUG_V4L("xioctl(VIDIOC_QBUF) re-deque succeeded for fd " << std::dec << _file_desc);
208  }
209  else
210  LOG_DEBUG_V4L("xioctl(VIDIOC_QBUF) Re-enqueue failed for fd " << std::dec << _file_desc);
211  }
212  else
213  LOG_DEBUG_V4L("Enqueue (e) buf " << std::dec << _dq_buf.index << " for fd " << _file_desc);
214  }
215  }
216  }
217 
218  std::shared_ptr<platform::buffer> _data_buf=nullptr;
219  v4l2_buffer _dq_buf{};
220  int _file_desc=-1;
221  bool _managed=false;
222  };
223 
224  std::array<kernel_buf_guard, e_max_kernel_buf_type>& get_buffers()
225  { return buffers; }
226 
227  private:
228  void* _md_start; // marks the address of metadata blob
229  uint8_t _md_size; // metadata size is bounded by 255 bytes by design
231 
232 
233  std::array<kernel_buf_guard, e_max_kernel_buf_type> buffers;
234  };
235 
237  {
238  virtual void capture_loop() = 0;
239 
240  virtual bool has_metadata() const = 0;
241 
242  virtual void streamon() const = 0;
243  virtual void streamoff() const = 0;
244  virtual void negotiate_kernel_buffers(size_t num) const = 0;
245 
246  virtual void allocate_io_buffers(size_t num) = 0;
247  virtual void map_device_descriptor() = 0;
248  virtual void unmap_device_descriptor() = 0;
249  virtual void set_format(stream_profile profile) = 0;
250  virtual void prepare_capture_buffers() = 0;
251  virtual void stop_data_capture() = 0;
252  virtual void acquire_metadata(buffers_mgr & buf_mgr,fd_set &fds, bool compressed_format) = 0;
253  };
254 
256  {
257  public:
258  static void foreach_uvc_device(
259  std::function<void(const uvc_device_info&,
260  const std::string&)> action);
261 
262  v4l_uvc_device(const uvc_device_info& info, bool use_memory_map = false);
263 
264  ~v4l_uvc_device() override;
265 
266  void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override;
267 
268  void stream_on(std::function<void(const notification& n)> error_handler) override;
269 
270  void start_callbacks() override;
271 
272  void stop_callbacks() override;
273 
274  void close(stream_profile) override;
275 
276  std::string fourcc_to_string(uint32_t id) const;
277 
278  void signal_stop();
279 
280  void poll();
281 
282  void set_power_state(power_state state) override;
283  power_state get_power_state() const override { return _state; }
284 
285  void init_xu(const extension_unit&) override {}
286  bool set_xu(const extension_unit& xu, uint8_t control, const uint8_t* data, int size) override;
287  bool get_xu(const extension_unit& xu, uint8_t control, uint8_t* data, int size) const override;
288  control_range get_xu_range(const extension_unit& xu, uint8_t control, int len) const override;
289 
290  bool get_pu(rs2_option opt, int32_t& value) const override;
291 
292  bool set_pu(rs2_option opt, int32_t value) override;
293 
294  control_range get_pu_range(rs2_option option) const override;
295 
296  std::vector<stream_profile> get_profiles() const override;
297 
298  void lock() const override;
299  void unlock() const override;
300 
301  std::string get_device_location() const override { return _device_path; }
302  usb_spec get_usb_specification() const override { return _device_usb_spec; }
303 
304  protected:
305  static uint32_t get_cid(rs2_option option);
306 
307  virtual void capture_loop() override;
308 
309  virtual bool has_metadata() const override;
310 
311  virtual void streamon() const override;
312  virtual void streamoff() const override;
313  virtual void negotiate_kernel_buffers(size_t num) const override;
314 
315  virtual void allocate_io_buffers(size_t num) override;
316  virtual void map_device_descriptor() override;
317  virtual void unmap_device_descriptor() override;
318  virtual void set_format(stream_profile profile) override;
319  virtual void prepare_capture_buffers() override;
320  virtual void stop_data_capture() override;
321  virtual void acquire_metadata(buffers_mgr & buf_mgr,fd_set &fds, bool compressed_format = false) override;
322 
323  power_state _state = D3;
325  std::string _device_path = "";
326  usb_spec _device_usb_spec = usb_undefined;
328 
329  std::vector<std::shared_ptr<buffer>> _buffers;
332  std::atomic<bool> _is_capturing;
333  std::atomic<bool> _is_alive;
334  std::atomic<bool> _is_started;
335  std::unique_ptr<std::thread> _thread;
336  std::unique_ptr<named_mutex> _named_mtx;
338  int _max_fd = 0; // specifies the maximal pipe number the polling process will monitor
339  std::vector<int> _fds; // list the file descriptors to be monitored during frames polling
340  buffers_mgr _buf_dispatch; // Holder for partial (MD only) frames that shall be preserved between 'select' calls when polling v4l buffers
341 
342  private:
343  int _fd = 0; // prevent unintentional abuse in derived class
344  int _stop_pipe_fd[2]; // write to _stop_pipe_fd[1] and read from _stop_pipe_fd[0]
345 
346  };
347 
348  // Composition layer for uvc/metadata split nodes introduced with kernel 4.16
350  {
351  public:
352  v4l_uvc_meta_device(const uvc_device_info& info, bool use_memory_map = false);
353 
355 
356  protected:
357 
358  void streamon() const;
359  void streamoff() const;
360  void negotiate_kernel_buffers(size_t num) const;
361  void allocate_io_buffers(size_t num);
362  void map_device_descriptor();
363  void unmap_device_descriptor();
364  void set_format(stream_profile profile);
365  void prepare_capture_buffers();
366  virtual void acquire_metadata(buffers_mgr & buf_mgr,fd_set &fds, bool compressed_format=false);
367 
368  int _md_fd = -1;
369  std::string _md_name = "";
370 
371  std::vector<std::shared_ptr<buffer>> _md_buffers;
373  };
374 
375  class v4l_backend : public backend
376  {
377  public:
378  std::shared_ptr<uvc_device> create_uvc_device(uvc_device_info info) const override;
379  std::vector<uvc_device_info> query_uvc_devices() const override;
380 
381  std::shared_ptr<command_transfer> create_usb_device(usb_device_info info) const override;
382  std::vector<usb_device_info> query_usb_devices() const override;
383 
384  std::shared_ptr<hid_device> create_hid_device(hid_device_info info) const override;
385  std::vector<hid_device_info> query_hid_devices() const override;
386 
387  std::shared_ptr<time_service> create_time_service() const override;
388  std::shared_ptr<device_watcher> create_device_watcher() const override;
389  };
390  }
391 }
static const textual_icon lock
Definition: model-views.h:218
static const textual_icon unlock
Definition: model-views.h:237
rs2_option
Defines general configuration controls. These can generally be mapped to camera UVC controls...
Definition: rs_option.h:22
std::vector< std::shared_ptr< buffer > > _md_buffers
Definition: backend-v4l2.h:371
void init_xu(const extension_unit &) override
Definition: backend-v4l2.h:285
GLfloat value
std::array< kernel_buf_guard, e_max_kernel_buf_type > & get_buffers()
Definition: backend-v4l2.h:224
constexpr bool metadata_node
Definition: backend-v4l2.h:48
buffers_mgr(bool memory_mapped_buf)
Definition: backend-v4l2.h:166
GLsizei const GLchar *const * string
Definition: arg_fwd.hpp:23
GLdouble n
Definition: glext.h:1966
unsigned char uint8_t
Definition: stdint.h:78
GLenum GLfloat * buffer
power_state get_power_state() const override
Definition: backend-v4l2.h:283
uint8_t * get_frame_start() const
Definition: backend-v4l2.h:138
GLuint num
Definition: glext.h:5648
void set_md_attributes(uint8_t md_size, void *md_start)
Definition: backend-v4l2.h:182
GLuint index
uint32_t get_full_length() const
Definition: backend-v4l2.h:135
GLenum GLuint GLenum GLsizei const GLchar * buf
GLenum GLsizei len
Definition: glext.h:3285
def info(name, value, persistent=False)
Definition: test.py:301
GLsizeiptr size
static std::map< std::string, int > _dev_mutex_cnt
Definition: backend-v4l2.h:114
unsigned int uint32_t
Definition: stdint.h:80
constexpr auto LOCAL_V4L2_BUF_TYPE_META_CAPTURE
Definition: backend-v4l2.h:72
def callback(frame)
Definition: t265_stereo.py:91
#define LOG_DEBUG_V4L(...)
Definition: backend-v4l2.h:68
static int xioctl(int fh, unsigned long request, void *arg)
const GLuint * buffers
action
Definition: enums.py:62
uint32_t get_length_frame_only() const
Definition: backend-v4l2.h:136
std::function< void(stream_profile, frame_object, std::function< void()>)> frame_callback
Definition: backend.h:177
static std::recursive_mutex _init_mutex
Definition: backend-v4l2.h:112
GLbitfield GLuint64 timeout
GLenum type
std::unique_ptr< named_mutex > _named_mtx
Definition: backend-v4l2.h:336
static std::map< std::string, std::recursive_mutex > _dev_mutex
Definition: backend-v4l2.h:113
std::string get_device_location() const override
Definition: backend-v4l2.h:301
usb_spec get_usb_specification() const override
Definition: backend-v4l2.h:302
signed int int32_t
Definition: stdint.h:77
std::array< kernel_buf_guard, e_max_kernel_buf_type > buffers
Definition: backend-v4l2.h:233
GLuint64 GLenum GLint fd
Definition: glext.h:7768
std::vector< std::shared_ptr< buffer > > _buffers
Definition: backend-v4l2.h:329
Definition: parser.hpp:150
std::unique_ptr< std::thread > _thread
Definition: backend-v4l2.h:335


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:45:06