16 #pragma GCC diagnostic ignored "-Woverflow"
25 const std::string
HID_CUSTOM_PATH(
"/sys/bus/platform/drivers/hid_sensor_custom");
29 #define LOG_DEBUG_HID(...) do { CLOG(DEBUG ,"librealsense") << __VA_ARGS__; } while(false)
31 #define LOG_DEBUG_HID(...)
41 static const std::string input_prefix =
"in_";
43 if (input_name.substr(0,input_prefix.size()) == input_prefix)
45 info.
input = input_name.substr(input_prefix.size(), input_name.size());
63 auto input_data = is_enable ? 1 : 0;
66 std::ofstream iio_device_file(element_path);
68 if (!iio_device_file.is_open())
72 iio_device_file << input_data;
73 iio_device_file.close();
83 static const std::string input_suffix =
"_en";
85 if (
info.
input.substr(
info.
input.size()-input_suffix.size(), input_suffix.size()) == input_suffix) {
91 std::ifstream device_type_file(read_scan_type_path);
92 if (!device_type_file)
99 char sign_char, endia_nchar;
101 auto ret = std::sscanf(
buffer,
114 device_type_file.close();
128 std::ifstream device_index_file(read_scan_index_path);
129 if (!device_index_file)
137 device_index_file.close();
141 std::ifstream device_enabled_file(read_enable_state_path);
142 if (!device_enabled_file)
150 device_enabled_file.close();
156 _custom_device_path(device_path),
157 _custom_sensor_name(sensor_name),
158 _custom_device_name(
""),
160 _is_capturing(
false),
175 LOG_ERROR(
"An error has occurred while hid_custom_sensor dtor()!");
181 static const std::map<custom_sensor_report_field, std::string> report_fields = {{
minimum,
"-minimum"},
189 auto& report_folder =
_reports.at(report_name);
190 auto report_path =
_custom_device_path +
"/" + report_folder +
"/" + report_folder + report_fields.at(report_field);
193 catch(std::out_of_range)
206 std::ostringstream device_path;
209 auto read_device_path_str = device_path.str();
210 std::ifstream device_file(read_device_path_str);
213 if (!device_file.good())
222 const auto max_retries = 10;
224 while(++retries < max_retries)
226 if ((
_fd = open(read_device_path_str.c_str(), O_RDONLY | O_NONBLOCK)) > 0)
230 std::this_thread::sleep_for(std::chrono::milliseconds(5));
233 if ((retries == max_retries) && (
_fd <= 0))
248 _hid_thread = std::unique_ptr<std::thread>(
new std::thread([
this, read_device_path_str](){
250 std::vector<uint8_t> raw_data(channel_size *
hid_buf_len);
259 size_t read_size = 0;
261 struct timeval tv = {5,0};
263 auto val = select(max_fd + 1, &fds,
nullptr,
nullptr, &tv);
276 LOG_INFO(
"hid_custom_sensor: Stream finished");
280 else if (FD_ISSET(
_fd, &fds))
282 read_size = read(
_fd, raw_data.data(), raw_data.size());
289 LOG_WARNING(
"HID unresolved event : after select->FD_ISSET");
293 auto sz= read_size / channel_size;
296 LOG_DEBUG(
"HID: Going to handle " << sz <<
" packets");
298 for (
auto i = 0;
i < read_size / channel_size; ++
i)
300 auto p_raw_data = raw_data.data() + channel_size *
i;
306 sens_data.fo = {channel_size, channel_size, p_raw_data, p_raw_data};
311 LOG_DEBUG(
"HID: Finished to handle " << sz <<
" packets");
316 LOG_WARNING(
"hid_custom_sensor: Frames didn't arrived within 5 seconds");
350 auto fd = open(name_report_path.c_str(), O_RDONLY | O_NONBLOCK);
354 std::vector<uint8_t>
buffer;
356 auto read_size = read(fd,
buffer.data(),
buffer.size());
369 static const char* prefix_feature_name =
"feature";
370 static const char* prefix_input_name =
"input";
371 static const char* suffix_name_field =
"name";
373 struct dirent* ent =
nullptr;
376 while ((ent = readdir(
dir)) !=
nullptr)
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)
382 DIR* report_dir =
nullptr;
383 struct dirent* report_ent =
nullptr;
385 if ((report_dir = opendir(report_path.c_str())) !=
nullptr)
387 while ((report_ent = readdir(report_dir)) !=
nullptr)
389 auto report_str = std::string(report_ent->d_name);
390 if (report_str.find(suffix_name_field) != std::string::npos)
392 auto name_report_path = report_path +
"/" + report_ent->d_name;
395 std::string name_report(
reinterpret_cast<char const*
>(
buffer.data()));
396 _reports.insert(std::make_pair(name_report, ent->d_name));
399 closedir(report_dir);
414 auto input_data =
state ? 1 : 0;
416 std::ofstream custom_device_file(element_path);
418 if (!custom_device_file.is_open())
422 custom_device_file << input_data;
423 custom_device_file.close();
443 throw linux_backend_exception(
"hid_custom_sensor: Could not signal video capture thread to stop. Error write to pipe.");
450 _iio_device_number(0),
451 _iio_device_path(device_path),
453 _sampling_frequency_name(
""),
455 _is_capturing(
false),
486 std::ostringstream iio_read_device_path;
489 auto iio_read_device_path_str = iio_read_device_path.str();
490 std::ifstream iio_device_file(iio_read_device_path_str);
493 if (!iio_device_file.good())
498 iio_device_file.close();
503 const auto max_retries = 10;
505 while(++retries < max_retries)
507 if ((
_fd = open(iio_read_device_path_str.c_str(), O_RDONLY | O_NONBLOCK)) > 0)
511 std::this_thread::sleep_for(std::chrono::milliseconds(5));
514 if ((retries == max_retries) && (
_fd <= 0))
529 _hid_thread = std::unique_ptr<std::thread>(
new std::thread([
this](){
533 std::vector<uint8_t> raw_data(raw_data_size);
544 ssize_t read_size = 0;
545 struct timeval tv = {5, 0};
547 auto val = select(max_fd + 1, &fds,
nullptr,
nullptr, &tv);
553 LOG_WARNING(
"iio_hid_sensor: select failed, return val = " <<
val);
562 LOG_INFO(
"iio_hid_sensor: Stream finished");
566 else if (FD_ISSET(
_fd, &fds))
568 read_size = read(
_fd, raw_data.data(), raw_data_size);
575 LOG_WARNING(
"HID IIO unresolved event : after select->FD_ISSET");
579 auto sz= read_size / channel_size;
582 LOG_DEBUG(
"HID: Going to handle " << sz <<
" packets");
585 for (
auto i = 0;
i < sz; ++
i)
588 auto p_raw_data = raw_data.data() + channel_size *
i;
597 meta_data.header.timestamp = *(
reinterpret_cast<uint64_t *
>(&p_raw_data[16]));
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};
615 meta_data.header.timestamp /=1000;
626 LOG_DEBUG(
"HID: Finished to handle " << sz <<
" packets");
631 LOG_WARNING(
"iio_hid_sensor: Frames didn't arrived within the predefined interval");
632 std::this_thread::sleep_for(std::chrono::milliseconds(2));
664 std::ostringstream iio_read_device_path;
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; });
675 create_channel_array();
677 const uint32_t channel_size = get_channel_size();
680 std::vector<uint8_t> raw_data(raw_data_size);
682 auto read_size = read(_fd, raw_data.data(), raw_data_size);
684 read_size = read(_fd, raw_data.data(), raw_data_size);
689 void iio_hid_sensor::set_frequency(
uint32_t frequency)
691 auto sampling_frequency_path = _iio_device_path +
"/" + _sampling_frequency_name;
692 std::ofstream iio_device_file(sampling_frequency_path);
694 if (!iio_device_file.is_open())
697 ". device path: " << sampling_frequency_path);
699 iio_device_file << frequency;
700 iio_device_file.close();
704 void iio_hid_sensor::set_power(
bool on)
706 auto path = _iio_device_path +
"/buffer/enable";
720 void iio_hid_sensor::signal_stop()
724 if (write(_stop_pipe_fd[1], buff, 1) < 0)
726 throw linux_backend_exception(
"iio_hid_sensor: Could not signal video capture thread to stop. Error write to pipe.");
730 bool iio_hid_sensor::has_metadata()
742 void iio_hid_sensor::create_channel_array()
745 for(
auto&
input : _inputs)
747 if (
input->get_hid_input_info().enabled)
749 _channels.push_back(
input);
753 _channels.sort(sort_hids);
759 std::ifstream iio_device_file(_iio_device_path +
"/name");
762 if (!iio_device_file.good())
767 char name_buffer[256] = {};
768 iio_device_file.getline(name_buffer,
sizeof(name_buffer));
769 _sensor_name = std::string(name_buffer);
771 iio_device_file.close();
775 auto pos = _iio_device_path.find_last_of(suffix_iio_device_path);
776 if (
pos == std::string::npos)
779 auto substr = _iio_device_path.substr(
pos + 1);
780 if (std::all_of(substr.begin(), substr.end(), ::isdigit))
782 _iio_device_number = atoi(substr.c_str());
789 _pm_dispatcher.start();
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](){
802 std::this_thread::sleep_for(std::chrono::milliseconds(50));
808 _pm_thread->detach();
811 read_device_inputs();
814 _sampling_frequency_name = get_sampling_frequency_name();
816 for (
auto&
input : _inputs)
819 set_frequency(frequency);
826 assert(!_channels.empty());
829 for (
auto& elem : _channels)
831 auto input_info = elem->get_hid_input_info();
832 if (
bytes % input_info.bytes == 0)
834 input_info.location =
bytes;
838 input_info.location =
bytes -
bytes % input_info.bytes
842 bytes = input_info.location + input_info.bytes;
851 assert(!_channels.empty());
854 for (
auto& elem : _channels)
856 auto input_info = elem->get_hid_input_info();
857 bits_used += input_info.bits_used;
860 return std::ceil(bits_used / CHAR_BIT);
863 std::string iio_hid_sensor::get_sampling_frequency_name()
const
865 std::string sampling_frequency_name =
"";
867 struct dirent *dir_ent =
nullptr;
870 dir = opendir(_iio_device_path.c_str());
877 while ((dir_ent = readdir(
dir)) !=
nullptr)
879 if (dir_ent->d_type != DT_DIR)
881 std::string file(dir_ent->d_name);
882 if (file.find(
"sampling_frequency") != std::string::npos)
884 sampling_frequency_name = file;
889 return sampling_frequency_name;
894 void iio_hid_sensor::read_device_inputs()
897 struct dirent *dir_ent =
nullptr;
899 auto scan_elements_path = _iio_device_path +
"/scan_elements";
901 dir = opendir(scan_elements_path.c_str());
908 while ((dir_ent = readdir(
dir)) !=
nullptr)
910 if (dir_ent->d_type != DT_DIR)
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) {
921 auto* new_input =
new hid_input(_iio_device_path, file);
923 _inputs.push_front(new_input);
939 v4l_hid_device::foreach_hid_device([&](
const hid_device_info& hid_dev_info){
942 _hid_device_infos.push_back(hid_dev_info);
951 v4l_hid_device::~v4l_hid_device()
953 for (
auto& elem : _streaming_iio_sensors)
955 elem->stop_capture();
958 for (
auto& elem : _streaming_custom_sensors)
960 elem->stop_capture();
964 void v4l_hid_device::open(
const std::vector<hid_profile>& hid_profiles)
966 _hid_profiles = hid_profiles;
980 for (
auto&
profile : hid_profiles)
1002 _iio_hid_sensors.clear();
1009 void v4l_hid_device::close()
1011 for (
auto& hid_iio_sensor : _iio_hid_sensors)
1013 hid_iio_sensor.reset();
1015 _iio_hid_sensors.clear();
1021 _hid_custom_sensors.clear();
1024 std::vector<hid_sensor> v4l_hid_device::get_sensors()
1026 std::vector<hid_sensor> iio_sensors;
1028 for (
auto&
sensor : _hid_profiles)
1029 iio_sensors.push_back({
sensor.sensor_name });
1032 for (
auto& elem : _hid_custom_sensors)
1034 iio_sensors.push_back(
hid_sensor{elem->get_sensor_name()});
1041 for (
auto&
profile : _hid_profiles)
1043 for (
auto&
sensor : _iio_hid_sensors)
1047 _streaming_iio_sensors.push_back(
sensor.get());
1051 for (
auto&
sensor : _hid_custom_sensors)
1055 _streaming_custom_sensors.push_back(
sensor.get());
1059 if (_streaming_iio_sensors.empty() && _streaming_custom_sensors.empty())
1063 if (!_streaming_iio_sensors.empty())
1065 std::vector<iio_hid_sensor*> captured_sensors;
1067 for (
auto& elem : _streaming_iio_sensors)
1070 captured_sensors.push_back(elem);
1075 for (
auto& elem : captured_sensors)
1076 elem->stop_capture();
1078 _streaming_iio_sensors.clear();
1083 if (!_streaming_custom_sensors.empty())
1085 std::vector<hid_custom_sensor*> captured_sensors;
1087 for (
auto& elem : _streaming_custom_sensors)
1090 captured_sensors.push_back(elem);
1095 for (
auto& elem : captured_sensors)
1096 elem->stop_capture();
1098 _streaming_custom_sensors.clear();
1105 void v4l_hid_device::stop_capture()
1107 for (
auto&
sensor : _iio_hid_sensors)
1112 _streaming_iio_sensors.clear();
1114 for (
auto&
sensor : _hid_custom_sensors)
1119 _streaming_custom_sensors.clear();
1122 std::vector<uint8_t> v4l_hid_device::get_custom_report_data(
const std::string& custom_sensor_name,
1123 const std::string& report_name,
1126 auto it = std::find_if(
begin(_hid_custom_sensors),
end(_hid_custom_sensors),
1127 [&](
const std::unique_ptr<hid_custom_sensor>& hcs)
1129 return hcs->get_sensor_name() == custom_sensor_name;
1131 if (
it !=
end(_hid_custom_sensors))
1133 return (*it)->get_report_data(report_name, report_field);
1142 struct dirent* ent =
nullptr;
1143 std::vector<std::string> common_sensors;
1146 while ((ent = readdir(
dir)) !=
nullptr)
1148 auto str = std::string(ent->d_name);
1155 for (
auto& elem : common_sensors)
1158 if(!get_hid_device_info(elem.c_str(), hid_dev_info))
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);
1173 LOG_INFO(
"Failed to read busnum/devnum. Device Path: " << elem);
1182 static const char* prefix_custom_sensor_name =
"HID-SENSOR-2000e1";
1183 std::vector<std::string> custom_sensors;
1188 while ((ent = readdir(
dir)) !=
nullptr)
1190 auto str = std::string(ent->d_name);
1191 if (
str.find(prefix_custom_sensor_name) != std::string::npos)
1198 for (
auto& elem : custom_sensors)
1201 if(!get_hid_device_info(elem.c_str(), hid_dev_info))
1203 LOG_WARNING(
"Failed to read busnum/devnum. Custom HID Device Path: " << elem);
1207 hid_dev_info.id = custom_id;
1214 char device_path[PATH_MAX] = {};
1215 if (
nullptr == realpath(dev_path, device_path))
1217 LOG_WARNING(
"Could not resolve HID path: " << dev_path);
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;
1228 if(std::ifstream(device_path_str +
"busnum") >> busnum)
1230 if(std::ifstream(device_path_str +
"devnum") >> devnum)
1232 if(std::ifstream(device_path_str +
"devpath") >> devpath)
1234 if(std::ifstream(device_path_str +
"idVendor") >> vid)
1236 if(std::ifstream(device_path_str +
"idProduct") >>
pid)
1238 if(std::ifstream(device_path_str +
"dev") >> dev_id)
1248 device_path_str +=
"../";
1255 device_info.unique_id = busnum +
"-" + devpath +
"-" + devnum;