backend-hid.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-hid.h"
6 #include "backend.h"
7 #include "types.h"
8 
9 #include <thread>
10 #include <chrono>
11 #include <ctime>
12 #include <dirent.h>
13 #include <fcntl.h>
14 #include <unistd.h>
15 
16 #pragma GCC diagnostic ignored "-Woverflow"
17 
18 const size_t MAX_DEV_PARENT_DIR = 10;
19 
20 const uint8_t HID_METADATA_SIZE = 8; // bytes
21 const size_t HID_DATA_ACTUAL_SIZE = 6; // bytes
22 
23 const std::string IIO_DEVICE_PREFIX("iio:device");
24 const std::string IIO_ROOT_PATH("/sys/bus/iio/devices");
25 const std::string HID_CUSTOM_PATH("/sys/bus/platform/drivers/hid_sensor_custom");
26 
27 //#define DEBUG_HID
28 #ifdef DEBUG_HID
29 #define LOG_DEBUG_HID(...) do { CLOG(DEBUG ,"librealsense") << __VA_ARGS__; } while(false)
30 #else
31 #define LOG_DEBUG_HID(...)
32 #endif //DEBUG_HID
33 
34 namespace librealsense
35 {
36  namespace platform
37  {
38  hid_input::hid_input(const std::string& iio_device_path, const std::string& input_name)
39  {
40  info.device_path = iio_device_path;
41  static const std::string input_prefix = "in_";
42  // validate if input includes th "in_" prefix. if it is . remove it.
43  if (input_name.substr(0,input_prefix.size()) == input_prefix)
44  {
45  info.input = input_name.substr(input_prefix.size(), input_name.size());
46  }
47  else
48  {
49  info.input = input_name;
50  }
51 
52  init();
53  }
54 
56  {
57  enable(false);
58  }
59 
60  // enable scan input. doing so cause the input to be part of the data provided in the polling.
61  void hid_input::enable(bool is_enable)
62  {
63  auto input_data = is_enable ? 1 : 0;
64  // open the element requested and enable and disable.
65  auto element_path = info.device_path + "/scan_elements/" + "in_" + info.input + "_en";
66  std::ofstream iio_device_file(element_path);
67 
68  if (!iio_device_file.is_open())
69  {
70  throw linux_backend_exception(to_string() << "Failed to open scan_element " << element_path);
71  }
72  iio_device_file << input_data;
73  iio_device_file.close();
74 
75  info.enabled = is_enable;
76  }
77 
78  // initialize the input by reading the input parameters.
80  {
81  char buffer[1024] = {};
82 
83  static const std::string input_suffix = "_en";
84  // check if input contains the "en" suffix, if it is . remove it.
85  if (info.input.substr(info.input.size()-input_suffix.size(), input_suffix.size()) == input_suffix) {
86  info.input = info.input.substr(0, info.input.size()-input_suffix.size());
87  }
88 
89  // read scan type.
90  auto read_scan_type_path = std::string(info.device_path + "/scan_elements/in_" + info.input + "_type");
91  std::ifstream device_type_file(read_scan_type_path);
92  if (!device_type_file)
93  {
94  throw linux_backend_exception(to_string() << "Failed to open read_scan_type " << read_scan_type_path);
95  }
96 
97  device_type_file.getline(buffer, sizeof(buffer));
98  uint32_t pad_int;
99  char sign_char, endia_nchar;
100  // TODO: parse with regex
101  auto ret = std::sscanf(buffer,
102  "%ce:%c%u/%u>>%u",
103  &endia_nchar,
104  &sign_char,
105  &info.bits_used,
106  &pad_int,
107  &info.shift);
108 
109  if (ret < 0)
110  {
111  throw linux_backend_exception(to_string() << "Failed to parse device_type " << read_scan_type_path);
112  }
113 
114  device_type_file.close();
115 
116  info.big_endian = (endia_nchar == 'b');
117  info.bytes = pad_int / 8;
118  info.is_signed = (sign_char == 's');
119 
120  if (info.bits_used == 64)
121  info.mask = ~0;
122  else
123  info.mask = (1ULL << info.bits_used) - 1;
124 
125 
126  // read scan index.
127  auto read_scan_index_path = info.device_path + "/scan_elements/in_" + info.input + "_index";
128  std::ifstream device_index_file(read_scan_index_path);
129  if (!device_index_file)
130  {
131  throw linux_backend_exception(to_string() << "Failed to open scan_index " << read_scan_index_path);
132  }
133 
134  device_index_file.getline(buffer, sizeof(buffer));
135  info.index = std::stoi(buffer);
136 
137  device_index_file.close();
138 
139  // read enable state.
140  auto read_enable_state_path = info.device_path + "/scan_elements/in_" + info.input + "_en";
141  std::ifstream device_enabled_file(read_enable_state_path);
142  if (!device_enabled_file)
143  {
144  throw linux_backend_exception(to_string() << "Failed to open scan_index " << read_enable_state_path);
145  }
146 
147  device_enabled_file.getline(buffer, sizeof(buffer));
148  info.enabled = (std::stoi(buffer) == 0) ? false : true;
149 
150  device_enabled_file.close();
151  }
152 
153  hid_custom_sensor::hid_custom_sensor(const std::string& device_path, const std::string& sensor_name)
154  : _fd(0),
155  _stop_pipe_fd{},
156  _custom_device_path(device_path),
157  _custom_sensor_name(sensor_name),
159  _callback(nullptr),
160  _is_capturing(false),
161  _hid_thread(nullptr)
162  {
163  init();
164  }
165 
167  {
168  try
169  {
170  if (_is_capturing)
171  stop_capture();
172  }
173  catch(...)
174  {
175  LOG_ERROR("An error has occurred while hid_custom_sensor dtor()!");
176  }
177  }
178 
179  std::vector<uint8_t> hid_custom_sensor::get_report_data(const std::string& report_name, custom_sensor_report_field report_field)
180  {
181  static const std::map<custom_sensor_report_field, std::string> report_fields = {{minimum, "-minimum"},
182  {maximum, "-maximum"},
183  {name, "-name"},
184  {size, "-size"},
185  {unit_expo, "-unit-expo"},
186  {units, "-units"},
187  {value, "-value"}};
188  try{
189  auto& report_folder = _reports.at(report_name);
190  auto report_path = _custom_device_path + "/" + report_folder + "/" + report_folder + report_fields.at(report_field);
191  return read_report(report_path);
192  }
193  catch(std::out_of_range)
194  {
195  throw invalid_value_exception(to_string() << "report directory name " << report_name << " not found!");
196  }
197  }
198 
199 
200  // start capturing and polling.
202  {
203  if (_is_capturing)
204  return;
205 
206  std::ostringstream device_path;
207  device_path << "/dev/" << _custom_device_name;
208 
209  auto read_device_path_str = device_path.str();
210  std::ifstream device_file(read_device_path_str);
211 
212  // find device in file system.
213  if (!device_file.good())
214  {
215  throw linux_backend_exception("custom hid device is busy or not found!");
216  }
217 
218  device_file.close();
219 
220  enable(true);
221 
222  const auto max_retries = 10;
223  auto retries = 0;
224  while(++retries < max_retries)
225  {
226  if ((_fd = open(read_device_path_str.c_str(), O_RDONLY | O_NONBLOCK)) > 0)
227  break;
228 
229  LOG_WARNING("open() failed!");
230  std::this_thread::sleep_for(std::chrono::milliseconds(5));
231  }
232 
233  if ((retries == max_retries) && (_fd <= 0))
234  {
235  enable(false);
236  throw linux_backend_exception("open() failed with all retries!");
237  }
238 
239  if (pipe(_stop_pipe_fd) < 0)
240  {
241  close(_fd);
242  enable(false);
243  throw linux_backend_exception("hid_custom_sensor: Cannot create pipe!");
244  }
245 
246  _callback = sensor_callback;
247  _is_capturing = true;
248  _hid_thread = std::unique_ptr<std::thread>(new std::thread([this, read_device_path_str](){
249  const uint32_t channel_size = 24; // TODO: why 24?
250  std::vector<uint8_t> raw_data(channel_size * hid_buf_len);
251 
252  do {
253  fd_set fds;
254  FD_ZERO(&fds);
255  FD_SET(_fd, &fds);
256  FD_SET(_stop_pipe_fd[0], &fds);
257 
258  int max_fd = std::max(_stop_pipe_fd[0], _fd);
259  size_t read_size = 0;
260 
261  struct timeval tv = {5,0};
262  LOG_DEBUG_HID("HID Select initiated");
263  auto val = select(max_fd + 1, &fds, nullptr, nullptr, &tv);
264  LOG_DEBUG_HID("HID Select done, val = " << val);
265  if (val < 0)
266  {
267  // TODO: write to log?
268  continue;
269  }
270  else if (val > 0)
271  {
272  if(FD_ISSET(_stop_pipe_fd[0], &fds))
273  {
274  if(!_is_capturing)
275  {
276  LOG_INFO("hid_custom_sensor: Stream finished");
277  return;
278  }
279  }
280  else if (FD_ISSET(_fd, &fds))
281  {
282  read_size = read(_fd, raw_data.data(), raw_data.size());
283  if (read_size <= 0 )
284  continue;
285  }
286  else
287  {
288  // TODO: write to log?
289  LOG_WARNING("HID unresolved event : after select->FD_ISSET");
290  continue;
291  }
292 
293  auto sz= read_size / channel_size;
294  if (sz > 2)
295  {
296  LOG_DEBUG("HID: Going to handle " << sz << " packets");
297  }
298  for (auto i = 0; i < read_size / channel_size; ++i)
299  {
300  auto p_raw_data = raw_data.data() + channel_size * i;
301 
302  // TODO: code refactoring to reduce latency
303  sensor_data sens_data{};
304  sens_data.sensor = hid_sensor{get_sensor_name()};
305 
306  sens_data.fo = {channel_size, channel_size, p_raw_data, p_raw_data};
307  this->_callback(sens_data);
308  }
309  if (sz > 2)
310  {
311  LOG_DEBUG("HID: Finished to handle " << sz << " packets");
312  }
313  }
314  else
315  {
316  LOG_WARNING("hid_custom_sensor: Frames didn't arrived within 5 seconds");
317  }
318  } while(this->_is_capturing);
319  }));
320  }
321 
323  {
324  if (!_is_capturing)
325  {
326  enable(false);
327  return;
328  }
329 
330  _is_capturing = false;
331  signal_stop();
332  _hid_thread->join();
333  enable(false);
334  _callback = nullptr;
335 
336  if(::close(_fd) < 0)
337  throw linux_backend_exception("hid_custom_sensor: close(_fd) failed");
338 
339  if(::close(_stop_pipe_fd[0]) < 0)
340  throw linux_backend_exception("hid_custom_sensor: close(_stop_pipe_fd[0]) failed");
341  if(::close(_stop_pipe_fd[1]) < 0)
342  throw linux_backend_exception("hid_custom_sensor: close(_stop_pipe_fd[1]) failed");
343 
344  _fd = 0;
345  _stop_pipe_fd[0] = _stop_pipe_fd[1] = 0;
346  }
347 
348  std::vector<uint8_t> hid_custom_sensor::read_report(const std::string& name_report_path)
349  {
350  auto fd = open(name_report_path.c_str(), O_RDONLY | O_NONBLOCK);
351  if (fd < 0)
352  throw linux_backend_exception("Failed to open report!");
353 
354  std::vector<uint8_t> buffer;
355  buffer.resize(MAX_INPUT);
356  auto read_size = read(fd, buffer.data(), buffer.size());
357  close(fd);
358 
359  if (read_size <= 0)
360  throw linux_backend_exception("Failed to read custom report!");
361 
362  buffer.resize(read_size);
363  buffer[buffer.size() - 1] = '\0'; // Replace '\n' with '\0'
364  return buffer;
365  }
366 
368  {
369  static const char* prefix_feature_name = "feature";
370  static const char* prefix_input_name = "input";
371  static const char* suffix_name_field = "name";
372  DIR* dir = nullptr;
373  struct dirent* ent = nullptr;
374  if ((dir = opendir(_custom_device_path.c_str())) != nullptr)
375  {
376  while ((ent = readdir(dir)) != nullptr)
377  {
378  auto str = std::string(ent->d_name);
379  if (str.find(prefix_feature_name) != std::string::npos ||
380  str.find(prefix_input_name) != std::string::npos)
381  {
382  DIR* report_dir = nullptr;
383  struct dirent* report_ent = nullptr;
384  auto report_path = _custom_device_path + "/" + ent->d_name;
385  if ((report_dir = opendir(report_path.c_str())) != nullptr)
386  {
387  while ((report_ent = readdir(report_dir)) != nullptr)
388  {
389  auto report_str = std::string(report_ent->d_name);
390  if (report_str.find(suffix_name_field) != std::string::npos)
391  {
392  auto name_report_path = report_path + "/" + report_ent->d_name;
393  auto buffer = read_report(name_report_path);
394 
395  std::string name_report(reinterpret_cast<char const*>(buffer.data()));
396  _reports.insert(std::make_pair(name_report, ent->d_name));
397  }
398  }
399  closedir(report_dir);
400  }
401  }
402  }
403  closedir(dir);
404  }
405 
406  // get device name
407  auto pos = _custom_device_path.find_last_of("/");
408  if (pos < _custom_device_path.size())
410  }
411 
413  {
414  auto input_data = state ? 1 : 0;
415  auto element_path = _custom_device_path + "/enable_sensor";
416  std::ofstream custom_device_file(element_path);
417 
418  if (!custom_device_file.is_open())
419  {
420  throw linux_backend_exception(to_string() << "Failed to enable_sensor " << element_path);
421  }
422  custom_device_file << input_data;
423  custom_device_file.close();
424 
425  // Work-around for the HID custom driver failing to release resources,
426  // preventing the device to enter idle U3 mode
427  // reading out the value will refresh the device tree in kernel
428 // if (!state)
429 // {
430 // std::string feat_val("default");
431 // if(!(std::ifstream(_custom_device_path + "/feature-0-200309/feature-0-200309-value") >> feat_val))
432 // throw linux_backend_exception("Failed to read feat_val");
433 // std::cout << "Feat value is " << feat_val << std::endl;
434 // }
435  }
436 
438  {
439  char buff[1];
440  buff[0] = 0;
441  if (write(_stop_pipe_fd[1], buff, 1) < 0)
442  {
443  throw linux_backend_exception("hid_custom_sensor: Could not signal video capture thread to stop. Error write to pipe.");
444  }
445  }
446 
447  iio_hid_sensor::iio_hid_sensor(const std::string& device_path, uint32_t frequency)
448  : _stop_pipe_fd{},
449  _fd(0),
451  _iio_device_path(device_path),
452  _sensor_name(""),
454  _callback(nullptr),
455  _is_capturing(false),
456  _pm_dispatcher(16) // queue for async power management commands
457  {
458  init(frequency);
459  }
460 
462  {
463  try
464  {
465  try {
466  // Ensure PM sync
468  stop_capture();
469  } catch(...){}
470 
471  clear_buffer();
472  }
473  catch(...){}
474 
475  // clear inputs.
476  _inputs.clear();
477  }
478 
479  // start capturing and polling.
481  {
482  if (_is_capturing)
483  return;
484 
485  set_power(true);
486  std::ostringstream iio_read_device_path;
487  iio_read_device_path << "/dev/" << IIO_DEVICE_PREFIX << _iio_device_number;
488 
489  auto iio_read_device_path_str = iio_read_device_path.str();
490  std::ifstream iio_device_file(iio_read_device_path_str);
491 
492  // find iio_device in file system.
493  if (!iio_device_file.good())
494  {
495  throw linux_backend_exception("iio hid device is busy or not found!");
496  }
497 
498  iio_device_file.close();
499 
500  // count number of enabled count elements and sort by their index.
502 
503  const auto max_retries = 10;
504  auto retries = 0;
505  while(++retries < max_retries)
506  {
507  if ((_fd = open(iio_read_device_path_str.c_str(), O_RDONLY | O_NONBLOCK)) > 0)
508  break;
509 
510  LOG_WARNING("open() failed!");
511  std::this_thread::sleep_for(std::chrono::milliseconds(5));
512  }
513 
514  if ((retries == max_retries) && (_fd <= 0))
515  {
516  _channels.clear();
517  throw linux_backend_exception("open() failed with all retries!");
518  }
519 
520  if (pipe(_stop_pipe_fd) < 0)
521  {
522  close(_fd);
523  _channels.clear();
524  throw linux_backend_exception("iio_hid_sensor: Cannot create pipe!");
525  }
526 
527  _callback = sensor_callback;
528  _is_capturing = true;
529  _hid_thread = std::unique_ptr<std::thread>(new std::thread([this](){
530  const uint32_t channel_size = get_channel_size();
531  size_t raw_data_size = channel_size*hid_buf_len;
532 
533  std::vector<uint8_t> raw_data(raw_data_size);
534  auto metadata = has_metadata();
535 
536  do {
537  fd_set fds;
538  FD_ZERO(&fds);
539  FD_SET(_fd, &fds);
540  FD_SET(_stop_pipe_fd[0], &fds);
541 
542  int max_fd = std::max(_stop_pipe_fd[0], _fd);
543 
544  ssize_t read_size = 0;
545  struct timeval tv = {5, 0};
546  LOG_DEBUG_HID("HID IIO Select initiated");
547  auto val = select(max_fd + 1, &fds, nullptr, nullptr, &tv);
548  LOG_DEBUG_HID("HID IIO Select done, val = " << val);
549 
550  if (val < 0)
551  {
552  // TODO: write to log?
553  LOG_WARNING("iio_hid_sensor: select failed, return val = " << val);
554  continue;
555  }
556  else if (val > 0)
557  {
558  if(FD_ISSET(_stop_pipe_fd[0], &fds))
559  {
560  if(!_is_capturing)
561  {
562  LOG_INFO("iio_hid_sensor: Stream finished");
563  return;
564  }
565  }
566  else if (FD_ISSET(_fd, &fds))
567  {
568  read_size = read(_fd, raw_data.data(), raw_data_size);
569  if (read_size < 0 )
570  continue;
571  }
572  else
573  {
574  // TODO: write to log?
575  LOG_WARNING("HID IIO unresolved event : after select->FD_ISSET");
576  continue;
577  }
578 
579  auto sz= read_size / channel_size;
580  if (sz > 2)
581  {
582  LOG_DEBUG("HID: Going to handle " << sz << " packets");
583  }
584  // TODO: code refactoring to reduce latency
585  for (auto i = 0; i < sz; ++i)
586  {
587  auto now_ts = std::chrono::duration<double, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
588  auto p_raw_data = raw_data.data() + channel_size * i;
589  sensor_data sens_data{};
590  sens_data.sensor = hid_sensor{get_sensor_name()};
591 
592  auto hid_data_size = channel_size - (metadata ? HID_METADATA_SIZE : 0);
593  // Populate HID IMU data - Header
594  metadata_hid_raw meta_data{};
596  meta_data.header.length = hid_header_size + metadata_imu_report_size;
597  meta_data.header.timestamp = *(reinterpret_cast<uint64_t *>(&p_raw_data[16]));
598  // Payload:
599  meta_data.report_type.imu_report.header.md_type_id = md_type::META_DATA_HID_IMU_REPORT_ID;
600  meta_data.report_type.imu_report.header.md_size = metadata_imu_report_size;
601 // meta_data.report_type.imu_report.flags = static_cast<uint8_t>( md_hid_imu_attributes::custom_timestamp_attirbute |
602 // md_hid_imu_attributes::imu_counter_attribute |
603 // md_hid_imu_attributes::usb_counter_attribute);
604 // meta_data.report_type.imu_report.custom_timestamp = meta_data.header.timestamp;
605 // meta_data.report_type.imu_report.imu_counter = p_raw_data[30];
606 // meta_data.report_type.imu_report.usb_counter = p_raw_data[31];
607 
608  sens_data.fo = {hid_data_size, metadata? meta_data.header.length: uint8_t(0),
609  p_raw_data, metadata? &meta_data : nullptr, now_ts};
610  //Linux HID provides timestamps in nanosec. Convert to usec (FW default)
611  if (metadata)
612  {
613  //auto* ts_nsec = reinterpret_cast<uint64_t*>(const_cast<void*>(sens_data.fo.metadata));
614  //*ts_nsec /=1000;
615  meta_data.header.timestamp /=1000;
616  }
617 
618 // for (auto i=0ul; i<channel_size; i++)
619 // std::cout << std::hex << int(p_raw_data[i]) << " ";
620 // std::cout << std::dec << std::endl;
621 
622  this->_callback(sens_data);
623  }
624  if (sz > 2)
625  {
626  LOG_DEBUG("HID: Finished to handle " << sz << " packets");
627  }
628  }
629  else
630  {
631  LOG_WARNING("iio_hid_sensor: Frames didn't arrived within the predefined interval");
632  std::this_thread::sleep_for(std::chrono::milliseconds(2));
633  }
634  } while(this->_is_capturing);
635  }));
636  }
637 
639  {
640  if (!_is_capturing)
641  return;
642 
643  _is_capturing = false;
644  set_power(false);
645  signal_stop();
646  _hid_thread->join();
647  _callback = nullptr;
648  _channels.clear();
649 
650  if(::close(_fd) < 0)
651  throw linux_backend_exception("iio_hid_sensor: close(_fd) failed");
652 
653  if(::close(_stop_pipe_fd[0]) < 0)
654  throw linux_backend_exception("iio_hid_sensor: close(_stop_pipe_fd[0]) failed");
655  if(::close(_stop_pipe_fd[1]) < 0)
656  throw linux_backend_exception("iio_hid_sensor: close(_stop_pipe_fd[1]) failed");
657 
658  _fd = 0;
659  _stop_pipe_fd[0] = _stop_pipe_fd[1] = 0;
660  }
661 
663  {
664  std::ostringstream iio_read_device_path;
665  iio_read_device_path << "/dev/" << IIO_DEVICE_PREFIX << _iio_device_number;
666 
667  std::unique_ptr<int, std::function<void(int*)> > fd(
668  new int (_fd = open(iio_read_device_path.str().c_str(), O_RDONLY | O_NONBLOCK)),
669  [&](int* d){ if (d && (*d)) { _fd = ::close(*d); } delete d; });
670 
671  if (!(*fd > 0))
672  throw linux_backend_exception("open() failed with all retries!");
673 
674  // count enabled elements and sort by their index.
676 
677  const uint32_t channel_size = get_channel_size();
678  auto raw_data_size = channel_size*hid_buf_len;
679 
680  std::vector<uint8_t> raw_data(raw_data_size);
681 
682  auto read_size = read(_fd, raw_data.data(), raw_data_size);
683  while(read_size > 0)
684  read_size = read(_fd, raw_data.data(), raw_data_size);
685 
686  _channels.clear();
687  }
688 
690  {
691  auto sampling_frequency_path = _iio_device_path + "/" + _sampling_frequency_name;
692  std::ofstream iio_device_file(sampling_frequency_path);
693 
694  if (!iio_device_file.is_open())
695  {
696  throw linux_backend_exception(to_string() << "Failed to set frequency " << frequency <<
697  ". device path: " << sampling_frequency_path);
698  }
699  iio_device_file << frequency;
700  iio_device_file.close();
701  }
702 
703  // Asynchronous power management
705  {
706  auto path = _iio_device_path + "/buffer/enable";
707 
708  // Enqueue power management change
710  {
711  //auto st = std::chrono::high_resolution_clock::now();
712 
713  if (!write_fs_attribute(path, on))
714  {
715  LOG_WARNING("HID set_power " << int(on) << " failed for " << path);
716  }
717  },true);
718  }
719 
721  {
722  char buff[1];
723  buff[0] = 0;
724  if (write(_stop_pipe_fd[1], buff, 1) < 0)
725  {
726  throw linux_backend_exception("iio_hid_sensor: Could not signal video capture thread to stop. Error write to pipe.");
727  }
728  }
729 
731  {
732  if(get_output_size() == HID_DATA_ACTUAL_SIZE + HID_METADATA_SIZE)
733  return true;
734  return false;
735  }
736 
738  {
739  return (second->get_hid_input_info().index >= first->get_hid_input_info().index);
740  }
741 
743  {
744  // build enabled channels.
745  for(auto& input : _inputs)
746  {
747  if (input->get_hid_input_info().enabled)
748  {
749  _channels.push_back(input);
750  }
751  }
752 
753  _channels.sort(sort_hids);
754  }
755 
756  // initialize the device sensor. reading its name and all of its inputs.
758  {
759  std::ifstream iio_device_file(_iio_device_path + "/name");
760 
761  // find iio_device in file system.
762  if (!iio_device_file.good())
763  {
764  throw linux_backend_exception(to_string() << "Failed to open device sensor. " << _iio_device_path);
765  }
766 
767  char name_buffer[256] = {};
768  iio_device_file.getline(name_buffer,sizeof(name_buffer));
769  _sensor_name = std::string(name_buffer);
770 
771  iio_device_file.close();
772 
773  // get IIO device number
774  static const std::string suffix_iio_device_path("/" + IIO_DEVICE_PREFIX);
775  auto pos = _iio_device_path.find_last_of(suffix_iio_device_path);
776  if (pos == std::string::npos)
777  throw linux_backend_exception(to_string() << "Wrong iio device path " << _iio_device_path);
778 
779  auto substr = _iio_device_path.substr(pos + 1);
780  if (std::all_of(substr.begin(), substr.end(), ::isdigit))
781  {
782  _iio_device_number = atoi(substr.c_str());
783  }
784  else
785  {
786  throw linux_backend_exception(to_string() << "IIO device number is incorrect! Failed to open device sensor. " << _iio_device_path);
787  }
788 
790 
791  // HID iio kernel driver async initialization may fail to map the kernel objects hierarchy (iio triggers) properly
792  // The patch will rectify this behaviour
793  std::string current_trigger = _sensor_name + "-dev" + _iio_device_path.back();
794  std::string path = _iio_device_path + "/trigger/current_trigger";
795  _pm_thread = std::unique_ptr<std::thread>(new std::thread([path,current_trigger](){
796  bool retry =true;
797  while (retry) {
798  try {
799  if (write_fs_attribute(path, current_trigger))
800  break;
801  else
802  std::this_thread::sleep_for(std::chrono::milliseconds(50));
803  }
804  catch(...){} // Device disconnect
805  retry = false;
806  }
807  }));
808  _pm_thread->detach();
809 
810  // read all available input of the iio_device
812 
813  // get the specific name of sampling_frequency
815 
816  for (auto& input : _inputs)
817  input->enable(true);
818 
819  set_frequency(frequency);
820  write_fs_attribute(_iio_device_path + "/buffer/length", hid_buf_len);
821  }
822 
823  // calculate the storage size of a scan
825  {
826  assert(!_channels.empty());
827  auto bytes = 0;
828 
829  for (auto& elem : _channels)
830  {
831  auto input_info = elem->get_hid_input_info();
832  if (bytes % input_info.bytes == 0)
833  {
834  input_info.location = bytes;
835  }
836  else
837  {
838  input_info.location = bytes - bytes % input_info.bytes
839  + input_info.bytes;
840  }
841 
842  bytes = input_info.location + input_info.bytes;
843  }
844 
845  return bytes;
846  }
847 
848  // calculate the actual size of data
850  {
851  assert(!_channels.empty());
852  auto bits_used = 0.;
853 
854  for (auto& elem : _channels)
855  {
856  auto input_info = elem->get_hid_input_info();
857  bits_used += input_info.bits_used;
858  }
859 
860  return std::ceil(bits_used / CHAR_BIT);
861  }
862 
864  {
865  std::string sampling_frequency_name = "";
866  DIR *dir = nullptr;
867  struct dirent *dir_ent = nullptr;
868 
869  // start enumerate the scan elemnts dir.
870  dir = opendir(_iio_device_path.c_str());
871  if (dir == nullptr)
872  {
873  throw linux_backend_exception(to_string() << "Failed to open scan_element " << _iio_device_path);
874  }
875 
876  // verify file format. should include in_ (input) and _en (enable).
877  while ((dir_ent = readdir(dir)) != nullptr)
878  {
879  if (dir_ent->d_type != DT_DIR)
880  {
881  std::string file(dir_ent->d_name);
882  if (file.find("sampling_frequency") != std::string::npos)
883  {
884  sampling_frequency_name = file;
885  }
886  }
887  }
888  closedir(dir);
889  return sampling_frequency_name;
890  }
891 
892 
893  // read the IIO device inputs.
895  {
896  DIR *dir = nullptr;
897  struct dirent *dir_ent = nullptr;
898 
899  auto scan_elements_path = _iio_device_path + "/scan_elements";
900  // start enumerate the scan elemnts dir.
901  dir = opendir(scan_elements_path.c_str());
902  if (dir == nullptr)
903  {
904  throw linux_backend_exception(to_string() << "Failed to open scan_element " << _iio_device_path);
905  }
906 
907  // verify file format. should include in_ (input) and _en (enable).
908  while ((dir_ent = readdir(dir)) != nullptr)
909  {
910  if (dir_ent->d_type != DT_DIR)
911  {
912  std::string file(dir_ent->d_name);
913  std::string prefix = "in_";
914  std::string suffix = "_en";
915  if (file.substr(0,prefix.size()) == prefix &&
916  file.substr(file.size()-suffix.size(),suffix.size()) == suffix) {
917  // initialize input.
918 
919  try
920  {
921  auto* new_input = new hid_input(_iio_device_path, file);
922  // push to input list.
923  _inputs.push_front(new_input);
924  }
925  catch(...)
926  {
927  // fail to initialize this input. continue to the next one.
928  continue;
929  }
930  }
931  }
932  }
933  closedir(dir);
934  }
935 
937  {
938  bool found = false;
939  v4l_hid_device::foreach_hid_device([&](const hid_device_info& hid_dev_info){
940  if (hid_dev_info.unique_id == info.unique_id)
941  {
942  _hid_device_infos.push_back(hid_dev_info);
943  found = true;
944  }
945  });
946 
947  if (!found)
948  throw linux_backend_exception("hid device is no longer connected!");
949  }
950 
952  {
953  for (auto& elem : _streaming_iio_sensors)
954  {
955  elem->stop_capture();
956  }
957 
958  for (auto& elem : _streaming_custom_sensors)
959  {
960  elem->stop_capture();
961  }
962  }
963 
964  void v4l_hid_device::open(const std::vector<hid_profile>& hid_profiles)
965  {
966  _hid_profiles = hid_profiles;
967  for (auto& device_info : _hid_device_infos)
968  {
969  try
970  {
971  if (device_info.id == custom_id)
972  {
973  auto device = std::unique_ptr<hid_custom_sensor>(new hid_custom_sensor(device_info.device_path,
974  device_info.id));
975  _hid_custom_sensors.push_back(std::move(device));
976  }
977  else
978  {
979  uint32_t frequency = 0;
980  for (auto& profile : hid_profiles)
981  {
982  if (profile.sensor_name == device_info.id)
983  {
984  frequency = profile.frequency;
985  break;
986  }
987  }
988 
989  if (frequency == 0)
990  continue;
991 
992  auto device = std::unique_ptr<iio_hid_sensor>(new iio_hid_sensor(device_info.device_path, frequency));
993  _iio_hid_sensors.push_back(std::move(device));
994  }
995  }
996  catch(...)
997  {
998  for (auto& hid_sensor : _iio_hid_sensors)
999  {
1000  hid_sensor.reset();
1001  }
1002  _iio_hid_sensors.clear();
1003  LOG_ERROR("Hid device is busy!");
1004  throw;
1005  }
1006  }
1007  }
1008 
1010  {
1011  for (auto& hid_iio_sensor : _iio_hid_sensors)
1012  {
1013  hid_iio_sensor.reset();
1014  }
1015  _iio_hid_sensors.clear();
1016 
1017  for (auto& hid_custom_sensor : _hid_custom_sensors)
1018  {
1019  hid_custom_sensor.reset();
1020  }
1021  _hid_custom_sensors.clear();
1022  }
1023 
1024  std::vector<hid_sensor> v4l_hid_device::get_sensors()
1025  {
1026  std::vector<hid_sensor> iio_sensors;
1027 
1028  for (auto& sensor : _hid_profiles)
1029  iio_sensors.push_back({ sensor.sensor_name });
1030 
1031 
1032  for (auto& elem : _hid_custom_sensors)
1033  {
1034  iio_sensors.push_back(hid_sensor{elem->get_sensor_name()});
1035  }
1036  return iio_sensors;
1037  }
1038 
1040  {
1041  for (auto& profile : _hid_profiles)
1042  {
1043  for (auto& sensor : _iio_hid_sensors)
1044  {
1045  if (sensor->get_sensor_name() == profile.sensor_name)
1046  {
1047  _streaming_iio_sensors.push_back(sensor.get());
1048  }
1049  }
1050 
1051  for (auto& sensor : _hid_custom_sensors)
1052  {
1053  if (sensor->get_sensor_name() == profile.sensor_name)
1054  {
1055  _streaming_custom_sensors.push_back(sensor.get());
1056  }
1057  }
1058 
1059  if (_streaming_iio_sensors.empty() && _streaming_custom_sensors.empty())
1060  LOG_ERROR("sensor " + profile.sensor_name + " not found!");
1061  }
1062 
1063  if (!_streaming_iio_sensors.empty())
1064  {
1065  std::vector<iio_hid_sensor*> captured_sensors;
1066  try{
1067  for (auto& elem : _streaming_iio_sensors)
1068  {
1069  elem->start_capture(callback);
1070  captured_sensors.push_back(elem);
1071  }
1072  }
1073  catch(...)
1074  {
1075  for (auto& elem : captured_sensors)
1076  elem->stop_capture();
1077 
1078  _streaming_iio_sensors.clear();
1079  throw;
1080  }
1081  }
1082 
1083  if (!_streaming_custom_sensors.empty())
1084  {
1085  std::vector<hid_custom_sensor*> captured_sensors;
1086  try{
1087  for (auto& elem : _streaming_custom_sensors)
1088  {
1089  elem->start_capture(callback);
1090  captured_sensors.push_back(elem);
1091  }
1092  }
1093  catch(...)
1094  {
1095  for (auto& elem : captured_sensors)
1096  elem->stop_capture();
1097 
1098  _streaming_custom_sensors.clear();
1099  throw;
1100  }
1101  }
1102 
1103  }
1104 
1106  {
1107  for (auto& sensor : _iio_hid_sensors)
1108  {
1109  sensor->stop_capture();
1110  }
1111 
1112  _streaming_iio_sensors.clear();
1113 
1114  for (auto& sensor : _hid_custom_sensors)
1115  {
1116  sensor->stop_capture();
1117  }
1118 
1119  _streaming_custom_sensors.clear();
1120  }
1121 
1122  std::vector<uint8_t> v4l_hid_device::get_custom_report_data(const std::string& custom_sensor_name,
1123  const std::string& report_name,
1124  custom_sensor_report_field report_field)
1125  {
1126  auto it = std::find_if(begin(_hid_custom_sensors), end(_hid_custom_sensors),
1127  [&](const std::unique_ptr<hid_custom_sensor>& hcs)
1128  {
1129  return hcs->get_sensor_name() == custom_sensor_name;
1130  });
1131  if (it != end(_hid_custom_sensors))
1132  {
1133  return (*it)->get_report_data(report_name, report_field);
1134  }
1135  throw linux_backend_exception(to_string() << " custom sensor " << custom_sensor_name << " not found!");
1136  }
1137 
1139  {
1140  // Common HID Sensors
1141  DIR* dir = nullptr;
1142  struct dirent* ent = nullptr;
1143  std::vector<std::string> common_sensors;
1144  if ((dir = opendir(IIO_ROOT_PATH.c_str())) != nullptr)
1145  {
1146  while ((ent = readdir(dir)) != nullptr)
1147  {
1148  auto str = std::string(ent->d_name);
1149  if (str.find(IIO_DEVICE_PREFIX) != std::string::npos)
1150  common_sensors.push_back(IIO_ROOT_PATH + "/" + str);
1151  }
1152  closedir(dir);
1153  }
1154 
1155  for (auto& elem : common_sensors)
1156  {
1157  hid_device_info hid_dev_info{};
1158  if(!get_hid_device_info(elem.c_str(), hid_dev_info))
1159  {
1160 #ifdef RS2_USE_CUDA
1161  /* On the Jetson TX, ina3221x is the power monitor (I2C bus)
1162  This code is checking the IIA device directory, but tries to compare as USB HID device
1163  The ina3221x is not a HID device. Check here to avoid spamming the console.
1164  Patch suggested by JetsonHacks: https://github.com/jetsonhacks/buildLibrealsense2TX */
1165  std::string device_path_str(elem.c_str());
1166  device_path_str+="/";
1167  std::string dev_name;
1168  std::ifstream(device_path_str + "name") >> dev_name;
1169  if (dev_name != std::string("ina3221x")) {
1170  LOG_WARNING("Failed to read busnum/devnum. Device Path: " << elem);
1171  }
1172 #else
1173  LOG_INFO("Failed to read busnum/devnum. Device Path: " << elem);
1174 #endif
1175  continue;
1176  }
1177  action(hid_dev_info);
1178  }
1179 
1180 
1181  // Custom HID Sensors
1182  static const char* prefix_custom_sensor_name = "HID-SENSOR-2000e1";
1183  std::vector<std::string> custom_sensors;
1184  dir = nullptr;
1185  ent = nullptr;
1186  if ((dir = opendir(HID_CUSTOM_PATH.c_str())) != nullptr)
1187  {
1188  while ((ent = readdir(dir)) != nullptr)
1189  {
1190  auto str = std::string(ent->d_name);
1191  if (str.find(prefix_custom_sensor_name) != std::string::npos)
1192  custom_sensors.push_back(HID_CUSTOM_PATH + "/" + str);
1193  }
1194  closedir(dir);
1195  }
1196 
1197 
1198  for (auto& elem : custom_sensors)
1199  {
1200  hid_device_info hid_dev_info{};
1201  if(!get_hid_device_info(elem.c_str(), hid_dev_info))
1202  {
1203  LOG_WARNING("Failed to read busnum/devnum. Custom HID Device Path: " << elem);
1204  continue;
1205  }
1206 
1207  hid_dev_info.id = custom_id;
1208  action(hid_dev_info);
1209  }
1210  }
1211 
1213  {
1214  char device_path[PATH_MAX] = {};
1215  if (nullptr == realpath(dev_path, device_path))
1216  {
1217  LOG_WARNING("Could not resolve HID path: " << dev_path);
1218  return false;
1219  }
1220 
1221  std::string device_path_str(device_path);
1222  device_path_str+="/";
1223  std::string busnum, devnum, devpath, vid, pid, dev_id, dev_name;
1224  std::ifstream(device_path_str + "name") >> dev_name;
1225  auto valid = false;
1226  for(auto i=0UL; i < MAX_DEV_PARENT_DIR; ++i)
1227  {
1228  if(std::ifstream(device_path_str + "busnum") >> busnum)
1229  {
1230  if(std::ifstream(device_path_str + "devnum") >> devnum)
1231  {
1232  if(std::ifstream(device_path_str + "devpath") >> devpath)
1233  {
1234  if(std::ifstream(device_path_str + "idVendor") >> vid)
1235  {
1236  if(std::ifstream(device_path_str + "idProduct") >> pid)
1237  {
1238  if(std::ifstream(device_path_str + "dev") >> dev_id)
1239  {
1240  valid = true;
1241  break;
1242  }
1243  }
1244  }
1245  }
1246  }
1247  }
1248  device_path_str += "../";
1249  }
1250 
1251  if (valid)
1252  {
1253  device_info.vid = vid;
1254  device_info.pid = pid;
1255  device_info.unique_id = busnum + "-" + devpath + "-" + devnum;
1256  device_info.id = dev_name;
1257  device_info.device_path = device_path;
1258  }
1259 
1260  return valid;
1261  }
1262  }
1263 }
void set_frequency(uint32_t frequency)
GLuint GLuint end
hid_custom_sensor(const std::string &device_path, const std::string &sensor_name)
v4l_hid_device(const hid_device_info &info)
const uint8_t HID_METADATA_SIZE
Definition: backend-hid.cpp:20
std::vector< uint8_t > get_report_data(const std::string &report_name, custom_sensor_report_field report_field)
iio_hid_sensor(const std::string &device_path, uint32_t frequency)
std::string get_sampling_frequency_name() const
metadata_hid_raw - HID metadata structure layout populated by backend
Definition: src/metadata.h:736
void start()
Definition: concurrency.h:286
void start_capture(hid_callback sensor_callback)
void start_capture(hid_callback sensor_callback)
#define LOG_WARNING(...)
Definition: src/types.h:241
std::unique_ptr< std::thread > _pm_thread
Definition: backend-hid.h:185
std::map< std::string, std::string > _reports
Definition: backend-hid.h:123
GLsizei const GLchar *const * path
Definition: glext.h:4276
hid_input(const std::string &iio_device_path, const std::string &input_name)
Definition: backend-hid.cpp:38
std::list< hid_input * > _channels
Definition: backend-hid.h:181
static void foreach_hid_device(std::function< void(const hid_device_info &)> action)
std::vector< hid_sensor > get_sensors() override
std::unique_ptr< std::thread > _hid_thread
Definition: backend-hid.h:184
const std::string HID_CUSTOM_PATH("/sys/bus/platform/drivers/hid_sensor_custom")
GLsizei const GLchar *const * string
d
Definition: rmse.py:171
unsigned char uint8_t
Definition: stdint.h:78
GLenum GLfloat * buffer
#define LOG_DEBUG_HID(...)
Definition: backend-hid.cpp:31
const size_t MAX_DEV_PARENT_DIR
Definition: backend-hid.cpp:18
GLuint GLfloat * val
def info(name, value, persistent=False)
Definition: test.py:301
not_this_one begin(...)
constexpr uint8_t metadata_imu_report_size
Definition: src/metadata.h:712
static bool sort_hids(hid_input *first, hid_input *second)
std::vector< uint8_t > read_report(const std::string &name_report_path)
const std::string IIO_DEVICE_PREFIX("iio:device")
unsigned int uint32_t
Definition: stdint.h:80
std::vector< uint8_t > get_custom_report_data(const std::string &custom_sensor_name, const std::string &report_name, custom_sensor_report_field report_field) override
bool flush()
Definition: concurrency.h:328
def callback(frame)
Definition: t265_stereo.py:91
unsigned __int64 uint64_t
Definition: stdint.h:90
const std::string IIO_ROOT_PATH("/sys/bus/iio/devices")
void start_capture(hid_callback callback) override
GLint first
#define LOG_ERROR(...)
Definition: src/types.h:242
action
Definition: enums.py:62
void open(const std::vector< hid_profile > &hid_profiles) override
std::unique_ptr< std::thread > _hid_thread
Definition: backend-hid.h:129
std::list< hid_input * > _inputs
Definition: backend-hid.h:180
const hid_input_info & get_hid_input_info() const
Definition: backend-hid.h:89
bool write_fs_attribute(const std::string &path, const T &val)
Definition: backend-hid.h:39
const uint32_t hid_buf_len
Definition: backend-hid.h:15
static bool get_hid_device_info(const char *dev_path, hid_device_info &device_info)
LOG_INFO("Log message using LOG_INFO()")
::realsense_legacy_msgs::metadata_< std::allocator< void > > metadata
static auto it
GLenum GLenum GLenum input
Definition: glext.h:10805
const std::string & get_sensor_name() const
Definition: backend-hid.h:106
GLint GLsizei count
void invoke(T item, bool is_blocking=false)
Definition: concurrency.h:254
typename::boost::move_detail::remove_reference< T >::type && move(T &&t) BOOST_NOEXCEPT
float rs2_vector::* pos
int stoi(const std::string &value)
platform::hid_header header
Definition: src/metadata.h:738
int i
std::function< void(const sensor_data &)> hid_callback
Definition: backend.h:327
#define LOG_DEBUG(...)
Definition: src/types.h:239
const std::string & get_sensor_name() const
Definition: backend-hid.h:144
GLuint64 GLenum GLint fd
Definition: glext.h:7768
constexpr uint8_t hid_header_size
Definition: backend.h:166
const size_t HID_DATA_ACTUAL_SIZE
Definition: backend-hid.cpp:21
std::string to_string(T value)


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