stream.c
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (C) 2010-2012 Ken Tossell
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the author nor other contributors may be
18 * used to endorse or promote products derived from this software
19 * without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
39 #include "libuvc.h"
40 #include "libuvc_internal.h"
41 
42 #ifdef _MSC_VER
43 
44 #define DELTA_EPOCH_IN_MICROSECS 116444736000000000Ui64
45 
46 // gettimeofday - get time of day for Windows;
47 // A gettimeofday implementation for Microsoft Windows;
48 // Public domain code, author "ponnada";
49 int gettimeofday(struct timeval *tv, struct timezone *tz)
50 {
51  FILETIME ft;
52  unsigned __int64 tmpres = 0;
53  static int tzflag = 0;
54  if (NULL != tv)
55  {
56  GetSystemTimeAsFileTime(&ft);
57  tmpres |= ft.dwHighDateTime;
58  tmpres <<= 32;
59  tmpres |= ft.dwLowDateTime;
60  tmpres /= 10;
61  tmpres -= DELTA_EPOCH_IN_MICROSECS;
62  tv->tv_sec = (long)(tmpres / 1000000UL);
63  tv->tv_usec = (long)(tmpres % 1000000UL);
64  }
65  return 0;
66 }
67 #endif // _MSC_VER
69  uint16_t format_id, uint16_t frame_id);
71  uint16_t format_id, uint16_t frame_id);
72 void *_uvc_user_caller(void *arg);
74 
75 
76 static uint8_t _uvc_frame_format_matches_guid(uint32_t fourcc, uint8_t guid[16]) {
77  return fourcc == *(const uint32_t *)guid ? 1 : 0;
78 }
79 
80 static uint32_t uvc_frame_format_for_guid(uint8_t guid[16]) {
81  return *(const uint32_t *)guid;
82 }
83 
92  uvc_device_handle_t *devh,
93  uvc_stream_ctrl_t *ctrl,
94  uint8_t probe,
95  enum uvc_req_code req) {
96  uint8_t buf[34];
97  size_t len;
98  uvc_error_t err;
99 
100  memset(buf, 0, sizeof(buf));
101 
102  if (devh->info->ctrl_if.bcdUVC >= 0x0110)
103  len = 34;
104  else
105  len = 26;
106 
107  /* prepare for a SET transfer */
108  if (req == UVC_SET_CUR) {
109  SHORT_TO_SW(ctrl->bmHint, buf);
110  buf[2] = ctrl->bFormatIndex;
111  buf[3] = ctrl->bFrameIndex;
112  INT_TO_DW(ctrl->dwFrameInterval, buf + 4);
113  SHORT_TO_SW(ctrl->wKeyFrameRate, buf + 8);
114  SHORT_TO_SW(ctrl->wPFrameRate, buf + 10);
115  SHORT_TO_SW(ctrl->wCompQuality, buf + 12);
116  SHORT_TO_SW(ctrl->wCompWindowSize, buf + 14);
117  SHORT_TO_SW(ctrl->wDelay, buf + 16);
118  INT_TO_DW(ctrl->dwMaxVideoFrameSize, buf + 18);
119  INT_TO_DW(ctrl->dwMaxPayloadTransferSize, buf + 22);
120 
121  if (len == 34) {
122  INT_TO_DW ( ctrl->dwClockFrequency, buf + 26 );
123  buf[30] = ctrl->bmFramingInfo;
124  buf[31] = ctrl->bPreferredVersion;
125  buf[32] = ctrl->bMinVersion;
126  buf[33] = ctrl->bMaxVersion;
128  }
129  }
130 
131  /* do the transfer */
132  err = libusb_control_transfer(
133  devh->usb_devh,
134  req == UVC_SET_CUR ? 0x21 : 0xA1,
135  req,
136  probe ? (UVC_VS_PROBE_CONTROL << 8) : (UVC_VS_COMMIT_CONTROL << 8),
137  ctrl->bInterfaceNumber,
138  buf, len, 0
139  );
140 
141  if (err <= 0) {
142  return err;
143  }
144 
145  /* now decode following a GET transfer */
146  if (req != UVC_SET_CUR) {
147  ctrl->bmHint = SW_TO_SHORT(buf);
148  ctrl->bFormatIndex = buf[2];
149  ctrl->bFrameIndex = buf[3];
150  ctrl->dwFrameInterval = DW_TO_INT(buf + 4);
151  ctrl->wKeyFrameRate = SW_TO_SHORT(buf + 8);
152  ctrl->wPFrameRate = SW_TO_SHORT(buf + 10);
153  ctrl->wCompQuality = SW_TO_SHORT(buf + 12);
154  ctrl->wCompWindowSize = SW_TO_SHORT(buf + 14);
155  ctrl->wDelay = SW_TO_SHORT(buf + 16);
156  ctrl->dwMaxVideoFrameSize = DW_TO_INT(buf + 18);
157  ctrl->dwMaxPayloadTransferSize = DW_TO_INT(buf + 22);
158 
159  if (len == 34) {
160  ctrl->dwClockFrequency = DW_TO_INT ( buf + 26 );
161  ctrl->bmFramingInfo = buf[30];
162  ctrl->bPreferredVersion = buf[31];
163  ctrl->bMinVersion = buf[32];
164  ctrl->bMaxVersion = buf[33];
166  }
167 
168  /* fix up block for cameras that fail to set dwMax* */
169  if (ctrl->dwMaxVideoFrameSize == 0) {
170  uvc_frame_desc_t *frame = uvc_find_frame_desc(devh, ctrl->bFormatIndex, ctrl->bFrameIndex);
171 
172  if (frame) {
174  }
175  }
176  }
177 
178  return UVC_SUCCESS;
179 }
180 
191  uvc_error_t ret;
192 
193  if (strmh->stream_if->bInterfaceNumber != ctrl->bInterfaceNumber)
195 
196  /* @todo Allow the stream to be modified without restarting the stream */
197  if (strmh->running)
198  return UVC_ERROR_BUSY;
199 
200  ret = uvc_query_stream_ctrl(strmh->devh, ctrl, 0, UVC_SET_CUR);
201  if (ret != UVC_SUCCESS)
202  return ret;
203 
204  strmh->cur_ctrl = *ctrl;
205  return UVC_SUCCESS;
206 }
207 
214 static uvc_frame_desc_t *_uvc_find_frame_desc_stream_if(uvc_streaming_interface_t *stream_if,
215  uint16_t format_id, uint16_t frame_id) {
216 
217  uvc_format_desc_t *format = NULL;
218  uvc_frame_desc_t *frame = NULL;
219 
220  DL_FOREACH(stream_if->format_descs, format) {
221  if (format->bFormatIndex == format_id) {
222  DL_FOREACH(format->frame_descs, frame) {
223  if (frame->bFrameIndex == frame_id)
224  return frame;
225  }
226  }
227  }
228 
229  return NULL;
230 }
231 
233  uint16_t format_id, uint16_t frame_id) {
234  return _uvc_find_frame_desc_stream_if(strmh->stream_if, format_id, frame_id);
235 }
236 
244  uint16_t format_id, uint16_t frame_id) {
245 
246  uvc_streaming_interface_t *stream_if;
247  uvc_frame_desc_t *frame;
248 
249  DL_FOREACH(devh->info->stream_ifs, stream_if) {
250  frame = _uvc_find_frame_desc_stream_if(stream_if, format_id, frame_id);
251  if (frame)
252  return frame;
253  }
254 
255  return NULL;
256 }
257 
269  uvc_device_handle_t *devh,
270  uvc_stream_ctrl_t *ctrl,
271  uint32_t fourcc,
272  int width, int height,
273  int fps) {
274  uvc_streaming_interface_t *stream_if;
275 
276  /* find a matching frame descriptor and interval */
277  DL_FOREACH(devh->info->stream_ifs, stream_if) {
279 
280  DL_FOREACH(stream_if->format_descs, format) {
281  uvc_frame_desc_t *frame;
282 
283  if(fourcc != *(const uint32_t *)format->guidFormat)
284  continue;
285 
286  DL_FOREACH(format->frame_descs, frame) {
287  if (frame->wWidth != width || frame->wHeight != height)
288  continue;
289 
290  uint32_t *interval;
291 
292  if (frame->intervals) {
293  for (interval = frame->intervals; *interval; ++interval) {
294  // allow a fps rate of zero to mean "accept first rate available"
295  int uvc_fps = 10000000 / *interval;
296  if (abs(uvc_fps - fps) <= 1 || fps == 0) {
297 
298  /* get the max values -- we need the interface number to be able
299  to do this */
300  ctrl->bInterfaceNumber = stream_if->bInterfaceNumber;
301  uvc_query_stream_ctrl( devh, ctrl, 1, UVC_GET_MAX);
302 
303  ctrl->bmHint = (1 << 0); /* don't negotiate interval */
304  ctrl->bFormatIndex = format->bFormatIndex;
305  ctrl->bFrameIndex = frame->bFrameIndex;
306  ctrl->dwFrameInterval = *interval;
307 
308  goto found;
309  }
310  }
311  } else {
312  uint32_t interval_100ns = 10000000 / fps;
313  uint32_t interval_offset = interval_100ns - frame->dwMinFrameInterval;
314 
315  if (interval_100ns >= frame->dwMinFrameInterval
316  && interval_100ns <= frame->dwMaxFrameInterval
317  && !(interval_offset
318  && (interval_offset % frame->dwFrameIntervalStep))) {
319 
320  /* get the max values -- we need the interface number to be able
321  to do this */
322  ctrl->bInterfaceNumber = stream_if->bInterfaceNumber;
323  uvc_query_stream_ctrl( devh, ctrl, 1, UVC_GET_MAX);
324 
325  ctrl->bmHint = (1 << 0);
326  ctrl->bFormatIndex = format->bFormatIndex;
327  ctrl->bFrameIndex = frame->bFrameIndex;
328  ctrl->dwFrameInterval = interval_100ns;
329 
330  goto found;
331  }
332  }
333  }
334  }
335  }
336 
337  return UVC_ERROR_INVALID_MODE;
338 
339 found:
340  return uvc_probe_stream_ctrl(devh, ctrl);
341 }
342 
350  uvc_device_handle_t *devh,
351  uvc_stream_ctrl_t *ctrl) {
352 
353  uvc_claim_if(devh, ctrl->bInterfaceNumber);
354 
356  devh, ctrl, 1, UVC_SET_CUR
357  );
358 
360  devh, ctrl, 1, UVC_GET_CUR
361  );
362 
364  return UVC_SUCCESS;
365 }
366 
371  uint8_t *tmp_buf;
372 
373  pthread_mutex_lock(&strmh->cb_mutex);
374 
375  /* swap the buffers */
376  tmp_buf = strmh->holdbuf;
377  strmh->hold_bytes = strmh->got_bytes;
378  strmh->holdbuf = strmh->outbuf;
379  strmh->outbuf = tmp_buf;
380  strmh->hold_last_scr = strmh->last_scr;
381  strmh->hold_pts = strmh->pts;
382  strmh->hold_seq = strmh->seq;
383 
384  pthread_cond_broadcast(&strmh->cb_cond);
385  pthread_mutex_unlock(&strmh->cb_mutex);
386 
387  strmh->seq++;
388  strmh->got_bytes = 0;
389  strmh->last_scr = 0;
390  strmh->pts = 0;
391 }
392 
403 void _uvc_process_payload(uvc_stream_handle_t *strmh, uint8_t *payload, size_t payload_len) {
404  size_t header_len;
405  uint8_t header_info;
406  size_t data_len;
407  struct libusb_iso_packet_descriptor *pkt;
408 
409  /* magic numbers for identifying header packets from some iSight cameras */
410  static uint8_t isight_tag[] = {
411  0x11, 0x22, 0x33, 0x44,
412  0xde, 0xad, 0xbe, 0xef, 0xde, 0xad, 0xfa, 0xce
413  };
414 
415  /* ignore empty payload transfers */
416  if (payload_len == 0)
417  return;
418 
419  /* Certain iSight cameras have strange behavior: They send header
420  * information in a packet with no image data, and then the following
421  * packets have only image data, with no more headers until the next frame.
422  *
423  * The iSight header: len(1), flags(1 or 2), 0x11223344(4),
424  * 0xdeadbeefdeadface(8), ??(16)
425  */
426 
427  if (strmh->devh->is_isight &&
428  (payload_len < 14 || memcmp(isight_tag, payload + 2, sizeof(isight_tag))) &&
429  (payload_len < 15 || memcmp(isight_tag, payload + 3, sizeof(isight_tag)))) {
430  /* The payload transfer doesn't have any iSight magic, so it's all image data */
431  header_len = 0;
432  data_len = payload_len;
433  } else {
434  header_len = payload[0];
435 
436  if (header_len > payload_len) {
437  UVC_DEBUG("bogus packet: actual_len=%zd, header_len=%zd\n", payload_len, header_len);
438  return;
439  }
440 
441  if (strmh->devh->is_isight)
442  data_len = 0;
443  else
444  data_len = payload_len - header_len;
445  }
446 
447  if (header_len < 2) {
448  header_info = 0;
449  } else {
451  size_t variable_offset = 2;
452 
453  header_info = payload[1];
454 
455  if (header_info & 0x40) {
456  UVC_DEBUG("bad packet: error bit set",);
457  return;
458  }
459 
460  if (strmh->fid != (header_info & 1) && strmh->got_bytes != 0) {
461  /* The frame ID bit was flipped, but we have image data sitting
462  around from prior transfers. This means the camera didn't send
463  an EOF for the last transfer of the previous frame. */
464  _uvc_swap_buffers(strmh);
465  }
466 
467  strmh->fid = header_info & 1;
468 
469  if (header_info & (1 << 2)) {
470  strmh->pts = DW_TO_INT(payload + variable_offset);
471  variable_offset += 4;
472  }
473 
474  if (header_info & (1 << 3)) {
476  strmh->last_scr = DW_TO_INT(payload + variable_offset);
477  variable_offset += 6;
478  }
479  }
480 
481  if (data_len > 0) {
482  memcpy(strmh->outbuf + strmh->got_bytes, payload + header_len, data_len);
483  strmh->got_bytes += data_len;
484 
485  if (header_info & (1 << 1)) {
486  /* The EOF bit is set, so publish the complete frame */
487  _uvc_swap_buffers(strmh);
488  }
489  }
490 }
491 
500 void LIBUSB_CALL _uvc_stream_callback(struct libusb_transfer *transfer) {
501  uvc_stream_handle_t *strmh = transfer->user_data;
502 
503  int resubmit = 1;
504 
505  switch (transfer->status) {
506  case LIBUSB_TRANSFER_COMPLETED:
507  if (transfer->num_iso_packets == 0) {
508  /* This is a bulk mode transfer, so it just has one payload transfer */
509  _uvc_process_payload(strmh, transfer->buffer, transfer->actual_length);
510  } else {
511  /* This is an isochronous mode transfer, so each packet has a payload transfer */
512  int packet_id;
513 
514  for (packet_id = 0; packet_id < transfer->num_iso_packets; ++packet_id) {
515  uint8_t *pktbuf;
516  struct libusb_iso_packet_descriptor *pkt;
517 
518  pkt = transfer->iso_packet_desc + packet_id;
519 
520  if (pkt->status != 0) {
521  UVC_DEBUG("bad packet (isochronous transfer); status: %d", pkt->status);
522  continue;
523  }
524 
525  pktbuf = libusb_get_iso_packet_buffer_simple(transfer, packet_id);
526 
527  _uvc_process_payload(strmh, pktbuf, pkt->actual_length);
528 
529  }
530  }
531  break;
532  case LIBUSB_TRANSFER_CANCELLED:
533  case LIBUSB_TRANSFER_ERROR:
534  case LIBUSB_TRANSFER_NO_DEVICE: {
535  int i;
536  UVC_DEBUG("transfer exception, status = %s", libusb_error_name(transfer->status));
537  pthread_mutex_lock(&strmh->cb_mutex);
538 
539  /* Mark transfer as deleted. */
540  for(i=0; i < strmh->num_transfer_bufs; i++) {
541  if(strmh->transfers[i] == transfer) {
542  UVC_DEBUG("Freeing transfer %d (%p)", i, transfer);
543  free(transfer->buffer);
544  libusb_free_transfer(transfer);
545  strmh->transfers[i] = NULL;
546  break;
547  }
548  }
549  if(i == strmh->num_transfer_bufs ) {
550  UVC_DEBUG("transfer %p not found; not freeing!", transfer);
551  }
552 
553  resubmit = 0;
554 
555  pthread_cond_broadcast(&strmh->cb_cond);
556  pthread_mutex_unlock(&strmh->cb_mutex);
557 
558  break;
559  }
560  case LIBUSB_TRANSFER_TIMED_OUT:
561  case LIBUSB_TRANSFER_STALL:
562  case LIBUSB_TRANSFER_OVERFLOW:
563  UVC_DEBUG("retrying transfer, status = %d", transfer->status);
564  break;
565  }
566 
567  if (strmh->running && resubmit)
568  libusb_submit_transfer(transfer);
569 }
570 
582  uvc_device_handle_t *devh,
583  uvc_stream_ctrl_t *ctrl,
585  void *user_ptr,
586  uint8_t flags,
587  int num_transfer_buffers
588 ) {
589  uvc_error_t ret;
590  uvc_stream_handle_t *strmh;
591 
592  ret = uvc_stream_open_ctrl(devh, &strmh, ctrl);
593  if (ret != UVC_SUCCESS)
594  return ret;
595 
596  ret = uvc_stream_start(strmh, cb, user_ptr, flags, num_transfer_buffers);
597  if (ret != UVC_SUCCESS) {
598  UVC_DEBUG("FAILED TO START STREAM: %i", ret);
599  uvc_stream_close(strmh);
600  return ret;
601  }
602 
603  ctrl->handle = strmh;
604 
605  return UVC_SUCCESS;
606 }
607 
609  uvc_stream_handle_t *strmh;
610 
611  DL_FOREACH(devh->streams, strmh) {
612  if (strmh->stream_if->bInterfaceNumber == interface_idx)
613  return strmh;
614  }
615 
616  return NULL;
617 }
618 
619 static uvc_streaming_interface_t *_uvc_get_stream_if(uvc_device_handle_t *devh, int interface_idx) {
620  uvc_streaming_interface_t *stream_if;
621 
622  DL_FOREACH(devh->info->stream_ifs, stream_if) {
623  if (stream_if->bInterfaceNumber == interface_idx)
624  return stream_if;
625  }
626 
627  return NULL;
628 }
629 
638  /* Chosen frame and format descriptors */
639  uvc_stream_handle_t *strmh = NULL;
640  uvc_streaming_interface_t *stream_if;
641  uvc_error_t ret;
642 
643  UVC_ENTER();
644 
645  if (_uvc_get_stream_by_interface(devh, ctrl->bInterfaceNumber) != NULL) {
646  ret = UVC_ERROR_BUSY; /* Stream is already opened */
647  goto fail;
648  }
649 
650  stream_if = _uvc_get_stream_if(devh, ctrl->bInterfaceNumber);
651  if (!stream_if) {
653  goto fail;
654  }
655 
656  strmh = calloc(1, sizeof(*strmh));
657  if (!strmh) {
658  ret = UVC_ERROR_NO_MEM;
659  goto fail;
660  }
661  strmh->devh = devh;
662  strmh->stream_if = stream_if;
663  strmh->frame.library_owns_data = 1;
664 
665  ret = uvc_claim_if(strmh->devh, strmh->stream_if->bInterfaceNumber);
666  if (ret != UVC_SUCCESS)
667  goto fail;
668 
669  ret = uvc_stream_ctrl(strmh, ctrl);
670  if (ret != UVC_SUCCESS)
671  goto fail;
672 
673  // Set up the streaming status and data space
674  strmh->running = 0;
676  strmh->outbuf = malloc( LIBUVC_XFER_BUF_SIZE );
677  strmh->holdbuf = malloc( LIBUVC_XFER_BUF_SIZE );
678 
679  pthread_mutex_init(&strmh->cb_mutex, NULL);
680  pthread_cond_init(&strmh->cb_cond, NULL);
681 
682  DL_APPEND(devh->streams, strmh);
683 
684  *strmhp = strmh;
685 
686  UVC_EXIT(0);
687  return UVC_SUCCESS;
688 
689 fail:
690  if(strmh)
691  free(strmh);
692  UVC_EXIT(ret);
693  return ret;
694 }
695 
705  uvc_stream_handle_t *strmh,
707  void *user_ptr,
708  uint8_t flags,
709  int num_transfer_buffers
710 ) {
711  /* USB interface we'll be using */
712  const struct libusb_interface *interface;
713  int interface_id;
714  char isochronous;
715  uvc_frame_desc_t *frame_desc;
716  uvc_format_desc_t *format_desc;
717  uvc_stream_ctrl_t *ctrl;
718  uvc_error_t ret;
719  /* Total amount of data per transfer */
720  size_t total_transfer_size;
721  struct libusb_transfer *transfer;
722  int transfer_id;
723 
724  ctrl = &strmh->cur_ctrl;
725 
726  UVC_ENTER();
727 
728  if (strmh->running) {
729  UVC_EXIT(UVC_ERROR_BUSY);
730  return UVC_ERROR_BUSY;
731  }
732 
733  strmh->running = 1;
734  strmh->seq = 0;
735  strmh->fid = 0;
736  strmh->pts = 0;
737  strmh->last_scr = 0;
738 
739  frame_desc = uvc_find_frame_desc_stream(strmh, ctrl->bFormatIndex, ctrl->bFrameIndex);
740  if (!frame_desc) {
742  goto fail;
743  }
744  format_desc = frame_desc->parent;
745 
746  strmh->fourcc = uvc_frame_format_for_guid(format_desc->guidFormat);
747 
748  // Get the interface that provides the chosen format and frame configuration
749  interface_id = strmh->stream_if->bInterfaceNumber;
750  interface = &strmh->devh->info->config->interface[interface_id];
751 
752  /* A VS interface uses isochronous transfers iff it has multiple altsettings.
753  * (UVC 1.5: 2.4.3. VideoStreaming Interface) */
754  isochronous = interface->num_altsetting > 1;
755 
756  if (isochronous) {
757  /* For isochronous streaming, we choose an appropriate altsetting for the endpoint
758  * and set up several transfers */
759  const struct libusb_interface_descriptor *altsetting;
760  const struct libusb_endpoint_descriptor *endpoint;
761  /* The greatest number of bytes that the device might provide, per packet, in this
762  * configuration */
763  size_t config_bytes_per_packet;
764  /* Number of packets per transfer */
765  size_t packets_per_transfer;
766  /* Size of packet transferable from the chosen endpoint */
767  size_t endpoint_bytes_per_packet;
768  /* Index of the altsetting */
769  int alt_idx, ep_idx;
770 
771  config_bytes_per_packet = strmh->cur_ctrl.dwMaxPayloadTransferSize;
772 
773  /* Go through the altsettings and find one whose packets are at least
774  * as big as our format's maximum per-packet usage. Assume that the
775  * packet sizes are increasing. */
776  for (alt_idx = 0; alt_idx < interface->num_altsetting; alt_idx++) {
777  altsetting = interface->altsetting + alt_idx;
778  endpoint_bytes_per_packet = 0;
779 
780  /* Find the endpoint with the number specified in the VS header */
781  for (ep_idx = 0; ep_idx < altsetting->bNumEndpoints; ep_idx++) {
782  endpoint = altsetting->endpoint + ep_idx;
783 
784  if (endpoint->bEndpointAddress == format_desc->parent->bEndpointAddress) {
785  endpoint_bytes_per_packet = endpoint->wMaxPacketSize;
786  // wMaxPacketSize: [unused:2 (multiplier-1):3 size:11]
787  endpoint_bytes_per_packet = (endpoint_bytes_per_packet & 0x07ff) *
788  (((endpoint_bytes_per_packet >> 11) & 3) + 1);
789  break;
790  }
791  }
792 
793  if (endpoint_bytes_per_packet >= config_bytes_per_packet) {
794  /* Transfers will be at most one frame long: Divide the maximum frame size
795  * by the size of the endpoint and round up */
796  packets_per_transfer = (ctrl->dwMaxVideoFrameSize +
797  endpoint_bytes_per_packet - 1) / endpoint_bytes_per_packet;
798 
799  /* But keep a reasonable limit: Otherwise we start dropping data */
800  if (packets_per_transfer > 32)
801  packets_per_transfer = 32;
802 
803  total_transfer_size = packets_per_transfer * endpoint_bytes_per_packet;
804  break;
805  }
806  }
807 
808  /* If we searched through all the altsettings and found nothing usable */
809  if (alt_idx == interface->num_altsetting) {
811  goto fail;
812  }
813 
814  /* Select the altsetting */
815  ret = libusb_set_interface_alt_setting(strmh->devh->usb_devh,
816  altsetting->bInterfaceNumber,
817  altsetting->bAlternateSetting);
818  if (ret != UVC_SUCCESS) {
819  UVC_DEBUG("libusb_set_interface_alt_setting failed",);
820  goto fail;
821  }
822 
823  /* Set up the transfers */
824  strmh->num_transfer_bufs = num_transfer_buffers;
825  strmh->transfers = malloc(sizeof(struct libusb_transfer *) * num_transfer_buffers);
826  strmh->transfer_bufs = malloc(sizeof(struct uint8_t *) * num_transfer_buffers);
827  for (transfer_id = 0; transfer_id < num_transfer_buffers; ++transfer_id)
828  {
829  transfer = libusb_alloc_transfer(packets_per_transfer);
830  strmh->transfers[transfer_id] = transfer;
831  strmh->transfer_bufs[transfer_id] = malloc(total_transfer_size);
832 
833  libusb_fill_iso_transfer(
834  transfer, strmh->devh->usb_devh, format_desc->parent->bEndpointAddress,
835  strmh->transfer_bufs[transfer_id],
836  total_transfer_size, packets_per_transfer, _uvc_stream_callback, (void*) strmh, 5000);
837 
838  libusb_set_iso_packet_lengths(transfer, endpoint_bytes_per_packet);
839  }
840  }
841 
842  else
843  {
844  strmh->num_transfer_bufs = num_transfer_buffers;
845  strmh->transfers = malloc(sizeof(struct libusb_transfer *) * num_transfer_buffers);
846  strmh->transfer_bufs = malloc(sizeof(struct uint8_t *) * num_transfer_buffers);
847  for (transfer_id = 0; transfer_id < num_transfer_buffers; ++transfer_id)
848  {
849  transfer = libusb_alloc_transfer(0);
850  strmh->transfers[transfer_id] = transfer;
851  strmh->transfer_bufs[transfer_id] = malloc (
852  strmh->cur_ctrl.dwMaxPayloadTransferSize );
853  libusb_fill_bulk_transfer ( transfer, strmh->devh->usb_devh,
854  format_desc->parent->bEndpointAddress,
855  strmh->transfer_bufs[transfer_id],
856  strmh->cur_ctrl.dwMaxPayloadTransferSize, _uvc_stream_callback,
857  ( void* ) strmh, 5000 );
858  }
859  }
860 
861  strmh->user_cb = cb;
862  strmh->user_ptr = user_ptr;
863 
864  /* If the user wants it, set up a thread that calls the user's function
865  * with the contents of each frame.
866  */
867  if (cb)
868  {
869  pthread_create(&strmh->cb_thread, NULL, _uvc_user_caller, (void*) strmh);
870  }
871 
872  for (transfer_id = 0; transfer_id < num_transfer_buffers; transfer_id++)
873  {
874  ret = libusb_submit_transfer(strmh->transfers[transfer_id]);
875  if (ret != UVC_SUCCESS)
876  {
877  UVC_DEBUG("libusb_submit_transfer failed",);
878  break;
879  }
880  }
881 
882  UVC_EXIT(ret);
883  return ret;
884 fail:
885  strmh->running = 0;
886  UVC_EXIT(ret);
887  return ret;
888 }
889 
895 void *_uvc_user_caller(void *arg) {
896  uvc_stream_handle_t *strmh = (uvc_stream_handle_t *) arg;
897 
898  uint32_t last_seq = 0;
899 
900  do {
901  pthread_mutex_lock(&strmh->cb_mutex);
902 
903  while (strmh->running && last_seq == strmh->hold_seq) {
904  pthread_cond_wait(&strmh->cb_cond, &strmh->cb_mutex);
905  }
906 
907  if (!strmh->running) {
908  pthread_mutex_unlock(&strmh->cb_mutex);
909  break;
910  }
911 
912  last_seq = strmh->hold_seq;
913  _uvc_populate_frame(strmh);
914 
915  pthread_mutex_unlock(&strmh->cb_mutex);
916 
917  strmh->user_cb(&strmh->frame, strmh->user_ptr);
918  } while(1);
919 
920  return NULL; // return value ignored
921 }
922 
928  size_t alloc_size = strmh->cur_ctrl.dwMaxVideoFrameSize;
929  uvc_frame_t *frame = &strmh->frame;
930  uvc_frame_desc_t *frame_desc;
931 
937  frame_desc = uvc_find_frame_desc(strmh->devh, strmh->cur_ctrl.bFormatIndex,
938  strmh->cur_ctrl.bFrameIndex);
939 
940  frame->fourcc = strmh->fourcc;
941 
942  frame->width = frame_desc->wWidth;
943  frame->height = frame_desc->wHeight;
944 
945 #pragma GCC diagnostic push
946 #pragma GCC diagnostic ignored "-Wmultichar"
947 #ifdef __APPLE__
948 #pragma GCC diagnostic ignored "-Wfour-char-constants"
949 #endif
950  switch (frame->fourcc) {
951  case '2YUY': /* YUY2 */
952  frame->step = frame->width * 2;
953  break;
954  default:
955  frame->step = 0;
956  break;
957  }
958 #pragma GCC diagnostic pop
959 
960  /* copy the image data from the hold buffer to the frame (unnecessary extra buf?) */
961  if (frame->data_bytes < strmh->hold_bytes) {
962  frame->data = realloc(frame->data, strmh->hold_bytes);
963  frame->data_bytes = strmh->hold_bytes;
964  }
965  memcpy(frame->data, strmh->holdbuf, frame->data_bytes);
966 
968 }
969 
978  uvc_frame_t **frame,
979  int32_t timeout_us) {
980  time_t add_secs;
981  time_t add_nsecs;
982  struct timespec ts;
983  struct timeval tv;
984 
985  if (!strmh->running)
987 
988  if (strmh->user_cb)
990 
991  pthread_mutex_lock(&strmh->cb_mutex);
992 
993  if (strmh->last_polled_seq < strmh->hold_seq) {
994  _uvc_populate_frame(strmh);
995  *frame = &strmh->frame;
996  strmh->last_polled_seq = strmh->hold_seq;
997  } else if (timeout_us != -1) {
998  if (timeout_us == 0) {
999  pthread_cond_wait(&strmh->cb_cond, &strmh->cb_mutex);
1000  } else {
1001  add_secs = timeout_us / 1000000;
1002  add_nsecs = (timeout_us % 1000000) * 1000;
1003  ts.tv_sec = 0;
1004  ts.tv_nsec = 0;
1005 
1006 #if _POSIX_TIMERS > 0
1007  clock_gettime(CLOCK_REALTIME, &ts);
1008 #else
1009  gettimeofday(&tv, NULL);
1010  ts.tv_sec = tv.tv_sec;
1011  ts.tv_nsec = tv.tv_usec * 1000;
1012 #endif
1013 
1014  ts.tv_sec += add_secs;
1015  ts.tv_nsec += add_nsecs;
1016 
1017  pthread_cond_timedwait(&strmh->cb_cond, &strmh->cb_mutex, &ts);
1018  }
1019 
1020  if (strmh->last_polled_seq < strmh->hold_seq) {
1021  _uvc_populate_frame(strmh);
1022  *frame = &strmh->frame;
1023  strmh->last_polled_seq = strmh->hold_seq;
1024  } else {
1025  *frame = NULL;
1026  }
1027  } else {
1028  *frame = NULL;
1029  }
1030 
1031  pthread_mutex_unlock(&strmh->cb_mutex);
1032 
1033  return UVC_SUCCESS;
1034 }
1035 
1044  uvc_stream_handle_t *strmh, *strmh_tmp;
1045 
1046  DL_FOREACH_SAFE(devh->streams, strmh, strmh_tmp) {
1047  uvc_stream_close(strmh);
1048  }
1049 }
1050 
1059  int i,timeout_s= 1,ret=UVC_SUCCESS;
1060  time_t add_secs;
1061  struct timespec ts;
1062  struct timeval tv;
1063  if (!strmh->running)
1064  return UVC_ERROR_INVALID_PARAM;
1065 
1066  strmh->running = 0;
1067 
1068  pthread_mutex_lock(&strmh->cb_mutex);
1069 
1070  for(i=0; i < strmh->num_transfer_bufs; i++) {
1071  if(strmh->transfers[i] != NULL) {
1072  int res = libusb_cancel_transfer(strmh->transfers[i]);
1073  if(res < 0) {
1074  free(strmh->transfers[i]->buffer);
1075  libusb_free_transfer(strmh->transfers[i]);
1076  strmh->transfers[i] = NULL;
1077  }
1078  }
1079  }
1080 
1081 
1082  /* Wait for transfers to complete/cancel */
1083  do {
1084  for(i=0; i < strmh->num_transfer_bufs; i++) {
1085  if(strmh->transfers[i] != NULL)
1086  break;
1087  }
1088 
1089  if(i == strmh->num_transfer_bufs )
1090  break;
1091 
1092  // this ones sometimes does not return.
1093  // pthread_cond_wait(&strmh->cb_cond, &strmh->cb_mutex);
1094 
1095  add_secs = timeout_s ;
1096  ts.tv_sec = 0;
1097  ts.tv_nsec = 0;
1098 
1099 #if _POSIX_TIMERS > 0
1100  clock_gettime(CLOCK_REALTIME, &ts);
1101 #else
1102  gettimeofday(&tv, NULL);
1103  ts.tv_sec = tv.tv_sec;
1104  ts.tv_nsec = tv.tv_usec * 1000;
1105 #endif
1106 
1107  ts.tv_sec += add_secs;
1108 
1109  if (ETIMEDOUT == pthread_cond_timedwait(&strmh->cb_cond, &strmh->cb_mutex, &ts)){
1110  ret = UVC_ERROR_TIMEOUT;
1111  break;
1112  }
1113  } while(1);
1114 
1115  // Kick the user thread awake
1116  pthread_cond_broadcast(&strmh->cb_cond);
1117  pthread_mutex_unlock(&strmh->cb_mutex);
1118 
1121  if (strmh->user_cb) {
1122  /* wait for the thread to stop (triggered by
1123  * LIBUSB_TRANSFER_CANCELLED transfer) */
1124  pthread_join(strmh->cb_thread, NULL);
1125  }
1126 
1127  return ret;
1128 }
1129 
1138  if (strmh->running)
1139  uvc_stream_stop(strmh);
1140 
1141  uvc_release_if(strmh->devh, strmh->stream_if->bInterfaceNumber);
1142 
1143  if (strmh->frame.data)
1144  free(strmh->frame.data);
1145 
1146  free(strmh->outbuf);
1147  free(strmh->holdbuf);
1148 
1149  pthread_cond_destroy(&strmh->cb_cond);
1150  pthread_mutex_destroy(&strmh->cb_mutex);
1151 
1152  DL_DELETE(strmh->devh->streams, strmh);
1153  free(strmh);
1154 }
static uint32_t uvc_frame_format_for_guid(uint8_t guid[16])
Definition: stream.c:80
void _uvc_populate_frame(uvc_stream_handle_t *strmh)
Definition: stream.c:927
uvc_req_code
Definition: libuvc.h:152
uvc_error_t uvc_query_stream_ctrl(uvc_device_handle_t *devh, uvc_stream_ctrl_t *ctrl, uint8_t probe, enum uvc_req_code req)
Definition: stream.c:91
enum uvc_error uvc_error_t
size_t data_bytes
Definition: libuvc.h:368
uvc_error_t uvc_stream_ctrl(uvc_stream_handle_t *strmh, uvc_stream_ctrl_t *ctrl)
Reconfigure stream with a new stream format.This may be executed whether or not the stream is running...
Definition: stream.c:190
uint16_t wWidth
Definition: libuvc.h:90
GLint GLint GLsizei GLsizei height
Definition: glext.h:112
uint32_t * intervals
Definition: libuvc.h:112
#define DL_FOREACH(head, el)
Definition: utlist.h:76
void uvc_stop_streaming(uvc_device_handle_t *devh)
Stop streaming videoCloses all streams, ends threads and cancels pollers.
Definition: stream.c:1043
uvc_error_t uvc_stream_start(uvc_stream_handle_t *strmh, uvc_frame_callback_t *cb, void *user_ptr, uint8_t flags, int num_transfer_buffers)
Definition: stream.c:704
uvc_error_t uvc_start_streaming(uvc_device_handle_t *devh, uvc_stream_ctrl_t *ctrl, uvc_frame_callback_t *cb, void *user_ptr, uint8_t flags, int num_transfer_buffers)
Definition: stream.c:581
uint16_t wHeight
Definition: libuvc.h:92
GLenum GLuint GLenum GLsizei const GLchar * buf
Definition: glext.h:2479
uint8_t bMaxVersion
Definition: libuvc.h:417
uvc_error_t uvc_stream_get_frame(uvc_stream_handle_t *strmh, uvc_frame_t **frame, int32_t timeout_us)
Definition: stream.c:977
uint32_t height
Definition: libuvc.h:372
GLbitfield flags
Definition: glext.h:1478
uint32_t dwFrameIntervalStep
Definition: libuvc.h:106
struct uvc_format_desc * parent
Definition: libuvc.h:82
uint32_t dwMaxVideoFrameBufferSize
Definition: libuvc.h:98
uvc_frame_desc_t * uvc_find_frame_desc(uvc_device_handle_t *devh, uint16_t format_id, uint16_t frame_id)
Definition: stream.c:243
GLenum GLsizei len
Definition: glext.h:3213
uint8_t bInterfaceNumber
Definition: libuvc.h:418
uvc_stream_handle_t * handle
Definition: libuvc.h:419
uint32_t fourcc
Definition: libuvc.h:374
uint32_t dwMaxPayloadTransferSize
Definition: libuvc.h:412
uint32_t width
Definition: libuvc.h:370
uvc_error_t uvc_stream_open_ctrl(uvc_device_handle_t *devh, uvc_stream_handle_t **strmhp, uvc_stream_ctrl_t *ctrl)
Definition: stream.c:637
uvc_frame_desc_t * uvc_find_frame_desc_stream(uvc_stream_handle_t *strmh, uint16_t format_id, uint16_t frame_id)
Definition: stream.c:232
uint32_t dwFrameInterval
Definition: libuvc.h:405
#define DL_DELETE(head, del)
Definition: utlist.h:58
uint8_t bFormatIndex
Definition: libuvc.h:126
void _uvc_swap_buffers(uvc_stream_handle_t *strmh)
Definition: stream.c:370
uvc_error_t uvc_stream_stop(uvc_stream_handle_t *strmh)
Stop stream.Stops stream, ends threads and cancels pollers.
Definition: stream.c:1058
uint32_t dwClockFrequency
Definition: libuvc.h:413
uvc_error_t uvc_claim_if(uvc_device_handle_t *devh, int idx)
Definition: dev.c:782
format
Formats: defines how each stream can be encoded. rs_format specifies how a frame is represented in me...
Definition: rs.hpp:42
Implementation-specific UVC constants and structures.
uint16_t bmHint
Definition: libuvc.h:402
uint8_t bmFramingInfo
Definition: libuvc.h:414
uint16_t wKeyFrameRate
Definition: libuvc.h:406
struct uvc_stream_handle uvc_stream_handle_t
Definition: libuvc.h:276
uint8_t guidFormat[16]
Definition: libuvc.h:130
static uvc_streaming_interface_t * _uvc_get_stream_if(uvc_device_handle_t *devh, int interface_idx)
Definition: stream.c:619
uvc_error_t uvc_get_stream_ctrl_format_size(uvc_device_handle_t *devh, uvc_stream_ctrl_t *ctrl, uint32_t fourcc, int width, int height, int fps)
Definition: stream.c:268
uint16_t wCompWindowSize
Definition: libuvc.h:409
static uvc_frame_desc_t * _uvc_find_frame_desc_stream_if(uvc_streaming_interface_t *stream_if, uint16_t format_id, uint16_t frame_id)
Definition: stream.c:214
uint16_t wPFrameRate
Definition: libuvc.h:407
uint32_t dwMaxVideoFrameSize
Definition: libuvc.h:411
uvc_error_t uvc_probe_stream_ctrl(uvc_device_handle_t *devh, uvc_stream_ctrl_t *ctrl)
Definition: stream.c:349
void LIBUSB_CALL _uvc_stream_callback(struct libusb_transfer *transfer)
Definition: stream.c:500
uint8_t bFrameIndex
Definition: libuvc.h:87
uint16_t wDelay
Definition: libuvc.h:410
GLint GLint GLsizei width
Definition: glext.h:112
uint8_t bFrameIndex
Definition: libuvc.h:404
GLbyte GLbyte tz
Definition: glext.h:6341
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glext.h:112
#define DL_APPEND(head, add)
Definition: utlist.h:44
GLuint res
Definition: glext.h:8310
size_t step
Definition: libuvc.h:376
struct uvc_device_handle uvc_device_handle_t
Definition: libuvc.h:268
#define DL_FOREACH_SAFE(head, el, tmp)
Definition: utlist.h:80
uint32_t dwMinFrameInterval
Definition: libuvc.h:102
uint16_t wCompQuality
Definition: libuvc.h:408
static uvc_stream_handle_t * _uvc_get_stream_by_interface(uvc_device_handle_t *devh, int interface_idx)
Definition: stream.c:608
uint8_t bMinVersion
Definition: libuvc.h:416
void _uvc_process_payload(uvc_stream_handle_t *strmh, uint8_t *payload, size_t payload_len)
Definition: stream.c:403
struct uvc_frame_desc * frame_descs
Definition: libuvc.h:148
uint8_t bPreferredVersion
Definition: libuvc.h:415
uint8_t bFormatIndex
Definition: libuvc.h:403
void * _uvc_user_caller(void *arg)
Definition: stream.c:895
void( uvc_frame_callback_t)(struct uvc_frame *frame, void *user_ptr)
Definition: libuvc.h:396
struct uvc_streaming_interface * parent
Definition: libuvc.h:121
uvc_error_t uvc_release_if(uvc_device_handle_t *devh, int idx)
Definition: dev.c:810
void uvc_stream_close(uvc_stream_handle_t *strmh)
Close stream.Closes stream, frees handle and all streaming resources.
Definition: stream.c:1137
static uint8_t _uvc_frame_format_matches_guid(uint32_t fourcc, uint8_t guid[16])
Definition: stream.c:76
void * data
Definition: libuvc.h:366


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Fri Mar 13 2020 03:16:17