stream.c
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (C) 2010-2012 Ken Tossell
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the author nor other contributors may be
00018 *     used to endorse or promote products derived from this software
00019 *     without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00039 #include "libuvc.h"
00040 #include "libuvc_internal.h"
00041 
00042 #ifdef _MSC_VER
00043 
00044 #define DELTA_EPOCH_IN_MICROSECS  116444736000000000Ui64
00045 
00046 // gettimeofday - get time of day for Windows;
00047 // A gettimeofday implementation for Microsoft Windows;
00048 // Public domain code, author "ponnada";
00049 int gettimeofday(struct timeval *tv, struct timezone *tz)
00050 {
00051     FILETIME ft;
00052     unsigned __int64 tmpres = 0;
00053     static int tzflag = 0;
00054     if (NULL != tv)
00055     {
00056         GetSystemTimeAsFileTime(&ft);
00057         tmpres |= ft.dwHighDateTime;
00058         tmpres <<= 32;
00059         tmpres |= ft.dwLowDateTime;
00060         tmpres /= 10;
00061         tmpres -= DELTA_EPOCH_IN_MICROSECS;
00062         tv->tv_sec = (long)(tmpres / 1000000UL);
00063         tv->tv_usec = (long)(tmpres % 1000000UL);
00064     }
00065     return 0;
00066 }
00067 #endif // _MSC_VER
00068 uvc_frame_desc_t *uvc_find_frame_desc_stream(uvc_stream_handle_t *strmh,
00069     uint16_t format_id, uint16_t frame_id);
00070 uvc_frame_desc_t *uvc_find_frame_desc(uvc_device_handle_t *devh,
00071     uint16_t format_id, uint16_t frame_id);
00072 void *_uvc_user_caller(void *arg);
00073 void _uvc_populate_frame(uvc_stream_handle_t *strmh);
00074 
00075 
00076 static uint8_t _uvc_frame_format_matches_guid(uint32_t fourcc, uint8_t guid[16]) {
00077   return fourcc == *(const uint32_t *)guid ? 1 : 0;
00078 }
00079 
00080 static uint32_t uvc_frame_format_for_guid(uint8_t guid[16]) {
00081     return *(const uint32_t *)guid;
00082 }
00083 
00091 uvc_error_t uvc_query_stream_ctrl(
00092     uvc_device_handle_t *devh,
00093     uvc_stream_ctrl_t *ctrl,
00094     uint8_t probe,
00095     enum uvc_req_code req) {
00096   uint8_t buf[34];
00097   size_t len;
00098   uvc_error_t err;
00099 
00100   memset(buf, 0, sizeof(buf));
00101 
00102   if (devh->info->ctrl_if.bcdUVC >= 0x0110)
00103     len = 34;
00104   else
00105     len = 26;
00106 
00107   /* prepare for a SET transfer */
00108   if (req == UVC_SET_CUR) {
00109     SHORT_TO_SW(ctrl->bmHint, buf);
00110     buf[2] = ctrl->bFormatIndex;
00111     buf[3] = ctrl->bFrameIndex;
00112     INT_TO_DW(ctrl->dwFrameInterval, buf + 4);
00113     SHORT_TO_SW(ctrl->wKeyFrameRate, buf + 8);
00114     SHORT_TO_SW(ctrl->wPFrameRate, buf + 10);
00115     SHORT_TO_SW(ctrl->wCompQuality, buf + 12);
00116     SHORT_TO_SW(ctrl->wCompWindowSize, buf + 14);
00117     SHORT_TO_SW(ctrl->wDelay, buf + 16);
00118     INT_TO_DW(ctrl->dwMaxVideoFrameSize, buf + 18);
00119     INT_TO_DW(ctrl->dwMaxPayloadTransferSize, buf + 22);
00120 
00121     if (len == 34) {
00122       INT_TO_DW ( ctrl->dwClockFrequency, buf + 26 );
00123       buf[30] = ctrl->bmFramingInfo;
00124       buf[31] = ctrl->bPreferredVersion;
00125       buf[32] = ctrl->bMinVersion;
00126       buf[33] = ctrl->bMaxVersion;
00128     }
00129   }
00130 
00131   /* do the transfer */
00132   err = libusb_control_transfer(
00133       devh->usb_devh,
00134       req == UVC_SET_CUR ? 0x21 : 0xA1,
00135       req,
00136       probe ? (UVC_VS_PROBE_CONTROL << 8) : (UVC_VS_COMMIT_CONTROL << 8),
00137       ctrl->bInterfaceNumber,
00138       buf, len, 0
00139   );
00140 
00141   if (err <= 0) {
00142     return err;
00143   }
00144 
00145   /* now decode following a GET transfer */
00146   if (req != UVC_SET_CUR) {
00147     ctrl->bmHint = SW_TO_SHORT(buf);
00148     ctrl->bFormatIndex = buf[2];
00149     ctrl->bFrameIndex = buf[3];
00150     ctrl->dwFrameInterval = DW_TO_INT(buf + 4);
00151     ctrl->wKeyFrameRate = SW_TO_SHORT(buf + 8);
00152     ctrl->wPFrameRate = SW_TO_SHORT(buf + 10);
00153     ctrl->wCompQuality = SW_TO_SHORT(buf + 12);
00154     ctrl->wCompWindowSize = SW_TO_SHORT(buf + 14);
00155     ctrl->wDelay = SW_TO_SHORT(buf + 16);
00156     ctrl->dwMaxVideoFrameSize = DW_TO_INT(buf + 18);
00157     ctrl->dwMaxPayloadTransferSize = DW_TO_INT(buf + 22);
00158 
00159     if (len == 34) {
00160       ctrl->dwClockFrequency = DW_TO_INT ( buf + 26 );
00161       ctrl->bmFramingInfo = buf[30];
00162       ctrl->bPreferredVersion = buf[31];
00163       ctrl->bMinVersion = buf[32];
00164       ctrl->bMaxVersion = buf[33];
00166     }
00167 
00168     /* fix up block for cameras that fail to set dwMax* */
00169     if (ctrl->dwMaxVideoFrameSize == 0) {
00170       uvc_frame_desc_t *frame = uvc_find_frame_desc(devh, ctrl->bFormatIndex, ctrl->bFrameIndex);
00171 
00172       if (frame) {
00173         ctrl->dwMaxVideoFrameSize = frame->dwMaxVideoFrameBufferSize;
00174       }
00175     }
00176   }
00177 
00178   return UVC_SUCCESS;
00179 }
00180 
00190 uvc_error_t uvc_stream_ctrl(uvc_stream_handle_t *strmh, uvc_stream_ctrl_t *ctrl) {
00191   uvc_error_t ret;
00192 
00193   if (strmh->stream_if->bInterfaceNumber != ctrl->bInterfaceNumber)
00194     return UVC_ERROR_INVALID_PARAM;
00195 
00196   /* @todo Allow the stream to be modified without restarting the stream */
00197   if (strmh->running)
00198     return UVC_ERROR_BUSY;
00199 
00200   ret = uvc_query_stream_ctrl(strmh->devh, ctrl, 0, UVC_SET_CUR);
00201   if (ret != UVC_SUCCESS)
00202     return ret;
00203 
00204   strmh->cur_ctrl = *ctrl;
00205   return UVC_SUCCESS;
00206 }
00207 
00214 static uvc_frame_desc_t *_uvc_find_frame_desc_stream_if(uvc_streaming_interface_t *stream_if,
00215     uint16_t format_id, uint16_t frame_id) {
00216  
00217   uvc_format_desc_t *format = NULL;
00218   uvc_frame_desc_t *frame = NULL;
00219 
00220   DL_FOREACH(stream_if->format_descs, format) {
00221     if (format->bFormatIndex == format_id) {
00222       DL_FOREACH(format->frame_descs, frame) {
00223         if (frame->bFrameIndex == frame_id)
00224           return frame;
00225       }
00226     }
00227   }
00228 
00229   return NULL;
00230 }
00231 
00232 uvc_frame_desc_t *uvc_find_frame_desc_stream(uvc_stream_handle_t *strmh,
00233     uint16_t format_id, uint16_t frame_id) {
00234   return _uvc_find_frame_desc_stream_if(strmh->stream_if, format_id, frame_id);
00235 }
00236 
00243 uvc_frame_desc_t *uvc_find_frame_desc(uvc_device_handle_t *devh,
00244     uint16_t format_id, uint16_t frame_id) {
00245  
00246   uvc_streaming_interface_t *stream_if;
00247   uvc_frame_desc_t *frame;
00248 
00249   DL_FOREACH(devh->info->stream_ifs, stream_if) {
00250     frame = _uvc_find_frame_desc_stream_if(stream_if, format_id, frame_id);
00251     if (frame)
00252       return frame;
00253   }
00254 
00255   return NULL;
00256 }
00257 
00268 uvc_error_t uvc_get_stream_ctrl_format_size(
00269     uvc_device_handle_t *devh,
00270     uvc_stream_ctrl_t *ctrl,
00271     uint32_t fourcc,
00272     int width, int height,
00273     int fps) {
00274   uvc_streaming_interface_t *stream_if;
00275 
00276   /* find a matching frame descriptor and interval */
00277   DL_FOREACH(devh->info->stream_ifs, stream_if) {
00278     uvc_format_desc_t *format;
00279 
00280     DL_FOREACH(stream_if->format_descs, format) {
00281       uvc_frame_desc_t *frame;
00282 
00283       if(fourcc != *(const uint32_t *)format->guidFormat)
00284         continue;
00285 
00286       DL_FOREACH(format->frame_descs, frame) {
00287         if (frame->wWidth != width || frame->wHeight != height)
00288           continue;
00289 
00290         uint32_t *interval;
00291 
00292         if (frame->intervals) {
00293           for (interval = frame->intervals; *interval; ++interval) {
00294             // allow a fps rate of zero to mean "accept first rate available"
00295             int uvc_fps = 10000000 / *interval;
00296             if (abs(uvc_fps - fps) <= 1 || fps == 0) {
00297 
00298               /* get the max values -- we need the interface number to be able
00299                  to do this */
00300               ctrl->bInterfaceNumber = stream_if->bInterfaceNumber;
00301               uvc_query_stream_ctrl( devh, ctrl, 1, UVC_GET_MAX);
00302 
00303               ctrl->bmHint = (1 << 0); /* don't negotiate interval */
00304               ctrl->bFormatIndex = format->bFormatIndex;
00305               ctrl->bFrameIndex = frame->bFrameIndex;
00306               ctrl->dwFrameInterval = *interval;
00307 
00308               goto found;
00309             }
00310           }
00311         } else {
00312           uint32_t interval_100ns = 10000000 / fps;
00313           uint32_t interval_offset = interval_100ns - frame->dwMinFrameInterval;
00314 
00315           if (interval_100ns >= frame->dwMinFrameInterval
00316               && interval_100ns <= frame->dwMaxFrameInterval
00317               && !(interval_offset
00318                    && (interval_offset % frame->dwFrameIntervalStep))) {
00319 
00320             /* get the max values -- we need the interface number to be able
00321                to do this */
00322             ctrl->bInterfaceNumber = stream_if->bInterfaceNumber;
00323             uvc_query_stream_ctrl( devh, ctrl, 1, UVC_GET_MAX);
00324 
00325             ctrl->bmHint = (1 << 0);
00326             ctrl->bFormatIndex = format->bFormatIndex;
00327             ctrl->bFrameIndex = frame->bFrameIndex;
00328             ctrl->dwFrameInterval = interval_100ns;
00329 
00330             goto found;
00331           }
00332         }
00333       }
00334     }
00335   }
00336 
00337   return UVC_ERROR_INVALID_MODE;
00338 
00339 found:
00340   return uvc_probe_stream_ctrl(devh, ctrl);
00341 }
00342 
00349 uvc_error_t uvc_probe_stream_ctrl(
00350     uvc_device_handle_t *devh,
00351     uvc_stream_ctrl_t *ctrl) {
00352  
00353   uvc_claim_if(devh, ctrl->bInterfaceNumber);
00354 
00355   uvc_query_stream_ctrl(
00356       devh, ctrl, 1, UVC_SET_CUR
00357   );
00358 
00359   uvc_query_stream_ctrl(
00360       devh, ctrl, 1, UVC_GET_CUR
00361   );
00362 
00364   return UVC_SUCCESS;
00365 }
00366 
00370 void _uvc_swap_buffers(uvc_stream_handle_t *strmh) {
00371   uint8_t *tmp_buf;
00372 
00373   pthread_mutex_lock(&strmh->cb_mutex);
00374 
00375   /* swap the buffers */
00376   tmp_buf = strmh->holdbuf;
00377   strmh->hold_bytes = strmh->got_bytes;
00378   strmh->holdbuf = strmh->outbuf;
00379   strmh->outbuf = tmp_buf;
00380   strmh->hold_last_scr = strmh->last_scr;
00381   strmh->hold_pts = strmh->pts;
00382   strmh->hold_seq = strmh->seq;
00383 
00384   pthread_cond_broadcast(&strmh->cb_cond);
00385   pthread_mutex_unlock(&strmh->cb_mutex);
00386 
00387   strmh->seq++;
00388   strmh->got_bytes = 0;
00389   strmh->last_scr = 0;
00390   strmh->pts = 0;
00391 }
00392 
00403 void _uvc_process_payload(uvc_stream_handle_t *strmh, uint8_t *payload, size_t payload_len) {
00404   size_t header_len;
00405   uint8_t header_info;
00406   size_t data_len;
00407   struct libusb_iso_packet_descriptor *pkt;
00408 
00409   /* magic numbers for identifying header packets from some iSight cameras */
00410   static uint8_t isight_tag[] = {
00411     0x11, 0x22, 0x33, 0x44,
00412     0xde, 0xad, 0xbe, 0xef, 0xde, 0xad, 0xfa, 0xce
00413   };
00414 
00415   /* ignore empty payload transfers */
00416   if (payload_len == 0)
00417     return;
00418 
00419   /* Certain iSight cameras have strange behavior: They send header
00420    * information in a packet with no image data, and then the following
00421    * packets have only image data, with no more headers until the next frame.
00422    *
00423    * The iSight header: len(1), flags(1 or 2), 0x11223344(4),
00424    * 0xdeadbeefdeadface(8), ??(16)
00425    */
00426 
00427   if (strmh->devh->is_isight &&
00428       (payload_len < 14 || memcmp(isight_tag, payload + 2, sizeof(isight_tag))) &&
00429       (payload_len < 15 || memcmp(isight_tag, payload + 3, sizeof(isight_tag)))) {
00430     /* The payload transfer doesn't have any iSight magic, so it's all image data */
00431     header_len = 0;
00432     data_len = payload_len;
00433   } else {
00434     header_len = payload[0];
00435 
00436     if (header_len > payload_len) {
00437       UVC_DEBUG("bogus packet: actual_len=%zd, header_len=%zd\n", payload_len, header_len);
00438       return;
00439     }
00440 
00441     if (strmh->devh->is_isight)
00442       data_len = 0;
00443     else
00444       data_len = payload_len - header_len;
00445   }
00446 
00447   if (header_len < 2) {
00448     header_info = 0;
00449   } else {
00451     size_t variable_offset = 2;
00452 
00453     header_info = payload[1];
00454 
00455     if (header_info & 0x40) {
00456       UVC_DEBUG("bad packet: error bit set",);
00457       return;
00458     }
00459 
00460     if (strmh->fid != (header_info & 1) && strmh->got_bytes != 0) {
00461       /* The frame ID bit was flipped, but we have image data sitting
00462          around from prior transfers. This means the camera didn't send
00463          an EOF for the last transfer of the previous frame. */
00464       _uvc_swap_buffers(strmh);
00465     }
00466 
00467     strmh->fid = header_info & 1;
00468 
00469     if (header_info & (1 << 2)) {
00470       strmh->pts = DW_TO_INT(payload + variable_offset);
00471       variable_offset += 4;
00472     }
00473 
00474     if (header_info & (1 << 3)) {
00476       strmh->last_scr = DW_TO_INT(payload + variable_offset);
00477       variable_offset += 6;
00478     }
00479   }
00480 
00481   if (data_len > 0) {
00482     memcpy(strmh->outbuf + strmh->got_bytes, payload + header_len, data_len);
00483     strmh->got_bytes += data_len;
00484 
00485     if (header_info & (1 << 1)) {
00486       /* The EOF bit is set, so publish the complete frame */
00487       _uvc_swap_buffers(strmh);
00488     }
00489   }
00490 }
00491 
00500 void LIBUSB_CALL _uvc_stream_callback(struct libusb_transfer *transfer) {
00501   uvc_stream_handle_t *strmh = transfer->user_data;
00502 
00503   int resubmit = 1;
00504 
00505   switch (transfer->status) {
00506   case LIBUSB_TRANSFER_COMPLETED:
00507     if (transfer->num_iso_packets == 0) {
00508       /* This is a bulk mode transfer, so it just has one payload transfer */
00509       _uvc_process_payload(strmh, transfer->buffer, transfer->actual_length);
00510     } else {
00511       /* This is an isochronous mode transfer, so each packet has a payload transfer */
00512       int packet_id;
00513 
00514       for (packet_id = 0; packet_id < transfer->num_iso_packets; ++packet_id) {
00515         uint8_t *pktbuf;
00516         struct libusb_iso_packet_descriptor *pkt;
00517 
00518         pkt = transfer->iso_packet_desc + packet_id;
00519 
00520         if (pkt->status != 0) {
00521           UVC_DEBUG("bad packet (isochronous transfer); status: %d", pkt->status);
00522           continue;
00523         }
00524 
00525         pktbuf = libusb_get_iso_packet_buffer_simple(transfer, packet_id);
00526 
00527         _uvc_process_payload(strmh, pktbuf, pkt->actual_length);
00528 
00529       }
00530     }
00531     break;
00532   case LIBUSB_TRANSFER_CANCELLED: 
00533   case LIBUSB_TRANSFER_ERROR:
00534   case LIBUSB_TRANSFER_NO_DEVICE: {
00535     int i;
00536     UVC_DEBUG("transfer exception, status = %s", libusb_error_name(transfer->status));
00537     pthread_mutex_lock(&strmh->cb_mutex);
00538 
00539     /* Mark transfer as deleted. */
00540     for(i=0; i < strmh->num_transfer_bufs; i++) {
00541       if(strmh->transfers[i] == transfer) {
00542         UVC_DEBUG("Freeing transfer %d (%p)", i, transfer);
00543         free(transfer->buffer);
00544         libusb_free_transfer(transfer);
00545         strmh->transfers[i] = NULL;
00546         break;
00547       }
00548     }
00549     if(i == strmh->num_transfer_bufs ) {
00550       UVC_DEBUG("transfer %p not found; not freeing!", transfer);
00551     }
00552 
00553     resubmit = 0;
00554 
00555     pthread_cond_broadcast(&strmh->cb_cond);
00556     pthread_mutex_unlock(&strmh->cb_mutex);
00557 
00558     break;
00559   }
00560   case LIBUSB_TRANSFER_TIMED_OUT:
00561   case LIBUSB_TRANSFER_STALL:
00562   case LIBUSB_TRANSFER_OVERFLOW:
00563     UVC_DEBUG("retrying transfer, status = %d", transfer->status);
00564     break;
00565   }
00566   
00567   if (strmh->running && resubmit)
00568     libusb_submit_transfer(transfer);
00569 }
00570 
00581 uvc_error_t uvc_start_streaming(
00582     uvc_device_handle_t *devh,
00583     uvc_stream_ctrl_t *ctrl,
00584     uvc_frame_callback_t *cb,
00585     void *user_ptr,
00586     uint8_t flags,
00587     int num_transfer_buffers
00588 ) {
00589   uvc_error_t ret;
00590   uvc_stream_handle_t *strmh;
00591 
00592   ret = uvc_stream_open_ctrl(devh, &strmh, ctrl);
00593   if (ret != UVC_SUCCESS)
00594     return ret;
00595 
00596   ret = uvc_stream_start(strmh, cb, user_ptr, flags, num_transfer_buffers);
00597   if (ret != UVC_SUCCESS) {
00598       UVC_DEBUG("FAILED TO START STREAM: %i", ret);
00599     uvc_stream_close(strmh);
00600     return ret;
00601   }
00602     
00603   ctrl->handle = strmh;
00604 
00605   return UVC_SUCCESS;
00606 }
00607 
00608 static uvc_stream_handle_t *_uvc_get_stream_by_interface(uvc_device_handle_t *devh, int interface_idx) {
00609   uvc_stream_handle_t *strmh;
00610 
00611   DL_FOREACH(devh->streams, strmh) {
00612     if (strmh->stream_if->bInterfaceNumber == interface_idx)
00613       return strmh;
00614   }
00615 
00616   return NULL;
00617 }
00618 
00619 static uvc_streaming_interface_t *_uvc_get_stream_if(uvc_device_handle_t *devh, int interface_idx) {
00620   uvc_streaming_interface_t *stream_if;
00621 
00622   DL_FOREACH(devh->info->stream_ifs, stream_if) {
00623     if (stream_if->bInterfaceNumber == interface_idx)
00624       return stream_if;
00625   }
00626   
00627   return NULL;
00628 }
00629 
00637 uvc_error_t uvc_stream_open_ctrl(uvc_device_handle_t *devh, uvc_stream_handle_t **strmhp, uvc_stream_ctrl_t *ctrl) {
00638   /* Chosen frame and format descriptors */
00639   uvc_stream_handle_t *strmh = NULL;
00640   uvc_streaming_interface_t *stream_if;
00641   uvc_error_t ret;
00642 
00643   UVC_ENTER();
00644 
00645   if (_uvc_get_stream_by_interface(devh, ctrl->bInterfaceNumber) != NULL) {
00646     ret = UVC_ERROR_BUSY; /* Stream is already opened */
00647     goto fail;
00648   }
00649 
00650   stream_if = _uvc_get_stream_if(devh, ctrl->bInterfaceNumber);
00651   if (!stream_if) {
00652     ret = UVC_ERROR_INVALID_PARAM;
00653     goto fail;
00654   }
00655 
00656   strmh = calloc(1, sizeof(*strmh));
00657   if (!strmh) {
00658     ret = UVC_ERROR_NO_MEM;
00659     goto fail;
00660   }
00661   strmh->devh = devh;
00662   strmh->stream_if = stream_if;
00663   strmh->frame.library_owns_data = 1;
00664 
00665   ret = uvc_claim_if(strmh->devh, strmh->stream_if->bInterfaceNumber);
00666   if (ret != UVC_SUCCESS)
00667     goto fail;
00668 
00669   ret = uvc_stream_ctrl(strmh, ctrl);
00670   if (ret != UVC_SUCCESS)
00671     goto fail;
00672 
00673   // Set up the streaming status and data space
00674   strmh->running = 0;
00676   strmh->outbuf = malloc( LIBUVC_XFER_BUF_SIZE );
00677   strmh->holdbuf = malloc( LIBUVC_XFER_BUF_SIZE );
00678    
00679   pthread_mutex_init(&strmh->cb_mutex, NULL);
00680   pthread_cond_init(&strmh->cb_cond, NULL);
00681 
00682   DL_APPEND(devh->streams, strmh);
00683 
00684   *strmhp = strmh;
00685 
00686   UVC_EXIT(0);
00687   return UVC_SUCCESS;
00688 
00689 fail:
00690   if(strmh)
00691     free(strmh);
00692   UVC_EXIT(ret);
00693   return ret;
00694 }
00695 
00704 uvc_error_t uvc_stream_start(
00705     uvc_stream_handle_t *strmh,
00706     uvc_frame_callback_t *cb,
00707     void *user_ptr,
00708     uint8_t flags,
00709     int num_transfer_buffers
00710 ) {
00711   /* USB interface we'll be using */
00712   const struct libusb_interface *interface;
00713   int interface_id;
00714   char isochronous;
00715   uvc_frame_desc_t *frame_desc;
00716   uvc_format_desc_t *format_desc;
00717   uvc_stream_ctrl_t *ctrl;
00718   uvc_error_t ret;
00719   /* Total amount of data per transfer */
00720   size_t total_transfer_size;
00721   struct libusb_transfer *transfer;
00722   int transfer_id;
00723 
00724   ctrl = &strmh->cur_ctrl;
00725 
00726   UVC_ENTER();
00727 
00728   if (strmh->running) {
00729     UVC_EXIT(UVC_ERROR_BUSY);
00730     return UVC_ERROR_BUSY;
00731   }
00732 
00733   strmh->running = 1;
00734   strmh->seq = 0;
00735   strmh->fid = 0;
00736   strmh->pts = 0;
00737   strmh->last_scr = 0;
00738 
00739   frame_desc = uvc_find_frame_desc_stream(strmh, ctrl->bFormatIndex, ctrl->bFrameIndex);
00740   if (!frame_desc) {
00741     ret = UVC_ERROR_INVALID_PARAM;
00742     goto fail;
00743   }
00744   format_desc = frame_desc->parent;
00745 
00746   strmh->fourcc = uvc_frame_format_for_guid(format_desc->guidFormat);
00747 
00748   // Get the interface that provides the chosen format and frame configuration
00749   interface_id = strmh->stream_if->bInterfaceNumber;
00750   interface = &strmh->devh->info->config->interface[interface_id];
00751 
00752   /* A VS interface uses isochronous transfers iff it has multiple altsettings.
00753    * (UVC 1.5: 2.4.3. VideoStreaming Interface) */
00754   isochronous = interface->num_altsetting > 1;
00755 
00756   if (isochronous) {
00757     /* For isochronous streaming, we choose an appropriate altsetting for the endpoint
00758      * and set up several transfers */
00759     const struct libusb_interface_descriptor *altsetting;
00760     const struct libusb_endpoint_descriptor *endpoint;
00761     /* The greatest number of bytes that the device might provide, per packet, in this
00762      * configuration */
00763     size_t config_bytes_per_packet;
00764     /* Number of packets per transfer */
00765     size_t packets_per_transfer;
00766     /* Size of packet transferable from the chosen endpoint */
00767     size_t endpoint_bytes_per_packet;
00768     /* Index of the altsetting */
00769     int alt_idx, ep_idx;
00770     
00771     config_bytes_per_packet = strmh->cur_ctrl.dwMaxPayloadTransferSize;
00772 
00773     /* Go through the altsettings and find one whose packets are at least
00774      * as big as our format's maximum per-packet usage. Assume that the
00775      * packet sizes are increasing. */
00776     for (alt_idx = 0; alt_idx < interface->num_altsetting; alt_idx++) {
00777       altsetting = interface->altsetting + alt_idx;
00778       endpoint_bytes_per_packet = 0;
00779 
00780       /* Find the endpoint with the number specified in the VS header */
00781       for (ep_idx = 0; ep_idx < altsetting->bNumEndpoints; ep_idx++) {
00782         endpoint = altsetting->endpoint + ep_idx;
00783 
00784         if (endpoint->bEndpointAddress == format_desc->parent->bEndpointAddress) {
00785           endpoint_bytes_per_packet = endpoint->wMaxPacketSize;
00786           // wMaxPacketSize: [unused:2 (multiplier-1):3 size:11]
00787           endpoint_bytes_per_packet = (endpoint_bytes_per_packet & 0x07ff) *
00788                                       (((endpoint_bytes_per_packet >> 11) & 3) + 1);
00789           break;
00790         }
00791       }
00792 
00793       if (endpoint_bytes_per_packet >= config_bytes_per_packet) {
00794         /* Transfers will be at most one frame long: Divide the maximum frame size
00795          * by the size of the endpoint and round up */
00796         packets_per_transfer = (ctrl->dwMaxVideoFrameSize +
00797                                 endpoint_bytes_per_packet - 1) / endpoint_bytes_per_packet;
00798 
00799         /* But keep a reasonable limit: Otherwise we start dropping data */
00800         if (packets_per_transfer > 32)
00801           packets_per_transfer = 32;
00802         
00803         total_transfer_size = packets_per_transfer * endpoint_bytes_per_packet;
00804         break;
00805       }
00806     }
00807 
00808     /* If we searched through all the altsettings and found nothing usable */
00809     if (alt_idx == interface->num_altsetting) {
00810       ret = UVC_ERROR_INVALID_MODE;
00811       goto fail;
00812     }
00813 
00814     /* Select the altsetting */
00815     ret = libusb_set_interface_alt_setting(strmh->devh->usb_devh,
00816                                            altsetting->bInterfaceNumber,
00817                                            altsetting->bAlternateSetting);
00818     if (ret != UVC_SUCCESS) {
00819       UVC_DEBUG("libusb_set_interface_alt_setting failed",);
00820       goto fail;
00821     }
00822 
00823     /* Set up the transfers */
00824     strmh->num_transfer_bufs = num_transfer_buffers;
00825     strmh->transfers = malloc(sizeof(struct libusb_transfer *) * num_transfer_buffers);
00826     strmh->transfer_bufs = malloc(sizeof(struct uint8_t *) * num_transfer_buffers);
00827     for (transfer_id = 0; transfer_id < num_transfer_buffers; ++transfer_id)
00828     {
00829       transfer = libusb_alloc_transfer(packets_per_transfer);
00830       strmh->transfers[transfer_id] = transfer;      
00831       strmh->transfer_bufs[transfer_id] = malloc(total_transfer_size);
00832 
00833       libusb_fill_iso_transfer(
00834         transfer, strmh->devh->usb_devh, format_desc->parent->bEndpointAddress,
00835         strmh->transfer_bufs[transfer_id],
00836         total_transfer_size, packets_per_transfer, _uvc_stream_callback, (void*) strmh, 5000);
00837 
00838       libusb_set_iso_packet_lengths(transfer, endpoint_bytes_per_packet);
00839     }
00840   }
00841     
00842   else
00843   {
00844     strmh->num_transfer_bufs = num_transfer_buffers;
00845     strmh->transfers = malloc(sizeof(struct libusb_transfer *) * num_transfer_buffers);
00846     strmh->transfer_bufs = malloc(sizeof(struct uint8_t *) * num_transfer_buffers);
00847     for (transfer_id = 0; transfer_id < num_transfer_buffers; ++transfer_id)
00848     {
00849       transfer = libusb_alloc_transfer(0);
00850       strmh->transfers[transfer_id] = transfer;
00851       strmh->transfer_bufs[transfer_id] = malloc (
00852           strmh->cur_ctrl.dwMaxPayloadTransferSize );
00853       libusb_fill_bulk_transfer ( transfer, strmh->devh->usb_devh,
00854           format_desc->parent->bEndpointAddress,
00855           strmh->transfer_bufs[transfer_id],
00856           strmh->cur_ctrl.dwMaxPayloadTransferSize, _uvc_stream_callback,
00857           ( void* ) strmh, 5000 );
00858     }
00859   }
00860 
00861   strmh->user_cb = cb;
00862   strmh->user_ptr = user_ptr;
00863 
00864   /* If the user wants it, set up a thread that calls the user's function
00865    * with the contents of each frame.
00866    */
00867   if (cb)
00868   {
00869     pthread_create(&strmh->cb_thread, NULL, _uvc_user_caller, (void*) strmh);
00870   }
00871 
00872   for (transfer_id = 0; transfer_id < num_transfer_buffers; transfer_id++)
00873   {
00874     ret = libusb_submit_transfer(strmh->transfers[transfer_id]);
00875     if (ret != UVC_SUCCESS)
00876     {
00877       UVC_DEBUG("libusb_submit_transfer failed",);
00878       break;
00879     }
00880   }
00881 
00882   UVC_EXIT(ret);
00883   return ret;
00884 fail:
00885   strmh->running = 0;
00886   UVC_EXIT(ret);
00887   return ret;
00888 }
00889 
00895 void *_uvc_user_caller(void *arg) {
00896   uvc_stream_handle_t *strmh = (uvc_stream_handle_t *) arg;
00897 
00898   uint32_t last_seq = 0;
00899 
00900   do {
00901     pthread_mutex_lock(&strmh->cb_mutex);
00902 
00903     while (strmh->running && last_seq == strmh->hold_seq) {
00904       pthread_cond_wait(&strmh->cb_cond, &strmh->cb_mutex);
00905     }
00906 
00907     if (!strmh->running) {
00908       pthread_mutex_unlock(&strmh->cb_mutex);
00909       break;
00910     }
00911     
00912     last_seq = strmh->hold_seq;
00913     _uvc_populate_frame(strmh);
00914     
00915     pthread_mutex_unlock(&strmh->cb_mutex);
00916     
00917     strmh->user_cb(&strmh->frame, strmh->user_ptr);
00918   } while(1);
00919 
00920   return NULL; // return value ignored
00921 }
00922 
00927 void _uvc_populate_frame(uvc_stream_handle_t *strmh) {
00928   size_t alloc_size = strmh->cur_ctrl.dwMaxVideoFrameSize;
00929   uvc_frame_t *frame = &strmh->frame;
00930   uvc_frame_desc_t *frame_desc;
00931 
00937   frame_desc = uvc_find_frame_desc(strmh->devh, strmh->cur_ctrl.bFormatIndex,
00938                    strmh->cur_ctrl.bFrameIndex);
00939 
00940   frame->fourcc = strmh->fourcc;
00941   
00942   frame->width = frame_desc->wWidth;
00943   frame->height = frame_desc->wHeight;
00944   
00945 #pragma GCC diagnostic push
00946 #pragma GCC diagnostic ignored "-Wmultichar"
00947 #ifdef __APPLE__
00948 #pragma GCC diagnostic ignored "-Wfour-char-constants"
00949 #endif
00950   switch (frame->fourcc) {
00951   case '2YUY': /* YUY2 */
00952     frame->step = frame->width * 2;
00953     break;
00954   default:
00955     frame->step = 0;
00956     break;
00957   }
00958 #pragma GCC diagnostic pop
00959   
00960   /* copy the image data from the hold buffer to the frame (unnecessary extra buf?) */
00961   if (frame->data_bytes < strmh->hold_bytes) {
00962     frame->data = realloc(frame->data, strmh->hold_bytes);
00963     frame->data_bytes = strmh->hold_bytes;
00964   }
00965   memcpy(frame->data, strmh->holdbuf, frame->data_bytes);
00966   
00968 }
00969 
00977 uvc_error_t uvc_stream_get_frame(uvc_stream_handle_t *strmh,
00978               uvc_frame_t **frame,
00979               int32_t timeout_us) {
00980   time_t add_secs;
00981   time_t add_nsecs;
00982   struct timespec ts;
00983   struct timeval tv;
00984 
00985   if (!strmh->running)
00986     return UVC_ERROR_INVALID_PARAM;
00987 
00988   if (strmh->user_cb)
00989     return UVC_ERROR_CALLBACK_EXISTS;
00990 
00991   pthread_mutex_lock(&strmh->cb_mutex);
00992 
00993   if (strmh->last_polled_seq < strmh->hold_seq) {
00994     _uvc_populate_frame(strmh);
00995     *frame = &strmh->frame;
00996     strmh->last_polled_seq = strmh->hold_seq;
00997   } else if (timeout_us != -1) {
00998     if (timeout_us == 0) {
00999       pthread_cond_wait(&strmh->cb_cond, &strmh->cb_mutex);
01000     } else {
01001       add_secs = timeout_us / 1000000;
01002       add_nsecs = (timeout_us % 1000000) * 1000;
01003       ts.tv_sec = 0;
01004       ts.tv_nsec = 0;
01005 
01006 #if _POSIX_TIMERS > 0
01007       clock_gettime(CLOCK_REALTIME, &ts);
01008 #else
01009       gettimeofday(&tv, NULL);
01010       ts.tv_sec = tv.tv_sec;
01011       ts.tv_nsec = tv.tv_usec * 1000;
01012 #endif
01013 
01014       ts.tv_sec += add_secs;
01015       ts.tv_nsec += add_nsecs;
01016 
01017       pthread_cond_timedwait(&strmh->cb_cond, &strmh->cb_mutex, &ts);
01018     }
01019     
01020     if (strmh->last_polled_seq < strmh->hold_seq) {
01021       _uvc_populate_frame(strmh);
01022       *frame = &strmh->frame;
01023       strmh->last_polled_seq = strmh->hold_seq;
01024     } else {
01025       *frame = NULL;
01026     }
01027   } else {
01028     *frame = NULL;
01029   }
01030 
01031   pthread_mutex_unlock(&strmh->cb_mutex);
01032 
01033   return UVC_SUCCESS;
01034 }
01035 
01043 void uvc_stop_streaming(uvc_device_handle_t *devh) {
01044   uvc_stream_handle_t *strmh, *strmh_tmp;
01045 
01046   DL_FOREACH_SAFE(devh->streams, strmh, strmh_tmp) {
01047     uvc_stream_close(strmh);
01048   }
01049 }
01050 
01058 uvc_error_t uvc_stream_stop(uvc_stream_handle_t *strmh) {
01059     int i,timeout_s= 1,ret=UVC_SUCCESS;
01060     time_t add_secs;
01061     struct timespec ts;
01062     struct timeval tv;
01063     if (!strmh->running)
01064         return UVC_ERROR_INVALID_PARAM;
01065     
01066     strmh->running = 0;
01067     
01068     pthread_mutex_lock(&strmh->cb_mutex);
01069     
01070     for(i=0; i < strmh->num_transfer_bufs; i++) {
01071         if(strmh->transfers[i] != NULL) {
01072             int res = libusb_cancel_transfer(strmh->transfers[i]);
01073             if(res < 0) {
01074                 free(strmh->transfers[i]->buffer);
01075                 libusb_free_transfer(strmh->transfers[i]);
01076                 strmh->transfers[i] = NULL;
01077             }
01078         }
01079     }
01080 
01081 
01082     /* Wait for transfers to complete/cancel */
01083     do {
01084         for(i=0; i < strmh->num_transfer_bufs; i++) {
01085             if(strmh->transfers[i] != NULL)
01086                 break;
01087         }
01088         
01089         if(i == strmh->num_transfer_bufs )
01090             break;
01091         
01092         // this ones sometimes does not return.
01093         // pthread_cond_wait(&strmh->cb_cond, &strmh->cb_mutex);
01094         
01095         add_secs = timeout_s ;
01096         ts.tv_sec = 0;
01097         ts.tv_nsec = 0;
01098         
01099 #if _POSIX_TIMERS > 0
01100         clock_gettime(CLOCK_REALTIME, &ts);
01101 #else
01102         gettimeofday(&tv, NULL);
01103         ts.tv_sec = tv.tv_sec;
01104         ts.tv_nsec = tv.tv_usec * 1000;
01105 #endif
01106         
01107         ts.tv_sec += add_secs;
01108         
01109         if (ETIMEDOUT == pthread_cond_timedwait(&strmh->cb_cond, &strmh->cb_mutex, &ts)){
01110             ret = UVC_ERROR_TIMEOUT;
01111             break;
01112         }
01113     } while(1);
01114     
01115     // Kick the user thread awake
01116     pthread_cond_broadcast(&strmh->cb_cond);
01117     pthread_mutex_unlock(&strmh->cb_mutex);
01118     
01121     if (strmh->user_cb) {
01122         /* wait for the thread to stop (triggered by
01123          * LIBUSB_TRANSFER_CANCELLED transfer) */
01124         pthread_join(strmh->cb_thread, NULL);
01125     }
01126     
01127     return ret;
01128 }
01129 
01137 void uvc_stream_close(uvc_stream_handle_t *strmh) {
01138   if (strmh->running)
01139     uvc_stream_stop(strmh);
01140 
01141   uvc_release_if(strmh->devh, strmh->stream_if->bInterfaceNumber);
01142 
01143   if (strmh->frame.data)
01144     free(strmh->frame.data);
01145 
01146   free(strmh->outbuf);
01147   free(strmh->holdbuf);
01148 
01149   pthread_cond_destroy(&strmh->cb_cond);
01150   pthread_mutex_destroy(&strmh->cb_mutex);
01151 
01152   DL_DELETE(strmh->devh->streams, strmh);
01153   free(strmh);
01154 }


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Tue Jun 25 2019 19:54:39