diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 5d7d42dd421..380a4e5701f 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -346,7 +346,6 @@ namespace librealsense }); } } - auto dev = dynamic_cast(&get_device()); auto dev_name = (dev) ? dev->get_info(RS2_CAMERA_INFO_NAME) : ""; diff --git a/src/environment.cpp b/src/environment.cpp index cd221175c61..400b6c3cf05 100644 --- a/src/environment.cpp +++ b/src/environment.cpp @@ -48,7 +48,6 @@ namespace librealsense void extrinsics_graph::register_extrinsics(const stream_interface & from, const stream_interface & to, rs2_extrinsics extr) { auto lazy_extr = std::make_shared>([=]() {return extr; }); - //_external_extrinsics.push_back(lazy_extr); register_extrinsics(from, to, lazy_extr); } diff --git a/src/environment.h b/src/environment.h index 54471768024..ed6170a54a0 100644 --- a/src/environment.h +++ b/src/environment.h @@ -56,7 +56,6 @@ namespace librealsense std::atomic _locks_count; std::map>>> _extrinsics; - //std::vector>> _external_extrinsics; std::map> _streams; }; diff --git a/src/win7/win7-usb.cpp b/src/win7/win7-usb.cpp index 00220595542..d0a54793967 100644 --- a/src/win7/win7-usb.cpp +++ b/src/win7/win7-usb.cpp @@ -414,8 +414,6 @@ namespace librealsense void win7_usb_interface::reset_interrupt_pipe() const { - LOG_DEBUG(__FUNCTION__); - auto sts = WinUsb_ResetPipe(_interface_handle, _interrupt_pipe_id); if (!sts) throw winapi_error("WinUsb_ResetPipe failed!"); @@ -423,9 +421,7 @@ namespace librealsense void win7_usb_interface::reset_pipe(pipe_direction pipeDirection) const { - LOG_DEBUG(__FUNCTION__); - - BOOL sts; + BOOL sts{}; if (pipeDirection == pipe_direction::InPipe) { diff --git a/src/win7/winusb_uvc/winusb_uvc.cpp b/src/win7/winusb_uvc/winusb_uvc.cpp index 618b9722661..e61061e752d 100644 --- a/src/win7/winusb_uvc/winusb_uvc.cpp +++ b/src/win7/winusb_uvc/winusb_uvc.cpp @@ -26,20 +26,20 @@ struct frame; typedef librealsense::small_heap frames_archive; struct frame { - frame() {} + frame() {} - // We don't want the frames to be overwritten at any point - // (deallocate resets item to "default" that we want to avoid) - frame(const frame& other) {} - frame(frame&& other) {} - frame& operator=(const frame& other) { return *this; } + // We don't want the frames to be overwritten at any point + // (deallocate resets item to "default" that we want to avoid) + frame(const frame& other) {} + frame(frame&& other) {} + frame& operator=(const frame& other) { return *this; } - std::vector pixels; - librealsense::platform::frame_object fo; - frames_archive* owner; // Keep pointer to owner for light-deleter + std::vector pixels; + librealsense::platform::frame_object fo; + frames_archive* owner; // Keep pointer to owner for light-deleter }; void cleanup_frame(frame* ptr) { - if (ptr) ptr->owner->deallocate(ptr); + if (ptr) ptr->owner->deallocate(ptr); }; typedef void(*cleanup_ptr)(frame*); // Unique_ptr is used as the simplest RAII, with static deleter @@ -51,82 +51,82 @@ uvc_error_t winusb_uvc_parse_vs(winusb_uvc_device *dev, winusb_uvc_device_info_t void free_and_set_null(void* ptr) { - if (ptr) { - free(ptr); - ptr = nullptr; - } + if (ptr) { + free(ptr); + ptr = nullptr; + } } void delete_and_set_null(void* ptr) { - if (ptr) { - delete ptr; - ptr = nullptr; - } + if (ptr) { + delete ptr; + ptr = nullptr; + } } void delete_array_and_set_null(void* ptr) { - if (ptr) { - delete[] ptr; - ptr = nullptr; - } + if (ptr) { + delete[] ptr; + ptr = nullptr; + } } void winusb_uvc_free_device_info(winusb_uvc_device_info_t *info) { - uvc_input_terminal_t *input_term, *input_term_tmp; - uvc_processing_unit_t *proc_unit, *proc_unit_tmp; - uvc_extension_unit_t *ext_unit, *ext_unit_tmp; - - winusb_uvc_streaming_interface_t *stream_if, *stream_if_tmp; - uvc_format_desc_t *format, *format_tmp; - uvc_frame_desc_t *frame, *frame_tmp; - - DL_FOREACH_SAFE(info->ctrl_if.input_term_descs, input_term, input_term_tmp) { - DL_DELETE(info->ctrl_if.input_term_descs, input_term); - free_and_set_null(input_term); - } - - DL_FOREACH_SAFE(info->ctrl_if.processing_unit_descs, proc_unit, proc_unit_tmp) { - DL_DELETE(info->ctrl_if.processing_unit_descs, proc_unit); - free_and_set_null(proc_unit); - } - - DL_FOREACH_SAFE(info->ctrl_if.extension_unit_descs, ext_unit, ext_unit_tmp) { - DL_DELETE(info->ctrl_if.extension_unit_descs, ext_unit); - free_and_set_null(ext_unit); - } - - DL_FOREACH_SAFE(info->stream_ifs, stream_if, stream_if_tmp) { - DL_FOREACH_SAFE(stream_if->format_descs, format, format_tmp) { - DL_FOREACH_SAFE(format->frame_descs, frame, frame_tmp) { - free_and_set_null(frame->intervals); - - - DL_DELETE(format->frame_descs, frame); - free_and_set_null(frame); - } - - DL_DELETE(stream_if->format_descs, format); - free_and_set_null(format); - } - - DL_DELETE(info->stream_ifs, stream_if); - WinUsb_Free(stream_if->associateHandle); - free_and_set_null(stream_if); - } - - if (info->interfaces) - { - for (int i = 0; i < MAX_USB_INTERFACES; i++) - { - //std::cout << std::endl << __FUNCTION__ << ", iface.extra: " << (int)info->interfaces->iface[i].extra << std::endl; - - delete_array_and_set_null(info->interfaces->iface[i].extra); - } - delete_and_set_null(info->interfaces); - } + uvc_input_terminal_t *input_term, *input_term_tmp; + uvc_processing_unit_t *proc_unit, *proc_unit_tmp; + uvc_extension_unit_t *ext_unit, *ext_unit_tmp; + + winusb_uvc_streaming_interface_t *stream_if, *stream_if_tmp; + uvc_format_desc_t *format, *format_tmp; + uvc_frame_desc_t *frame, *frame_tmp; + + DL_FOREACH_SAFE(info->ctrl_if.input_term_descs, input_term, input_term_tmp) { + DL_DELETE(info->ctrl_if.input_term_descs, input_term); + free_and_set_null(input_term); + } + + DL_FOREACH_SAFE(info->ctrl_if.processing_unit_descs, proc_unit, proc_unit_tmp) { + DL_DELETE(info->ctrl_if.processing_unit_descs, proc_unit); + free_and_set_null(proc_unit); + } + + DL_FOREACH_SAFE(info->ctrl_if.extension_unit_descs, ext_unit, ext_unit_tmp) { + DL_DELETE(info->ctrl_if.extension_unit_descs, ext_unit); + free_and_set_null(ext_unit); + } + + DL_FOREACH_SAFE(info->stream_ifs, stream_if, stream_if_tmp) { + DL_FOREACH_SAFE(stream_if->format_descs, format, format_tmp) { + DL_FOREACH_SAFE(format->frame_descs, frame, frame_tmp) { + free_and_set_null(frame->intervals); + + + DL_DELETE(format->frame_descs, frame); + free_and_set_null(frame); + } + + DL_DELETE(stream_if->format_descs, format); + free_and_set_null(format); + } + + DL_DELETE(info->stream_ifs, stream_if); + WinUsb_Free(stream_if->associateHandle); + free_and_set_null(stream_if); + } + + if (info->interfaces) + { + for (int i = 0; i < MAX_USB_INTERFACES; i++) + { + //std::cout << std::endl << __FUNCTION__ << ", iface.extra: " << (int)info->interfaces->iface[i].extra << std::endl; + + delete_array_and_set_null(info->interfaces->iface[i].extra); + } + delete_and_set_null(info->interfaces); + } } @@ -136,36 +136,36 @@ void winusb_uvc_free_device_info(winusb_uvc_device_info_t *info) { * @ingroup device */ uvc_error_t uvc_parse_vc_header(winusb_uvc_device *dev, - winusb_uvc_device_info_t *info, - const unsigned char *block, size_t block_size) { - size_t i; - uvc_error_t scan_ret, ret = UVC_SUCCESS; - - - info->ctrl_if.bcdUVC = SW_TO_SHORT(&block[3]); - - switch (info->ctrl_if.bcdUVC) { - case 0x0100: - info->ctrl_if.dwClockFrequency = DW_TO_INT(block + 7); - case 0x010a: - info->ctrl_if.dwClockFrequency = DW_TO_INT(block + 7); - case 0x0110: - case 0x0150: - info->ctrl_if.dwClockFrequency = 0; - break; - default: - return UVC_ERROR_NOT_SUPPORTED; - } - - for (i = 12; i < block_size; ++i) { - scan_ret = winusb_uvc_scan_streaming(dev, info, block[i]); - if (scan_ret != UVC_SUCCESS) { - ret = scan_ret; - break; - } - } - - return ret; + winusb_uvc_device_info_t *info, + const unsigned char *block, size_t block_size) { + size_t i; + uvc_error_t scan_ret, ret = UVC_SUCCESS; + + + info->ctrl_if.bcdUVC = SW_TO_SHORT(&block[3]); + + switch (info->ctrl_if.bcdUVC) { + case 0x0100: + info->ctrl_if.dwClockFrequency = DW_TO_INT(block + 7); + case 0x010a: + info->ctrl_if.dwClockFrequency = DW_TO_INT(block + 7); + case 0x0110: + case 0x0150: + info->ctrl_if.dwClockFrequency = 0; + break; + default: + return UVC_ERROR_NOT_SUPPORTED; + } + + for (i = 12; i < block_size; ++i) { + scan_ret = winusb_uvc_scan_streaming(dev, info, block[i]); + if (scan_ret != UVC_SUCCESS) { + ret = scan_ret; + break; + } + } + + return ret; } /** @internal @@ -173,30 +173,30 @@ uvc_error_t uvc_parse_vc_header(winusb_uvc_device *dev, * @ingroup device */ uvc_error_t uvc_parse_vc_input_terminal(winusb_uvc_device *dev, - winusb_uvc_device_info_t *info, - const unsigned char *block, size_t block_size) { - uvc_input_terminal_t *term; - size_t i; + winusb_uvc_device_info_t *info, + const unsigned char *block, size_t block_size) { + uvc_input_terminal_t *term; + size_t i; - /* only supporting camera-type input terminals */ - if (SW_TO_SHORT(&block[4]) != UVC_ITT_CAMERA) { - return UVC_SUCCESS; - } + /* only supporting camera-type input terminals */ + if (SW_TO_SHORT(&block[4]) != UVC_ITT_CAMERA) { + return UVC_SUCCESS; + } - term = (uvc_input_terminal_t *)calloc(1, sizeof(*term)); + term = (uvc_input_terminal_t *)calloc(1, sizeof(*term)); - term->bTerminalID = block[3]; - term->wTerminalType = (uvc_it_type)SW_TO_SHORT(&block[4]); - term->wObjectiveFocalLengthMin = SW_TO_SHORT(&block[8]); - term->wObjectiveFocalLengthMax = SW_TO_SHORT(&block[10]); - term->wOcularFocalLength = SW_TO_SHORT(&block[12]); + term->bTerminalID = block[3]; + term->wTerminalType = (uvc_it_type)SW_TO_SHORT(&block[4]); + term->wObjectiveFocalLengthMin = SW_TO_SHORT(&block[8]); + term->wObjectiveFocalLengthMax = SW_TO_SHORT(&block[10]); + term->wOcularFocalLength = SW_TO_SHORT(&block[12]); - for (i = 14 + block[14]; i >= 15; --i) - term->bmControls = block[i] + (term->bmControls << 8); + for (i = 14 + block[14]; i >= 15; --i) + term->bmControls = block[i] + (term->bmControls << 8); - DL_APPEND(info->ctrl_if.input_term_descs, term); + DL_APPEND(info->ctrl_if.input_term_descs, term); - return UVC_SUCCESS; + return UVC_SUCCESS; } /** @internal @@ -204,21 +204,21 @@ uvc_error_t uvc_parse_vc_input_terminal(winusb_uvc_device *dev, * @ingroup device */ uvc_error_t uvc_parse_vc_processing_unit(winusb_uvc_device *dev, - winusb_uvc_device_info_t *info, - const unsigned char *block, size_t block_size) { - uvc_processing_unit_t *unit; - size_t i; + winusb_uvc_device_info_t *info, + const unsigned char *block, size_t block_size) { + uvc_processing_unit_t *unit; + size_t i; - unit = (uvc_processing_unit_t *)calloc(1, sizeof(*unit)); - unit->bUnitID = block[3]; - unit->bSourceID = block[4]; + unit = (uvc_processing_unit_t *)calloc(1, sizeof(*unit)); + unit->bUnitID = block[3]; + unit->bSourceID = block[4]; - for (i = 7 + block[7]; i >= 8; --i) - unit->bmControls = block[i] + (unit->bmControls << 8); + for (i = 7 + block[7]; i >= 8; --i) + unit->bmControls = block[i] + (unit->bmControls << 8); - DL_APPEND(info->ctrl_if.processing_unit_descs, unit); + DL_APPEND(info->ctrl_if.processing_unit_descs, unit); - return UVC_SUCCESS; + return UVC_SUCCESS; } /** @internal @@ -226,16 +226,16 @@ uvc_error_t uvc_parse_vc_processing_unit(winusb_uvc_device *dev, * @ingroup device */ uvc_error_t uvc_parse_vc_selector_unit(winusb_uvc_device *dev, - winusb_uvc_device_info_t *info, - const unsigned char *block, size_t block_size) { - uvc_selector_unit_t *unit; + winusb_uvc_device_info_t *info, + const unsigned char *block, size_t block_size) { + uvc_selector_unit_t *unit; - unit = (uvc_selector_unit_t *)calloc(1, sizeof(*unit)); - unit->bUnitID = block[3]; + unit = (uvc_selector_unit_t *)calloc(1, sizeof(*unit)); + unit->bUnitID = block[3]; - DL_APPEND(info->ctrl_if.selector_unit_descs, unit); + DL_APPEND(info->ctrl_if.selector_unit_descs, unit); - return UVC_SUCCESS; + return UVC_SUCCESS; } /** @internal @@ -243,26 +243,26 @@ uvc_error_t uvc_parse_vc_selector_unit(winusb_uvc_device *dev, * @ingroup device */ uvc_error_t uvc_parse_vc_extension_unit(winusb_uvc_device *dev, - winusb_uvc_device_info_t *info, - const unsigned char *block, size_t block_size) { - uvc_extension_unit_t *unit = (uvc_extension_unit_t *)calloc(1, sizeof(*unit)); - const uint8_t *start_of_controls; - int size_of_controls, num_in_pins; - int i; + winusb_uvc_device_info_t *info, + const unsigned char *block, size_t block_size) { + uvc_extension_unit_t *unit = (uvc_extension_unit_t *)calloc(1, sizeof(*unit)); + const uint8_t *start_of_controls; + int size_of_controls, num_in_pins; + int i; - unit->bUnitID = block[3]; - memcpy(unit->guidExtensionCode, &block[4], 16); + unit->bUnitID = block[3]; + memcpy(unit->guidExtensionCode, &block[4], 16); - num_in_pins = block[21]; - size_of_controls = block[22 + num_in_pins]; - start_of_controls = &block[23 + num_in_pins]; + num_in_pins = block[21]; + size_of_controls = block[22 + num_in_pins]; + start_of_controls = &block[23 + num_in_pins]; - for (i = size_of_controls - 1; i >= 0; --i) - unit->bmControls = start_of_controls[i] + (unit->bmControls << 8); + for (i = size_of_controls - 1; i >= 0; --i) + unit->bmControls = start_of_controls[i] + (unit->bmControls << 8); - DL_APPEND(info->ctrl_if.extension_unit_descs, unit); + DL_APPEND(info->ctrl_if.extension_unit_descs, unit); - return UVC_SUCCESS; + return UVC_SUCCESS; } /** @internal @@ -270,90 +270,90 @@ uvc_error_t uvc_parse_vc_extension_unit(winusb_uvc_device *dev, * @ingroup device */ uvc_error_t uvc_parse_vc( - winusb_uvc_device *dev, - winusb_uvc_device_info_t *info, - const unsigned char *block, size_t block_size) { - int descriptor_subtype; - uvc_error_t ret = UVC_SUCCESS; - - - if (block[1] != 36) { // not a CS_INTERFACE descriptor?? - return UVC_SUCCESS; // UVC_ERROR_INVALID_DEVICE; - } - - descriptor_subtype = block[2]; - - switch (descriptor_subtype) { - case UVC_VC_HEADER: - ret = uvc_parse_vc_header(dev, info, block, block_size); - break; - case UVC_VC_INPUT_TERMINAL: - ret = uvc_parse_vc_input_terminal(dev, info, block, block_size); - break; - case UVC_VC_OUTPUT_TERMINAL: - break; - case UVC_VC_SELECTOR_UNIT: - ret = uvc_parse_vc_selector_unit(dev, info, block, block_size); - break; - case UVC_VC_PROCESSING_UNIT: - ret = uvc_parse_vc_processing_unit(dev, info, block, block_size); - break; - case UVC_VC_EXTENSION_UNIT: - ret = uvc_parse_vc_extension_unit(dev, info, block, block_size); - break; - default: - ret = UVC_ERROR_INVALID_DEVICE; - } - - return ret; + winusb_uvc_device *dev, + winusb_uvc_device_info_t *info, + const unsigned char *block, size_t block_size) { + int descriptor_subtype; + uvc_error_t ret = UVC_SUCCESS; + + + if (block[1] != 36) { // not a CS_INTERFACE descriptor?? + return UVC_SUCCESS; // UVC_ERROR_INVALID_DEVICE; + } + + descriptor_subtype = block[2]; + + switch (descriptor_subtype) { + case UVC_VC_HEADER: + ret = uvc_parse_vc_header(dev, info, block, block_size); + break; + case UVC_VC_INPUT_TERMINAL: + ret = uvc_parse_vc_input_terminal(dev, info, block, block_size); + break; + case UVC_VC_OUTPUT_TERMINAL: + break; + case UVC_VC_SELECTOR_UNIT: + ret = uvc_parse_vc_selector_unit(dev, info, block, block_size); + break; + case UVC_VC_PROCESSING_UNIT: + ret = uvc_parse_vc_processing_unit(dev, info, block, block_size); + break; + case UVC_VC_EXTENSION_UNIT: + ret = uvc_parse_vc_extension_unit(dev, info, block, block_size); + break; + default: + ret = UVC_ERROR_INVALID_DEVICE; + } + + return ret; } uvc_error_t winusb_uvc_scan_control(winusb_uvc_device *dev, winusb_uvc_device_info_t *info) { - USB_INTERFACE_DESCRIPTOR *if_desc; - uvc_error_t parse_ret, ret; - int interface_idx; - const unsigned char *buffer; - size_t buffer_left, block_size; + USB_INTERFACE_DESCRIPTOR *if_desc; + uvc_error_t parse_ret, ret; + int interface_idx; + const unsigned char *buffer; + size_t buffer_left, block_size; - ret = UVC_SUCCESS; - if_desc = NULL; + ret = UVC_SUCCESS; + if_desc = NULL; - for (interface_idx = 0; interface_idx < MAX_USB_INTERFACES; ++interface_idx) { - if_desc = &info->interfaces->iface[interface_idx].desc; + for (interface_idx = 0; interface_idx < MAX_USB_INTERFACES; ++interface_idx) { + if_desc = &info->interfaces->iface[interface_idx].desc; - if (if_desc->bInterfaceClass == 14 && if_desc->bInterfaceSubClass == 1) // Video, Control - break; + if (if_desc->bInterfaceClass == 14 && if_desc->bInterfaceSubClass == 1) // Video, Control + break; - if_desc = NULL; - } + if_desc = NULL; + } - if (if_desc == NULL) { - return UVC_ERROR_INVALID_DEVICE; - } + if (if_desc == NULL) { + return UVC_ERROR_INVALID_DEVICE; + } - info->ctrl_if.bInterfaceNumber = interface_idx; - if (if_desc->bNumEndpoints != 0) { - info->ctrl_if.bEndpointAddress = info->interfaces->iface[interface_idx].endpoints[0].bEndpointAddress; - } + info->ctrl_if.bInterfaceNumber = interface_idx; + if (if_desc->bNumEndpoints != 0) { + info->ctrl_if.bEndpointAddress = info->interfaces->iface[interface_idx].endpoints[0].bEndpointAddress; + } - buffer = info->interfaces->iface[interface_idx].extra; - buffer_left = info->interfaces->iface[interface_idx].extraLength; + buffer = info->interfaces->iface[interface_idx].extra; + buffer_left = info->interfaces->iface[interface_idx].extraLength; - while (buffer_left >= 3) { // parseX needs to see buf[0,2] = length,type - block_size = buffer[0]; - //printf("%d %x %d\n", buffer[0], buffer[1],buffer_left); - parse_ret = uvc_parse_vc(dev, info, buffer, block_size); + while (buffer_left >= 3) { // parseX needs to see buf[0,2] = length,type + block_size = buffer[0]; + //printf("%d %x %d\n", buffer[0], buffer[1],buffer_left); + parse_ret = uvc_parse_vc(dev, info, buffer, block_size); - if (parse_ret != UVC_SUCCESS) { - ret = parse_ret; - break; - } + if (parse_ret != UVC_SUCCESS) { + ret = parse_ret; + break; + } - buffer_left -= block_size; - buffer += block_size; - } + buffer_left -= block_size; + buffer += block_size; + } - return ret; + return ret; } /** @internal @@ -361,42 +361,42 @@ uvc_error_t winusb_uvc_scan_control(winusb_uvc_device *dev, winusb_uvc_device_in * @ingroup device */ uvc_error_t winusb_uvc_scan_streaming(winusb_uvc_device *dev, - winusb_uvc_device_info_t *info, - int interface_idx) { - const struct winusb_uvc_interface *if_desc; - const unsigned char *buffer; - int buffer_left, block_size; - uvc_error_t ret, parse_ret; - winusb_uvc_streaming_interface_t *stream_if; - - - ret = UVC_SUCCESS; - - if_desc = &(info->interfaces->iface[interface_idx]); - buffer = if_desc->extra; - buffer_left = if_desc->extraLength - sizeof(USB_INTERFACE_DESCRIPTOR); - - stream_if = (winusb_uvc_streaming_interface_t *)calloc(1, sizeof(*stream_if)); - memset(stream_if, 0, sizeof(winusb_uvc_streaming_interface_t)); - stream_if->parent = info; - stream_if->bInterfaceNumber = if_desc->desc.bInterfaceNumber; - DL_APPEND(info->stream_ifs, stream_if); - - while (buffer_left >= 3) { - block_size = buffer[0]; - parse_ret = winusb_uvc_parse_vs(dev, info, stream_if, buffer, block_size); - - if (parse_ret != UVC_SUCCESS) { - ret = parse_ret; - break; - } - - buffer_left -= block_size; - //printf("%d %x %x %d\n", buffer[0], buffer[1], buffer[2], buffer_left); - buffer += block_size; - } - - return ret; + winusb_uvc_device_info_t *info, + int interface_idx) { + const struct winusb_uvc_interface *if_desc; + const unsigned char *buffer; + int buffer_left, block_size; + uvc_error_t ret, parse_ret; + winusb_uvc_streaming_interface_t *stream_if; + + + ret = UVC_SUCCESS; + + if_desc = &(info->interfaces->iface[interface_idx]); + buffer = if_desc->extra; + buffer_left = if_desc->extraLength - sizeof(USB_INTERFACE_DESCRIPTOR); + + stream_if = (winusb_uvc_streaming_interface_t *)calloc(1, sizeof(*stream_if)); + memset(stream_if, 0, sizeof(winusb_uvc_streaming_interface_t)); + stream_if->parent = info; + stream_if->bInterfaceNumber = if_desc->desc.bInterfaceNumber; + DL_APPEND(info->stream_ifs, stream_if); + + while (buffer_left >= 3) { + block_size = buffer[0]; + parse_ret = winusb_uvc_parse_vs(dev, info, stream_if, buffer, block_size); + + if (parse_ret != UVC_SUCCESS) { + ret = parse_ret; + break; + } + + buffer_left -= block_size; + //printf("%d %x %x %d\n", buffer[0], buffer[1], buffer[2], buffer_left); + buffer += block_size; + } + + return ret; } /** @internal @@ -404,13 +404,13 @@ uvc_error_t winusb_uvc_scan_streaming(winusb_uvc_device *dev, * @ingroup device */ uvc_error_t winusb_uvc_parse_vs_input_header(winusb_uvc_streaming_interface_t *stream_if, - const unsigned char *block, - size_t block_size) { + const unsigned char *block, + size_t block_size) { - stream_if->bEndpointAddress = block[6] & 0x8f; - stream_if->bTerminalLink = block[8]; + stream_if->bEndpointAddress = block[6] & 0x8f; + stream_if->bTerminalLink = block[8]; - return UVC_SUCCESS; + return UVC_SUCCESS; } /** @internal @@ -418,27 +418,27 @@ uvc_error_t winusb_uvc_parse_vs_input_header(winusb_uvc_streaming_interface_t *s * @ingroup device */ uvc_error_t winusb_uvc_parse_vs_format_uncompressed(winusb_uvc_streaming_interface_t *stream_if, - const unsigned char *block, - size_t block_size) { - - uvc_format_desc_t *format = (uvc_format_desc_t *)calloc(1, sizeof(*format)); - memset(format, 0, sizeof(uvc_format_desc_t)); - format->parent = stream_if; - format->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; - format->bFormatIndex = block[3]; - //format->bmCapabilities = block[4]; - //format->bmFlags = block[5]; - memcpy(format->guidFormat, &block[5], 16); - format->bBitsPerPixel = block[21]; - format->bDefaultFrameIndex = block[22]; - format->bAspectRatioX = block[23]; - format->bAspectRatioY = block[24]; - format->bmInterlaceFlags = block[25]; - format->bCopyProtect = block[26]; - - DL_APPEND(stream_if->format_descs, format); - - return UVC_SUCCESS; + const unsigned char *block, + size_t block_size) { + + uvc_format_desc_t *format = (uvc_format_desc_t *)calloc(1, sizeof(*format)); + memset(format, 0, sizeof(uvc_format_desc_t)); + format->parent = stream_if; + format->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; + format->bFormatIndex = block[3]; + //format->bmCapabilities = block[4]; + //format->bmFlags = block[5]; + memcpy(format->guidFormat, &block[5], 16); + format->bBitsPerPixel = block[21]; + format->bDefaultFrameIndex = block[22]; + format->bAspectRatioX = block[23]; + format->bAspectRatioY = block[24]; + format->bmInterlaceFlags = block[25]; + format->bCopyProtect = block[26]; + + DL_APPEND(stream_if->format_descs, format); + + return UVC_SUCCESS; } /** @internal @@ -446,27 +446,27 @@ uvc_error_t winusb_uvc_parse_vs_format_uncompressed(winusb_uvc_streaming_interfa * @ingroup device */ uvc_error_t winusb_uvc_parse_vs_frame_format(winusb_uvc_streaming_interface_t *stream_if, - const unsigned char *block, - size_t block_size) { - - uvc_format_desc_t *format = (uvc_format_desc_t *)malloc(sizeof(uvc_format_desc_t)); - memset(format, 0, sizeof(uvc_format_desc_t)); - format->parent = stream_if; - format->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; - format->bFormatIndex = block[3]; - format->bNumFrameDescriptors = block[4]; - memcpy(format->guidFormat, &block[5], 16); - format->bBitsPerPixel = block[21]; - format->bDefaultFrameIndex = block[22]; - format->bAspectRatioX = block[23]; - format->bAspectRatioY = block[24]; - format->bmInterlaceFlags = block[25]; - format->bCopyProtect = block[26]; - format->bVariableSize = block[27]; - - DL_APPEND(stream_if->format_descs, format); - - return UVC_SUCCESS; + const unsigned char *block, + size_t block_size) { + + uvc_format_desc_t *format = (uvc_format_desc_t *)malloc(sizeof(uvc_format_desc_t)); + memset(format, 0, sizeof(uvc_format_desc_t)); + format->parent = stream_if; + format->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; + format->bFormatIndex = block[3]; + format->bNumFrameDescriptors = block[4]; + memcpy(format->guidFormat, &block[5], 16); + format->bBitsPerPixel = block[21]; + format->bDefaultFrameIndex = block[22]; + format->bAspectRatioX = block[23]; + format->bAspectRatioY = block[24]; + format->bmInterlaceFlags = block[25]; + format->bCopyProtect = block[26]; + format->bVariableSize = block[27]; + + DL_APPEND(stream_if->format_descs, format); + + return UVC_SUCCESS; } /** @internal @@ -474,26 +474,26 @@ uvc_error_t winusb_uvc_parse_vs_frame_format(winusb_uvc_streaming_interface_t *s * @ingroup device */ uvc_error_t winusb_uvc_parse_vs_format_mjpeg(winusb_uvc_streaming_interface_t *stream_if, - const unsigned char *block, - size_t block_size) { - - uvc_format_desc_t *format = (uvc_format_desc_t *)calloc(1, sizeof(*format)); - - format->parent = stream_if; - format->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; - format->bFormatIndex = block[3]; - memcpy(format->fourccFormat, "MJPG", 4); - format->bmFlags = block[5]; - format->bBitsPerPixel = 0; - format->bDefaultFrameIndex = block[6]; - format->bAspectRatioX = block[7]; - format->bAspectRatioY = block[8]; - format->bmInterlaceFlags = block[9]; - format->bCopyProtect = block[10]; - - DL_APPEND(stream_if->format_descs, format); - - return UVC_SUCCESS; + const unsigned char *block, + size_t block_size) { + + uvc_format_desc_t *format = (uvc_format_desc_t *)calloc(1, sizeof(*format)); + + format->parent = stream_if; + format->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; + format->bFormatIndex = block[3]; + memcpy(format->fourccFormat, "MJPG", 4); + format->bmFlags = block[5]; + format->bBitsPerPixel = 0; + format->bDefaultFrameIndex = block[6]; + format->bAspectRatioX = block[7]; + format->bAspectRatioY = block[8]; + format->bmInterlaceFlags = block[9]; + format->bCopyProtect = block[10]; + + DL_APPEND(stream_if->format_descs, format); + + return UVC_SUCCESS; } /** @internal @@ -501,49 +501,49 @@ uvc_error_t winusb_uvc_parse_vs_format_mjpeg(winusb_uvc_streaming_interface_t *s * @ingroup device */ uvc_error_t winusb_uvc_parse_vs_frame_frame(winusb_uvc_streaming_interface_t *stream_if, - const unsigned char *block, - size_t block_size) { - uvc_format_desc_t *format; - uvc_frame_desc_t *frame; - - const unsigned char *p; - int i; - - format = stream_if->format_descs->prev; - frame = (uvc_frame_desc_t *)malloc(sizeof(uvc_frame_desc_t)); - memset(frame, 0, sizeof(uvc_frame_desc_t)); - frame->parent = format; - - frame->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; - frame->bFrameIndex = block[3]; - frame->bmCapabilities = block[4]; - frame->wWidth = block[5] + (block[6] << 8); - frame->wHeight = block[7] + (block[8] << 8); - frame->dwMinBitRate = DW_TO_INT(&block[9]); - frame->dwMaxBitRate = DW_TO_INT(&block[13]); - frame->dwDefaultFrameInterval = DW_TO_INT(&block[17]); - frame->bFrameIntervalType = block[21]; - frame->dwBytesPerLine = DW_TO_INT(&block[22]); - - if (block[21] == 0) { - frame->dwMinFrameInterval = DW_TO_INT(&block[26]); - frame->dwMaxFrameInterval = DW_TO_INT(&block[30]); - frame->dwFrameIntervalStep = DW_TO_INT(&block[34]); - } - else { - frame->intervals = (uint32_t *)malloc((block[21] + 1) * sizeof(uint32_t)); - p = &block[26]; - - for (i = 0; i < block[21]; ++i) { - frame->intervals[i] = DW_TO_INT(p); - p += 4; - } - frame->intervals[block[21]] = 0; - } - - DL_APPEND(format->frame_descs, frame); - - return UVC_SUCCESS; + const unsigned char *block, + size_t block_size) { + uvc_format_desc_t *format; + uvc_frame_desc_t *frame; + + const unsigned char *p; + int i; + + format = stream_if->format_descs->prev; + frame = (uvc_frame_desc_t *)malloc(sizeof(uvc_frame_desc_t)); + memset(frame, 0, sizeof(uvc_frame_desc_t)); + frame->parent = format; + + frame->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; + frame->bFrameIndex = block[3]; + frame->bmCapabilities = block[4]; + frame->wWidth = block[5] + (block[6] << 8); + frame->wHeight = block[7] + (block[8] << 8); + frame->dwMinBitRate = DW_TO_INT(&block[9]); + frame->dwMaxBitRate = DW_TO_INT(&block[13]); + frame->dwDefaultFrameInterval = DW_TO_INT(&block[17]); + frame->bFrameIntervalType = block[21]; + frame->dwBytesPerLine = DW_TO_INT(&block[22]); + + if (block[21] == 0) { + frame->dwMinFrameInterval = DW_TO_INT(&block[26]); + frame->dwMaxFrameInterval = DW_TO_INT(&block[30]); + frame->dwFrameIntervalStep = DW_TO_INT(&block[34]); + } + else { + frame->intervals = (uint32_t *)malloc((block[21] + 1) * sizeof(uint32_t)); + p = &block[26]; + + for (i = 0; i < block[21]; ++i) { + frame->intervals[i] = DW_TO_INT(p); + p += 4; + } + frame->intervals[block[21]] = 0; + } + + DL_APPEND(format->frame_descs, frame); + + return UVC_SUCCESS; } /** @internal @@ -551,49 +551,49 @@ uvc_error_t winusb_uvc_parse_vs_frame_frame(winusb_uvc_streaming_interface_t *st * @ingroup device */ uvc_error_t winusb_uvc_parse_vs_frame_uncompressed(winusb_uvc_streaming_interface_t *stream_if, - const unsigned char *block, - size_t block_size) { - uvc_format_desc_t *format; - uvc_frame_desc_t *frame; - - const unsigned char *p; - int i; - - format = stream_if->format_descs->prev; - frame = (uvc_frame_desc_t *)calloc(1, sizeof(*frame)); - memset(frame, 0, sizeof(uvc_frame_desc_t)); - frame->parent = format; - - frame->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; - frame->bFrameIndex = block[3]; - frame->bmCapabilities = block[4]; - frame->wWidth = block[5] + (block[6] << 8); - frame->wHeight = block[7] + (block[8] << 8); - frame->dwMinBitRate = DW_TO_INT(&block[9]); - frame->dwMaxBitRate = DW_TO_INT(&block[13]); - frame->dwMaxVideoFrameBufferSize = DW_TO_INT(&block[17]); - frame->dwDefaultFrameInterval = DW_TO_INT(&block[21]); - frame->bFrameIntervalType = block[25]; - - if (block[25] == 0) { - frame->dwMinFrameInterval = DW_TO_INT(&block[26]); - frame->dwMaxFrameInterval = DW_TO_INT(&block[30]); - frame->dwFrameIntervalStep = DW_TO_INT(&block[34]); - } - else { - frame->intervals = (uint32_t *)calloc(block[25] + 1, sizeof(frame->intervals[0])); - p = &block[26]; - - for (i = 0; i < block[25]; ++i) { - frame->intervals[i] = DW_TO_INT(p); - p += 4; - } - frame->intervals[block[25]] = 0; - } - - DL_APPEND(format->frame_descs, frame); - - return UVC_SUCCESS; + const unsigned char *block, + size_t block_size) { + uvc_format_desc_t *format; + uvc_frame_desc_t *frame; + + const unsigned char *p; + int i; + + format = stream_if->format_descs->prev; + frame = (uvc_frame_desc_t *)calloc(1, sizeof(*frame)); + memset(frame, 0, sizeof(uvc_frame_desc_t)); + frame->parent = format; + + frame->bDescriptorSubtype = (uvc_vs_desc_subtype)block[2]; + frame->bFrameIndex = block[3]; + frame->bmCapabilities = block[4]; + frame->wWidth = block[5] + (block[6] << 8); + frame->wHeight = block[7] + (block[8] << 8); + frame->dwMinBitRate = DW_TO_INT(&block[9]); + frame->dwMaxBitRate = DW_TO_INT(&block[13]); + frame->dwMaxVideoFrameBufferSize = DW_TO_INT(&block[17]); + frame->dwDefaultFrameInterval = DW_TO_INT(&block[21]); + frame->bFrameIntervalType = block[25]; + + if (block[25] == 0) { + frame->dwMinFrameInterval = DW_TO_INT(&block[26]); + frame->dwMaxFrameInterval = DW_TO_INT(&block[30]); + frame->dwFrameIntervalStep = DW_TO_INT(&block[34]); + } + else { + frame->intervals = (uint32_t *)calloc(block[25] + 1, sizeof(frame->intervals[0])); + p = &block[26]; + + for (i = 0; i < block[25]; ++i) { + frame->intervals[i] = DW_TO_INT(p); + p += 4; + } + frame->intervals[block[25]] = 0; + } + + DL_APPEND(format->frame_descs, frame); + + return UVC_SUCCESS; } /** @internal @@ -601,79 +601,79 @@ uvc_error_t winusb_uvc_parse_vs_frame_uncompressed(winusb_uvc_streaming_interfac * @ingroup device */ uvc_error_t winusb_uvc_parse_vs( - winusb_uvc_device *dev, - winusb_uvc_device_info_t *info, - winusb_uvc_streaming_interface_t *stream_if, - const unsigned char *block, size_t block_size) { - - uvc_error_t ret; - int descriptor_subtype; - - ret = UVC_SUCCESS; - descriptor_subtype = block[2]; - - switch (descriptor_subtype) { - case UVC_VS_INPUT_HEADER: - ret = winusb_uvc_parse_vs_input_header(stream_if, block, block_size); - break; - case UVC_VS_FORMAT_UNCOMPRESSED: - ret = winusb_uvc_parse_vs_format_uncompressed(stream_if, block, block_size); - break; - case UVC_VS_FORMAT_MJPEG: - ret = winusb_uvc_parse_vs_format_mjpeg(stream_if, block, block_size); - break; - case UVC_VS_FRAME_UNCOMPRESSED: - case UVC_VS_FRAME_MJPEG: - ret = winusb_uvc_parse_vs_frame_uncompressed(stream_if, block, block_size); - break; - case UVC_VS_FORMAT_FRAME_BASED: - ret = winusb_uvc_parse_vs_frame_format(stream_if, block, block_size); - break; - case UVC_VS_FRAME_FRAME_BASED: - ret = winusb_uvc_parse_vs_frame_frame(stream_if, block, block_size); - break; - case UVC_VS_COLORFORMAT: - break; - - default: - /** @todo handle JPEG and maybe still frames or even DV... */ - /*fprintf(stderr, "unsupported descriptor subtype: %d\n", - descriptor_subtype);*/ - break; - } - - return ret; + winusb_uvc_device *dev, + winusb_uvc_device_info_t *info, + winusb_uvc_streaming_interface_t *stream_if, + const unsigned char *block, size_t block_size) { + + uvc_error_t ret; + int descriptor_subtype; + + ret = UVC_SUCCESS; + descriptor_subtype = block[2]; + + switch (descriptor_subtype) { + case UVC_VS_INPUT_HEADER: + ret = winusb_uvc_parse_vs_input_header(stream_if, block, block_size); + break; + case UVC_VS_FORMAT_UNCOMPRESSED: + ret = winusb_uvc_parse_vs_format_uncompressed(stream_if, block, block_size); + break; + case UVC_VS_FORMAT_MJPEG: + ret = winusb_uvc_parse_vs_format_mjpeg(stream_if, block, block_size); + break; + case UVC_VS_FRAME_UNCOMPRESSED: + case UVC_VS_FRAME_MJPEG: + ret = winusb_uvc_parse_vs_frame_uncompressed(stream_if, block, block_size); + break; + case UVC_VS_FORMAT_FRAME_BASED: + ret = winusb_uvc_parse_vs_frame_format(stream_if, block, block_size); + break; + case UVC_VS_FRAME_FRAME_BASED: + ret = winusb_uvc_parse_vs_frame_frame(stream_if, block, block_size); + break; + case UVC_VS_COLORFORMAT: + break; + + default: + /** @todo handle JPEG and maybe still frames or even DV... */ + /*fprintf(stderr, "unsupported descriptor subtype: %d\n", + descriptor_subtype);*/ + break; + } + + return ret; } uvc_error_t update_stream_if_handle(winusb_uvc_device *devh, int interface_idx) { - winusb_uvc_streaming_interface_t *stream_if; - - DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { - if (stream_if->bInterfaceNumber == interface_idx) - if (!stream_if->associateHandle && interface_idx < MAX_USB_INTERFACES) - { - // WinUsb_GetAssociatedInterface returns the associated interface (Video stream interface which is associated to Video control interface) - // A value of 0 indicates the first associated interface (Video Stream 1), a value of 1 indicates the second associated interface (video stream 2) - // WinUsbInterfaceNumber is the actual interface number taken from the USB config descriptor - // A value of 0 indicates the first interface (Video Control Interface), A value of 1 indicates the first associated interface (Video Stream 1) - // For this reason, when calling to WinUsb_GetAssociatedInterface, we must decrease 1 to receive the associated interface - uint8_t winusbInterfaceNumber = devh->deviceData.interfaces->iface[interface_idx].winusbInterfaceNumber; - if (!WinUsb_GetAssociatedInterface(devh->winusbHandle, winusbInterfaceNumber - 1, &stream_if->associateHandle)) - return UVC_ERROR_INVALID_PARAM; - } - } - return UVC_SUCCESS; + winusb_uvc_streaming_interface_t *stream_if; + + DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { + if (stream_if->bInterfaceNumber == interface_idx) + if (!stream_if->associateHandle && interface_idx < MAX_USB_INTERFACES) + { + // WinUsb_GetAssociatedInterface returns the associated interface (Video stream interface which is associated to Video control interface) + // A value of 0 indicates the first associated interface (Video Stream 1), a value of 1 indicates the second associated interface (video stream 2) + // WinUsbInterfaceNumber is the actual interface number taken from the USB config descriptor + // A value of 0 indicates the first interface (Video Control Interface), A value of 1 indicates the first associated interface (Video Stream 1) + // For this reason, when calling to WinUsb_GetAssociatedInterface, we must decrease 1 to receive the associated interface + uint8_t winusbInterfaceNumber = devh->deviceData.interfaces->iface[interface_idx].winusbInterfaceNumber; + if (!WinUsb_GetAssociatedInterface(devh->winusbHandle, winusbInterfaceNumber - 1, &stream_if->associateHandle)) + return UVC_ERROR_INVALID_PARAM; + } + } + return UVC_SUCCESS; } /* Return only Video stream interfaces */ static winusb_uvc_streaming_interface_t *winusb_uvc_get_stream_if(winusb_uvc_device *devh, int interface_idx) { - winusb_uvc_streaming_interface_t *stream_if; + winusb_uvc_streaming_interface_t *stream_if; - DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { - if (stream_if->bInterfaceNumber == interface_idx) - return stream_if; - } + DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { + if (stream_if->bInterfaceNumber == interface_idx) + return stream_if; + } - return NULL; + return NULL; } /** Return all device formats available @@ -686,97 +686,97 @@ static winusb_uvc_streaming_interface_t *winusb_uvc_get_stream_if(winusb_uvc_dev #define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00FF0000) >> 8) | (((x) & 0x0000FF00) << 8) | ((x) << 24)) uvc_error_t winusb_get_available_formats( - winusb_uvc_device *devh, - int interface_idx, - uvc_format_t **formats) { - - winusb_uvc_streaming_interface_t *stream_if = NULL; - uvc_format_t *prev_format = NULL; - uvc_format_desc_t *format; - - stream_if = winusb_uvc_get_stream_if(devh, interface_idx); - if (stream_if == NULL) { - return UVC_ERROR_INVALID_PARAM; - } - - DL_FOREACH(stream_if->format_descs, format) { - uint32_t *interval_ptr; - uvc_frame_desc_t *frame_desc; - - DL_FOREACH(format->frame_descs, frame_desc) { - for (interval_ptr = frame_desc->intervals; - *interval_ptr; - ++interval_ptr) { - uvc_format_t *cur_format = (uvc_format_t *)malloc(sizeof(uvc_format_t)); - cur_format->height = frame_desc->wHeight; - cur_format->width = frame_desc->wWidth; - cur_format->fourcc = SWAP_UINT32(*(const uint32_t *)format->guidFormat); - - cur_format->fps = 10000000 / *interval_ptr; - cur_format->next = NULL; - - if (prev_format != NULL) { - prev_format->next = cur_format; - } - else { - *formats = cur_format; - } - - prev_format = cur_format; - } - } - } - return UVC_SUCCESS; + winusb_uvc_device *devh, + int interface_idx, + uvc_format_t **formats) { + + winusb_uvc_streaming_interface_t *stream_if = NULL; + uvc_format_t *prev_format = NULL; + uvc_format_desc_t *format; + + stream_if = winusb_uvc_get_stream_if(devh, interface_idx); + if (stream_if == NULL) { + return UVC_ERROR_INVALID_PARAM; + } + + DL_FOREACH(stream_if->format_descs, format) { + uint32_t *interval_ptr; + uvc_frame_desc_t *frame_desc; + + DL_FOREACH(format->frame_descs, frame_desc) { + for (interval_ptr = frame_desc->intervals; + *interval_ptr; + ++interval_ptr) { + uvc_format_t *cur_format = (uvc_format_t *)malloc(sizeof(uvc_format_t)); + cur_format->height = frame_desc->wHeight; + cur_format->width = frame_desc->wWidth; + cur_format->fourcc = SWAP_UINT32(*(const uint32_t *)format->guidFormat); + + cur_format->fps = 10000000 / *interval_ptr; + cur_format->next = NULL; + + if (prev_format != NULL) { + prev_format->next = cur_format; + } + else { + *formats = cur_format; + } + + prev_format = cur_format; + } + } + } + return UVC_SUCCESS; } // Return linked list of uvc_format_t of all available formats inside winusb device uvc_error_t winusb_get_available_formats_all(winusb_uvc_device *devh, uvc_format_t **formats) { - winusb_uvc_streaming_interface_t *stream_if = NULL; - uvc_format_t *prev_format = NULL; - uvc_format_desc_t *format; - - DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { - DL_FOREACH(stream_if->format_descs, format) { - uint32_t *interval_ptr; - uvc_frame_desc_t *frame_desc; - - DL_FOREACH(format->frame_descs, frame_desc) { - for (interval_ptr = frame_desc->intervals; - *interval_ptr; - ++interval_ptr) { - uvc_format_t *cur_format = (uvc_format_t *)malloc(sizeof(uvc_format_t)); - cur_format->height = frame_desc->wHeight; - cur_format->width = frame_desc->wWidth; - cur_format->fourcc = SWAP_UINT32(*(const uint32_t *)format->guidFormat); - cur_format->interfaceNumber = stream_if->bInterfaceNumber; - - cur_format->fps = 10000000 / *interval_ptr; - cur_format->next = NULL; - - if (prev_format != NULL) { - prev_format->next = cur_format; - } - else { - *formats = cur_format; - } - - prev_format = cur_format; - } - } - } - } - return UVC_SUCCESS; + winusb_uvc_streaming_interface_t *stream_if = NULL; + uvc_format_t *prev_format = NULL; + uvc_format_desc_t *format; + + DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { + DL_FOREACH(stream_if->format_descs, format) { + uint32_t *interval_ptr; + uvc_frame_desc_t *frame_desc; + + DL_FOREACH(format->frame_descs, frame_desc) { + for (interval_ptr = frame_desc->intervals; + *interval_ptr; + ++interval_ptr) { + uvc_format_t *cur_format = (uvc_format_t *)malloc(sizeof(uvc_format_t)); + cur_format->height = frame_desc->wHeight; + cur_format->width = frame_desc->wWidth; + cur_format->fourcc = SWAP_UINT32(*(const uint32_t *)format->guidFormat); + cur_format->interfaceNumber = stream_if->bInterfaceNumber; + + cur_format->fps = 10000000 / *interval_ptr; + cur_format->next = NULL; + + if (prev_format != NULL) { + prev_format->next = cur_format; + } + else { + *formats = cur_format; + } + + prev_format = cur_format; + } + } + } + } + return UVC_SUCCESS; } uvc_error_t winusb_free_formats(uvc_format_t *formats) { - uvc_format_t *cur_format = formats; - while (cur_format != NULL) { - uvc_format_t *format = cur_format; - cur_format = cur_format->next; - free(format); - } - - return UVC_SUCCESS; + uvc_format_t *cur_format = formats; + while (cur_format != NULL) { + uvc_format_t *format = cur_format; + cur_format = cur_format->next; + free(format); + } + + return UVC_SUCCESS; } /** @internal @@ -786,26 +786,26 @@ uvc_error_t winusb_free_formats(uvc_format_t *formats) { * @param frame_id Index of frame descriptor */ static uvc_frame_desc_t *winusb_uvc_find_frame_desc_stream_if(winusb_uvc_streaming_interface_t *stream_if, - uint16_t format_id, uint16_t frame_id) { + uint16_t format_id, uint16_t frame_id) { - uvc_format_desc_t *format = NULL; - uvc_frame_desc_t *frame = NULL; + uvc_format_desc_t *format = NULL; + uvc_frame_desc_t *frame = NULL; - DL_FOREACH(stream_if->format_descs, format) { - if (format->bFormatIndex == format_id) { - DL_FOREACH(format->frame_descs, frame) { - if (frame->bFrameIndex == frame_id) - return frame; - } - } - } + DL_FOREACH(stream_if->format_descs, format) { + if (format->bFormatIndex == format_id) { + DL_FOREACH(format->frame_descs, frame) { + if (frame->bFrameIndex == frame_id) + return frame; + } + } + } - return NULL; + return NULL; } uvc_frame_desc_t *winusb_uvc_find_frame_desc_stream(winusb_uvc_stream_handle_t *strmh, - uint16_t format_id, uint16_t frame_id) { - return winusb_uvc_find_frame_desc_stream_if(strmh->stream_if, format_id, frame_id); + uint16_t format_id, uint16_t frame_id) { + return winusb_uvc_find_frame_desc_stream_if(strmh->stream_if, format_id, frame_id); } @@ -815,343 +815,336 @@ uvc_frame_desc_t *winusb_uvc_find_frame_desc_stream(winusb_uvc_stream_handle_t * void winusb_uvc_swap_buffers(winusb_uvc_stream_handle_t *strmh) { - uint8_t *tmp_buf; - - //pthread_mutex_lock(&strmh->cb_mutex); - - /* swap the buffers */ - tmp_buf = strmh->holdbuf; - strmh->hold_bytes = strmh->got_bytes; - strmh->holdbuf = strmh->outbuf; - strmh->outbuf = tmp_buf; - strmh->hold_last_scr = strmh->last_scr; - strmh->hold_pts = strmh->pts; - strmh->hold_seq = strmh->seq; - - //pthread_cond_broadcast(&strmh->cb_cond); - //pthread_mutex_unlock(&strmh->cb_mutex); - - strmh->seq++; - strmh->got_bytes = 0; - strmh->last_scr = 0; - strmh->pts = 0; + uint8_t *tmp_buf; + + //pthread_mutex_lock(&strmh->cb_mutex); + + /* swap the buffers */ + tmp_buf = strmh->holdbuf; + strmh->hold_bytes = strmh->got_bytes; + strmh->holdbuf = strmh->outbuf; + strmh->outbuf = tmp_buf; + strmh->hold_last_scr = strmh->last_scr; + strmh->hold_pts = strmh->pts; + strmh->hold_seq = strmh->seq; + + //pthread_cond_broadcast(&strmh->cb_cond); + //pthread_mutex_unlock(&strmh->cb_mutex); + + strmh->seq++; + strmh->got_bytes = 0; + strmh->last_scr = 0; + strmh->pts = 0; } void winusb_uvc_process_payload(winusb_uvc_stream_handle_t *strmh, - uint8_t *payload, size_t payload_len, frames_archive* archive, frames_queue* queue) { - uint8_t header_len; - uint8_t header_info; - size_t data_len; - - /* magic numbers for identifying header packets from some iSight cameras */ - static uint8_t isight_tag[] = { - 0x11, 0x22, 0x33, 0x44, - 0xde, 0xad, 0xbe, 0xef, 0xde, 0xad, 0xfa, 0xce - }; - - /* ignore empty payload transfers */ - if (payload_len == 0) - { - LOG_ERROR("Zero-size packet arrived, header size =" << header_len); - return; - } - - /* Certain iSight cameras have strange behavior: They send header - * information in a packet with no image data, and then the following - * packets have only image data, with no more headers until the next frame. - * - * The iSight header: len(1), flags(1 or 2), 0x11223344(4), - * 0xdeadbeefdeadface(8), ??(16) - */ - - header_len = payload[0]; - if (header_len > payload_len) - { - LOG_ERROR("bogus packet: actual_len= " << std::dec << " , header_len=" << header_len); - return; - } - else - { - //printf("Frame %d %d: header info = %08X\n", strmh->seq, counter, payload[1]); - } - - data_len = payload_len - header_len; - - if (header_len < 2) { - header_info = 0; - } - else { - /** @todo we should be checking the end-of-header bit */ - size_t variable_offset = 2; - - header_info = payload[1]; - - if (header_info & 0x40) { - LOG_ERROR("bad packet: error bit set [header info=" << std::hex << header_info << std::dec << "]"); - return; - } - - if (strmh->fid != (header_info & 1) && strmh->got_bytes != 0) { - /* The frame ID bit was flipped, but we have image data sitting - around from prior transfers. This means the camera didn't send - an EOF for the last transfer of the previous frame. */ - LOG_DEBUG("complete buffer : length: " << strmh->got_bytes); - winusb_uvc_swap_buffers(strmh); - } - - strmh->fid = header_info & 1; - - if (header_info & (1 << 2)) { - strmh->pts = DW_TO_INT(payload + variable_offset); - variable_offset += 4; - } - - if (header_info & (1 << 3)) { - /** @todo read the SOF token counter */ - strmh->last_scr = DW_TO_INT(payload + variable_offset); - variable_offset += 6; - } - } - - if ((data_len > 0) && (strmh->cur_ctrl.dwMaxVideoFrameSize == (data_len))) - { - //if (header_info & (1 << 1)) { // Temp patch to allow old firmware - /* The EOF bit is set, so publish the complete frame */ - winusb_uvc_swap_buffers(strmh); - - auto frame_p = archive->allocate(); - if (frame_p) - { - frame_ptr fp(frame_p, &cleanup_frame); - - memcpy(fp->pixels.data(), payload, data_len + header_len); - - LOG_DEBUG("Passing packet to user CB with size " << data_len + header_len); - librealsense::platform::frame_object fo{ data_len, header_len, - fp->pixels.data() + header_len , fp->pixels.data() }; - fp->fo = fo; - - queue->enqueue(std::move(fp)); - } - else - { - LOG_INFO("WinUSB backend is dropping a frame because librealsense wasn't fast enough"); - } - //} - } - else - { - LOG_DEBUG("Partial frame arrived, expected = " << std::dec << strmh->cur_ctrl.dwMaxVideoFrameSize - << ", actual = " << data_len); - } + uint8_t *payload, size_t payload_len, frames_archive* archive, frames_queue* queue) { + uint8_t header_len; + uint8_t header_info; + size_t data_len; + + /* magic numbers for identifying header packets from some iSight cameras */ + static uint8_t isight_tag[] = { + 0x11, 0x22, 0x33, 0x44, + 0xde, 0xad, 0xbe, 0xef, 0xde, 0xad, 0xfa, 0xce + }; + + /* ignore empty payload transfers */ + if (payload_len == 0) + { + LOG_ERROR("Zero-size packet arrived, header size =" << header_len); + return; + } + + /* Certain iSight cameras have strange behavior: They send header + * information in a packet with no image data, and then the following + * packets have only image data, with no more headers until the next frame. + * + * The iSight header: len(1), flags(1 or 2), 0x11223344(4), + * 0xdeadbeefdeadface(8), ??(16) + */ + + header_len = payload[0]; + if (header_len > payload_len) + { + LOG_ERROR("bogus packet: actual_len= " << std::dec << " , header_len=" << header_len); + return; + } + else + { + //printf("Frame %d %d: header info = %08X\n", strmh->seq, counter, payload[1]); + } + + data_len = payload_len - header_len; + + if (header_len < 2) { + header_info = 0; + } + else { + /** @todo we should be checking the end-of-header bit */ + size_t variable_offset = 2; + + header_info = payload[1]; + + if (header_info & 0x40) { + LOG_ERROR("bad packet: error bit set [header info=" << std::hex << header_info << std::dec << "]"); + return; + } + + if (strmh->fid != (header_info & 1) && strmh->got_bytes != 0) { + /* The frame ID bit was flipped, but we have image data sitting + around from prior transfers. This means the camera didn't send + an EOF for the last transfer of the previous frame. */ + LOG_DEBUG("complete buffer : length: " << strmh->got_bytes); + winusb_uvc_swap_buffers(strmh); + } + + strmh->fid = header_info & 1; + + if (header_info & (1 << 2)) { + strmh->pts = DW_TO_INT(payload + variable_offset); + variable_offset += 4; + } + + if (header_info & (1 << 3)) { + /** @todo read the SOF token counter */ + strmh->last_scr = DW_TO_INT(payload + variable_offset); + variable_offset += 6; + } + } + + if ((data_len > 0) && (strmh->cur_ctrl.dwMaxVideoFrameSize == (data_len))) + { + //if (header_info & (1 << 1)) { // Temp patch to allow old firmware + /* The EOF bit is set, so publish the complete frame */ + winusb_uvc_swap_buffers(strmh); + + auto frame_p = archive->allocate(); + if (frame_p) + { + frame_ptr fp(frame_p, &cleanup_frame); + + memcpy(fp->pixels.data(), payload, data_len + header_len); + + LOG_DEBUG("Passing packet to user CB with size " << data_len + header_len); + librealsense::platform::frame_object fo{ data_len, header_len, + fp->pixels.data() + header_len , fp->pixels.data() }; + fp->fo = fo; + + queue->enqueue(std::move(fp)); + } + else + { + LOG_INFO("WinUSB backend is dropping a frame because librealsense wasn't fast enough"); + } + //} + } + else + { + LOG_DEBUG("Partial frame arrived, expected = " << std::dec << strmh->cur_ctrl.dwMaxVideoFrameSize + << ", actual = " << data_len); + } } void stream_thread(winusb_uvc_stream_context *strctx) { - PUCHAR buffer = (PUCHAR)malloc(strctx->maxPayloadTransferSize); - memset(buffer, 0, strctx->maxPayloadTransferSize); - - frames_archive archive; - std::atomic_bool keep_sending_callbacks = true; - frames_queue queue; - - // Get all pointers from archive and initialize their content - std::vector frames; - for (auto i = 0; i < frames_archive::CAPACITY; i++) - { - auto ptr = archive.allocate(); - ptr->pixels.resize(strctx->maxPayloadTransferSize, 0); - ptr->owner = &archive; - frames.push_back(ptr); - } - for (auto ptr : frames) - { - archive.deallocate(ptr); - } - - // Preemptively clean pipe state to prepare for future transactions - auto ret = WinUsb_FlushPipe(strctx->stream->stream_if->associateHandle, strctx->endpoint); - ret = WinUsb_ResetPipe(strctx->stream->stream_if->associateHandle, strctx->endpoint); - - std::thread t([&]() { - while (keep_sending_callbacks) - { - frame_ptr fp(nullptr, [](frame*) {}); - if (queue.dequeue(&fp, 50)) - { - strctx->stream->user_cb(&fp->fo, strctx->stream->user_ptr); - } - } - }); - - do { - DWORD transferred{}; - if (!WinUsb_ReadPipe(strctx->stream->stream_if->associateHandle, - strctx->endpoint, - buffer, - strctx->maxPayloadTransferSize, - &transferred, - NULL)) - { - auto err = GetLastError(); - LOG_ERROR("WinUsb_ReadPipe Error: " << err); - break; - } - - //LOG_DEBUG("Packet received with size " << std::dec << transferred); - winusb_uvc_process_payload(strctx->stream, buffer, transferred, &archive, &queue); - } while (strctx->stream->running); - - // reseting pipe after use - ret = WinUsb_FlushPipe(strctx->stream->stream_if->associateHandle, strctx->endpoint); - ret = WinUsb_ResetPipe(strctx->stream->stream_if->associateHandle, strctx->endpoint); - - free(buffer); - free(strctx); - - queue.clear(); - archive.stop_allocation(); - archive.wait_until_empty(); - keep_sending_callbacks = false; - LOG_DEBUG(__FUNCTION__ << ": start joining user callbacks thread"); - t.join(); - LOG_DEBUG(__FUNCTION__ << ": user callbacks thread joined"); + PUCHAR buffer = (PUCHAR)malloc(strctx->maxPayloadTransferSize); + memset(buffer, 0, strctx->maxPayloadTransferSize); + + frames_archive archive; + std::atomic_bool keep_sending_callbacks = true; + frames_queue queue; + + // Get all pointers from archive and initialize their content + std::vector frames; + for (auto i = 0; i < frames_archive::CAPACITY; i++) + { + auto ptr = archive.allocate(); + ptr->pixels.resize(strctx->maxPayloadTransferSize, 0); + ptr->owner = &archive; + frames.push_back(ptr); + } + for (auto ptr : frames) + { + archive.deallocate(ptr); + } + + // Preemptively clean pipe state to prepare for future transactions + auto ret = WinUsb_FlushPipe(strctx->stream->stream_if->associateHandle, strctx->endpoint); + ret = WinUsb_ResetPipe(strctx->stream->stream_if->associateHandle, strctx->endpoint); + + std::thread t([&]() { + while (keep_sending_callbacks) + { + frame_ptr fp(nullptr, [](frame*) {}); + if (queue.dequeue(&fp, 50)) + { + strctx->stream->user_cb(&fp->fo, strctx->stream->user_ptr); + } + } + }); + + do { + DWORD transferred{}; + if (!WinUsb_ReadPipe(strctx->stream->stream_if->associateHandle, + strctx->endpoint, + buffer, + strctx->maxPayloadTransferSize, + &transferred, + NULL)) + { + auto err = GetLastError(); + LOG_ERROR("WinUsb_ReadPipe Error: " << err); + break; + } + + //LOG_DEBUG("Packet received with size " << std::dec << transferred); + winusb_uvc_process_payload(strctx->stream, buffer, transferred, &archive, &queue); + } while (strctx->stream->running); + + // reseting pipe after use + ret = WinUsb_FlushPipe(strctx->stream->stream_if->associateHandle, strctx->endpoint); + ret = WinUsb_ResetPipe(strctx->stream->stream_if->associateHandle, strctx->endpoint); + + free(buffer); + free(strctx); + + queue.clear(); + archive.stop_allocation(); + archive.wait_until_empty(); + keep_sending_callbacks = false; + LOG_DEBUG(__FUNCTION__ << ": start joining user callbacks thread"); + t.join(); + LOG_DEBUG(__FUNCTION__ << ": user callbacks thread joined"); }; uvc_error_t winusb_uvc_stream_start( - winusb_uvc_stream_handle_t *strmh, - winusb_uvc_frame_callback_t *cb, - void *user_ptr, - uint8_t flags + winusb_uvc_stream_handle_t *strmh, + winusb_uvc_frame_callback_t *cb, + void *user_ptr, + uint8_t flags ) { - /* USB interface we'll be using */ - winusb_uvc_interface *iface; - int interface_id; - uvc_frame_desc_t *frame_desc; - uvc_format_desc_t *format_desc; - uvc_stream_ctrl_t *ctrl; - uvc_error_t ret; - /* Total amount of data per transfer */ - - ctrl = &strmh->cur_ctrl; - - if (strmh->running) { - return UVC_ERROR_BUSY; - } - - strmh->running = 1; - strmh->seq = 1; - strmh->fid = 0; - strmh->pts = 0; - strmh->last_scr = 0; - - frame_desc = winusb_uvc_find_frame_desc_stream(strmh, ctrl->bFormatIndex, ctrl->bFrameIndex); - if (!frame_desc) { - ret = UVC_ERROR_INVALID_PARAM; - goto fail; - } - format_desc = frame_desc->parent; - - // Get the interface that provides the chosen format and frame configuration - interface_id = strmh->stream_if->bInterfaceNumber; - iface = &strmh->devh->deviceData.interfaces->iface[interface_id]; - - winusb_uvc_stream_context *streamctx = new winusb_uvc_stream_context; - - //printf("Starting stream on EP = 0x%X, interface 0x%X\n", format_desc->parent->bEndpointAddress, iface); - - streamctx->stream = strmh; - streamctx->endpoint = format_desc->parent->bEndpointAddress; - streamctx->iface = iface; - streamctx->maxPayloadTransferSize = strmh->cur_ctrl.dwMaxPayloadTransferSize; - strmh->user_ptr = user_ptr; - - // WinUsb_SetPipePolicy function sets the policy for a specific pipe associated with an endpoint on the device - // PIPE_TRANSFER_TIMEOUT: Waits for a time-out interval before canceling the request - ULONG timeout_milliseconds = 5000; - if (WinUsb_SetPipePolicy(streamctx->stream->stream_if->associateHandle, streamctx->endpoint, PIPE_TRANSFER_TIMEOUT, sizeof(timeout_milliseconds), &timeout_milliseconds) == FALSE) - return UVC_ERROR_OTHER; - - strmh->cb_thread = std::thread(stream_thread, streamctx); - strmh->user_cb = cb; - - - return UVC_SUCCESS; + /* USB interface we'll be using */ + winusb_uvc_interface *iface; + int interface_id; + uvc_frame_desc_t *frame_desc; + uvc_format_desc_t *format_desc; + uvc_stream_ctrl_t *ctrl; + uvc_error_t ret; + /* Total amount of data per transfer */ + + ctrl = &strmh->cur_ctrl; + + if (strmh->running) { + return UVC_ERROR_BUSY; + } + + strmh->running = 1; + strmh->seq = 1; + strmh->fid = 0; + strmh->pts = 0; + strmh->last_scr = 0; + + frame_desc = winusb_uvc_find_frame_desc_stream(strmh, ctrl->bFormatIndex, ctrl->bFrameIndex); + if (!frame_desc) { + ret = UVC_ERROR_INVALID_PARAM; + goto fail; + } + format_desc = frame_desc->parent; + + // Get the interface that provides the chosen format and frame configuration + interface_id = strmh->stream_if->bInterfaceNumber; + iface = &strmh->devh->deviceData.interfaces->iface[interface_id]; + + winusb_uvc_stream_context *streamctx = new winusb_uvc_stream_context; + + //printf("Starting stream on EP = 0x%X, interface 0x%X\n", format_desc->parent->bEndpointAddress, iface); + + streamctx->stream = strmh; + streamctx->endpoint = format_desc->parent->bEndpointAddress; + streamctx->iface = iface; + streamctx->maxPayloadTransferSize = strmh->cur_ctrl.dwMaxPayloadTransferSize; + strmh->user_ptr = user_ptr; + + // WinUsb_SetPipePolicy function sets the policy for a specific pipe associated with an endpoint on the device + // PIPE_TRANSFER_TIMEOUT: Waits for a time-out interval before canceling the request + ULONG timeout_milliseconds = 5000; + if (WinUsb_SetPipePolicy(streamctx->stream->stream_if->associateHandle, streamctx->endpoint, PIPE_TRANSFER_TIMEOUT, sizeof(timeout_milliseconds), &timeout_milliseconds) == FALSE) + return UVC_ERROR_OTHER; + + strmh->cb_thread = std::thread(stream_thread, streamctx); + strmh->user_cb = cb; + + + return UVC_SUCCESS; fail: - strmh->running = 0; - return ret; + strmh->running = 0; + return ret; } uvc_error_t winusb_uvc_stream_stop(winusb_uvc_stream_handle_t *strmh) { - LOG_DEBUG(__FUNCTION__); - if (!strmh->running) - return UVC_ERROR_INVALID_PARAM; - - strmh->running = 0; - // Terminate the thread. - LOG_DEBUG(__FUNCTION__ << ": start joining streaming thread"); - auto future = std::async(std::launch::async, &std::thread::join, &strmh->cb_thread); - if (future.wait_for(std::chrono::seconds(10)) == std::future_status::timeout) - { - LOG_ERROR(__FUNCTION__ << ": app stall encountered"); - } - LOG_DEBUG(__FUNCTION__ << ": streaming thread joined"); - - //strmh->cb_thread.join(); - - return UVC_SUCCESS; + if (!strmh->running) + return UVC_ERROR_INVALID_PARAM; + + strmh->running = 0; + // Terminate the thread. + LOG_DEBUG(__FUNCTION__ << ": start joining streaming thread"); + auto future = std::async(std::launch::async, &std::thread::join, &strmh->cb_thread); + if (future.wait_for(std::chrono::seconds(10)) == std::future_status::timeout) + { + LOG_ERROR(__FUNCTION__ << ": app stall encountered"); + } + LOG_DEBUG(__FUNCTION__ << ": streaming thread joined"); + + return UVC_SUCCESS; } void winusb_uvc_stream_close(winusb_uvc_stream_handle_t *strmh) { - if (strmh->running) - { - winusb_uvc_stream_stop(strmh); - } + if (strmh->running) + { + winusb_uvc_stream_stop(strmh); + } - free(strmh->outbuf); - free(strmh->holdbuf); + free(strmh->outbuf); + free(strmh->holdbuf); - DL_DELETE(strmh->devh->streams, strmh); + DL_DELETE(strmh->devh->streams, strmh); - free(strmh); + free(strmh); } uvc_error_t winusb_uvc_stream_open_ctrl(winusb_uvc_device *devh, winusb_uvc_stream_handle_t **strmhp, uvc_stream_ctrl_t *ctrl); uvc_error_t winusb_start_streaming(winusb_uvc_device *devh, uvc_stream_ctrl_t *ctrl, winusb_uvc_frame_callback_t *cb, void *user_ptr, uint8_t flags) { - LOG_DEBUG(__FUNCTION__); - - uvc_error_t ret; - winusb_uvc_stream_handle_t *strmh; - - ret = winusb_uvc_stream_open_ctrl(devh, &strmh, ctrl); - if (ret != UVC_SUCCESS) - { - return ret; - } - - ret = winusb_uvc_stream_start(strmh, cb, user_ptr, flags); - if (ret != UVC_SUCCESS) - { - winusb_uvc_stream_close(strmh); - return ret; - } - - return UVC_SUCCESS; + uvc_error_t ret; + winusb_uvc_stream_handle_t *strmh; + + ret = winusb_uvc_stream_open_ctrl(devh, &strmh, ctrl); + if (ret != UVC_SUCCESS) + { + return ret; + } + + ret = winusb_uvc_stream_start(strmh, cb, user_ptr, flags); + if (ret != UVC_SUCCESS) + { + winusb_uvc_stream_close(strmh); + return ret; + } + + return UVC_SUCCESS; } void winusb_stop_streaming(winusb_uvc_device *devh) { - LOG_DEBUG(__FUNCTION__); - - winusb_uvc_stream_handle_t *strmh, *strmh_tmp; + winusb_uvc_stream_handle_t *strmh, *strmh_tmp; - DL_FOREACH_SAFE(devh->streams, strmh, strmh_tmp) { - winusb_uvc_stream_close(strmh); - } + DL_FOREACH_SAFE(devh->streams, strmh, strmh_tmp) { + winusb_uvc_stream_close(strmh); + } } @@ -1162,124 +1155,124 @@ void winusb_stop_streaming(winusb_uvc_device *devh) * @param frame_id Index of frame descriptor */ uvc_frame_desc_t *winusb_uvc_find_frame_desc(winusb_uvc_device *devh, - uint16_t format_id, uint16_t frame_id) { + uint16_t format_id, uint16_t frame_id) { - winusb_uvc_streaming_interface_t *stream_if; - uvc_frame_desc_t *frame; + winusb_uvc_streaming_interface_t *stream_if; + uvc_frame_desc_t *frame; - DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { - frame = winusb_uvc_find_frame_desc_stream_if(stream_if, format_id, frame_id); - if (frame) - return frame; - } + DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { + frame = winusb_uvc_find_frame_desc_stream_if(stream_if, format_id, frame_id); + if (frame) + return frame; + } - return NULL; + return NULL; } uvc_error_t winusb_uvc_query_stream_ctrl(winusb_uvc_device *devh, uvc_stream_ctrl_t *ctrl, uint8_t probe, int req) { - uint8_t buf[48]; - size_t len; - int err; - - memset(buf, 0, sizeof(buf)); - len = 34; - - /* prepare for a SET transfer */ - if (req == UVC_SET_CUR) - { - SHORT_TO_SW(ctrl->bmHint, buf); - buf[2] = ctrl->bFormatIndex; - buf[3] = ctrl->bFrameIndex; - INT_TO_DW(ctrl->dwFrameInterval, buf + 4); - SHORT_TO_SW(ctrl->wKeyFrameRate, buf + 8); - SHORT_TO_SW(ctrl->wPFrameRate, buf + 10); - SHORT_TO_SW(ctrl->wCompQuality, buf + 12); - SHORT_TO_SW(ctrl->wCompWindowSize, buf + 14); - SHORT_TO_SW(ctrl->wDelay, buf + 16); - INT_TO_DW(ctrl->dwMaxVideoFrameSize, buf + 18); - INT_TO_DW(ctrl->dwMaxPayloadTransferSize, buf + 22); - INT_TO_DW(0, buf + 18); - INT_TO_DW(0, buf + 22); - - if (len >= 34) { - INT_TO_DW(ctrl->dwClockFrequency, buf + 26); - buf[30] = ctrl->bmFramingInfo; - buf[31] = ctrl->bPreferredVersion; - buf[32] = ctrl->bMinVersion; - buf[33] = ctrl->bMaxVersion; - /** @todo support UVC 1.1 */ - } - - if (len == 48) { - buf[34] = ctrl->bUsage; - buf[35] = ctrl->bBitDepthLuma; - buf[36] = ctrl->bmSettings; - buf[37] = ctrl->bMaxNumberOfRefFramesPlus1; - SHORT_TO_SW(ctrl->bmRateControlModes, buf + 38); - QUAD_TO_QW(ctrl->bmLayoutPerStream, buf + 40); - } - } - - auto str_if = winusb_uvc_get_stream_if(devh, ctrl->bInterfaceNumber); - /* do the transfer */ - err = winusb_SendControl( - str_if->associateHandle, - req == UVC_SET_CUR ? UVC_REQ_TYPE_INTERFACE_SET : UVC_REQ_TYPE_INTERFACE_GET, - req, - probe ? (UVC_VS_PROBE_CONTROL << 8) : (UVC_VS_COMMIT_CONTROL << 8), - 0, // When requestType is directed to an interface, WinUsb driver automatically passes the interface number in the low byte of index - buf, len); - - if (err <= 0) - { - return (uvc_error_t)err; - } - - /* now decode following a GET transfer */ - if (req != UVC_SET_CUR) { - ctrl->bmHint = SW_TO_SHORT(buf); - ctrl->bFormatIndex = buf[2]; - ctrl->bFrameIndex = buf[3]; - ctrl->dwFrameInterval = DW_TO_INT(buf + 4); - ctrl->wKeyFrameRate = SW_TO_SHORT(buf + 8); - ctrl->wPFrameRate = SW_TO_SHORT(buf + 10); - ctrl->wCompQuality = SW_TO_SHORT(buf + 12); - ctrl->wCompWindowSize = SW_TO_SHORT(buf + 14); - ctrl->wDelay = SW_TO_SHORT(buf + 16); - ctrl->dwMaxVideoFrameSize = DW_TO_INT(buf + 18); - ctrl->dwMaxPayloadTransferSize = DW_TO_INT(buf + 22); - - if (len >= 34) { - ctrl->dwClockFrequency = DW_TO_INT(buf + 26); - ctrl->bmFramingInfo = buf[30]; - ctrl->bPreferredVersion = buf[31]; - ctrl->bMinVersion = buf[32]; - ctrl->bMaxVersion = buf[33]; - /** @todo support UVC 1.1 */ - } - - if (len == 48) { - ctrl->bUsage = buf[34]; - ctrl->bBitDepthLuma = buf[35]; - ctrl->bmSettings = buf[36]; - ctrl->bMaxNumberOfRefFramesPlus1 = buf[37]; - ctrl->bmRateControlModes = DW_TO_INT(buf + 38); - ctrl->bmLayoutPerStream = QW_TO_QUAD(buf + 40); - } - // else - // ctrl->dwClockFrequency = devh->info->ctrl_if.dwClockFrequency; - - /* fix up block for cameras that fail to set dwMax* */ - if (ctrl->dwMaxVideoFrameSize == 0) { - uvc_frame_desc_t *frame = winusb_uvc_find_frame_desc(devh, ctrl->bFormatIndex, ctrl->bFrameIndex); - - if (frame) { - ctrl->dwMaxVideoFrameSize = frame->dwMaxVideoFrameBufferSize; - } - } - } - return UVC_SUCCESS; + uint8_t buf[48]; + size_t len; + int err; + + memset(buf, 0, sizeof(buf)); + len = 34; + + /* prepare for a SET transfer */ + if (req == UVC_SET_CUR) + { + SHORT_TO_SW(ctrl->bmHint, buf); + buf[2] = ctrl->bFormatIndex; + buf[3] = ctrl->bFrameIndex; + INT_TO_DW(ctrl->dwFrameInterval, buf + 4); + SHORT_TO_SW(ctrl->wKeyFrameRate, buf + 8); + SHORT_TO_SW(ctrl->wPFrameRate, buf + 10); + SHORT_TO_SW(ctrl->wCompQuality, buf + 12); + SHORT_TO_SW(ctrl->wCompWindowSize, buf + 14); + SHORT_TO_SW(ctrl->wDelay, buf + 16); + INT_TO_DW(ctrl->dwMaxVideoFrameSize, buf + 18); + INT_TO_DW(ctrl->dwMaxPayloadTransferSize, buf + 22); + INT_TO_DW(0, buf + 18); + INT_TO_DW(0, buf + 22); + + if (len >= 34) { + INT_TO_DW(ctrl->dwClockFrequency, buf + 26); + buf[30] = ctrl->bmFramingInfo; + buf[31] = ctrl->bPreferredVersion; + buf[32] = ctrl->bMinVersion; + buf[33] = ctrl->bMaxVersion; + /** @todo support UVC 1.1 */ + } + + if (len == 48) { + buf[34] = ctrl->bUsage; + buf[35] = ctrl->bBitDepthLuma; + buf[36] = ctrl->bmSettings; + buf[37] = ctrl->bMaxNumberOfRefFramesPlus1; + SHORT_TO_SW(ctrl->bmRateControlModes, buf + 38); + QUAD_TO_QW(ctrl->bmLayoutPerStream, buf + 40); + } + } + + auto str_if = winusb_uvc_get_stream_if(devh, ctrl->bInterfaceNumber); + /* do the transfer */ + err = winusb_SendControl( + str_if->associateHandle, + req == UVC_SET_CUR ? UVC_REQ_TYPE_INTERFACE_SET : UVC_REQ_TYPE_INTERFACE_GET, + req, + probe ? (UVC_VS_PROBE_CONTROL << 8) : (UVC_VS_COMMIT_CONTROL << 8), + 0, // When requestType is directed to an interface, WinUsb driver automatically passes the interface number in the low byte of index + buf, len); + + if (err <= 0) + { + return (uvc_error_t)err; + } + + /* now decode following a GET transfer */ + if (req != UVC_SET_CUR) { + ctrl->bmHint = SW_TO_SHORT(buf); + ctrl->bFormatIndex = buf[2]; + ctrl->bFrameIndex = buf[3]; + ctrl->dwFrameInterval = DW_TO_INT(buf + 4); + ctrl->wKeyFrameRate = SW_TO_SHORT(buf + 8); + ctrl->wPFrameRate = SW_TO_SHORT(buf + 10); + ctrl->wCompQuality = SW_TO_SHORT(buf + 12); + ctrl->wCompWindowSize = SW_TO_SHORT(buf + 14); + ctrl->wDelay = SW_TO_SHORT(buf + 16); + ctrl->dwMaxVideoFrameSize = DW_TO_INT(buf + 18); + ctrl->dwMaxPayloadTransferSize = DW_TO_INT(buf + 22); + + if (len >= 34) { + ctrl->dwClockFrequency = DW_TO_INT(buf + 26); + ctrl->bmFramingInfo = buf[30]; + ctrl->bPreferredVersion = buf[31]; + ctrl->bMinVersion = buf[32]; + ctrl->bMaxVersion = buf[33]; + /** @todo support UVC 1.1 */ + } + + if (len == 48) { + ctrl->bUsage = buf[34]; + ctrl->bBitDepthLuma = buf[35]; + ctrl->bmSettings = buf[36]; + ctrl->bMaxNumberOfRefFramesPlus1 = buf[37]; + ctrl->bmRateControlModes = DW_TO_INT(buf + 38); + ctrl->bmLayoutPerStream = QW_TO_QUAD(buf + 40); + } + // else + // ctrl->dwClockFrequency = devh->info->ctrl_if.dwClockFrequency; + + /* fix up block for cameras that fail to set dwMax* */ + if (ctrl->dwMaxVideoFrameSize == 0) { + uvc_frame_desc_t *frame = winusb_uvc_find_frame_desc(devh, ctrl->bFormatIndex, ctrl->bFrameIndex); + + if (frame) { + ctrl->dwMaxVideoFrameSize = frame->dwMaxVideoFrameBufferSize; + } + } + } + return UVC_SUCCESS; } /** @brief Reconfigure stream with a new stream format. @@ -1293,119 +1286,119 @@ uvc_error_t winusb_uvc_query_stream_ctrl(winusb_uvc_device *devh, uvc_stream_ctr */ uvc_error_t winusb_uvc_stream_ctrl(winusb_uvc_stream_handle_t *strmh, uvc_stream_ctrl_t *ctrl) { - uvc_error_t ret = UVC_SUCCESS; - - if (strmh->stream_if->bInterfaceNumber != ctrl->bInterfaceNumber) - { - return UVC_ERROR_INVALID_PARAM; - } - - /* @todo Allow the stream to be modified without restarting the stream */ - if (strmh->running) - { - return UVC_ERROR_BUSY; - } - - // Set streaming control block - ret = winusb_uvc_query_stream_ctrl(strmh->devh, ctrl, 0, UVC_SET_CUR); - if (ret != UVC_SUCCESS) - { - return ret; - } - - strmh->cur_ctrl = *ctrl; - return UVC_SUCCESS; + uvc_error_t ret = UVC_SUCCESS; + + if (strmh->stream_if->bInterfaceNumber != ctrl->bInterfaceNumber) + { + return UVC_ERROR_INVALID_PARAM; + } + + /* @todo Allow the stream to be modified without restarting the stream */ + if (strmh->running) + { + return UVC_ERROR_BUSY; + } + + // Set streaming control block + ret = winusb_uvc_query_stream_ctrl(strmh->devh, ctrl, 0, UVC_SET_CUR); + if (ret != UVC_SUCCESS) + { + return ret; + } + + strmh->cur_ctrl = *ctrl; + return UVC_SUCCESS; } static winusb_uvc_stream_handle_t *winusb_uvc_get_stream_by_interface(winusb_uvc_device *devh, int interface_idx) { - winusb_uvc_stream_handle_t *strmh; + winusb_uvc_stream_handle_t *strmh; - DL_FOREACH(devh->streams, strmh) { - if (strmh->stream_if->bInterfaceNumber == interface_idx) - return strmh; - } + DL_FOREACH(devh->streams, strmh) { + if (strmh->stream_if->bInterfaceNumber == interface_idx) + return strmh; + } - return NULL; + return NULL; } uvc_error_t winusb_uvc_stream_open_ctrl(winusb_uvc_device *devh, winusb_uvc_stream_handle_t **strmhp, uvc_stream_ctrl_t *ctrl) { - /* Chosen frame and format descriptors */ - winusb_uvc_stream_handle_t *strmh = NULL; - winusb_uvc_streaming_interface_t *stream_if; - uvc_error_t ret; - - if (winusb_uvc_get_stream_by_interface(devh, ctrl->bInterfaceNumber) != NULL) { - ret = UVC_ERROR_BUSY; /* Stream is already opened */ - goto fail; - } - - stream_if = winusb_uvc_get_stream_if(devh, ctrl->bInterfaceNumber); - if (!stream_if) { - ret = UVC_ERROR_INVALID_PARAM; - goto fail; - } - - strmh = (winusb_uvc_stream_handle_t *)calloc(1, sizeof(*strmh)); - if (!strmh) { - ret = UVC_ERROR_NO_MEM; - goto fail; - } - memset(strmh, 0, sizeof(winusb_uvc_stream_handle_t)); - strmh->devh = devh; - strmh->stream_if = stream_if; - //strmh->frame.library_owns_data = 1; - - /*ret = uvc_claim_if(strmh->devh, strmh->stream_if->bInterfaceNumber); - if (ret != UVC_SUCCESS) - goto fail;*/ - - ret = winusb_uvc_stream_ctrl(strmh, ctrl); - if (ret != UVC_SUCCESS) - goto fail; - - // Set up the streaming status and data space - strmh->running = 0; - /** @todo take only what we need */ - strmh->outbuf = (uint8_t *)malloc(LIBUVC_XFER_BUF_SIZE); - strmh->holdbuf = (uint8_t *)malloc(LIBUVC_XFER_BUF_SIZE); - - /*pthread_mutex_init(&strmh->cb_mutex, NULL); - pthread_cond_init(&strmh->cb_cond, NULL);*/ - - DL_APPEND(devh->streams, strmh); - - *strmhp = strmh; - - return UVC_SUCCESS; + /* Chosen frame and format descriptors */ + winusb_uvc_stream_handle_t *strmh = NULL; + winusb_uvc_streaming_interface_t *stream_if; + uvc_error_t ret; + + if (winusb_uvc_get_stream_by_interface(devh, ctrl->bInterfaceNumber) != NULL) { + ret = UVC_ERROR_BUSY; /* Stream is already opened */ + goto fail; + } + + stream_if = winusb_uvc_get_stream_if(devh, ctrl->bInterfaceNumber); + if (!stream_if) { + ret = UVC_ERROR_INVALID_PARAM; + goto fail; + } + + strmh = (winusb_uvc_stream_handle_t *)calloc(1, sizeof(*strmh)); + if (!strmh) { + ret = UVC_ERROR_NO_MEM; + goto fail; + } + memset(strmh, 0, sizeof(winusb_uvc_stream_handle_t)); + strmh->devh = devh; + strmh->stream_if = stream_if; + //strmh->frame.library_owns_data = 1; + + /*ret = uvc_claim_if(strmh->devh, strmh->stream_if->bInterfaceNumber); + if (ret != UVC_SUCCESS) + goto fail;*/ + + ret = winusb_uvc_stream_ctrl(strmh, ctrl); + if (ret != UVC_SUCCESS) + goto fail; + + // Set up the streaming status and data space + strmh->running = 0; + /** @todo take only what we need */ + strmh->outbuf = (uint8_t *)malloc(LIBUVC_XFER_BUF_SIZE); + strmh->holdbuf = (uint8_t *)malloc(LIBUVC_XFER_BUF_SIZE); + + /*pthread_mutex_init(&strmh->cb_mutex, NULL); + pthread_cond_init(&strmh->cb_cond, NULL);*/ + + DL_APPEND(devh->streams, strmh); + + *strmhp = strmh; + + return UVC_SUCCESS; fail: - if (strmh) - free(strmh); - return ret; + if (strmh) + free(strmh); + return ret; } // Probe (Set and Get) streaming control block uvc_error_t winusb_uvc_probe_stream_ctrl(winusb_uvc_device *devh, uvc_stream_ctrl_t *ctrl) { - uvc_error_t ret = UVC_SUCCESS; - - // Sending probe SET request - UVC_SET_CUR request in a probe/commit structure containing desired values for VS Format index, VS Frame index, and VS Frame Interval - // UVC device will check the VS Format index, VS Frame index, and Frame interval properties to verify if possible and update the probe/commit structure if feasible - ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_SET_CUR); - if (ret != UVC_SUCCESS) - { - return ret; - } - - // Sending probe GET request - UVC_GET_CUR request to read the updated values from UVC device - ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_CUR); - if (ret != UVC_SUCCESS) - { - return ret; - } - - /** @todo make sure that worked */ - return UVC_SUCCESS; + uvc_error_t ret = UVC_SUCCESS; + + // Sending probe SET request - UVC_SET_CUR request in a probe/commit structure containing desired values for VS Format index, VS Frame index, and VS Frame Interval + // UVC device will check the VS Format index, VS Frame index, and Frame interval properties to verify if possible and update the probe/commit structure if feasible + ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_SET_CUR); + if (ret != UVC_SUCCESS) + { + return ret; + } + + // Sending probe GET request - UVC_GET_CUR request to read the updated values from UVC device + ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_CUR); + if (ret != UVC_SUCCESS) + { + return ret; + } + + /** @todo make sure that worked */ + return UVC_SUCCESS; } /** Get a negotiated streaming control block for some common parameters for specific interface @@ -1419,91 +1412,91 @@ uvc_error_t winusb_uvc_probe_stream_ctrl(winusb_uvc_device *devh, uvc_stream_ctr * @param[in] fps Frame rate, frames per second */ uvc_error_t winusb_get_stream_ctrl_format_size( - winusb_uvc_device *devh, - uvc_stream_ctrl_t *ctrl, - uint32_t fourcc, - int width, int height, - int fps, - int interface_idx + winusb_uvc_device *devh, + uvc_stream_ctrl_t *ctrl, + uint32_t fourcc, + int width, int height, + int fps, + int interface_idx ) { - winusb_uvc_streaming_interface_t *stream_if; - uvc_format_desc_t *format; - uvc_error_t ret = UVC_SUCCESS; - - stream_if = winusb_uvc_get_stream_if(devh, interface_idx); - if (stream_if == NULL) - { - return UVC_ERROR_INVALID_PARAM; - } - - DL_FOREACH(stream_if->format_descs, format) { - uvc_frame_desc_t *frame; - - if (SWAP_UINT32(fourcc) != *(const uint32_t *)format->guidFormat) - continue; - - DL_FOREACH(format->frame_descs, frame) { - if (frame->wWidth != width || frame->wHeight != height) - continue; - - uint32_t *interval; - - if (frame->intervals) { - for (interval = frame->intervals; *interval; ++interval) { - // allow a fps rate of zero to mean "accept first rate available" - if (10000000 / *interval == (unsigned int)fps || fps == 0) { - - /* get the max values -- we need the interface number to be able - to do this */ - ctrl->bInterfaceNumber = stream_if->bInterfaceNumber; - ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_MAX); - if (ret != UVC_SUCCESS) - { - return ret; - } - - ctrl->bmHint = (1 << 0); /* don't negotiate interval */ - ctrl->bFormatIndex = format->bFormatIndex; - ctrl->bFrameIndex = frame->bFrameIndex; - ctrl->dwFrameInterval = *interval; - - goto found; - } - } - } - else { - uint32_t interval_100ns = 10000000 / fps; - uint32_t interval_offset = interval_100ns - frame->dwMinFrameInterval; - - if (interval_100ns >= frame->dwMinFrameInterval - && interval_100ns <= frame->dwMaxFrameInterval - && !(interval_offset - && (interval_offset % frame->dwFrameIntervalStep))) { - - /* get the max values -- we need the interface number to be able - to do this */ - ctrl->bInterfaceNumber = stream_if->bInterfaceNumber; - ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_MAX); - if (ret != UVC_SUCCESS) - { - return ret; - } - - ctrl->bmHint = (1 << 0); - ctrl->bFormatIndex = format->bFormatIndex; - ctrl->bFrameIndex = frame->bFrameIndex; - ctrl->dwFrameInterval = interval_100ns; - - goto found; - } - } - } - } - - return UVC_ERROR_INVALID_MODE; + winusb_uvc_streaming_interface_t *stream_if; + uvc_format_desc_t *format; + uvc_error_t ret = UVC_SUCCESS; + + stream_if = winusb_uvc_get_stream_if(devh, interface_idx); + if (stream_if == NULL) + { + return UVC_ERROR_INVALID_PARAM; + } + + DL_FOREACH(stream_if->format_descs, format) { + uvc_frame_desc_t *frame; + + if (SWAP_UINT32(fourcc) != *(const uint32_t *)format->guidFormat) + continue; + + DL_FOREACH(format->frame_descs, frame) { + if (frame->wWidth != width || frame->wHeight != height) + continue; + + uint32_t *interval; + + if (frame->intervals) { + for (interval = frame->intervals; *interval; ++interval) { + // allow a fps rate of zero to mean "accept first rate available" + if (10000000 / *interval == (unsigned int)fps || fps == 0) { + + /* get the max values -- we need the interface number to be able + to do this */ + ctrl->bInterfaceNumber = stream_if->bInterfaceNumber; + ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_MAX); + if (ret != UVC_SUCCESS) + { + return ret; + } + + ctrl->bmHint = (1 << 0); /* don't negotiate interval */ + ctrl->bFormatIndex = format->bFormatIndex; + ctrl->bFrameIndex = frame->bFrameIndex; + ctrl->dwFrameInterval = *interval; + + goto found; + } + } + } + else { + uint32_t interval_100ns = 10000000 / fps; + uint32_t interval_offset = interval_100ns - frame->dwMinFrameInterval; + + if (interval_100ns >= frame->dwMinFrameInterval + && interval_100ns <= frame->dwMaxFrameInterval + && !(interval_offset + && (interval_offset % frame->dwFrameIntervalStep))) { + + /* get the max values -- we need the interface number to be able + to do this */ + ctrl->bInterfaceNumber = stream_if->bInterfaceNumber; + ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_MAX); + if (ret != UVC_SUCCESS) + { + return ret; + } + + ctrl->bmHint = (1 << 0); + ctrl->bFormatIndex = format->bFormatIndex; + ctrl->bFrameIndex = frame->bFrameIndex; + ctrl->dwFrameInterval = interval_100ns; + + goto found; + } + } + } + } + + return UVC_ERROR_INVALID_MODE; found: - return winusb_uvc_probe_stream_ctrl(devh, ctrl); + return winusb_uvc_probe_stream_ctrl(devh, ctrl); } /** Get a negotiated streaming control block for some common parameters for all interface @@ -1517,157 +1510,156 @@ uvc_error_t winusb_get_stream_ctrl_format_size( * @param[in] fps Frame rate, frames per second */ uvc_error_t winusb_get_stream_ctrl_format_size_all( - winusb_uvc_device *devh, - uvc_stream_ctrl_t *ctrl, - uint32_t fourcc, - int width, int height, - int fps + winusb_uvc_device *devh, + uvc_stream_ctrl_t *ctrl, + uint32_t fourcc, + int width, int height, + int fps ) { - winusb_uvc_streaming_interface_t *stream_if; - uvc_format_desc_t *format; - uvc_error_t ret = UVC_SUCCESS; - - DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { - - DL_FOREACH(stream_if->format_descs, format) { - uvc_frame_desc_t *frame; - - if (SWAP_UINT32(fourcc) != *(const uint32_t *)format->guidFormat) - continue; - - DL_FOREACH(format->frame_descs, frame) { - if (frame->wWidth != width || frame->wHeight != height) - continue; - - uint32_t *interval; - - if (frame->intervals) { - for (interval = frame->intervals; *interval; ++interval) { - // allow a fps rate of zero to mean "accept first rate available" - if (10000000 / *interval == (unsigned int)fps || fps == 0) { - - /* get the max values -- we need the interface number to be able - to do this */ - ctrl->bInterfaceNumber = stream_if->bInterfaceNumber; - ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_MAX); - if (ret != UVC_SUCCESS) - { - return ret; - } - - ctrl->bmHint = (1 << 0); /* don't negotiate interval */ - ctrl->bFormatIndex = format->bFormatIndex; - ctrl->bFrameIndex = frame->bFrameIndex; - ctrl->dwFrameInterval = *interval; - - goto found; - } - } - } - else { - uint32_t interval_100ns = 10000000 / fps; - uint32_t interval_offset = interval_100ns - frame->dwMinFrameInterval; - - if (interval_100ns >= frame->dwMinFrameInterval - && interval_100ns <= frame->dwMaxFrameInterval - && !(interval_offset - && (interval_offset % frame->dwFrameIntervalStep))) { - - /* get the max values -- we need the interface number to be able - to do this */ - ctrl->bInterfaceNumber = stream_if->bInterfaceNumber; - ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_MAX); - if (ret != UVC_SUCCESS) - { - return ret; - } - - ctrl->bmHint = (1 << 0); - ctrl->bFormatIndex = format->bFormatIndex; - ctrl->bFrameIndex = frame->bFrameIndex; - ctrl->dwFrameInterval = interval_100ns; - - goto found; - } - } - } - } - } - - return UVC_ERROR_INVALID_MODE; + winusb_uvc_streaming_interface_t *stream_if; + uvc_format_desc_t *format; + uvc_error_t ret = UVC_SUCCESS; + + DL_FOREACH(devh->deviceData.stream_ifs, stream_if) { + + DL_FOREACH(stream_if->format_descs, format) { + uvc_frame_desc_t *frame; + + if (SWAP_UINT32(fourcc) != *(const uint32_t *)format->guidFormat) + continue; + + DL_FOREACH(format->frame_descs, frame) { + if (frame->wWidth != width || frame->wHeight != height) + continue; + + uint32_t *interval; + + if (frame->intervals) { + for (interval = frame->intervals; *interval; ++interval) { + // allow a fps rate of zero to mean "accept first rate available" + if (10000000 / *interval == (unsigned int)fps || fps == 0) { + + /* get the max values -- we need the interface number to be able + to do this */ + ctrl->bInterfaceNumber = stream_if->bInterfaceNumber; + ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_MAX); + if (ret != UVC_SUCCESS) + { + return ret; + } + + ctrl->bmHint = (1 << 0); /* don't negotiate interval */ + ctrl->bFormatIndex = format->bFormatIndex; + ctrl->bFrameIndex = frame->bFrameIndex; + ctrl->dwFrameInterval = *interval; + + goto found; + } + } + } + else { + uint32_t interval_100ns = 10000000 / fps; + uint32_t interval_offset = interval_100ns - frame->dwMinFrameInterval; + + if (interval_100ns >= frame->dwMinFrameInterval + && interval_100ns <= frame->dwMaxFrameInterval + && !(interval_offset + && (interval_offset % frame->dwFrameIntervalStep))) { + + /* get the max values -- we need the interface number to be able + to do this */ + ctrl->bInterfaceNumber = stream_if->bInterfaceNumber; + ret = winusb_uvc_query_stream_ctrl(devh, ctrl, 1, UVC_GET_MAX); + if (ret != UVC_SUCCESS) + { + return ret; + } + + ctrl->bmHint = (1 << 0); + ctrl->bFormatIndex = format->bFormatIndex; + ctrl->bFrameIndex = frame->bFrameIndex; + ctrl->dwFrameInterval = interval_100ns; + + goto found; + } + } + } + } + } + + return UVC_ERROR_INVALID_MODE; found: - return winusb_uvc_probe_stream_ctrl(devh, ctrl); + return winusb_uvc_probe_stream_ctrl(devh, ctrl); } bool wait_for_async_operation(WINUSB_INTERFACE_HANDLE interfaceHandle, - int ep, OVERLAPPED &hOvl, ULONG &lengthTransferred, USHORT timeout) + int ep, OVERLAPPED &hOvl, ULONG &lengthTransferred, USHORT timeout) { - if (GetOverlappedResult(interfaceHandle, &hOvl, &lengthTransferred, FALSE)) - return true; - - auto lastResult = GetLastError(); - if (lastResult == ERROR_IO_PENDING || lastResult == ERROR_IO_INCOMPLETE) - { - WaitForSingleObject(hOvl.hEvent, timeout); - auto res = GetOverlappedResult(interfaceHandle, &hOvl, &lengthTransferred, FALSE); - if (res != 1) - { - return false; - } - } - else - { - lengthTransferred = 0; - WinUsb_ResetPipe(interfaceHandle, ep); - return false; - } - - return true; + if (GetOverlappedResult(interfaceHandle, &hOvl, &lengthTransferred, FALSE)) + return true; + + auto lastResult = GetLastError(); + if (lastResult == ERROR_IO_PENDING || lastResult == ERROR_IO_INCOMPLETE) + { + WaitForSingleObject(hOvl.hEvent, timeout); + auto res = GetOverlappedResult(interfaceHandle, &hOvl, &lengthTransferred, FALSE); + if (res != 1) + { + return false; + } + } + else + { + lengthTransferred = 0; + WinUsb_ResetPipe(interfaceHandle, ep); + return false; + } + + return true; } void poll_interrupts(WINUSB_INTERFACE_HANDLE handle, int ep, uint16_t timeout) { - static const unsigned short interrupt_buf_size = 0x400; - uint8_t buffer[interrupt_buf_size]; /* 64 byte transfer buffer - dedicated channel*/ - ULONG num_bytes = 0; /* Actual bytes transferred. */ - OVERLAPPED hOvl; - safe_handle sh(CreateEvent(nullptr, false, false, nullptr)); - hOvl.hEvent = sh.GetHandle(); - int res = WinUsb_ReadPipe(handle, ep, buffer, interrupt_buf_size, &num_bytes, &hOvl); - if (0 == res) - { - auto lastError = GetLastError(); - if (lastError == ERROR_IO_PENDING) - { - auto sts = wait_for_async_operation(handle, ep, hOvl, num_bytes, timeout); - lastError = GetLastError(); - if (lastError == ERROR_OPERATION_ABORTED) - { - LOG_ERROR(__FUNCTION__ << " : Read interrupt_ep bytes failed. err=" << GetLastError()); - } - if (!sts) - return; - } - else - { - WinUsb_ResetPipe(handle, ep); - LOG_ERROR(__FUNCTION__ << " : Read interrupt_ep bytes failed. err=" << GetLastError()); - return; - } - - if (num_bytes == 0) - return; - - // TODO: Complete XU set instead of using retries - } - else - { - // todo exception - perror("receiving interrupt_ep bytes failed"); - fprintf(stderr, "Error receiving message.\n"); - } + static const unsigned short interrupt_buf_size = 0x400; + uint8_t buffer[interrupt_buf_size]; /* 64 byte transfer buffer - dedicated channel*/ + ULONG num_bytes = 0; /* Actual bytes transferred. */ + OVERLAPPED hOvl; + safe_handle sh(CreateEvent(nullptr, false, false, nullptr)); + hOvl.hEvent = sh.GetHandle(); + int res = WinUsb_ReadPipe(handle, ep, buffer, interrupt_buf_size, &num_bytes, &hOvl); + if (0 == res) + { + auto lastError = GetLastError(); + if (lastError == ERROR_IO_PENDING) + { + auto sts = wait_for_async_operation(handle, ep, hOvl, num_bytes, timeout); + lastError = GetLastError(); + if (lastError == ERROR_OPERATION_ABORTED) + { + LOG_ERROR(__FUNCTION__ << " : Read interrupt_ep bytes failed. err=" << GetLastError()); + } + if (!sts) + return; + } + else + { + WinUsb_ResetPipe(handle, ep); + LOG_ERROR(__FUNCTION__ << " : Read interrupt_ep bytes failed. err=" << GetLastError()); + return; + } + + if (num_bytes == 0) + return; + + // TODO: Complete XU set instead of using retries + } + else + { + // todo exception + LOG_ERROR(__FUNCTION__ << "receiving interrupt_ep bytes failed. err=" << GetLastError()); + } } /** @@ -1681,24 +1673,24 @@ void poll_interrupts(WINUSB_INTERFACE_HANDLE handle, int ep, uint16_t timeout) * @ingroup ctrl */ int uvc_get_ctrl_len(winusb_uvc_device *devh, uint8_t unit, uint8_t ctrl) { - unsigned char buf[2]; - - if (!devh) - return UVC_ERROR_NO_DEVICE; - - int ret = winusb_SendControl( - devh->winusbHandle, - UVC_REQ_TYPE_INTERFACE_GET, - UVC_GET_LEN, - ctrl << 8, - unit << 8 | devh->deviceData.ctrl_if.bInterfaceNumber, - buf, - 2); - - if (ret < 0) - return ret; - else - return (unsigned short)SW_TO_SHORT(buf); + unsigned char buf[2]; + + if (!devh) + return UVC_ERROR_NO_DEVICE; + + int ret = winusb_SendControl( + devh->winusbHandle, + UVC_REQ_TYPE_INTERFACE_GET, + UVC_GET_LEN, + ctrl << 8, + unit << 8 | devh->deviceData.ctrl_if.bInterfaceNumber, + buf, + 2); + + if (ret < 0) + return ret; + else + return (unsigned short)SW_TO_SHORT(buf); } /** @@ -1715,16 +1707,16 @@ int uvc_get_ctrl_len(winusb_uvc_device *devh, uint8_t unit, uint8_t ctrl) { * @ingroup ctrl */ int uvc_get_ctrl(winusb_uvc_device *devh, uint8_t unit, uint8_t ctrl, void *data, int len, enum uvc_req_code req_code) { - if (!devh) - return UVC_ERROR_NO_DEVICE; - - return winusb_SendControl( - devh->winusbHandle, - UVC_REQ_TYPE_INTERFACE_GET, req_code, - ctrl << 8, - unit << 8 | devh->deviceData.ctrl_if.bInterfaceNumber, // XXX saki - static_cast(data), - len); + if (!devh) + return UVC_ERROR_NO_DEVICE; + + return winusb_SendControl( + devh->winusbHandle, + UVC_REQ_TYPE_INTERFACE_GET, req_code, + ctrl << 8, + unit << 8 | devh->deviceData.ctrl_if.bInterfaceNumber, // XXX saki + static_cast(data), + len); } /** @@ -1740,60 +1732,60 @@ int uvc_get_ctrl(winusb_uvc_device *devh, uint8_t unit, uint8_t ctrl, void *data * @ingroup ctrl */ int uvc_set_ctrl(winusb_uvc_device *devh, uint8_t unit, uint8_t ctrl, void *data, int len) { - if (!devh) - return UVC_ERROR_NO_DEVICE; - - return winusb_SendControl( - devh->winusbHandle, - UVC_REQ_TYPE_INTERFACE_SET, UVC_SET_CUR, - ctrl << 8, - unit << 8 | devh->deviceData.ctrl_if.bInterfaceNumber, // XXX saki - static_cast(data), - len); + if (!devh) + return UVC_ERROR_NO_DEVICE; + + return winusb_SendControl( + devh->winusbHandle, + UVC_REQ_TYPE_INTERFACE_SET, UVC_SET_CUR, + ctrl << 8, + unit << 8 | devh->deviceData.ctrl_if.bInterfaceNumber, // XXX saki + static_cast(data), + len); } uvc_error_t uvc_get_power_mode(winusb_uvc_device *devh, enum uvc_device_power_mode *mode, enum uvc_req_code req_code) { - uint8_t mode_char; - int ret; - if (!devh) - return UVC_ERROR_NO_DEVICE; - - ret = winusb_SendControl( - devh->winusbHandle, - UVC_REQ_TYPE_INTERFACE_GET, req_code, - UVC_VC_VIDEO_POWER_MODE_CONTROL << 8, - devh->deviceData.ctrl_if.bInterfaceNumber, - (unsigned char *)&mode_char, - sizeof(mode_char)); - - if (ret == 1) { - *mode = static_cast(mode_char); - return UVC_SUCCESS; - } - else { - return static_cast(ret); - } + uint8_t mode_char; + int ret; + if (!devh) + return UVC_ERROR_NO_DEVICE; + + ret = winusb_SendControl( + devh->winusbHandle, + UVC_REQ_TYPE_INTERFACE_GET, req_code, + UVC_VC_VIDEO_POWER_MODE_CONTROL << 8, + devh->deviceData.ctrl_if.bInterfaceNumber, + (unsigned char *)&mode_char, + sizeof(mode_char)); + + if (ret == 1) { + *mode = static_cast(mode_char); + return UVC_SUCCESS; + } + else { + return static_cast(ret); + } } uvc_error_t uvc_set_power_mode(winusb_uvc_device *devh, enum uvc_device_power_mode mode) { - uint8_t mode_char = mode; - int ret; - - if (!devh) - return UVC_ERROR_NO_DEVICE; - - ret = winusb_SendControl( - devh->winusbHandle, - UVC_REQ_TYPE_INTERFACE_SET, UVC_SET_CUR, - UVC_VC_VIDEO_POWER_MODE_CONTROL << 8, - devh->deviceData.ctrl_if.bInterfaceNumber, - (unsigned char *)&mode_char, - sizeof(mode_char)); - - if (ret == 1) - return UVC_SUCCESS; - else - return static_cast(ret); + uint8_t mode_char = mode; + int ret; + + if (!devh) + return UVC_ERROR_NO_DEVICE; + + ret = winusb_SendControl( + devh->winusbHandle, + UVC_REQ_TYPE_INTERFACE_SET, UVC_SET_CUR, + UVC_VC_VIDEO_POWER_MODE_CONTROL << 8, + devh->deviceData.ctrl_if.bInterfaceNumber, + (unsigned char *)&mode_char, + sizeof(mode_char)); + + if (ret == 1) + return UVC_SUCCESS; + else + return static_cast(ret); } // Sending control packet using vendor-defined control transfer directed to WinUSB interface @@ -1809,497 +1801,496 @@ uvc_error_t uvc_set_power_mode(winusb_uvc_device *devh, enum uvc_device_power_mo // Length: Number of bytes to transfer (0x0000-0xFFFF) int winusb_SendControl(WINUSB_INTERFACE_HANDLE ihandle, int requestType, int request, int value, int index, unsigned char *buffer, int buflen) { - WINUSB_SETUP_PACKET setupPacket; - ULONG lengthOutput; - - setupPacket.RequestType = requestType; - setupPacket.Request = request; - setupPacket.Value = value; - setupPacket.Index = index; - setupPacket.Length = buflen; - - if (!WinUsb_ControlTransfer(ihandle, setupPacket, buffer, buflen, &lengthOutput, NULL)) - { - return -1; - } - else - { - return lengthOutput; - } - - return 0; + WINUSB_SETUP_PACKET setupPacket; + ULONG lengthOutput; + + setupPacket.RequestType = requestType; + setupPacket.Request = request; + setupPacket.Value = value; + setupPacket.Index = index; + setupPacket.Length = buflen; + + if (!WinUsb_ControlTransfer(ihandle, setupPacket, buffer, buflen, &lengthOutput, NULL)) + { + return -1; + } + else + { + return lengthOutput; + } + + return 0; } uvc_error_t winusb_find_devices(const std::string &uvc_interface, int vid, int pid, winusb_uvc_device ***devs, int& devs_count) { - GUID guid; - std::wstring guidWStr(uvc_interface.begin(), uvc_interface.end()); - CLSIDFromString(guidWStr.c_str(), static_cast(&guid)); - - HDEVINFO hDevInfo; - SP_DEVICE_INTERFACE_DATA DevIntfData; - PSP_DEVICE_INTERFACE_DETAIL_DATA DevIntfDetailData; - SP_DEVINFO_DATA DevData; - winusb_uvc_device **list_internal; - int num_uvc_devices = 0; - DWORD dwSize = 0; - DWORD dwMemberIdx = 0; - uvc_error_t ret = UVC_SUCCESS; - - list_internal = (winusb_uvc_device **)malloc(sizeof(*list_internal)); - *list_internal = NULL; - - // Return a handle to device information set with connected IVCAM device - hDevInfo = SetupDiGetClassDevs(&guid, NULL, 0, DIGCF_DEVICEINTERFACE | DIGCF_PRESENT); - if (hDevInfo != INVALID_HANDLE_VALUE) - { - DevIntfData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); - dwMemberIdx = 0; - - // Enumerates IVCAM device - SetupDiEnumDeviceInterfaces(hDevInfo, NULL, &guid, dwMemberIdx, &DevIntfData); - - DevIntfDetailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA)malloc(2048); - - while (GetLastError() != ERROR_NO_MORE_ITEMS) - { - DevData.cbSize = sizeof(DevData); - - // Returns required buffer size for saving device interface details - SetupDiGetDeviceInterfaceDetail(hDevInfo, &DevIntfData, NULL, 0, &dwSize, NULL); - - DevIntfDetailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); - - // Returns details about connected device - if (SetupDiGetDeviceInterfaceDetail(hDevInfo, &DevIntfData, DevIntfDetailData, dwSize, &dwSize, &DevData) == TRUE) - { - - // Create a handle for I/O operations to the IVCAM device - HANDLE Devicehandle = CreateFile(DevIntfDetailData->DevicePath, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); - if (Devicehandle != INVALID_HANDLE_VALUE) - { - WINUSB_INTERFACE_HANDLE winusbHandle; - - // Create WinUSB device handle for the IVCAM device - if (WinUsb_Initialize(Devicehandle, &winusbHandle) == TRUE) - { - USB_DEVICE_DESCRIPTOR deviceDescriptor; - ULONG returnLength; - - // Returns IVCAM device descriptor which includes PID/VID info - if (!WinUsb_GetDescriptor(winusbHandle, USB_DEVICE_DESCRIPTOR_TYPE, 0, 0, (PUCHAR)&deviceDescriptor, sizeof(USB_DEVICE_DESCRIPTOR), &returnLength)) - { - LOG_ERROR("WinUsb_GetDescriptor failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - else - { - if (deviceDescriptor.idVendor == vid && deviceDescriptor.idProduct == pid) - { - winusb_uvc_device *winUsbDevice = new winusb_uvc_device; - winUsbDevice->deviceHandle = NULL; - winUsbDevice->winusbHandle = NULL; - winUsbDevice->pid = pid; - winUsbDevice->vid = vid; - size_t devicePathLength = wcslen(DevIntfDetailData->DevicePath); - winUsbDevice->devPath = new WCHAR[devicePathLength + 1]; - wcscpy_s(winUsbDevice->devPath, devicePathLength + 1, DevIntfDetailData->DevicePath); - - num_uvc_devices++; - list_internal = (winusb_uvc_device **)realloc(list_internal, (num_uvc_devices + 1) * sizeof(*list_internal)); - list_internal[num_uvc_devices - 1] = winUsbDevice; - list_internal[num_uvc_devices] = NULL; - } - } - - WinUsb_Free(winusbHandle); - } - else - { - LOG_ERROR("WinUsb_Initialize failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - } - else - { - auto error = GetLastError(); - LOG_ERROR("CreateFile failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - - if (Devicehandle != INVALID_HANDLE_VALUE) - { - CloseHandle(Devicehandle); - } - } - else - { - LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - - // Continue looping on all connected IVCAM devices - SetupDiEnumDeviceInterfaces(hDevInfo, NULL, &guid, ++dwMemberIdx, &DevIntfData); - } - - if (DevIntfDetailData != NULL) - { - free(DevIntfDetailData); - } - - SetupDiDestroyDeviceInfoList(hDevInfo); - - *devs = list_internal; - devs_count = num_uvc_devices; - } - else - { - LOG_ERROR("SetupDiGetClassDevs failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - - return ret; + GUID guid; + std::wstring guidWStr(uvc_interface.begin(), uvc_interface.end()); + CLSIDFromString(guidWStr.c_str(), static_cast(&guid)); + + HDEVINFO hDevInfo; + SP_DEVICE_INTERFACE_DATA DevIntfData; + PSP_DEVICE_INTERFACE_DETAIL_DATA DevIntfDetailData; + SP_DEVINFO_DATA DevData; + winusb_uvc_device **list_internal; + int num_uvc_devices = 0; + DWORD dwSize = 0; + DWORD dwMemberIdx = 0; + uvc_error_t ret = UVC_SUCCESS; + + list_internal = (winusb_uvc_device **)malloc(sizeof(*list_internal)); + *list_internal = NULL; + + // Return a handle to device information set with connected IVCAM device + hDevInfo = SetupDiGetClassDevs(&guid, NULL, 0, DIGCF_DEVICEINTERFACE | DIGCF_PRESENT); + if (hDevInfo != INVALID_HANDLE_VALUE) + { + DevIntfData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + dwMemberIdx = 0; + + // Enumerates IVCAM device + SetupDiEnumDeviceInterfaces(hDevInfo, NULL, &guid, dwMemberIdx, &DevIntfData); + + DevIntfDetailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA)malloc(2048); + + while (GetLastError() != ERROR_NO_MORE_ITEMS) + { + DevData.cbSize = sizeof(DevData); + + // Returns required buffer size for saving device interface details + SetupDiGetDeviceInterfaceDetail(hDevInfo, &DevIntfData, NULL, 0, &dwSize, NULL); + + DevIntfDetailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); + + // Returns details about connected device + if (SetupDiGetDeviceInterfaceDetail(hDevInfo, &DevIntfData, DevIntfDetailData, dwSize, &dwSize, &DevData) == TRUE) + { + + // Create a handle for I/O operations to the IVCAM device + HANDLE Devicehandle = CreateFile(DevIntfDetailData->DevicePath, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); + if (Devicehandle != INVALID_HANDLE_VALUE) + { + WINUSB_INTERFACE_HANDLE winusbHandle; + + // Create WinUSB device handle for the IVCAM device + if (WinUsb_Initialize(Devicehandle, &winusbHandle) == TRUE) + { + USB_DEVICE_DESCRIPTOR deviceDescriptor; + ULONG returnLength; + + // Returns IVCAM device descriptor which includes PID/VID info + if (!WinUsb_GetDescriptor(winusbHandle, USB_DEVICE_DESCRIPTOR_TYPE, 0, 0, (PUCHAR)&deviceDescriptor, sizeof(USB_DEVICE_DESCRIPTOR), &returnLength)) + { + LOG_ERROR("WinUsb_GetDescriptor failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + else + { + if (deviceDescriptor.idVendor == vid && deviceDescriptor.idProduct == pid) + { + winusb_uvc_device *winUsbDevice = new winusb_uvc_device; + winUsbDevice->deviceHandle = NULL; + winUsbDevice->winusbHandle = NULL; + winUsbDevice->pid = pid; + winUsbDevice->vid = vid; + size_t devicePathLength = wcslen(DevIntfDetailData->DevicePath); + winUsbDevice->devPath = new WCHAR[devicePathLength + 1]; + wcscpy_s(winUsbDevice->devPath, devicePathLength + 1, DevIntfDetailData->DevicePath); + + num_uvc_devices++; + list_internal = (winusb_uvc_device **)realloc(list_internal, (num_uvc_devices + 1) * sizeof(*list_internal)); + list_internal[num_uvc_devices - 1] = winUsbDevice; + list_internal[num_uvc_devices] = NULL; + } + } + + WinUsb_Free(winusbHandle); + } + else + { + LOG_ERROR("WinUsb_Initialize failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + } + else + { + auto error = GetLastError(); + LOG_ERROR("CreateFile failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + + if (Devicehandle != INVALID_HANDLE_VALUE) + { + CloseHandle(Devicehandle); + } + } + else + { + LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + + // Continue looping on all connected IVCAM devices + SetupDiEnumDeviceInterfaces(hDevInfo, NULL, &guid, ++dwMemberIdx, &DevIntfData); + } + + if (DevIntfDetailData != NULL) + { + free(DevIntfDetailData); + } + + SetupDiDestroyDeviceInfoList(hDevInfo); + + *devs = list_internal; + devs_count = num_uvc_devices; + } + else + { + LOG_ERROR("SetupDiGetClassDevs failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + + return ret; } // Return list of all connected IVCAM devices uvc_error_t winusb_find_devices(winusb_uvc_device ***devs, int vid, int pid) { - // Intel(R) RealSense(TM) 415 Depth - MI 0 - // bInterfaceNumber 0 video control - endpoint 0x87 (FW->Host) - // bInterfaceNumber 1 video stream - endpoint 0x82 (FW->Host) - // bInterfaceNumber 2 video stream - endpoint 0x83 (FW->Host) - //const GUID IVCAM_WIN_USB_DEVICE_GUID = { 0xe659c3ec, 0xbf3c, 0x48a5,{ 0x81, 0x92, 0x30, 0x73, 0xe8, 0x22, 0xd7, 0xcd } }; - - // Intel(R) RealSense(TM) 415 RGB - MI 3 - // bInterfaceNumber 3 video control - // bInterfaceNumber 4 video stream - endpoint 0x84 (FW->Host) - const GUID IVCAM_WIN_USB_DEVICE_GUID = { 0x50537bc3, 0x2919, 0x452d,{ 0x88, 0xa9, 0xb1, 0x3b, 0xbf, 0x7d, 0x24, 0x59 } }; - - HDEVINFO hDevInfo; - SP_DEVICE_INTERFACE_DATA DevIntfData; - PSP_DEVICE_INTERFACE_DETAIL_DATA DevIntfDetailData; - SP_DEVINFO_DATA DevData; - winusb_uvc_device **list_internal; - int num_uvc_devices = 0; - DWORD dwSize = 0; - DWORD dwMemberIdx = 0; - uvc_error_t ret = UVC_SUCCESS; - - list_internal = (winusb_uvc_device **)malloc(sizeof(*list_internal)); - *list_internal = NULL; - - // Return a handle to device information set with connected IVCAM device - hDevInfo = SetupDiGetClassDevs(&IVCAM_WIN_USB_DEVICE_GUID, NULL, 0, DIGCF_DEVICEINTERFACE | DIGCF_PRESENT); - if (hDevInfo != INVALID_HANDLE_VALUE) - { - DevIntfData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); - dwMemberIdx = 0; - - // Enumerates IVCAM device - SetupDiEnumDeviceInterfaces(hDevInfo, NULL, &IVCAM_WIN_USB_DEVICE_GUID, dwMemberIdx, &DevIntfData); - - DevIntfDetailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA)malloc(2048); - - while (GetLastError() != ERROR_NO_MORE_ITEMS) - { - DevData.cbSize = sizeof(DevData); - - // Returns required buffer size for saving device interface details - SetupDiGetDeviceInterfaceDetail(hDevInfo, &DevIntfData, NULL, 0, &dwSize, NULL); - - DevIntfDetailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); - - // Returns details about connected device - if (SetupDiGetDeviceInterfaceDetail(hDevInfo, &DevIntfData, DevIntfDetailData, dwSize, &dwSize, &DevData) == TRUE) - { - - // Create a handle for I/O operations to the IVCAM device - HANDLE Devicehandle = CreateFile(DevIntfDetailData->DevicePath, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); - if (Devicehandle != INVALID_HANDLE_VALUE) - { - WINUSB_INTERFACE_HANDLE winusbHandle; - - // Create WinUSB device handle for the IVCAM device - if (WinUsb_Initialize(Devicehandle, &winusbHandle) == TRUE) - { - USB_DEVICE_DESCRIPTOR deviceDescriptor; - ULONG returnLength; - - // Returns IVCAM device descriptor which includes PID/VID info - if (!WinUsb_GetDescriptor(winusbHandle, USB_DEVICE_DESCRIPTOR_TYPE, 0, 0, (PUCHAR)&deviceDescriptor, sizeof(USB_DEVICE_DESCRIPTOR), &returnLength)) - { - LOG_ERROR("WinUsb_GetDescriptor failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - else - { - if (deviceDescriptor.idVendor == vid && deviceDescriptor.idProduct == pid) - { - winusb_uvc_device *winUsbDevice = new winusb_uvc_device; - winUsbDevice->deviceHandle = NULL; - winUsbDevice->winusbHandle = NULL; - winUsbDevice->pid = pid; - winUsbDevice->vid = vid; - size_t devicePathLength = wcslen(DevIntfDetailData->DevicePath); - winUsbDevice->devPath = new WCHAR[devicePathLength + 1]; - wcscpy_s(winUsbDevice->devPath, devicePathLength + 1, DevIntfDetailData->DevicePath); - - num_uvc_devices++; - list_internal = (winusb_uvc_device **)realloc(list_internal, (num_uvc_devices + 1) * sizeof(*list_internal)); - - list_internal[num_uvc_devices - 1] = winUsbDevice; - list_internal[num_uvc_devices] = NULL; - } - } - - WinUsb_Free(winusbHandle); - } - else - { - LOG_ERROR("WinUsb_Initialize failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - } - else - { - LOG_ERROR("CreateFile failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - - if (Devicehandle != INVALID_HANDLE_VALUE) - { - CloseHandle(Devicehandle); - } - } - else - { - LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - - // Continue looping on all connected IVCAM devices - SetupDiEnumDeviceInterfaces(hDevInfo, NULL, &IVCAM_WIN_USB_DEVICE_GUID, ++dwMemberIdx, &DevIntfData); - } - - if (DevIntfDetailData != NULL) - { - free(DevIntfDetailData); - } - - SetupDiDestroyDeviceInfoList(hDevInfo); - - *devs = list_internal; - } - else - { - LOG_ERROR("SetupDiGetClassDevs failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - } - - return ret; + // Intel(R) RealSense(TM) 415 Depth - MI 0 + // bInterfaceNumber 0 video control - endpoint 0x87 (FW->Host) + // bInterfaceNumber 1 video stream - endpoint 0x82 (FW->Host) + // bInterfaceNumber 2 video stream - endpoint 0x83 (FW->Host) + //const GUID IVCAM_WIN_USB_DEVICE_GUID = { 0xe659c3ec, 0xbf3c, 0x48a5,{ 0x81, 0x92, 0x30, 0x73, 0xe8, 0x22, 0xd7, 0xcd } }; + + // Intel(R) RealSense(TM) 415 RGB - MI 3 + // bInterfaceNumber 3 video control + // bInterfaceNumber 4 video stream - endpoint 0x84 (FW->Host) + const GUID IVCAM_WIN_USB_DEVICE_GUID = { 0x50537bc3, 0x2919, 0x452d,{ 0x88, 0xa9, 0xb1, 0x3b, 0xbf, 0x7d, 0x24, 0x59 } }; + + HDEVINFO hDevInfo; + SP_DEVICE_INTERFACE_DATA DevIntfData; + PSP_DEVICE_INTERFACE_DETAIL_DATA DevIntfDetailData; + SP_DEVINFO_DATA DevData; + winusb_uvc_device **list_internal; + int num_uvc_devices = 0; + DWORD dwSize = 0; + DWORD dwMemberIdx = 0; + uvc_error_t ret = UVC_SUCCESS; + + list_internal = (winusb_uvc_device **)malloc(sizeof(*list_internal)); + *list_internal = NULL; + + // Return a handle to device information set with connected IVCAM device + hDevInfo = SetupDiGetClassDevs(&IVCAM_WIN_USB_DEVICE_GUID, NULL, 0, DIGCF_DEVICEINTERFACE | DIGCF_PRESENT); + if (hDevInfo != INVALID_HANDLE_VALUE) + { + DevIntfData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + dwMemberIdx = 0; + + // Enumerates IVCAM device + SetupDiEnumDeviceInterfaces(hDevInfo, NULL, &IVCAM_WIN_USB_DEVICE_GUID, dwMemberIdx, &DevIntfData); + + DevIntfDetailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA)malloc(2048); + + while (GetLastError() != ERROR_NO_MORE_ITEMS) + { + DevData.cbSize = sizeof(DevData); + + // Returns required buffer size for saving device interface details + SetupDiGetDeviceInterfaceDetail(hDevInfo, &DevIntfData, NULL, 0, &dwSize, NULL); + + DevIntfDetailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); + + // Returns details about connected device + if (SetupDiGetDeviceInterfaceDetail(hDevInfo, &DevIntfData, DevIntfDetailData, dwSize, &dwSize, &DevData) == TRUE) + { + + // Create a handle for I/O operations to the IVCAM device + HANDLE Devicehandle = CreateFile(DevIntfDetailData->DevicePath, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_OVERLAPPED, NULL); + if (Devicehandle != INVALID_HANDLE_VALUE) + { + WINUSB_INTERFACE_HANDLE winusbHandle; + + // Create WinUSB device handle for the IVCAM device + if (WinUsb_Initialize(Devicehandle, &winusbHandle) == TRUE) + { + USB_DEVICE_DESCRIPTOR deviceDescriptor; + ULONG returnLength; + + // Returns IVCAM device descriptor which includes PID/VID info + if (!WinUsb_GetDescriptor(winusbHandle, USB_DEVICE_DESCRIPTOR_TYPE, 0, 0, (PUCHAR)&deviceDescriptor, sizeof(USB_DEVICE_DESCRIPTOR), &returnLength)) + { + LOG_ERROR("WinUsb_GetDescriptor failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + else + { + if (deviceDescriptor.idVendor == vid && deviceDescriptor.idProduct == pid) + { + winusb_uvc_device *winUsbDevice = new winusb_uvc_device; + winUsbDevice->deviceHandle = NULL; + winUsbDevice->winusbHandle = NULL; + winUsbDevice->pid = pid; + winUsbDevice->vid = vid; + size_t devicePathLength = wcslen(DevIntfDetailData->DevicePath); + winUsbDevice->devPath = new WCHAR[devicePathLength + 1]; + wcscpy_s(winUsbDevice->devPath, devicePathLength + 1, DevIntfDetailData->DevicePath); + + num_uvc_devices++; + list_internal = (winusb_uvc_device **)realloc(list_internal, (num_uvc_devices + 1) * sizeof(*list_internal)); + + list_internal[num_uvc_devices - 1] = winUsbDevice; + list_internal[num_uvc_devices] = NULL; + } + } + + WinUsb_Free(winusbHandle); + } + else + { + LOG_ERROR("WinUsb_Initialize failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + } + else + { + LOG_ERROR("CreateFile failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + + if (Devicehandle != INVALID_HANDLE_VALUE) + { + CloseHandle(Devicehandle); + } + } + else + { + LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + + // Continue looping on all connected IVCAM devices + SetupDiEnumDeviceInterfaces(hDevInfo, NULL, &IVCAM_WIN_USB_DEVICE_GUID, ++dwMemberIdx, &DevIntfData); + } + + if (DevIntfDetailData != NULL) + { + free(DevIntfDetailData); + } + + SetupDiDestroyDeviceInfoList(hDevInfo); + + *devs = list_internal; + } + else + { + LOG_ERROR("SetupDiGetClassDevs failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + } + + return ret; } // Iterate over all descriptors and parse all Interface and Endpoint descriptors void winusb_uvc_parse_config_descriptors(USB_CONFIGURATION_DESCRIPTOR *cfgDesc, winusb_uvc_interfaces **interfaces) { - int length = cfgDesc->wTotalLength - sizeof(USB_CONFIGURATION_DESCRIPTOR); - int curLocation = 0; - int interfaceDescStart = -1; - winusb_uvc_interfaces *descInterfaces = new winusb_uvc_interfaces; - - memset(descInterfaces, 0, sizeof(winusb_uvc_interfaces)); - winusb_uvc_interface *curInterface = NULL; - USB_INTERFACE_DESCRIPTOR *curDescriptor = NULL; - descInterfaces->numInterfaces = 0; - - PUCHAR descriptors = (PUCHAR)cfgDesc; - descriptors += cfgDesc->bLength; - - // iterate over all descriptors - do { - int descLen = descriptors[curLocation]; - //printf("parse : length : %d type : %x first byte: %x\n", descLen, descriptors[curLocation + 1], descriptors[curLocation + 2]); - if (descriptors[curLocation + 1] == USB_INTERFACE_DESCRIPTOR_TYPE || (curLocation + descLen) >= length) - { - if (interfaceDescStart != -1) - { - // Saving previous interface with all extra data afterwards (up to this interface) - memcpy(&curInterface->desc, &descriptors[interfaceDescStart], sizeof(curInterface->desc)); - interfaceDescStart += sizeof(USB_INTERFACE_DESCRIPTOR); - curInterface->extraLength = curLocation - interfaceDescStart; - curInterface->extra = new UCHAR[curInterface->extraLength]; - memcpy(curInterface->extra, &descriptors[interfaceDescStart], curInterface->extraLength); - curInterface->winusbInterfaceNumber = descInterfaces->numInterfaces; - descInterfaces->numInterfaces++; - } - - if ((curLocation + descLen) < length) - { - // Setting to new found interface descriptor pointer - interfaceDescStart = curLocation; - curDescriptor = (USB_INTERFACE_DESCRIPTOR *)&descriptors[interfaceDescStart]; - curInterface = &descInterfaces->iface[curDescriptor->bInterfaceNumber]; - curInterface->numEndpoints = 0; - } - } - else if (descriptors[curLocation + 1] == USB_ENDPOINT_DESCRIPTOR_TYPE) - { - memcpy(&curInterface->endpoints[curInterface->numEndpoints], &descriptors[curLocation], descLen); - curInterface->numEndpoints++; - } - - curLocation += descLen; - //printf("length = %d\n", curLocation); - } while (curLocation < length); - - *interfaces = descInterfaces; + int length = cfgDesc->wTotalLength - sizeof(USB_CONFIGURATION_DESCRIPTOR); + int curLocation = 0; + int interfaceDescStart = -1; + winusb_uvc_interfaces *descInterfaces = new winusb_uvc_interfaces; + + memset(descInterfaces, 0, sizeof(winusb_uvc_interfaces)); + winusb_uvc_interface *curInterface = NULL; + USB_INTERFACE_DESCRIPTOR *curDescriptor = NULL; + descInterfaces->numInterfaces = 0; + + PUCHAR descriptors = (PUCHAR)cfgDesc; + descriptors += cfgDesc->bLength; + + // iterate over all descriptors + do { + int descLen = descriptors[curLocation]; + //printf("parse : length : %d type : %x first byte: %x\n", descLen, descriptors[curLocation + 1], descriptors[curLocation + 2]); + if (descriptors[curLocation + 1] == USB_INTERFACE_DESCRIPTOR_TYPE || (curLocation + descLen) >= length) + { + if (interfaceDescStart != -1) + { + // Saving previous interface with all extra data afterwards (up to this interface) + memcpy(&curInterface->desc, &descriptors[interfaceDescStart], sizeof(curInterface->desc)); + interfaceDescStart += sizeof(USB_INTERFACE_DESCRIPTOR); + curInterface->extraLength = curLocation - interfaceDescStart; + curInterface->extra = new UCHAR[curInterface->extraLength]; + memcpy(curInterface->extra, &descriptors[interfaceDescStart], curInterface->extraLength); + curInterface->winusbInterfaceNumber = descInterfaces->numInterfaces; + descInterfaces->numInterfaces++; + } + + if ((curLocation + descLen) < length) + { + // Setting to new found interface descriptor pointer + interfaceDescStart = curLocation; + curDescriptor = (USB_INTERFACE_DESCRIPTOR *)&descriptors[interfaceDescStart]; + curInterface = &descInterfaces->iface[curDescriptor->bInterfaceNumber]; + curInterface->numEndpoints = 0; + } + } + else if (descriptors[curLocation + 1] == USB_ENDPOINT_DESCRIPTOR_TYPE) + { + memcpy(&curInterface->endpoints[curInterface->numEndpoints], &descriptors[curLocation], descLen); + curInterface->numEndpoints++; + } + + curLocation += descLen; + //printf("length = %d\n", curLocation); + } while (curLocation < length); + + *interfaces = descInterfaces; } uvc_error_t winusb_open(winusb_uvc_device *device) { - USB_CONFIGURATION_DESCRIPTOR cfgDesc; - winusb_uvc_interfaces* interfaces = NULL; - UCHAR *descriptors = NULL; - uvc_error_t ret = UVC_SUCCESS; - ULONG returnLength = 0; - - device->streams = NULL; - - // Start by clearing deviceData, otherwise - // winusb_uvc_free_device_info(&device->deviceData); - // will do something not good - memset(&device->deviceData, 0, sizeof(winusb_uvc_device_info_t)); - - // Create a handle for I/O operations to the IVCAM device - device->deviceHandle = CreateFile(device->devPath, - GENERIC_READ | GENERIC_WRITE, - FILE_SHARE_READ | FILE_SHARE_WRITE, - NULL, - OPEN_EXISTING, - FILE_FLAG_OVERLAPPED, - NULL); - - if (device->deviceHandle == INVALID_HANDLE_VALUE) - { - LOG_ERROR("CreateFile failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - goto fail; - } - - // Create WinUSB device handle for the IVCAM device - if (!WinUsb_Initialize(device->deviceHandle, &device->winusbHandle)) - { - LOG_ERROR("WinUsb_Initialize failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - goto fail; - } - - // Returns IVCAM configuration descriptor - if (!WinUsb_GetDescriptor(device->winusbHandle, USB_CONFIGURATION_DESCRIPTOR_TYPE, 0, 0, (PUCHAR)&cfgDesc, sizeof(cfgDesc), &returnLength)) - { - printf("WinUsb_GetDescriptor failed - GetLastError = %d\n", GetLastError()); - LOG_ERROR("WinUsb_GetDescriptor failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - goto fail; - } - - descriptors = new UCHAR[cfgDesc.wTotalLength]; - - // Returns IVCAM configuration descriptor - including all interface, endpoint, class-specific, and vendor-specific descriptors - if (!WinUsb_GetDescriptor(device->winusbHandle, USB_CONFIGURATION_DESCRIPTOR_TYPE, 0, 0, descriptors, cfgDesc.wTotalLength, &returnLength)) - { - LOG_ERROR("WinUsb_GetDescriptor failed - GetLastError = " << GetLastError()); - ret = UVC_ERROR_INVALID_PARAM; - goto fail; - } - device->deviceData.config = cfgDesc; - - // Iterate over all descriptors and parse all Interface and Endpoint descriptors - winusb_uvc_parse_config_descriptors((USB_CONFIGURATION_DESCRIPTOR *)descriptors, &interfaces); - device->deviceData.interfaces = interfaces; - - if (descriptors) - { - delete[] descriptors; - descriptors = NULL; - } - - // Fill fields of uvc_device_info on device - ret = winusb_uvc_scan_control(device, &device->deviceData); - if (ret != UVC_SUCCESS) - { - LOG_ERROR("uvc_scan_control failed\n"); - goto fail; - } - - return ret; + USB_CONFIGURATION_DESCRIPTOR cfgDesc; + winusb_uvc_interfaces* interfaces = NULL; + UCHAR *descriptors = NULL; + uvc_error_t ret = UVC_SUCCESS; + ULONG returnLength = 0; + + device->streams = NULL; + + // Start by clearing deviceData, otherwise + // winusb_uvc_free_device_info(&device->deviceData); + // will do something not good + memset(&device->deviceData, 0, sizeof(winusb_uvc_device_info_t)); + + // Create a handle for I/O operations to the IVCAM device + device->deviceHandle = CreateFile(device->devPath, + GENERIC_READ | GENERIC_WRITE, + FILE_SHARE_READ | FILE_SHARE_WRITE, + NULL, + OPEN_EXISTING, + FILE_FLAG_OVERLAPPED, + NULL); + + if (device->deviceHandle == INVALID_HANDLE_VALUE) + { + LOG_ERROR("CreateFile failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + goto fail; + } + + // Create WinUSB device handle for the IVCAM device + if (!WinUsb_Initialize(device->deviceHandle, &device->winusbHandle)) + { + LOG_ERROR("WinUsb_Initialize failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + goto fail; + } + + // Returns IVCAM configuration descriptor + if (!WinUsb_GetDescriptor(device->winusbHandle, USB_CONFIGURATION_DESCRIPTOR_TYPE, 0, 0, (PUCHAR)&cfgDesc, sizeof(cfgDesc), &returnLength)) + { + LOG_ERROR("WinUsb_GetDescriptor failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + goto fail; + } + + descriptors = new UCHAR[cfgDesc.wTotalLength]; + + // Returns IVCAM configuration descriptor - including all interface, endpoint, class-specific, and vendor-specific descriptors + if (!WinUsb_GetDescriptor(device->winusbHandle, USB_CONFIGURATION_DESCRIPTOR_TYPE, 0, 0, descriptors, cfgDesc.wTotalLength, &returnLength)) + { + LOG_ERROR("WinUsb_GetDescriptor failed - GetLastError = " << GetLastError()); + ret = UVC_ERROR_INVALID_PARAM; + goto fail; + } + device->deviceData.config = cfgDesc; + + // Iterate over all descriptors and parse all Interface and Endpoint descriptors + winusb_uvc_parse_config_descriptors((USB_CONFIGURATION_DESCRIPTOR *)descriptors, &interfaces); + device->deviceData.interfaces = interfaces; + + if (descriptors) + { + delete[] descriptors; + descriptors = NULL; + } + + // Fill fields of uvc_device_info on device + ret = winusb_uvc_scan_control(device, &device->deviceData); + if (ret != UVC_SUCCESS) + { + LOG_ERROR("uvc_scan_control failed\n"); + goto fail; + } + + return ret; fail: - if (descriptors) - { - delete[] descriptors; - } - - if (device) - { - winusb_uvc_free_device_info(&device->deviceData); - - if (device->deviceHandle) - { - CloseHandle(device->deviceHandle); - device->deviceHandle = NULL; - } - - if (device->winusbHandle) - { - WinUsb_Free(device->winusbHandle); - device->winusbHandle = NULL; - } - } - - return ret; + if (descriptors) + { + delete[] descriptors; + } + + if (device) + { + winusb_uvc_free_device_info(&device->deviceData); + + if (device->deviceHandle) + { + CloseHandle(device->deviceHandle); + device->deviceHandle = NULL; + } + + if (device->winusbHandle) + { + WinUsb_Free(device->winusbHandle); + device->winusbHandle = NULL; + } + } + + return ret; } uvc_error_t winusb_close(winusb_uvc_device *device) { - uvc_error_t ret = UVC_SUCCESS; - - if (device != NULL) - { - winusb_uvc_free_device_info(&device->deviceData); - memset(&device->deviceData, 0, sizeof(winusb_uvc_device_info_t)); - - if (device->winusbHandle != NULL) - { - WinUsb_Free(device->winusbHandle); - device->winusbHandle = NULL; - } - - if (device->deviceHandle != NULL) - { - CloseHandle(device->deviceHandle); - device->deviceHandle = NULL; - } - - device->streams = NULL; - - free(device->devPath); - free(device); - } - else - { - LOG_ERROR("Error: device == NULL"); - ret = UVC_ERROR_NO_DEVICE; - } - - return ret; + uvc_error_t ret = UVC_SUCCESS; + + if (device != NULL) + { + winusb_uvc_free_device_info(&device->deviceData); + memset(&device->deviceData, 0, sizeof(winusb_uvc_device_info_t)); + + if (device->winusbHandle != NULL) + { + WinUsb_Free(device->winusbHandle); + device->winusbHandle = NULL; + } + + if (device->deviceHandle != NULL) + { + CloseHandle(device->deviceHandle); + device->deviceHandle = NULL; + } + + device->streams = NULL; + + free(device->devPath); + free(device); + } + else + { + LOG_ERROR("Error: device == NULL"); + ret = UVC_ERROR_NO_DEVICE; + } + + return ret; } int winusb_init() { - return 0; + return 0; } void winusb_deinit() { @@ -2308,26 +2299,26 @@ void winusb_deinit() { bool read_all_uvc_descriptors(winusb_uvc_device *device, PUCHAR buffer, ULONG bufferLength, PULONG lengthReturned) { - UCHAR b[2048]; - ULONG returnLength; - - /*WinUsb_GetAssociatedInterface(device->deviceHande, 1 - , &iHandle);*/ - - if (!WinUsb_GetDescriptor(device->deviceHandle, - USB_CONFIGURATION_DESCRIPTOR_TYPE, - 0, - 0, - b, - 2048, - &returnLength)) - { - LOG_ERROR("GetLastError = " << GetLastError()); - } - - LOG_DEBUG(__FUNCTION__ << ": success"); - - return 0; + UCHAR b[2048]; + ULONG returnLength; + + /*WinUsb_GetAssociatedInterface(device->deviceHande, 1 + , &iHandle);*/ + + if (!WinUsb_GetDescriptor(device->deviceHandle, + USB_CONFIGURATION_DESCRIPTOR_TYPE, + 0, + 0, + b, + 2048, + &returnLength)) + { + LOG_ERROR("GetLastError = " << GetLastError()); + } + + LOG_DEBUG(__FUNCTION__ << ": success"); + + return 0; } #endif diff --git a/unit-tests/internal/internal-tests-extrinsic.cpp b/unit-tests/internal/internal-tests-extrinsic.cpp new file mode 100644 index 00000000000..c0722285f89 --- /dev/null +++ b/unit-tests/internal/internal-tests-extrinsic.cpp @@ -0,0 +1,79 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2015 Intel Corporation. All Rights Reserved. + +#include "catch/catch.hpp" +#include +#include +#include +#include +#include +#include +#include +#include "../../common/tiny-profiler.h" +#include "./../src/environment.h" + +using namespace librealsense; +using namespace librealsense::platform; + +// Require that vector is exactly the zero vector +inline void require_zero_vector(const float(&vector)[3]) +{ + for (int i = 1; i < 3; ++i) REQUIRE(vector[i] == 0.0f); +} + +// Require that matrix is exactly the identity matrix +inline void require_identity_matrix(const float(&matrix)[9]) +{ + static const float identity_matrix_3x3[] = { 1,0,0, 0,1,0, 0,0,1 }; + for (int i = 0; i < 9; ++i) REQUIRE(matrix[i] == Approx(identity_matrix_3x3[i])); +} + +TEST_CASE("Extrinsic graph management", "[live]") +{ + // Require at least one device to be plugged in + rs2::context ctx; + { + auto list = ctx.query_devices(); + REQUIRE(list.size()); + + // For each device + auto& b = environment::get_instance().get_extrinsics_graph(); + for (int i = 0; i < 5000; i++) + { + //for (auto&& dev : list) + for (int j=0; j < list.size(); j++) + { + rs2::device dev{}; + { + //scoped_timer t1("Dev generation"); + dev = list[j]; + } + + //std::cout << __LINE__ << " Iteration " << i << " : Extrinsic graph map size is " << b._streams.size() << std::endl; + for (auto&& snr : dev.query_sensors()) + { + std::vector profs; + REQUIRE_NOTHROW(profs = snr.get_stream_profiles()); + //snr.get_info(RS2_CAMERA_INFO_NAME); + REQUIRE(profs.size() > 0); + + //std::cout << __LINE__ << " " << snr.get_info(RS2_CAMERA_INFO_NAME) <<" : Extrinsic graph map size is " << b._streams.size() << std::endl; + + //rs2_extrinsics extrin{}; + //try { + // auto prof = profs[0]; + // extrin = prof.get_extrinsics_to(prof); + //} + //catch (const rs2::error &e) { + // // if device isn't calibrated, get_extrinsics must error out (according to old comment. Might not be true under new API) + // WARN(e.what()); + // continue; + //} + + /*require_identity_matrix(extrin.rotation); + require_zero_vector(extrin.translation);*/ + } + } + } + } +}