00001
00002
00003 #include <chrono>
00004 #include <iostream>
00005
00006
00007 #ifdef RS_USE_WMF_BACKEND
00008
00009 #if (_MSC_FULL_VER < 180031101)
00010 #error At least Visual Studio 2013 Update 4 is required to compile this backend
00011 #endif
00012
00013 #include <windows.h>
00014 #include <usbioctl.h>
00015 #include <sstream>
00016
00017 #include "uvc.h"
00018
00019 #include <Shlwapi.h>
00020 #include <mfapi.h>
00021 #include <mfidl.h>
00022 #include <mfreadwrite.h>
00023 #include <mferror.h>
00024 #include "hw-monitor.h"
00025
00026 #pragma comment(lib, "Shlwapi.lib")
00027 #pragma comment(lib, "mf.lib")
00028 #pragma comment(lib, "mfplat.lib")
00029 #pragma comment(lib, "mfreadwrite.lib")
00030 #pragma comment(lib, "mfuuid.lib")
00031
00032 #pragma comment(lib, "setupapi.lib")
00033 #pragma comment(lib, "winusb.lib")
00034
00035 #include <uuids.h>
00036 #include <vidcap.h>
00037 #include <ksmedia.h>
00038 #include <ksproxy.h>
00039
00040 #include <Cfgmgr32.h>
00041
00042 #pragma comment(lib, "cfgmgr32.lib")
00043
00044 #include <SetupAPI.h>
00045 #include <WinUsb.h>
00046
00047 #include <functional>
00048 #include <thread>
00049 #include <chrono>
00050 #include <algorithm>
00051 #include <regex>
00052 #include <map>
00053
00054 #include <strsafe.h>
00055
00056 DEFINE_GUID(GUID_DEVINTERFACE_USB_DEVICE, 0xA5DCBF10L, 0x6530, 0x11D2, 0x90, 0x1F, 0x00, \
00057 0xC0, 0x4F, 0xB9, 0x51, 0xED);
00058 DEFINE_GUID(GUID_DEVINTERFACE_IMAGE, 0x6bdd1fc6L, 0x810f, 0x11d0, 0xbe, 0xc7, 0x08, 0x00, \
00059 0x2b, 0xe2, 0x09, 0x2f);
00060
00061 namespace rsimpl
00062 {
00063 namespace uvc
00064 {
00065 const auto FISHEYE_HWMONITOR_INTERFACE = 2;
00066 const uvc::guid FISHEYE_WIN_USB_DEVICE_GUID = { 0xC0B55A29, 0xD7B6, 0x436E, { 0xA6, 0xEF, 0x2E, 0x76, 0xED, 0x0A, 0xBC, 0xA5 } };
00067
00068 const std::map<uint32_t, uint32_t> fourcc_map = { { 0x59382020, 0x47524559 },
00069 { 0x494e5649, 0x59313020 },
00070 { 0x494e565a, 0x5a313620 },
00071 { 0x52573130, 0x70524141 } };
00072
00073 static std::string win_to_utf(const WCHAR * s)
00074 {
00075 int len = WideCharToMultiByte(CP_UTF8, 0, s, -1, nullptr, 0, NULL, NULL);
00076 if(len == 0) throw std::runtime_error(to_string() << "WideCharToMultiByte(...) returned 0 and GetLastError() is " << GetLastError());
00077 std::string buffer(len-1, ' ');
00078 len = WideCharToMultiByte(CP_UTF8, 0, s, -1, &buffer[0], (int)buffer.size()+1, NULL, NULL);
00079 if(len == 0) throw std::runtime_error(to_string() << "WideCharToMultiByte(...) returned 0 and GetLastError() is " << GetLastError());
00080 return buffer;
00081 }
00082
00083 static void check(const char * call, HRESULT hr)
00084 {
00085 if(FAILED(hr)) throw std::runtime_error(to_string() << call << "(...) returned 0x" << std::hex << (uint32_t)hr);
00086 }
00087
00088 template<class T> class com_ptr
00089 {
00090 T * p;
00091
00092 void ref(T * new_p)
00093 {
00094 if(p == new_p) return;
00095 unref();
00096 p = new_p;
00097 if(p) p->AddRef();
00098 }
00099
00100 void unref()
00101 {
00102 if(p)
00103 {
00104 p->Release();
00105 p = nullptr;
00106 }
00107 }
00108 public:
00109 com_ptr() : p() {}
00110 com_ptr(T * p) : com_ptr() { ref(p); }
00111 com_ptr(const com_ptr & r) : com_ptr(r.p) {}
00112 ~com_ptr() { unref(); }
00113
00114 operator T * () const { return p; }
00115 T & operator * () const { return *p; }
00116 T * operator -> () const { return p; }
00117
00118 T ** operator & () { unref(); return &p; }
00119 com_ptr & operator = (const com_ptr & r) { ref(r.p); return *this; }
00120 };
00121
00122 std::vector<std::string> tokenize(std::string string, char separator)
00123 {
00124 std::vector<std::string> tokens;
00125 std::string::size_type i1 = 0;
00126 while(true)
00127 {
00128 auto i2 = string.find(separator, i1);
00129 if(i2 == std::string::npos)
00130 {
00131 tokens.push_back(string.substr(i1));
00132 return tokens;
00133 }
00134 tokens.push_back(string.substr(i1, i2-i1));
00135 i1 = i2+1;
00136 }
00137 }
00138
00139 bool parse_usb_path(int & vid, int & pid, int & mi, std::string & unique_id, const std::string & path)
00140 {
00141 auto name = path;
00142 std::transform(begin(name), end(name), begin(name), ::tolower);
00143 auto tokens = tokenize(name, '#');
00144 if(tokens.size() < 1 || tokens[0] != R"(\\?\usb)") return false;
00145 if(tokens.size() < 3)
00146 {
00147 LOG_ERROR("malformed usb device path: " << name);
00148 return false;
00149 }
00150
00151 auto ids = tokenize(tokens[1], '&');
00152 if(ids[0].size() != 8 || ids[0].substr(0,4) != "vid_" || !(std::istringstream(ids[0].substr(4,4)) >> std::hex >> vid))
00153 {
00154 LOG_ERROR("malformed vid string: " << tokens[1]);
00155 return false;
00156 }
00157
00158 if(ids[1].size() != 8 || ids[1].substr(0,4) != "pid_" || !(std::istringstream(ids[1].substr(4,4)) >> std::hex >> pid))
00159 {
00160 LOG_ERROR("malformed pid string: " << tokens[1]);
00161 return false;
00162 }
00163
00164 if(ids[2].size() != 5 || ids[2].substr(0,3) != "mi_" || !(std::istringstream(ids[2].substr(3,2)) >> mi))
00165 {
00166 LOG_ERROR("malformed mi string: " << tokens[1]);
00167 return false;
00168 }
00169
00170 ids = tokenize(tokens[2], '&');
00171 if(ids.size() < 2)
00172 {
00173 LOG_ERROR("malformed id string: " << tokens[2]);
00174 return false;
00175 }
00176 unique_id = ids[1];
00177 return true;
00178 }
00179
00180 bool parse_usb_path_from_device_id(int & vid, int & pid, int & mi, std::string & unique_id, const std::string & device_id)
00181 {
00182 auto name = device_id;
00183 std::transform(begin(name), end(name), begin(name), ::tolower);
00184 auto tokens = tokenize(name, '\\');
00185 if (tokens.size() < 1 || tokens[0] != R"(usb)") return false;
00186
00187 auto ids = tokenize(tokens[1], '&');
00188 if (ids[0].size() != 8 || ids[0].substr(0, 4) != "vid_" || !(std::istringstream(ids[0].substr(4, 4)) >> std::hex >> vid))
00189 {
00190 LOG_ERROR("malformed vid string: " << tokens[1]);
00191 return false;
00192 }
00193
00194 if (ids[1].size() != 8 || ids[1].substr(0, 4) != "pid_" || !(std::istringstream(ids[1].substr(4, 4)) >> std::hex >> pid))
00195 {
00196 LOG_ERROR("malformed pid string: " << tokens[1]);
00197 return false;
00198 }
00199
00200 if (ids[2].size() != 5 || ids[2].substr(0, 3) != "mi_" || !(std::istringstream(ids[2].substr(3, 2)) >> mi))
00201 {
00202 LOG_ERROR("malformed mi string: " << tokens[1]);
00203 return false;
00204 }
00205
00206 ids = tokenize(tokens[2], '&');
00207 if (ids.size() < 2)
00208 {
00209 LOG_ERROR("malformed id string: " << tokens[2]);
00210 return false;
00211 }
00212 unique_id = ids[1];
00213 return true;
00214 }
00215
00216
00217 struct context
00218 {
00219 context()
00220 {
00221 CoInitializeEx(NULL, COINIT_APARTMENTTHREADED);
00222 std::this_thread::sleep_for(std::chrono::milliseconds(100));
00223 MFStartup(MF_VERSION, MFSTARTUP_NOSOCKET);
00224 }
00225 ~context()
00226 {
00227 MFShutdown();
00228 CoUninitialize();
00229 }
00230 };
00231
00232 class reader_callback : public IMFSourceReaderCallback
00233 {
00234 std::weak_ptr<device> owner;
00235 int subdevice_index;
00236 ULONG ref_count;
00237 volatile bool streaming = false;
00238 public:
00239 reader_callback(std::weak_ptr<device> owner, int subdevice_index) : owner(owner), subdevice_index(subdevice_index), ref_count() {}
00240
00241 bool is_streaming() const { return streaming; }
00242 void on_start() { streaming = true; }
00243
00244 #pragma warning( push )
00245 #pragma warning( disable: 4838 )
00246
00247 HRESULT STDMETHODCALLTYPE QueryInterface(REFIID riid, void ** ppvObject) override
00248 {
00249 static const QITAB table[] = {QITABENT(reader_callback, IUnknown), QITABENT(reader_callback, IMFSourceReaderCallback), {0}};
00250 return QISearch(this, table, riid, ppvObject);
00251 }
00252 #pragma warning( pop )
00253
00254 ULONG STDMETHODCALLTYPE AddRef() override { return InterlockedIncrement(&ref_count); }
00255 ULONG STDMETHODCALLTYPE Release() override
00256 {
00257 ULONG count = InterlockedDecrement(&ref_count);
00258 if(count == 0) delete this;
00259 return count;
00260 }
00261
00262
00263 HRESULT STDMETHODCALLTYPE OnReadSample(HRESULT hrStatus, DWORD dwStreamIndex, DWORD dwStreamFlags, LONGLONG llTimestamp, IMFSample * sample) override;
00264 HRESULT STDMETHODCALLTYPE OnFlush(DWORD dwStreamIndex) override { streaming = false; return S_OK; }
00265 HRESULT STDMETHODCALLTYPE OnEvent(DWORD dwStreamIndex, IMFMediaEvent *pEvent) override { return S_OK; }
00266 };
00267
00268 struct subdevice
00269 {
00270 com_ptr<reader_callback> reader_callback;
00271 com_ptr<IMFActivate> mf_activate;
00272 com_ptr<IMFMediaSource> mf_media_source;
00273 com_ptr<IAMCameraControl> am_camera_control;
00274 com_ptr<IAMVideoProcAmp> am_video_proc_amp;
00275 std::map<int, com_ptr<IKsControl>> ks_controls;
00276 com_ptr<IMFSourceReader> mf_source_reader;
00277 video_channel_callback callback = nullptr;
00278 data_channel_callback channel_data_callback = nullptr;
00279 int vid, pid;
00280
00281 void set_data_channel_cfg(data_channel_callback callback)
00282 {
00283 this->channel_data_callback = callback;
00284 }
00285
00286 com_ptr<IMFMediaSource> get_media_source()
00287 {
00288 if(!mf_media_source)
00289 {
00290 check("IMFActivate::ActivateObject", mf_activate->ActivateObject(__uuidof(IMFMediaSource), (void **)&mf_media_source));
00291 if (mf_media_source)
00292 {
00293 check("IMFMediaSource::QueryInterface", mf_media_source->QueryInterface(__uuidof(IAMCameraControl), (void **)&am_camera_control));
00294 if (SUCCEEDED(mf_media_source->QueryInterface(__uuidof(IAMVideoProcAmp), (void **)&am_video_proc_amp))) LOG_DEBUG("obtained IAMVideoProcAmp");
00295 }
00296 else throw std::runtime_error(to_string() << "Invalid media source");
00297 }
00298 return mf_media_source;
00299
00300 }
00301
00302 static bool wait_for_async_operation(WINUSB_INTERFACE_HANDLE interfaceHandle, OVERLAPPED &hOvl, ULONG &lengthTransferred, USHORT timeout)
00303 {
00304 if (GetOverlappedResult(interfaceHandle, &hOvl, &lengthTransferred, FALSE))
00305 return true;
00306
00307 auto lastResult = GetLastError();
00308 if (lastResult == ERROR_IO_PENDING || lastResult == ERROR_IO_INCOMPLETE)
00309 {
00310 WaitForSingleObject(hOvl.hEvent, timeout);
00311 auto res = GetOverlappedResult(interfaceHandle, &hOvl, &lengthTransferred, FALSE);
00312 if (res != 1)
00313 {
00314 return false;
00315 }
00316 }
00317 else
00318 {
00319 lengthTransferred = 0;
00320 WinUsb_ResetPipe(interfaceHandle, 0x84);
00321 return false;
00322 }
00323
00324 return true;
00325 }
00326
00327 class safe_handle
00328 {
00329 public:
00330 safe_handle(HANDLE handle) :_handle(handle)
00331 {
00332
00333 }
00334
00335 ~safe_handle()
00336 {
00337 if (_handle != nullptr)
00338 {
00339 CloseHandle(_handle);
00340 _handle = nullptr;
00341 }
00342 }
00343
00344 bool Set()
00345 {
00346 if (_handle == nullptr) return false;
00347 SetEvent(_handle);
00348 return true;
00349 }
00350
00351 bool Wait(DWORD timeout) const
00352 {
00353 if (_handle == nullptr) return false;
00354
00355 return WaitForSingleObject(_handle, timeout) == WAIT_OBJECT_0;
00356 }
00357
00358
00359 HANDLE GetHandle() const { return _handle; }
00360 private:
00361 safe_handle() = delete;
00362
00363
00364 safe_handle(const safe_handle&) = delete;
00365 safe_handle& operator=(const safe_handle&) = delete;
00366
00367 HANDLE _handle;
00368 };
00369
00370 static void poll_interrupts(HANDLE *handle, const std::vector<subdevice *> & subdevices,uint16_t timeout)
00371 {
00372 static const unsigned short interrupt_buf_size = 0x400;
00373 uint8_t buffer[interrupt_buf_size];
00374 ULONG num_bytes = 0;
00375 OVERLAPPED hOvl;
00376 safe_handle sh(CreateEvent(nullptr, false, false, nullptr));
00377 hOvl.hEvent = sh.GetHandle();
00378
00379 int res = WinUsb_ReadPipe(*handle, 0x84, buffer, interrupt_buf_size, &num_bytes, &hOvl);
00380 if (0 == res)
00381 {
00382 auto lastError = GetLastError();
00383 if (lastError == ERROR_IO_PENDING)
00384 {
00385 auto sts = wait_for_async_operation(*handle, hOvl, num_bytes, timeout);
00386 lastError = GetLastError();
00387 if (lastError == ERROR_OPERATION_ABORTED)
00388 {
00389 perror("receiving interrupt_ep bytes failed");
00390 fprintf(stderr, "Error receiving message.\n");
00391 }
00392 if (!sts)
00393 return;
00394 }
00395 else
00396 {
00397 WinUsb_ResetPipe(*handle, 0x84);
00398 perror("receiving interrupt_ep bytes failed");
00399 fprintf(stderr, "Error receiving message.\n");
00400 return;
00401 }
00402
00403 if (num_bytes == 0)
00404 return;
00405
00406
00407 for(auto & sub : subdevices)
00408 if (sub->channel_data_callback)
00409 sub->channel_data_callback(buffer, (unsigned long)num_bytes);
00410 }
00411 else
00412 {
00413
00414 perror("receiving interrupt_ep bytes failed");
00415 fprintf(stderr, "Error receiving message.\n");
00416 }
00417 }
00418
00419 IKsControl * get_ks_control(const uvc::extension_unit & xu)
00420 {
00421 auto it = ks_controls.find(xu.node);
00422 if(it != end(ks_controls)) return it->second;
00423
00424 get_media_source();
00425
00426
00427 com_ptr<IKsTopologyInfo> ks_topology_info = NULL;
00428 check("QueryInterface", mf_media_source->QueryInterface(__uuidof(IKsTopologyInfo), (void **)&ks_topology_info));
00429
00430 GUID node_type;
00431 check("get_NodeType", ks_topology_info->get_NodeType(xu.node, &node_type));
00432 const GUID KSNODETYPE_DEV_SPECIFIC_LOCAL{0x941C7AC0L, 0xC559, 0x11D0, {0x8A, 0x2B, 0x00, 0xA0, 0xC9, 0x25, 0x5A, 0xC1}};
00433 if(node_type != KSNODETYPE_DEV_SPECIFIC_LOCAL) throw std::runtime_error(to_string() << "Invalid extension unit node ID: " << xu.node);
00434
00435 com_ptr<IUnknown> unknown;
00436 check("CreateNodeInstance", ks_topology_info->CreateNodeInstance(xu.node, IID_IUnknown, (LPVOID *)&unknown));
00437
00438 com_ptr<IKsControl> ks_control;
00439 check("QueryInterface", unknown->QueryInterface(__uuidof(IKsControl), (void **)&ks_control));
00440 LOG_INFO("Obtained KS control node " << xu.node);
00441 return ks_controls[xu.node] = ks_control;
00442 }
00443 };
00444
00445 struct device
00446 {
00447 const std::shared_ptr<context> parent;
00448 const int vid, pid;
00449 const std::string unique_id;
00450
00451 std::vector<subdevice> subdevices;
00452
00453 HANDLE usb_file_handle = INVALID_HANDLE_VALUE;
00454 WINUSB_INTERFACE_HANDLE usb_interface_handle = INVALID_HANDLE_VALUE;
00455
00456 std::vector<int> claimed_interfaces;
00457
00458 int aux_vid, aux_pid;
00459 std::string aux_unique_id;
00460 std::thread data_channel_thread;
00461 volatile bool data_stop;
00462
00463 device(std::shared_ptr<context> parent, int vid, int pid, std::string unique_id) : parent(move(parent)), vid(vid), pid(pid), unique_id(move(unique_id)), aux_pid(0), aux_vid(0), data_stop(false)
00464 {
00465 }
00466
00467 ~device() { stop_streaming(); stop_data_acquisition(); close_win_usb(); }
00468
00469 IKsControl * get_ks_control(const uvc::extension_unit & xu)
00470 {
00471 return subdevices[xu.subdevice].get_ks_control(xu);
00472 }
00473
00474 void start_data_acquisition()
00475 {
00476 std::vector<subdevice *> data_channel_subs;
00477 for (auto & sub : subdevices)
00478 {
00479 if (sub.channel_data_callback)
00480 {
00481 data_channel_subs.push_back(&sub);
00482 }
00483 }
00484
00485 if (claimed_interfaces.size())
00486 {
00487 data_channel_thread = std::thread([this, data_channel_subs]()
00488 {
00489
00490 while (!data_stop)
00491 {
00492 subdevice::poll_interrupts(&usb_interface_handle, data_channel_subs, 100);
00493 }
00494 });
00495 }
00496 }
00497
00498 void stop_data_acquisition()
00499 {
00500 if (data_channel_thread.joinable())
00501 {
00502 data_stop = true;
00503 data_channel_thread.join();
00504 data_stop = false;
00505 }
00506 }
00507
00508 void start_streaming()
00509 {
00510 for(auto & sub : subdevices)
00511 {
00512 if(sub.mf_source_reader)
00513 {
00514 sub.reader_callback->on_start();
00515 check("IMFSourceReader::ReadSample", sub.mf_source_reader->ReadSample(MF_SOURCE_READER_FIRST_VIDEO_STREAM, 0, NULL, NULL, NULL, NULL));
00516 }
00517 }
00518 }
00519
00520 void stop_streaming()
00521 {
00522 for(auto & sub : subdevices)
00523 {
00524 if(sub.mf_source_reader) sub.mf_source_reader->Flush(MF_SOURCE_READER_FIRST_VIDEO_STREAM);
00525 }
00526 while(true)
00527 {
00528 bool is_streaming = false;
00529 for(auto & sub : subdevices) is_streaming |= sub.reader_callback->is_streaming();
00530 if(is_streaming) std::this_thread::sleep_for(std::chrono::milliseconds(10));
00531 else break;
00532 }
00533
00534
00535 for(auto & sub : subdevices)
00536 {
00537 sub.mf_source_reader = nullptr;
00538 sub.am_camera_control = nullptr;
00539 sub.am_video_proc_amp = nullptr;
00540 sub.ks_controls.clear();
00541 if(sub.mf_media_source)
00542 {
00543 sub.mf_media_source = nullptr;
00544 check("IMFActivate::ShutdownObject", sub.mf_activate->ShutdownObject());
00545 }
00546 sub.callback = {};
00547 }
00548 }
00549
00550 com_ptr<IMFMediaSource> get_media_source(int subdevice_index)
00551 {
00552 return subdevices[subdevice_index].get_media_source();
00553 }
00554
00555 void open_win_usb(int vid, int pid, std::string unique_id, const guid & interface_guid, int interface_number) try
00556 {
00557 static_assert(sizeof(guid) == sizeof(GUID), "struct packing error");
00558 HDEVINFO device_info = SetupDiGetClassDevs((const GUID *)&interface_guid, nullptr, nullptr, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE);
00559 if (device_info == INVALID_HANDLE_VALUE) throw std::runtime_error("SetupDiGetClassDevs");
00560 auto di = std::shared_ptr<void>(device_info, SetupDiDestroyDeviceInfoList);
00561
00562 for(int member_index = 0; ; ++member_index)
00563 {
00564
00565 SP_DEVICE_INTERFACE_DATA interfaceData = {sizeof(SP_DEVICE_INTERFACE_DATA)};
00566 if(SetupDiEnumDeviceInterfaces(device_info, nullptr, (const GUID *)&interface_guid, member_index, &interfaceData) == FALSE)
00567 {
00568 if(GetLastError() == ERROR_NO_MORE_ITEMS) break;
00569 continue;
00570 }
00571
00572
00573 unsigned long detail_data_size = 0;
00574 SetupDiGetDeviceInterfaceDetail(device_info, &interfaceData, nullptr, 0, &detail_data_size, nullptr);
00575 if(GetLastError() != ERROR_INSUFFICIENT_BUFFER)
00576 {
00577 LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed");
00578 continue;
00579 }
00580 auto alloc = std::malloc(detail_data_size);
00581 if(!alloc) throw std::bad_alloc();
00582
00583
00584 auto detail_data = std::shared_ptr<SP_DEVICE_INTERFACE_DETAIL_DATA>(reinterpret_cast<SP_DEVICE_INTERFACE_DETAIL_DATA *>(alloc), std::free);
00585 detail_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);
00586 if (!SetupDiGetDeviceInterfaceDetail(device_info, &interfaceData, detail_data.get(), detail_data_size, nullptr, nullptr))
00587 {
00588 LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed");
00589 continue;
00590 }
00591 if (detail_data->DevicePath == nullptr) continue;
00592
00593 int usb_vid, usb_pid, usb_mi; std::string usb_unique_id;
00594 if(!parse_usb_path(usb_vid, usb_pid, usb_mi, usb_unique_id, win_to_utf(detail_data->DevicePath))) continue;
00595 if(usb_vid != vid || usb_pid != pid || usb_mi != interface_number || usb_unique_id != unique_id) continue;
00596
00597 HANDLE* file_handle = nullptr;
00598 WINUSB_INTERFACE_HANDLE* usb_handle = nullptr;
00599
00600 file_handle = &usb_file_handle;
00601 usb_handle = &usb_interface_handle;
00602
00603 *file_handle = CreateFile(detail_data->DevicePath, GENERIC_WRITE | GENERIC_READ, FILE_SHARE_WRITE | FILE_SHARE_READ, nullptr, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, nullptr);
00604 if (*file_handle == INVALID_HANDLE_VALUE) throw std::runtime_error("CreateFile(...) failed");
00605
00606 if (!WinUsb_Initialize(*file_handle, usb_handle))
00607 {
00608 LOG_ERROR("Last Error: " << GetLastError());
00609 throw std::runtime_error("could not initialize winusb");
00610 }
00611
00612
00613 return;
00614 }
00615 throw std::runtime_error("Unable to open device via WinUSB");
00616 }
00617 catch(...)
00618 {
00619 close_win_usb();
00620 throw;
00621 }
00622
00623 void close_win_usb()
00624 {
00625 if (usb_interface_handle != INVALID_HANDLE_VALUE)
00626 {
00627 WinUsb_Free(usb_interface_handle);
00628 usb_interface_handle = INVALID_HANDLE_VALUE;
00629 }
00630
00631 if(usb_file_handle != INVALID_HANDLE_VALUE)
00632 {
00633 CloseHandle(usb_file_handle);
00634 usb_file_handle = INVALID_HANDLE_VALUE;
00635 }
00636 }
00637
00638 bool usb_synchronous_read(uint8_t endpoint, void * buffer, int bufferLength, int * actual_length, DWORD TimeOut)
00639 {
00640 if (usb_interface_handle == INVALID_HANDLE_VALUE) throw std::runtime_error("winusb has not been initialized");
00641
00642 auto result = false;
00643
00644 BOOL bRetVal = true;
00645
00646 ULONG lengthTransferred;
00647
00648 bRetVal = WinUsb_ReadPipe(usb_interface_handle, endpoint, (PUCHAR)buffer, bufferLength, &lengthTransferred, NULL);
00649
00650 if (bRetVal)
00651 result = true;
00652 else
00653 {
00654 auto lastResult = GetLastError();
00655 WinUsb_ResetPipe(usb_interface_handle, endpoint);
00656 result = false;
00657 }
00658
00659 *actual_length = lengthTransferred;
00660 return result;
00661 }
00662
00663 bool usb_synchronous_write(uint8_t endpoint, void * buffer, int bufferLength, DWORD TimeOut)
00664 {
00665 if (usb_interface_handle == INVALID_HANDLE_VALUE) throw std::runtime_error("winusb has not been initialized");
00666
00667 auto result = false;
00668
00669 ULONG lengthWritten;
00670 auto bRetVal = WinUsb_WritePipe(usb_interface_handle, endpoint, (PUCHAR)buffer, bufferLength, &lengthWritten, NULL);
00671 if (bRetVal)
00672 result = true;
00673 else
00674 {
00675 auto lastError = GetLastError();
00676 WinUsb_ResetPipe(usb_interface_handle, endpoint);
00677 LOG_ERROR("WinUsb_ReadPipe failure... lastError: " << lastError);
00678 result = false;
00679 }
00680
00681 return result;
00682 }
00683 };
00684
00685 HRESULT reader_callback::OnReadSample(HRESULT hrStatus, DWORD dwStreamIndex, DWORD dwStreamFlags, LONGLONG llTimestamp, IMFSample * sample)
00686 {
00687 if(auto owner_ptr = owner.lock())
00688 {
00689 if(sample)
00690 {
00691 com_ptr<IMFMediaBuffer> buffer = NULL;
00692 if(SUCCEEDED(sample->GetBufferByIndex(0, &buffer)))
00693 {
00694 BYTE * byte_buffer; DWORD max_length, current_length;
00695 if(SUCCEEDED(buffer->Lock(&byte_buffer, &max_length, ¤t_length)))
00696 {
00697 auto continuation = [buffer, this]()
00698 {
00699 buffer->Unlock();
00700 };
00701
00702 owner_ptr->subdevices[subdevice_index].callback(byte_buffer, continuation);
00703 }
00704 }
00705 }
00706
00707 if (auto owner_ptr_new = owner.lock())
00708 {
00709 auto hr = owner_ptr_new->subdevices[subdevice_index].mf_source_reader->ReadSample(MF_SOURCE_READER_FIRST_VIDEO_STREAM, 0, NULL, NULL, NULL, NULL);
00710 switch (hr)
00711 {
00712 case S_OK: break;
00713 case MF_E_INVALIDREQUEST: LOG_ERROR("ReadSample returned MF_E_INVALIDREQUEST"); break;
00714 case MF_E_INVALIDSTREAMNUMBER: LOG_ERROR("ReadSample returned MF_E_INVALIDSTREAMNUMBER"); break;
00715 case MF_E_NOTACCEPTING: LOG_ERROR("ReadSample returned MF_E_NOTACCEPTING"); break;
00716 case E_INVALIDARG: LOG_ERROR("ReadSample returned E_INVALIDARG"); break;
00717 case MF_E_VIDEO_RECORDING_DEVICE_INVALIDATED: LOG_ERROR("ReadSample returned MF_E_VIDEO_RECORDING_DEVICE_INVALIDATED"); break;
00718 default: LOG_ERROR("ReadSample returned HRESULT " << std::hex << (uint32_t)hr); break;
00719 }
00720 if (hr != S_OK) streaming = false;
00721 }
00722 }
00723 return S_OK;
00724 }
00725
00727
00729
00730 int get_vendor_id(const device & device) { return device.vid; }
00731 int get_product_id(const device & device) { return device.pid; }
00732
00733 void get_control(const device & device, const extension_unit & xu, uint8_t ctrl, void *data, int len)
00734 {
00735 auto ks_control = const_cast<uvc::device &>(device).get_ks_control(xu);
00736
00737 KSP_NODE node;
00738 memset(&node, 0, sizeof(KSP_NODE));
00739 node.Property.Set = reinterpret_cast<const GUID &>(xu.id);
00740 node.Property.Id = ctrl;
00741 node.Property.Flags = KSPROPERTY_TYPE_GET | KSPROPERTY_TYPE_TOPOLOGY;
00742 node.NodeId = xu.node;
00743
00744 ULONG bytes_received = 0;
00745 check("IKsControl::KsProperty", ks_control->KsProperty((PKSPROPERTY)&node, sizeof(node), data, len, &bytes_received));
00746 if(bytes_received != len) throw std::runtime_error("XU read did not return enough data");
00747 }
00748
00749 void set_control(device & device, const extension_unit & xu, uint8_t ctrl, void *data, int len)
00750 {
00751 auto ks_control = device.get_ks_control(xu);
00752
00753 KSP_NODE node;
00754 memset(&node, 0, sizeof(KSP_NODE));
00755 node.Property.Set = reinterpret_cast<const GUID &>(xu.id);
00756 node.Property.Id = ctrl;
00757 node.Property.Flags = KSPROPERTY_TYPE_SET | KSPROPERTY_TYPE_TOPOLOGY;
00758 node.NodeId = xu.node;
00759
00760 ULONG bytes_received = 0;
00761 check("IKsControl::KsProperty", ks_control->KsProperty((PKSPROPERTY)&node, sizeof(KSP_NODE), data, len, &bytes_received));
00762 }
00763
00764 void claim_interface(device & device, const guid & interface_guid, int interface_number)
00765 {
00766 device.open_win_usb(device.vid, device.pid, device.unique_id, interface_guid, interface_number);
00767 device.claimed_interfaces.push_back(interface_number);
00768 }
00769
00770 void claim_aux_interface(device & device, const guid & interface_guid, int interface_number)
00771 {
00772 device.open_win_usb(device.aux_vid, device.aux_pid, device.aux_unique_id, interface_guid, interface_number);
00773 device.claimed_interfaces.push_back(interface_number);
00774 }
00775
00776 void bulk_transfer(device & device, uint8_t endpoint, void * data, int length, int *actual_length, unsigned int timeout)
00777 {
00778 if(USB_ENDPOINT_DIRECTION_OUT(endpoint))
00779 {
00780 device.usb_synchronous_write(endpoint, data, length, timeout);
00781 }
00782
00783 if(USB_ENDPOINT_DIRECTION_IN(endpoint))
00784 {
00785 device.usb_synchronous_read(endpoint, data, length, actual_length, timeout);
00786 }
00787 }
00788
00789 void set_subdevice_mode(device & device, int subdevice_index, int width, int height, uint32_t fourcc, int fps, video_channel_callback callback)
00790 {
00791 auto & sub = device.subdevices[subdevice_index];
00792
00793 if(!sub.mf_source_reader)
00794 {
00795 com_ptr<IMFAttributes> pAttributes;
00796 check("MFCreateAttributes", MFCreateAttributes(&pAttributes, 1));
00797 check("IMFAttributes::SetUnknown", pAttributes->SetUnknown(MF_SOURCE_READER_ASYNC_CALLBACK, static_cast<IUnknown *>(sub.reader_callback)));
00798 check("MFCreateSourceReaderFromMediaSource", MFCreateSourceReaderFromMediaSource(sub.get_media_source(), pAttributes, &sub.mf_source_reader));
00799 }
00800
00801 for (DWORD j = 0; ; j++)
00802 {
00803 com_ptr<IMFMediaType> media_type;
00804 HRESULT hr = sub.mf_source_reader->GetNativeMediaType((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, j, &media_type);
00805 if (hr == MF_E_NO_MORE_TYPES) break;
00806 check("IMFSourceReader::GetNativeMediaType", hr);
00807
00808 UINT32 uvc_width, uvc_height, uvc_fps_num, uvc_fps_denom; GUID subtype;
00809 check("MFGetAttributeSize", MFGetAttributeSize(media_type, MF_MT_FRAME_SIZE, &uvc_width, &uvc_height));
00810 if(uvc_width != width || uvc_height != height) continue;
00811
00812 check("IMFMediaType::GetGUID", media_type->GetGUID(MF_MT_SUBTYPE, &subtype));
00813 uint32_t device_fourcc = reinterpret_cast<const big_endian<uint32_t> &>(subtype.Data1);
00814 if ((device_fourcc != fourcc) && (!fourcc_map.count(device_fourcc) || (fourcc != fourcc_map.at(device_fourcc)))) continue;
00815
00816 check("MFGetAttributeRatio", MFGetAttributeRatio(media_type, MF_MT_FRAME_RATE, &uvc_fps_num, &uvc_fps_denom));
00817 if(uvc_fps_denom == 0) continue;
00818 int uvc_fps = uvc_fps_num / uvc_fps_denom;
00819 if(std::abs(fps - uvc_fps) > 1) continue;
00820
00821 check("IMFSourceReader::SetCurrentMediaType", sub.mf_source_reader->SetCurrentMediaType((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, NULL, media_type));
00822 sub.callback = callback;
00823 return;
00824 }
00825 throw std::runtime_error(to_string() << "no matching media type for pixel format " << std::hex << fourcc);
00826 }
00827
00828 void set_subdevice_data_channel_handler(device & device, int subdevice_index, data_channel_callback callback)
00829 {
00830 device.subdevices[subdevice_index].set_data_channel_cfg(callback);
00831 }
00832
00833 void start_streaming(device & device, int num_transfer_bufs) { device.start_streaming(); }
00834 void stop_streaming(device & device) { device.stop_streaming(); }
00835
00836 void start_data_acquisition(device & device)
00837 {
00838 device.start_data_acquisition();
00839 }
00840
00841 void stop_data_acquisition(device & device)
00842 {
00843 device.stop_data_acquisition();
00844 }
00845
00846 struct pu_control { rs_option option; long property; bool enable_auto; };
00847 static const pu_control pu_controls[] = {
00848 {RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, VideoProcAmp_BacklightCompensation},
00849 {RS_OPTION_COLOR_BRIGHTNESS, VideoProcAmp_Brightness},
00850 {RS_OPTION_COLOR_CONTRAST, VideoProcAmp_Contrast},
00851 {RS_OPTION_COLOR_GAIN, VideoProcAmp_Gain},
00852 {RS_OPTION_COLOR_GAMMA, VideoProcAmp_Gamma},
00853 {RS_OPTION_COLOR_HUE, VideoProcAmp_Hue},
00854 {RS_OPTION_COLOR_SATURATION, VideoProcAmp_Saturation},
00855 {RS_OPTION_COLOR_SHARPNESS, VideoProcAmp_Sharpness},
00856 {RS_OPTION_COLOR_WHITE_BALANCE, VideoProcAmp_WhiteBalance},
00857 {RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE, VideoProcAmp_WhiteBalance, true},
00858 {RS_OPTION_FISHEYE_GAIN, VideoProcAmp_Gain}
00859 };
00860
00861 void set_pu_control(device & device, int subdevice, rs_option option, int value)
00862 {
00863 auto & sub = device.subdevices[subdevice];
00864 sub.get_media_source();
00865 if (option == RS_OPTION_COLOR_EXPOSURE)
00866 {
00867 check("IAMCameraControl::Set", sub.am_camera_control->Set(CameraControl_Exposure, static_cast<int>(value), CameraControl_Flags_Manual));
00868 return;
00869 }
00870 if(option == RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE)
00871 {
00872 if(value) check("IAMCameraControl::Set", sub.am_camera_control->Set(CameraControl_Exposure, 0, CameraControl_Flags_Auto));
00873 else
00874 {
00875 long min, max, step, def, caps;
00876 check("IAMCameraControl::GetRange", sub.am_camera_control->GetRange(CameraControl_Exposure, &min, &max, &step, &def, &caps));
00877 check("IAMCameraControl::Set", sub.am_camera_control->Set(CameraControl_Exposure, def, CameraControl_Flags_Manual));
00878 }
00879 return;
00880 }
00881 for(auto & pu : pu_controls)
00882 {
00883 if(option == pu.option)
00884 {
00885 if(pu.enable_auto)
00886 {
00887 if(value) check("IAMVideoProcAmp::Set", sub.am_video_proc_amp->Set(pu.property, 0, VideoProcAmp_Flags_Auto));
00888 else
00889 {
00890 long min, max, step, def, caps;
00891 check("IAMVideoProcAmp::GetRange", sub.am_video_proc_amp->GetRange(pu.property, &min, &max, &step, &def, &caps));
00892 check("IAMVideoProcAmp::Set", sub.am_video_proc_amp->Set(pu.property, def, VideoProcAmp_Flags_Manual));
00893 }
00894 }
00895 else check("IAMVideoProcAmp::Set", sub.am_video_proc_amp->Set(pu.property, value, VideoProcAmp_Flags_Manual));
00896 return;
00897 }
00898 }
00899 throw std::runtime_error("unsupported control");
00900 }
00901
00902 void get_pu_control_range(const device & device, int subdevice, rs_option option, int * min, int * max, int * step, int * def)
00903 {
00904 if(option >= RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE && option <= RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE)
00905 {
00906 if(min) *min = 0;
00907 if(max) *max = 1;
00908 if(step) *step = 1;
00909 if(def) *def = 1;
00910 return;
00911 }
00912
00913 auto & sub = device.subdevices[subdevice];
00914 const_cast<uvc::subdevice &>(sub).get_media_source();
00915 long minVal=0, maxVal=0, steppingDelta=0, defVal=0, capsFlag=0;
00916 if (option == RS_OPTION_COLOR_EXPOSURE)
00917 {
00918 check("IAMCameraControl::Get", sub.am_camera_control->GetRange(CameraControl_Exposure, &minVal, &maxVal, &steppingDelta, &defVal, &capsFlag));
00919 if (min) *min = minVal;
00920 if (max) *max = maxVal;
00921 if (step) *step = steppingDelta;
00922 if (def) *def = defVal;
00923 return;
00924 }
00925 for(auto & pu : pu_controls)
00926 {
00927 if(option == pu.option)
00928 {
00929 check("IAMVideoProcAmp::GetRange", sub.am_video_proc_amp->GetRange(pu.property, &minVal, &maxVal, &steppingDelta, &defVal, &capsFlag));
00930 if(min) *min = static_cast<int>(minVal);
00931 if(max) *max = static_cast<int>(maxVal);
00932 if(step) *step = static_cast<int>(steppingDelta);
00933 if(def) *def = static_cast<int>(defVal);
00934 return;
00935 }
00936 }
00937 throw std::runtime_error("unsupported control");
00938 }
00939
00940 void get_extension_control_range(const device & device, const extension_unit & xu, char control , int * min, int * max, int * step, int * def)
00941 {
00942 auto ks_control = const_cast<uvc::device &>(device).get_ks_control(xu);
00943
00944
00945 KSP_NODE node;
00946 memset(&node, 0, sizeof(KSP_NODE));
00947 node.Property.Set = reinterpret_cast<const GUID &>(xu.id);
00948 node.Property.Id = control;
00949 node.Property.Flags = KSPROPERTY_TYPE_BASICSUPPORT | KSPROPERTY_TYPE_TOPOLOGY;
00950 node.NodeId = xu.node;
00951
00952 KSPROPERTY_DESCRIPTION description;
00953 unsigned long bytes_received = 0;
00954 check("IKsControl::KsProperty", ks_control->KsProperty(
00955 (PKSPROPERTY)&node,
00956 sizeof(node),
00957 &description,
00958 sizeof(KSPROPERTY_DESCRIPTION),
00959 &bytes_received));
00960
00961 unsigned long size = description.DescriptionSize;
00962 std::vector<BYTE> buffer((long)size);
00963
00964 check("IKsControl::KsProperty", ks_control->KsProperty(
00965 (PKSPROPERTY)&node,
00966 sizeof(node),
00967 buffer.data(),
00968 size,
00969 &bytes_received));
00970
00971 if (bytes_received != size) { throw std::runtime_error("wrong data"); }
00972
00973 BYTE * pRangeValues = buffer.data() + sizeof(KSPROPERTY_MEMBERSHEADER) + sizeof(KSPROPERTY_DESCRIPTION);
00974
00975 *step = (int)*pRangeValues;
00976 pRangeValues++;
00977 *min = (int)*pRangeValues;
00978 pRangeValues++;
00979 *max = (int)*pRangeValues;
00980
00981
00982
00983 memset(&node, 0, sizeof(KSP_NODE));
00984 node.Property.Set = reinterpret_cast<const GUID &>(xu.id);
00985 node.Property.Id = control;
00986 node.Property.Flags = KSPROPERTY_TYPE_DEFAULTVALUES | KSPROPERTY_TYPE_TOPOLOGY;
00987 node.NodeId = xu.node;
00988
00989 bytes_received = 0;
00990 check("IKsControl::KsProperty", ks_control->KsProperty(
00991 (PKSPROPERTY)&node,
00992 sizeof(node),
00993 &description,
00994 sizeof(KSPROPERTY_DESCRIPTION),
00995 &bytes_received));
00996
00997 size = description.DescriptionSize;
00998 buffer.clear();
00999 buffer.resize(size);
01000
01001 check("IKsControl::KsProperty", ks_control->KsProperty(
01002 (PKSPROPERTY)&node,
01003 sizeof(node),
01004 buffer.data(),
01005 size,
01006 &bytes_received));
01007
01008 if (bytes_received != size) { throw std::runtime_error("wrong data"); }
01009
01010 pRangeValues = buffer.data() + sizeof(KSPROPERTY_MEMBERSHEADER) + sizeof(KSPROPERTY_DESCRIPTION);
01011
01012 *def = (int)*pRangeValues;
01013 }
01014
01015 int get_pu_control(const device & device, int subdevice, rs_option option)
01016 {
01017 auto & sub = device.subdevices[subdevice];
01018
01019 const_cast<uvc::subdevice &>(sub).get_media_source();
01020 long value=0, flags=0;
01021 if (option == RS_OPTION_COLOR_EXPOSURE)
01022 {
01023
01024 check("IAMCameraControl::Get", sub.am_camera_control->Get(CameraControl_Exposure, &value, &flags));
01025 return value;
01026 }
01027 if(option == RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE)
01028 {
01029 check("IAMCameraControl::Get", sub.am_camera_control->Get(CameraControl_Exposure, &value, &flags));
01030 return flags == CameraControl_Flags_Auto;
01031 }
01032 for(auto & pu : pu_controls)
01033 {
01034 if(option == pu.option)
01035 {
01036 check("IAMVideoProcAmp::Get", sub.am_video_proc_amp->Get(pu.property, &value, &flags));
01037 if(pu.enable_auto) return flags == VideoProcAmp_Flags_Auto;
01038 else return value;
01039 }
01040 }
01041 throw std::runtime_error("unsupported control");
01042 }
01043
01045
01047
01048 std::shared_ptr<context> create_context()
01049 {
01050 return std::make_shared<context>();
01051 }
01052
01053 bool is_device_connected(device & device, int vid, int pid)
01054 {
01055 for(auto& dev : device.subdevices)
01056 {
01057 if(dev.vid == vid && dev.pid == pid)
01058 return true;
01059 }
01060
01061 return false;
01062 }
01063
01064 std::vector<std::shared_ptr<device>> query_devices(std::shared_ptr<context> context)
01065 {
01066 IMFAttributes * pAttributes = NULL;
01067 check("MFCreateAttributes", MFCreateAttributes(&pAttributes, 1));
01068 check("IMFAttributes::SetGUID", pAttributes->SetGUID(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE, MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID));
01069
01070 IMFActivate ** ppDevices;
01071 UINT32 numDevices;
01072 check("MFEnumDeviceSources", MFEnumDeviceSources(pAttributes, &ppDevices, &numDevices));
01073
01074 std::vector<std::shared_ptr<device>> devices;
01075 for(UINT32 i=0; i<numDevices; ++i)
01076 {
01077 com_ptr<IMFActivate> pDevice;
01078 *&pDevice = ppDevices[i];
01079
01080 WCHAR * wchar_name = NULL; UINT32 length;
01081 pDevice->GetAllocatedString(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_SYMBOLIC_LINK, &wchar_name, &length);
01082 auto name = win_to_utf(wchar_name);
01083 CoTaskMemFree(wchar_name);
01084
01085 int vid, pid, mi; std::string unique_id;
01086 if(!parse_usb_path(vid, pid, mi, unique_id, name)) continue;
01087
01088 std::shared_ptr<device> dev;
01089 for(auto & d : devices)
01090 {
01091 if(d->vid == vid && d->pid == pid && d->unique_id == unique_id)
01092 {
01093 dev = d;
01094 }
01095 }
01096 if(!dev)
01097 {
01098 dev = std::make_shared<device>(context, vid, pid, unique_id);
01099 devices.push_back(dev);
01100 }
01101
01102 size_t subdevice_index = mi/2;
01103 if(subdevice_index >= dev->subdevices.size()) dev->subdevices.resize(subdevice_index+1);
01104
01105 dev->subdevices[subdevice_index].reader_callback = new reader_callback(dev, static_cast<int>(subdevice_index));
01106 dev->subdevices[subdevice_index].mf_activate = pDevice;
01107 dev->subdevices[subdevice_index].vid = vid;
01108 dev->subdevices[subdevice_index].pid = pid;
01109 }
01110
01111 for(auto& devA : devices)
01112 {
01113 if(devA->vid == VID_INTEL_CAMERA && devA->pid == ZR300_FISHEYE_PID)
01114 {
01115 for(auto& devB : devices)
01116 {
01117 if(devB->vid == VID_INTEL_CAMERA && devB->pid == ZR300_CX3_PID)
01118 {
01119 devB->subdevices.resize(4);
01120 devB->subdevices[3].reader_callback = new reader_callback(devB, static_cast<int>(3));
01121 devB->subdevices[3].mf_activate = devA->subdevices[0].mf_activate;
01122 devB->subdevices[3].vid = devB->aux_vid = VID_INTEL_CAMERA;
01123 devB->subdevices[3].pid = devB->aux_pid = ZR300_FISHEYE_PID;
01124 devB->aux_unique_id = devA->unique_id;
01125 devices.erase(std::remove(devices.begin(), devices.end(), devA), devices.end());
01126 break;
01127 }
01128 }
01129 break;
01130 }
01131 }
01132
01133 CoTaskMemFree(ppDevices);
01134 return devices;
01135 }
01136
01137 std::wstring getPath(HANDLE h, ULONG index)
01138 {
01139
01140 USB_NODE_CONNECTION_NAME name;
01141 name.ConnectionIndex = index;
01142 if (!DeviceIoControl(h, IOCTL_USB_GET_NODE_CONNECTION_NAME, &name, sizeof(name), &name, sizeof(name), nullptr, nullptr))
01143 {
01144 return std::wstring(L"");
01145 }
01146
01147
01148 if (name.ActualLength < sizeof(name)) return std::wstring(L"");
01149 auto alloc = std::malloc(name.ActualLength);
01150 auto pName = std::shared_ptr<USB_NODE_CONNECTION_NAME>(reinterpret_cast<USB_NODE_CONNECTION_NAME *>(alloc), std::free);
01151
01152
01153 pName->ConnectionIndex = index;
01154 if (DeviceIoControl(h, IOCTL_USB_GET_NODE_CONNECTION_NAME, pName.get(), name.ActualLength, pName.get(), name.ActualLength, nullptr, nullptr))
01155 {
01156 return std::wstring(pName->NodeName);
01157 }
01158
01159 return std::wstring(L"");
01160 }
01161
01162 bool handleNode(const std::wstring & targetKey, HANDLE h, ULONG index)
01163 {
01164 USB_NODE_CONNECTION_DRIVERKEY_NAME key;
01165 key.ConnectionIndex = index;
01166
01167 if (!DeviceIoControl(h, IOCTL_USB_GET_NODE_CONNECTION_DRIVERKEY_NAME, &key, sizeof(key), &key, sizeof(key), nullptr, nullptr))
01168 {
01169 return false;
01170 }
01171
01172 if (key.ActualLength < sizeof(key)) return false;
01173
01174 auto alloc = std::malloc(key.ActualLength);
01175 if (!alloc) throw std::bad_alloc();
01176 auto pKey = std::shared_ptr<USB_NODE_CONNECTION_DRIVERKEY_NAME>(reinterpret_cast<USB_NODE_CONNECTION_DRIVERKEY_NAME *>(alloc), std::free);
01177
01178 pKey->ConnectionIndex = index;
01179 if (DeviceIoControl(h, IOCTL_USB_GET_NODE_CONNECTION_DRIVERKEY_NAME, pKey.get(), key.ActualLength, pKey.get(), key.ActualLength, nullptr, nullptr))
01180 {
01181
01182 if (targetKey == pKey->DriverKeyName) {
01183 return true;
01184 }
01185 else return false;
01186 }
01187
01188 return false;
01189 }
01190
01191 std::string handleHub(const std::wstring & targetKey, const std::wstring & path)
01192 {
01193 if (path == L"") return "";
01194 std::wstring fullPath = L"\\\\.\\" + path;
01195
01196 HANDLE h = CreateFile(fullPath.c_str(), GENERIC_WRITE, FILE_SHARE_WRITE, nullptr, OPEN_EXISTING, 0, nullptr);
01197 if (h == INVALID_HANDLE_VALUE) return "";
01198 auto h_gc = std::shared_ptr<void>(h, CloseHandle);
01199
01200 USB_NODE_INFORMATION info;
01201 if (!DeviceIoControl(h, IOCTL_USB_GET_NODE_INFORMATION, &info, sizeof(info), &info, sizeof(info), nullptr, nullptr))
01202 return "";
01203
01204
01205 for (ULONG i = 1; i <= info.u.HubInformation.HubDescriptor.bNumberOfPorts; ++i)
01206 {
01207
01208 char buf[sizeof(USB_NODE_CONNECTION_INFORMATION_EX)] = { 0 };
01209 PUSB_NODE_CONNECTION_INFORMATION_EX pConInfo = (PUSB_NODE_CONNECTION_INFORMATION_EX)buf;
01210
01211
01212 pConInfo->ConnectionIndex = i;
01213 if (!DeviceIoControl(h, IOCTL_USB_GET_NODE_CONNECTION_INFORMATION_EX, pConInfo, sizeof(buf), pConInfo, sizeof(buf), nullptr, nullptr))
01214 {
01215 continue;
01216 }
01217
01218
01219 if (pConInfo->ConnectionStatus != DeviceConnected)
01220 {
01221 continue;
01222 }
01223
01224
01225 std::string ret = "";
01226 if (pConInfo->DeviceIsHub) ret = handleHub(targetKey, getPath(h, i));
01227 else
01228 {
01229 if (handleNode(targetKey, h, i))
01230 {
01231 ret = win_to_utf(fullPath.c_str()) + " " + std::to_string(i);
01232 }
01233 }
01234 if (ret != "") return ret;
01235 }
01236
01237 return "";
01238 }
01239
01240 std::string get_usb_port_id(const device & device)
01241 {
01242 SP_DEVINFO_DATA devInfo = { sizeof(SP_DEVINFO_DATA) };
01243 static_assert(sizeof(guid) == sizeof(GUID), "struct packing error");
01244
01245
01246 HDEVINFO device_info = SetupDiGetClassDevsEx((const GUID *)&GUID_DEVINTERFACE_IMAGE,
01247 nullptr,
01248 nullptr,
01249 DIGCF_PRESENT,
01250 nullptr,
01251 nullptr,
01252 nullptr);
01253 if (device_info == INVALID_HANDLE_VALUE) throw std::runtime_error("SetupDiGetClassDevs");
01254 auto di = std::shared_ptr<void>(device_info, SetupDiDestroyDeviceInfoList);
01255
01256
01257 for (int member_index = 0; ; ++member_index)
01258 {
01259 SP_DEVICE_INTERFACE_DATA interfaceData = { sizeof(SP_DEVICE_INTERFACE_DATA) };
01260 unsigned long buf_size = 0;
01261
01262 if (SetupDiEnumDeviceInfo(device_info, member_index, &devInfo) == FALSE)
01263 {
01264 if (GetLastError() == ERROR_NO_MORE_ITEMS) break;
01265 continue;
01266 }
01267
01268
01269 if (CM_Get_Device_ID_Size(&buf_size, devInfo.DevInst, 0) != CR_SUCCESS)
01270 {
01271 LOG_ERROR("CM_Get_Device_ID_Size failed");
01272 return "";
01273 }
01274
01275 auto alloc = std::malloc(buf_size * sizeof(WCHAR) + sizeof(WCHAR));
01276 if (!alloc) throw std::bad_alloc();
01277 auto pInstID = std::shared_ptr<WCHAR>(reinterpret_cast<WCHAR *>(alloc), std::free);
01278 if (CM_Get_Device_ID(devInfo.DevInst, pInstID.get(), buf_size * sizeof(WCHAR) + sizeof(WCHAR), 0) != CR_SUCCESS) {
01279 LOG_ERROR("CM_Get_Device_ID failed");
01280 return "";
01281 }
01282
01283 if (pInstID == nullptr) continue;
01284
01285
01286 int usb_vid, usb_pid, usb_mi; std::string usb_unique_id;
01287 if (!parse_usb_path_from_device_id(usb_vid, usb_pid, usb_mi, usb_unique_id, std::string(win_to_utf(pInstID.get())))) continue;
01288 if (usb_vid != device.vid || usb_pid != device.pid || usb_unique_id != device.unique_id) continue;
01289
01290
01291 DEVINST instance;
01292 if (CM_Get_Parent(&instance, devInfo.DevInst, 0) != CR_SUCCESS)
01293 {
01294 LOG_ERROR("CM_Get_Parent failed");
01295 return "";
01296 }
01297
01298
01299 if (CM_Get_Device_ID_Size(&buf_size, instance, 0) != CR_SUCCESS)
01300 {
01301 LOG_ERROR("CM_Get_Device_ID_Size failed");
01302 return "";
01303 }
01304 alloc = std::malloc(buf_size*sizeof(WCHAR) + sizeof(WCHAR));
01305 if (!alloc) throw std::bad_alloc();
01306 pInstID = std::shared_ptr<WCHAR>(reinterpret_cast<WCHAR *>(alloc), std::free);
01307 if (CM_Get_Device_ID(instance, pInstID.get(), buf_size * sizeof(WCHAR) + sizeof(WCHAR), 0) != CR_SUCCESS) {
01308 LOG_ERROR("CM_Get_Device_ID failed");
01309 return "";
01310 }
01311
01312
01313 device_info = SetupDiGetClassDevs(nullptr, pInstID.get(), nullptr, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE | DIGCF_ALLCLASSES);
01314 if (device_info == INVALID_HANDLE_VALUE) {
01315 LOG_ERROR("SetupDiGetClassDevs failed");
01316 return "";
01317 }
01318 auto di_gc = std::shared_ptr<void>(device_info, SetupDiDestroyDeviceInfoList);
01319
01320 interfaceData = { sizeof(SP_DEVICE_INTERFACE_DATA) };
01321 if (SetupDiEnumDeviceInterfaces(device_info, nullptr, &GUID_DEVINTERFACE_USB_DEVICE, 0, &interfaceData) == FALSE)
01322 {
01323 LOG_ERROR("SetupDiEnumDeviceInterfaces failed");
01324 return "";
01325 }
01326
01327
01328 buf_size = 0;
01329 SetupDiGetDeviceInterfaceDetail(device_info, &interfaceData, nullptr, 0, &buf_size, nullptr);
01330 if (GetLastError() != ERROR_INSUFFICIENT_BUFFER)
01331 {
01332 LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed");
01333 return "";
01334 }
01335 alloc = std::malloc(buf_size);
01336 if (!alloc) throw std::bad_alloc();
01337 auto detail_data = std::shared_ptr<SP_DEVICE_INTERFACE_DETAIL_DATA>(reinterpret_cast<SP_DEVICE_INTERFACE_DETAIL_DATA *>(alloc), std::free);
01338 detail_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);
01339 SP_DEVINFO_DATA parent_data = { sizeof(SP_DEVINFO_DATA) };
01340 if (!SetupDiGetDeviceInterfaceDetail(device_info, &interfaceData, detail_data.get(), buf_size, nullptr, &parent_data))
01341 {
01342 LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed");
01343 return "";
01344 }
01345
01346
01347 buf_size = 0;
01348 SetupDiGetDeviceRegistryProperty(device_info, &parent_data, SPDRP_DRIVER, nullptr, nullptr, 0, &buf_size);
01349 if (GetLastError() != ERROR_INSUFFICIENT_BUFFER)
01350 {
01351 LOG_ERROR("SetupDiGetDeviceRegistryProperty failed in an unexpected manner");
01352 return "";
01353 }
01354 alloc = std::malloc(buf_size);
01355 if (!alloc) throw std::bad_alloc();
01356 auto driver_key = std::shared_ptr<BYTE>(reinterpret_cast<BYTE*>(alloc), std::free);
01357 if (!SetupDiGetDeviceRegistryProperty(device_info, &parent_data, SPDRP_DRIVER, nullptr, driver_key.get(), buf_size, nullptr))
01358 {
01359 LOG_ERROR("SetupDiGetDeviceRegistryProperty failed");
01360 return "";
01361 }
01362
01363
01364 std::wstring targetKey(reinterpret_cast<const wchar_t*>(driver_key.get()));
01365
01366
01367 std::wstringstream buf;
01368 for (int i = 0;; i++)
01369 {
01370 buf << "\\\\.\\HCD" << i;
01371 std::wstring hcd = buf.str();
01372
01373
01374 HANDLE h = CreateFile(hcd.c_str(), GENERIC_WRITE | GENERIC_READ, FILE_SHARE_READ | FILE_SHARE_WRITE, nullptr, OPEN_EXISTING, 0, nullptr);
01375 auto h_gc = std::shared_ptr<void>(h, CloseHandle);
01376 if (h == INVALID_HANDLE_VALUE)
01377 {
01378 LOG_ERROR("CreateFile failed");
01379 break;
01380 }
01381 else
01382 {
01383 USB_ROOT_HUB_NAME name;
01384
01385
01386 if (!DeviceIoControl(h, IOCTL_USB_GET_ROOT_HUB_NAME, nullptr, 0, &name, sizeof(name), nullptr, nullptr)) {
01387 LOG_ERROR("DeviceIoControl failed");
01388 return "";
01389 }
01390
01391
01392 alloc = std::malloc(name.ActualLength);
01393 if (!alloc) throw std::bad_alloc();
01394 auto pName = std::shared_ptr<USB_ROOT_HUB_NAME>(reinterpret_cast<USB_ROOT_HUB_NAME *>(alloc), std::free);
01395
01396
01397 if (!DeviceIoControl(h, IOCTL_USB_GET_ROOT_HUB_NAME, nullptr, 0, pName.get(), name.ActualLength, nullptr, nullptr)) {
01398 LOG_ERROR("DeviceIoControl failed");
01399 return "";
01400 }
01401
01402
01403 std::string ret = handleHub(targetKey, std::wstring(pName->RootHubName));
01404 if (ret != "") return ret;
01405 }
01406 }
01407 }
01408 throw std::exception("could not find camera in windows device tree");
01409 }
01410 }
01411 }
01412
01413 #endif