diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h index 3c94aa64e..e2b4de860 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid.h @@ -1,52 +1,139 @@ -/** - ****************************************************************************** - * - * @file pjrc_rawhid.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup RawHIDPlugin Raw HID Plugin - * @{ - * @brief Impliments a HID USB connection to the flight hardware as a QIODevice - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PJRC_RAWHID_H -#define PJRC_RAWHID_H - -#include -#include -#include -#include -#include -#include "rawhid_global.h" - -class RAWHID_EXPORT pjrc_rawhid -{ -public: - pjrc_rawhid(); - int open(int max, int vid, int pid, int usage_page, int usage); - int receive(int num, void *buf, int len, int timeout); - void close(int num); - int send(int num, void *buf, int len, int timeout); - QString getserial(int num); - void mytest(int num); -private: - -}; - -#endif // PJRC_RAWHID_H +/** + ****************************************************************************** + * + * @file pjrc_rawhid.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup RawHIDPlugin Raw HID Plugin + * @{ + * @brief Impliments a HID USB connection to the flight hardware as a QIODevice + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PJRC_RAWHID_H +#define PJRC_RAWHID_H + +#include +#include +#include +#include +#include +#include "rawhid_global.h" + +#if defined( Q_OS_MAC) + + // todo: + +#elif defined(Q_OS_UNIX) +//#elif defined(Q_OS_LINUX) + #include + #include + #include +#elif defined(Q_OS_WIN32) + #include + #include + #include + #include +#endif + +// ************ + +#if defined( Q_OS_MAC) + + // todo: + +#elif defined(Q_OS_UNIX) +//#elif defined(Q_OS_LINUX) + + typedef struct hid_struct hid_t; + struct hid_struct + { + usb_dev_handle *usb; + int open; + int iface; + int ep_in; + int ep_out; + struct hid_struct *prev; + struct hid_struct *next; + }; + +#elif defined(Q_OS_WIN32) + + typedef struct hid_struct hid_t; + struct hid_struct + { + HANDLE handle; + int open; + struct hid_struct *prev; + struct hid_struct *next; + }; + +#endif + +// ************ + +class RAWHID_EXPORT pjrc_rawhid +{ + +public: + pjrc_rawhid(); + ~pjrc_rawhid(); + + int open(int max, int vid, int pid, int usage_page, int usage); + int receive(int num, void *buf, int len, int timeout); + void close(int num); + int send(int num, void *buf, int len, int timeout); + QString getserial(int num); + void mytest(int num); + +private: + #if defined( Q_OS_MAC) + + // todo: + + #elif defined(Q_OS_UNIX) + //#elif defined(Q_OS_LINUX) + + hid_t *first_hid; + hid_t *last_hid; + + void add_hid(hid_t *h); + hid_t * get_hid(int num); + void free_all_hid(void); + void hid_close(hid_t *hid); + int hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *end); + + #elif defined(Q_OS_WIN32) + + hid_t *first_hid; + hid_t *last_hid; + HANDLE rx_event; + HANDLE tx_event; + CRITICAL_SECTION rx_mutex; + CRITICAL_SECTION tx_mutex; + + void add_hid(hid_t *h); + hid_t * get_hid(int num); + void free_all_hid(void); + void hid_close(hid_t *hid); + void print_win32_err(void); + + #endif +}; + +#endif diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp index 52f2829e2..34c78fa8f 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp @@ -1,449 +1,453 @@ -/* @file pjrc_rawhid_mac.cpp - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup RawHIDPlugin Raw HID Plugin - * @{ - * @brief Impliments a HID USB connection to the flight hardware as a QIODevice - *****************************************************************************/ - -/* Simple Raw HID functions for Linux - for use with Teensy RawHID example - * http://www.pjrc.com/teensy/rawhid.html - * Copyright (c) 2009 PJRC.COM, LLC - * - * rawhid_open - open 1 or more devices - * rawhid_recv - receive a packet - * rawhid_send - send a packet - * rawhid_close - close a device - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above description, website URL and copyright notice and this permission - * notice shall be included in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - * - * Version 1.0: Initial Release - */ - - -#include "pjrc_rawhid.h" - -#include -#include -#include -#include -#include - -#define BUFFER_SIZE 64 - -//#define printf qDebug -#define printf - -typedef struct hid_struct hid_t; -typedef struct buffer_struct buffer_t; -static hid_t *first_hid = NULL; -static hid_t *last_hid = NULL; -struct hid_struct { - IOHIDDeviceRef ref; - int open; - uint8_t buffer[BUFFER_SIZE]; - buffer_t *first_buffer; - buffer_t *last_buffer; - struct hid_struct *prev; - struct hid_struct *next; -}; -struct buffer_struct { - struct buffer_struct *next; - uint32_t len; - uint8_t buf[BUFFER_SIZE]; -}; - -static void add_hid(hid_t *); -static hid_t * get_hid(int); -static void free_all_hid(void); -static void hid_close(hid_t *); -static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); -static void detach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); -static void timeout_callback(CFRunLoopTimerRef, void *); -static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); -static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len); - -pjrc_rawhid::pjrc_rawhid() -{ - first_hid = NULL; - last_hid = NULL; -} - -// open - open 1 or more devices -// -// Inputs: -// max = maximum number of devices to open -// vid = Vendor ID, or -1 if any -// pid = Product ID, or -1 if any -// usage_page = top level usage page, or -1 if any -// usage = top level usage number, or -1 if any -// Output: -// actual number of devices opened -// -int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) -{ - static IOHIDManagerRef hid_manager=NULL; - CFMutableDictionaryRef dict; - CFNumberRef num; - IOReturn ret; - hid_t *p; - int count=0; - - if (first_hid) free_all_hid(); - //printf("pjrc_rawhid_open, max=%d\n", max); - if (max < 1) return 0; - // Start the HID Manager - // http://developer.apple.com/technotes/tn2007/tn2187.html - if (!hid_manager) { - hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); - if (hid_manager == NULL || CFGetTypeID(hid_manager) != IOHIDManagerGetTypeID()) { - if (hid_manager) CFRelease(hid_manager); - return 0; - } - } - if (vid > 0 || pid > 0 || usage_page > 0 || usage > 0) { - // Tell the HID Manager what type of devices we want - dict = CFDictionaryCreateMutable(kCFAllocatorDefault, 0, - &kCFTypeDictionaryKeyCallBacks, &kCFTypeDictionaryValueCallBacks); - if (!dict) return 0; - if (vid > 0) { - num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &vid); - CFDictionarySetValue(dict, CFSTR(kIOHIDVendorIDKey), num); - CFRelease(num); - } - if (pid > 0) { - num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &pid); - CFDictionarySetValue(dict, CFSTR(kIOHIDProductIDKey), num); - CFRelease(num); - } - if (usage_page > 0) { - num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &usage_page); - CFDictionarySetValue(dict, CFSTR(kIOHIDPrimaryUsagePageKey), num); - CFRelease(num); - } - if (usage > 0) { - num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &usage); - CFDictionarySetValue(dict, CFSTR(kIOHIDPrimaryUsageKey), num); - CFRelease(num); - } - IOHIDManagerSetDeviceMatching(hid_manager, dict); - CFRelease(dict); - } else { - IOHIDManagerSetDeviceMatching(hid_manager, NULL); - } - // set up a callbacks for device attach & detach - IOHIDManagerScheduleWithRunLoop(hid_manager, CFRunLoopGetCurrent(), - kCFRunLoopDefaultMode); - IOHIDManagerRegisterDeviceMatchingCallback(hid_manager, attach_callback, NULL); - IOHIDManagerRegisterDeviceRemovalCallback(hid_manager, detach_callback, NULL); - ret = IOHIDManagerOpen(hid_manager, kIOHIDOptionsTypeNone); - if (ret != kIOReturnSuccess) { - printf("Could not start IOHIDManager"); - IOHIDManagerUnscheduleFromRunLoop(hid_manager, - CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); - CFRelease(hid_manager); - return 0; - } - printf("run loop\n"); - // let it do the callback for all devices - while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, true) == kCFRunLoopRunHandledSource) ; - // count up how many were added by the callback - for (p = first_hid; p; p = p->next) count++; - return count; -} - -// recveive - receive a packet -// Inputs: -// num = device to receive from (zero based) -// buf = buffer to receive packet -// len = buffer's size -// timeout = time to wait, in milliseconds -// Output: -// number of bytes received, or -1 on error -// -int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) -{ - hid_t *hid; - buffer_t *b; - CFRunLoopTimerRef timer=NULL; - CFRunLoopTimerContext context; - int ret=0, timeout_occurred=0; - - if (len < 1) return 0; - hid = get_hid(num); - if (!hid || !hid->open) return -1; - if ((b = hid->first_buffer) != NULL) { - if (len > b->len) len = b->len; - memcpy(buf, b->buf, len); - hid->first_buffer = b->next; - free(b); - return len; - } - memset(&context, 0, sizeof(context)); - context.info = &timeout_occurred; - timer = CFRunLoopTimerCreate(NULL, CFAbsoluteTimeGetCurrent() + - (double)timeout / 1000.0, 0, 0, 0, timeout_callback, &context); - CFRunLoopAddTimer(CFRunLoopGetCurrent(), timer, kCFRunLoopDefaultMode); - while (1) { - CFRunLoopRun(); - if ((b = hid->first_buffer) != NULL) { - if (len > b->len) len = b->len; - memcpy(buf, b->buf, len); - hid->first_buffer = b->next; - free(b); - ret = len; - break; - } - if (!hid->open) { - printf("pjrc_rawhid_recv, device not open\n"); - ret = -1; - break; - } - if (timeout_occurred) break; - } - CFRunLoopTimerInvalidate(timer); - CFRelease(timer); - return ret; -} - -// send - send a packet -// Inputs: -// num = device to transmit to (zero based) -// buf = buffer containing packet to send -// len = number of bytes to transmit -// timeout = time to wait, in milliseconds -// Output: -// number of bytes sent, or -1 on error -// -int pjrc_rawhid::send(int num, void *buf, int len, int timeout) -{ - hid_t *hid; - int result=-100; - - hid = get_hid(num); - if (!hid || !hid->open) return -1; -#if 1 -#warning "Send timeout not implemented on MACOSX" - uint8_t *report_buf = (uint8_t *) malloc(len); - memcpy(&report_buf[0], buf,len); - // Note: packet processing done in OS indepdent code - IOReturn ret = IOHIDDeviceSetReport(hid->ref, kIOHIDReportTypeOutput, 2, (uint8_t *)report_buf, len); - result = (ret == kIOReturnSuccess) ? len : -1; - if (err_get_system(ret) == err_get_system(sys_iokit)) - { - - // The error was in the I/O Kit system - UInt32 codeValue = err_get_code(ret); - qDebug("Returned: %x", codeValue); - // Can now perform test on error code, display it to user, or whatever. - usleep(1000000); - } - -#endif -#if 0 - // No matter what I tried this never actually sends an output - // report and output_callback never gets called. Why?? - // Did I miss something? This is exactly the same params as - // the sync call that works. Is it an Apple bug? - // (submitted to Apple on 22-sep-2009, problem ID 7245050) - // - IOHIDDeviceScheduleWithRunLoop(hid->ref, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); - // should already be scheduled with run loop by attach_callback, - // sadly this doesn't make any difference either way - // - IOHIDDeviceSetReportWithCallback(hid->ref, kIOHIDReportTypeOutput, - 0, buf, len, (double)timeout / 1000.0, output_callback, &result); - while (1) { - printf("enter run loop (send)\n"); - CFRunLoopRun(); - printf("leave run loop (send)\n"); - if (result > -100) break; - if (!hid->open) { - result = -1; - break; - } - } -#endif - return result; -} - -QString pjrc_rawhid::getserial(int num) { - hid_t *hid; - char buf[128]; - - hid = get_hid(num); - - if (!hid || !hid->open) return QString("Error"); - - CFTypeRef serialnum = IOHIDDeviceGetProperty(hid->ref, CFSTR(kIOHIDSerialNumberKey)); - if(serialnum && CFGetTypeID(serialnum) == CFStringGetTypeID()) - { - //Note: I'm not sure it will always succeed if encoded as MacRoman but that - //is a superset of UTF8 so I think this is fine - CFStringRef str = (CFStringRef)serialnum; - const char * buf = CFStringGetCStringPtr(str, kCFStringEncodingMacRoman); - return QString(buf); - } - - return QString("Error"); -} - -// close - close a device -// -// Inputs: -// num = device to close (zero based) -// Output -// (nothing) -// -void pjrc_rawhid::close(int num) -{ - hid_t *hid; - - hid = get_hid(num); - if (!hid || !hid->open) return; - hid_close(hid); - hid->open = 0; -} - -// -// -// Private Functions -// -// -static void input_callback(void *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) -{ - buffer_t *n; - hid_t *hid; - - printf("input_callback, report id: %i buf: %x %x, len: %d\n", id, data[0], data[1], len); - if (ret != kIOReturnSuccess || len < 1) return; - hid = (hid_t*)context; - if (!hid || hid->ref != sender) return; - n = (buffer_t *)malloc(sizeof(buffer_t)); - if (!n) return; - if (len > BUFFER_SIZE) len = BUFFER_SIZE; - // Note: packet preprocessing done in OS independent code - memcpy(n->buf, &data[0], len); - n->len = len; - n->next = NULL; - if (!hid->first_buffer || !hid->last_buffer) { - hid->first_buffer = hid->last_buffer = n; - } else { - hid->last_buffer->next = n; - hid->last_buffer = n; - } - CFRunLoopStop(CFRunLoopGetCurrent()); -} - -static void timeout_callback(CFRunLoopTimerRef timer, void *info) -{ - printf("timeout_callback\n"); - *(int *)info = 1; - CFRunLoopStop(CFRunLoopGetCurrent()); -} - -static void add_hid(hid_t *h) -{ - if (!first_hid || !last_hid) { - first_hid = last_hid = h; - h->next = h->prev = NULL; - return; - } - last_hid->next = h; - h->prev = last_hid; - h->next = NULL; - last_hid = h; -} - - -static hid_t * get_hid(int num) -{ - hid_t *p; - for (p = first_hid; p && num > 0; p = p->next, num--) ; - return p; -} - - -static void free_all_hid(void) -{ - hid_t *p, *q; - - for (p = first_hid; p; p = p->next) { - hid_close(p); - } - p = first_hid; - while (p) { - q = p; - p = p->next; - free(q); - } - first_hid = last_hid = NULL; -} - - -static void hid_close(hid_t *hid) -{ - if (!hid || !hid->open || !hid->ref) return; - IOHIDDeviceUnscheduleFromRunLoop(hid->ref, CFRunLoopGetCurrent( ), kCFRunLoopDefaultMode); - IOHIDDeviceClose(hid->ref, kIOHIDOptionsTypeNone); - hid->ref = NULL; -} - -static void detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) -{ - hid_t *p; - - printf("detach callback\n"); - for (p = first_hid; p; p = p->next) { - if (p->ref == dev) { - p->open = 0; - CFRunLoopStop(CFRunLoopGetCurrent()); - return; - } - } -} - -static void attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) -{ - struct hid_struct *h; - - printf("attach callback\n"); - if (IOHIDDeviceOpen(dev, kIOHIDOptionsTypeNone) != kIOReturnSuccess) return; - h = (hid_t *)malloc(sizeof(hid_t)); - if (!h) return; - memset(h, 0, sizeof(hid_t)); - IOHIDDeviceScheduleWithRunLoop(dev, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); - IOHIDDeviceRegisterInputReportCallback(dev, h->buffer, sizeof(h->buffer), input_callback, h); - h->ref = dev; - h->open = 1; - add_hid(h); -} - -static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) -{ - printf("output_callback, r=%d\n", ret); - if (ret == kIOReturnSuccess) { - *(int *)context = len; - } else { - // timeout if not success? - *(int *)context = 0; - } - CFRunLoopStop(CFRunLoopGetCurrent()); -} - +/* @file pjrc_rawhid_mac.cpp + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup RawHIDPlugin Raw HID Plugin + * @{ + * @brief Impliments a HID USB connection to the flight hardware as a QIODevice + *****************************************************************************/ + +/* Simple Raw HID functions for Linux - for use with Teensy RawHID example + * http://www.pjrc.com/teensy/rawhid.html + * Copyright (c) 2009 PJRC.COM, LLC + * + * rawhid_open - open 1 or more devices + * rawhid_recv - receive a packet + * rawhid_send - send a packet + * rawhid_close - close a device + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above description, website URL and copyright notice and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * Version 1.0: Initial Release + */ + + +#include "pjrc_rawhid.h" + +#include +#include +#include +#include +#include + +#define BUFFER_SIZE 64 + +//#define printf qDebug +#define printf + +typedef struct hid_struct hid_t; +typedef struct buffer_struct buffer_t; +static hid_t *first_hid = NULL; +static hid_t *last_hid = NULL; +struct hid_struct { + IOHIDDeviceRef ref; + int open; + uint8_t buffer[BUFFER_SIZE]; + buffer_t *first_buffer; + buffer_t *last_buffer; + struct hid_struct *prev; + struct hid_struct *next; +}; +struct buffer_struct { + struct buffer_struct *next; + uint32_t len; + uint8_t buf[BUFFER_SIZE]; +}; + +static void add_hid(hid_t *); +static hid_t * get_hid(int); +static void free_all_hid(void); +static void hid_close(hid_t *); +static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); +static void detach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); +static void timeout_callback(CFRunLoopTimerRef, void *); +static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); +static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len); + +pjrc_rawhid::pjrc_rawhid() +{ + first_hid = NULL; + last_hid = NULL; +} + +pjrc_rawhid::~pjrc_rawhid() +{ +} + +// open - open 1 or more devices +// +// Inputs: +// max = maximum number of devices to open +// vid = Vendor ID, or -1 if any +// pid = Product ID, or -1 if any +// usage_page = top level usage page, or -1 if any +// usage = top level usage number, or -1 if any +// Output: +// actual number of devices opened +// +int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) +{ + static IOHIDManagerRef hid_manager=NULL; + CFMutableDictionaryRef dict; + CFNumberRef num; + IOReturn ret; + hid_t *p; + int count=0; + + if (first_hid) free_all_hid(); + //printf("pjrc_rawhid_open, max=%d\n", max); + if (max < 1) return 0; + // Start the HID Manager + // http://developer.apple.com/technotes/tn2007/tn2187.html + if (!hid_manager) { + hid_manager = IOHIDManagerCreate(kCFAllocatorDefault, kIOHIDOptionsTypeNone); + if (hid_manager == NULL || CFGetTypeID(hid_manager) != IOHIDManagerGetTypeID()) { + if (hid_manager) CFRelease(hid_manager); + return 0; + } + } + if (vid > 0 || pid > 0 || usage_page > 0 || usage > 0) { + // Tell the HID Manager what type of devices we want + dict = CFDictionaryCreateMutable(kCFAllocatorDefault, 0, + &kCFTypeDictionaryKeyCallBacks, &kCFTypeDictionaryValueCallBacks); + if (!dict) return 0; + if (vid > 0) { + num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &vid); + CFDictionarySetValue(dict, CFSTR(kIOHIDVendorIDKey), num); + CFRelease(num); + } + if (pid > 0) { + num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &pid); + CFDictionarySetValue(dict, CFSTR(kIOHIDProductIDKey), num); + CFRelease(num); + } + if (usage_page > 0) { + num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &usage_page); + CFDictionarySetValue(dict, CFSTR(kIOHIDPrimaryUsagePageKey), num); + CFRelease(num); + } + if (usage > 0) { + num = CFNumberCreate(kCFAllocatorDefault, kCFNumberIntType, &usage); + CFDictionarySetValue(dict, CFSTR(kIOHIDPrimaryUsageKey), num); + CFRelease(num); + } + IOHIDManagerSetDeviceMatching(hid_manager, dict); + CFRelease(dict); + } else { + IOHIDManagerSetDeviceMatching(hid_manager, NULL); + } + // set up a callbacks for device attach & detach + IOHIDManagerScheduleWithRunLoop(hid_manager, CFRunLoopGetCurrent(), + kCFRunLoopDefaultMode); + IOHIDManagerRegisterDeviceMatchingCallback(hid_manager, attach_callback, NULL); + IOHIDManagerRegisterDeviceRemovalCallback(hid_manager, detach_callback, NULL); + ret = IOHIDManagerOpen(hid_manager, kIOHIDOptionsTypeNone); + if (ret != kIOReturnSuccess) { + printf("Could not start IOHIDManager"); + IOHIDManagerUnscheduleFromRunLoop(hid_manager, + CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); + CFRelease(hid_manager); + return 0; + } + printf("run loop\n"); + // let it do the callback for all devices + while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, true) == kCFRunLoopRunHandledSource) ; + // count up how many were added by the callback + for (p = first_hid; p; p = p->next) count++; + return count; +} + +// recveive - receive a packet +// Inputs: +// num = device to receive from (zero based) +// buf = buffer to receive packet +// len = buffer's size +// timeout = time to wait, in milliseconds +// Output: +// number of bytes received, or -1 on error +// +int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) +{ + hid_t *hid; + buffer_t *b; + CFRunLoopTimerRef timer=NULL; + CFRunLoopTimerContext context; + int ret=0, timeout_occurred=0; + + if (len < 1) return 0; + hid = get_hid(num); + if (!hid || !hid->open) return -1; + if ((b = hid->first_buffer) != NULL) { + if (len > b->len) len = b->len; + memcpy(buf, b->buf, len); + hid->first_buffer = b->next; + free(b); + return len; + } + memset(&context, 0, sizeof(context)); + context.info = &timeout_occurred; + timer = CFRunLoopTimerCreate(NULL, CFAbsoluteTimeGetCurrent() + + (double)timeout / 1000.0, 0, 0, 0, timeout_callback, &context); + CFRunLoopAddTimer(CFRunLoopGetCurrent(), timer, kCFRunLoopDefaultMode); + while (1) { + CFRunLoopRun(); + if ((b = hid->first_buffer) != NULL) { + if (len > b->len) len = b->len; + memcpy(buf, b->buf, len); + hid->first_buffer = b->next; + free(b); + ret = len; + break; + } + if (!hid->open) { + printf("pjrc_rawhid_recv, device not open\n"); + ret = -1; + break; + } + if (timeout_occurred) break; + } + CFRunLoopTimerInvalidate(timer); + CFRelease(timer); + return ret; +} + +// send - send a packet +// Inputs: +// num = device to transmit to (zero based) +// buf = buffer containing packet to send +// len = number of bytes to transmit +// timeout = time to wait, in milliseconds +// Output: +// number of bytes sent, or -1 on error +// +int pjrc_rawhid::send(int num, void *buf, int len, int timeout) +{ + hid_t *hid; + int result=-100; + + hid = get_hid(num); + if (!hid || !hid->open) return -1; +#if 1 +#warning "Send timeout not implemented on MACOSX" + uint8_t *report_buf = (uint8_t *) malloc(len); + memcpy(&report_buf[0], buf,len); + // Note: packet processing done in OS indepdent code + IOReturn ret = IOHIDDeviceSetReport(hid->ref, kIOHIDReportTypeOutput, 2, (uint8_t *)report_buf, len); + result = (ret == kIOReturnSuccess) ? len : -1; + if (err_get_system(ret) == err_get_system(sys_iokit)) + { + + // The error was in the I/O Kit system + UInt32 codeValue = err_get_code(ret); + qDebug("Returned: %x", codeValue); + // Can now perform test on error code, display it to user, or whatever. + usleep(1000000); + } + +#endif +#if 0 + // No matter what I tried this never actually sends an output + // report and output_callback never gets called. Why?? + // Did I miss something? This is exactly the same params as + // the sync call that works. Is it an Apple bug? + // (submitted to Apple on 22-sep-2009, problem ID 7245050) + // + IOHIDDeviceScheduleWithRunLoop(hid->ref, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); + // should already be scheduled with run loop by attach_callback, + // sadly this doesn't make any difference either way + // + IOHIDDeviceSetReportWithCallback(hid->ref, kIOHIDReportTypeOutput, + 0, buf, len, (double)timeout / 1000.0, output_callback, &result); + while (1) { + printf("enter run loop (send)\n"); + CFRunLoopRun(); + printf("leave run loop (send)\n"); + if (result > -100) break; + if (!hid->open) { + result = -1; + break; + } + } +#endif + return result; +} + +QString pjrc_rawhid::getserial(int num) { + hid_t *hid; + char buf[128]; + + hid = get_hid(num); + + if (!hid || !hid->open) return QString("Error"); + + CFTypeRef serialnum = IOHIDDeviceGetProperty(hid->ref, CFSTR(kIOHIDSerialNumberKey)); + if(serialnum && CFGetTypeID(serialnum) == CFStringGetTypeID()) + { + //Note: I'm not sure it will always succeed if encoded as MacRoman but that + //is a superset of UTF8 so I think this is fine + CFStringRef str = (CFStringRef)serialnum; + const char * buf = CFStringGetCStringPtr(str, kCFStringEncodingMacRoman); + return QString(buf); + } + + return QString("Error"); +} + +// close - close a device +// +// Inputs: +// num = device to close (zero based) +// Output +// (nothing) +// +void pjrc_rawhid::close(int num) +{ + hid_t *hid; + + hid = get_hid(num); + if (!hid || !hid->open) return; + hid_close(hid); + hid->open = 0; +} + +// +// +// Private Functions +// +// +static void input_callback(void *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) +{ + buffer_t *n; + hid_t *hid; + + printf("input_callback, report id: %i buf: %x %x, len: %d\n", id, data[0], data[1], len); + if (ret != kIOReturnSuccess || len < 1) return; + hid = (hid_t*)context; + if (!hid || hid->ref != sender) return; + n = (buffer_t *)malloc(sizeof(buffer_t)); + if (!n) return; + if (len > BUFFER_SIZE) len = BUFFER_SIZE; + // Note: packet preprocessing done in OS independent code + memcpy(n->buf, &data[0], len); + n->len = len; + n->next = NULL; + if (!hid->first_buffer || !hid->last_buffer) { + hid->first_buffer = hid->last_buffer = n; + } else { + hid->last_buffer->next = n; + hid->last_buffer = n; + } + CFRunLoopStop(CFRunLoopGetCurrent()); +} + +static void timeout_callback(CFRunLoopTimerRef timer, void *info) +{ + printf("timeout_callback\n"); + *(int *)info = 1; + CFRunLoopStop(CFRunLoopGetCurrent()); +} + +static void add_hid(hid_t *h) +{ + if (!first_hid || !last_hid) { + first_hid = last_hid = h; + h->next = h->prev = NULL; + return; + } + last_hid->next = h; + h->prev = last_hid; + h->next = NULL; + last_hid = h; +} + + +static hid_t * get_hid(int num) +{ + hid_t *p; + for (p = first_hid; p && num > 0; p = p->next, num--) ; + return p; +} + + +static void free_all_hid(void) +{ + hid_t *p, *q; + + for (p = first_hid; p; p = p->next) { + hid_close(p); + } + p = first_hid; + while (p) { + q = p; + p = p->next; + free(q); + } + first_hid = last_hid = NULL; +} + + +static void hid_close(hid_t *hid) +{ + if (!hid || !hid->open || !hid->ref) return; + IOHIDDeviceUnscheduleFromRunLoop(hid->ref, CFRunLoopGetCurrent( ), kCFRunLoopDefaultMode); + IOHIDDeviceClose(hid->ref, kIOHIDOptionsTypeNone); + hid->ref = NULL; +} + +static void detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) +{ + hid_t *p; + + printf("detach callback\n"); + for (p = first_hid; p; p = p->next) { + if (p->ref == dev) { + p->open = 0; + CFRunLoopStop(CFRunLoopGetCurrent()); + return; + } + } +} + +static void attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) +{ + struct hid_struct *h; + + printf("attach callback\n"); + if (IOHIDDeviceOpen(dev, kIOHIDOptionsTypeNone) != kIOReturnSuccess) return; + h = (hid_t *)malloc(sizeof(hid_t)); + if (!h) return; + memset(h, 0, sizeof(hid_t)); + IOHIDDeviceScheduleWithRunLoop(dev, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); + IOHIDDeviceRegisterInputReportCallback(dev, h->buffer, sizeof(h->buffer), input_callback, h); + h->ref = dev; + h->open = 1; + add_hid(h); +} + +static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) +{ + printf("output_callback, r=%d\n", ret); + if (ret == kIOReturnSuccess) { + *(int *)context = len; + } else { + // timeout if not success? + *(int *)context = 0; + } + CFRunLoopStop(CFRunLoopGetCurrent()); +} + diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp index f3d9e2f8b..8a687263b 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_unix.cpp @@ -1,365 +1,374 @@ -/* @file pjrc_rawhid_unix.cpp - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup RawHIDPlugin Raw HID Plugin - * @{ - * @brief Impliments a HID USB connection to the flight hardware as a QIODevice - *****************************************************************************/ - -/* Simple Raw HID functions for Linux - for use with Teensy RawHID example - * http://www.pjrc.com/teensy/rawhid.html - * Copyright (c) 2009 PJRC.COM, LLC - * - * rawhid_open - open 1 or more devices - * rawhid_recv - receive a packet - * rawhid_send - send a packet - * rawhid_close - close a device - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above description, website URL and copyright notice and this permission - * notice shall be included in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - * - * Version 1.0: Initial Release - */ - -#include "pjrc_rawhid.h" -#include -#include -#include - -typedef struct hid_struct hid_t; -static hid_t *first_hid; -static hid_t *last_hid; -struct hid_struct { - usb_dev_handle *usb; - int open; - int iface; - int ep_in; - int ep_out; - struct hid_struct *prev; - struct hid_struct *next; -}; - -static void add_hid(hid_t *h); -static hid_t * get_hid(int num); -static void free_all_hid(void); -static void hid_close(hid_t *hid); -static int hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *end); - -#define printf qDebug - -pjrc_rawhid::pjrc_rawhid() -{ - first_hid = NULL; - last_hid = NULL; -} - -// open - open 1 or more devices -// -// Inputs: -// max = maximum number of devices to open -// vid = Vendor ID, or -1 if any -// pid = Product ID, or -1 if any -// usage_page = top level usage page, or -1 if any -// usage = top level usage number, or -1 if any -// Output: -// actual number of devices opened -// -int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) -{ - struct usb_bus *bus; - struct usb_device *dev; - struct usb_interface *iface; - struct usb_interface_descriptor *desc; - struct usb_endpoint_descriptor *ep; - usb_dev_handle *u; - uint8_t buf[1024], *p; - int i, n, len, tag, ep_in, ep_out, count=0, claimed; - uint32_t val=0, parsed_usage, parsed_usage_page; - hid_t *hid; - - if (first_hid) free_all_hid(); - //printf("pjrc_rawhid_open, max=%d\n", max); - if (max < 1) return 0; - usb_init(); - usb_find_busses(); - usb_find_devices(); - for (bus = usb_get_busses(); bus; bus = bus->next) { - for (dev = bus->devices; dev; dev = dev->next) { - if (vid > 0 && dev->descriptor.idVendor != vid) continue; - if (pid > 0 && dev->descriptor.idProduct != pid) continue; - if (!dev->config) continue; - if (dev->config->bNumInterfaces < 1) continue; - printf("device: vid=%04X, pic=%04X, with %d iface", - dev->descriptor.idVendor, - dev->descriptor.idProduct, - dev->config->bNumInterfaces); - iface = dev->config->interface; - u = NULL; - claimed = 0; - for (i=0; iconfig->bNumInterfaces && iface; i++, iface++) { - desc = iface->altsetting; - if (!desc) continue; - printf(" type %d, %d, %d", desc->bInterfaceClass, - desc->bInterfaceSubClass, desc->bInterfaceProtocol); - if (desc->bInterfaceClass != 3) continue; - if (desc->bInterfaceSubClass != 0) continue; - if (desc->bInterfaceProtocol != 0) continue; - ep = desc->endpoint; - ep_in = ep_out = 0; - for (n = 0; n < desc->bNumEndpoints; n++, ep++) { - if (ep->bEndpointAddress & 0x80) { - if (!ep_in) ep_in = ep->bEndpointAddress & 0x7F; - qDebug() << " IN endpoint " << ep_in; - } else { - if (!ep_out) ep_out = ep->bEndpointAddress; - qDebug() << " OUT endpoint " << ep_out; - } - } - if (!ep_in) continue; - if (!u) { - u = usb_open(dev); - if (!u) { - qDebug() << " unable to open device"; - break; - } - } - qDebug() << " hid interface (generic)"; - if (usb_get_driver_np(u, i, (char *)buf, sizeof(buf)) >= 0) { - printf(" in use by driver \"%s\"", buf); - if (usb_detach_kernel_driver_np(u, i) < 0) { - printf(" unable to detach from kernel"); - continue; - } - } - if (usb_claim_interface(u, i) < 0) { - printf(" unable claim interface %d", i); - continue; - } - len = usb_control_msg(u, 0x81, 6, 0x2200, i, (char *)buf, sizeof(buf), 250); - printf(" descriptor, len=%d", len); - if (len < 2) { - usb_release_interface(u, i); - continue; - } - p = buf; - parsed_usage_page = parsed_usage = 0; - while ((tag = hid_parse_item(&val, &p, buf + len)) >= 0) { - printf(" tag: %X, val %X", tag, val); - if (tag == 4) parsed_usage_page = val; - if (tag == 8) parsed_usage = val; - if (parsed_usage_page && parsed_usage) break; - } - if ((!parsed_usage_page) || (!parsed_usage) || - (usage_page > 0 && parsed_usage_page != (uint32_t)usage_page) || - (usage > 0 && parsed_usage != (uint32_t)usage)) { - usb_release_interface(u, i); - continue; - } - hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); - if (!hid) { - usb_release_interface(u, i); - continue; - } - hid->usb = u; - hid->iface = i; - hid->ep_in = ep_in; - hid->ep_out = ep_out; - hid->open = 1; - add_hid(hid); - claimed++; - count++; - if (count >= max) return count; - } - if (u && !claimed) usb_close(u); - } - } - return count; -} - -// recveive - receive a packet -// Inputs: -// num = device to receive from (zero based) -// buf = buffer to receive packet -// len = buffer's size -// timeout = time to wait, in milliseconds -// Output: -// number of bytes received, or -1 on error -// -int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) -{ - hid_t *hid; - int r; - - hid = get_hid(num); - if (!hid || !hid->open) return -1; - r = usb_interrupt_read(hid->usb, hid->ep_in, (char *)buf, len, timeout); - if (r >= 0) return r; - if (r == -110) return 0; // timeout - return -1; -} - -// send - send a packet -// Inputs: -// num = device to transmit to (zero based) -// buf = buffer containing packet to send -// len = number of bytes to transmit -// timeout = time to wait, in milliseconds -// Output: -// number of bytes sent, or -1 on error -// -int pjrc_rawhid::send(int num, void *buf, int len, int timeout) -{ - hid_t *hid; - - hid = get_hid(num); - if (!hid || !hid->open) return -1; - if (hid->ep_out) { - return usb_interrupt_write(hid->usb, hid->ep_out, (char *)buf, len, timeout); - } else { - return usb_control_msg(hid->usb, 0x21, 9, 0, hid->iface, (char *)buf, len, timeout); - } -} - -// getserial - get the serialnumber of the device -// -// Inputs: -// num = device to close (zero based) -// buf = buffer to read the serialnumber into -// Output -// number of bytes in found, or -1 on error -// -QString pjrc_rawhid::getserial(int num) { - hid_t *hid; - char buf[128]; - hid = get_hid(num); - if (!hid || !hid->open) return QString(""); - - int retlen = usb_get_string_simple(hid->usb, 3, buf, 128); - return QString().fromAscii(buf,-1); -} - -// close - close a device -// -// Inputs: -// num = device to close (zero based) -// Output -// (nothing) -// -void pjrc_rawhid::close(int num) -{ - hid_t *hid; - - hid = get_hid(num); - if (!hid || !hid->open) return; - hid_close(hid); -} - -// -// -// Private Functions -// -// - -// Chuck Robey wrote a real HID report parser -// (chuckr@telenix.org) chuckr@chuckr.org -// http://people.freebsd.org/~chuckr/code/python/uhidParser-0.2.tbz -// this tiny thing only needs to extract the top-level usage page -// and usage, and even then is may not be truly correct, but it does -// work with the Teensy Raw HID example. -static int hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *end) -{ - const uint8_t *p = *data; - uint8_t tag; - int table[4] = {0, 1, 2, 4}; - int len; - - if (p >= end) return -1; - if (p[0] == 0xFE) { - // long item, HID 1.11, 6.2.2.3, page 27 - if (p + 5 >= end || p + p[1] >= end) return -1; - tag = p[2]; - *val = 0; - len = p[1] + 5; - } else { - // short item, HID 1.11, 6.2.2.2, page 26 - tag = p[0] & 0xFC; - len = table[p[0] & 0x03]; - if (p + len + 1 >= end) return -1; - switch (p[0] & 0x03) { - case 3: *val = p[1] | (p[2] << 8) | (p[3] << 16) | (p[4] << 24); break; - case 2: *val = p[1] | (p[2] << 8); break; - case 1: *val = p[1]; break; - case 0: *val = 0; break; - } - } - *data += len + 1; - return tag; -} - - -static void add_hid(hid_t *h) -{ - if (!first_hid || !last_hid) { - first_hid = last_hid = h; - h->next = h->prev = NULL; - return; - } - last_hid->next = h; - h->prev = last_hid; - h->next = NULL; - last_hid = h; -} - - -static hid_t * get_hid(int num) -{ - hid_t *p; - for (p = first_hid; p && num > 0; p = p->next, num--) ; - return p; -} - - -static void free_all_hid(void) -{ - hid_t *p, *q; - - for (p = first_hid; p; p = p->next) { - hid_close(p); - } - p = first_hid; - while (p) { - q = p; - p = p->next; - free(q); - } - first_hid = last_hid = NULL; -} - - -static void hid_close(hid_t *hid) -{ - hid_t *p; - int others=0; - - usb_release_interface(hid->usb, hid->iface); - for (p = first_hid; p; p = p->next) { - if (p->open && p->usb == hid->usb) others++; - } - if (!others) usb_close(hid->usb); - hid->usb = NULL; -} +/* @file pjrc_rawhid_unix.cpp + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup RawHIDPlugin Raw HID Plugin + * @{ + * @brief Impliments a HID USB connection to the flight hardware as a QIODevice + *****************************************************************************/ + +/* Simple Raw HID functions for Linux - for use with Teensy RawHID example + * http://www.pjrc.com/teensy/rawhid.html + * Copyright (c) 2009 PJRC.COM, LLC + * + * rawhid_open - open 1 or more devices + * rawhid_recv - receive a packet + * rawhid_send - send a packet + * rawhid_close - close a device + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above description, website URL and copyright notice and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * Version 1.0: Initial Release + */ + +#include "pjrc_rawhid.h" + +#define printf qDebug + +pjrc_rawhid::pjrc_rawhid() +{ + first_hid = NULL; + last_hid = NULL; +} + +pjrc_rawhid::~pjrc_rawhid() +{ +} + +// open - open 1 or more devices +// +// Inputs: +// max = maximum number of devices to open +// vid = Vendor ID, or -1 if any +// pid = Product ID, or -1 if any +// usage_page = top level usage page, or -1 if any +// usage = top level usage number, or -1 if any +// Output: +// actual number of devices opened +// +int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) +{ + struct usb_bus *bus; + struct usb_device *dev; + struct usb_interface *iface; + struct usb_interface_descriptor *desc; + struct usb_endpoint_descriptor *ep; + usb_dev_handle *u; + uint8_t buf[1024], *p; + int i, n, len, tag, ep_in, ep_out, count=0, claimed; + uint32_t val=0, parsed_usage, parsed_usage_page; + hid_t *hid; + + if (first_hid) free_all_hid(); + //printf("pjrc_rawhid_open, max=%d\n", max); + + if (max < 1) return 0; + + usb_init(); + usb_find_busses(); + usb_find_devices(); + + for (bus = usb_get_busses(); bus; bus = bus->next) + { + for (dev = bus->devices; dev; dev = dev->next) + { + if (vid > 0 && dev->descriptor.idVendor != vid) continue; + if (pid > 0 && dev->descriptor.idProduct != pid) continue; + if (!dev->config) continue; + if (dev->config->bNumInterfaces < 1) continue; + printf("device: vid=%04X, pic=%04X, with %d iface", + dev->descriptor.idVendor, + dev->descriptor.idProduct, + dev->config->bNumInterfaces); + iface = dev->config->interface; + u = NULL; + claimed = 0; + for (i=0; iconfig->bNumInterfaces && iface; i++, iface++) + { + desc = iface->altsetting; + if (!desc) continue; + + printf(" type %d, %d, %d", desc->bInterfaceClass, desc->bInterfaceSubClass, desc->bInterfaceProtocol); + + if (desc->bInterfaceClass != 3) continue; + if (desc->bInterfaceSubClass != 0) continue; + if (desc->bInterfaceProtocol != 0) continue; + + ep = desc->endpoint; + ep_in = ep_out = 0; + for (n = 0; n < desc->bNumEndpoints; n++, ep++) + { + if (ep->bEndpointAddress & 0x80) + { + if (!ep_in) ep_in = ep->bEndpointAddress & 0x7F; + qDebug() << " IN endpoint " << ep_in; + } + else + { + if (!ep_out) ep_out = ep->bEndpointAddress; + qDebug() << " OUT endpoint " << ep_out; + } + } + if (!ep_in) continue; + + if (!u) + { + u = usb_open(dev); + if (!u) + { + qDebug() << " unable to open device"; + break; + } + } + qDebug() << " hid interface (generic)"; + if (usb_get_driver_np(u, i, (char *)buf, sizeof(buf)) >= 0) + { + printf(" in use by driver \"%s\"", buf); + if (usb_detach_kernel_driver_np(u, i) < 0) + { + printf(" unable to detach from kernel"); + continue; + } + } + + if (usb_claim_interface(u, i) < 0) + { + printf(" unable claim interface %d", i); + continue; + } + + len = usb_control_msg(u, 0x81, 6, 0x2200, i, (char *)buf, sizeof(buf), 250); + printf(" descriptor, len=%d", len); + if (len < 2) + { + usb_release_interface(u, i); + continue; + } + + p = buf; + parsed_usage_page = parsed_usage = 0; + while ((tag = hid_parse_item(&val, &p, buf + len)) >= 0) + { + printf(" tag: %X, val %X", tag, val); + if (tag == 4) parsed_usage_page = val; + if (tag == 8) parsed_usage = val; + if (parsed_usage_page && parsed_usage) break; + } + if ((!parsed_usage_page) || (!parsed_usage) || + (usage_page > 0 && parsed_usage_page != (uint32_t)usage_page) || + (usage > 0 && parsed_usage != (uint32_t)usage)) + { + usb_release_interface(u, i); + continue; + } + + hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); + if (!hid) + { + usb_release_interface(u, i); + continue; + } + + hid->usb = u; + hid->iface = i; + hid->ep_in = ep_in; + hid->ep_out = ep_out; + hid->open = 1; + add_hid(hid); + + claimed++; + count++; + if (count >= max) return count; + } + + if (u && !claimed) usb_close(u); + } + } + + return count; +} + +// recveive - receive a packet +// Inputs: +// num = device to receive from (zero based) +// buf = buffer to receive packet +// len = buffer's size +// timeout = time to wait, in milliseconds +// Output: +// number of bytes received, or -1 on error +// +int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) +{ + if (!buf) return -1; + + hid_t *hid = get_hid(num); + if (!hid || !hid->open) return -1; + + int r = usb_interrupt_read(hid->usb, hid->ep_in, (char *)buf, len, timeout); + if (r >= 0) return r; + if (r == -110) return 0; // timeout + + return -1; +} + +// send - send a packet +// Inputs: +// num = device to transmit to (zero based) +// buf = buffer containing packet to send +// len = number of bytes to transmit +// timeout = time to wait, in milliseconds +// Output: +// number of bytes sent, or -1 on error +// +int pjrc_rawhid::send(int num, void *buf, int len, int timeout) +{ + hid_t *hid; + + hid = get_hid(num); + if (!hid || !hid->open) return -1; + if (hid->ep_out) { + return usb_interrupt_write(hid->usb, hid->ep_out, (char *)buf, len, timeout); + } else { + return usb_control_msg(hid->usb, 0x21, 9, 0, hid->iface, (char *)buf, len, timeout); + } +} + +// getserial - get the serialnumber of the device +// +// Inputs: +// num = device to close (zero based) +// buf = buffer to read the serialnumber into +// Output +// number of bytes in found, or -1 on error +// +QString pjrc_rawhid::getserial(int num) { + hid_t *hid; + char buf[128]; + hid = get_hid(num); + if (!hid || !hid->open) return QString(""); + + int retlen = usb_get_string_simple(hid->usb, 3, buf, 128); + return QString().fromAscii(buf,-1); +} + +// close - close a device +// +// Inputs: +// num = device to close (zero based) +// Output +// (nothing) +// +void pjrc_rawhid::close(int num) +{ + hid_t *hid = get_hid(num); + if (hid && !hid->open) + hid_close(hid); +} + +// Chuck Robey wrote a real HID report parser +// (chuckr@telenix.org) chuckr@chuckr.org +// http://people.freebsd.org/~chuckr/code/python/uhidParser-0.2.tbz +// this tiny thing only needs to extract the top-level usage page +// and usage, and even then is may not be truly correct, but it does +// work with the Teensy Raw HID example. +int pjrc_rawhid::hid_parse_item(uint32_t *val, uint8_t **data, const uint8_t *end) +{ + const uint8_t *p = *data; + uint8_t tag; + int table[4] = {0, 1, 2, 4}; + int len; + + if (p >= end) return -1; + if (p[0] == 0xFE) { + // long item, HID 1.11, 6.2.2.3, page 27 + if (p + 5 >= end || p + p[1] >= end) return -1; + tag = p[2]; + *val = 0; + len = p[1] + 5; + } else { + // short item, HID 1.11, 6.2.2.2, page 26 + tag = p[0] & 0xFC; + len = table[p[0] & 0x03]; + if (p + len + 1 >= end) return -1; + switch (p[0] & 0x03) { + case 3: *val = p[1] | (p[2] << 8) | (p[3] << 16) | (p[4] << 24); break; + case 2: *val = p[1] | (p[2] << 8); break; + case 1: *val = p[1]; break; + case 0: *val = 0; break; + } + } + *data += len + 1; + return tag; +} + +void pjrc_rawhid::add_hid(hid_t *h) +{ + if (!h) return; + + if (!first_hid || !last_hid) + { + first_hid = last_hid = h; + h->next = h->prev = NULL; + return; + } + last_hid->next = h; + h->prev = last_hid; + h->next = NULL; + last_hid = h; +} + +hid_t * pjrc_rawhid::get_hid(int num) +{ + hid_t *p = NULL; + for (p = first_hid; p && num > 0; p = p->next, num--) ; + return p; +} + +void pjrc_rawhid::free_all_hid(void) +{ + for (hid_t *p = first_hid; p; p = p->next) + hid_close(p); + + hid_t *p = first_hid; + while (p) + { + hid_t *q = p; + p = p->next; + free(q); + } + + first_hid = last_hid = NULL; +} + +void pjrc_rawhid::hid_close(hid_t *hid) +{ + if (!hid) return; + + usb_release_interface(hid->usb, hid->iface); + + int others = 0; + for (hid_t *p = first_hid; p; p = p->next) + { + if (p->open && p->usb == hid->usb) + others++; + } + if (!others) + usb_close(hid->usb); + + hid->usb = NULL; +} diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp index 8f3c0b144..5c0bfd3b9 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_win.cpp @@ -1,391 +1,411 @@ -/* @file pjrc_rawhid_windows.cpp - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup RawHIDPlugin Raw HID Plugin - * @{ - * @brief Impliments a HID USB connection to the flight hardware as a QIODevice - *****************************************************************************/ - -/* Simple Raw HID functions for Windows - for use with Teensy RawHID example - * http://www.pjrc.com/teensy/rawhid.html - * Copyright (c) 2009 PJRC.COM, LLC - * - * rawhid_open - open 1 or more devices - * rawhid_recv - receive a packet - * rawhid_send - send a packet - * rawhid_close - close a device - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above description, website URL and copyright notice and this permission - * notice shall be included in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - * - * Version 1.0: Initial Release - */ -/* See: http://msdn.microsoft.com/en-us/library/ms794141.aspx */ - -#include "pjrc_rawhid.h" - -#include -#include -#include -#include -#include - -#define printf qDebug - -typedef struct hid_struct hid_t; -struct hid_struct { - HANDLE handle; - int open; - struct hid_struct *prev; - struct hid_struct *next; -}; -hid_t *first_hid; -hid_t *last_hid; -HANDLE rx_event; -HANDLE tx_event; -CRITICAL_SECTION rx_mutex; -CRITICAL_SECTION tx_mutex; - -static void add_hid(hid_t *h); -static hid_t* get_hid(int num); -static void free_all_hid(void); -static void hid_close(hid_t *hid); -static void print_win32_err(void); - -pjrc_rawhid::pjrc_rawhid() -{ - first_hid = NULL; - last_hid = NULL; - rx_event = NULL; - tx_event = NULL; -} - -// open - open 1 or more devices -// -// Inputs: -// max = maximum number of devices to open -// vid = Vendor ID, or -1 if any -// pid = Product ID, or -1 if any -// usage_page = top level usage page, or -1 if any -// usage = top level usage number, or -1 if any -// Output: -// actual number of devices opened -// -int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) -{ - GUID guid; - HDEVINFO info; - DWORD index=0, reqd_size; - SP_DEVICE_INTERFACE_DATA iface; - SP_DEVICE_INTERFACE_DETAIL_DATA *details; - HIDD_ATTRIBUTES attrib; - PHIDP_PREPARSED_DATA hid_data; - HIDP_CAPS capabilities; - HANDLE h; - BOOL ret; - hid_t *hid; - int count=0; - - if (first_hid) free_all_hid(); - if (max < 1) return 0; - if (!rx_event) { - rx_event = CreateEvent(NULL, TRUE, TRUE, NULL); - tx_event = CreateEvent(NULL, TRUE, TRUE, NULL); - InitializeCriticalSection(&rx_mutex); - InitializeCriticalSection(&tx_mutex); - } - HidD_GetHidGuid(&guid); - info = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); - if (info == INVALID_HANDLE_VALUE) return 0; - for (index=0; 1 ;index++) { - iface.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); - ret = SetupDiEnumDeviceInterfaces(info, NULL, &guid, index, &iface); - if (!ret) return count; - SetupDiGetInterfaceDeviceDetail(info, &iface, NULL, 0, &reqd_size, NULL); - details = (SP_DEVICE_INTERFACE_DETAIL_DATA *)malloc(reqd_size); - if (details == NULL) continue; - - memset(details, 0, reqd_size); - details->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); - ret = SetupDiGetDeviceInterfaceDetail(info, &iface, details, - reqd_size, NULL, NULL); - if (!ret) { - free(details); - continue; - } - h = CreateFile(details->DevicePath, GENERIC_READ|GENERIC_WRITE, - FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, - OPEN_EXISTING, - FILE_FLAG_OVERLAPPED, - NULL); - if( h == INVALID_HANDLE_VALUE ) - { - // I get ERROR_ACCESS_DENIED with most/all my input devices (mice/trackballs/tablet). - // Let's not log it :) - if(GetLastError() == ERROR_ACCESS_DENIED) { - free(details); - continue; - } - - // qDebug wipes the GetLastError() it seems, so do that after print_win32_err(). - print_win32_err(); - qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); - free(details); - continue; - } - free(details); - - attrib.Size = sizeof(HIDD_ATTRIBUTES); - ret = HidD_GetAttributes(h, &attrib); - //printf("vid: %4x\n", attrib.VendorID); - if (!ret || (vid > 0 && attrib.VendorID != vid) || - (pid > 0 && attrib.ProductID != pid) || - !HidD_GetPreparsedData(h, &hid_data)) { - CloseHandle(h); - continue; - } - if (!HidP_GetCaps(hid_data, &capabilities) || - (usage_page > 0 && capabilities.UsagePage != usage_page) || - (usage > 0 && capabilities.Usage != usage)) { - HidD_FreePreparsedData(hid_data); - CloseHandle(h); - continue; - } - HidD_FreePreparsedData(hid_data); - hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); - if (!hid) { - CloseHandle(h); - continue; - } - qDebug("Open: Handle address: %li for num: %i", (long int) h, count); - hid->handle = h; - hid->open = 1; - add_hid(hid); - count++; - if (count >= max) return count; - } - return count; -} - -// recveive - receive a packet -// Inputs: -// num = device to receive from (zero based) -// buf = buffer to receive packet -// len = buffer's size -// timeout = time to wait, in milliseconds -// Output: -// number of bytes received, or -1 on error -// -int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) -{ - hid_t *hid; - OVERLAPPED ov; - DWORD n, r; - - hid = get_hid(num); - if (!hid || !hid->open) return -1; - - EnterCriticalSection(&rx_mutex); - ResetEvent(&rx_event); - memset(&ov, 0, sizeof(ov)); - ov.hEvent = rx_event; - - if (!ReadFile(hid->handle, buf, len, NULL, &ov)) { - if (GetLastError() != ERROR_IO_PENDING) goto return_error; - r = WaitForSingleObject(rx_event, timeout); - if (r == WAIT_TIMEOUT) goto return_timeout; - if (r != WAIT_OBJECT_0) goto return_error; - } - - if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) goto return_error; - LeaveCriticalSection(&rx_mutex); - if (n <= 0) return -1; - -// qDebug("Received %i bytes, first %x, second %x", len, *((char *) buf),*((char *)buf + 1)); - - if ((int)n > len) n = len; - return n; -return_timeout: - CancelIo(hid->handle); - LeaveCriticalSection(&rx_mutex); - return 0; -return_error: - print_win32_err(); - LeaveCriticalSection(&rx_mutex); - return -1; -} - -// send - send a packet -// Inputs: -// num = device to transmit to (zero based) -// buf = buffer containing packet to send -// len = number of bytes to transmit -// timeout = time to wait, in milliseconds -// Output: -// number of bytes sent, or -1 on error -// -int pjrc_rawhid::send(int num, void *buf, int len, int timeout) -{ - hid_t *hid; - OVERLAPPED ov; - DWORD n, r; - - hid = get_hid(num); - if (!hid || !hid->open) return -1; - -// qDebug("Send: Handle address: %li for num: %i", (long int) hid->handle, num); - - EnterCriticalSection(&tx_mutex); - ResetEvent(&tx_event); - memset(&ov, 0, sizeof(ov)); - ov.hEvent = tx_event; - -// qDebug("Trying to write %u bytes. First %x second %x",len, *((char *) buf), *((char *)buf + 1)); - - if (!WriteFile(hid->handle, buf, len, NULL, &ov)) { - DWORD err = GetLastError(); - if ( err == ERROR_SUCCESS || err == ERROR_IO_PENDING ) - { -// qDebug("Waiting for write to finish"); - r = WaitForSingleObject(tx_event, timeout); - if (r == WAIT_TIMEOUT) goto return_timeout; - if (r != WAIT_OBJECT_0) goto return_error; - } - else - { -// qDebug("Error writing to file"); - print_win32_err(); - LeaveCriticalSection(&tx_mutex); - return -1; - } - } - - if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) - { - qDebug("Problem getting overlapped result"); - print_win32_err(); - } - LeaveCriticalSection(&tx_mutex); - if (n <= 0) return -1; - return n; -return_timeout: - CancelIo(hid->handle); - LeaveCriticalSection(&tx_mutex); - return 0; -return_error: - print_win32_err(); - LeaveCriticalSection(&tx_mutex); - return -1; -} - -QString pjrc_rawhid::getserial(int num) -{ - hid_t *hid; - char temp[126]; - - hid = get_hid(num); - if (!hid || !hid->open) - return ""; - - /* Should we do some "critical section" stuff here?? */ - if(!HidD_GetSerialNumberString(hid->handle, temp, sizeof(temp))) { - print_win32_err(); - return QString("Error"); - } - - return QString().fromUtf16((ushort*)temp,-1); -} - -// close - close a device -// -// Inputs: -// num = device to close (zero based) -// Output -// (nothing) -// -void pjrc_rawhid::close(int num) -{ - hid_t *hid; - - hid = get_hid(num); - if (!hid || !hid->open) return; - hid_close(hid); -} - -// -// -// Private Functions -// -// -static void add_hid(hid_t *h) -{ - if (!first_hid || !last_hid) { - first_hid = last_hid = h; - h->next = h->prev = NULL; - return; - } - last_hid->next = h; - h->prev = last_hid; - h->next = NULL; - last_hid = h; -} - - -static hid_t * get_hid(int num) -{ - hid_t *p; - for (p = first_hid; p && num > 0; p = p->next, num--) ; - return p; -} - - -static void free_all_hid(void) -{ - hid_t *p, *q; - - for (p = first_hid; p; p = p->next) { - hid_close(p); - } - p = first_hid; - while (p) { - q = p; - p = p->next; - free(q); - } - first_hid = last_hid = NULL; -} - - -static void hid_close(hid_t *hid) -{ - CloseHandle(hid->handle); - hid->handle = NULL; -} - -static void print_win32_err(void) -{ - char buf[256]; - char temp[256]; - DWORD err; - - err = GetLastError(); - //FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, 0, (WCHAR*)buf, sizeof(buf), NULL); - FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, MAKELANGID(LANG_ENGLISH, SUBLANG_DEFAULT), (WCHAR*)buf, sizeof(buf), NULL); - WideCharToMultiByte( CP_ACP, 0, (WCHAR*)buf, sizeof(buf), temp, sizeof(temp), NULL, NULL ); - printf("err %ld: %s\n", err, temp); -} +/* @file pjrc_rawhid_windows.cpp + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup RawHIDPlugin Raw HID Plugin + * @{ + * @brief Impliments a HID USB connection to the flight hardware as a QIODevice + *****************************************************************************/ + +/* Simple Raw HID functions for Windows - for use with Teensy RawHID example + * http://www.pjrc.com/teensy/rawhid.html + * Copyright (c) 2009 PJRC.COM, LLC + * + * rawhid_open - open 1 or more devices + * rawhid_recv - receive a packet + * rawhid_send - send a packet + * rawhid_close - close a device + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above description, website URL and copyright notice and this permission + * notice shall be included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * Version 1.0: Initial Release + */ +/* See: http://msdn.microsoft.com/en-us/library/ms794141.aspx */ + +#include "pjrc_rawhid.h" + +#include + +#define printf qDebug + +pjrc_rawhid::pjrc_rawhid() +{ + first_hid = NULL; + last_hid = NULL; + rx_event = NULL; + tx_event = NULL; +} + +pjrc_rawhid::~pjrc_rawhid() +{ +} + +// open - open 1 or more devices +// +// Inputs: +// max = maximum number of devices to open +// vid = Vendor ID, or -1 if any +// pid = Product ID, or -1 if any +// usage_page = top level usage page, or -1 if any +// usage = top level usage number, or -1 if any +// Output: +// actual number of devices opened +// +int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) +{ + GUID guid; + HDEVINFO info; + DWORD index=0, reqd_size; + SP_DEVICE_INTERFACE_DATA iface; + SP_DEVICE_INTERFACE_DETAIL_DATA *details; + HIDD_ATTRIBUTES attrib; + PHIDP_PREPARSED_DATA hid_data; + HIDP_CAPS capabilities; + HANDLE h; + BOOL ret; + hid_t *hid; + + int count=0; + + if (first_hid) free_all_hid(); + + if (max < 1) return 0; + + if (!rx_event) + { + rx_event = CreateEvent(NULL, TRUE, TRUE, NULL); + tx_event = CreateEvent(NULL, TRUE, TRUE, NULL); + InitializeCriticalSection(&rx_mutex); + InitializeCriticalSection(&tx_mutex); + } + + HidD_GetHidGuid(&guid); + + info = SetupDiGetClassDevs(&guid, NULL, NULL, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); + if (info == INVALID_HANDLE_VALUE) return 0; + + for (index=0; 1 ;index++) + { + iface.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + ret = SetupDiEnumDeviceInterfaces(info, NULL, &guid, index, &iface); + if (!ret) return count; + + SetupDiGetInterfaceDeviceDetail(info, &iface, NULL, 0, &reqd_size, NULL); + details = (SP_DEVICE_INTERFACE_DETAIL_DATA *)malloc(reqd_size); + if (details == NULL) continue; + + memset(details, 0, reqd_size); + details->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); + ret = SetupDiGetDeviceInterfaceDetail(info, &iface, details, reqd_size, NULL, NULL); + if (!ret) + { + free(details); + continue; + } + + h = CreateFile(details->DevicePath, GENERIC_READ|GENERIC_WRITE, FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); + if (h == INVALID_HANDLE_VALUE) + { + // I get ERROR_ACCESS_DENIED with most/all my input devices (mice/trackballs/tablet). + // Let's not log it :) + if (GetLastError() == ERROR_ACCESS_DENIED) + { + free(details); + continue; + } + + // qDebug wipes the GetLastError() it seems, so do that after print_win32_err(). + print_win32_err(); + qDebug() << "Problem opening handle, path: " << QString().fromWCharArray(details->DevicePath); + + free(details); + continue; + } + + free(details); + + attrib.Size = sizeof(HIDD_ATTRIBUTES); + ret = HidD_GetAttributes(h, &attrib); + //printf("vid: %4x\n", attrib.VendorID); + if (!ret || (vid > 0 && attrib.VendorID != vid) || + (pid > 0 && attrib.ProductID != pid) || + !HidD_GetPreparsedData(h, &hid_data)) + { + CloseHandle(h); + continue; + } + + if (!HidP_GetCaps(hid_data, &capabilities) || + (usage_page > 0 && capabilities.UsagePage != usage_page) || + (usage > 0 && capabilities.Usage != usage)) + { + HidD_FreePreparsedData(hid_data); + CloseHandle(h); + continue; + } + + HidD_FreePreparsedData(hid_data); + + hid = (struct hid_struct *)malloc(sizeof(struct hid_struct)); + if (!hid) + { + CloseHandle(h); + continue; + } + + qDebug("Open: Handle address: %li for num: %i", (long int) h, count); + + hid->handle = h; + hid->open = 1; + add_hid(hid); + + count++; + if (count >= max) return count; + } + + return count; +} + +// recveive - receive a packet +// Inputs: +// num = device to receive from (zero based) +// buf = buffer to receive packet +// len = buffer's size +// timeout = time to wait, in milliseconds +// Output: +// number of bytes received, or -1 on error +// +int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) +{ + OVERLAPPED ov; + DWORD n; + + hid_t *hid = get_hid(num); + if (!hid || !hid->open) return -1; + + EnterCriticalSection(&rx_mutex); + + ResetEvent(&rx_event); + + memset(&ov, 0, sizeof(ov)); + ov.hEvent = rx_event; + + if (!ReadFile(hid->handle, buf, len, NULL, &ov)) + { + if (GetLastError() != ERROR_IO_PENDING) + { + print_win32_err(); + LeaveCriticalSection(&rx_mutex); + return -1; + } + + DWORD r = WaitForSingleObject(rx_event, timeout); + if (r == WAIT_TIMEOUT) + { + CancelIo(hid->handle); + LeaveCriticalSection(&rx_mutex); + return 0; + } + if (r != WAIT_OBJECT_0) + { + print_win32_err(); + LeaveCriticalSection(&rx_mutex); + return -1; + } + } + + if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) + { + print_win32_err(); + LeaveCriticalSection(&rx_mutex); + return -1; + } + + LeaveCriticalSection(&rx_mutex); + + if (n <= 0) return -1; + +// qDebug("Received %i bytes, first %x, second %x", len, *((char *) buf),*((char *)buf + 1)); + + if ((int)n > len) n = len; + return n; +} + +// send - send a packet +// Inputs: +// num = device to transmit to (zero based) +// buf = buffer containing packet to send +// len = number of bytes to transmit +// timeout = time to wait, in milliseconds +// Output: +// number of bytes sent, or -1 on error +// +int pjrc_rawhid::send(int num, void *buf, int len, int timeout) +{ + OVERLAPPED ov; + DWORD n, r; + + hid_t *hid = get_hid(num); + if (!hid || !hid->open) return -1; + +// qDebug("Send: Handle address: %li for num: %i", (long int) hid->handle, num); + + EnterCriticalSection(&tx_mutex); + + ResetEvent(&tx_event); + + memset(&ov, 0, sizeof(ov)); + ov.hEvent = tx_event; + +// qDebug("Trying to write %u bytes. First %x second %x",len, *((char *) buf), *((char *)buf + 1)); + + if (!WriteFile(hid->handle, buf, len, NULL, &ov)) + { + DWORD err = GetLastError(); + if ( err == ERROR_SUCCESS || err == ERROR_IO_PENDING ) + { +// qDebug("Waiting for write to finish"); + r = WaitForSingleObject(tx_event, timeout); + if (r == WAIT_TIMEOUT) + { + CancelIo(hid->handle); + LeaveCriticalSection(&tx_mutex); + return 0; + } + if (r != WAIT_OBJECT_0) + { + print_win32_err(); + LeaveCriticalSection(&tx_mutex); + return -1; + } + } + else + { +// qDebug("Error writing to file"); + print_win32_err(); + LeaveCriticalSection(&tx_mutex); + return -1; + } + } + + if (!GetOverlappedResult(hid->handle, &ov, &n, FALSE)) + { + qDebug("Problem getting overlapped result"); + print_win32_err(); + } + + LeaveCriticalSection(&tx_mutex); + + if (n <= 0) return -1; + return n; +} + +QString pjrc_rawhid::getserial(int num) +{ + hid_t *hid = get_hid(num); + if (!hid || !hid->open) + return ""; + + // Should we do some "critical section" stuff here?? + char temp[126]; + if (!HidD_GetSerialNumberString(hid->handle, temp, sizeof(temp))) + { + print_win32_err(); + return QString("Error"); + } + + return QString().fromUtf16((ushort*)temp,-1); +} + +// close - close a device +// +// Inputs: +// num = device to close (zero based) +// Output +// (nothing) +// +void pjrc_rawhid::close(int num) +{ + hid_t *hid = get_hid(num); + if (hid && hid->open) + hid_close(hid); +} + +void pjrc_rawhid::add_hid(hid_t *h) +{ + if (!h) return; + + if (!first_hid || !last_hid) + { + first_hid = last_hid = h; + h->next = h->prev = NULL; + return; + } + last_hid->next = h; + h->prev = last_hid; + h->next = NULL; + last_hid = h; +} + +hid_t * pjrc_rawhid::get_hid(int num) +{ + hid_t *p; + for (p = first_hid; p && num > 0; p = p->next, num--) ; + return p; +} + +void pjrc_rawhid::free_all_hid(void) +{ + for (hid_t *p = first_hid; p; p = p->next) + hid_close(p); + + hid_t *p = first_hid; + while (p) + { + hid_t *q = p; + p = p->next; + free(q); + } + + first_hid = last_hid = NULL; +} + +void pjrc_rawhid::hid_close(hid_t *hid) +{ + if (!hid) return; + + if (hid->handle) + { + CloseHandle(hid->handle); + hid->handle = NULL; + } +} + +void pjrc_rawhid::print_win32_err(void) +{ + char buf[256]; + char temp[256]; + DWORD err; + + err = GetLastError(); + + //FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, 0, (WCHAR*)buf, sizeof(buf), NULL); + FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, MAKELANGID(LANG_ENGLISH, SUBLANG_DEFAULT), (WCHAR*)buf, sizeof(buf), NULL); + WideCharToMultiByte( CP_ACP, 0, (WCHAR*)buf, sizeof(buf), temp, sizeof(temp), NULL, NULL ); + printf("err %ld: %s\n", err, temp); +} diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp index 4eb0958c5..e31d0251f 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp @@ -1,388 +1,388 @@ -/** - ****************************************************************************** - * - * @file rawhid.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup RawHIDPlugin Raw HID Plugin - * @{ - * @brief Impliments a HID USB connection to the flight hardware as a QIODevice - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "rawhid.h" - -#include "rawhid_const.h" -#include "coreplugin/connectionmanager.h" -#include -#include -#include -#include - -class IConnection; - -//timeout value used when we want to return directly without waiting -static const int READ_TIMEOUT = 200; -static const int READ_SIZE = 64; - -static const int WRITE_TIMEOUT = 200; -static const int WRITE_SIZE = 64; - - - - -/** -* Thread to desynchronize reading from the device -*/ -class RawHIDReadThread : public QThread -{ -public: - RawHIDReadThread(RawHID *hid); - virtual ~RawHIDReadThread(); - - /** Return the data read so far without waiting */ - int getReadData(char *data, int size); - - /** return the bytes buffered */ - qint64 getBytesAvailable(); - -public slots: - void terminate() { - m_running = false; - } - -protected: - void run(); - - /** QByteArray might not be the most efficient way to implement - a circular buffer but it's good enough and very simple */ - QByteArray m_readBuffer; - - /** A mutex to protect read buffer */ - QMutex m_readBufMtx; - - RawHID *m_hid; - - pjrc_rawhid *hiddev; - int hidno; - - bool m_running; -}; - - -/** -* This class is nearly the same than RawHIDReadThread but for writing -*/ -class RawHIDWriteThread : public QThread -{ -public: - RawHIDWriteThread(RawHID *hid); - virtual ~RawHIDWriteThread(); - - /** Add some data to be written without waiting */ - int pushDataToWrite(const char *data, int size); - - /** Return the number of bytes buffered */ - qint64 getBytesToWrite(); - -public slots: - void terminate() { - m_running = false; - } - -protected: - void run(); - - /** QByteArray might not be the most efficient way to implement - a circular buffer but it's good enough and very simple */ - QByteArray m_writeBuffer; - - /** A mutex to protect read buffer */ - QMutex m_writeBufMtx; - - /** Synchronize task with data arival */ - QWaitCondition m_newDataToWrite; - - RawHID *m_hid; - - pjrc_rawhid *hiddev; - int hidno; - - bool m_running; -}; - - - -RawHIDReadThread::RawHIDReadThread(RawHID *hid) - : m_hid(hid), - hiddev(&hid->dev), - hidno(hid->m_deviceNo), - m_running(true) -{ -} - -RawHIDReadThread::~RawHIDReadThread() -{ - m_running = false; - //wait for the thread to terminate - if(wait(1000) == false) - qDebug() << "Cannot terminate RawHIDReadThread"; -} - -void RawHIDReadThread::run() -{ - qDebug() << "Read thread started"; - while(m_running) - { - //here we use a temporary buffer so we don't need to lock - //the mutex while we are reading from the device - - // Want to read in regular chunks that match the packet size the device - // is using. In this case it is 64 bytes (the interrupt packet limit) - // although it would be nice if the device had a different report to - // configure this - char buffer[READ_SIZE] = {0}; - - int ret = hiddev->receive(hidno, buffer, READ_SIZE, READ_TIMEOUT); - - if(ret > 0) //read some data - { - QMutexLocker lock(&m_readBufMtx); - // Note: Preprocess the USB packets in this OS independent code - // First byte is report ID, second byte is the number of valid bytes - m_readBuffer.append(&buffer[2], buffer[1]); - - emit m_hid->readyRead(); - } - else if(ret == 0) //nothing read - { - } - else // < 0 => error - { - //TODO! make proper error handling, this only quick hack for unplug freeze - m_running=false; - } - } -} - -int RawHIDReadThread::getReadData(char *data, int size) -{ - QMutexLocker lock(&m_readBufMtx); - - size = qMin(size, m_readBuffer.size()); - - memcpy(data, m_readBuffer.constData(), size); - m_readBuffer.remove(0, size); - - return size; -} - -qint64 RawHIDReadThread::getBytesAvailable() -{ - QMutexLocker lock(&m_readBufMtx); - return m_readBuffer.size(); -} - -RawHIDWriteThread::RawHIDWriteThread(RawHID *hid) - : m_hid(hid), - hiddev(&hid->dev), - hidno(hid->m_deviceNo), - m_running(true) -{ -} - -RawHIDWriteThread::~RawHIDWriteThread() -{ - m_running = false; - //wait for the thread to terminate - if(wait(1000) == false) - qDebug() << "Cannot terminate RawHIDReadThread"; -} - -void RawHIDWriteThread::run() -{ - qDebug() << "Write thread started"; - while(m_running) - { - char buffer[WRITE_SIZE] = {0}; - - m_writeBufMtx.lock(); - int size = qMin(WRITE_SIZE-2, m_writeBuffer.size()); - while(size <= 0) - { - //wait on new data to write condition, the timeout - //enable the thread to shutdown properly - m_newDataToWrite.wait(&m_writeBufMtx, 200); - if(!m_running) - return; - - size = m_writeBuffer.size(); - } - - //NOTE: data size is limited to 2 bytes less than the - //usb packet size (64 bytes for interrupt) to make room - //for the reportID and valid data length - size = qMin(WRITE_SIZE-2, m_writeBuffer.size()); - memcpy(&buffer[2], m_writeBuffer.constData(), size); - buffer[1] = size; //valid data length - buffer[0] = 2; //reportID - m_writeBufMtx.unlock(); - - // must hold lock through the send to know how much was sent - int ret = hiddev->send(hidno, buffer, WRITE_SIZE, WRITE_TIMEOUT); - - if(ret > 0) - { - //only remove the size actually written to the device - QMutexLocker lock(&m_writeBufMtx); - m_writeBuffer.remove(0, size); - - emit m_hid->bytesWritten(ret - 2); - } - else if(ret < 0) // < 0 => error - { - //TODO! make proper error handling, this only quick hack for unplug freeze - m_running=false; - qDebug() << "Error writing to device"; - } - else - { - qDebug() << "No data written to device ??"; - } - } -} - -int RawHIDWriteThread::pushDataToWrite(const char *data, int size) -{ - QMutexLocker lock(&m_writeBufMtx); - - m_writeBuffer.append(data, size); - m_newDataToWrite.wakeOne(); //signal that new data arrived - - return size; -} - -qint64 RawHIDWriteThread::getBytesToWrite() -{ - // QMutexLocker lock(&m_writeBufMtx); - return m_writeBuffer.size(); -} - -RawHID::RawHID(const QString &deviceName) - :QIODevice(), - serialNumber(deviceName), - m_deviceNo(-1), - m_readThread(NULL), - m_writeThread(NULL) -{ - //find the device the user want to open and close the other - int opened = dev.open(MAX_DEVICES, VID, PID, USAGE_PAGE, USAGE); - - //for each devices found, get serial number and close - for(int i=0; istart(); - m_writeThread->start(); -} - -RawHID::~RawHID() -{ - if(m_readThread) - delete m_readThread; - - if(m_writeThread) - delete m_writeThread; -} - -bool RawHID::open(OpenMode mode) -{ - if(m_deviceNo < 0) - return false; - - QIODevice::open(mode); - - if(!m_readThread) - m_readThread = new RawHIDReadThread(this); - - if(!m_writeThread) - m_writeThread = new RawHIDWriteThread(this); - - return true; -} - - -void RawHID::close() -{ - emit aboutToClose(); - if(m_readThread) { - m_readThread->terminate(); - delete m_readThread; // calls wait - m_readThread = NULL; - } - - if(m_writeThread) { - m_writeThread->terminate(); - delete m_writeThread; - m_writeThread = NULL; - } - - dev.close(m_deviceNo); - QIODevice::close(); -} - -bool RawHID::isSequential() const -{ - return true; -} - -qint64 RawHID::bytesAvailable() const -{ - return m_readThread->getBytesAvailable() + QIODevice::bytesAvailable(); -} - -qint64 RawHID::bytesToWrite() const -{ - return m_writeThread->getBytesToWrite() + QIODevice::bytesToWrite(); -} - -qint64 RawHID::readData(char *data, qint64 maxSize) -{ - return m_readThread->getReadData(data, maxSize); -} - -qint64 RawHID::writeData(const char *data, qint64 maxSize) -{ - return m_writeThread->pushDataToWrite(data, maxSize); -} - +/** + ****************************************************************************** + * + * @file rawhid.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup RawHIDPlugin Raw HID Plugin + * @{ + * @brief Impliments a HID USB connection to the flight hardware as a QIODevice + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "rawhid.h" + +#include "rawhid_const.h" +#include "coreplugin/connectionmanager.h" +#include +#include +#include +#include + +class IConnection; + +//timeout value used when we want to return directly without waiting +static const int READ_TIMEOUT = 200; +static const int READ_SIZE = 64; + +static const int WRITE_TIMEOUT = 200; +static const int WRITE_SIZE = 64; + + + + +/** +* Thread to desynchronize reading from the device +*/ +class RawHIDReadThread : public QThread +{ +public: + RawHIDReadThread(RawHID *hid); + virtual ~RawHIDReadThread(); + + /** Return the data read so far without waiting */ + int getReadData(char *data, int size); + + /** return the bytes buffered */ + qint64 getBytesAvailable(); + +public slots: + void terminate() { + m_running = false; + } + +protected: + void run(); + + /** QByteArray might not be the most efficient way to implement + a circular buffer but it's good enough and very simple */ + QByteArray m_readBuffer; + + /** A mutex to protect read buffer */ + QMutex m_readBufMtx; + + RawHID *m_hid; + + pjrc_rawhid *hiddev; + int hidno; + + bool m_running; +}; + + +/** +* This class is nearly the same than RawHIDReadThread but for writing +*/ +class RawHIDWriteThread : public QThread +{ +public: + RawHIDWriteThread(RawHID *hid); + virtual ~RawHIDWriteThread(); + + /** Add some data to be written without waiting */ + int pushDataToWrite(const char *data, int size); + + /** Return the number of bytes buffered */ + qint64 getBytesToWrite(); + +public slots: + void terminate() { + m_running = false; + } + +protected: + void run(); + + /** QByteArray might not be the most efficient way to implement + a circular buffer but it's good enough and very simple */ + QByteArray m_writeBuffer; + + /** A mutex to protect read buffer */ + QMutex m_writeBufMtx; + + /** Synchronize task with data arival */ + QWaitCondition m_newDataToWrite; + + RawHID *m_hid; + + pjrc_rawhid *hiddev; + int hidno; + + bool m_running; +}; + + + +RawHIDReadThread::RawHIDReadThread(RawHID *hid) + : m_hid(hid), + hiddev(&hid->dev), + hidno(hid->m_deviceNo), + m_running(true) +{ +} + +RawHIDReadThread::~RawHIDReadThread() +{ + m_running = false; + //wait for the thread to terminate + if(wait(1000) == false) + qDebug() << "Cannot terminate RawHIDReadThread"; +} + +void RawHIDReadThread::run() +{ + qDebug() << "Read thread started"; + while(m_running) + { + //here we use a temporary buffer so we don't need to lock + //the mutex while we are reading from the device + + // Want to read in regular chunks that match the packet size the device + // is using. In this case it is 64 bytes (the interrupt packet limit) + // although it would be nice if the device had a different report to + // configure this + char buffer[READ_SIZE] = {0}; + + int ret = hiddev->receive(hidno, buffer, READ_SIZE, READ_TIMEOUT); + + if(ret > 0) //read some data + { + QMutexLocker lock(&m_readBufMtx); + // Note: Preprocess the USB packets in this OS independent code + // First byte is report ID, second byte is the number of valid bytes + m_readBuffer.append(&buffer[2], buffer[1]); + + emit m_hid->readyRead(); + } + else if(ret == 0) //nothing read + { + } + else // < 0 => error + { + //TODO! make proper error handling, this only quick hack for unplug freeze + m_running=false; + } + } +} + +int RawHIDReadThread::getReadData(char *data, int size) +{ + QMutexLocker lock(&m_readBufMtx); + + size = qMin(size, m_readBuffer.size()); + + memcpy(data, m_readBuffer.constData(), size); + m_readBuffer.remove(0, size); + + return size; +} + +qint64 RawHIDReadThread::getBytesAvailable() +{ + QMutexLocker lock(&m_readBufMtx); + return m_readBuffer.size(); +} + +RawHIDWriteThread::RawHIDWriteThread(RawHID *hid) + : m_hid(hid), + hiddev(&hid->dev), + hidno(hid->m_deviceNo), + m_running(true) +{ +} + +RawHIDWriteThread::~RawHIDWriteThread() +{ + m_running = false; + //wait for the thread to terminate + if(wait(1000) == false) + qDebug() << "Cannot terminate RawHIDReadThread"; +} + +void RawHIDWriteThread::run() +{ + qDebug() << "Write thread started"; + while(m_running) + { + char buffer[WRITE_SIZE] = {0}; + + m_writeBufMtx.lock(); + int size = qMin(WRITE_SIZE-2, m_writeBuffer.size()); + while(size <= 0) + { + //wait on new data to write condition, the timeout + //enable the thread to shutdown properly + m_newDataToWrite.wait(&m_writeBufMtx, 200); + if(!m_running) + return; + + size = m_writeBuffer.size(); + } + + //NOTE: data size is limited to 2 bytes less than the + //usb packet size (64 bytes for interrupt) to make room + //for the reportID and valid data length + size = qMin(WRITE_SIZE-2, m_writeBuffer.size()); + memcpy(&buffer[2], m_writeBuffer.constData(), size); + buffer[1] = size; //valid data length + buffer[0] = 2; //reportID + m_writeBufMtx.unlock(); + + // must hold lock through the send to know how much was sent + int ret = hiddev->send(hidno, buffer, WRITE_SIZE, WRITE_TIMEOUT); + + if(ret > 0) + { + //only remove the size actually written to the device + QMutexLocker lock(&m_writeBufMtx); + m_writeBuffer.remove(0, size); + + emit m_hid->bytesWritten(ret - 2); + } + else if(ret < 0) // < 0 => error + { + //TODO! make proper error handling, this only quick hack for unplug freeze + m_running=false; + qDebug() << "Error writing to device"; + } + else + { + qDebug() << "No data written to device ??"; + } + } +} + +int RawHIDWriteThread::pushDataToWrite(const char *data, int size) +{ + QMutexLocker lock(&m_writeBufMtx); + + m_writeBuffer.append(data, size); + m_newDataToWrite.wakeOne(); //signal that new data arrived + + return size; +} + +qint64 RawHIDWriteThread::getBytesToWrite() +{ + // QMutexLocker lock(&m_writeBufMtx); + return m_writeBuffer.size(); +} + +RawHID::RawHID(const QString &deviceName) + :QIODevice(), + serialNumber(deviceName), + m_deviceNo(-1), + m_readThread(NULL), + m_writeThread(NULL) +{ + //find the device the user want to open and close the other + int opened = dev.open(USB_MAX_DEVICES, USB_VID, USB_PID, USB_USAGE_PAGE, USB_USAGE); + + //for each devices found, get serial number and close + for (int i=0; istart(); + m_writeThread->start(); +} + +RawHID::~RawHID() +{ + if(m_readThread) + delete m_readThread; + + if(m_writeThread) + delete m_writeThread; +} + +bool RawHID::open(OpenMode mode) +{ + if(m_deviceNo < 0) + return false; + + QIODevice::open(mode); + + if(!m_readThread) + m_readThread = new RawHIDReadThread(this); + + if(!m_writeThread) + m_writeThread = new RawHIDWriteThread(this); + + return true; +} + + +void RawHID::close() +{ + emit aboutToClose(); + if(m_readThread) { + m_readThread->terminate(); + delete m_readThread; // calls wait + m_readThread = NULL; + } + + if(m_writeThread) { + m_writeThread->terminate(); + delete m_writeThread; + m_writeThread = NULL; + } + + dev.close(m_deviceNo); + QIODevice::close(); +} + +bool RawHID::isSequential() const +{ + return true; +} + +qint64 RawHID::bytesAvailable() const +{ + return m_readThread->getBytesAvailable() + QIODevice::bytesAvailable(); +} + +qint64 RawHID::bytesToWrite() const +{ + return m_writeThread->getBytesToWrite() + QIODevice::bytesToWrite(); +} + +qint64 RawHID::readData(char *data, qint64 maxSize) +{ + return m_readThread->getReadData(data, maxSize); +} + +qint64 RawHID::writeData(const char *data, qint64 maxSize) +{ + return m_writeThread->pushDataToWrite(data, maxSize); +} + diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h b/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h index 73579e3d2..db85c4add 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid_const.h @@ -1,42 +1,41 @@ -/** - ****************************************************************************** - * - * @file rawhid_const.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup RawHIDPlugin Raw HID Plugin - * @{ - * @brief Impliments a HID USB connection to the flight hardware as a QIODevice - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef RAWHID_CONST_H -#define RAWHID_CONST_H - - -static const int MAX_DEVICES = 10; - -static const int VID = 0x20A0; -static const int PID = 0x4117; - -static const int USAGE_PAGE = 0xFF9C; -static const int USAGE = 0x0001; - -static const int DEV_SERIAL_LEN = 24; - -#endif // RAWHID_CONST_H +/** + ****************************************************************************** + * + * @file rawhid_const.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup RawHIDPlugin Raw HID Plugin + * @{ + * @brief Impliments a HID USB connection to the flight hardware as a QIODevice + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef RAWHID_CONST_H +#define RAWHID_CONST_H + +static const int USB_MAX_DEVICES = 10; + +static const int USB_VID = 0x20A0; +static const int USB_PID = 0x4117; + +static const int USB_USAGE_PAGE = 0xFF9C; +static const int USB_USAGE = 0x0001; + +static const int USB_DEV_SERIAL_LEN = 24; + +#endif // RAWHID_CONST_H diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp index 243b487fc..ecdcc98db 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhidplugin.cpp @@ -156,7 +156,7 @@ QStringList RawHIDConnection::availableDevices() if (enablePolling) { pjrc_rawhid dev; //open all device we can - int opened = dev.open(MAX_DEVICES, VID, PID, USAGE_PAGE, USAGE); + int opened = dev.open(USB_MAX_DEVICES, USB_VID, USB_PID, USB_USAGE_PAGE, USB_USAGE); //for each devices found, get serial number and close it back for(int i=0; i