uvc-wmf.cpp
Go to the documentation of this file.
00001 // License: Apache 2.0. See LICENSE file in root directory.
00002 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
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>        // For QISearch, etc.
00020 #include <mfapi.h>          // For MFStartup, etc.
00021 #include <mfidl.h>          // For MF_DEVSOURCE_*, etc.
00022 #include <mfreadwrite.h>    // MFCreateSourceReaderFromMediaSource
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         // Translation of user-provided fourcc code into device-advertized:
00068         const std::map<uint32_t, uint32_t> fourcc_map = { { 0x59382020, 0x47524559 },    /* 'Y8  '  as 'GREY' */
00069                                                           { 0x494e5649, 0x59313020 },    /* 'INVI'  as 'Y10'  */
00070                                                           { 0x494e565a, 0x5a313620 },    /* 'INVZ'  as 'Z16'  */
00071                                                           { 0x52573130, 0x70524141 } };  /* 'pRAA'  as 'RW10' */
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; // Not a USB device
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; // Not a USB device
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; // The device holds a reference to us, so use weak_ptr to prevent a cycle
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             // Implement IUnknown
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             // Implement IMFSourceReaderCallback
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; // Return true only if object was signaled
00356                 }
00357 
00358 
00359                 HANDLE GetHandle() const { return _handle; }
00360             private:
00361                 safe_handle() = delete;
00362 
00363                 // Disallow copy:
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];                         /* 64 byte transfer buffer  - dedicated channel*/
00374                 ULONG num_bytes = 0;                                        /* Actual bytes transferred. */
00375                 OVERLAPPED hOvl;
00376                 safe_handle sh(CreateEvent(nullptr, false, false, nullptr));
00377                 hOvl.hEvent = sh.GetHandle();
00378                 // TODO - replace hard-coded values : 0x82 and 1000
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                     // Propagate the data to device layer
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                     // todo exception
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                 // Attempt to retrieve IKsControl
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                         // Polling
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                 // Free up our source readers, our KS control nodes, and our media sources, but retain our original IMFActivate objects for later reuse
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                     // Enumerate all the device interfaces in the device information set.
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                     // Allocate space for a detail data struct
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                     // Retrieve the detail data struct
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                     // Check if this is our device
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                     // We successfully set up a WinUsb interface handle to our device
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, &current_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         // device //
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             /* get step, min and max values*/
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             /* get def value*/
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             // first call to get_media_source is also initializing the am_camera_control pointer, required for this method
01019             const_cast<uvc::subdevice &>(sub).get_media_source(); // initialize am_camera_control
01020             long value=0, flags=0;
01021             if (option == RS_OPTION_COLOR_EXPOSURE)
01022             {
01023                 // am_camera_control != null, because get_media_source was called at least once
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         // context //
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) // Look for CX3 Fisheye camera
01112             {
01113                if(devA->vid == VID_INTEL_CAMERA && devA->pid == ZR300_FISHEYE_PID)
01114                {
01115                     for(auto& devB : devices) // Look for DS ZR300 camera
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             // get name length
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             // alloc space
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             // get name
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                 //std::wcout << pKey->DriverKeyName << std::endl;
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             // for each port on the hub
01205             for (ULONG i = 1; i <= info.u.HubInformation.HubDescriptor.bNumberOfPorts; ++i)
01206             {
01207                 // allocate something or other
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                 // get info about port i
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                 // check if device is connected
01219                 if (pConInfo->ConnectionStatus != DeviceConnected)
01220                 {
01221                     continue; // almost assuredly silently. I think this flag gets set for any port without a device
01222                 }
01223 
01224                 // if connected, handle correctly, setting the location info if the device is found
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) // Not implemented for Windows at this point
01241         {
01242             SP_DEVINFO_DATA devInfo = { sizeof(SP_DEVINFO_DATA) };
01243             static_assert(sizeof(guid) == sizeof(GUID), "struct packing error"); // not sure this is needed. maybe because the original function gets the guid object from outside?
01244 
01245             // build a device info represent all imaging devices.
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             // enumerate all imaging devices.
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; // stop when none left
01265                     continue; // silently ignore other errors
01266                 }
01267 
01268                 // get the device ID of current device.
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                 // Check if this is our device 
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_mi != device->mi || */ usb_unique_id != device.unique_id) continue;
01289 
01290                 // get parent (composite device) instance
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                 // get composite device instance id
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                 // upgrade to DEVINFO_DATA for SetupDiGetDeviceRegistryProperty
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                 // get the SP_DEVICE_INTERFACE_DETAIL_DATA object, and also grab the SP_DEVINFO_DATA object for the device
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                 // get driver key for composite device
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                 // contains composite device key
01364                 std::wstring targetKey(reinterpret_cast<const wchar_t*>(driver_key.get()));
01365 
01366                 // recursively check all hubs, searching for composite device
01367                 std::wstringstream buf;
01368                 for (int i = 0;; i++)
01369                 { 
01370                     buf << "\\\\.\\HCD" << i;
01371                     std::wstring hcd = buf.str();
01372 
01373                     // grab handle
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                         // get required space
01386                         if (!DeviceIoControl(h, IOCTL_USB_GET_ROOT_HUB_NAME, nullptr, 0, &name, sizeof(name), nullptr, nullptr)) {
01387                             LOG_ERROR("DeviceIoControl failed");
01388                             return ""; // alt: fail silently and hope its on a different root hub
01389                         }
01390 
01391                         // alloc space
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                         // get name
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 ""; // alt: fail silently and hope its on a different root hub
01400                         }
01401 
01402                         // return location if device is connected under this root hub
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


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Tue Jun 25 2019 19:54:39