enumerateusbdevices.cpp

Go to the documentation of this file.

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

#ifdef _WIN32
#include <windows.h>

#include <devguid.h>
#include <regstr.h>
#include <setupapi.h>
#include <string.h>
#else
#include <dirent.h>
#include <cstdlib>
#include <cstring>
#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
}