libusbemu.cpp
Go to the documentation of this file.
1 /*
2 * This file is part of the OpenKinect Project. http://www.openkinect.org
3 *
4 * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
5 * for details.
6 *
7 * This code is licensed to you under the terms of the Apache License, version
8 * 2.0, or, at your option, the terms of the GNU General Public License,
9 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
10 * or the following URLs:
11 * http://www.apache.org/licenses/LICENSE-2.0
12 * http://www.gnu.org/licenses/gpl-2.0.txt
13 *
14 * If you redistribute this file in source form, modified or unmodified, you
15 * may:
16 * 1) Leave this header intact and distribute it under the same terms,
17 * accompanying it with the APACHE20 and GPL20 files, or
18 * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
19 * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file
20 * In all cases you must keep the copyright notice intact and include a copy
21 * of the CONTRIB file.
22 *
23 * Binary distributions must follow the binary distribution requirements of
24 * either License.
25 */
26 
27 // Headers required to enable Visual C++ Memory Leak mechanism:
28 // (NOTE: this should only be activated in Debug mode!)
29 #if defined(_MSC_VER) && defined(_DEBUG)
30 
31  // MSVC version predefined macros:
32  // http://social.msdn.microsoft.com/Forums/en-US/vcgeneral/thread/17a84d56-4713-48e4-a36d-763f4dba0c1c/
33  // _MSC_VER | MSVC Edition
34  // ---------+---------------
35  // 1300 | MSVC .NET 2002
36  // 1310 | MSVC .NET 2003
37  // 1400 | MSVC 2005
38  // 1500 | MSVC 2008
39  // 1600 | MSVC 2010
40 
41  // until VC71 (MSVC2003) the human-readable map/dump mode flag is CRTDBG_MAP_ALLOC:
42  // http://msdn.microsoft.com/en-us/library/x98tx3cf(v=vs.71).aspx
43  // but from VC80 (MSVC2005) and on the flag should be prefixed by a "_":
44  // http://msdn.microsoft.com/en-us/library/x98tx3cf(v=vs.80).aspx
45  #if _MSC_VER >= 1400
46  #define _CRTDBG_MAP_ALLOC
47  #else
48  #define CRTDBG_MAP_ALLOC
49  #endif
50  #include <stdlib.h>
51  #include <crtdbg.h>
52 
53  // Also, gotta redefine the new operator ...
54  // http://support.microsoft.com/kb/140858
55  // but before redefining it, for MSVC version prior to 2010, the STL <map>
56  // header must be included in order to avoid some scary compilation issues:
57  // http://social.msdn.microsoft.com/forums/en-US/vclanguage/thread/a6a148ed-aff1-4ec0-95d2-a82cd4c29cbb
58  #if _MSC_VER < 1600
59  #include <map>
60  #endif
61  // now it is safe to redefine the new operator
62  #define LIBUSBEMU_DEBUG_NEW new ( _NORMAL_BLOCK, __FILE__, __LINE__)
63  #define new LIBUSBEMU_DEBUG_NEW
64 
65 #endif
66 
67 #include "libusb.h"
68 #include "libusbemu_internal.h"
69 #include "failguard.h"
70 #include <cassert>
71 #include <algorithm>
72 #include "freenect_internal.h"
73 
74 using namespace libusbemu;
75 
76 #ifdef _DEBUG
77  #define LIBUSBEMU_DEBUG_BUILD
78 #endif//_DEBUG
79 
80 #ifdef LIBUSBEMU_DEBUG_BUILD
81  #define LIBUSB_DEBUG_CMD(cmd) cmd
82 #else
83  #define LIBUSB_DEBUG_CMD(cmd)
84 #endif//LIBUSBEMU_DEBUG_BUILD
85 
87 
89 {
90  usb_init();
92  (
93  const usb_version* version = usb_get_version();
94  fprintf(stdout, "libusb-win32: dll version %d.%d.%d.%d | driver (libusb0.sys) version %d.%d.%d.%d\n",
95  version->dll.major, version->dll.minor, version->dll.micro, version->dll.nano,
96  version->driver.major, version->driver.minor, version->driver.micro, version->driver.nano);
97  );
98  // there is no such a thing like 'context' in libusb-0.1...
99  // however, it is wise to emulate such context structure to localize and
100  // keep track of any resource and/or internal data structures, as well as
101  // to be able to clean-up itself at libusb_exit()
102  if (context == NULL)
103  context = &default_context;
104 
105  *context = new libusb_context;
106  // 0 on success; LIBUSB_ERROR on failure
107  return(0);
108 }
109 
111 {
112  if (ctx == NULL)
113  ctx = default_context;
114 
115  ctx->mutex.Enter();
116  // before deleting the context, delete all devices/transfers still in there:
117  while (!ctx->devices.empty())
118  {
119  libusb_context::TMapDevices::iterator itDevice (ctx->devices.begin());
120  libusb_device& device (itDevice->second);
121  if (NULL != device.handles)
122  {
123  // a simple "while(!device.handles->empty())" loop is impossible here
124  // because after a call to libusb_close() the device may be already
125  // destroyed (that's the official libusb-1.0 semantics when the ref
126  // count reaches zero), rendering "device.handles" to a corrupt state.
127  // an accurate "for" loop on the number of handles is the way to go.
128  const int handles = device.handles->size();
129  for (int h=0; h<handles; ++h)
130  {
131  libusb_device::TMapHandles::iterator itHandle = device.handles->begin();
132  libusb_device_handle* handle (&(itHandle->second));
133  libusb_close(handle);
134  }
135  }
136  }
137  ctx->mutex.Leave();
138  delete(ctx);
139 }
140 
141 
143 {
144  // Note: libusb-win32 doesn't support context-specific loglevels, so this
145  // sets the loglevel globally. If this is actually an issue for you, I
146  // will be surprised.
147  usb_set_debug(level);
148  return;
149 }
150 
152 {
153  if (ctx == NULL)
154  ctx = default_context;
155 
156  // libusb_device*** list demystified:
157  // libusb_device*** is the C equivalent to libusb_device**& in C++; such declaration
158  // allows the scope of this function libusb_get_device_list() to write on a variable
159  // of type libusb_device** declared within the function scope of the caller.
160  // libusb_device** is better understood as libusb_device*[], which is an array of
161  // pointers of type libusb_device*, each pointing to a struct that holds actual data.
162  usb_find_busses();
163  usb_find_devices();
164  usb_bus* bus = usb_get_busses();
165  while (bus)
166  {
167  struct usb_device* device = bus->devices;
168  while (device)
169  {
170  libusbemu_register_device(ctx, device);
171  device = device->next;
172  };
173  bus = bus->next;
174  };
175  // populate the device list that will be returned to the client:
176  // the list must be NULL-terminated to follow the semantics of libusb-1.0!
177  RAIIMutex lock (ctx->mutex);
178  libusb_device**& devlist = *list;
179  devlist = new libusb_device* [ctx->devices.size()+1]; // +1 is for a finalization mark
180  libusb_context::TMapDevices::iterator it (ctx->devices.begin());
181  libusb_context::TMapDevices::iterator end (ctx->devices.end());
182  for (int i=0; it!=end; ++it, ++i)
183  devlist[i] = &it->second;
184  // finalization mark to assist later calls to libusb_free_device_list()
185  devlist[ctx->devices.size()] = NULL;
186  // the number of devices in the outputted list, or LIBUSB_ERROR_NO_MEM on memory allocation failure.
187  return(ctx->devices.size());
188 }
189 
190 void libusb_free_device_list(libusb_device** list, int unref_devices)
191 {
192  if (NULL == list)
193  return;
194  if (unref_devices)
195  {
196  int i (0);
197  while (list[i] != NULL)
198  {
200  ++i;
201  }
202  }
203  delete[](list);
204 }
205 
207 {
208  struct usb_device* device (dev->device);
209  usb_device_descriptor& device_desc (device->descriptor);
210  // plain copy of one descriptor on to another: this is a safe operation because
211  // usb_device_descriptor and libusb_device_descriptor have the same signature
212  memcpy(desc, &device_desc, sizeof(libusb_device_descriptor));
213  // 0 on success; LIBUSB_ERROR on failure
214  return(0);
215 }
216 
218 {
219  RAIIMutex lock (dev->ctx->mutex);
220 
221  usb_dev_handle* usb_handle (usb_open(dev->device));
222  if (NULL == usb_handle)
223  {
225  return(LIBUSB_ERROR_OTHER);
226  }
227 
228  if (NULL == dev->handles)
230 
231  libusb_device_handle_t dummy = { dev, usb_handle };
232  libusb_device::TMapHandles::iterator it = dev->handles->insert(std::make_pair(usb_handle,dummy)).first;
233  *handle = &(it->second);
234  assert((*handle)->dev == dev);
235  assert((*handle)->handle == usb_handle);
236  dev->refcount++;
237 
238  if (NULL == dev->isoTransfers)
240 
241  //0 on success
242  // LIBUSB_ERROR_NO_MEM on memory allocation failure
243  // LIBUSB_ERROR_ACCESS if the user has insufficient permissions
244  // LIBUSB_ERROR_NO_DEVICE if the device has been disconnected
245  // another LIBUSB_ERROR code on other failure
246  return(0);
247 }
248 
250 {
251  RAIIMutex lock (dev_handle->dev->ctx->mutex);
252  libusb_device* device (dev_handle->dev);
253  if (device->handles->find(dev_handle->handle) == device->handles->end())
254  {
255  LIBUSBEMU_ERROR("libusb_close() attempted to close an unregistered handle!\n");
256  return;
257  }
258  if (0 != usb_close(dev_handle->handle))
259  {
261  return;
262  }
263  device->handles->erase(dev_handle->handle);
265 }
266 
267 int libusb_get_string_descriptor(libusb_device_handle *dev_handle, uint8_t desc_index, uint16_t langid, unsigned char *data, int length)
268 {
269  RAIIMutex lock (dev_handle->dev->ctx->mutex);
270  int bytes = usb_get_string(dev_handle->handle, (int)desc_index, (int)langid, (char*)data, (size_t)length);
271  if (bytes < 0) {
273  }
274  return bytes;
275 }
276 int libusb_get_string_descriptor_ascii(libusb_device_handle *dev_handle, uint8_t desc_index, unsigned char *data, int length)
277 {
278  RAIIMutex lock (dev_handle->dev->ctx->mutex);
279  int bytes = usb_get_string_simple(dev_handle->handle, (int)desc_index, (char*)data, (size_t)length);
280  if (bytes < 0) {
282  }
283  return bytes;
284 }
285 
286 int libusb_set_configuration(libusb_device_handle *dev, int configuration)
287 {
288  RAIIMutex lock (dev->dev->ctx->mutex);
289  int ret = usb_set_configuration(dev->handle, configuration);
290  if (0 != ret)
291  {
293  return ret;
294  }
295  // returns:
296  // 0 on success
297  // LIBUSB_ERROR_NOT_FOUND if the requested configuration does not exist
298  // LIBUSB_ERROR_BUSY if interfaces are currently claimed
299  // LIBUSB_ERROR_NO_DEVICE if the device has been disconnected
300  // another LIBUSB_ERROR code on other failure
301  //
302  return 0;
303 }
304 
305 int libusb_set_interface_alt_setting(libusb_device_handle *dev, int interface_number,int alternate_setting){
306  RAIIMutex lock (dev->dev->ctx->mutex);
307  if (0 != usb_set_altinterface(dev->handle, alternate_setting))
308  {
310  return(LIBUSB_ERROR_OTHER);
311  }
312 
313  return(0);
314 }
315 
316 int libusb_claim_interface(libusb_device_handle* dev, int interface_number)
317 {
318  RAIIMutex lock (dev->dev->ctx->mutex);
319  // according to the official libusb-win32 usb_set_configuration() documentation:
320  // http://sourceforge.net/apps/trac/libusb-win32/wiki/libusbwin32_documentation
321  // "Must be called!: usb_set_configuration() must be called with a valid
322  // configuration (not 0) before you can claim the interface. This might
323  // not be be necessary in the future. This behavior is different from
324  // Linux libusb-0.1."
325  if (0 != usb_set_configuration(dev->handle, 1))
326  {
328  return(LIBUSB_ERROR_OTHER);
329  }
330  if (0 != usb_claim_interface(dev->handle, interface_number))
331  {
333  return(LIBUSB_ERROR_OTHER);
334  }
335 
336  //if (0 != usb_set_altinterface(dev->handle, 0))
337  // return(LIBUSB_ERROR_OTHER);
338 
339  // LIBUSB_ERROR_NOT_FOUND if the requested interface does not exist
340  // LIBUSB_ERROR_BUSY if another program or driver has claimed the interface
341  // LIBUSB_ERROR_NO_DEVICE if the device has been disconnected
342  // a LIBUSB_ERROR code on other failure
343  // 0 on success
344  return(0);
345 }
346 
347 int libusb_release_interface(libusb_device_handle* dev, int interface_number)
348 {
349  RAIIMutex lock (dev->dev->ctx->mutex);
350  if (0 != usb_release_interface(dev->handle, interface_number))
351  {
353  return(LIBUSB_ERROR_OTHER);
354  }
355  // LIBUSB_ERROR_NOT_FOUND if the interface was not claimed
356  // LIBUSB_ERROR_NO_DEVICE if the device has been disconnected
357  // another LIBUSB_ERROR code on other failure
358  // 0 on success
359  return(0);
360 }
361 
363 {
364  int num_devices;
365  libusb_device** list;
366  libusb_device_handle *dev_handle = NULL;
367 
368  if (ctx == NULL)
369  ctx = default_context;
370 
371  num_devices = libusb_get_device_list(ctx, &list);
372  if (num_devices < 0)
373  return NULL;
374 
375  unsigned int i = 0;
376  while (list[i] != NULL) {
377  struct libusb_device_descriptor desc;
378  int ret = libusb_get_device_descriptor(list[i], &desc);
379  if (ret < 0)
380  break;
381 
382  if (desc.idVendor == vendor_id && desc.idProduct == product_id) {
383  ret = libusb_open(list[i], &dev_handle);
384  if (ret)
385  break;
386  }
387  i++;
388  }
389 
390  libusb_free_device_list(list, 1);
391  return dev_handle;
392 }
393 
394 int libusb_control_transfer(libusb_device_handle* dev_handle, uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char* data, uint16_t wLength, unsigned int timeout)
395 {
396  // in libusb-1.0 a timeout of zero it means 'wait indefinitely'; in libusb-0.1, a timeout of zero means 'return immediatelly'!
397  timeout = (0 == timeout) ? 60000 : timeout; // wait 60000ms (60s = 1min) if the transfer is supposed to wait indefinitely...
398  int bytes_transferred = usb_control_msg(dev_handle->handle, bmRequestType, bRequest, wValue, wIndex, (char*)data, wLength, timeout);
399  if (bytes_transferred < 0)
400  {
402  return(LIBUSB_ERROR_OTHER);
403  }
404  // on success, the number of bytes actually transferred
405  // LIBUSB_ERROR_TIMEOUT if the transfer timed out
406  // LIBUSB_ERROR_PIPE if the control request was not supported by the device
407  // LIBUSB_ERROR_NO_DEVICE if the device has been disconnected
408  // another LIBUSB_ERROR code on other failures
409  return(bytes_transferred);
410 }
411 
412 int libusb_bulk_transfer(libusb_device_handle* dev_handle, uint8_t endpoint, uint8_t *data, int length, int *transferred, unsigned int timeout)
413 {
414  // in libusb-1.0 a timeout of zero it means 'wait indefinitely'; in libusb-0.1, a timeout of zero means 'return immediately'!
415  timeout = (0 == timeout) ? 60000 : timeout; // wait 60000ms (60s = 1min) if the transfer is supposed to wait indefinitely...
416  int bytes_transferred;
417  if (endpoint & LIBUSB_ENDPOINT_IN) { // Device to Host
418  bytes_transferred = usb_bulk_read(dev_handle->handle, endpoint, (char*)data, length, timeout);
419  } else { // Host to Device
420  bytes_transferred = usb_bulk_write(dev_handle->handle, endpoint, (char*)data, length, timeout);
421  }
422  if (bytes_transferred < 0) {
423  // 0 on success (and populates transferred)
424  // LIBUSB_ERROR_TIMEOUT if the transfer timed out (and populates transferred)
425  // LIBUSB_ERROR_PIPE if the endpoint halted
426  // LIBUSB_ERROR_OVERFLOW if the device offered more data, see Packets and overflows
427  // LIBUSB_ERROR_NO_DEVICE if the device has been disconnected
428  // another LIBUSB_ERROR code on other failures
429  *transferred = 0;
431  return(LIBUSB_ERROR_OTHER);
432  }
433  *transferred = bytes_transferred;
434  return 0;
435 }
436 
437 // FROM HERE ON CODE BECOMES QUITE MESSY: ASYNCHRONOUS TRANSFERS MANAGEMENT
438 
439 struct libusb_transfer* libusb_alloc_transfer(int iso_packets)
440 {
441  transfer_wrapper* wrapper = new transfer_wrapper;
442  memset(wrapper, 0, sizeof(transfer_wrapper));
443  libusb_transfer& transfer (wrapper->libusb);
444  transfer.num_iso_packets = iso_packets;
445  transfer.iso_packet_desc = new libusb_iso_packet_descriptor [iso_packets];
446  memset(transfer.iso_packet_desc, 0, iso_packets*sizeof(libusb_iso_packet_descriptor));
447  // a newly allocated transfer, or NULL on error
448  return(&transfer);
449 }
450 
451 void libusb_free_transfer(struct libusb_transfer* transfer)
452 {
453  // according to the official libusb_free_transfer() documentation:
454  // "It is legal to call this function with a NULL transfer.
455  // In this case, the function will simply return safely."
456  if (NULL == transfer)
457  return;
458 
459  // according to the official libusb_free_transfer() documentation:
460  // "It is not legal to free an active transfer
461  // (one which has been submitted and has not yet completed)."
462  // that means that only "orphan" transfers can be deleted:
464  if (!libusb_device::TListTransfers::Orphan(wrapper))
465  {
466  LIBUSBEMU_ERROR("libusb_free_transfer() attempted to free an active transfer!");
467  return;
468  }
469 
470  if (0 != usb_free_async(&wrapper->usb))
471  {
473  return;
474  }
475 
476  if (NULL != transfer->iso_packet_desc)
477  delete[](transfer->iso_packet_desc);
478 
479  // according to the official libusb_free_transfer() documentation:
480  // "If the LIBUSB_TRANSFER_FREE_BUFFER flag is set and the transfer buffer
481  // is non-NULL, this function will also free the transfer buffer using the
482  // standard system memory allocator (e.g. free())."
483  if (transfer->flags & LIBUSB_TRANSFER_FREE_BUFFER)
484  free(transfer->buffer);
485 
486  delete(wrapper);
487 }
488 
490 {
491  // according to the official libusb_fill_iso_transfer() documentation:
492  // "libusb_fill_iso_transfer() is a helper function to populate the required
493  // libusb_transfer fields for an isochronous transfer."
494  // What this means is that the library client is not required to call this
495  // helper function in order to setup the fields within the libusb_transfer
496  // struct. Thus, this is NOT the place for any sort of special processing
497  // because there are no guarantees that such function will ever be invoked.
498  transfer->dev_handle = dev_handle;
499  transfer->endpoint = endpoint;
500  transfer->buffer = buffer;
501  transfer->length = length;
502  transfer->num_iso_packets = num_iso_packets;
503  transfer->callback = callback;
504  transfer->timeout = timeout;
505  transfer->user_data = user_data;
507 
508  // control some additonal library duties such as:
509  // LIBUSB_TRANSFER_SHORT_NOT_OK, LIBUSB_TRANSFER_FREE_BUFFER, LIBUSB_TRANSFER_FREE_TRANSFER
510  transfer->flags;
511  // these two are output parameters coming from actual transfers...
512  transfer->actual_length;
513  transfer->status;
514 }
515 
516 void libusb_set_iso_packet_lengths(struct libusb_transfer* transfer, unsigned int length)
517 {
518  // according to the official libusb_fill_iso_transfer() documentation:
519  // "Convenience function to set the length of all packets in an isochronous
520  // transfer, based on the num_iso_packets field in the transfer structure."
521  // For the same reasons as in libusb_fill_iso_transfer(), no additional
522  // processing should ever happen withing this function...
523  for (int i=0; i < transfer->num_iso_packets; ++i)
524  transfer->iso_packet_desc[i].length = length;
525 }
526 
527 int ReapThreadProc(void* params);
528 
530 {
532 
533  // the first time a transfer is submitted, the libusb-0.1 transfer context
534  // (the void*) must be created and initialized with a proper call to one of
535  // the usb_***_setup_async() functions; one could thing of doing this setup
536  // within libusb_fill_***_transfer(), but the latter are just convenience
537  // functions to fill the transfer data structure: the library client is not
538  // forced to call them and could fill the fields directly within the struct.
539  if (NULL == wrapper->usb)
540  libusbemu_setup_transfer(wrapper);
541 
542  libusbemu_clear_transfer(wrapper);
543 
544  int ret = usb_submit_async(wrapper->usb, (char*)transfer->buffer, transfer->length);
545  if (ret < 0)
546  {
547  // TODO: better error handling...
548  // what does usb_submit_async() actually returns on error?
549  // LIBUSB_ERROR_NO_DEVICE if the device has been disconnected
550  // LIBUSB_ERROR_BUSY if the transfer has already been submitted.
551  // another LIBUSB_ERROR code on other failure
553  return(ret);
554  }
555 
556  libusb_device::TMapIsocTransfers& isoTransfers (*transfer->dev_handle->dev->isoTransfers);
557  libusb_device::TMapIsocTransfers::iterator it = isoTransfers.find(transfer->endpoint);
558  if (isoTransfers.end() == it)
559  {
561  it = isoTransfers.insert(std::make_pair(transfer->endpoint, dummy)).first;
562  }
563  libusb_device::isoc_handle& iso (it->second);
564  iso.listTransfers.Append(wrapper);
565 
566  if (NULL == iso.poReapThread)
567  {
568  void** state = new void* [2];
569  state[0] = transfer->dev_handle;
570  state[1] = (void*)transfer->endpoint;
571  iso.poReapThread = new QuickThread(ReapThreadProc, (void*)state, true);
572  }
573 
574  // 0 on success
575  return(0);
576 }
577 
579 {
581  // according to the official libusb_cancel_transfer() documentation:
582  // "This function returns immediately, but this does not indicate
583  // cancellation is complete. Your callback function will be invoked at
584  // some later time with a transfer status of LIBUSB_TRANSFER_CANCELLED."
585  // This semantic can be emulated by setting the transfer->status flag to
586  // LIBUSB_TRANSFER_CANCELLED, leaving the rest to libusb_handle_events().
587  transfer->status = LIBUSB_TRANSFER_CANCELLED;
588  int ret = usb_cancel_async(wrapper->usb);
589  if (ret != 0)
591  // 0 on success
592  // LIBUSB_ERROR_NOT_FOUND if the transfer is already complete or cancelled.
593  // a LIBUSB_ERROR code on failure
594  return(ret);
595 }
596 
597 // FROM HERE ON CODE BECOMES REALLY REALLY REALLY MESSY: HANDLE EVENTS STUFF
598 
599 int libusbemu_handle_isochronous(libusb_context* ctx, const unsigned int milliseconds)
600 {
601  if (ctx == NULL)
602  ctx = default_context;
603 
604  //QuickThread::Myself().RaisePriority();
605  RAIIMutex lock (ctx->mutDeliveryPool);
606  int index = ctx->hWantToDeliverPool.WaitAnyUntilTimeout(milliseconds);
607  if (-1 != index)
608  {
609  EventList hDoneDeliveringPoolLocal;
610  while (-1 != (index = ctx->hWantToDeliverPool.CheckAny()))
611  {
612  ctx->hAllowDeliveryPool[index]->Signal();
613  ctx->hWantToDeliverPool[index]->Reset();
614  hDoneDeliveringPoolLocal.AttachEvent(ctx->hDoneDeliveringPool[index]);
615  }
616  hDoneDeliveringPoolLocal.WaitAll();
617  }
618  //QuickThread::Myself().LowerPriority();
619  return(0);
620 }
621 
623 {
624  if (ctx == NULL)
625  ctx = default_context;
626 
627  if (failguard::Abort())
628  return(LIBUSB_ERROR_INTERRUPTED);
629 
630  RAIIMutex lock (ctx->mutex);
631 
632  libusbemu_handle_isochronous(ctx, 60000);
633 
634  // 0 on success, or a LIBUSB_ERROR code on failure
635  return(0);
636 }
637 
639 {
640  if (ctx == NULL)
641  ctx = default_context;
642 
643  if (failguard::Abort())
644  return(LIBUSB_ERROR_INTERRUPTED);
645 
646  RAIIMutex lock (ctx->mutex);
647 
648  if (timeout == NULL)
650  else
651  libusbemu_handle_isochronous(ctx, (timeout->tv_sec * 1000) + (timeout->tv_usec / 1000));
652 
653  // 0 on success, or a LIBUSB_ERROR code on failure
654  return(0);
655 }
656 
657 void PreprocessTransferNaive(libusb_transfer* transfer, const int read);
658 void PreprocessTransferFreenect(libusb_transfer* transfer, const int read);
660 
662 {
663  // paranoid check...
664  assert(libusb_device::TListTransfers::Orphan(wrapper));
665 
666  libusb_transfer* transfer = &wrapper->libusb;
667 
668  // if data is effectively acquired (non-zero bytes transfer), all of the
669  // associated iso packed descriptors must be filled properly; this is an
670  // application specific task and requires knowledge of the logic behind
671  // the streams being transferred: PreprocessTransfer() is an user-defined
672  // "library-injected" routine that should perform this task.
673  if (transfer->actual_length > 0)
674  PreprocessTransfer(transfer, transfer->actual_length);
675 
676  // callback the library client through the callback; at this point, the
677  // client is assumed to do whatever it wants to the data and, possibly,
678  // resubmitting the transfer, which would then place the transfer at the
679  // end of its related asynchronous list (orphan transfer is adopted).
680  transfer->callback(transfer);
681 }
682 
684 
685 int ReapThreadProc(void* params)
686 {
687  LIBUSB_DEBUG_CMD(fprintf(stdout, "Thread execution started.\n"));
688 
689  void** state = (void**)params;
691  const int endpoint = (int)state[1];
692  delete[](state);
693 
694  libusb_device::TMapIsocTransfers& isocTransfers = *(dev_handle->dev->isoTransfers);
695  libusb_device::isoc_handle& isocHandle = isocTransfers[endpoint];
696  libusb_device::TListTransfers& listTransfers (isocHandle.listTransfers);
697  QuickThread*& poThreadObject = isocHandle.poReapThread;
698  libusb_context* ctx (dev_handle->dev->ctx);
699 
700  bool boAbort (false);
701 
702  bool boDeliverRequested (false);
703  QuickEvent wannaDeliver;
704  QuickEvent allowDeliver;
705  QuickEvent doneDelivering;
706  ctx->mutDeliveryPool.Enter();
707  ctx->hWantToDeliverPool.AttachEvent(&wannaDeliver);
708  ctx->hAllowDeliveryPool.AttachEvent(&allowDeliver);
709  ctx->hDoneDeliveringPool.AttachEvent(&doneDelivering);
710  ctx->mutDeliveryPool.Leave();
711 
712  libusb_device::TListTransfers listReadyLocal;
713 
714  // isochronous I/O must be handled in high-priority! (at least TIME_CRITICAL)
715  // otherwise, sequence losses are prone to happen...
717 
718  // keep the thread alive as long as there are pending or ready transfers
719  while(!listTransfers.Empty() || !listReadyLocal.Empty())
720  {
721  // prioritize transfers that are ready to be delivered
722  if (!listReadyLocal.Empty())
723  {
724  // signal the delivery request, if not signaled yet
725  if (!boDeliverRequested)
726  {
727  doneDelivering.Reset();
728  wannaDeliver.Signal();
729  boDeliverRequested = true;
730  }
731  // delivery request already signaled; wait for the delivery permission
732  else if (allowDeliver.WaitUntilTimeout(1))
733  {
734  boDeliverRequested = false;
735  while (!listReadyLocal.Empty())
736  {
737  transfer_wrapper* wrapper = listReadyLocal.Head();
738  listReadyLocal.Remove(wrapper);
740  }
741  doneDelivering.Signal();
742  }
743  }
744 
745  // check for pending transfers coming from the device stream
746  if (!listTransfers.Empty())
747  {
748  transfer_wrapper* wrapper = listTransfers.Head();
749  int read = ReapTransfer(wrapper, 10000, &listReadyLocal);
750  if (-5 == read)
751  {
752  while (!listTransfers.Empty())
753  listTransfers.Remove(listTransfers.Head());
754  while (!listReadyLocal.Empty())
755  listReadyLocal.Remove(listReadyLocal.Head());
756  }
757  }
758  // if there are no pending transfers, wait the ready ones to be delivered
759  else
760  {
761  LIBUSB_DEBUG_CMD(fprintf(stdout, "ReapThreadProc(): no pending transfers, sleeping until delivery...\n"));
762  if (!boDeliverRequested)
763  {
764  doneDelivering.Reset();
765  wannaDeliver.Signal();
766  boDeliverRequested = true;
767  }
768  allowDeliver.Wait();
769  }
770 
771  // finally, check the thread failguard
772  if (failguard::Check() && !boAbort)
773  {
775  if (failguard::Abort())
776  {
777  LIBUSB_DEBUG_CMD(fprintf(stderr, "Thread is aborting: releasing transfers...\n"));
779  boDeliverRequested = true;
780  allowDeliver.Signal();
781  boAbort = true;
782  }
783  }
784  }
785 
787 
789  (
790  if (boAbort)
791  fprintf(stderr, "Thread loop aborted.\n");
792  );
793 
794  while(!ctx->mutDeliveryPool.TryEnter())
795  {
796  wannaDeliver.Signal();
797  doneDelivering.Signal();
798  }
799  ctx->hWantToDeliverPool.DetachEvent(&wannaDeliver);
800  ctx->hAllowDeliveryPool.DetachEvent(&allowDeliver);
801  ctx->hDoneDeliveringPool.DetachEvent(&doneDelivering);
802  ctx->mutDeliveryPool.Leave();
803 
804  poThreadObject = NULL;
805 
806  LIBUSB_DEBUG_CMD(fprintf(stdout, "Thread execution finished.\n"));
807  return(0);
808 }
809 
811 {
812  void* context (wrapper->usb);
813  libusb_transfer* transfer (&wrapper->libusb);
814 
815  const int read = usb_reap_async_nocancel(context, timeout);
816  if (read >= 0)
817  {
818  // data successfully acquired (0 bytes is also a go!)
819  transfer->status = LIBUSB_TRANSFER_COMPLETED;
820 
821  // according to the official libusb_transfer struct reference:
822  // "int libusb_transfer::actual_length
823  // Actual length of data that was transferred.
824  // Read-only, and only for use within transfer callback function.
825  // Not valid for isochronous endpoint transfers."
826  // since the client will allegedly not read from this field, we'll be using
827  // it here just to simplify the emulation implementation, more specifically
828  // the libusb_handle_events() and libusbemu_clear_transfer().
829  transfer->actual_length = read;
830 
831  // the transfer should be removed from the head of the list and put into
832  // an orphan state; it is up to the client code to resubmit the transfer
833  // which will possibly happen during the client callback.
834  libusb_device::TListTransfers::RemoveNode(wrapper);
835 
836  // two possibilities here: either deliver the transfer now or postpone the
837  // delivery to keep it in sync with libusb_handle_events(); in the latter
838  // case, a destination list must be provided.
839  if (NULL != lstReady)
840  lstReady->Append(wrapper);
841  else
843  }
844  else
845  {
846  // something bad happened:
847  // (a) the timeout passed to usb_reap_async_nocancel() expired;
848  // (b) the transfer was cancelled via usb_cancel_async();
849  // (c) some fatal error triggered.
850 #undef EIO
851 #undef EINVAL
852 #undef ETIMEOUT
853  enum EReapResult { EIO = -5, EINVAL = -22, ETIMEOUT = -116 };
854  switch(read)
855  {
856  // When usb_reap_async_nocancel() returns ETIMEOUT, then either:
857  // (a) the timeout indeed expired;
858  // (b) the transfer was cancelled.
859  case ETIMEOUT :
860  // when usb_reap_async_nocancel() returns ETIMEOUT, then either:
861  // (a) the timeout indeed expired: in this case the transfer should
862  // remain as the head of the transfer list (do not remove the node)
863  // and silently return without calling back the client (or perhaps
864  // set the transfer status to LIBUSB_TRANSFER_TIMED_OUT and then
865  // call back - MORE INVESTIGATION REQUIRED)
866  // (b) the transfer was cancelled: in this case the transfer should be
867  // removed from the list and reported back through the callback.
868  if (LIBUSB_TRANSFER_CANCELLED == transfer->status)
869  {
870  libusb_device::TListTransfers::RemoveNode(wrapper);
871  for (int i=0; i<transfer->num_iso_packets; ++i)
873  transfer->callback(transfer);
874  }
875  break;
876  case EINVAL :
877  // I guess -22 is returned if one attempts to reap a context that does
878  // not exist anymore (one that has already been deleted)
880  break;
881  case EIO :
882  // Error code -5 seems to be triggered when the device is lost...
884  libusb_device::TListTransfers::RemoveNode(wrapper);
885  transfer->status = LIBUSB_TRANSFER_NO_DEVICE;
886  transfer->callback(transfer);
887  libusb_cancel_transfer(transfer);
888  transfer->status = LIBUSB_TRANSFER_NO_DEVICE;
889  break;
890  default :
891  // I have not stumbled into any other negative values coming from the
892  // usb_reap_async_nocancel()... Anyway, cancel seems to be a simple yet
893  // plausible preemptive approach... MORE INVESTIGATION NEEDED!
895  libusb_cancel_transfer(transfer);
896  break;
897  }
898  }
899 
900  return(read);
901 }
902 
903 // Naive transfer->iso_packet_desc array filler. It will probably never work
904 // with any device, but it serves as a template and as a default handler...
905 void PreprocessTransferNaive(libusb_transfer* transfer, const int read)
906 {
907  unsigned int remaining (read);
908  const int pkts (transfer->num_iso_packets);
909  for (int i=0; i<pkts; ++i)
910  {
911  libusb_iso_packet_descriptor& desc (transfer->iso_packet_desc[i]);
913  desc.actual_length = MIN(remaining, desc.length);
914  remaining -= desc.actual_length;
915  }
916 }
917 
918 // This is were the transfer->iso_packet_desc array is built. Knowledge of
919 // the underlying device stream protocol is required in order to properly
920 // setup this array. Moreover, it is also necessary to sneak into some of
921 // the libfreenect internals so that the proper length of each iso packet
922 // descriptor can be inferred. Fortunately, libfreenect has this information
923 // avaliable in the "transfer->user_data" field which holds a pointer to a
924 // fnusb_isoc_stream struct with all the information required in there.
925 void PreprocessTransferFreenect(libusb_transfer* transfer, const int read)
926 {
927  fnusb_isoc_stream* xferstrm = (fnusb_isoc_stream*)transfer->user_data;
928  freenect_device* dev = xferstrm->parent->parent;
929  packet_stream* pktstrm = (transfer->endpoint == 0x81) ? &dev->video : &dev->depth;
930 
931  // Kinect Camera Frame Packet Header (12 bytes total):
932  struct pkt_hdr
933  {
934  uint8_t magic[2];
935  uint8_t pad;
936  uint8_t flag;
937  uint8_t unk1;
938  uint8_t seq;
939  uint8_t unk2;
940  uint8_t unk3;
941  uint32_t timestamp;
942  };
943 
944  //packet sizes:
945  // first middle last
946  // Bayer 1920 1920 24
947  // IR 1920 1920 1180
948  // YUV422 1920 1920 36
949  // Depth 1760 1760 1144
950  const unsigned int pktlen = sizeof(pkt_hdr) + pktstrm->pkt_size;
951  const unsigned int pktend = sizeof(pkt_hdr) + pktstrm->last_pkt_size;
952 
953  unsigned int remaining (read);
954  unsigned int leftover (transfer->length);
955  unsigned char* pktbuffer (transfer->buffer);
956  const int pkts (transfer->num_iso_packets);
957  for (int i=0; i<pkts; ++i)
958  {
959  libusb_iso_packet_descriptor& desc (transfer->iso_packet_desc[i]);
961  const pkt_hdr& header (*(pkt_hdr*)pktbuffer);
962  if ((header.magic[0] == 'R') && (header.magic[1] == 'B'))
963  {
964  switch(header.flag & 0x0F)
965  {
966  case 0x01 : // first frame packet
967  case 0x02 : // intermediate frame packets
968  desc.actual_length = MIN(remaining, pktlen);
969  break;
970  case 0x05 : // last frame packet
971  desc.actual_length = MIN(remaining, pktend);
972  break;
973  default :
974  fprintf(stdout, "0x%02X\n", header.flag);
975  break;
976  }
977  }
978  else
979  {
980  desc.actual_length = 0;
981  }
982  remaining -= desc.actual_length;
983  pktbuffer += desc.length; // a.k.a: += 1920
984  leftover -= desc.length; // a.k.a: -= 1920
985  }
986 
988  (
989  if (remaining > 0)
990  {
991  fprintf(stdout, "%d remaining out of %d\n", remaining, read);
992  if (remaining == read)
993  fprintf(stdout, "no bytes consumed!\n");
994  }
995  );
996 }
struct usb_device * device
int WaitAnyUntilTimeout(const unsigned int milliseconds)
#define LIBUSB_DEBUG_CMD(cmd)
Definition: libusbemu.cpp:83
const bool Remove(T *node)
int ReapThreadProc(void *params)
Definition: libusbemu.cpp:685
enum libusb_transfer_status status
Definition: libusb.h:177
void libusb_free_device_list(libusb_device **list, int unref_devices)
Definition: libusbemu.cpp:190
uint8_t magic[2]
Definition: cameras.c:77
void libusb_close(libusb_device_handle *dev_handle)
Definition: libusbemu.cpp:249
void libusb_exit(libusb_context *ctx)
Definition: libusbemu.cpp:110
enum libusb_transfer_status status
Definition: libusb.h:163
long ssize_t
Definition: unistd.h:36
uint8_t flags
Definition: libusb.h:159
unsigned short uint16_t
int libusbemu_handle_isochronous(libusb_context *ctx, const unsigned int milliseconds)
Definition: libusbemu.cpp:599
int libusb_control_transfer(libusb_device_handle *dev_handle, uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout)
Definition: libusbemu.cpp:394
#define LIBUSBEMU_ERROR_LIBUSBWIN32()
void PreprocessTransferFreenect(libusb_transfer *transfer, const int read)
Definition: libusbemu.cpp:925
int libusb_get_string_descriptor(libusb_device_handle *dev_handle, uint8_t desc_index, uint16_t langid, unsigned char *data, int length)
Definition: libusbemu.cpp:267
unsigned char * buffer
Definition: libusb.h:168
int libusb_get_device_descriptor(libusb_device *dev, struct libusb_device_descriptor *desc)
Definition: libusbemu.cpp:206
void libusbemu_unregister_device(libusb_device *dev)
void libusb_fill_iso_transfer(struct libusb_transfer *transfer, libusb_device_handle *dev_handle, unsigned char endpoint, unsigned char *buffer, int length, int num_iso_packets, libusb_transfer_cb_fn callback, void *user_data, unsigned int timeout)
Definition: libusbemu.cpp:489
QuickList< transfer_wrapper > TListTransfers
void libusb_set_iso_packet_lengths(struct libusb_transfer *transfer, unsigned int length)
Definition: libusbemu.cpp:516
EventList hDoneDeliveringPool
int libusb_get_string_descriptor_ascii(libusb_device_handle *dev_handle, uint8_t desc_index, unsigned char *data, int length)
Definition: libusbemu.cpp:276
void(* libusb_transfer_cb_fn)(struct libusb_transfer *transfer)
Definition: libusb.h:94
unsigned char uint8_t
static void(* PreprocessTransfer)(libusb_transfer *, const int)(PreprocessTransferFreenect)
Definition: libusbemu.cpp:659
const bool Abort()
Definition: failguard.cpp:46
unsigned int timeout
Definition: libusb.h:162
int libusb_bulk_transfer(libusb_device_handle *dev_handle, uint8_t endpoint, uint8_t *data, int length, int *transferred, unsigned int timeout)
Definition: libusbemu.cpp:412
QuickMutex mutDeliveryPool
libusb_transfer_cb_fn callback
Definition: libusb.h:166
const bool WaitUntilTimeout(const unsigned int milliseconds)
int libusb_init(libusb_context **context)
Definition: libusbemu.cpp:88
const bool AttachEvent(QuickEvent *poEvent)
TMapHandles * handles
int libusb_set_configuration(libusb_device_handle *dev, int configuration)
Definition: libusbemu.cpp:286
static libusb_context * default_context
Definition: libusbemu.cpp:86
unsigned int actual_length
Definition: libusb.h:176
void libusbemu_clear_transfer(transfer_wrapper *wrapper)
fnusb_dev * parent
Definition: usb_libusb10.h:75
libusb_device * libusbemu_register_device(libusb_context *ctx, struct usb_device *dev)
int libusb_handle_events(libusb_context *ctx)
Definition: libusbemu.cpp:622
EventList hAllowDeliveryPool
void libusbemu_deliver_transfer(transfer_wrapper *wrapper)
Definition: libusbemu.cpp:661
void libusb_free_transfer(struct libusb_transfer *transfer)
Definition: libusbemu.cpp:451
int libusb_cancel_transfer(struct libusb_transfer *transfer)
Definition: libusbemu.cpp:578
void * user_data
Definition: libusb.h:167
std::map< usb_dev_handle *, libusb_device_handle > TMapHandles
MyFreenectDevice * device
libusb_device_handle * libusb_open_device_with_vid_pid(libusb_context *ctx, uint16_t vendor_id, uint16_t product_id)
Definition: libusbemu.cpp:362
int libusb_handle_events_timeout(libusb_context *ctx, struct timeval *timeout)
Definition: libusbemu.cpp:638
struct libusb_iso_packet_descriptor * iso_packet_desc
Definition: libusb.h:170
void(* callback)(void)
struct libusb_transfer * libusb_alloc_transfer(int iso_packets)
Definition: libusbemu.cpp:439
libusb_device_handle * dev_handle
Definition: libusb.h:158
int libusbemu_setup_transfer(transfer_wrapper *wrapper)
freenect_device * parent
Definition: usb_libusb10.h:69
#define LIBUSBEMU_ERROR(msg)
void PreprocessTransferNaive(libusb_transfer *transfer, const int read)
Definition: libusbemu.cpp:905
typedef void(ONI_CALLBACK_TYPE *DeviceConnectedCallback)(const OniDeviceInfo *
uint8_t flag
Definition: cameras.c:79
int num_iso_packets
Definition: libusb.h:169
static freenect_context * ctx
int actual_length
Definition: libusb.h:165
const bool Empty() const
int libusb_claim_interface(libusb_device_handle *dev, int interface_number)
Definition: libusbemu.cpp:316
T MIN(const T &v1, const T &v2)
transfer_wrapper * libusbemu_get_transfer_wrapper(libusb_transfer *transfer)
int libusb_open(libusb_device *dev, libusb_device_handle **handle)
Definition: libusbemu.cpp:217
unsigned int uint32_t
void libusb_set_debug(libusb_context *ctx, int level)
Definition: libusbemu.cpp:142
capture state
Definition: micview.c:53
EventList hWantToDeliverPool
struct libusb_context_t libusb_context
Definition: libusb.h:59
const bool DetachEvent(QuickEvent *poEvent)
unsigned char endpoint
Definition: libusb.h:160
int libusb_submit_transfer(struct libusb_transfer *transfer)
Definition: libusbemu.cpp:529
int ReapTransfer(transfer_wrapper *, unsigned int, libusb_device::TListTransfers *)
Definition: libusbemu.cpp:810
TMapIsocTransfers * isoTransfers
const bool Check()
Definition: failguard.cpp:16
int libusb_release_interface(libusb_device_handle *dev, int interface_number)
Definition: libusbemu.cpp:347
ssize_t libusb_get_device_list(libusb_context *ctx, libusb_device ***list)
Definition: libusbemu.cpp:151
libusb_transfer libusb
std::map< int, isoc_handle > TMapIsocTransfers
libusb_context * ctx
int libusb_set_interface_alt_setting(libusb_device_handle *dev, int interface_number, int alternate_setting)
Definition: libusbemu.cpp:305
unsigned char type
Definition: libusb.h:161


libfreenect
Author(s): Hector Martin, Josh Blake, Kyle Machulis, OpenKinect community
autogenerated on Thu Jun 6 2019 19:25:38