scanner.cpp

Go to the documentation of this file.

 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
 
//  Copyright (c) 2003-2021 Xsens Technologies B.V. or subsidiaries worldwide.
//  All rights reserved.
//  
//  Redistribution and use in source and binary forms, with or without modification,
//  are permitted provided that the following conditions are met:
//  
//  1.  Redistributions of source code must retain the above copyright notice,
//      this list of conditions, and the following disclaimer.
//  
//  2.  Redistributions in binary form must reproduce the above copyright notice,
//      this list of conditions, and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//  
//  3.  Neither the names of the copyright holders nor the names of their contributors
//      may be used to endorse or promote products derived from this software without
//      specific prior written permission.
//  
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
//  THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
//  SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
//  OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY OR
//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.THE LAWS OF THE NETHERLANDS 
//  SHALL BE EXCLUSIVELY APPLICABLE AND ANY DISPUTES SHALL BE FINALLY SETTLED UNDER THE RULES 
//  OF ARBITRATION OF THE INTERNATIONAL CHAMBER OF COMMERCE IN THE HAGUE BY ONE OR MORE 
//  ARBITRATORS APPOINTED IN ACCORDANCE WITH SAID RULES.
//  
 
#include "scanner.h"
 
#ifdef _WIN32
        #include <devguid.h>
        #include <initguid.h>
        #include <Usbiodef.h>
        #include <cfgmgr32.h>
        #include <regstr.h>
        #include <regex>
        #include "idfetchhelpers.h"
#else
        #include <stdlib.h>
        #include <string.h>
        #include <dirent.h>
        #include "xslibusb.h"
        #include "udev.h"
#endif
#include <algorithm>
 
#include "serialportcommunicator.h"
#include "usbcommunicator.h"
#include <iostream>
#include <atomic>
#include <xscommon/xsens_janitors.h>
#include <xstypes/xsbusid.h>
#include <xstypes/xsportinfo.h>
#include <xstypes/xsintarray.h>
#include <xstypes/xsstringarray.h>
#include "enumerateusbdevices.h"
#include <xscommon/journaller.h>
 
namespace XsScannerNamespace
{
volatile std::atomic_bool abortPortScan{false};
Scanner* gScanner = nullptr;
XsScanLogCallbackFunc gScanLogCallback = nullptr;
}
using namespace XsScannerNamespace;
 
Scanner& Scanner::Accessor::scanner() const
{
        if (!gScanner)
                gScanner = new Scanner();
        return *gScanner;
}
 
Scanner::~Scanner()
{
}
 
void Scanner::setScanLogCallback(XsScanLogCallbackFunc cb)
{
        gScanLogCallback = cb;
}
 
XsResultValue Scanner::fetchBasicInfo(XsPortInfo& portInfo, uint32_t singleScanTimeout, bool detectRs485)
{
        auto serial = Communicator::createUniquePtr<SerialPortCommunicator>();
        auto usb = Communicator::createUniquePtr<UsbCommunicator>();
 
        SerialPortCommunicator* port = (portInfo.isUsb() ? usb.get() : serial.get());
 
        port->setGotoConfigTimeout(singleScanTimeout);
        LOGXSSCAN("Opening port " << portInfo.portName() << " (" << portInfo.portNumber() << ") @ " << portInfo.baudrate() << " baud, expected device " << portInfo.deviceId());
        if (!port->openPort(portInfo, OPS_OpenPort))
        {
                LOGXSSCAN("Failed to open port because: " << XsResultValue_toString(port->lastResult()));
                return port->lastResult();
        }
 
        if (!port->openPort(portInfo, OPS_InitStart, detectRs485))
        {
                LOGXSSCAN("Failed to initialize port because: " << XsResultValue_toString(port->lastResult()));
                if (port->lastResult() < XRV_ERROR)
                {
                        LOGXSSCAN("Attempting to reset device");
                        // we got an xbus protocol error message, attempt to reset the device and try again (once)
                        XsMessage snd(XMID_Reset);
                        snd.setBusId(XS_BID_MASTER);
                        if (!port->writeMessage(snd))
                        {
                                LOGXSSCAN("Failed to wriite reset to device because: " << XsResultValue_toString(port->lastResult()));
                                return port->lastResult();
                        }
                        LOGXSSCAN("Reopening port after reset");
                        port->closePort();
                        XsTime::msleep(2000);
                        if (!port->openPort(portInfo, OPS_Full, detectRs485))
                        {
                                LOGXSSCAN("Failed to reopen port after reset because: " << XsResultValue_toString(port->lastResult()));
                                return port->lastResult();
                        }
                }
                else
                        return port->lastResult();
        }
 
        LOGXSSCAN("Port " << portInfo.portName() << " opened successfully, device is " << port->masterDeviceId());
        portInfo.setDeviceId(port->masterDeviceId());
 
        // Enable flow control for Awinda2 stations/dongles which support this:
        if (port->masterDeviceId().isAwinda2())
        {
                XsPortLinesOptions portLinesOptions = portInfo.linesOptions();
                XsVersion fwVersion = port->firmwareRevision();
                XsVersion hwVersion = port->hardwareRevision();
 
                if (port->masterDeviceId().isAwinda2Station() && fwVersion.major() != 255 && fwVersion >= XsVersion(4, 2, 1))
                        portLinesOptions = (XsPortLinesOptions)(portLinesOptions | XPLO_RtsCtsFlowControl);
                else if (port->masterDeviceId().isAwinda2Dongle() && fwVersion.major() != 255 && fwVersion >= XsVersion(4, 3, 2) && hwVersion >= XsVersion(2, 3))
                        portLinesOptions = (XsPortLinesOptions)(portLinesOptions | XPLO_RtsCtsFlowControl);
                else
                        portLinesOptions = (XsPortLinesOptions)(portLinesOptions & ~XPLO_RtsCtsFlowControl);
                LOGXSSCAN("Setting flow control options for Awinda device to " << JLHEXLOG(portLinesOptions));
                portInfo.setLinesOptions(portLinesOptions);
        }
 
        return XRV_OK;
}
 
bool Scanner::xsScanPort(XsPortInfo& portInfo, XsBaudRate baud, uint32_t singleScanTimeout, bool detectRs485)
{
        LOGXSSCAN("Scanning port " << portInfo.portName() << " at baudrate " << baud << " with timeout " << singleScanTimeout << " detectRs485 " << detectRs485);
        XsResultValue res;
        XsBaudRate baudrate;
 
        if (baud == 0)
                baudrate = XBR_115k2;
        else
                baudrate = baud;
 
        while (!abortPortScan)
        {
                portInfo.setBaudrate(baudrate);
                res = fetchBasicInfo(portInfo, singleScanTimeout, detectRs485);
                if (res == XRV_OK)
                {
                        LOGXSSCAN("Scan successfully found device " << portInfo.deviceId() << " on port " << portInfo.portName());
                        portInfo.setBaudrate(baudrate);
                        return true;
                }
 
                // failed, determine if we need to scan other baudrates or not
                if (res != XRV_TIMEOUT && res != XRV_TIMEOUTNODATA && res != XRV_CONFIGCHECKFAIL)
                {
                        LOGXSSCAN("Failed to fetch basic info: " << XsResultValue_toString(res));
                        return false;
                }
                LOGXSSCAN("Failed to fetch basic info within timeout");
 
                // not detected, try next baudrate
                if (baud != 0)
                {
                        LOGXSSCAN("Failed to find device");
                        return false;
                }
                switch (baudrate)
                {
                        default:
                        case XBR_115k2:
                                baudrate = XBR_2000k;
                                break;
                        case XBR_2000k:
                                baudrate = XBR_921k6;
                                break;
                        case XBR_921k6:
                                baudrate = XBR_460k8;
                                break;
                        case XBR_460k8:
                                baudrate = XBR_230k4;
                                break;
                        case XBR_230k4:
                                // On some systems a delay of about 100ms seems necessary to successfully perform the scan.
                                XsTime::msleep(100);
                                baudrate = XBR_57k6;
                                break;
                        case XBR_57k6:
                                baudrate = XBR_38k4;
                                break;
                        case XBR_38k4:
                                baudrate = XBR_19k2;
                                break;
                        case XBR_19k2:
                                baudrate = XBR_9600;
                                break;
                        case XBR_9600:
                                LOGXSSCAN("No more available baudrates, failed to find device");
                                return false;   // could not detect Xsens device, return false
                }
                LOGXSSCAN("Checking next baudrate: " << baudrate);
        }
        LOGXSSCAN("Port scan aborted by external trigger");
        return false;
}
 
bool Scanner::xsScanPorts(XsPortInfoArray& ports, XsBaudRate baudrate, uint32_t singleScanTimeout, bool ignoreNonXsensDevices, bool detectRs485)
{
        ports.clear();
 
        if (!xsEnumerateSerialPorts(ports, ignoreNonXsensDevices))
                return false;
 
        if (!xsEnumerateUsbDevices(ports))
                return false;
 
        return xsFilterResponsiveDevices(ports, baudrate, singleScanTimeout, detectRs485);
}
 
bool Scanner::xsFilterResponsiveDevices(XsPortInfoArray& ports, XsBaudRate baudrate, uint32_t singleScanTimeout, bool detectRs485)
{
        // try to connect so we can detect if there really is an MT / XM attached
        unsigned p = 0;
        while (!abortPortScan && p < ports.size())
        {
                if (ports[p].isNetwork() || xsScanPort(ports[p], baudrate, singleScanTimeout, detectRs485))
                        ++p;
                else
                {
                        LOGXSSCAN("Port : " << ports[p].portName() << " is not responsive, discarding");
                        ports.erase(ports.begin() + p);
                }
        }
 
        if (abortPortScan)
        {
                abortPortScan = false;
                return false;
        }
 
        // Now sort the final list by ascending port nr
        std::sort(ports.begin(), ports.end());
        abortPortScan = false;
        return true;
}
 
#ifdef _WIN32
 
std::string Scanner::getDevicePath(HDEVINFO hDevInfo, SP_DEVINFO_DATA* DeviceInfoData)
{
        char deviceInstanceID[MAX_DEVICE_ID_LEN];
        SetupDiGetDeviceInstanceIdA(hDevInfo, DeviceInfoData, deviceInstanceID, MAX_DEVICE_ID_LEN, NULL);
        return std::string(deviceInstanceID);
}
#endif
 
bool Scanner::isXsensUsbDevice(uint16_t vid, uint16_t pid)
{
        switch (vid)
        {
                // Xsens
                case XSENS_VENDOR_ID:
                        // ignore the body pack serial port
                        return (pid != 0x0100);
 
                // FTDI reserved PIDs
                case FTDI_VENDOR_ID:
                        return (pid >= 0xd388 && pid <= 0xd38f);
 
                default:
                        return false;
        }
}
 
bool Scanner::xsEnumerateSerialPorts(XsPortInfoArray& ports, bool ignoreNonXsensDevices)
{
        LOGXSSCAN("Enumerating USB devices");
        XsPortInfo current;
 
#ifdef _WIN32
        HDEVINFO hDevInfo;
        SP_DEVINFO_DATA DeviceInfoData;
        DWORD i;
 
        // Create a HDEVINFO with all present devices.
        hDevInfo = SetupDiGetClassDevs(&GUID_DEVCLASS_PORTS, 0, 0, DIGCF_PRESENT | DIGCF_PROFILE | DIGCF_ALLCLASSES);
 
        if (hDevInfo == INVALID_HANDLE_VALUE)
        {
                LOGXSSCAN("Failed to get any USB device information, check permissions");
                return false;
        }
 
        // Enumerate through all devices in Set.
        DeviceInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
        for (i = 0; !abortPortScan && SetupDiEnumDeviceInfo(hDevInfo, i, &DeviceInfoData); ++i)
        {
                // Get the registry key which stores the ports settings
                HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &DeviceInfoData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE);
                if (hDeviceKey == INVALID_HANDLE_VALUE)
                        continue;
 
                xsens::JanitorFuncStdCall<HKEY, LSTATUS> devkeyCleaner(&RegCloseKey, hDeviceKey, true);
 
                // Read in the name of the port
                char pszPortName[256];
                DWORD dwSize = 256;
                DWORD dwType = 0;
                if ((RegQueryValueExA(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) != ERROR_SUCCESS) || (dwType != REG_SZ))
                        continue;
 
                // If it looks like "COMX" then
                // add it to the array which will be returned
                if (_strnicmp(pszPortName, "COM", 3))
                        continue;
                int32_t nPort = atoi(&pszPortName[3]);
                if (nPort == 0)
                        continue;
 
                std::string devpath = getDevicePath(hDevInfo, &DeviceInfoData);
                uint16_t vid = vidFromString(devpath);
                uint16_t pid = pidFromString(devpath);
 
                LOGXSSCAN("Found USB device " << devpath);
                if (ignoreNonXsensDevices)
                {
                        if (!isXsensUsbDevice(vid, pid))
                        {
                                LOGXSSCAN("Ignoring non-Xsens device " << devpath);
                                continue;
                        }
                }
 
                current.setPortName(pszPortName);
                current.setBaudrate(XBR_Invalid);
                current.setDeviceId(deviceIdFromDevPath(devpath));
                current.setVidPid(vid, pid);
                ports.push_back(current);
 
                current.clear();
        }
 
        //  Cleanup
 
        SetupDiDestroyDeviceInfoList(hDevInfo);
 
        // Now sort the list by ascending port nr
        std::sort(ports.begin(), ports.end());
 
#else // !_WIN32
        Udev xsudev;
        struct udev* udev = xsudev.unew();
        if (udev)
        {
                struct udev_enumerate* enumerate = xsudev.enumerate_new(udev);
 
                xsudev.enumerate_add_match_subsystem(enumerate, "tty");
                xsudev.enumerate_scan_devices(enumerate);
 
                struct udev_list_entry* devices, *dev_list_entry;
                devices = xsudev.enumerate_get_list_entry(enumerate);
                for (dev_list_entry = devices; dev_list_entry != nullptr; dev_list_entry = xsudev.list_entry_get_next(dev_list_entry))
                {
                        const char* path = xsudev.list_entry_get_name(dev_list_entry);
                        auto devcleaner = [&](struct udev_device * d)
                        {
                                xsudev.device_unref(d);
                        };
                        std::unique_ptr<struct udev_device, decltype(devcleaner)>
                        device(xsudev.device_new_from_syspath(udev, path), devcleaner);
                        if (!device)
                                continue;
 
                        if (!xsudev.device_get_parent(device.get()))
                                // this is probably a console, definitely not a physical serial port device
                                continue;
 
                        struct udev_device* usbParentDevice = xsudev.device_get_parent_with_subsystem_devtype(device.get(), "usb", "usb_device");
                        unsigned int vid = 0, pid = 0;
                        if (usbParentDevice)
                        {
                                const char* vendor = xsudev.device_get_sysattr_value(usbParentDevice, "idVendor");
                                const char* product = xsudev.device_get_sysattr_value(usbParentDevice, "idProduct");
 
                                if (vendor && product)
                                {
                                        sscanf(vendor, "%x", &vid);
                                        sscanf(product, "%x", &pid);
                                }
                        }
 
                        LOGXSSCAN("Found USB device " << path);
 
                        if (ignoreNonXsensDevices)
                        {
                                if (!isXsensUsbDevice(vid, pid))
                                {
                                        LOGXSSCAN("Ignoring non-Xsens device " << path);
                                        continue;
                                }
                        }
 
                        XsPortInfo portInfo;
 
                        const char* devnode = xsudev.device_get_devnode(device.get());
                        if (strlen(devnode) > 255 || strncmp(devnode, "/dev/ttyS", 9) == 0)
                                continue;
                        portInfo.setPortName(devnode);
                        portInfo.setVidPid(vid, pid);
 
                        if (usbParentDevice)
                        {
                                const char* deviceidstring;
                                deviceidstring = xsudev.device_get_sysattr_value(usbParentDevice, "serial");
                                if (deviceidstring)
                                {
                                        int deviceId = 0;
                                        sscanf(deviceidstring, "%08X", &deviceId);
                                        portInfo.setDeviceId(deviceId);
                                }
                        }
 
                        ports.push_back(portInfo);
                }
                xsudev.enumerate_unref(enumerate);
                xsudev.unref(udev);
        }
        else
        {
                (void)ignoreNonXsensDevices;
                DIR* dir;
                struct dirent* entry;
 
                if ((dir = opendir("/dev/")) == NULL)
                        return false;
 
                while ((entry = readdir(dir)))
                {
                        if (strncmp("ttyUSB", entry->d_name, 6) == 0)
                        {
                                char name[261];
                                sprintf(name, "/dev/%s", entry->d_name);
                                current.setPortName(name);
                                ports.push_back(current);
                                LOGXSSCAN("Found USB device " << name);
                        }
                }
                closedir(dir);
        }
#endif // _WIN32
        return true;
}
 
bool Scanner::xsEnumerateNetworkDevices(XsPortInfoArray& ports)
{
        (void)ports;
        return false;
}
 
bool Scanner::xsEnumerateBluetoothDevices(XsPortInfoArray& ports)
{
        (void)ports;
        return false;
}
 
 
#define ILLEGAL_HUB     (0)
#ifdef _WIN32
#define HUB_SEARCH_STRING ("Hub_#")
 
int Scanner::xsScanGetHubNumber(HDEVINFO hDevInfo, SP_DEVINFO_DATA* deviceInfoData)
{
        DWORD DataT;
        char buffer[256];
        int result = ILLEGAL_HUB;
 
        if (SetupDiGetDeviceRegistryPropertyA(hDevInfo,
                        deviceInfoData,
                        SPDRP_LOCATION_INFORMATION,
                        &DataT,
                        (PBYTE)buffer,
                        256,
                        NULL))
        {
                LOGXSSCAN("Registry access successful: \"" << buffer << "\"");
                char const* hubString = strstr((char const*)buffer, HUB_SEARCH_STRING);
                if (hubString)
                        result = strtol(hubString + strlen(HUB_SEARCH_STRING), 0, 10);
        }
        else
                LOGXSSCAN("Get Hub Number failed with error " << GetLastError());
 
        return result;
}
 
XsPortInfo Scanner::xsScanPortByHubId(const char* id)
{
        assert(id != nullptr);
        if (!id)
                return XsPortInfo();
 
        LOGXSSCAN("xsScanPortByHubId with id " << id);
 
        // Get device interface info set handle for all devices attached to system
        HDEVINFO hDevInfo = SetupDiGetClassDevs(
                        &GUID_DEVCLASS_PORTS, /* CONST GUID * ClassGuid - USB class GUID */
                        NULL, /* PCTSTR Enumerator */
                        NULL, /* HWND hwndParent */
                        DIGCF_PRESENT | DIGCF_DEVICEINTERFACE /* DWORD Flags */
                );
 
        if (hDevInfo == INVALID_HANDLE_VALUE)
        {
                LOGXSSCAN("Failed to get any USB device information, check permissions");
                return XsPortInfo();
        }
 
        // Retrieve a context structure for a device interface of a device
        // information set.
 
        SP_DEVICE_INTERFACE_DATA devInterfaceData;
        ZeroMemory(&devInterfaceData, sizeof(SP_DEVICE_INTERFACE_DATA));
        devInterfaceData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA);
        XsPortInfo portInfo;
        int port = 0;
        for (DWORD dwIndex = 0; port == 0; ++dwIndex)   //lint !e440 funky condition is ok
        {
                BOOL bRet = SetupDiEnumDeviceInterfaces(
                                hDevInfo, /* HDEVINFO DeviceInfoSet */
                                NULL, /* PSP_DEVINFO_DATA DeviceInfoData */
                                &GUID_DEVINTERFACE_SERENUM_BUS_ENUMERATOR, /* CONST GUID * InterfaceClassGuid */
                                dwIndex,
                                &devInterfaceData /* PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData */
                        );
                if (!bRet)
                {
                        if (GetLastError() == ERROR_NO_MORE_ITEMS)
                                break;
                }
                else
                {
                        size_t buffer[1024 / sizeof(size_t)];   // we use size_t instead of char to ensure proper data alignment
 
                        SP_DEVINFO_DATA diData;
                        diData.cbSize = sizeof(SP_DEVINFO_DATA);
                        SP_DEVICE_INTERFACE_DETAIL_DATA_A& ifdData = *(SP_DEVICE_INTERFACE_DETAIL_DATA_A*) buffer;
                        ifdData.cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A);
                        DWORD reqSize;
                        if (SetupDiGetDeviceInterfaceDetailA(hDevInfo, &devInterfaceData, &ifdData, 1020, &reqSize, &diData))
                        {
                                // determine my own id
                                // strip A#0000#* from path
                                char* end = strrchr(ifdData.DevicePath, '#');
                                if (end)
                                        end[0] = 0;
                                end = strrchr(ifdData.DevicePath, '#');
                                if (end)
                                        end[0] = 0;
                                char* start = strrchr(ifdData.DevicePath, '+');
                                if (!start || !end)
                                        continue;
                                ++start;
 
                                // compare IDs
                                if (_strnicmp(id, start, strlen(id)) == 0)
                                {
                                        // we found a match, determine port nr
                                        // Get the registry key which stores the ports settings
                                        HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &diData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE);
                                        if (hDeviceKey != INVALID_HANDLE_VALUE)
                                        {
                                                // Read in the name of the port
                                                char pszPortName[256] = "";
                                                DWORD dwSize = 256;
                                                DWORD dwType = 0;
                                                if ((RegQueryValueExA(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) == ERROR_SUCCESS) && (dwType == REG_SZ))
                                                {
                                                        // If it looks like "COMX" then
                                                        // add it to the array which will be returned
                                                        if (!_strnicmp(pszPortName, "COM", 3))
                                                                port = atoi(&pszPortName[3]);
                                                        portInfo.setPortName(pszPortName);
                                                        std::string devpath = getDevicePath(hDevInfo, &diData);
                                                        uint16_t vid = vidFromString(devpath);
                                                        uint16_t pid = pidFromString(devpath);
                                                        LOGXSSCAN("Adding " << pszPortName << " with vid " << JLHEXLOG(vid) << " and pid " << JLHEXLOG(pid));
                                                        portInfo.setVidPid(vid, pid);
                                                }
                                        }
                                        // Close the key now that we are finished with it
                                        RegCloseKey(hDeviceKey);
                                }
                        }
                }
        }
 
        SetupDiDestroyDeviceInfoList(hDevInfo);
 
        return portInfo;
}
 
bool Scanner::xsScanXsensUsbHubs(XsIntArray& hubs, XsPortInfoArray& ports)
{
        LOGXSSCAN("xsScanXsensUsbHubs");
 
        // clear the lists
        hubs.clear();
        ports.clear();
 
        // Get device interface info set handle for all devices attached to system
        HDEVINFO hDevInfo = SetupDiGetClassDevs(
                        &GUID_DEVINTERFACE_USB_DEVICE, /* CONST GUID * ClassGuid - USB class GUID */
                        NULL, /* PCTSTR Enumerator */
                        NULL, /* HWND hwndParent */
                        DIGCF_PRESENT | DIGCF_DEVICEINTERFACE /* DWORD Flags */
                );
 
        if (hDevInfo == INVALID_HANDLE_VALUE)
        {
                LOGXSSCAN("Failed to get any USB device information, check permissions");
                return false;
        }
 
        // Retrieve a context structure for a device interface of a device
        // information set.
 
        SP_DEVICE_INTERFACE_DATA devInterfaceData;
        ZeroMemory(&devInterfaceData, sizeof(SP_DEVICE_INTERFACE_DATA));
        devInterfaceData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA);
        for (DWORD dwIndex = 0; ; ++dwIndex)
        {
                BOOL bRet = SetupDiEnumDeviceInterfaces(
                                hDevInfo, /* HDEVINFO DeviceInfoSet */
                                NULL, /* PSP_DEVINFO_DATA DeviceInfoData */
                                &GUID_DEVINTERFACE_USB_DEVICE, /* CONST GUID * InterfaceClassGuid */
                                dwIndex,
                                &devInterfaceData /* PSP_DEVICE_INTERFACE_DATA DeviceInterfaceData */
                        );
                if (!bRet)
                {
                        if (GetLastError() == ERROR_NO_MORE_ITEMS)
                                break;
                        continue;
                }
 
                size_t tmp1[1024 / sizeof(size_t)];
                size_t tmp2[1024 / sizeof(size_t)];
                char* buffer = (char*) tmp1;
                SP_DEVINFO_DATA diData;
                diData.cbSize = sizeof(SP_DEVINFO_DATA);
 
                SP_DEVICE_INTERFACE_DETAIL_DATA_A& ifdData = *(SP_DEVICE_INTERFACE_DETAIL_DATA_A*) tmp2;
                ifdData.cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A);
                DWORD reqSize;
                XsPortInfo port;
                int hubNr = 0;
                while (SetupDiGetDeviceInterfaceDetailA(hDevInfo, &devInterfaceData, &ifdData, 1020, &reqSize, &diData))
                {
                        DWORD dataT;
                        if (SetupDiGetDeviceRegistryPropertyA(hDevInfo, &diData, SPDRP_MFG, &dataT, (PBYTE)buffer, 256, NULL))
                        {
                                if (_strnicmp(buffer, "xsens", 5))      // if this is NOT an xsens device, ignore it
                                        break;
                        }
                        else
                                break;
 
                        hubNr = xsScanGetHubNumber(hDevInfo, &diData);
                        // we found something, determine its port
                        if (hubNr)
                        {
                                char* hubEnd = strrchr(ifdData.DevicePath, '#');
                                if (!hubEnd || hubEnd == ifdData.DevicePath)
                                        break;
                                hubEnd[0] = 0;
                                char* hubStart = strrchr(ifdData.DevicePath, '#');
                                if (!hubStart || hubStart == ifdData.DevicePath)
                                        break;
                                ++hubStart;
                                port = xsScanPortByHubId(hubStart);
                        }
                        break;
                }
                if (port.portName().size())
                {
                        LOGXSSCAN("xsScanXsensUsbHubs Adding hub " << hubNr << " and port " << port.portName());
                        hubs.push_back(hubNr);
                        ports.push_back(port);
                }
        }
 
        SetupDiDestroyDeviceInfoList(hDevInfo);
 
        return true;
}
#endif // _WIN32
 
XsUsbHubInfo Scanner::xsScanUsbHub(const XsPortInfo& portInfo)
{
#ifdef _WIN32
        int behindXsensHub = ILLEGAL_HUB;
        XsIntArray hubs;
        XsPortInfoArray ports;
        if (!xsScanXsensUsbHubs(hubs, ports))
                return XsUsbHubInfo();
 
        // check if it's in the ports list before going through all THAT trouble...
        for (size_t i = 0; i < ports.size(); ++i)
                if (_stricmp(ports[i].portName_c_str(), portInfo.portName_c_str()) == 0)
                        return XsUsbHubInfo(hubs[i]);
 
        HDEVINFO hDevInfo;
        SP_DEVINFO_DATA devInfoData;
 
        // Create a HDEVINFO with all present devices.
        hDevInfo = SetupDiGetClassDevs(&GUID_DEVCLASS_PORTS, 0, 0, DIGCF_PRESENT | DIGCF_PROFILE);
 
        if (hDevInfo == INVALID_HANDLE_VALUE)
                return XsUsbHubInfo();
 
        // Enumerate through all devices in Set.
        devInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
        for (DWORD i = 0; behindXsensHub == ILLEGAL_HUB && SetupDiEnumDeviceInfo(hDevInfo, i, &devInfoData); ++i)
        {
                DWORD dataT;
                char buffer[256];
 
                // Call function with null to begin with,
                // then use the returned buffer size
                // to Alloc the buffer. Keep calling until
                // success or an unknown failure.
                //
 
                if (!SetupDiGetDeviceRegistryPropertyA(hDevInfo,
                                &devInfoData,
                                SPDRP_MFG,
                                &dataT,
                                (PBYTE)buffer,
                                256,
                                NULL))
                        continue;
 
                // Get the registry key which stores the ports settings
                HKEY hDeviceKey = SetupDiOpenDevRegKey(hDevInfo, &devInfoData, DICS_FLAG_GLOBAL, 0, DIREG_DEV, KEY_QUERY_VALUE);
                if (hDeviceKey != INVALID_HANDLE_VALUE)
                {
                        // Read in the name of the port
                        char pszPortName[256];
                        DWORD dwSize = 256;
                        DWORD dwType = 0;
                        if ((RegQueryValueExA(hDeviceKey, "PortName", NULL, &dwType, (LPBYTE) pszPortName, &dwSize) == ERROR_SUCCESS) && (dwType == REG_SZ))
                        {
                                // If it looks like "COMX" then
                                // add it to the array which will be returned
                                int32_t nLen = (int32_t) strlen(pszPortName);
                                if (nLen > 3)
                                {
                                        if (_strnicmp(pszPortName, "COM", 3))
                                                continue;
                                        //int32_t nPort = atoi(&pszPortName[3]);
                                        if (_stricmp(pszPortName, portInfo.portName_c_str()) == 0)
                                        {
                                                //This is the port we are looking for
                                                //check if the hub number is in the hubs list
                                                int hub = xsScanGetHubNumber(hDevInfo, &devInfoData);
                                                JLDEBUGG("Got hub " << hub << " for port " << std::string(pszPortName));
 
                                                if (hub != ILLEGAL_HUB)
                                                {
                                                        for (XsIntArray::const_iterator hi = hubs.begin(); hi != hubs.end(); ++hi)
                                                        {
                                                                if (*hi == hub)
                                                                {
                                                                        behindXsensHub = hub;
                                                                        break;
                                                                }
                                                        }
                                                }
                                        }
                                }
                        }
                }
                // Close the key now that we are finished with it
                RegCloseKey(hDeviceKey);
        }
 
        //  Cleanup
 
        SetupDiDestroyDeviceInfoList(hDevInfo);
        return XsUsbHubInfo(behindXsensHub);
#else
        XsUsbHubInfo inf;
 
        Udev xsudev;
 
        udev* udevInstance = xsudev.unew();
        if (udevInstance == NULL)
        {
                fprintf(stderr, "Unable to create udev object\n");
                return XsUsbHubInfo();
        }
 
        udev_enumerate* enumerate = xsudev.enumerate_new(udevInstance);
        xsudev.enumerate_scan_devices(enumerate);
 
        udev_list_entry* devices = xsudev.enumerate_get_list_entry(enumerate);
        udev_list_entry* dev;
        for (dev = devices; dev != NULL; dev = xsudev.list_entry_get_next(dev))
        {
                const char* path = xsudev.list_entry_get_name(dev);
                udev_device* device = xsudev.device_new_from_syspath(udevInstance, path);
                if (!device)
                        return XsUsbHubInfo();
 
                const char* devnode = xsudev.device_get_devnode(device);
                if (!devnode || strcmp(devnode, portInfo.portName_c_str()) != 0)
                        continue;
 
                udev_device* parent = xsudev.device_get_parent_with_subsystem_devtype(device, "usb", "usb_device");
                if (!parent)
                        break;
 
                const char* devpath = xsudev.device_get_sysattr_value(parent, "devpath");
                if (!devpath)
                        break;
 
                inf = XsUsbHubInfo(devpath);
                break;
        }
        xsudev.enumerate_unref(enumerate);
        xsudev.unref(udevInstance);
        return inf;
#endif
}