12 #if defined(USING_UDEV)
15 #include "../polling-device-watcher.h"
48 #include <sys/ioctl.h>
49 #include <sys/sysmacros.h>
50 #include <linux/usb/video.h>
51 #include <linux/media.h>
52 #include <linux/uvcvideo.h>
53 #include <linux/videodev2.h>
59 #include <sys/signalfd.h>
61 #pragma GCC diagnostic ignored "-Woverflow"
114 int lockf64(
int fd,
int cmd, off64_t
length)
118 memset(&fl, 0,
sizeof(fl));
119 fl.l_whence = SEEK_CUR;
123 if (
cmd == F_ULOCK) {
126 return fcntl(fd, F_SETLK64, &fl);
131 return fcntl(fd, F_SETLKW64, &fl);
134 if (
cmd == F_TLOCK) {
136 return fcntl(fd, F_SETLK64, &fl);
140 if (fcntl(fd, F_GETLK64, &fl) == -1)
return -1;
141 if (fl.l_type == F_UNLCK || fl.l_pid == getpid())
return 0;
165 : _device_path(device_path),
168 _object_lock_counter(0)
187 LOG_DEBUG(
"Error while unlocking mutex" );
213 auto ret = lockf(
_fildes, F_TLOCK, 0);
237 auto ret = lockf(
_fildes, F_LOCK, 0);
264 auto ret = lockf(
_fildes, F_ULOCK, 0);
266 err_msg =
"lockf(...) failed";
271 err_msg =
"close(...) failed";
278 if (!err_msg.empty())
283 static int xioctl(
int fh,
unsigned long request,
void *arg)
287 r = ioctl(fh, request, arg);
288 }
while (
r < 0 && errno == EINTR);
293 : _type(
type), _use_memory_map(use_memory_map), _index(
index)
295 v4l2_buffer
buf = {};
296 struct v4l2_plane planes[VIDEO_MAX_PLANES] = {};
301 if (
type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
302 buf.m.planes = planes;
303 buf.length = VIDEO_MAX_PLANES;
305 if(
xioctl(fd, VIDIOC_QUERYBUF, &
buf) < 0)
312 if (
type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
322 PROT_READ | PROT_WRITE, MAP_SHARED,
338 v4l2_buffer
buf = {};
339 struct v4l2_plane planes[VIDEO_MAX_PLANES] = {};
344 if (
_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
345 buf.m.planes = planes;
350 buf.m.userptr =
reinterpret_cast<unsigned long>(
_start);
355 LOG_DEBUG_V4L(
"prepare_for_streaming fd " << std::dec << fd);
399 LOG_ERROR(
"xioctl(VIDIOC_QBUF) failed when requesting new frame! fd: " << fd <<
" error: " << strerror(errno));
409 std::shared_ptr<platform::buffer> data_buf
419 this->
buffers.at(buf_type)._managed =
true;
423 buffers.at(buf_type)._file_desc = file_desc;
424 buffers.at(buf_type)._managed =
false;
425 buffers.at(buf_type)._data_buf = data_buf;
426 buffers.at(buf_type)._dq_buf = v4l_buf;
435 if (
buf._data_buf && (
buf._file_desc >= 0))
436 buf._data_buf->request_next_frame(
buf._file_desc);
444 void* md_start =
nullptr;
449 static const int d4xx_md_size = 248;
456 auto md_appendix_sz = 0L;
457 if (compressed && (dq.bytesused < fr_payload_size))
458 md_appendix_sz = d4xx_md_size;
460 md_appendix_sz = long(dq.bytesused) - fr_payload_size;
462 if (md_appendix_sz >0 )
465 md_size = (*(
static_cast<uint8_t*
>(md_start)));
466 int md_flags = (*(
static_cast<uint8_t*
>(md_start)+1));
468 if ((md_appendix_sz != md_size) || (!
val_in_range(md_flags, {0x8e, 0x8f})))
476 if (
nullptr == md_start)
508 char usb_actual_path[PATH_MAX] = {0};
509 if (realpath(
path.c_str(), usb_actual_path) !=
nullptr)
513 if(!(std::ifstream(
path +
"/version") >> camera_usb_version))
520 if (std::string::npos != camera_usb_version.find(usb_name))
534 std::unique_ptr<int, std::function<
void(
int*)> > fd(
535 new int (open(dev_name.c_str(), O_RDWR | O_NONBLOCK, 0)),
536 [](
int*
d){ if (d && (*d)) {::close(*d); }
delete d; });
541 v4l2_capability cap = {};
542 if(
xioctl(*fd, VIDIOC_QUERYCAP, &cap) < 0)
545 throw linux_backend_exception(
rsutils::string::from() << __FUNCTION__ <<
" " << dev_name <<
" is no V4L2 device");
547 throw linux_backend_exception(
rsutils::string::from() <<__FUNCTION__ <<
" xioctl(VIDIOC_QUERYCAP) failed");
566 v4l2_memory mem_type, v4l2_buf_type
type)
568 struct v4l2_requestbuffers req = {
count,
type, mem_type, {}};
570 if(
xioctl(fd, VIDIOC_REQBUFS, &req) < 0)
573 LOG_ERROR(dev_name +
" does not support memory mapping");
583 std::ifstream uevent_file(video_path +
"/uevent");
586 LOG_ERROR(
"Cannot access " + video_path +
"/uevent");
590 std::string uevent_line, major_string, minor_string;
591 while (std::getline(uevent_file, uevent_line) && (major_string.empty() || minor_string.empty()))
593 if (uevent_line.find(
"MAJOR=") != std::string::npos)
595 major_string = uevent_line.substr(uevent_line.find_last_of(
'=') + 1);
597 else if (uevent_line.find(
"MINOR=") != std::string::npos)
599 minor_string = uevent_line.substr(uevent_line.find_last_of(
'=') + 1);
604 if (major_string.empty() || minor_string.empty())
606 LOG_ERROR(
"No Major or Minor number found for " + video_path);
610 DIR *
dir = opendir(
"/dev");
616 while (dirent * entry = readdir(
dir))
622 if (stat(
path.c_str(), &
st) < 0)
639 std::vector<std::string> v4l_uvc_device::get_video_paths()
641 std::vector<std::string> video_paths;
643 DIR *
dir = opendir(
"/sys/class/video4linux");
646 LOG_INFO(
"Cannot access /sys/class/video4linux");
649 while (dirent * entry = readdir(
dir))
652 if(
name ==
"." ||
name ==
"..")
continue;
655 static const std::regex video_dev_pattern(
"(\\/video\\d+)$");
659 char buff[PATH_MAX] = {0};
660 if (realpath(
path.c_str(), buff) !=
nullptr)
663 if (real_path.find(
"virtual") != std::string::npos)
665 if (!std::regex_search(real_path, video_dev_pattern))
672 video_paths.push_back(real_path);
681 std::sort(video_paths.begin(), video_paths.end(),
685 std::string first_video = first.substr(first.find_last_of(
'/') + 1);
686 std::string second_video = second.substr(second.find_last_of(
'/') + 1);
689 std::stringstream first_index(first_video.substr(first_video.find_first_of(
"0123456789")));
690 std::stringstream second_index(second_video.substr(second_video.find_first_of(
"0123456789")));
691 int left_id = 0, right_id = 0;
692 first_index >> left_id;
693 second_index >> right_id;
694 return left_id < right_id;
703 if(stat(dev_name.c_str(), &
st) < 0)
707 if(!S_ISCHR(
st.st_mode))
711 auto valid_path =
false;
712 std::ostringstream
ss;
713 ss <<
"/sys/dev/char/" <<
major(
st.st_rdev) <<
":" <<
minor(
st.st_rdev) <<
"/device/";
715 auto char_dev_path =
ss.str();
719 if(std::ifstream(char_dev_path +
"busnum") >> busnum)
721 if(std::ifstream(char_dev_path +
"devnum") >> devnum)
723 if(std::ifstream(char_dev_path +
"devpath") >> devpath)
730 char_dev_path +=
"../";
743 if (!is_usb_path_valid(video_path, dev_name, busnum, devnum, devpath))
748 LOG_INFO(
"Failed to read busnum/devnum. Device Path: " << (
"/sys/class/video4linux/" +
name));
753 LOG_INFO(
"Enumerating UVC " <<
name <<
" realpath=" << video_path);
758 if(!(std::ifstream(
"/sys/class/video4linux/" +
name +
"/device/modalias") >> modalias))
760 if(modalias.size() < 14 || modalias.substr(0,5) !=
"usb:v" || modalias[9] !=
'p')
766 if(!(std::ifstream(
"/sys/class/video4linux/" +
name +
"/device/bInterfaceNumber") >> std::hex >> mi))
783 info.device_path = video_path;
784 info.unique_id = busnum +
"-" + devpath +
"-" + devnum;
785 info.conn_spec = usb_specification;
791 void v4l_uvc_device::get_mipi_device_info(
const std::string& dev_name,
794 struct v4l2_capability vcap;
795 int fd = open(dev_name.c_str(), O_RDWR);
798 int err = ioctl(fd, VIDIOC_QUERYCAP, &vcap);
801 struct media_device_info mdi;
803 err = ioctl(fd, MEDIA_IOC_DEVICE_INFO, &mdi);
807 bus_info = mdi.bus_info;
819 bus_info =
reinterpret_cast<const char *
>(vcap.bus_info);
820 card =
reinterpret_cast<const char *
>(vcap.card);
831 auto dev_name =
"/dev/" +
name;
833 get_mipi_device_info(dev_name, bus_info, card);
843 if (std::regex_search(
name, match, video_dev_index))
849 LOG_WARNING(
"Unresolved Video4Linux device pattern: " <<
name <<
", device is skipped");
860 static int first_video_index = ind;
862 const int camera_video_nodes = 6;
863 int cam_id = ind / camera_video_nodes;
864 ind = (ind - first_video_index) % camera_video_nodes;
865 if (ind == 0 || ind == 2 || ind == 4)
867 else if (ind == 1 | ind == 3)
873 LOG_WARNING(
"Unresolved Video4Linux device mi, device is skipped");
882 info.device_path = video_path;
890 std::array<std::string, 2> dfu_device_paths = {
"/dev/d4xx-dfu504",
"/dev/d4xx-dfu-30-0010"};
891 for (
const auto& dfu_device_path: dfu_device_paths) {
892 int vfd = open(dfu_device_path.c_str(), O_RDONLY | O_NONBLOCK);
895 info.dfu_device_path = dfu_device_path;
900 info.conn_spec = usb_specification;
906 bool v4l_uvc_device::is_usb_device_path(
const std::string& video_path)
908 static const std::regex uvc_pattern(
"(\\/usb\\d+\\/)\\w+");
909 return std::regex_search(video_path, uvc_pattern);
912 void v4l_uvc_device::foreach_uvc_device(
916 std::vector<std::string> video_paths = get_video_paths();
917 typedef std::pair<uvc_device_info,std::string> node_info;
918 std::vector<node_info> uvc_nodes,uvc_devices;
919 std::vector<node_info> mipi_rs_enum_nodes;
921 std::vector<std::string> video_sensors = {
"depth",
"color",
"ir",
"imu"};
922 const int MAX_V4L2_DEVICES = 8;
924 for (
int i = 0;
i < MAX_V4L2_DEVICES;
i++ ) {
925 for (
const auto &
vs: video_sensors) {
930 std::string video_md_path =
"/dev/" + device_md_path;
936 vfd = open(video_path.c_str(), O_RDONLY | O_NONBLOCK);
944 info = get_info_from_mipi_device_path(video_path, device_path);
946 catch(
const std::exception &
e)
953 vfd = open(dfu_device_path.c_str(), O_RDONLY | O_NONBLOCK);
957 info.dfu_device_path = dfu_device_path;
960 info.mi =
vs.compare(
"imu") ? 0 : 4;
963 mipi_rs_enum_nodes.emplace_back(
info, video_path);
966 vfd = open(video_md_path.c_str(), O_RDONLY | O_NONBLOCK);
975 info = get_info_from_mipi_device_path(video_md_path, device_md_path);
977 catch(
const std::exception &
e)
979 LOG_WARNING(
"MIPI video metadata device issue: " <<
e.what());
984 mipi_rs_enum_nodes.emplace_back(
info, video_md_path);
989 if(mipi_rs_enum_nodes.size())
991 uvc_nodes.insert(uvc_nodes.end(), mipi_rs_enum_nodes.begin(), mipi_rs_enum_nodes.end());
996 for(
auto&& video_path : video_paths)
999 auto name = video_path.substr(video_path.find_last_of(
'/') + 1);
1004 if (is_usb_device_path(video_path))
1006 info = get_info_from_usb_device_path(video_path,
name);
1008 else if(mipi_rs_enum_nodes.empty())
1011 static const std::regex rs_mipi_compatible(
".vi:|ipu6");
1012 info = get_info_from_mipi_device_path(video_path,
name);
1013 if (!regex_search(
info.unique_id, rs_mipi_compatible)) {
1025 uvc_nodes.emplace_back(
info, dev_name);
1028 catch(
const std::exception &
e)
1030 LOG_INFO(
"Not a USB video device: " <<
e.what());
1037 for (
auto&& cur_node : uvc_nodes)
1042 uvc_devices.emplace_back(cur_node);
1045 if (uvc_devices.empty())
1047 LOG_ERROR(
"UVC meta-node with no video streaming node encountered: " <<
std::string(cur_node.first));
1052 auto uvc_node = uvc_devices.back();
1060 if (uvc_node.first.has_metadata_node)
1062 LOG_ERROR(
"Metadata node for uvc device: " <<
std::string(uvc_node.first) <<
" was previously assigned ");
1067 uvc_node.first.has_metadata_node =
true;
1068 uvc_node.first.metadata_node_id = cur_node.first.id;
1069 uvc_devices.back() = uvc_node;
1072 catch(
const std::exception &
e)
1074 LOG_ERROR(
"Failed to map meta-node " <<
std::string(cur_node.first) <<
", error encountered: " <<
e.what());
1081 for (
auto&&
dev : uvc_devices)
1084 catch(
const std::exception &
e)
1086 LOG_ERROR(
"Registration of UVC device failed: " <<
e.what());
1092 bool is_kpi_violated =
false;
1093 long int timestamp_usec =
static_cast<long int> (
timestamp.tv_sec * 1000000 +
timestamp.tv_usec);
1096 auto it = std::find_if(drops_per_stream.begin(), drops_per_stream.end(),
1098 {return profile == sp_deq.first; });
1102 if (
it != drops_per_stream.end())
1105 int time_limit = 30;
1108 int max_num_of_drops =
profile.
fps * _kpi_frames_drops_pct * time_limit;
1110 auto& queue_for_profile =
it->second;
1112 while (queue_for_profile.size() > 0)
1114 auto delta_ts_usec = timestamp_usec - queue_for_profile.front();
1115 if (delta_ts_usec > (time_limit * 1000000))
1117 queue_for_profile.pop_front();
1123 if (queue_for_profile.size() >= max_num_of_drops)
1125 is_kpi_violated =
true;
1126 queue_for_profile.clear();
1129 queue_for_profile.push_back(timestamp_usec);
1134 std::deque<long int> deque_to_add;
1135 deque_to_add.push_back(timestamp_usec);
1136 drops_per_stream.push_back(std::make_pair(
profile, deque_to_add));
1138 return is_kpi_violated;
1142 : _name(
""), _info(),
1143 _is_capturing(false),
1147 _named_mtx(nullptr),
1148 _use_memory_map(use_memory_map),
1151 _buf_dispatch(use_memory_map),
1160 _device_path =
i.device_path;
1161 _device_usb_spec =
i.conn_spec;
1165 throw linux_backend_exception(
"device is no longer connected!");
1167 _named_mtx = std::unique_ptr<named_mutex>(
new named_mutex(_name, 5000));
1174 for (
auto&& fd :
_fds)
1176 try {
if (fd)
::close(fd);}
catch (...) {}
1184 v4l2_fmtdesc pixel_format = {};
1187 while (ioctl(
_fd, VIDIOC_ENUM_FMT, &pixel_format) == 0)
1189 v4l2_frmsizeenum frame_size = {};
1190 frame_size.pixel_format = pixel_format.pixelformat;
1194 if (pixel_format.pixelformat == 0)
1199 static const std::set<std::string> pending_formats = {
1200 "00000050-0000-0010-8000-00aa003",
1201 "00000032-0000-0010-8000-00aa003",
1205 pending_formats.end(),
1206 (
const char*)pixel_format.description) ==
1207 pending_formats.end())
1213 if (std::regex_search(
s.begin(),
s.end(), match, rgx))
1215 std::stringstream
ss;
1218 ss >> std::hex >>
id;
1224 <<
"' is not natively supported by the running Linux kernel and likely requires a patch");
1229 ++pixel_format.index;
1234 v4l2_streamparm parm = {};
1236 if(
xioctl(
_fd, VIDIOC_G_PARM, &parm) < 0)
1239 parm.parm.capture.timeperframe.numerator = 1;
1240 parm.parm.capture.timeperframe.denominator =
profile.
fps;
1241 if(
xioctl(
_fd, VIDIOC_S_PARM, &parm) < 0)
1329 char fourcc_buff[
sizeof(device_fourcc)+1];
1330 std::memcpy( fourcc_buff, &device_fourcc,
sizeof( device_fourcc ) );
1331 fourcc_buff[
sizeof(device_fourcc)] = 0;
1347 using namespace std::chrono;
1354 auto ms = duration_cast<milliseconds>(
now.time_since_epoch()) % 1000;
1357 auto timer = system_clock::to_time_t(
now);
1360 std::tm bt = *std::localtime(&
timer);
1362 std::ostringstream oss;
1364 oss << std::put_time(&bt,
"%H:%M:%S");
1365 oss <<
'.' << std::setfill(
'0') << std::setw(3) << ms.count();
1373 for (
auto fd :
_fds)
1378 struct timespec mono_time;
1379 int ret = clock_gettime(CLOCK_MONOTONIC, &mono_time);
1382 struct timeval expiration_time = { mono_time.tv_sec + 5, mono_time.tv_nsec / 1000 };
1389 struct timeval remaining;
1390 ret = clock_gettime(CLOCK_MONOTONIC, &mono_time);
1393 struct timeval
current_time = { mono_time.tv_sec, mono_time.tv_nsec / 1000 };
1396 # define timercmp_lt(a, b) \
1397 (((a)->tv_sec == (b)->tv_sec) ? ((a)->tv_usec < (b)->tv_usec) : ((a)->tv_sec < (b)->tv_sec))
1399 val = select(
_max_fd + 1, &fds,
nullptr,
nullptr, &remaining);
1407 LOG_DEBUG_V4L(
"Select interrupted, val = " <<
val <<
", error = " << errno);
1408 }
while (
val < 0 && errno == EINTR);
1432 LOG_ERROR(
"Stop pipe was signalled during streaming");
1438 bool md_extracted =
false;
1439 bool keep_md =
false;
1440 bool wa_applied =
false;
1445 md_extracted =
true;
1456 md_extracted =
true;
1465 if(FD_ISSET(
_fd, &fds))
1468 v4l2_buffer
buf = {};
1469 struct v4l2_plane planes[VIDEO_MAX_PLANES] = {};
1472 if (
_dev.
buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1473 buf.m.planes = planes;
1474 buf.length = VIDEO_MAX_PLANES;
1483 if (
_dev.
buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1484 buf.bytesused =
buf.m.planes[0].bytesused;
1491 if(
buf.bytesused == 0)
1500 if (
_dev.
buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1502 partial_frame =
false;
1503 overflow_frame =
false;
1505 if (partial_frame || overflow_frame)
1508 std::stringstream
s;
1511 s <<
"Incomplete video frame detected!\nSize " <<
buf.bytesused
1516 LOG_ERROR(
"Corrupted UVC frame data, underflow and overflow reported:\n" <<
s.str().c_str());
1522 s <<
"overflow video frame detected!\nSize " <<
buf.bytesused
1525 LOG_DEBUG(
"Incomplete frame received: " <<
s.str());
1536 LOG_WARNING(
"Metadata was present when partial frame arrived, mark md as extracted");
1537 md_extracted =
true;
1538 LOG_DEBUG_V4L(
"Discarding md due to invalid video payload");
1540 md_buf._data_buf->request_next_frame(md_buf._file_desc,
true);
1549 auto timestamp = (double)
buf.timestamp.tv_sec*1000.f + (
double)
buf.timestamp.tv_usec/1000.f;
1554 md_extracted =
true;
1579 LOG_WARNING(
"Video frame dropped, video and metadata buffers inconsistency");
1584 auto timestamp = (double)
buf.timestamp.tv_sec * 1000.f + (
double)
buf.timestamp.tv_usec / 1000.f;
1619 LOG_WARNING(
"Video frame dropped, video and metadata buffers inconsistency");
1640 LOG_DEBUG(
"FD_ISSET: no data on video node sink");
1649 LOG_WARNING(
"Frames didn't arrived within 5 seconds");
1667 *md_start = &meta_data;
1674 std::shared_ptr<v4l2_buffer> video_v4l2_buffer;
1675 std::shared_ptr<v4l2_buffer> md_v4l2_buffer;
1679 int video_fd = -1, md_fd = -1;
1684 video_buffer->attach_buffer(*video_v4l2_buffer);
1695 auto metadata_buffer =
get_md_buffer(md_v4l2_buffer->index);
1697 metadata_buffer->attach_buffer(*md_v4l2_buffer);
1705 auto frame_sz = buf_mgr.
md_node_present() ? video_v4l2_buffer->bytesused :
1707 video_buffer->get_length_frame_only());
1709 auto timestamp = (double)video_v4l2_buffer->timestamp.tv_sec * 1000.f + (
double)video_v4l2_buffer->timestamp.tv_usec / 1000.f;
1724 LOG_DEBUG(
"video_md_syncer - synchronized video and md could not be pulled");
1733 md_start + uvc_md_start_offset);
1745 std::string driver_str =
reinterpret_cast<char*
>(cap.driver);
1747 size_t pos = driver_str.find(
"tegra");
1748 return pos != std::string::npos;
1779 if (errno == EIO || errno == EAGAIN || errno == EBUSY)
1794 if (errno == EIO || errno == EAGAIN || errno == EBUSY)
1811 struct uvc_xu_control_query xquery = {};
1812 memset(&xquery, 0,
sizeof(xquery));
1818 xquery.unit = xu.
unit;
1819 xquery.data = (__u8 *)&
size;
1821 if(-1 == ioctl(
_fd,UVCIOC_CTRL_QUERY,&xquery)){
1827 std::vector<uint8_t>
buf;
1828 auto buf_size = std::max((
size_t)len,
sizeof(__u32));
1829 buf.resize(buf_size);
1834 xquery.unit = xu.
unit;
1835 xquery.data =
buf.data();
1836 if(-1 == ioctl(
_fd,UVCIOC_CTRL_QUERY,&xquery)){
1839 result.min.resize(buf_size);
1845 xquery.unit = xu.
unit;
1846 xquery.data =
buf.data();
1847 if(-1 == ioctl(
_fd,UVCIOC_CTRL_QUERY,&xquery)){
1850 result.max.resize(buf_size);
1856 xquery.unit = xu.
unit;
1857 xquery.data =
buf.data();
1858 if(-1 == ioctl(
_fd,UVCIOC_CTRL_QUERY,&xquery)){
1861 result.def.resize(buf_size);
1867 xquery.unit = xu.
unit;
1868 xquery.data =
buf.data();
1869 if(-1 == ioctl(
_fd,UVCIOC_CTRL_QUERY,&xquery)){
1872 result.step.resize(buf_size);
1873 std::copy(
buf.begin(),
buf.end(),
result.step.begin());
1883 if (errno == EIO || errno == EAGAIN || errno == EBUSY)
1911 auto local_id = *id;
1913 unsubscribe_from_ctrl_event( local_id );
1917 subscribe_to_ctrl_event(
control.id);
1922 if (errno == EIO || errno == EAGAIN || errno == EBUSY)
1928 if (!pend_for_ctrl_status_event())
1939 static const int32_t min = 0, max = 1, step = 1, def = 1;
1945 struct v4l2_queryctrl
query = {};
1962 std::vector<stream_profile> results;
1966 v4l2_fmtdesc pixel_format = {};
1968 while (ioctl(
_fd, VIDIOC_ENUM_FMT, &pixel_format) == 0)
1970 v4l2_frmsizeenum frame_size = {};
1971 frame_size.pixel_format = pixel_format.pixelformat;
1975 if (pixel_format.pixelformat == 0)
1980 std::set<std::string> known_problematic_formats = {
1981 "00000050-0000-0010-8000-00aa003",
1982 "00000032-0000-0010-8000-00aa003",
1985 if (
std::find(known_problematic_formats.begin(),
1986 known_problematic_formats.end(),
1987 (
const char*)pixel_format.description) ==
1988 known_problematic_formats.end())
1994 if (std::regex_search(
s.begin(),
s.end(), match, rgx))
1996 std::stringstream
ss;
1999 ss >> std::hex >>
id;
2003 LOG_WARNING(
"Pixel format " << pixel_format.description <<
" likely requires patch for fourcc code " << format_str <<
"!");
2009 LOG_DEBUG(
"Recognized pixel-format " << pixel_format.description);
2012 while (ioctl(
_fd, VIDIOC_ENUM_FRAMESIZES, &frame_size) == 0)
2014 v4l2_frmivalenum frame_interval = {};
2015 frame_interval.pixel_format = pixel_format.pixelformat;
2016 frame_interval.width = frame_size.discrete.width;
2017 frame_interval.height = frame_size.discrete.height;
2018 while (ioctl(
_fd, VIDIOC_ENUM_FRAMEINTERVALS, &frame_interval) == 0)
2020 if (frame_interval.type == V4L2_FRMIVAL_TYPE_DISCRETE)
2022 if (frame_interval.discrete.numerator != 0)
2025 static_cast<float>(frame_interval.discrete.denominator) /
2026 static_cast<float>(frame_interval.discrete.numerator);
2030 p.width = frame_size.discrete.width;
2031 p.height = frame_size.discrete.height;
2033 if (fourcc != 0) results.push_back(
p);
2037 ++frame_interval.index;
2043 ++pixel_format.index;
2088 catch (
const std::exception& ex)
2141 _fd = open(
_name.c_str(), O_RDWR | O_NONBLOCK, 0);
2151 _fds.insert(
_fds.end(),{_fd,_stop_pipe_fd[0],_stop_pipe_fd[1]});
2154 v4l2_capability cap = {};
2155 if(
xioctl(
_fd, VIDIOC_QUERYCAP, &cap) < 0)
2162 if(!(cap.capabilities & (V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_CAPTURE)))
2165 if(!(cap.capabilities & V4L2_CAP_STREAMING))
2171 if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) {
2173 }
else if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
2179 v4l2_cropcap cropcap = {};
2181 if(
xioctl(
_fd, VIDIOC_CROPCAP, &cropcap) == 0)
2183 v4l2_crop
crop = {};
2185 crop.c = cropcap.defrect;
2215 v4l2_format
fmt = {};
2217 if (
_dev.
buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
2221 fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
2223 fmt.fmt.pix_mp.flags = 0;
2225 for (
int i = 0;
i <
fmt.fmt.pix_mp.num_planes;
i++) {
2226 fmt.fmt.pix_mp.plane_fmt[
i].bytesperline = 0;
2227 fmt.fmt.pix_mp.plane_fmt[
i].sizeimage = 0;
2233 fmt.fmt.pix.field = V4L2_FIELD_NONE;
2247 struct v4l2_event_subscription event_subscription ;
2248 event_subscription.flags = V4L2_EVENT_SUB_FL_ALLOW_FEEDBACK;
2249 event_subscription.type = V4L2_EVENT_CTRL;
2250 event_subscription.id = control_id;
2251 memset(event_subscription.reserved,0,
sizeof(event_subscription.reserved));
2252 if (
xioctl(
_fd, VIDIOC_SUBSCRIBE_EVENT, &event_subscription) < 0)
2260 struct v4l2_event_subscription event_subscription ;
2261 event_subscription.flags = V4L2_EVENT_SUB_FL_ALLOW_FEEDBACK;
2262 event_subscription.type = V4L2_EVENT_CTRL;
2263 event_subscription.id = control_id;
2264 memset(event_subscription.reserved,0,
sizeof(event_subscription.reserved));
2265 if (
xioctl(
_fd, VIDIOC_UNSUBSCRIBE_EVENT, &event_subscription) < 0)
2274 struct v4l2_event event;
2275 memset(&event, 0 ,
sizeof(event));
2278 static int MAX_POLL_RETRIES = 10;
2279 for (
int i = 0 ;
i < MAX_POLL_RETRIES &&
event.type != V4L2_EVENT_CTRL ;
i++)
2281 if(
xioctl(
_fd, VIDIOC_DQEVENT, &event) < 0)
2283 std::this_thread::sleep_for(std::chrono::milliseconds(2));
2287 return event.type == V4L2_EVENT_CTRL;
2293 _md_name(
info.metadata_node_id)
2304 if ((
_md_fd != -1) && jetson_platform)
2315 if ((
_md_fd != -1) && !jetson_platform)
2326 if (jetson_platform)
2334 if (!jetson_platform)
2396 v4l2_capability cap = {};
2408 if(!(cap.capabilities & V4L2_CAP_STREAMING))
2450 memcpy(
fmt.fmt.raw_data, &request,
sizeof(request));
2455 struct v4l2_meta_format {
2463 memcpy(&meta,
fmt.fmt.raw_data,
sizeof(meta));
2465 meta.dataformat = request;
2468 memcpy(
fmt.fmt.raw_data, &meta,
sizeof(meta));
2510 LOG_WARNING(
"Metadata override requested but avoided skipped");
2518 md_buf._data_buf->request_next_frame(md_buf._file_desc,
true);
2534 auto hwts = *(
uint32_t*)((mdbuf+2));
2535 auto fn = *(
uint32_t*)((mdbuf+38));
2537 <<
" fn " << fn <<
" hw ts " << hwts
2538 <<
" v4lbuf ts usec " <<
buf.timestamp.tv_usec);
2576 v4l2_ext_controls ctrls_block {
control.id&0xffff0000, 1, 0, 0, 0, &
control};
2578 if (
xioctl(
_fd, VIDIOC_G_EXT_CTRLS, &ctrls_block) < 0)
2580 if (errno == EIO || errno == EAGAIN)
2597 control.value =
value ? V4L2_EXPOSURE_APERTURE_PRIORITY : V4L2_EXPOSURE_MANUAL;
2600 v4l2_ext_controls ctrls_block{
control.id & 0xffff0000, 1, 0, 0, 0, &
control };
2601 if (
xioctl(
_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block) < 0)
2603 if (errno == EIO || errno == EAGAIN)
2617 case 1: xctrl.value = *(
reinterpret_cast<const uint8_t*
>(
data));
break;
2618 case 2: xctrl.value = *
reinterpret_cast<const uint16_t*
>(
data);
break;
2619 case 4: xctrl.value = *
reinterpret_cast<const int32_t*
>(
data);
break;
2620 case 8: xctrl.value64 = *
reinterpret_cast<const int64_t*
>(
data);
break;
2626 xctrl.value = xctrl.value ? V4L2_EXPOSURE_APERTURE_PRIORITY : V4L2_EXPOSURE_MANUAL;
2629 v4l2_ext_controls ctrls_block { xctrl.id&0xffff0000, 1, 0, 0, 0, &xctrl };
2631 int retVal =
xioctl(
_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block);
2634 if (errno == EIO || errno == EAGAIN)
2647 v4l2_ext_controls ext {xctrl.id & 0xffff0000, 1, 0, 0, 0, &xctrl};
2654 int ret =
xioctl(
_fd, VIDIOC_G_EXT_CTRLS, &ext);
2662 xctrl.value = (V4L2_EXPOSURE_MANUAL == xctrl.value) ? 0 : 1;
2666 if (
size <
sizeof(__s64))
2667 memcpy(
data,(
void*)(&xctrl.value),
size);
2673 if (errno == EIO || errno == EAGAIN)
2680 v4l2_query_ext_ctrl xctrl_query{};
2683 if(0 > ioctl(
_fd,VIDIOC_QUERY_EXT_CTRL,&xctrl_query)){
2687 if ((xctrl_query.elems !=1 ) ||
2689 (xctrl_query.maximum > std::numeric_limits<int32_t>::max()))
2691 <<
" is not compliant with backend interface: [min,max,default,step]:\n"
2692 << xctrl_query.minimum <<
", " << xctrl_query.maximum <<
", "
2693 << xctrl_query.default_value <<
", " << xctrl_query.step
2694 <<
"\n Elements = " << xctrl_query.elems);
2697 return {0, 1, 1, 1};
2698 return {
static_cast<int32_t>(xctrl_query.minimum),
static_cast<int32_t>(xctrl_query.maximum),
2699 static_cast<int32_t>(xctrl_query.step),
static_cast<int32_t>(xctrl_query.default_value)};
2740 case RS_EXPOSURE:
return V4L2_CID_EXPOSURE_ABSOLUTE;
2761 bool mipi_device = 0xABCD ==
info.pid;
2762 auto v4l_uvc_dev = mipi_device ? std::make_shared<v4l_mipi_device>(
info) :
2763 ((!
info.has_metadata_node) ? std::make_shared<v4l_uvc_device>(
info) :
2764 std::make_shared<v4l_uvc_meta_device>(
info));
2766 return std::make_shared<platform::retry_controls_work_around>(v4l_uvc_dev);
2771 std::vector<uvc_device_info> uvc_nodes;
2775 uvc_nodes.push_back(
i);
2785 return std::make_shared<platform::command_transfer_usb>(
dev);
2792 return device_infos;
2797 return std::make_shared<v4l_hid_device>(
info);
2802 std::vector<hid_device_info> results;
2804 results.push_back(hid_dev_info);
2811 #if defined(USING_UDEV)
2812 return std::make_shared< udev_device_watcher >(
this );
2814 return std::make_shared< polling_device_watcher >(
this );
2820 return std::make_shared<v4l_backend>();
2828 LOG_DEBUG_V4L(
"video_md_syncer - push_video called but syncer not ready");
2847 LOG_DEBUG_V4L(
"video_md_syncer - push_metadata called but syncer not ready");
2853 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");
2863 LOG_DEBUG_V4L(
"video_md_syncer - calling enqueue_front_buffer_before_throwing_it - md queue size is: " <<
_md_queue.size());
2870 int& video_fd,
int& md_fd)
2875 LOG_DEBUG_V4L(
"video_md_syncer - pull_video_with_metadata called but syncer not ready");
2894 video_fd = video_candidate.
_fd;
2895 md_fd = md_candidate.
_fd;
2900 video_buffer = video_candidate.
_v4l2_buf;
2905 LOG_DEBUG_V4L(
"video_md_syncer - video and md pulled with sequence " << video_candidate.
_v4l2_buf->sequence);
2909 LOG_DEBUG_V4L(
"video_md_syncer - video_candidate seq " << video_candidate.
_v4l2_buf->sequence <<
", md_candidate seq " << md_candidate.
_v4l2_buf->sequence);
2918 auto alternative_md_candidate =
_md_queue.front();
2920 if (video_candidate.
_v4l2_buf->sequence == alternative_md_candidate._v4l2_buf->sequence)
2922 video_buffer = video_candidate.
_v4l2_buf;
2923 md_buffer = alternative_md_candidate._v4l2_buf;
2927 LOG_DEBUG_V4L(
"video_md_syncer - video and md pulled with sequence " << video_candidate.
_v4l2_buf->sequence);
2938 auto alternative_video_candidate =
_video_queue.front();
2940 if (alternative_video_candidate._v4l2_buf->sequence == md_candidate.
_v4l2_buf->sequence)
2942 video_buffer = alternative_video_candidate._v4l2_buf;
2947 LOG_DEBUG_V4L(
"video_md_syncer - video and md pulled with sequence " << md_candidate.
_v4l2_buf->sequence);
2960 LOG_ERROR(
"xioctl(VIDIOC_QBUF) failed when requesting new frame! fd: " << sb.
_fd <<
" error: " << strerror(errno));
2967 LOG_DEBUG_V4L(
"video_md_syncer - Enqueue buf " << std::dec << sync_queue.front()._buffer_index <<
" for fd " << sync_queue.front()._fd <<
" before dropping it");
2968 if (
xioctl(sync_queue.front()._fd, VIDIOC_QBUF, sync_queue.front()._v4l2_buf.get()) < 0)
2970 LOG_ERROR(
"xioctl(VIDIOC_QBUF) failed when requesting new frame! fd: " << sync_queue.front()._fd <<
" error: " << strerror(errno));
2985 LOG_DEBUG_V4L(
"video_md_syncer - flush video and md queues");