enumerateusbdevices.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#include "xcommunicationconfig.h"
#include "enumerateusbdevices.h"

#ifdef _WIN32
#include <windows.h>
#include <string.h>
#include <setupapi.h>
#include <devguid.h>
#include <regstr.h>
#else
#include <stdlib.h>
#include <string.h>
#include <dirent.h>
#include "xslibusb.h"
#endif

#define XSENS_VENDOR_ID 0x2639
#define ATMEL_VENDOR_ID 0x03eb  // needed for old MTw
#define ATMEL_BORROWED_PRODUCT_ID 0x2307  // needed for old MTw

/*! \brief Enumerate Xsens USB devices

        If the OS already has drivers running for a device, the device should
   already have been
        found by xsEnumerateSerialPorts().

        \param[in,out] ports The list of serial ports to append to
*/
bool xsEnumerateUsbDevices(XsPortInfoList& ports)
{
        XsPortInfo current;
#ifdef USE_WINUSB
        BOOL bResult = FALSE;
        ULONG length;
        ULONG requiredLength = 0;

        // {FD51225C-700A-47e5-9999-B2D9031B88ED}
        GUID guid = {0xfd51225c,
                                 0x700a,
                                 0x47e5,
                                 {0x99, 0x99, 0xb2, 0xd9, 0x3, 0x1b, 0x88, 0xed}};

        HDEVINFO deviceInfo;
        SP_DEVICE_INTERFACE_DATA interfaceData;
        PSP_DEVICE_INTERFACE_DETAIL_DATA_A detailData = nullptr;

        deviceInfo = SetupDiGetClassDevs(
                &guid, nullptr, nullptr, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE);

        // Initialize variables.
        interfaceData.cbSize = sizeof(SP_INTERFACE_DEVICE_DATA);
        int port = 0;
        for (DWORD dwIndex = 0; port == 0; ++dwIndex)
        {
                BOOL bRet = SetupDiEnumDeviceInterfaces(
                        deviceInfo, nullptr, &guid, dwIndex, &interfaceData);
                if (!bRet)
                {
                        if (GetLastError() == ERROR_NO_MORE_ITEMS) break;
                }
                else
                {
                        if (!SetupDiGetDeviceInterfaceDetail(
                                        deviceInfo, &interfaceData, nullptr, 0, &requiredLength,
                                        nullptr))
                        {
                                if (GetLastError() != ERROR_INSUFFICIENT_BUFFER)
                                {
                                        SetupDiDestroyDeviceInfoList(deviceInfo);
                                        return false;
                                }
                        }
                        detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA_A)LocalAlloc(
                                LMEM_FIXED, requiredLength);
                        if (nullptr == detailData)
                        {
                                SetupDiDestroyDeviceInfoList(deviceInfo);
                                return false;
                        }

                        detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A);
                        length = requiredLength;
                        SP_DEVINFO_DATA DevInfoData;
                        DevInfoData.cbSize = sizeof(SP_DEVINFO_DATA);
                        bResult = SetupDiGetDeviceInterfaceDetailA(
                                deviceInfo, &interfaceData, detailData, length, &requiredLength,
                                &DevInfoData);

                        if (!bResult)
                        {
                                LocalFree(detailData);
                                SetupDiDestroyDeviceInfoList(deviceInfo);
                                return false;
                        }

                        unsigned char serialNumber[256];
                        char *ptrEnd, *ptrStart = strchr(detailData->DevicePath, '#');
                        if (!ptrStart) continue;
                        ptrStart = strchr(ptrStart + 1, '#');
                        if (!ptrStart) continue;
                        ptrEnd = strchr(ptrStart + 1, '#');
                        if (!ptrEnd) continue;

                        strncpy((char*)serialNumber, ptrStart + 1, ptrEnd - ptrStart - 1);
                        serialNumber[ptrEnd - ptrStart - 1] = '\0';

                        current.setPortName(detailData->DevicePath);

                        int id = 0;
                        sscanf((const char*)serialNumber, "%X", &id);
                        current.setDeviceId((uint32_t)id);

                        ports.push_back(current);
                }
        }

        SetupDiDestroyDeviceInfoList(deviceInfo);
        return true;
#else
        XsLibUsb libUsb;
        libusb_context* context;
        int result = libUsb.init(&context);
        if (result != LIBUSB_SUCCESS) return false;

        libusb_device** deviceList;
        ssize_t deviceCount = libUsb.get_device_list(context, &deviceList);
        for (ssize_t i = 0; i < deviceCount; i++)
        {
                libusb_device* device = deviceList[i];
                libusb_device_descriptor desc;
                result = libUsb.get_device_descriptor(device, &desc);
                if (result != LIBUSB_SUCCESS) continue;

                if (desc.idVendor != XSENS_VENDOR_ID &&
                        desc.idVendor != ATMEL_VENDOR_ID)
                        continue;

                libusb_device_handle* handle;
                result = libUsb.open(device, &handle);
                if (result != LIBUSB_SUCCESS)
                {
                        libUsb.unref_device(device);
                        continue;
                }

                unsigned char serialNumber[256];
                result = libUsb.get_string_descriptor_ascii(
                        handle, desc.iSerialNumber, serialNumber, 256);

                if (desc.idVendor == ATMEL_VENDOR_ID &&
                        desc.idProduct == ATMEL_BORROWED_PRODUCT_ID)
                {
                        unsigned char productName[256];
                        result = libUsb.get_string_descriptor_ascii(
                                handle, desc.iProduct, productName, 256);

                        if (strcmp("Xsens COM port", (const char*)productName) != 0)
                        {
                                libUsb.close(handle);
                                continue;
                        }
                }

                libusb_config_descriptor* configDesc;
                result = libUsb.get_active_config_descriptor(device, &configDesc);
                if (result != LIBUSB_SUCCESS) continue;

                bool kernelActive = false;
                for (uint8_t ifCount = 0; ifCount < configDesc->bNumInterfaces;
                         ++ifCount)
                {
                        int res = libUsb.kernel_driver_active(handle, ifCount);
                        kernelActive |= (res == 1);
                }

                libUsb.free_config_descriptor(configDesc);

                if (!kernelActive)
                {
                        char name[256];
                        sprintf(
                                name, "USB%03u:%03u", libUsb.get_bus_number(device),
                                libUsb.get_device_address(device));
                        current.setPortName(name);

                        int id = 0;
                        sscanf((const char*)serialNumber, "%d", &id);
                        current.setDeviceId((uint32_t)id);
                        ports.push_back(current);
                }
                else
                {
                        JLDEBUG(
                                gJournal, "Kernel driver active on USB"
                                                          << libUsb.get_bus_number(device) << ":"
                                                          << libUsb.get_device_address(device) << " device "
                                                          << serialNumber);
                }
                libUsb.close(handle);
        }
        libUsb.free_device_list(deviceList, 1);
        libUsb.exit(context);
        return true;
#endif
}