Main MRPT website > C++ reference for MRPT 1.9.9
usbinterface.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #include <xsens/xsthread.h>
10 #include <xsens/xsportinfo.h>
11 #include "usbinterface.h"
12 #include <errno.h>
13 
14 #ifdef USE_WINUSB
15 #include "xswinusb.h"
16 #else
17 #include "xslibusb.h"
18 #endif
19 
20 #ifndef _WIN32
21 #include <string.h> // strcpy
22 #else
23 #include <winbase.h>
24 #include <io.h>
25 #endif
26 
27 #ifndef _CRT_SECURE_NO_DEPRECATE
28 #define _CRT_SECURE_NO_DEPRECATE
29 #ifdef _WIN32
30 #pragma warning(disable : 4996)
31 #endif
32 #endif
33 
34 /*! \brief Private object for UsbInterface */
36 {
37  public:
38 #ifdef LOG_RX_TX
39  XsFileHandle* rx_log;
40  XsFileHandle* tx_log;
41 #endif
42 
43  //! The time at which an operation will end in ms, used by several
44  //! functions.
46  //! The last result of an operation
48  /*! The default timeout value to use during blocking operations.
49  A value of 0 means that all operations become non-blocking.
50  */
52 
57  char m_portname[256];
58 
59 #ifdef USE_WINUSB
60  XsWinUsb m_winUsb;
61  WINUSB_INTERFACE_HANDLE m_usbHandle[2];
62  uint8_t m_bulkInPipe, m_bulkOutPipe, m_interruptPipe, m_deviceSpeed;
63  uint16_t m_bulkInPipePacketSize;
65 
66  XsThreadId m_threadId;
67  XsThread m_threadHandle;
68  static const int m_oCount = MAXIMUM_WAIT_OBJECTS - 1;
69  OVERLAPPED m_overlapped[m_oCount];
70  XsByteArray m_varBuffer;
71  static const int m_fixedBufferSize = 8192;
72  static const int m_fastPolicyThreshold = 3;
73  uint8_t m_fixedBuffer[m_oCount][m_fixedBufferSize];
74  // int m_offset;
75  CRITICAL_SECTION m_mutex;
76  HANDLE m_quitEvent;
77  volatile bool m_running;
78  HANDLE m_waitEvents[m_oCount];
79  int m_readIdx;
80 
81  void threadFunc();
82 
83 #else
84  libusb_device_handle* m_deviceHandle;
85  /*! \brief A context manager for libusb
86 
87  For predictable operation with libusb, it is recommended to use at least
88  one context per library.
89  */
90  class UsbContext
91  {
92  public:
93  /*! \brief Create the USB context
94  */
96  {
98  // libusb_set_debug(m_usbContext, 3);
99  }
100 
101  /*! \brief Destroy the USB context */
103  libusb_context* m_usbContext; // needed for proper use of libusb
105  };
106 
107  // JLBC for MRPT: Avoid crashes in apps exit, even when XSens code is not
108  // called:
109  // -> Converted into a singleton:
110  // Was: static UsbContext m_contextManager;
112  {
113  static UsbContext obj;
114  return obj;
115  }
116 
117  /*! \brief Map a libusb_error to XsResultValue
118 
119  \a param libusbError [in] the result code to convert
120  \a param hint give a hint for the code to return when in doubt
121  */
123  int libusbError, XsResultValue hint = XRV_ERROR)
124  {
125  switch (libusbError)
126  {
127  case LIBUSB_SUCCESS:
128  return XRV_OK;
129 
130  case LIBUSB_ERROR_IO:
131  return hint;
132 
133  case LIBUSB_ERROR_INVALID_PARAM:
134  return XRV_INVALIDPARAM;
135 
136  case LIBUSB_ERROR_ACCESS:
137  return hint;
138 
139  case LIBUSB_ERROR_NO_DEVICE:
140  return XRV_NOPORTOPEN;
141 
142  case LIBUSB_ERROR_NOT_FOUND:
143  return XRV_NOTFOUND;
144 
145  case LIBUSB_ERROR_BUSY:
146  return XRV_INVALIDOPERATION;
147 
148  case LIBUSB_ERROR_TIMEOUT:
149  return XRV_TIMEOUT;
150 
151  case LIBUSB_ERROR_OVERFLOW:
152  return hint;
153 
154  case LIBUSB_ERROR_PIPE:
155  return hint;
156 
157  case LIBUSB_ERROR_INTERRUPTED:
158  return XRV_UNEXPECTEDMSG;
159 
160  case LIBUSB_ERROR_NO_MEM:
161  return XRV_OUTOFMEMORY;
162 
163  case LIBUSB_ERROR_NOT_SUPPORTED:
164  return XRV_NOTIMPLEMENTED;
165 
166  case LIBUSB_ERROR_OTHER:
167  return hint;
168  }
169  return hint;
170  }
171 
172  /*! \brief Convert a libusb error to a human-readable string */
173  const char* libusbErrorToString(int libusbError)
174  {
175  switch (libusbError)
176  {
177  case LIBUSB_SUCCESS:
178  return "LIBUSB_SUCCESS";
179 
180  case LIBUSB_ERROR_IO:
181  return "LIBUSB_ERROR_IO";
182 
183  case LIBUSB_ERROR_INVALID_PARAM:
184  return "LIBUSB_ERROR_INVALID_PARAM";
185 
186  case LIBUSB_ERROR_ACCESS:
187  return "LIBUSB_ERROR_ACCESS";
188 
189  case LIBUSB_ERROR_NO_DEVICE:
190  return "LIBUSB_ERROR_NO_DEVICE";
191 
192  case LIBUSB_ERROR_NOT_FOUND:
193  return "LIBUSB_ERROR_NOT_FOUND";
194 
195  case LIBUSB_ERROR_BUSY:
196  return "LIBUSB_ERROR_BUSY";
197 
198  case LIBUSB_ERROR_TIMEOUT:
199  return "LIBUSB_ERROR_TIMEOUT";
200 
201  case LIBUSB_ERROR_OVERFLOW:
202  return "LIBUSB_ERROR_OVERFLOW";
203 
204  case LIBUSB_ERROR_PIPE:
205  return "LIBUSB_ERROR_PIPE";
206 
207  case LIBUSB_ERROR_INTERRUPTED:
208  return "LIBUSB_ERROR_INTERRUPTED";
209 
210  case LIBUSB_ERROR_NO_MEM:
211  return "LIBUSB_ERROR_NO_MEM";
212 
213  case LIBUSB_ERROR_NOT_SUPPORTED:
214  return "LIBUSB_ERROR_NOT_SUPPORTED";
215 
216  case LIBUSB_ERROR_OTHER:
217  return "LIBUSB_ERROR_OTHER";
218  }
219  return "Unknown";
220  }
221 #endif
222 };
223 
224 #ifdef USE_WINUSB
225 DWORD usbReadThreadFunc(void* obj)
226 {
228  d->m_running = true;
231  xsNameThisThread("USB reader");
232  try
233  {
234  d->threadFunc();
235  d->m_running = false;
236  }
237  catch (...)
238  {
239  xsNameThisThread("Crashed USB reader");
240  d->m_running = false;
241  XsTime::msleep(10000);
242  }
243  return 0;
244 }
245 
246 void UsbInterfacePrivate::threadFunc()
247 {
248  HANDLE handles[1 + m_oCount];
249  handles[0] = m_quitEvent;
250  handles[m_oCount] = m_waitEvents[m_oCount - 1];
251  //= { m_quitEvent, m_waitEvents[0], m_waitEvents[1] };
252 
253  // start first read operation
254  for (m_readIdx = 0; m_readIdx < (m_oCount - 1); ++m_readIdx)
255  {
256  handles[m_readIdx + 1] = m_waitEvents[m_readIdx];
257  // m_readIdx = 0;
258  m_overlapped[m_readIdx] = OVERLAPPED();
259  ::ResetEvent(m_waitEvents[m_readIdx]); // lint !e534
260  m_overlapped[m_readIdx].hEvent = m_waitEvents[m_readIdx];
261  m_winUsb.ReadPipe(
262  m_usbHandle[1], m_bulkInPipe, m_fixedBuffer[m_readIdx],
263  (ULONG)m_fixedBufferSize, 0,
264  &m_overlapped[m_readIdx]); // lint !e534
265  }
266  int fastCount = 0;
267  // m_readIdx = 1;
268  bool policyFast = false;
269  bool run = true;
270  while (run)
271  {
272  // start follow-up read operation
273  m_overlapped[m_readIdx] = OVERLAPPED();
274  ::ResetEvent(m_waitEvents[m_readIdx]); // lint !e534
275  m_overlapped[m_readIdx].hEvent = m_waitEvents[m_readIdx];
276  m_winUsb.ReadPipe(
277  m_usbHandle[1], m_bulkInPipe, m_fixedBuffer[m_readIdx],
278  (ULONG)m_fixedBufferSize, 0,
279  &m_overlapped[m_readIdx]); // lint !e534
280  m_readIdx = (m_readIdx + 1) % m_oCount;
281  int64_t tBegin = XsTime_timeStampNow(0);
282  DWORD waitResult =
283  ::WaitForMultipleObjects(1 + m_oCount, handles, FALSE, INFINITE);
284 #if 0 // not sure if this causes problems, but it should help in catching up
285  int64_t tEnd = XsTime_timeStampNow(0);
286  switch (tEnd - tBegin)
287  {
288  case 0:
289  if (++fastCount > m_fastPolicyThreshold && !policyFast)
290  {
291  policyFast = true;
292  // set fast policy
293  UCHAR enable = TRUE;
294  m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
295  }
296  break;
297 
298  case 1:
299  if (fastCount)
300  --fastCount;
301  if (policyFast && fastCount <= m_fastPolicyThreshold)
302  {
303  // reset policy
304  policyFast = false;
305  UCHAR enable = FALSE;
306  m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
307  }
308  break;
309 
310  default:
311  fastCount = 0;
312  if (policyFast)
313  {
314  // reset policy
315  policyFast = false;
316  UCHAR enable = FALSE;
317  m_winUsb.SetPipePolicy(m_usbHandle[1], m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR), &enable); //lint !e534
318  }
319  break;
320  }
321 #endif
322 
323  // handle data
324  switch (waitResult)
325  {
326  case WAIT_TIMEOUT:
327  case WAIT_FAILED:
328  case WAIT_OBJECT_0:
329  run = false;
330  break;
331 
332  default:
333  if (waitResult >= WAIT_ABANDONED_0)
334  {
335  JLDEBUG(
336  gJournal,
337  "WFMO abandoned: " << (waitResult - WAIT_OBJECT_0));
338  break;
339  }
340 
341 #ifndef XSENS_RELEASE
342  JLDEBUG(
343  gJournal, "WFMO trigger: " << (waitResult - WAIT_OBJECT_0));
344 #endif
345  {
346  // put data into buffer
347  int idx = m_readIdx;
348  DWORD dataRead = 0;
349  if (!m_winUsb.GetOverlappedResult(
350  m_usbHandle[0], &m_overlapped[idx], &dataRead,
351  FALSE))
352  {
353  // error
354  DWORD err = ::GetLastError();
355  switch (err)
356  {
357  case ERROR_SEM_TIMEOUT:
358  case ERROR_IO_INCOMPLETE:
359  // JLDEBUG(gJournal,
360  // "m_winUsb.GetOverlappedResult resulted in
361  // acceptable windows error " << err);
362  break;
363 
364  default:
365  JLALERT(
366  gJournal,
367  "m_winUsb.GetOverlappedResult resulted in "
368  "windows error "
369  << err);
370  run = false;
371  break;
372  }
373  // assert (err == ERROR_IO_INCOMPLETE);
374  }
375  else
376  {
377  // append unread data to var buffer
378  JLTRACE(
379  gJournal,
380  "m_winUsb.GetOverlappedResult resulted in "
381  << dataRead << " bytes being read");
383  &m_fixedBuffer[idx][0], dataRead, XSDF_None);
384  ::EnterCriticalSection(&m_mutex);
385  m_varBuffer.append(ref);
386  ::LeaveCriticalSection(&m_mutex);
387  }
388  }
389  break;
390  }
391  }
392 }
393 
394 #else
395 // UsbInterfacePrivate::UsbContext UsbInterfacePrivate::m_contextManager; //
396 // JLBC for MRPT: Removed due to factoring as singleton
397 // => UsbInterfacePrivate::getContextManager()
398 #endif
399 
400 /*! \class UsbInterface
401  \brief An IoInterface for dealing specifically with Xsens USB devices
402 */
403 
404 /*! \brief Default constructor, initializes all members to their default values.
405 */
407 {
408  d->m_lastResult = XRV_OK;
409  d->m_timeout = 20;
410  d->m_endTime = 0;
411  d->m_dataInEndPoint = -1;
412  d->m_dataOutEndPoint = -1;
413  d->m_deviceHandle = 0;
414  d->m_portname[0] = 0;
415 #ifdef LOG_RX_TX
416  d->rx_log = nullptr;
417  d->tx_log = nullptr;
418 #endif
419 #ifdef USE_WINUSB
420  d->m_threadHandle = INVALID_HANDLE_VALUE;
421  ::InitializeCriticalSection(&d->m_mutex);
422  d->m_usbHandle[0] = 0;
423  d->m_usbHandle[1] = 0;
424  for (int i = 0; i < d->m_oCount; ++i)
425  {
426  d->m_waitEvents[i] = ::CreateEvent(nullptr, TRUE, FALSE, nullptr);
427  ::ResetEvent(d->m_waitEvents[i]);
428  }
429  d->m_quitEvent = ::CreateEvent(nullptr, TRUE, FALSE, nullptr);
430  ::ResetEvent(d->m_quitEvent);
431  d->m_readIdx = 0;
432 // d->m_offset = 0;
433 #endif
434 }
435 
436 //! Destructor, de-initializes, frees memory allocated for buffers, etc.
438 {
439  try
440  {
441  closeUsb(); // lint !e534
442 #ifdef USE_WINUSB
443  ::CloseHandle(d->m_quitEvent); // lint !e534
444  for (int i = 0; i < d->m_oCount; ++i)
445  ::CloseHandle(d->m_waitEvents[i]); // lint !e534
446  ::DeleteCriticalSection(&d->m_mutex);
447 #endif
448  delete d;
449  }
450  catch (...)
451  {
452  }
453 }
454 
455 /*! \brief Close the USB communication port.
456 */
458 /*! \brief Close the USB communication port.
459  \returns XRV_OK if the port was closed successfully
460  \note Linux:\n
461  If a kernel driver was detached when communication with the device started,
462  attach it again. No guarantee is given that udev will pick up on it though.
463 */
465 {
466 // lint --e{534}
467 #ifdef LOG_RX_TX
468  if (d->rx_log != nullptr) fclose(d->rx_log);
469  if (d->tx_log != nullptr) fclose(d->tx_log);
470  d->rx_log = nullptr;
471  d->tx_log = nullptr;
472 #endif
473  if (!isOpen()) return d->m_lastResult = XRV_NOPORTOPEN;
474 
475  d->m_lastResult = XRV_OK;
476 #ifdef USE_WINUSB
477  if (d->m_threadHandle != INVALID_HANDLE_VALUE)
478  {
479  while (d->m_running)
480  {
481  ::SetEvent(d->m_quitEvent);
482  XsTime::msleep(10);
483  }
484  ::CloseHandle(d->m_threadHandle);
485  }
486 
487  flushData();
488  if (d->m_usbHandle[0])
489  {
490  d->m_winUsb.Free(d->m_usbHandle[0]);
491  d->m_usbHandle[0] = nullptr;
492  }
493  if (d->m_usbHandle[1])
494  {
495  d->m_winUsb.Free(d->m_usbHandle[1]);
496  d->m_usbHandle[1] = nullptr;
497  }
498  if (d->m_deviceHandle)
499  {
500  CloseHandle(d->m_deviceHandle);
501  d->m_deviceHandle = nullptr;
502  }
503 #else
504  flushData();
505  libusb_device* dev =
507  d->m_deviceHandle);
508  for (int i = 0; i < d->m_interfaceCount; i++)
509  {
510  int result = LIBUSB_ERROR_OTHER;
511  while (result != LIBUSB_SUCCESS)
512  {
515  if (result == LIBUSB_SUCCESS)
516  {
519  }
520  }
521  }
522 
524  d->m_deviceHandle = nullptr;
525 
527  d->m_interface = -1;
528  d->m_dataInEndPoint = -1;
529  d->m_dataOutEndPoint = -1;
530 #endif
531  d->m_endTime = 0;
532 
533  return d->m_lastResult;
534 }
535 
536 //////////////////////////////////////////////////////////////////////////////////////////
537 // Flush all data to be transmitted / received.
539 {
540  // lint --e{534}
541  d->m_lastResult = XRV_OK;
542 #ifdef USE_WINUSB
543  if (d->m_usbHandle[0])
544  {
545  d->m_winUsb.AbortPipe(d->m_usbHandle[0], d->m_bulkInPipe);
546  d->m_winUsb.FlushPipe(d->m_usbHandle[0], d->m_bulkInPipe);
547  d->m_winUsb.AbortPipe(d->m_usbHandle[0], d->m_bulkOutPipe);
548  d->m_winUsb.FlushPipe(d->m_usbHandle[0], d->m_bulkOutPipe);
549  }
550  if (d->m_usbHandle[1])
551  {
552  d->m_winUsb.AbortPipe(d->m_usbHandle[1], d->m_bulkInPipe);
553  d->m_winUsb.FlushPipe(d->m_usbHandle[1], d->m_bulkInPipe);
554  d->m_winUsb.AbortPipe(d->m_usbHandle[1], d->m_bulkOutPipe);
555  d->m_winUsb.FlushPipe(d->m_usbHandle[1], d->m_bulkOutPipe);
556  }
557 #else
558  unsigned char flushBuffer[256];
559  int actual;
560  for (int i = 0; i < 64; ++i)
561  {
562  if (UsbInterfacePrivate::getContextManager().m_libUsb.bulk_transfer(
563  d->m_deviceHandle, d->m_dataInEndPoint | LIBUSB_ENDPOINT_IN,
564  flushBuffer, sizeof(flushBuffer), &actual, 1) != LIBUSB_SUCCESS)
565  break;
566  if (actual == 0) break;
567  }
568 #endif
569  d->m_endTime = 0;
570  return d->m_lastResult;
571 }
572 
573 //! Return the error code of the last operation.
575 {
576  return d->m_lastResult;
577 }
578 
579 //! Return the current timeout value
581 //! Return whether the USB communication port is open or not.
582 bool UsbInterface::isOpen(void) const { return d->m_deviceHandle != nullptr; }
583 /*! \brief Open a communication channel to the given USB port name. */
585 {
586  d->m_endTime = 0;
587 
588 #ifdef USE_WINUSB
589  JLDEBUG(gJournal, "Open usb port " << portInfo.portName().toStdString());
590 #else
591  JLDEBUG(
592  gJournal, "Open usb port " << portInfo.usbBus() << ":"
593  << portInfo.usbAddress());
594 #endif
595 
596  if (isOpen())
597  {
598  JLALERT(
599  gJournal, "Port " << portInfo.portName().toStdString()
600  << " already open");
601  return (d->m_lastResult = XRV_ALREADYOPEN);
602  }
603 
604 #ifdef USE_WINUSB
605  d->m_deviceHandle = CreateFileA(
606  portInfo.portName().c_str(), GENERIC_WRITE | GENERIC_READ,
607  FILE_SHARE_WRITE | FILE_SHARE_READ, nullptr, OPEN_EXISTING,
608  FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, nullptr);
609 
610  if (d->m_deviceHandle == INVALID_HANDLE_VALUE)
611  {
612  d->m_deviceHandle = nullptr;
613  return (d->m_lastResult = XRV_PORTNOTFOUND);
614  }
615 
616  BOOL result = FALSE;
617  UCHAR speed = 0;
618  ULONG length = 0;
619  USB_INTERFACE_DESCRIPTOR interfaceDescriptor = {0, 0, 0, 0, 0, 0, 0, 0, 0};
620  WINUSB_PIPE_INFORMATION pipeInfo;
621 
622  result = d->m_winUsb.Initialize(d->m_deviceHandle, &d->m_usbHandle[0]);
623  if (result)
624  {
625  result = d->m_winUsb.GetAssociatedInterface(
626  d->m_usbHandle[0], 0, &d->m_usbHandle[1]);
627  }
628  else
629  {
630 #ifdef XSENS_DEBUG
631  DWORD err = GetLastError();
632  assert(result);
633 #endif
634  return (d->m_lastResult = XRV_ERROR);
635  }
636 
637  for (int k = 0; k < 2; k++)
638  {
639  if (result)
640  {
641  assert(d->m_usbHandle[k] != 0);
642  length = sizeof(UCHAR);
643  result = d->m_winUsb.QueryDeviceInformation(
644  d->m_usbHandle[k], DEVICE_SPEED, &length, &speed);
645  }
646 
647  if (result)
648  {
649  d->m_deviceSpeed = speed;
650  result = d->m_winUsb.QueryInterfaceSettings(
651  d->m_usbHandle[k], 0, &interfaceDescriptor);
652  }
653  if (result)
654  {
655  for (int i = 0; i < interfaceDescriptor.bNumEndpoints; i++)
656  {
657  result = d->m_winUsb.QueryPipe(
658  d->m_usbHandle[k], 0, (UCHAR)i, &pipeInfo);
659 
660  if (pipeInfo.PipeType == UsbdPipeTypeBulk &&
661  USB_ENDPOINT_DIRECTION_IN(pipeInfo.PipeId))
662  {
663  d->m_bulkInPipe = pipeInfo.PipeId;
664  d->m_bulkInPipePacketSize = pipeInfo.MaximumPacketSize;
665  }
666  else if (
667  pipeInfo.PipeType == UsbdPipeTypeBulk &&
668  USB_ENDPOINT_DIRECTION_OUT(pipeInfo.PipeId))
669  {
670  d->m_bulkOutPipe = pipeInfo.PipeId;
671  }
672  else if (pipeInfo.PipeType == UsbdPipeTypeInterrupt)
673  {
674  d->m_interruptPipe = pipeInfo.PipeId;
675  }
676  else
677  {
678  result = FALSE;
679  break;
680  }
681  }
682  }
683  }
684 
685  setTimeout(0); // lint !e534
686  flushData(); // lint !e534
687 
688  sprintf(d->m_portname, "%s", portInfo.portName().c_str());
689 
690  // d->m_offset = 0;
691  ::ResetEvent(&d->m_quitEvent); // lint !e534
692  d->m_threadHandle = xsStartThread(usbReadThreadFunc, d, &d->m_threadId);
693  if (d->m_threadHandle == XSENS_INVALID_THREAD)
694  {
695 #ifdef XSENS_DEBUG
696  assert(0);
697 #endif
698  return (d->m_lastResult = XRV_ERROR);
699  }
700 
701 #else // !USE_WINUSB
702  libusb_device** deviceList;
703  ssize_t listLength =
705  UsbInterfacePrivate::getContextManager().m_usbContext, &deviceList);
706  if (listLength < 0)
707  return d->m_lastResult = d->libusbErrorToXrv((int)listLength);
708 
709  // "USBxxx:yyy"
710  uint8_t bus = XsPortInfo_usbBus(&portInfo);
712 
713  XsResultValue xrv = XRV_OK;
714  int result;
715  libusb_device* device = nullptr;
716  for (int i = 0; i < listLength && device == nullptr; ++i)
717  {
718  libusb_device* dev = deviceList[i];
719  if (UsbInterfacePrivate::getContextManager().m_libUsb.get_bus_number(
720  dev) != bus ||
723  continue;
724 
725  libusb_device_descriptor desc;
727  .m_libUsb.get_device_descriptor(dev, &desc);
728  if (result != LIBUSB_SUCCESS) break;
729 
730  libusb_config_descriptor* configDesc;
732  .m_libUsb.get_active_config_descriptor(dev, &configDesc);
733  if (result != LIBUSB_SUCCESS) break;
734 
735  d->m_interface = -1;
736  d->m_interfaceCount = configDesc->bNumInterfaces;
737  // find the bulk transfer endpoints
738  for (uint8_t ifCount = 0;
739  ifCount < configDesc->bNumInterfaces && d->m_interface == -1;
740  ++ifCount)
741  {
742  for (uint8_t altsettingCount = 0;
743  altsettingCount <
744  configDesc->interface[ifCount].num_altsetting;
745  altsettingCount++)
746  {
747  const libusb_endpoint_descriptor* endpoints =
748  configDesc->interface[ifCount]
749  .altsetting[altsettingCount]
750  .endpoint;
751  int inEndpoint = -1, outEndpoint = -1;
752  for (uint8_t i = 0; i < configDesc->interface[ifCount]
753  .altsetting[altsettingCount]
754  .bNumEndpoints;
755  i++)
756  {
757  if ((endpoints[i].bmAttributes &
758  LIBUSB_TRANSFER_TYPE_MASK) !=
759  LIBUSB_TRANSFER_TYPE_BULK)
760  continue;
761 
762  switch (endpoints[i].bEndpointAddress &
763  LIBUSB_ENDPOINT_DIR_MASK)
764  {
765  case LIBUSB_ENDPOINT_IN:
766  inEndpoint = endpoints[i].bEndpointAddress &
767  LIBUSB_ENDPOINT_ADDRESS_MASK;
768  break;
769 
770  case LIBUSB_ENDPOINT_OUT:
771  outEndpoint = endpoints[i].bEndpointAddress &
772  LIBUSB_ENDPOINT_ADDRESS_MASK;
773  break;
774  }
775  }
776 
777  if (outEndpoint == -1 || inEndpoint == -1) continue;
778 
779  d->m_interface = ifCount;
780  d->m_dataOutEndPoint = outEndpoint;
781  d->m_dataInEndPoint = inEndpoint;
782  }
783  }
784  if (d->m_interface == -1)
785  {
787  break;
788  }
789 
791  .m_libUsb.free_config_descriptor(configDesc);
793  device = dev;
794  result = LIBUSB_SUCCESS;
795  }
796 
798  deviceList, 1);
799  if (result != LIBUSB_SUCCESS)
800  {
802  return d->m_lastResult = d->libusbErrorToXrv(result);
803  }
804 
805  if (xrv != XRV_OK)
806  {
808  return d->m_lastResult = xrv;
809  }
810 
811  libusb_device_handle* handle;
812  result =
814  if (result != LIBUSB_SUCCESS)
815  {
817  return d->m_lastResult = d->libusbErrorToXrv(result);
818  }
819 
820  // be rude and claim all interfaces
821  for (int i = 0; i < d->m_interfaceCount; i++)
822  {
824  .m_libUsb.kernel_driver_active(handle, i);
825  if (result > 0)
827  .m_libUsb.detach_kernel_driver(handle, i);
828  if (result == LIBUSB_SUCCESS)
830  .m_libUsb.claim_interface(handle, i);
831  if (result != LIBUSB_SUCCESS)
832  {
833  for (int j = 0; j < i; j++)
834  {
835  while (result != LIBUSB_SUCCESS)
836  {
838  .m_libUsb.release_interface(handle, j);
840  .m_libUsb.attach_kernel_driver(handle, j);
841  }
842  }
843 
846  device);
847  return d->m_lastResult = d->libusbErrorToXrv(result);
848  }
849  }
850 
851  d->m_deviceHandle = handle;
852  sprintf(d->m_portname, "%s", portInfo.portName().c_str());
853 
854  flushData();
855 
856 #endif // !USE_WINUSB
857  JLDEBUG(gJournal, "USB Port opened");
858  return (d->m_lastResult = XRV_OK);
859 }
860 
861 /*! \brief Read data from the USB port and put it into the data buffer.
862  \details This function reads up to \a maxLength bytes from the port
863  (non-blocking) and
864  puts it into the \a data buffer.
865  \param maxLength The maximum amount of data read.
866  \param data The buffer that will store the received data.
867  \returns XRV_OK if no error occurred. It can be that no data is available
868  and XRV_OK will be
869  returned. Check data.size() for the number of bytes that were read.
870 */
872 {
873  XsSize length = 0;
874  data.setSize(maxLength);
876  data.pop_back(maxLength - length);
877  return res;
878 }
879 
880 /*! \brief Read data from the serial port and put it into the data buffer.
881  \details This function reads up to \a maxLength bytes from the USB port
882  (non-blocking) and
883  puts it into the \a data buffer.
884  \param maxLength The maximum number of bytes to read.
885  \param data Pointer to a buffer that will store the received data.
886  \param length The number of bytes placed into \c data.
887  \returns XRV_OK if no error occurred. It can be that no data is available
888  and XRV_OK will be
889  returned. Check *length for the number of bytes that were read.
890 */
892  const XsSize maxLength, void* data, XsSize* length)
893 {
894  JLTRACE(
895  gJournal, "maxLength=" << maxLength << ", data=0x" << data
896  << ", length=0x" << length);
897  XsSize ln;
898  if (length == nullptr) length = &ln;
899 
900  if (!isOpen()) return (d->m_lastResult = XRV_NOPORTOPEN);
901 
902 #ifdef USE_WINUSB
903  XsSize remaining = 0;
904  ::EnterCriticalSection(&d->m_mutex);
905  remaining = *length = d->m_varBuffer.size();
906  if (*length > maxLength) *length = maxLength;
907  if (*length)
908  {
909  memcpy(data, d->m_varBuffer.data(), *length);
910  d->m_varBuffer.erase(0, *length);
911  remaining = d->m_varBuffer.size();
912  }
913  ::LeaveCriticalSection(&d->m_mutex);
914  JLTRACE(
915  gJournal, "returned success, read "
916  << *length << " of " << maxLength
917  << " bytes, first: " << JLHEXLOG(((char*)data)[0]) << ", "
918  << remaining << " remaining in buffer");
919 #else
920  int actual = 0;
921  JLTRACE(gJournal, "starting bulk read, timeout = " << d->m_timeout);
923  d->m_deviceHandle, (d->m_dataInEndPoint | LIBUSB_ENDPOINT_IN),
924  (unsigned char*)data, maxLength, &actual, d->m_timeout);
925  JLTRACE(
926  gJournal, "bulk read returned: " << d->libusbErrorToString(res) << ". "
927  << actual << " bytes received");
928  if ((res != LIBUSB_SUCCESS && res != LIBUSB_ERROR_TIMEOUT) ||
929  (res == LIBUSB_ERROR_TIMEOUT && actual <= 0))
930  return d->m_lastResult = d->libusbErrorToXrv(res);
931 
932  *length = actual;
933 #endif
934 
935 #ifdef LOG_RX_TX
936  if (*length > 0)
937  {
938  if (d->rx_log == nullptr)
939  {
940  char fname[XS_MAX_FILENAME_LENGTH];
941  sprintf(fname, "rx_USB%03u_%03u.log", usbBus(), usbAddress());
942  d->rx_log = fopen(fname, "wb");
943  }
944  fwrite(data, 1, *length, d->rx_log);
945 #ifdef LOG_RX_TX_FLUSH
946  fflush(d->rx_log);
947 #endif
948  }
949 #endif
950 
951  return (d->m_lastResult = XRV_OK);
952 }
953 
954 /*! \brief Set the default timeout value to use in blocking operations.
955  \details This function sets the value of m_timeout. There is no infinity
956  value. The value 0
957  means that the default value is used.
958  \param ms The desired timeout in milliseconds
959  \returns XRV_OK if the timeout value was successfully updated
960 */
962 {
963 #ifdef USE_WINUSB
964  JLDEBUG(
965  gJournal, "Request to set timeout to "
966  << ms << " ms overridden, setting to 0 ms instead");
967  ms = 0; // no timeout ever
968  UCHAR enable = FALSE;
969 
970  d->m_winUsb.SetPipePolicy(
971  d->m_usbHandle[1], d->m_bulkInPipe, IGNORE_SHORT_PACKETS, sizeof(UCHAR),
972  &enable); // lint !e534
973  d->m_winUsb.SetPipePolicy(
974  d->m_usbHandle[1], d->m_bulkInPipe, PIPE_TRANSFER_TIMEOUT,
975  sizeof(ULONG), &ms); // lint !e534
976 
977  d->m_timeout = ms;
978 #else
979  JLDEBUG(gJournal, "Setting timeout to " << ms);
980  if (ms == 0)
981  d->m_timeout = 1;
982  else
983  d->m_timeout = ms;
984 #endif
985  return (d->m_lastResult = XRV_OK);
986 }
987 
988 /*! \brief Sets the RAWIO mode of the USB interface
989  \note Only applies to WinUSB implementations
990  \param enable : If true will enable RAW IO mode
991 */
992 void UsbInterface::setRawIo(bool enable)
993 {
994  JLDEBUG(gJournal, "Setting RAWIO mode to " << enable);
995 
996 #ifdef USE_WINUSB
997  enable = false; // never use raw IO
998  UCHAR rawIo = (UCHAR)enable;
999  d->m_winUsb.SetPipePolicy(
1000  d->m_usbHandle[1], d->m_bulkInPipe, RAW_IO, sizeof(UCHAR),
1001  &rawIo); // lint !e534
1002 #else
1003  (void)enable;
1004 #endif
1005  d->m_lastResult = XRV_OK;
1006 }
1007 
1008 /*! \brief Retrieves the state of the RAWIO mode of the USB interface
1009  \returns true if raw IO mode is enabled
1010  \note Only applies to WinUSB implementations
1011 */
1013 {
1014 #ifdef USE_WINUSB
1015  UCHAR rawIo = 0;
1016  ULONG someSize = sizeof(UCHAR);
1017  d->m_winUsb.GetPipePolicy(
1018  d->m_usbHandle[1], d->m_bulkInPipe, RAW_IO, &someSize,
1019  &rawIo); // lint !e534
1020  return (rawIo != 0);
1021 #else
1022  return false;
1023 #endif
1024 }
1025 
1026 /*! \brief Wait for data to arrive or a timeout to occur.
1027  \details The function waits until \c maxLength data is available or until a
1028  timeout occurs.
1029  The function returns success if data is available or XsResultValue::TIMEOUT
1030  if a
1031  timeout occurred. A timeout value of 0 indicates that the default timeout
1032  stored
1033  in the class should be used.
1034  \param maxLength The maximum number of bytes to wait for
1035  \param data A buffer that will be filled with the read data. It must be able
1036  to contain at
1037  least \a maxLength bytes.
1038  \param length An optional pointer to storage for the actual number of bytes
1039  read.
1040  \returns XRV_OK if the requested data was read
1041 */
1043  const XsSize maxLength, void* data, XsSize* length)
1044 {
1045  JLTRACE(
1046  gJournal, "timeout=" << d->m_timeout << ", data=" << data
1047  << ", length=" << length);
1048  uint32_t timeout = d->m_timeout;
1049 
1050  char* bdata = (char*)data;
1051 
1052  XsSize ln;
1053  if (length == nullptr) length = &ln;
1054  uint32_t eTime = XsTime::getTimeOfDay() + timeout;
1055  XsSize newLength = 0;
1056 
1057  *length = 0;
1058  while ((*length < maxLength) && (XsTime::getTimeOfDay() <= eTime))
1059  {
1060  if (readData(maxLength - *length, bdata + *length, &newLength))
1061  return d->m_lastResult;
1062  *length += newLength;
1063  }
1064  JLTRACE(gJournal, "read " << length[0] << " of " << maxLength << " bytes");
1065 
1066  if (length[0] < maxLength)
1067  return (d->m_lastResult = XRV_TIMEOUT);
1068  else
1069  return (d->m_lastResult = XRV_OK);
1070 }
1071 
1072 /*! \brief Write the data to the USB port.
1073  \details The function writes the given data to the connected USB port.
1074  The default timeout is respected in this operation.
1075  \param data The data to be written
1076  \param written An optional pointer to storage for the actual number of bytes
1077  that were written
1078  \returns XRV_OK if the data was successfully written
1079  \sa writeData(const XsSize, const void *, XsSize*)
1080 */
1082 {
1083  return writeData(data.size(), data.data(), written);
1084 }
1085 
1086 /*! \brief Write the data to the USB port.
1087  \details The function writes the given data to the connected USB port.
1088  The default timeout is respected in this operation.
1089  \param length The number of bytes to write.
1090  \param data A pointer to a memory buffer that contains the bytes to send
1091  \param written An optional pointer to storage for the actual number of bytes
1092  that were written
1093  \returns XRV_OK if the data was successfully written
1094  \sa writeData(const XsByteArray&, XsSize*)
1095 */
1097  const XsSize length, const void* data, XsSize* written)
1098 {
1099  XsSize bytes;
1100  if (written == nullptr) written = &bytes;
1101 
1102  if (!isOpen()) return (d->m_lastResult = XRV_NOPORTOPEN);
1103 
1104  *written = 0;
1105 
1106 #ifdef USE_WINUSB
1107  ULONG dataWritten;
1108  d->m_winUsb.WritePipe(
1109  d->m_usbHandle[1], d->m_bulkOutPipe, (uint8_t*)data, (ULONG)length,
1110  &dataWritten,
1111  nullptr); // lint !e534
1112 
1113  *written = dataWritten;
1114 #else
1115  *written = 0;
1116  while (*written < length)
1117  {
1118  int actual;
1119  int result =
1121  d->m_deviceHandle, (d->m_dataOutEndPoint | LIBUSB_ENDPOINT_OUT),
1122  (unsigned char*)data, length, &actual, 0);
1123  *written += actual;
1124  if (result != LIBUSB_SUCCESS)
1125  {
1126  JLALERT(
1127  gJournal,
1128  "bulk write failed: " << d->libusbErrorToString(result) << ". "
1129  << actual << " bytes sent");
1130  return (d->m_lastResult = d->libusbErrorToXrv(result));
1131  }
1132  }
1133 
1134 #endif
1135 
1136 #ifdef LOG_RX_TX
1137  if (*written > 0)
1138  {
1139  if (d->tx_log == nullptr)
1140  {
1141  char fname[XS_MAX_FILENAME_LENGTH];
1142  sprintf(fname, "tx_USB%03u_%03u.log", usbBus(), usbAddress());
1143  d->tx_log = fopen(fname, "wb");
1144  }
1145  fwrite(data, 1, *written, d->tx_log);
1146 #ifdef LOG_RX_TX_FLUSH
1147  fflush(d->tx_log);
1148 #endif
1149  }
1150 #endif
1151 
1152  return (d->m_lastResult = XRV_OK);
1153 }
1154 
1155 /*! \brief The USB bus number this device is on (libusb/linux only) */
1157 {
1158 #ifdef USE_WINUSB
1159  return 0;
1160 #else
1161  if (!d->m_deviceHandle) return 0;
1162 
1163  libusb_device* dev =
1165  d->m_deviceHandle);
1167  dev);
1168 #endif
1169 }
1170 
1171 /*! \brief The address of the device (libusb/linux only) */
1173 {
1174 #ifdef USE_WINUSB
1175  return 0;
1176 #else
1177  if (!d->m_deviceHandle) return 0;
1178 
1179  libusb_device* dev =
1181  d->m_deviceHandle);
1183  dev);
1184 #endif
1185 }
1186 
1187 //! Retrieve the port name that was last successfully opened.
1189 {
1190  portname = d->m_portname;
1191 }
libUSB_attach_kernel_driver attach_kernel_driver
Definition: xslibusb.h:63
Operation was performed successfully.
Definition: xsens_std.h:34
#define xsStartThread(func, param, pid)
Start a function as a thread.
Definition: xsthread.h:99
XSTYPES_DLL_API int XsPortInfo_usbAddress(XsPortInfo const *thisPtr)
#define XS_MAX_FILENAME_LENGTH
Definition: iointerface.h:30
uint8_t usbAddress() const
The address of the device (libusb/linux only)
unsigned __int16 uint16_t
Definition: rptypes.h:44
libUSB_close close
Definition: xslibusb.h:61
UsbInterface()
Default constructor, initializes all members to their default values.
libUSB_unref_device unref_device
Definition: xslibusb.h:66
uint32_t getTimeout(void) const
Return the current timeout value.
UsbInterfacePrivate * d
Definition: usbinterface.h:78
bool isOpen(void) const
Return whether the USB communication port is open or not.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
libUSB_open open
Definition: xslibusb.h:60
GLenum GLint ref
Definition: glext.h:4050
HANDLE XsIoHandle
Definition: xsfilepos.h:38
libUSB_init init
Definition: xslibusb.h:58
struct XsByteArray XsByteArray
Definition: xsbytearray.h:25
libUSB_free_config_descriptor free_config_descriptor
Definition: xslibusb.h:70
UsbContext()
Create the USB context.
const char * libusbErrorToString(int libusbError)
Convert a libusb error to a human-readable string.
size_t XsSize
XsSize must be unsigned number!
Definition: xstypedefs.h:19
int BOOL
Definition: xstypedefs.h:76
No flag set.
Definition: xstypedefs.h:44
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
XsResultValue libusbErrorToXrv(int libusbError, XsResultValue hint=XRV_ERROR)
Map a libusb_error to XsResultValue.
unsigned char uint8_t
Definition: rptypes.h:41
libUSB_free_device_list free_device_list
Definition: xslibusb.h:76
void setRawIo(bool enable)
Sets the RAWIO mode of the USB interface.
XsResultValue m_lastResult
The last result of an operation.
libusb_device_handle * m_deviceHandle
XsResultValue
Xsens result values.
Definition: xsresultvalue.h:27
void msleep(uint32_t ms)
A platform-independent sleep routine.
Definition: xsens_time.cpp:82
__int64 int64_t
Definition: rptypes.h:49
uint32_t m_endTime
The time at which an operation will end in ms, used by several functions.
#define FALSE
Definition: jmorecfg.h:216
libUSB_get_device_list get_device_list
Definition: xslibusb.h:75
Class for dynamic loading of winusb.
bool getRawIo(void)
Retrieves the state of the RAWIO mode of the USB interface.
libUSB_claim_interface claim_interface
Definition: xslibusb.h:67
uint8_t usbBus() const
The USB bus number this device is on (libusb/linux only)
libUSB_get_device_descriptor get_device_descriptor
Definition: xslibusb.h:74
A context manager for libusb.
XsResultValue setTimeout(uint32_t ms)
Set the default timeout value to use in blocking operations.
XsResultValue closeUsb(void)
Close the USB communication port.
#define TRUE
Definition: jmorecfg.h:219
libUSB_get_device get_device
Definition: xslibusb.h:72
virtual XsResultValue writeData(const XsByteArray &data, XsSize *written=nullptr)
Write the data to the USB port.
HANDLE XsThread
A handle for a thread.
Definition: xsthread.h:91
#define xsGetCurrentThreadHandle()
Definition: xsthread.h:105
libUSB_kernel_driver_active kernel_driver_active
Definition: xslibusb.h:62
~UsbInterface()
Destructor, de-initializes, frees memory allocated for buffers, etc.
libUSB_exit exit
Definition: xslibusb.h:59
Private object for UsbInterface.
XsResultValue waitForData(XsSize maxLength, void *data, XsSize *length=nullptr)
Wait for data to arrive or a timeout to occur.
#define JLDEBUG(...)
XsResultValue getLastResult(void) const
Return the error code of the last operation.
libUSB_detach_kernel_driver detach_kernel_driver
Definition: xslibusb.h:64
DWORD XsThreadId
Definition: xsthread.h:95
#define xsSetThreadPriority(thrd, prio)
Definition: xsthread.h:108
void getPortName(XsString &portname) const
Retrieve the port name that was last successfully opened.
static UsbContext & getContextManager()
GLuint GLsizei GLsizei * length
Definition: glext.h:4064
XSTYPES_DLL_API int64_t XsTime_timeStampNow(XsTimeStamp *now)
FILE XsFileHandle
Definition: xsfilepos.h:51
GLuint address
Definition: glext.h:6947
#define XSENS_INVALID_THREAD
Definition: xsthread.h:77
XsResultValue flushData(void)
Flush all data in the buffers to and from the device.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:254
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
libUSB_get_device_address get_device_address
Definition: xslibusb.h:73
GLsizei maxLength
Definition: glext.h:4932
#define JLTRACE(...)
GLuint res
Definition: glext.h:7268
XsResultValue close(void)
Close the USB communication port.
struct XsString XsString
Definition: xsstring.h:34
virtual XsResultValue readData(XsSize maxLength, XsByteArray &data)
Read data from the USB port and put it into the data buffer.
unsigned __int32 uint32_t
Definition: rptypes.h:47
libUSB_release_interface release_interface
Definition: xslibusb.h:68
libUSB_get_bus_number get_bus_number
Definition: xslibusb.h:71
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
uint32_t getTimeOfDay(tm *date_, time_t *secs_)
A platform-independent clock.
Definition: xsens_time.cpp:34
libUSB_get_active_config_descriptor get_active_config_descriptor
Definition: xslibusb.h:69
#define JLALERT(...)
XsResultValue open(const XsPortInfo &portInfo, uint32_t readBufSize=0, uint32_t writeBufSize=0)
Open a communication channel to the given USB port name.
XSTYPES_DLL_API void xsNameThisThread(const char *threadName)
XSTYPES_DLL_API int XsPortInfo_usbBus(XsPortInfo const *thisPtr)
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:188
libUSB_bulk_transfer bulk_transfer
Definition: xslibusb.h:78
~UsbContext()
Destroy the USB context.
libUSB_ref_device ref_device
Definition: xslibusb.h:65
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:355



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019