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