18 #pragma GCC diagnostic ignored "-Woverflow"
31 #define LOG_DEBUG_HID(...) do { CLOG(DEBUG ,LIBREALSENSE_ELPP_ID) << __VA_ARGS__; } while(false)
33 #define LOG_DEBUG_HID(...)
45 if (input_name.substr(0,input_prefix.size()) == input_prefix)
47 info.
input = input_name.substr(input_prefix.size(), input_name.size());
65 LOG_DEBUG(
"Error while disabling a hid device" );
72 auto input_data = is_enable ? 1 : 0;
75 std::ofstream iio_device_file(element_path);
77 if (!iio_device_file.is_open())
81 iio_device_file << input_data;
82 iio_device_file.close();
94 if (
info.
input.substr(
info.
input.size()-input_suffix.size(), input_suffix.size()) == input_suffix) {
100 std::ifstream device_type_file(read_scan_type_path);
101 if (!device_type_file)
108 char sign_char, endia_nchar;
110 auto ret = std::sscanf(
buffer,
123 device_type_file.close();
137 std::ifstream device_index_file(read_scan_index_path);
138 if (!device_index_file)
146 device_index_file.close();
150 std::ifstream device_enabled_file(read_enable_state_path);
151 if (!device_enabled_file)
159 device_enabled_file.close();
165 _custom_device_path(device_path),
167 _custom_device_name(
""),
169 _is_capturing(
false),
184 LOG_ERROR(
"An error has occurred while hid_custom_sensor dtor()!");
190 static const std::map<custom_sensor_report_field, std::string> report_fields = {{
minimum,
"-minimum"},
198 auto& report_folder =
_reports.at(report_name);
199 auto report_path =
_custom_device_path +
"/" + report_folder +
"/" + report_folder + report_fields.at(report_field);
202 catch(std::out_of_range)
215 std::ostringstream device_path;
218 auto read_device_path_str = device_path.str();
219 std::ifstream device_file(read_device_path_str);
222 if (!device_file.good())
231 const auto max_retries = 10;
233 while(++retries < max_retries)
235 if ((
_fd = open(read_device_path_str.c_str(), O_RDONLY | O_NONBLOCK)) > 0)
239 std::this_thread::sleep_for(std::chrono::milliseconds(5));
242 if ((retries == max_retries) && (
_fd <= 0))
257 _hid_thread = std::unique_ptr<std::thread>(
new std::thread([
this, read_device_path_str](){
259 std::vector<uint8_t> raw_data(channel_size *
hid_buf_len);
268 size_t read_size = 0;
270 struct timeval tv = {5,0};
272 auto val = select(max_fd + 1, &fds,
nullptr,
nullptr, &tv);
285 LOG_INFO(
"hid_custom_sensor: Stream finished");
289 else if (FD_ISSET(
_fd, &fds))
291 read_size = read(
_fd, raw_data.data(), raw_data.size());
298 LOG_WARNING(
"HID unresolved event : after select->FD_ISSET");
302 auto sz= read_size / channel_size;
305 LOG_DEBUG(
"HID: Going to handle " << sz <<
" packets");
307 for (
auto i = 0;
i < read_size / channel_size; ++
i)
309 auto p_raw_data = raw_data.data() + channel_size *
i;
315 sens_data.fo = {channel_size, channel_size, p_raw_data, p_raw_data};
320 LOG_DEBUG(
"HID: Finished to handle " << sz <<
" packets");
325 LOG_WARNING(
"hid_custom_sensor: Frames didn't arrived within 5 seconds");
359 auto fd = open(name_report_path.c_str(), O_RDONLY | O_NONBLOCK);
363 std::vector<uint8_t>
buffer;
365 auto read_size = read(fd,
buffer.data(),
buffer.size());
378 static const char* prefix_feature_name =
"feature";
379 static const char* prefix_input_name =
"input";
380 static const char* suffix_name_field =
"name";
382 struct dirent* ent =
nullptr;
385 while ((ent = readdir(
dir)) !=
nullptr)
388 if (
str.find(prefix_feature_name) != std::string::npos ||
389 str.find(prefix_input_name) != std::string::npos)
391 DIR* report_dir =
nullptr;
392 struct dirent* report_ent =
nullptr;
394 if ((report_dir = opendir(report_path.c_str())) !=
nullptr)
396 while ((report_ent = readdir(report_dir)) !=
nullptr)
399 if (report_str.find(suffix_name_field) != std::string::npos)
401 auto name_report_path = report_path +
"/" + report_ent->d_name;
405 _reports.insert(std::make_pair(name_report, ent->d_name));
408 closedir(report_dir);
423 auto input_data =
state ? 1 : 0;
425 std::ofstream custom_device_file(element_path);
427 if (!custom_device_file.is_open())
431 custom_device_file << input_data;
432 custom_device_file.close();
452 throw linux_backend_exception(
"hid_custom_sensor: Could not signal video capture thread to stop. Error write to pipe.");
459 _iio_device_number(0),
460 _iio_device_path(device_path),
462 _sampling_frequency_name(
""),
464 _is_capturing(
false),
467 init(frequency, sensitivity);
495 std::ostringstream iio_read_device_path;
498 auto iio_read_device_path_str = iio_read_device_path.str();
499 std::ifstream iio_device_file(iio_read_device_path_str);
502 if (!iio_device_file.good())
507 iio_device_file.close();
512 const auto max_retries = 10;
514 while(++retries < max_retries)
516 if ((
_fd = open(iio_read_device_path_str.c_str(), O_RDONLY | O_NONBLOCK)) > 0)
520 std::this_thread::sleep_for(std::chrono::milliseconds(5));
523 if ((retries == max_retries) && (
_fd <= 0))
538 _hid_thread = std::unique_ptr<std::thread>(
new std::thread([
this](){
542 std::vector<uint8_t> raw_data(raw_data_size);
553 ssize_t read_size = 0;
554 struct timeval tv = {5, 0};
556 auto val = select(max_fd + 1, &fds,
nullptr,
nullptr, &tv);
562 LOG_WARNING(
"iio_hid_sensor: select failed, return val = " <<
val);
571 LOG_INFO(
"iio_hid_sensor: Stream finished");
575 else if (FD_ISSET(
_fd, &fds))
577 read_size = read(
_fd, raw_data.data(), raw_data_size);
584 LOG_WARNING(
"HID IIO unresolved event : after select->FD_ISSET");
588 auto sz= read_size / channel_size;
591 LOG_DEBUG(
"HID: Going to handle " << sz <<
" packets");
594 for (
auto i = 0;
i < sz; ++
i)
597 auto p_raw_data = raw_data.data() + channel_size *
i;
606 meta_data.header.timestamp = *(
reinterpret_cast<uint64_t *
>(&p_raw_data[16]));
617 sens_data.fo = {hid_data_size,
metadata? meta_data.header.length:
uint8_t(0),
618 p_raw_data,
metadata? &meta_data :
nullptr, now_ts};
624 meta_data.header.timestamp /=1000;
635 LOG_DEBUG(
"HID: Finished to handle " << sz <<
" packets");
640 LOG_WARNING(
"iio_hid_sensor: Frames didn't arrived within the predefined interval");
641 std::this_thread::sleep_for(std::chrono::milliseconds(2));
673 std::ostringstream iio_read_device_path;
676 std::unique_ptr<int, std::function<
void(
int*)> > fd(
677 new int (
_fd = open(iio_read_device_path.str().c_str(), O_RDONLY | O_NONBLOCK)),
678 [&](
int*
d){ if (d && (*d)) { _fd = ::close(*d); }
delete d; });
684 create_channel_array();
686 const uint32_t channel_size = get_channel_size();
689 std::vector<uint8_t> raw_data(raw_data_size);
691 auto read_size = read(_fd, raw_data.data(), raw_data_size);
693 read_size = read(_fd, raw_data.data(), raw_data_size);
698 void iio_hid_sensor::set_frequency(
uint32_t frequency)
700 auto sampling_frequency_path = _iio_device_path +
"/" + _sampling_frequency_name;
701 std::ofstream iio_device_file(sampling_frequency_path);
703 if (!iio_device_file.is_open())
706 ". device path: " << sampling_frequency_path);
708 iio_device_file << frequency;
709 iio_device_file.close();
712 void iio_hid_sensor::set_sensitivity(
float sensitivity )
714 auto sensitivity_path = _iio_device_path +
"/" + _sensitivity_name;
715 std::ofstream iio_device_file( sensitivity_path );
717 if( ! iio_device_file.is_open() )
720 <<
"Failed to set sensitivity " << sensitivity
721 <<
". device path: " << sensitivity_path );
723 iio_device_file << sensitivity;
724 iio_device_file.close();
728 void iio_hid_sensor::set_power(
bool on)
730 auto path = _iio_device_path +
"/buffer/enable";
744 void iio_hid_sensor::signal_stop()
748 if (write(_stop_pipe_fd[1], buff, 1) < 0)
750 throw linux_backend_exception(
"iio_hid_sensor: Could not signal video capture thread to stop. Error write to pipe.");
754 bool iio_hid_sensor::has_metadata()
767 void iio_hid_sensor::create_channel_array()
770 for(
auto&
input : _inputs)
772 if (
input->get_hid_input_info().enabled)
774 _channels.push_back(
input);
778 _channels.sort(sort_hids);
784 std::ifstream iio_device_file(_iio_device_path +
"/name");
787 if (!iio_device_file.good())
792 char name_buffer[256] = {};
793 iio_device_file.getline(name_buffer,
sizeof(name_buffer));
796 iio_device_file.close();
800 auto pos = _iio_device_path.find_last_of(suffix_iio_device_path);
801 if (pos == std::string::npos)
804 auto substr = _iio_device_path.substr(pos + 1);
805 if (std::all_of(substr.begin(), substr.end(), ::isdigit))
807 _iio_device_number = atoi(substr.c_str());
814 _pm_dispatcher.start();
818 std::string current_trigger = _sensor_name +
"-dev" + _iio_device_path.back();
820 _pm_thread = std::unique_ptr<std::thread>(
new std::thread([
path,current_trigger](){
827 std::this_thread::sleep_for(std::chrono::milliseconds(50));
833 _pm_thread->detach();
836 read_device_inputs();
839 _sampling_frequency_name = get_sampling_frequency_name();
842 _sensitivity_name = get_sensitivity_name();
844 for (
auto&
input : _inputs)
847 set_frequency(frequency);
848 set_sensitivity( sensitivity );
855 assert(!_channels.empty());
858 for (
auto& elem : _channels)
860 auto input_info = elem->get_hid_input_info();
861 if (
bytes % input_info.bytes == 0)
863 input_info.location =
bytes;
867 input_info.location =
bytes -
bytes % input_info.bytes
871 bytes = input_info.location + input_info.bytes;
880 assert(!_channels.empty());
883 for (
auto& elem : _channels)
885 auto input_info = elem->get_hid_input_info();
886 bits_used += input_info.bits_used;
889 return std::ceil(bits_used / CHAR_BIT);
896 struct dirent *dir_ent =
nullptr;
899 dir = opendir(_iio_device_path.c_str());
906 while ((dir_ent = readdir(
dir)) !=
nullptr)
908 if (dir_ent->d_type != DT_DIR)
911 if (file.find(
"sampling_frequency") != std::string::npos)
913 sampling_frequency_name = file;
918 return sampling_frequency_name;
927 struct dirent * dir_ent =
nullptr;
930 dir = opendir( _iio_device_path.c_str() );
934 <<
"Failed to open scan_element " << _iio_device_path );
938 while( ( dir_ent = readdir(
dir ) ) !=
nullptr )
940 if( dir_ent->d_type != DT_DIR )
943 if( file.find(
"hysteresis" ) != std::string::npos )
945 sensitivity_name = file;
950 return sensitivity_name;
955 void iio_hid_sensor::read_device_inputs()
958 struct dirent *dir_ent =
nullptr;
960 auto scan_elements_path = _iio_device_path +
"/scan_elements";
962 dir = opendir(scan_elements_path.c_str());
969 while ((dir_ent = readdir(
dir)) !=
nullptr)
971 if (dir_ent->d_type != DT_DIR)
976 if (file.substr(0,prefix.size()) == prefix &&
977 file.substr(file.size()-suffix.size(),suffix.size()) == suffix) {
982 auto* new_input =
new hid_input(_iio_device_path, file);
984 _inputs.push_front(new_input);
1000 v4l_hid_device::foreach_hid_device([&](
const hid_device_info& hid_dev_info){
1003 _hid_device_infos.push_back(hid_dev_info);
1012 v4l_hid_device::~v4l_hid_device()
1014 for (
auto& elem : _streaming_iio_sensors)
1018 elem->stop_capture();
1022 LOG_DEBUG(
"Error while stopping capture sensor" );
1026 for (
auto& elem : _streaming_custom_sensors)
1030 elem->stop_capture();
1034 LOG_DEBUG(
"Error while stopping capture sensor" );
1039 void v4l_hid_device::open(
const std::vector<hid_profile>& hid_profiles)
1041 _hid_profiles = hid_profiles;
1050 _hid_custom_sensors.push_back(std::move(
device));
1055 float sensitivity = 0;
1056 for (
auto&
profile : hid_profiles)
1060 frequency =
profile.frequency;
1061 sensitivity =
profile.sensitivity;
1070 _iio_hid_sensors.push_back(std::move(
device));
1079 _iio_hid_sensors.clear();
1086 void v4l_hid_device::close()
1088 for (
auto& hid_iio_sensor : _iio_hid_sensors)
1090 hid_iio_sensor.reset();
1092 _iio_hid_sensors.clear();
1098 _hid_custom_sensors.clear();
1101 std::vector<hid_sensor> v4l_hid_device::get_sensors()
1103 std::vector<hid_sensor> iio_sensors;
1105 for (
auto&
sensor : _hid_profiles)
1106 iio_sensors.push_back({
sensor.sensor_name });
1109 for (
auto& elem : _hid_custom_sensors)
1111 iio_sensors.push_back(
hid_sensor{elem->get_sensor_name()});
1118 for (
auto&
profile : _hid_profiles)
1120 for (
auto&
sensor : _iio_hid_sensors)
1124 _streaming_iio_sensors.push_back(
sensor.get());
1128 for (
auto&
sensor : _hid_custom_sensors)
1132 _streaming_custom_sensors.push_back(
sensor.get());
1136 if (_streaming_iio_sensors.empty() && _streaming_custom_sensors.empty())
1140 if (!_streaming_iio_sensors.empty())
1142 std::vector<iio_hid_sensor*> captured_sensors;
1144 for (
auto& elem : _streaming_iio_sensors)
1146 elem->start_capture(callback);
1147 captured_sensors.push_back(elem);
1152 for (
auto& elem : captured_sensors)
1153 elem->stop_capture();
1155 _streaming_iio_sensors.clear();
1160 if (!_streaming_custom_sensors.empty())
1162 std::vector<hid_custom_sensor*> captured_sensors;
1164 for (
auto& elem : _streaming_custom_sensors)
1166 elem->start_capture(callback);
1167 captured_sensors.push_back(elem);
1172 for (
auto& elem : captured_sensors)
1173 elem->stop_capture();
1175 _streaming_custom_sensors.clear();
1182 void v4l_hid_device::stop_capture()
1184 for (
auto&
sensor : _iio_hid_sensors)
1189 _streaming_iio_sensors.clear();
1191 for (
auto&
sensor : _hid_custom_sensors)
1196 _streaming_custom_sensors.clear();
1199 std::vector<uint8_t> v4l_hid_device::get_custom_report_data(
const std::string& custom_sensor_name,
1203 auto it = std::find_if(begin(_hid_custom_sensors),
end(_hid_custom_sensors),
1204 [&](
const std::unique_ptr<hid_custom_sensor>& hcs)
1206 return hcs->get_sensor_name() == custom_sensor_name;
1208 if (
it !=
end(_hid_custom_sensors))
1210 return (*it)->get_report_data(report_name, report_field);
1219 struct dirent* ent =
nullptr;
1220 std::vector<std::string> common_sensors;
1223 while ((ent = readdir(
dir)) !=
nullptr)
1232 for (
auto& elem : common_sensors)
1235 if(!get_hid_device_info(elem.c_str(), hid_dev_info))
1243 device_path_str+=
"/";
1245 std::ifstream(device_path_str +
"name") >> dev_name;
1247 LOG_WARNING(
"Failed to read busnum/devnum. Device Path: " << elem);
1250 LOG_INFO(
"Failed to read busnum/devnum. Device Path: " << elem);
1259 static const char* prefix_custom_sensor_name =
"HID-SENSOR-2000e1";
1260 std::vector<std::string> custom_sensors;
1265 while ((ent = readdir(
dir)) !=
nullptr)
1268 if (
str.find(prefix_custom_sensor_name) != std::string::npos)
1275 for (
auto& elem : custom_sensors)
1278 if(!get_hid_device_info(elem.c_str(), hid_dev_info))
1280 LOG_WARNING(
"Failed to read busnum/devnum. Custom HID Device Path: " << elem);
1284 hid_dev_info.id = custom_id;
1291 char device_path[PATH_MAX] = {};
1292 if (
nullptr == realpath(dev_path, device_path))
1294 LOG_WARNING(
"Could not resolve HID path: " << dev_path);
1299 device_path_str+=
"/";
1301 std::ifstream(device_path_str +
"name") >> dev_name;
1305 if(std::ifstream(device_path_str +
"busnum") >> busnum)
1307 if(std::ifstream(device_path_str +
"devnum") >> devnum)
1309 if(std::ifstream(device_path_str +
"devpath") >> devpath)
1311 if(std::ifstream(device_path_str +
"idVendor") >> vid)
1313 if(std::ifstream(device_path_str +
"idProduct") >>
pid)
1315 if(std::ifstream(device_path_str +
"dev") >> dev_id)
1325 device_path_str +=
"../";
1332 device_info.unique_id = busnum +
"-" + devpath +
"-" + devnum;