1 // License: Apache 2.0. See LICENSE file in root directory. 2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved. 3 4 #ifdef RS_USE_LIBUVC_BACKEND 5 6 //#define ENABLE_DEBUG_SPAM 7 8 #include "uvc.h" 9 #include "libuvc/libuvc.h" 10 #include "libuvc/libuvc_internal.h" // For LibUSB punchthrough 11 #include <thread> 12 13 namespace rsimpl 14 { 15 namespace uvc 16 { check(const char * call,uvc_error_t status)17 static void check(const char * call, uvc_error_t status) 18 { 19 if (status < 0) throw std::runtime_error(to_string() << call << "(...) returned " << uvc_strerror(status)); 20 } 21 #define CALL_UVC(name, ...) check(#name, name(__VA_ARGS__)) 22 23 struct context 24 { 25 uvc_context_t * ctx; 26 contextrsimpl::uvc::context27 context() : ctx() { check("uvc_init", uvc_init(&ctx, nullptr)); } ~contextrsimpl::uvc::context28 ~context() { if(ctx) uvc_exit(ctx); } 29 }; 30 31 struct subdevice 32 { 33 uvc_device_handle_t * handle = nullptr; 34 uvc_stream_ctrl_t ctrl; 35 uint8_t unit; 36 std::function<void(const void * frame)> callback; 37 }; 38 39 struct device 40 { 41 const std::shared_ptr<context> parent; 42 uvc_device_t * uvcdevice; 43 int vid, pid; 44 std::vector<subdevice> subdevices; 45 std::vector<int> claimed_interfaces; 46 devicersimpl::uvc::device47 device(std::shared_ptr<context> parent, uvc_device_t * uvcdevice) : parent(parent), uvcdevice(uvcdevice) 48 { 49 get_subdevice(0); 50 51 uvc_device_descriptor_t * desc; 52 CALL_UVC(uvc_get_device_descriptor, uvcdevice, &desc); 53 vid = desc->idVendor; 54 pid = desc->idProduct; 55 uvc_free_device_descriptor(desc); 56 } ~devicersimpl::uvc::device57 ~device() 58 { 59 for(auto interface_number : claimed_interfaces) 60 { 61 int status = libusb_release_interface(get_subdevice(0).handle->usb_devh, interface_number); 62 if(status < 0) LOG_ERROR("libusb_release_interface(...) returned " << libusb_error_name(status)); 63 } 64 65 for(auto & sub : subdevices) if(sub.handle) uvc_close(sub.handle); 66 if(claimed_interfaces.size()) if(uvcdevice) uvc_unref_device(uvcdevice); 67 } 68 get_subdevicersimpl::uvc::device69 subdevice & get_subdevice(int subdevice_index) 70 { 71 if(subdevice_index >= subdevices.size()) subdevices.resize(subdevice_index+1); 72 if(!subdevices[subdevice_index].handle) check("uvc_open2", uvc_open2(uvcdevice, &subdevices[subdevice_index].handle, subdevice_index)); 73 return subdevices[subdevice_index]; 74 } 75 }; 76 77 //////////// 78 // device // 79 //////////// 80 get_vendor_id(const device & device)81 int get_vendor_id(const device & device) { return device.vid; } get_product_id(const device & device)82 int get_product_id(const device & device) { return device.pid; } 83 get_control(const device & dev,const extension_unit & xu,uint8_t ctrl,void * data,int len)84 void get_control(const device & dev, const extension_unit & xu, uint8_t ctrl, void * data, int len) 85 { 86 int status = uvc_get_ctrl(const_cast<device &>(dev).get_subdevice(xu.subdevice).handle, xu.unit, ctrl, data, len, UVC_GET_CUR); 87 if(status < 0) throw std::runtime_error(to_string() << "uvc_get_ctrl(...) returned " << libusb_error_name(status)); 88 } 89 set_control(device & device,const extension_unit & xu,uint8_t ctrl,void * data,int len)90 void set_control(device & device, const extension_unit & xu, uint8_t ctrl, void * data, int len) 91 { 92 int status = uvc_set_ctrl(device.get_subdevice(xu.subdevice).handle, xu.unit, ctrl, data, len); 93 if(status < 0) throw std::runtime_error(to_string() << "uvc_set_ctrl(...) returned " << libusb_error_name(status)); 94 } 95 claim_interface(device & device,const guid & interface_guid,int interface_number)96 void claim_interface(device & device, const guid & interface_guid, int interface_number) 97 { 98 int status = libusb_claim_interface(device.get_subdevice(0).handle->usb_devh, interface_number); 99 if(status < 0) throw std::runtime_error(to_string() << "libusb_claim_interface(...) returned " << libusb_error_name(status)); 100 device.claimed_interfaces.push_back(interface_number); 101 } 102 bulk_transfer(device & device,unsigned char endpoint,void * data,int length,int * actual_length,unsigned int timeout)103 void bulk_transfer(device & device, unsigned char endpoint, void * data, int length, int *actual_length, unsigned int timeout) 104 { 105 int status = libusb_bulk_transfer(device.get_subdevice(0).handle->usb_devh, endpoint, (unsigned char *)data, length, actual_length, timeout); 106 if(status < 0) throw std::runtime_error(to_string() << "libusb_bulk_transfer(...) returned " << libusb_error_name(status)); 107 } 108 set_subdevice_mode(device & device,int subdevice_index,int width,int height,uint32_t fourcc,int fps,std::function<void (const void * frame)> callback)109 void set_subdevice_mode(device & device, int subdevice_index, int width, int height, uint32_t fourcc, int fps, std::function<void(const void * frame)> callback) 110 { 111 auto & sub = device.get_subdevice(subdevice_index); 112 check("get_stream_ctrl_format_size", uvc_get_stream_ctrl_format_size(sub.handle, &sub.ctrl, reinterpret_cast<const big_endian<uint32_t> &>(fourcc), width, height, fps)); 113 sub.callback = callback; 114 } 115 start_streaming(device & device,int num_transfer_bufs)116 void start_streaming(device & device, int num_transfer_bufs) 117 { 118 for(auto & sub : device.subdevices) 119 { 120 if(sub.callback) 121 { 122 #if defined (ENABLE_DEBUG_SPAM) 123 uvc_print_stream_ctrl(&sub.ctrl, stdout); 124 #endif 125 126 check("uvc_start_streaming", uvc_start_streaming(sub.handle, &sub.ctrl, [](uvc_frame * frame, void * user) 127 { 128 reinterpret_cast<subdevice *>(user)->callback(frame->data); 129 }, &sub, 0, num_transfer_bufs)); 130 } 131 } 132 } 133 stop_streaming(device & device)134 void stop_streaming(device & device) 135 { 136 // Stop all streaming 137 for(auto & sub : device.subdevices) 138 { 139 if(sub.handle) uvc_stop_streaming(sub.handle); 140 sub.ctrl = {}; 141 sub.callback = {}; 142 } 143 } 144 set_pu(uvc_device_handle_t * devh,int subdevice,uint8_t unit,uint8_t control,int value)145 template<class T> void set_pu(uvc_device_handle_t * devh, int subdevice, uint8_t unit, uint8_t control, int value) 146 { 147 const int REQ_TYPE_SET = 0x21; 148 unsigned char buffer[4]; 149 if(sizeof(T)==1) buffer[0] = value; 150 if(sizeof(T)==2) SHORT_TO_SW(value, buffer); 151 if(sizeof(T)==4) INT_TO_DW(value, buffer); 152 int status = libusb_control_transfer(devh->usb_devh, REQ_TYPE_SET, UVC_SET_CUR, control << 8, unit << 8 | (subdevice*2), buffer, sizeof(T), 0); 153 if(status < 0) throw std::runtime_error(to_string() << "libusb_control_transfer(...) returned " << libusb_error_name(status)); 154 if(status != sizeof(T)) throw std::runtime_error("insufficient data written to usb"); 155 } 156 get_pu(uvc_device_handle_t * devh,int subdevice,uint8_t unit,uint8_t control,int uvc_get_thing)157 template<class T> int get_pu(uvc_device_handle_t * devh, int subdevice, uint8_t unit, uint8_t control, int uvc_get_thing) 158 { 159 const int REQ_TYPE_GET = 0xa1; 160 unsigned char buffer[4]; 161 int status = libusb_control_transfer(devh->usb_devh, REQ_TYPE_GET, uvc_get_thing, control << 8, unit << 8 | (subdevice*2), buffer, sizeof(T), 0); 162 if(status < 0) throw std::runtime_error(to_string() << "libusb_control_transfer(...) returned " << libusb_error_name(status)); 163 if(status != sizeof(T)) throw std::runtime_error("insufficient data read from usb"); 164 if(sizeof(T)==1) return buffer[0]; 165 if(sizeof(T)==2) return SW_TO_SHORT(buffer); 166 if(sizeof(T)==4) return DW_TO_INT(buffer); 167 } 168 get_pu_range(uvc_device_handle_t * devh,int subdevice,uint8_t unit,uint8_t control,int * min,int * max)169 template<class T> void get_pu_range(uvc_device_handle_t * devh, int subdevice, uint8_t unit, uint8_t control, int * min, int * max) 170 { 171 if(min) *min = get_pu<T>(devh, subdevice, unit, control, UVC_GET_MIN); 172 if(max) *max = get_pu<T>(devh, subdevice, unit, control, UVC_GET_MAX); 173 } 174 get_pu_control_range(const device & device,int subdevice,rs_option option,int * min,int * max)175 void get_pu_control_range(const device & device, int subdevice, rs_option option, int * min, int * max) 176 { 177 auto handle = const_cast<uvc::device &>(device).get_subdevice(subdevice).handle; 178 int ct_unit = 0, pu_unit = 0; 179 for(auto ct = uvc_get_input_terminals(handle); ct; ct = ct->next) ct_unit = ct->bTerminalID; // todo - Check supported caps 180 for(auto pu = uvc_get_processing_units(handle); pu; pu = pu->next) pu_unit = pu->bUnitID; // todo - Check supported caps 181 182 switch(option) 183 { 184 case RS_OPTION_COLOR_BACKLIGHT_COMPENSATION: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_BACKLIGHT_COMPENSATION_CONTROL, min, max); 185 case RS_OPTION_COLOR_BRIGHTNESS: return get_pu_range<int16_t>(handle, subdevice, pu_unit, UVC_PU_BRIGHTNESS_CONTROL, min, max); 186 case RS_OPTION_COLOR_CONTRAST: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_CONTRAST_CONTROL, min, max); 187 case RS_OPTION_COLOR_EXPOSURE: return get_pu_range<uint32_t>(handle, subdevice, ct_unit, UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL, min, max); 188 case RS_OPTION_COLOR_GAIN: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAIN_CONTROL, min, max); 189 case RS_OPTION_COLOR_GAMMA: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAMMA_CONTROL, min, max); 190 case RS_OPTION_COLOR_HUE: if(min) *min = 0; if(max) *max = 0; return; //return get_pu_range<int16_t>(handle, subdevice, pu_unit, UVC_PU_HUE_CONTROL, min, max); 191 case RS_OPTION_COLOR_SATURATION: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SATURATION_CONTROL, min, max); 192 case RS_OPTION_COLOR_SHARPNESS: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SHARPNESS_CONTROL, min, max); 193 case RS_OPTION_COLOR_WHITE_BALANCE: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL, min, max); 194 case RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE: if(min) *min = 0; if(max) *max = 1; return; // The next 2 options do not support range operations 195 case RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE: if(min) *min = 0; if(max) *max = 1; return; 196 default: throw std::logic_error("invalid option"); 197 } 198 } 199 set_pu_control(device & device,int subdevice,rs_option option,int value)200 void set_pu_control(device & device, int subdevice, rs_option option, int value) 201 { 202 auto handle = device.get_subdevice(subdevice).handle; 203 int ct_unit = 0, pu_unit = 0; 204 for(auto ct = uvc_get_input_terminals(handle); ct; ct = ct->next) ct_unit = ct->bTerminalID; // todo - Check supported caps 205 for(auto pu = uvc_get_processing_units(handle); pu; pu = pu->next) pu_unit = pu->bUnitID; // todo - Check supported caps 206 207 switch(option) 208 { 209 case RS_OPTION_COLOR_BACKLIGHT_COMPENSATION: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_BACKLIGHT_COMPENSATION_CONTROL, value); 210 case RS_OPTION_COLOR_BRIGHTNESS: return set_pu<int16_t>(handle, subdevice, pu_unit, UVC_PU_BRIGHTNESS_CONTROL, value); 211 case RS_OPTION_COLOR_CONTRAST: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_CONTRAST_CONTROL, value); 212 case RS_OPTION_COLOR_EXPOSURE: return set_pu<uint32_t>(handle, subdevice, ct_unit, UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL, value); 213 case RS_OPTION_COLOR_GAIN: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAIN_CONTROL, value); 214 case RS_OPTION_COLOR_GAMMA: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAMMA_CONTROL, value); 215 case RS_OPTION_COLOR_HUE: return; // set_pu<int16_t>(handle, subdevice, pu_unit, UVC_PU_HUE_CONTROL, value); // Causes LIBUSB_ERROR_PIPE, may be related to not being able to set UVC_PU_HUE_AUTO_CONTROL 216 case RS_OPTION_COLOR_SATURATION: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SATURATION_CONTROL, value); 217 case RS_OPTION_COLOR_SHARPNESS: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SHARPNESS_CONTROL, value); 218 case RS_OPTION_COLOR_WHITE_BALANCE: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL, value); 219 case RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE: return set_pu<uint8_t>(handle, subdevice, ct_unit, UVC_CT_AE_MODE_CONTROL, value ? 2 : 1); // Modes - (1: manual) (2: auto) (4: shutter priority) (8: aperture priority) 220 case RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE: return set_pu<uint8_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL, value); 221 default: throw std::logic_error("invalid option"); 222 } 223 } 224 get_pu_control(const device & device,int subdevice,rs_option option)225 int get_pu_control(const device & device, int subdevice, rs_option option) 226 { 227 auto handle = const_cast<uvc::device &>(device).get_subdevice(subdevice).handle; 228 int ct_unit = 0, pu_unit = 0; 229 for(auto ct = uvc_get_input_terminals(handle); ct; ct = ct->next) ct_unit = ct->bTerminalID; // todo - Check supported caps 230 for(auto pu = uvc_get_processing_units(handle); pu; pu = pu->next) pu_unit = pu->bUnitID; // todo - Check supported caps 231 232 switch(option) 233 { 234 case RS_OPTION_COLOR_BACKLIGHT_COMPENSATION: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_BACKLIGHT_COMPENSATION_CONTROL, UVC_GET_CUR); 235 case RS_OPTION_COLOR_BRIGHTNESS: return get_pu<int16_t>(handle, subdevice, pu_unit, UVC_PU_BRIGHTNESS_CONTROL, UVC_GET_CUR); 236 case RS_OPTION_COLOR_CONTRAST: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_CONTRAST_CONTROL,UVC_GET_CUR); 237 case RS_OPTION_COLOR_EXPOSURE: return get_pu<uint32_t>(handle, subdevice, ct_unit, UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL, UVC_GET_CUR); 238 case RS_OPTION_COLOR_GAIN: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAIN_CONTROL, UVC_GET_CUR); 239 case RS_OPTION_COLOR_GAMMA: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAMMA_CONTROL, UVC_GET_CUR); 240 case RS_OPTION_COLOR_HUE: return 0; //get_pu<int16_t>(handle, subdevice, pu_unit, UVC_PU_HUE_CONTROL, UVC_GET_CUR); 241 case RS_OPTION_COLOR_SATURATION: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SATURATION_CONTROL, UVC_GET_CUR); 242 case RS_OPTION_COLOR_SHARPNESS: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SHARPNESS_CONTROL, UVC_GET_CUR); 243 case RS_OPTION_COLOR_WHITE_BALANCE: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL, UVC_GET_CUR); 244 case RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE: return get_pu<uint8_t>(handle, subdevice, ct_unit, UVC_CT_AE_MODE_CONTROL, UVC_GET_CUR) > 1; // Modes - (1: manual) (2: auto) (4: shutter priority) (8: aperture priority) 245 case RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE: return get_pu<uint8_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL, UVC_GET_CUR); 246 default: throw std::logic_error("invalid option"); 247 } 248 } 249 250 ///////////// 251 // context // 252 ///////////// 253 create_context()254 std::shared_ptr<context> create_context() 255 { 256 return std::make_shared<context>(); 257 } 258 query_devices(std::shared_ptr<context> context)259 std::vector<std::shared_ptr<device>> query_devices(std::shared_ptr<context> context) 260 { 261 std::vector<std::shared_ptr<device>> devices; 262 263 uvc_device_t ** list; 264 CALL_UVC(uvc_get_device_list, context->ctx, &list); 265 for(auto it = list; *it; ++it) devices.push_back(std::make_shared<device>(context, *it)); 266 uvc_free_device_list(list, 1); 267 return devices; 268 } 269 } 270 } 271 272 #endif