From 5732c2fd4c0e566c59dbe293e7ebc29fec827664 Mon Sep 17 00:00:00 2001 From: Mathieu Rondonneau Date: Sat, 1 Jun 2013 22:13:04 -0700 Subject: [PATCH] OP-958L: Some fixes for 15s hang and unplug/plug issue reported lately. This is not cleanedup but I want to get this in so other people can test. --- .../src/plugins/ophid/inc/ophid_const.h | 1 + .../src/plugins/ophid/inc/ophid_usbmon.h | 2 +- .../plugins/ophid/src/ophid_usbmon_win.cpp | 225 ++++++++++++------ 3 files changed, 153 insertions(+), 75 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_const.h b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_const.h index f0928891e..e583cc8ec 100644 --- a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_const.h +++ b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_const.h @@ -60,5 +60,6 @@ #define OPHID_ERROR_PARAMETER -3 #define OPHID_ERROR_HANDLE -4 #define OPHID_ERROR_INIT -5 +#define OPHID_ERROR_ENUMERATION -6 #endif // OPHID_CONST_H diff --git a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h index b39c74952..89bf6a806 100644 --- a/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h +++ b/ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h @@ -194,7 +194,7 @@ private: */ static QString getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData, DWORD property); static int infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & devInfo, DWORD & index); - static void enumerateDevicesWin(const GUID & guidDev, QList *infoList); + void enumerateDevicesWin(const GUID & guidDev); bool matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam); #ifdef QT_GUI_LIB USBRegistrationWidget *notificationWidget; diff --git a/ground/openpilotgcs/src/plugins/ophid/src/ophid_usbmon_win.cpp b/ground/openpilotgcs/src/plugins/ophid/src/ophid_usbmon_win.cpp index 8d16e7099..f0fb8b2e4 100644 --- a/ground/openpilotgcs/src/plugins/ophid/src/ophid_usbmon_win.cpp +++ b/ground/openpilotgcs/src/plugins/ophid/src/ophid_usbmon_win.cpp @@ -36,6 +36,8 @@ #define printf qDebug +static QMutex DeviceChangeMutex; + void USBMonitor::deviceEventReceived() { qDebug() << "Device event"; @@ -80,34 +82,34 @@ USBMonitor::~USBMonitor() */ QList USBMonitor::availableDevices(int vid, int pid, int bcdDeviceMSB, int bcdDeviceLSB) { - QList allPorts = availableDevices(); - qDebug() << "USBMonitor::availableDevices list off all ports:"; - qDebug() << "USBMonitor::availableDevices total ports:" << allPorts.length(); - foreach(USBPortInfo info, allPorts) { - qDebug() << "----------"; - qDebug() << "bcdDevice:" << info.bcdDevice; - qDebug() << "devicePath:" << info.devicePath; - qDebug() << "product:" << info.product; - } - qDebug() << "END OF LIST"; + int i; QList thePortsWeWant; - qDebug() << "USBMonitor::availableDevices bcdLSB=" << bcdDeviceLSB; - foreach(USBPortInfo port, allPorts) { - qDebug() << "USBMonitorWin:Port VID=" << port.vendorID << "PID=" << port.productID << "bcddevice=" << port.bcdDevice; - if ((port.vendorID == vid || vid == -1) && (port.productID == pid || pid == -1) && ((port.bcdDevice >> 8) == bcdDeviceMSB || bcdDeviceMSB == -1) && - ((port.bcdDevice & 0x00ff) == bcdDeviceLSB || bcdDeviceLSB == -1)) { - thePortsWeWant.append(port); + + OPHID_TRACE("IN"); + + // Trigger enumeration + //OPHID_DEBUG("Enumerate"); + //availableDevices(); + + // Print the list + qDebug() << "List off (" << knowndevices.length() << ") devices that are tracked:"; + foreach(USBPortInfo info, knowndevices /*thePortsWeWant*/) { + qDebug() << "product:" << info.product + << " bcdDevice:" << info.bcdDevice + << " devicePath:" << info.devicePath; + + // + if ((info.vendorID == vid || vid == -1) && + (info.productID == pid || pid == -1) && + ((info.bcdDevice >> 8) == bcdDeviceMSB || bcdDeviceMSB == -1) && + ((info.bcdDevice & 0x00ff) == bcdDeviceLSB || bcdDeviceLSB == -1)) { + thePortsWeWant.append(info); + OPHID_DEBUG("Found device."); } } - qDebug() << "USBMonitor::availableDevices list off matching ports vid pid bcdMSD bcdLSD:" << vid << pid << bcdDeviceMSB << bcdDeviceLSB; - qDebug() << "USBMonitor::availableDevices total matching ports:" << thePortsWeWant.length(); - foreach(USBPortInfo info, thePortsWeWant) { - qDebug() << "----------"; - qDebug() << "bcdDevice:" << info.bcdDevice; - qDebug() << "devicePath:" << info.devicePath; - qDebug() << "product:" << info.product; - } - qDebug() << "END OF LIST"; + + OPHID_TRACE("OUT"); + return thePortsWeWant; } @@ -131,8 +133,8 @@ QList USBMonitor::availableDevices(int vid, int pid, int bcdDeviceM #define TCHARToQString(x) QString::fromLocal8Bit((x)) #define TCHARToQStringN(x, y) QString::fromLocal8Bit((x), (y)) #endif /*UNICODE*/ -#define MATCHOPHIDSTRING "COL01" -#define MATCHOPBLSTRING "161A0549" +#define HID2IGNOREUC "COL02" +#define HID2IGNORElc "col02" void USBMonitor::setUpNotifications() { @@ -160,6 +162,7 @@ void USBMonitor::setUpNotifications() } LRESULT USBMonitor::onDeviceChangeWin(WPARAM wParam, LPARAM lParam) { + OPHID_TRACE("IN"); if (DBT_DEVICEARRIVAL == wParam || DBT_DEVICEREMOVECOMPLETE == wParam) { PDEV_BROADCAST_HDR pHdr = (PDEV_BROADCAST_HDR)lParam; if (pHdr->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) { @@ -169,34 +172,46 @@ LRESULT USBMonitor::onDeviceChangeWin(WPARAM wParam, LPARAM lParam) matchAndDispatchChangedDevice(deviceID, guid_hid, wParam); } } + OPHID_TRACE("OUT"); return 0; } #ifdef QT_GUI_LIB bool USBRegistrationWidget::winEvent(MSG *message, long *result) { + bool ret = false; + if (message->message == WM_DEVICECHANGE) { + OPHID_TRACE("IN"); qese->onDeviceChangeWin(message->wParam, message->lParam); *result = 1; - return true; + ret = true; + OPHID_TRACE("OUT"); } - return false; + + return ret; } #endif bool USBMonitor::matchAndDispatchChangedDevice(const QString & deviceID, const GUID & guid, WPARAM wParam) { qDebug() << "USB_MONITOR matchAndDispatchChangedDevice deviceID=" << deviceID; bool rv = false; - DWORD dwFlag = (DBT_DEVICEARRIVAL == wParam) ? DIGCF_PRESENT : DIGCF_ALLCLASSES; + bool rc; + DWORD dwFlag = (DBT_DEVICEARRIVAL == wParam) ? DIGCF_PRESENT : 0/*DIGCF_ALLCLASSES*/; HDEVINFO devInfo; +// QString qHIDSymbolicName="HID"; +// WCHAR wHIDSymbolicName[10]; +// qHIDSymbolicName.toWCharArray(wHIDSymbolicName); + if ((devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, dwFlag | DIGCF_DEVICEINTERFACE)) != INVALID_HANDLE_VALUE) { SP_DEVINFO_DATA spDevInfoData; spDevInfoData.cbSize = sizeof(SP_DEVINFO_DATA); for (DWORD i = 0; SetupDiEnumDeviceInfo(devInfo, i, &spDevInfoData); i++) { DWORD nSize = 0; TCHAR buf[MAX_PATH]; - if (SetupDiGetDeviceInstanceId(devInfo, &spDevInfoData, buf, MAX_PATH, &nSize) && - deviceID.contains(TCHARToQString(buf)) && (deviceID.contains(MATCHOPHIDSTRING) || - deviceID.contains(MATCHOPBLSTRING))) { // we found a match + rc = SetupDiGetDeviceInstanceId(devInfo, &spDevInfoData, buf, MAX_PATH, &nSize); + qDebug() << "PATH:" << TCHARToQString(buf); + if (rc && deviceID.contains(TCHARToQString(buf)) /*&& !QString().contains(TCHARToQString(HID2IGNOREUC))*/) { // we found a match + qDebug() << "ACCEPTED" << TCHARToQString(buf); USBPortInfo info; info.devicePath = deviceID; if (wParam == DBT_DEVICEARRIVAL) { @@ -205,46 +220,61 @@ bool USBMonitor::matchAndDispatchChangedDevice(const QString & deviceID, const G qDebug() << "USB_MONITOR infoFromHandle failed on matchAndDispatchChangedDevice"; break; } - bool m_break = false; - foreach(USBPortInfo m_info, knowndevices) { - if (m_info.serialNumber == info.serialNumber && m_info.productID == info.productID && m_info.bcdDevice == info.bcdDevice && m_info.devicePath == info.devicePath) { - qDebug() << "USB_MONITOR device already present don't emit signal"; - m_break = true; + if (knowndevices.length()) { + foreach(USBPortInfo m_info, knowndevices) { + + qDebug() << m_info.devicePath; + qDebug() << info.devicePath; + + if (m_info.serialNumber == info.serialNumber && + m_info.productID == info.productID && + m_info.bcdDevice == info.bcdDevice && + m_info.devicePath == info.devicePath) { + qDebug() << "USB_MONITOR device already present don't emit signal"; + break; + } } - } - if (m_break) { - break; - } + } if (info.bcdDevice == 0 || info.product.isEmpty()) { qDebug() << "USB_MONITOR empty information on device not emiting signal"; break; } knowndevices.append(info); - qDebug() << "USB_MONITOR emit device discovered on device:" << info.product << info.bcdDevice; + qDebug() << "USB_MONITOR emit device discovered on device:" + << info.product + << info.bcdDevice; emit deviceDiscovered(info); break; } else if (wParam == DBT_DEVICEREMOVECOMPLETE) { bool found = false; + // DeviceChangeMutex.lock(); for (int x = 0; x < knowndevices.count(); ++x) { - if (knowndevices[x].devicePath == deviceID) { + // if (knowndevices[x].devicePath == deviceID) { USBPortInfo temp = knowndevices.at(x); knowndevices.removeAt(x); qDebug() << "USB_MONITOR emit device removed on device:" << temp.product << temp.bcdDevice; - emit deviceRemoved(temp); + + //emit deviceRemoved(temp); + found = true; - break; - } + // break; + // } } - if (!found) { - qDebug() << "USB_MONITOR emit device removed on unknown device"; + + //if (!found) { + // qDebug() << "USB_MONITOR emit device removed on unknown device"; emit deviceRemoved(info); - } + //} + //else + break; +// DeviceChangeMutex.unlock(); } break; } } SetupDiDestroyDeviceInfoList(devInfo); } + OPHID_TRACE("OUT"); return rv; } @@ -259,44 +289,57 @@ QString USBMonitor::getDeviceProperty(HDEVINFO devInfo, PSP_DEVINFO_DATA devData delete[] buff; return result; } -/** - Returns a list of all currently available devices - */ -QList USBMonitor::availableDevices() -{ - QList ports; - enumerateDevicesWin(guid_hid, &ports); - // qDebug()<<"USBMonitorWin availabledevices="< USBMonitor::availableDevices() { + + enumerateDevicesWin(guid_hid); + return knowndevices; } -void USBMonitor::enumerateDevicesWin(const GUID & guid, QList *infoList) + +void USBMonitor::enumerateDevicesWin(const GUID & guid) { HDEVINFO devInfo; USBPortInfo info; + DWORD j = 0; + DWORD i = 0; + OPHID_TRACE("IN"); + DeviceChangeMutex.lock(); +// QString qHIDSymbolicName = "HID"; +// WCHAR wHIDSymbolicName[10]; +// qHIDSymbolicName.toWCharArray(wHIDSymbolicName); // qDebug()<<"enumerateDevicesWin1"; if ((devInfo = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE)) != INVALID_HANDLE_VALUE) { // qDebug()<<"enumerateDevicesWin2"; SP_DEVINFO_DATA devInfoData; devInfoData.cbSize = sizeof(SP_DEVINFO_DATA); - for (DWORD i = 0; SetupDiEnumDeviceInfo(devInfo, i, &devInfoData); i++) { + // only handle the first one enumerated that matches openpilot vendor + // the second one is expected to be the gamepad controller which we don't want to deal with + for (i = 0; SetupDiEnumDeviceInfo(devInfo, i, &devInfoData); i++) { int r = infoFromHandle(guid, info, devInfo, i); if (r == 1) { - infoList->append(info); - } else if (r == 0) { + //infoList->append(info); + knowndevices.append(info); + j++; + // break; + } else if (r == OPHID_ERROR_ENUMERATION) { break; } } - SetupDiDestroyDeviceInfoList(devInfo); } + OPHID_DEBUG("Added %d device(s).", j); + DeviceChangeMutex.unlock(); + OPHID_TRACE("OUT"); } int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & devInfo, DWORD & index) { + OPHID_TRACE("IN"); // qDebug()<<"index0="<DevicePath /*DevicePath*/, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); if (h == INVALID_HANDLE_VALUE) { DWORD err = GetLastError(); @@ -335,13 +398,15 @@ int USBMonitor::infoFromHandle(const GUID & guid, USBPortInfo & info, HDEVINFO & // Let's not log it :) if (err == ERROR_ACCESS_DENIED) { free(details); - return 2; + ret = 2; + goto leave; } qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); free(details); - return 2; + ret = 2; + goto leave; } // qDebug()<<"index4="<