Add UVC driver and test/example

3 files changed, 1140 insertions, 22 deletions

diff --git a/os/hal/include/usbh/dev/uvc.h b/os/hal/include/usbh/dev/uvc.h
new file mode 100644
index 0000000..845b93e
--- /dev/null
+++ b/os/hal/include/usbh/dev/uvc.h
@@ -0,0 +1,471 @@
+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015..2016 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+ */
+
+#ifndef USBH_INCLUDE_USBH_UVC_H_
+#define USBH_INCLUDE_USBH_UVC_H_
+
+#include "hal_usbh.h"
+
+#if HAL_USE_USBH && HAL_USBH_USE_UVC
+
+/* TODO:
+ *
+ *
+ */
+
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+#define USBHUVC_MAX_STATUS_PACKET_SZ	16
+
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+
+typedef enum {
+	UVC_CS_INTERFACE 	= 0x24,
+	UVC_CS_ENDPOINT 	= 0x25
+} usbh_uvc_cstype_t;
+
+typedef enum {
+	UVC_CC_VIDEO 		= 0x0e
+} usbh_uvc_cctype_t;
+
+typedef enum {
+	UVC_SC_UNKNOWN = 0x00,
+	UVC_SC_VIDEOCONTROL,
+	UVC_SC_VIDEOSTREAMING,
+	UVC_SC_VIDEO_INTERFACE_COLLECTION
+} usbh_uvc_sctype_t;
+
+typedef enum {
+	UVC_VC_UNDEF = 0x00,
+	UVC_VC_HEADER,
+	UVC_VC_INPUT_TERMINAL,
+	UVC_VC_OUTPUT_TERMINAL,
+	UVC_VC_SELECTOR_UNIT,
+	UVC_VC_PROCESSING_UNIT,
+	UVC_VC_EXTENSION_UNIT
+} usbh_uvc_vctype_t;
+
+typedef enum {
+	UVC_VS_UNDEF = 0x00,
+	UVC_VS_INPUT_HEADER,
+	UVC_VS_OUTPUT_HEADER,
+	UVC_VS_STILL_IMAGE_FRAME,
+	UVC_VS_FORMAT_UNCOMPRESSED,
+	UVC_VS_FRAME_UNCOMPRESSED,
+	UVC_VS_FORMAT_MJPEG,
+	UVC_VS_FRAME_MJPEG,
+	UVC_VS_RESERVED_0,
+	UVC_VS_RESERVED_1,
+	UVC_VS_FORMAT_MPEG2TS,
+	UVC_VS_RESERVED_2,
+	UVC_VS_FORMAT_DV,
+	UVC_VS_COLOR_FORMAT,
+	UVC_VS_RESERVED_3,
+	UVC_VS_RESERVED_4,
+	UVC_VS_FORMAT_FRAME_BASED,
+	UVC_VS_FRAME_FRAME_BASED,
+	UVC_VS_FORMAT_STREAM_BASED
+} usbh_uvc_vstype_t;
+
+typedef enum {
+	UVC_TT_VENDOR_SPECIFIC 			= 0x0100,
+	UVC_TT_STREAMING 				= 0x0101,
+	UVC_ITT_VENDOR_SPECIFIC 		= 0x0200,
+	UVC_ITT_CAMERA 					= 0x0201,
+	UVC_ITT_MEDIA_TRANSPORT_INPUT 	= 0x0202,
+	UVC_OTT_VENDOR_SPECIFIC 		= 0x0300,
+	UVC_OTT_DISPLAY 				= 0x0301,
+	UVC_OTT_MEDIA_TRANSPORT 		= 0x0302
+} usbh_uvc_tttype_t;
+
+typedef enum {
+	UVC_SET_CUR	 =	0x01,
+	UVC_GET_CUR	 =	0x81,
+	UVC_GET_MIN	 =	0x82,
+	UVC_GET_MAX	 =	0x83,
+	UVC_GET_RES  =	0x84,
+	UVC_GET_LEN  = 	0x85,
+	UVC_GET_INFO =	0x86,
+	UVC_GET_DEF  =	0x87
+} usbh_uvc_ctrlops_t;
+
+typedef enum {
+	UVC_CTRL_VC_CONTROL_UNDEFINED  = 0x00,
+	UVC_CTRL_VC_VIDEO_POWER_MODE_CONTROL = 0x01,
+	UVC_CTRL_VC_REQUEST_ERROR_CODE_CONTROL = 0x02,
+} usbh_uvc_ctrl_vc_interface_controls_t;
+
+typedef enum {
+	UVC_CTRL_SU_CONTROL_UNDEFINED = 0x00,
+	UVC_CTRL_SU_INPUT_SELECT_CONTROL = 0x01,
+} usbh_uvc_ctrl_vc_selectorunit_controls_t;
+
+typedef enum {
+	UVC_CTRL_CT_CONTROL_UNDEFINED = 0x00,
+	UVC_CTRL_CT_SCANNING_MODE_CONTROL = 0x01,
+	UVC_CTRL_CT_AE_MODE_CONTROL = 0x02,
+	UVC_CTRL_CT_AE_PRIORITY_CONTROL = 0x03,
+	UVC_CTRL_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL = 0x04,
+	UVC_CTRL_CT_EXPOSURE_TIME_RELATIVE_CONTROL = 0x05,
+	UVC_CTRL_CT_FOCUS_ABSOLUTE_CONTROL = 0x06,
+	UVC_CTRL_CT_FOCUS_RELATIVE_CONTROL = 0x07,
+	UVC_CTRL_CT_FOCUS_AUTO_CONTROL = 0x08,
+	UVC_CTRL_CT_IRIS_ABSOLUTE_CONTROL = 0x09,
+	UVC_CTRL_CT_IRIS_RELATIVE_CONTROL = 0x0A,
+	UVC_CTRL_CT_ZOOM_ABSOLUTE_CONTROL = 0x0B,
+	UVC_CTRL_CT_ZOOM_RELATIVE_CONTROL = 0x0C,
+	UVC_CTRL_CT_PANTILT_ABSOLUTE_CONTROL = 0x0D,
+	UVC_CTRL_CT_PANTILT_RELATIVE_CONTROL = 0x0E,
+	UVC_CTRL_CT_ROLL_ABSOLUTE_CONTROL = 0x0F,
+	UVC_CTRL_CT_ROLL_RELATIVE_CONTROL = 0x10,
+	UVC_CTRL_CT_PRIVACY_CONTROL = 0x11
+} usbh_uvc_ctrl_vc_cameraterminal_controls_t;
+
+typedef enum {
+	UVC_CTRL_PU_CONTROL_UNDEFINED = 0x00,
+	UVC_CTRL_PU_BACKLIGHT_COMPENSATION_CONTROL = 0x01,
+	UVC_CTRL_PU_BRIGHTNESS_CONTROL = 0x02,
+	UVC_CTRL_PU_CONTRAST_CONTROL = 0x03,
+	UVC_CTRL_PU_GAIN_CONTROL = 0x04,
+	UVC_CTRL_PU_POWER_LINE_FREQUENCY_CONTROL = 0x05,
+	UVC_CTRL_PU_HUE_CONTROL = 0x06,
+	UVC_CTRL_PU_SATURATION_CONTROL = 0x07,
+	UVC_CTRL_PU_SHARPNESS_CONTROL = 0x08,
+	UVC_CTRL_PU_GAMMA_CONTROL = 0x09,
+	UVC_CTRL_PU_WHITE_BALANCE_TEMPERATURE_CONTROL = 0x0A,
+	UVC_CTRL_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL = 0x0B,
+	UVC_CTRL_PU_WHITE_BALANCE_COMPONENT_CONTROL = 0x0C,
+	UVC_CTRL_PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL = 0x0D,
+	UVC_CTRL_PU_DIGITAL_MULTIPLIER_CONTROL = 0x0E,
+	UVC_CTRL_PU_DIGITAL_MULTIPLIER_LIMIT_CONTROL = 0x0F,
+	UVC_CTRL_PU_HUE_AUTO_CONTROL = 0x10,
+	UVC_CTRL_PU_ANALOG_VIDEO_STANDARD_CONTROL = 0x11,
+	UVC_CTRL_PU_ANALOG_LOCK_STATUS_CONTROL = 0x12,
+} usbh_uvc_ctrl_vc_processingunit_controls_t;
+
+typedef enum {
+	UVC_CTRL_VS_CONTROL_UNDEFINED = 0x00,
+	UVC_CTRL_VS_PROBE_CONTROL = 0x01,
+	UVC_CTRL_VS_COMMIT_CONTROL = 0x02,
+	UVC_CTRL_VS_STILL_PROBE_CONTROL = 0x03,
+	UVC_CTRL_VS_STILL_COMMIT_CONTROL = 0x04,
+	UVC_CTRL_VS_STILL_IMAGE_TRIGGER_CONTROL = 0x05,
+	UVC_CTRL_VS_STREAM_ERROR_CODE_CONTROL = 0x06,
+	UVC_CTRL_VS_GENERATE_KEY_FRAME_CONTROL = 0x07,
+	UVC_CTRL_VS_UPDATE_FRAME_SEGMENT_CONTROL = 0x08,
+	UVC_CTRL_VS_SYNCH_DELAY_CONTROL = 0x09
+} usbh_uvc_ctrl_vs_interface_controls_t;
+
+
+typedef struct {
+	uint8_t bLength;
+	uint8_t bDescriptorType;
+	uint8_t bDescriptorSubType;
+	uint8_t bFormatIndex;
+	uint8_t bNumFrameDescriptors;
+	uint8_t bmFlags;
+	uint8_t bDefaultFrameIndex;
+	uint8_t bAspectRatioX;
+	uint8_t bAspectRatioY;
+	uint8_t bmInterfaceFlags;
+	uint8_t bCopyProtect;
+} __attribute__((__packed__)) usbh_uvc_format_mjpeg_t;
+
+typedef struct {
+	uint8_t  bLength;
+	uint8_t  bDescriptorType;
+	uint8_t  bDescriptorSubType;
+	uint8_t  bFrameIndex;
+	uint8_t  bmCapabilities;
+	uint16_t wWidth;
+	uint16_t wHeight;
+	uint32_t dwMinBitRate;
+	uint32_t dwMaxBitRate;
+	uint32_t dwMaxVideoFrameBufferSize;
+	uint32_t dwDefaultFrameInterval;
+	uint8_t bFrameIntervalType;
+	uint32_t dwFrameInterval[0];
+} __attribute__((__packed__)) usbh_uvc_frame_mjpeg_t;
+
+
+typedef struct {
+	uint8_t bLength;
+	uint8_t bDescriptorType;
+	uint8_t bDescriptorSubType;
+	uint8_t bFrameIndex;
+	uint8_t bmCapabilities;
+	uint16_t wWidth;
+	uint16_t wHeight;
+	uint32_t dwMinBitRate;
+	uint32_t dwMaxBitRate;
+	uint32_t dwMaxVideoFrameBufferSize;
+	uint32_t dwDefaultFrameInterval;
+	uint8_t bFrameIntervalType;
+	uint32_t dwFrameInterval[0];
+} __attribute__((__packed__)) usbh_uvc_frame_uncompressed_t;
+
+typedef struct {
+	uint8_t  bLength;
+	uint8_t  bDescriptorType;
+	uint8_t  bDescriptorSubType;
+	uint8_t  bFormatIndex;
+	uint8_t  bNumFrameDescriptors;
+	uint8_t  guidFormat[16];
+	uint8_t  bBitsPerPixel;
+	uint8_t  bDefaultFrameIndex;
+	uint8_t  bAspectRatioX;
+	uint8_t  bAspectRatioY;
+	uint8_t  bmInterfaceFlags;
+	uint8_t  bCopyProtect;
+} __attribute__((__packed__)) usbh_uvc_format_uncompressed;
+
+typedef struct {
+    uint16_t bmHint;
+    uint8_t bFormatIndex;
+    uint8_t bFrameIndex;
+    uint32_t dwFrameInterval;
+    uint16_t wKeyFrameRate;
+    uint16_t wPFrameRate;
+    uint16_t wCompQuality;
+    uint16_t wCompWindowSize;
+    uint16_t wDelay;
+    uint32_t dwMaxVideoFrameSize;
+    uint32_t dwMaxPayloadTransferSize;
+//    uint32_t dwClockFrequency;
+//    uint8_t bmFramingInfo;
+//    uint8_t bPreferedVersion;
+//    uint8_t bMinVersion;
+//    uint8_t bMaxVersion;
+} __attribute__((__packed__)) usbh_uvc_ctrl_vs_probecommit_data_t;
+
+
+
+/* D0: Frame ID.
+ * 	For frame-based formats, this bit toggles between 0 and 1 every time a new video frame begins.
+ * 	For stream-based formats, this bit toggles between 0 and 1 at the start of each new codec-specific
+ * 	segment. This behavior is required for frame-based payload formats (e.g., DV) and is optional
+ * 	for stream-based payload formats (e.g., MPEG-2 TS). For stream-based formats, support for this
+ * 	bit must be indicated via the bmFramingInfofield of the Video Probe and Commitcontrols
+ * 	(see section 4.3.1.1, “Video Probe and Commit Controls”).
+ *
+ * D1: End of Frame.
+ *  This bit is set if the following payload data marks the end of the current video or still image
+ *  frame (for framebased formats), or to indicate the end of a codec-specific segment
+ *  (for stream-based formats). This behavior is optional for all payload formats.
+ *  For stream-based formats, support for this bit must be indicated via the bmFramingInfofield
+ *  of the Video Probe and CommitControls (see section 4.3.1.1, “Video Probe and Commit Controls”).
+ *
+ * D2: Presentation Time.
+ *  This bit is set if the dwPresentationTimefield is being sent as part of the header.
+ *
+ * D3: Source Clock Reference
+ *  This bit is set if the dwSourceClockfield is being sent as part of the header.
+ *
+ * D4: Reserved
+ *
+ * D5: Still Image
+ *  This bit is set ifthe following data is part of a still image frame, and is only used for
+ *  methods 2 and 3 of still image capture.
+ *
+ * D6: Error
+ *  This bit is set ifthere was an error in the video or still image transmission
+ *  for this payload. The StreamError Code control would reflect the cause of the error.
+ *
+ * D7: End of header
+ *  This bit is set if this is the last header group in the packet, where the
+ *  header group refers to this field and any optional fields identified by the bits in this
+ *  field (Defined for future extension)
+*/
+
+#define UVC_HDR_EOH				(1 << 7)	/* End of header */
+#define UVC_HDR_ERR				(1 << 6)	/* Error */
+#define UVC_HDR_STILL			(1 << 5)	/* Still Image */
+#define UVC_HDR_SCR				(1 << 3)	/* Source Clock Reference */
+#define UVC_HDR_PT				(1 << 2)	/* Presentation Time */
+#define UVC_HDR_EOF				(1 << 1)	/* End of Frame */
+#define UVC_HDR_FID				(1 << 0)	/* Frame ID */
+
+
+
+typedef struct USBHUVCDriver USBHUVCDriver;
+
+#define USBHUVC_MESSAGETYPE_STATUS	1
+#define USBHUVC_MESSAGETYPE_DATA	2
+
+
+#define _usbhuvc_message_base_data				\
+		uint16_t type;							\
+		uint16_t length;						\
+		systime_t timestamp;
+
+typedef struct {
+	_usbhuvc_message_base_data
+} usbhuvc_message_base_t;
+
+typedef struct {
+	_usbhuvc_message_base_data
+	USBH_DECLARE_STRUCT_MEMBER(uint8_t data[0]);
+} usbhuvc_message_data_t;
+
+typedef struct {
+	_usbhuvc_message_base_data
+	USBH_DECLARE_STRUCT_MEMBER(uint8_t data[USBHUVC_MAX_STATUS_PACKET_SZ]);
+} usbhuvc_message_status_t;
+
+
+typedef enum {
+	USBHUVC_STATE_UNINITIALIZED = 0,	//must call usbhuvcObjectInit
+	USBHUVC_STATE_STOP	 		= 1,	//the device is disconnected
+	USBHUVC_STATE_ACTIVE 		= 2,	//the device is connected
+	USBHUVC_STATE_READY		 	= 3,	//the device has negotiated the parameters
+	USBHUVC_STATE_STREAMING 	= 4,	//the device is streaming data
+	USBHUVC_STATE_BUSY	 		= 5		//the driver is busy performing some action
+} usbhuvc_state_t;
+
+
+struct USBHUVCDriver {
+	/* inherited from abstract class driver */
+	_usbh_base_classdriver_data
+
+	usbhuvc_state_t state;
+
+	usbh_ep_t ep_int;
+	usbh_ep_t ep_iso;
+
+	usbh_urb_t urb_iso;
+	usbh_urb_t urb_int;
+
+	if_iterator_t ivc;
+	if_iterator_t ivs;
+
+	USBH_DECLARE_STRUCT_MEMBER(usbh_uvc_ctrl_vs_probecommit_data_t pc);
+	USBH_DECLARE_STRUCT_MEMBER(usbh_uvc_ctrl_vs_probecommit_data_t pc_min);
+	USBH_DECLARE_STRUCT_MEMBER(usbh_uvc_ctrl_vs_probecommit_data_t pc_max);
+
+	mailbox_t mb;
+	msg_t mb_buff[HAL_USBHUVC_MAX_MAILBOX_SZ];
+
+	memory_pool_t mp_data;
+	void *mp_data_buffer;
+
+	memory_pool_t mp_status;
+	usbhuvc_message_status_t mp_status_buffer[HAL_USBHUVC_STATUS_PACKETS_COUNT];
+
+	mutex_t mtx;
+};
+
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+extern USBHUVCDriver USBHUVCD[HAL_USBHUVC_MAX_INSTANCES];
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	void usbhuvcObjectInit(USBHUVCDriver *uvcd);
+
+	static inline usbhuvc_state_t usbhuvcGetState(USBHUVCDriver *uvcd) {
+		return uvcd->state;
+	}
+
+	bool usbhuvcVCRequest(USBHUVCDriver *uvcdp,
+			uint8_t bRequest, uint8_t entity, uint8_t control,
+			uint16_t wLength, uint8_t *data);
+	bool usbhuvcVSRequest(USBHUVCDriver *uvcdp,
+			uint8_t bRequest, uint8_t control,
+			uint16_t wLength, uint8_t *data);
+	bool usbhuvcFindVSDescriptor(USBHUVCDriver *uvcdp,
+			generic_iterator_t *ics,
+			uint8_t bDescriptorSubtype,
+			bool start);
+	uint32_t usbhuvcEstimateRequiredEPSize(USBHUVCDriver *uvcdp, const uint8_t *formatdesc,
+			const uint8_t *framedesc, uint32_t dwFrameInterval);
+
+#if	USBH_DEBUG_ENABLE && USBHUVC_DEBUG_ENABLE_INFO
+	void usbhuvcPrintProbeCommit(const usbh_uvc_ctrl_vs_probecommit_data_t *pc);
+#else
+#	define usbhuvcPrintProbeCommit(pc) do {} while(0)
+#endif
+	bool usbhuvcProbe(USBHUVCDriver *uvcdp);
+	bool usbhuvcCommit(USBHUVCDriver *uvcdp);
+	void usbhuvcResetPC(USBHUVCDriver *uvcdp);
+	static inline const usbh_uvc_ctrl_vs_probecommit_data_t *usbhuvcGetPCMin(USBHUVCDriver *uvcdp) {
+		return &uvcdp->pc_min;
+	}
+	static inline const usbh_uvc_ctrl_vs_probecommit_data_t *usbhuvcGetPCMax(USBHUVCDriver *uvcdp) {
+		return &uvcdp->pc_min;
+	}
+	static inline usbh_uvc_ctrl_vs_probecommit_data_t *usbhuvcGetPC(USBHUVCDriver *uvcdp) {
+		return &uvcdp->pc;
+	}
+
+	bool usbhuvcStreamStart(USBHUVCDriver *uvcdp, uint16_t min_ep_sz);
+	bool usbhuvcStreamStop(USBHUVCDriver *uvcdp);
+
+	static inline msg_t usbhuvcLockAndFetchS(USBHUVCDriver *uvcdp, msg_t *msg, systime_t timeout) {
+		chMtxLockS(&uvcdp->mtx);
+		msg_t ret = chMBFetchS(&uvcdp->mb, msg, timeout);
+		if (ret != MSG_OK)
+			chMtxUnlockS(&uvcdp->mtx);
+		return ret;
+	}
+	static inline msg_t usbhuvcLockAndFetch(USBHUVCDriver *uvcdp, msg_t *msg, systime_t timeout) {
+		osalSysLock();
+		msg_t ret = usbhuvcLockAndFetchS(uvcdp, msg, timeout);
+		osalSysUnlock();
+		return ret;
+	}
+	static inline void usbhuvcUnlock(USBHUVCDriver *uvcdp) {
+		chMtxUnlock(&uvcdp->mtx);
+	}
+	static inline void usbhuvcFreeDataMessage(USBHUVCDriver *uvcdp, usbhuvc_message_data_t *msg) {
+		chPoolFree(&uvcdp->mp_data, msg);
+	}
+	static inline void usbhuvcFreeStatusMessage(USBHUVCDriver *uvcdp, usbhuvc_message_status_t *msg) {
+		chPoolFree(&uvcdp->mp_status, msg);
+	}
+
+
+	/* global initializer */
+	void usbhuvcInit(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+#endif /* USBH_INCLUDE_USBH_UVC_H_ */
diff --git a/os/hal/src/hal_usbh.c b/os/hal/src/hal_usbh.c
index 457abba..d242086 100644
--- a/os/hal/src/hal_usbh.c
+++ b/os/hal/src/hal_usbh.c
@@ -28,6 +28,7 @@
 #include "usbh/dev/ftdi.h"
 #include "usbh/dev/msd.h"
 #include "usbh/dev/hid.h"
+#include "usbh/dev/uvc.h"
 
 #if USBH_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
@@ -125,6 +126,9 @@ void usbhInit(void) {
 #if HAL_USBH_USE_HID
 	usbhhidInit();
 #endif
+#if HAL_USBH_USE_UVC
+	usbhuvcInit();
+#endif
 #if HAL_USBH_USE_HUB
 	usbhhubInit();
 #endif
@@ -421,7 +425,8 @@ usbh_urbstatus_t usbhControlRequestExtended(usbh_device_t *dev,
 		uint32_t *actual_len,
 		systime_t timeout) {
 
-	_check_dev(dev);
+	if (!dev) return USBH_URBSTATUS_DISCONNECTED;
+
 	osalDbgCheck(req != NULL);
 
 	usbh_urb_t urb;
@@ -1310,6 +1315,9 @@ static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {
 #if HAL_USBH_USE_HID
 	&usbhhidClassDriverInfo,
 #endif
+#if HAL_USBH_USE_UVC
+	&usbhuvcClassDriverInfo,
+#endif
 #if HAL_USBH_USE_HUB
 	&usbhhubClassDriverInfo,
 #endif
@@ -1341,7 +1349,7 @@ static bool _classdriver_load(usbh_device_t *dev, uint8_t class,
 #if HAL_USBH_USE_IAD
 			/* special case: */
 			if (info == &usbhiadClassDriverInfo)
-				return HAL_SUCCESS;
+				goto success; //return HAL_SUCCESS;
 #endif
 
 			if (drv != NULL)
diff --git a/os/hal/src/usbh/hal_usbh_uvc.c b/os/hal/src/usbh/hal_usbh_uvc.c
index 025fbee..72c15c0 100644
--- a/os/hal/src/usbh/hal_usbh_uvc.c
+++ b/os/hal/src/usbh/hal_usbh_uvc.c
@@ -1,22 +1,17 @@
 /*
-    ChibiOS - Copyright (C) 2006..2017 Giovanni Di Sirio
-              Copyright (C) 2015..2017 Diego Ismirlian, (dismirlian (at) google's mail)
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-        http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing, software
-    distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-    See the License for the specific language governing permissions and
-    limitations under the License.
-*/
+ * usbh_uvc.c
+ *
+ *  Created on: 14 de set. de 2015
+ *      Author: Diego Ismirlian (dismirlian (at) google's mail (dot) com)
+ *
+ *  License:
+ *
+ *  This file is free for non-commercial use until the company I work for decides what to do.
+ *  If in doubt, please contact me.
+ *
+ */
 
 #include "hal.h"
-#include "hal_usbh.h"
 
 #if HAL_USBH_USE_UVC
 
@@ -28,6 +23,10 @@
 #error "USBHUVC needs HAL_USBH_USE_IAD"
 #endif
 
+#include "usbh/dev/uvc.h"
+#include "usbh/internal.h"
+#include <string.h>
+
 #if USBHUVC_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
 #define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
@@ -61,6 +60,9 @@
 #endif
 
 
+USBHUVCDriver USBHUVCD[HAL_USBHUVC_MAX_INSTANCES];
+
+
 static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,
 		const uint8_t *descriptor, uint16_t rem);
 static void uvc_unload(usbh_baseclassdriver_t *drv);
@@ -73,16 +75,653 @@ const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {
 	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt
 };
 
+static bool _request(USBHUVCDriver *uvcdp,
+		uint8_t bRequest, uint8_t entity, uint8_t control,
+		uint16_t wLength, uint8_t *data, uint8_t interf) {
+
+	usbh_urbstatus_t res;
+
+	if (bRequest & 0x80) {
+		res = usbhControlRequest(uvcdp->dev,
+				USBH_REQTYPE_CLASSIN(USBH_REQTYPE_RECIP_INTERFACE),
+				bRequest,
+				((control) << 8),
+				(interf) | ((entity) << 8),
+				wLength, data);
+	} else {
+		res = usbhControlRequest(uvcdp->dev,
+				USBH_REQTYPE_CLASSOUT(USBH_REQTYPE_RECIP_INTERFACE),
+				bRequest,
+				((control) << 8),
+				(interf) | ((entity) << 8),
+				wLength, data);
+	}
+
+	if (res != USBH_URBSTATUS_OK)
+		return HAL_FAILED;
+
+	 return HAL_SUCCESS;
+}
+
+bool usbhuvcVCRequest(USBHUVCDriver *uvcdp,
+		uint8_t bRequest, uint8_t entity, uint8_t control,
+		uint16_t wLength, uint8_t *data) {
+	return _request(uvcdp, bRequest, entity, control, wLength, data, if_get(&uvcdp->ivc)->bInterfaceNumber);
+}
+
+bool usbhuvcVSRequest(USBHUVCDriver *uvcdp,
+		uint8_t bRequest, uint8_t control,
+		uint16_t wLength, uint8_t *data) {
+
+	return _request(uvcdp, bRequest, 0, control, wLength, data, if_get(&uvcdp->ivs)->bInterfaceNumber);
+}
+
+static bool _set_vs_alternate(USBHUVCDriver *uvcdp, uint16_t min_ep_size) {
+
+	if (min_ep_size == 0) {
+		uinfo("Selecting Alternate setting 0");
+		return usbhStdReqSetInterface(uvcdp->dev, if_get(&uvcdp->ivs)->bInterfaceNumber, 0);
+	}
+
+	if_iterator_t iif = uvcdp->ivs;
+	generic_iterator_t iep;
+	const usbh_endpoint_descriptor_t *ep = NULL;
+	uint8_t alt = 0;
+	uint16_t sz = 0xffff;
+
+	uinfof("Searching alternate setting with min_ep_size=%d", min_ep_size);
+
+	for (; iif.valid; if_iter_next(&iif)) {
+		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+
+		if ((ifdesc->bInterfaceClass != UVC_CC_VIDEO)
+				|| (ifdesc->bInterfaceSubClass != UVC_SC_VIDEOSTREAMING))
+			continue;
+
+		uinfof("\tScanning alternate setting=%d", ifdesc->bAlternateSetting);
+
+		if (ifdesc->bNumEndpoints == 0)
+			continue;
+
+		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+			if (((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_ISO)
+					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
+
+				uinfof("\t  Endpoint wMaxPacketSize = %d", epdesc->wMaxPacketSize);
+
+				if (epdesc->wMaxPacketSize >= min_ep_size) {
+					if (epdesc->wMaxPacketSize < sz) {
+						uinfo("\t    Found new optimal alternate setting");
+						sz = epdesc->wMaxPacketSize;
+						alt = ifdesc->bAlternateSetting;
+						ep = epdesc;
+					}
+				}
+			}
+		}
+	}
+
+	if (ep && alt) {
+		uinfof("\tSelecting Alternate setting %d", alt);
+		if (usbhStdReqSetInterface(uvcdp->dev, if_get(&uvcdp->ivs)->bInterfaceNumber, alt) == HAL_SUCCESS) {
+			usbhEPObjectInit(&uvcdp->ep_iso, uvcdp->dev, ep);
+			usbhEPSetName(&uvcdp->ep_iso, "UVC[ISO ]");
+			return HAL_SUCCESS;
+		}
+	}
+
+	return HAL_FAILED;
+}
+
+#if	USBH_DEBUG_ENABLE && USBHUVC_DEBUG_ENABLE_INFO
+void usbhuvcPrintProbeCommit(const usbh_uvc_ctrl_vs_probecommit_data_t *pc) {
+
+	//uinfof("UVC: probe/commit data:");
+	uinfof("\tbmHint=%04x", pc->bmHint);
+	uinfof("\tbFormatIndex=%d, bFrameIndex=%d, dwFrameInterval=%u",
+			pc->bFormatIndex, pc->bFrameIndex, pc->dwFrameInterval);
+	uinfof("\twKeyFrameRate=%d, wPFrameRate=%d, wCompQuality=%u, wCompWindowSize=%u",
+			pc->wKeyFrameRate, pc->wPFrameRate, pc->wCompQuality, pc->wCompWindowSize);
+	uinfof("\twDelay=%d", pc->wDelay);
+	uinfof("\tdwMaxVideoFrameSize=%u", pc->dwMaxVideoFrameSize);
+	uinfof("\tdwMaxPayloadTransferSize=%u", pc->dwMaxPayloadTransferSize);
+/*	uinfof("\tdwClockFrequency=%u", pc->dwClockFrequency);
+	uinfof("\tbmFramingInfo=%02x", pc->bmFramingInfo);
+	uinfof("\tbPreferedVersion=%d, bMinVersion=%d, bMaxVersion=%d",
+			pc->bPreferedVersion, pc->bMinVersion, pc->bMaxVersion); */
+}
+#endif
+
+static void _post(USBHUVCDriver *uvcdp, usbh_urb_t *urb, memory_pool_t *mp, uint16_t type) {
+	usbhuvc_message_base_t *const msg = (usbhuvc_message_base_t *)urb->buff - 1;
+	msg->timestamp = osalOsGetSystemTimeX();
+
+	usbhuvc_message_base_t *const new_msg = (usbhuvc_message_base_t *)chPoolAllocI(mp);
+	if (new_msg != NULL) {
+		/* allocated the new buffer, now try to post the message to the mailbox */
+		if (chMBPostI(&uvcdp->mb, (msg_t)msg) == MSG_OK) {
+			/* everything OK, complete the missing fields */
+			msg->type = type;
+			msg->length = urb->actualLength;
+
+			/* change the URB's buffer to the newly allocated one */
+			urb->buff = (uint8_t *)(new_msg + 1);
+		} else {
+			/* couldn't post the message, free the newly allocated buffer */
+			uerr("UVC: error, mailbox overrun");
+			chPoolFreeI(&uvcdp->mp_status, new_msg);
+		}
+	} else {
+		uerrf("UVC: error, %s pool overrun", mp == &uvcdp->mp_data ? "data" : "status");
+	}
+}
+
+static void _cb_int(usbh_urb_t *urb) {
+	USBHUVCDriver *uvcdp = (USBHUVCDriver *)urb->userData;
+
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		if (urb->actualLength >= 2) {
+			_post(uvcdp, urb, &uvcdp->mp_status, USBHUVC_MESSAGETYPE_STATUS);
+		} else {
+			uerrf("UVC: INT IN, actualLength=%d", urb->actualLength);
+		}
+		break;
+	case USBH_URBSTATUS_TIMEOUT:	/* the device NAKed */
+		udbg("UVC: INT IN no info");
+		break;
+	case USBH_URBSTATUS_DISCONNECTED:
+	case USBH_URBSTATUS_CANCELLED:
+		uwarn("UVC: INT IN status = DISCONNECTED/CANCELLED, aborting");
+		return;
+	default:
+		uerrf("UVC: INT IN error, unexpected status = %d", urb->status);
+		break;
+	}
+
+	usbhURBObjectResetI(urb);
+	usbhURBSubmitI(urb);
+}
+
+static void _cb_iso(usbh_urb_t *urb) {
+	USBHUVCDriver *uvcdp = (USBHUVCDriver *)urb->userData;
+
+	if ((urb->status == USBH_URBSTATUS_DISCONNECTED)
+			|| (urb->status == USBH_URBSTATUS_CANCELLED)) {
+		uwarn("UVC: ISO IN status = DISCONNECTED/CANCELLED, aborting");
+		return;
+	}
+
+	if (urb->status != USBH_URBSTATUS_OK) {
+		uerrf("UVC: ISO IN error, unexpected status = %d", urb->status);
+	} else if (urb->actualLength >= 2) {
+		const uint8_t *const buff = (const uint8_t *)urb->buff;
+		if (buff[0] < 2) {
+			uerrf("UVC: ISO IN, bHeaderLength=%d", buff[0]);
+		} else if (buff[0] > urb->actualLength) {
+			uerrf("UVC: ISO IN, bHeaderLength=%d > actualLength=%d", buff[0], urb->actualLength);
+		} else {
+			udbgf("UVC: ISO IN len=%d, hdr=%d, FID=%d, EOF=%d, ERR=%d, EOH=%d",
+						urb->actualLength,
+						buff[0],
+						buff[1] & UVC_HDR_FID,
+						buff[1] & UVC_HDR_EOF,
+						buff[1] & UVC_HDR_ERR,
+						buff[1] & UVC_HDR_EOH);
+
+			if ((urb->actualLength > buff[0])
+					|| (buff[1] & (UVC_HDR_EOF | UVC_HDR_ERR))) {
+				_post(uvcdp, urb, &uvcdp->mp_data, USBHUVC_MESSAGETYPE_DATA);
+			} else {
+				udbgf("UVC: ISO IN skip: len=%d, hdr=%d, FID=%d, EOF=%d, ERR=%d, EOH=%d",
+						urb->actualLength,
+						buff[0],
+						buff[1] & UVC_HDR_FID,
+						buff[1] & UVC_HDR_EOF,
+						buff[1] & UVC_HDR_ERR,
+						buff[1] & UVC_HDR_EOH);
+			}
+		}
+	} else if (urb->actualLength > 0) {
+		uerrf("UVC: ISO IN, actualLength=%d", urb->actualLength);
+	}
+
+	usbhURBObjectResetI(urb);
+	usbhURBSubmitI(urb);
+}
+
+
+bool usbhuvcStreamStart(USBHUVCDriver *uvcdp, uint16_t min_ep_sz) {
+	bool ret = HAL_FAILED;
+	osalSysLock();
+	osalDbgCheck(uvcdp && (uvcdp->state != USBHUVC_STATE_UNINITIALIZED) &&
+					(uvcdp->state != USBHUVC_STATE_BUSY));
+	if (uvcdp->state == USBHUVC_STATE_STREAMING) {
+		osalSysUnlock();
+		return HAL_SUCCESS;
+	}
+	if (uvcdp->state != USBHUVC_STATE_READY) {
+		osalSysUnlock();
+		return HAL_FAILED;
+	}
+	uvcdp->state = USBHUVC_STATE_BUSY;
+	osalSysUnlock();
+
+	//set the alternate setting
+	if (_set_vs_alternate(uvcdp, min_ep_sz) != HAL_SUCCESS)
+		goto exit;
+
+	//reserve working RAM
+	uint32_t datapackets;
+	uint32_t data_sz = (uvcdp->ep_iso.wMaxPacketSize + sizeof(usbhuvc_message_data_t) + 3) & ~3;
+
+	datapackets = HAL_USBHUVC_WORK_RAM_SIZE / data_sz;
+	if (datapackets == 0) {
+		uerr("Not enough work RAM");
+		goto failed;
+	}
+
+	uint32_t workramsz = datapackets * data_sz;
+	uinfof("Reserving %u bytes of RAM (%d data packets of %d bytes)", workramsz, datapackets, data_sz);
+	if (datapackets > (HAL_USBHUVC_MAX_MAILBOX_SZ - HAL_USBHUVC_STATUS_PACKETS_COUNT)) {
+		uwarn("Mailbox may overflow, use a larger HAL_USBHUVC_MAX_MAILBOX_SZ. UVC will under-utilize the assigned work RAM.");
+	}
+	chMBResumeX(&uvcdp->mb);
+
+	uvcdp->mp_data_buffer = chHeapAlloc(NULL, workramsz);
+	if (uvcdp->mp_data_buffer == NULL) {
+		uerr("Couldn't reserve RAM");
+		goto failed;
+	}
+
+	//initialize the mempool
+	const uint8_t *elem = (const uint8_t *)uvcdp->mp_data_buffer;
+	chPoolObjectInit(&uvcdp->mp_data, data_sz, NULL);
+	while (datapackets--) {
+		chPoolFree(&uvcdp->mp_data, (void *)elem);
+		elem += data_sz;
+	}
+
+	//open the endpoint
+	usbhEPOpen(&uvcdp->ep_iso);
+
+	//allocate 1 buffer and submit the first transfer
+	usbhuvc_message_data_t *const msg = (usbhuvc_message_data_t *)chPoolAlloc(&uvcdp->mp_data);
+	osalDbgCheck(msg);
+	usbhURBObjectInit(&uvcdp->urb_iso, &uvcdp->ep_iso, _cb_iso, uvcdp, msg->data, uvcdp->ep_iso.wMaxPacketSize);
+	osalSysLock();
+	usbhURBSubmitI(&uvcdp->urb_iso);
+	osalOsRescheduleS();
+	osalSysUnlock();
+
+	ret = HAL_SUCCESS;
+	goto exit;
+
+failed:
+	_set_vs_alternate(uvcdp, 0);
+	if (uvcdp->mp_data_buffer)
+		chHeapFree(uvcdp->mp_data_buffer);
+
+exit:
+	osalSysLock();
+	if (ret == HAL_SUCCESS)
+		uvcdp->state = USBHUVC_STATE_STREAMING;
+	else
+		uvcdp->state = USBHUVC_STATE_READY;
+	osalSysUnlock();
+	return ret;
+}
+
+bool usbhuvcStreamStop(USBHUVCDriver *uvcdp) {
+	osalSysLock();
+	osalDbgCheck(uvcdp && (uvcdp->state != USBHUVC_STATE_UNINITIALIZED) &&
+					(uvcdp->state != USBHUVC_STATE_BUSY));
+	if (uvcdp->state != USBHUVC_STATE_STREAMING) {
+		osalSysUnlock();
+		return HAL_SUCCESS;
+	}
+	uvcdp->state = USBHUVC_STATE_BUSY;
+
+	//close the ISO endpoint
+	usbhEPCloseS(&uvcdp->ep_iso);
+
+	//purge the mailbox
+	chMBResetI(&uvcdp->mb);		//TODO: the status messages are lost!!
+	chMtxLockS(&uvcdp->mtx);
+	osalSysUnlock();
+
+	//free the working memory
+	chHeapFree(uvcdp->mp_data_buffer);
+	uvcdp->mp_data_buffer = 0;
+
+	//set alternate setting to 0
+	_set_vs_alternate(uvcdp, 0);
+
+	osalSysLock();
+	uvcdp->state = USBHUVC_STATE_READY;
+	chMtxUnlockS(&uvcdp->mtx);
+	osalSysUnlock();
+	return HAL_SUCCESS;
+}
+
+bool usbhuvcFindVSDescriptor(USBHUVCDriver *uvcdp,
+		generic_iterator_t *ics,
+		uint8_t bDescriptorSubtype,
+		bool start) {
+
+	if (start)
+		cs_iter_init(ics, (generic_iterator_t *)&uvcdp->ivs);
+	else
+		cs_iter_next(ics);
+
+	for (; ics->valid; cs_iter_next(ics)) {
+		if (ics->curr[1] != UVC_CS_INTERFACE)
+			break;
+		if (ics->curr[2] == bDescriptorSubtype)
+			return HAL_SUCCESS;
+		if (!start)
+			break;
+	}
+	return HAL_FAILED;
+}
+
+void usbhuvcResetPC(USBHUVCDriver *uvcdp) {
+	memset(&uvcdp->pc, 0, sizeof(uvcdp->pc));
+}
+
+bool usbhuvcProbe(USBHUVCDriver *uvcdp) {
+//	memset(&uvcdp->pc_min, 0, sizeof(uvcdp->pc_min));
+//	memset(&uvcdp->pc_max, 0, sizeof(uvcdp->pc_max));
+
+	if (usbhuvcVSRequest(uvcdp, UVC_SET_CUR, UVC_CTRL_VS_PROBE_CONTROL, sizeof(uvcdp->pc), (uint8_t *)&uvcdp->pc) != HAL_SUCCESS)
+		return HAL_FAILED;
+	if (usbhuvcVSRequest(uvcdp, UVC_GET_CUR, UVC_CTRL_VS_PROBE_CONTROL, sizeof(uvcdp->pc), (uint8_t *)&uvcdp->pc) != HAL_SUCCESS)
+		return HAL_FAILED;
+	if (usbhuvcVSRequest(uvcdp, UVC_GET_MAX, UVC_CTRL_VS_PROBE_CONTROL, sizeof(uvcdp->pc_max), (uint8_t *)&uvcdp->pc_max) != HAL_SUCCESS)
+		return HAL_FAILED;
+	if (usbhuvcVSRequest(uvcdp, UVC_GET_MIN, UVC_CTRL_VS_PROBE_CONTROL, sizeof(uvcdp->pc_min), (uint8_t *)&uvcdp->pc_min) != HAL_SUCCESS)
+		return HAL_FAILED;
+	return HAL_SUCCESS;
+}
+
+bool usbhuvcCommit(USBHUVCDriver *uvcdp) {
+	if (usbhuvcVSRequest(uvcdp, UVC_SET_CUR, UVC_CTRL_VS_COMMIT_CONTROL, sizeof(uvcdp->pc), (uint8_t *)&uvcdp->pc) != HAL_SUCCESS)
+		return HAL_FAILED;
+
+	osalSysLock();
+	if (uvcdp->state == USBHUVC_STATE_ACTIVE)
+		uvcdp->state = USBHUVC_STATE_READY;
+	osalSysUnlock();
+	return HAL_SUCCESS;
+}
+
+uint32_t usbhuvcEstimateRequiredEPSize(USBHUVCDriver *uvcdp, const uint8_t *formatdesc,
+		const uint8_t *framedesc, uint32_t dwFrameInterval) {
+
+	osalDbgCheck(framedesc);
+	osalDbgCheck(framedesc[0] > 3);
+	osalDbgCheck(framedesc[1] == UVC_CS_INTERFACE);
+	osalDbgCheck(formatdesc);
+	osalDbgCheck(formatdesc[0] > 3);
+	osalDbgCheck(formatdesc[1] == UVC_CS_INTERFACE);
+
+	uint16_t w, h, div, mul;
+	uint8_t bpp;
+
+	switch (framedesc[2]) {
+	case UVC_VS_FRAME_MJPEG: {
+		const usbh_uvc_frame_mjpeg_t *frame = (const usbh_uvc_frame_mjpeg_t *)framedesc;
+		//const usbh_uvc_format_mjpeg_t *fmt = (const usbh_uvc_format_mjpeg_t *)formatdesc;
+		w = frame->wWidth;
+		h = frame->wHeight;
+		bpp = 16;	//TODO: check this!!
+		mul = 1;
+		div = 5;	//TODO: check this estimate
+	}	break;
+	case UVC_VS_FRAME_UNCOMPRESSED: {
+		const usbh_uvc_frame_uncompressed_t *frame = (const usbh_uvc_frame_uncompressed_t *)framedesc;
+		const usbh_uvc_format_uncompressed *fmt = (const usbh_uvc_format_uncompressed *)formatdesc;
+		w = frame->wWidth;
+		h = frame->wHeight;
+		bpp = fmt->bBitsPerPixel;
+		mul = div = 1;
+	}	break;
+	default:
+		uwarn("Unsupported format");
+		return 0xffffffff;
+	}
+
+	uint32_t sz = w * h / 8 * bpp;
+	sz *= 10000000UL / dwFrameInterval;
+	sz /= 1000;
+
+	if (uvcdp->dev->speed == USBH_DEVSPEED_HIGH)
+		div *= 8;
+
+	return (sz * mul) / div + 12;
+}
+
+void usbhuvcObjectInit(USBHUVCDriver *uvcdp) {
+	osalDbgCheck(uvcdp != NULL);
+	memset(uvcdp, 0, sizeof(*uvcdp));
+	uvcdp->info = &usbhuvcClassDriverInfo;
+	chMBObjectInit(&uvcdp->mb, uvcdp->mb_buff, HAL_USBHUVC_MAX_MAILBOX_SZ);
+	chMtxObjectInit(&uvcdp->mtx);
+	uvcdp->state = USBHUVC_STATE_STOP;
+}
+
 
 static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
-	(void)dev;
-	(void)descriptor;
-	(void)rem;
+
+	USBHUVCDriver *uvcdp;
+	uint8_t i;
+
+	if (descriptor[1] != USBH_DT_INTERFACE_ASSOCIATION)
+		return NULL;
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHUVC_MAX_INSTANCES; i++) {
+		if (USBHUVCD[i].dev == NULL) {
+			uvcdp = &USBHUVCD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("Can't alloc UVC driver");
+
+	/* can't alloc */
 	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	uvcdp->ivc.curr = uvcdp->ivs.curr = NULL;
+
+	usbhEPSetName(&dev->ctrl, "UVC[CTRL]");
+
+	const usbh_ia_descriptor_t *iad = (const usbh_ia_descriptor_t *)descriptor;
+	if_iterator_t iif;
+	generic_iterator_t ics;
+	generic_iterator_t iep;
+
+	iif.iad = iad;
+	iif.curr = descriptor;
+	iif.rem = rem;
+
+	for (if_iter_next(&iif); iif.valid; if_iter_next(&iif)) {
+		if (iif.iad != iad) break;
+
+		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+		if (ifdesc->bInterfaceClass != UVC_CC_VIDEO) {
+			uwarnf("Skipping Interface %d (class != UVC_CC_VIDEO)",
+					ifdesc->bInterfaceNumber);
+			continue;
+		}
+
+		uinfof("Interface %d, Alt=%d, Class=UVC_CC_VIDEO, Subclass=%02x",
+				ifdesc->bInterfaceNumber,
+				ifdesc->bAlternateSetting,
+				ifdesc->bInterfaceSubClass);
+
+		switch (ifdesc->bInterfaceSubClass) {
+		case UVC_SC_VIDEOCONTROL:
+			if (uvcdp->ivc.curr == NULL) {
+				uvcdp->ivc = iif;
+			}
+			for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {
+				if (ics.curr[1] != UVC_CS_INTERFACE) {
+					uwarnf("Unknown descriptor=%02X", ics.curr[1]);
+					continue;
+				}
+				switch (ics.curr[2]) {
+				case UVC_VC_HEADER:
+					uinfo("  VC_HEADER"); break;
+				case UVC_VC_INPUT_TERMINAL:
+					uinfof("    VC_INPUT_TERMINAL, ID=%d", ics.curr[3]); break;
+				case UVC_VC_OUTPUT_TERMINAL:
+					uinfof("    VC_OUTPUT_TERMINAL, ID=%d", ics.curr[3]); break;
+				case UVC_VC_SELECTOR_UNIT:
+					uinfof("    VC_SELECTOR_UNIT, ID=%d", ics.curr[3]); break;
+				case UVC_VC_PROCESSING_UNIT:
+					uinfof("    VC_PROCESSING_UNIT, ID=%d", ics.curr[3]); break;
+				case UVC_VC_EXTENSION_UNIT:
+					uinfof("    VC_EXTENSION_UNIT, ID=%d", ics.curr[3]); break;
+				default:
+					uwarnf("Unknown video bDescriptorSubtype=%02x", ics.curr[2]);
+					break;
+				}
+			}
+			break;
+		case UVC_SC_VIDEOSTREAMING:
+			if (uvcdp->ivs.curr == NULL) {
+				uvcdp->ivs = iif;
+			}
+			for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {
+				if (ics.curr[1] != UVC_CS_INTERFACE) {
+					uwarnf("Unknown descriptor=%02X", ics.curr[1]);
+					continue;
+				}
+				switch (ics.curr[2]) {
+				case UVC_VS_INPUT_HEADER:
+					uinfo("  VS_INPUT_HEADER"); break;
+				case UVC_VS_OUTPUT_HEADER:
+					uinfo("  VS_OUTPUT_HEADER"); break;
+				case UVC_VS_STILL_IMAGE_FRAME:
+					uinfo("    VS_STILL_IMAGE_FRAME"); break;
+
+				case UVC_VS_FORMAT_UNCOMPRESSED:
+					uinfof("    VS_FORMAT_UNCOMPRESSED, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_MPEG2TS:
+					uinfof("    VS_FORMAT_MPEG2TS, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_DV:
+					uinfof("    VS_FORMAT_DV, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_MJPEG:
+					uinfof("    VS_FORMAT_MJPEG, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_FRAME_BASED:
+					uinfof("    VS_FORMAT_FRAME_BASED, bFormatIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FORMAT_STREAM_BASED:
+					uinfof("    VS_FORMAT_STREAM_BASED, bFormatIndex=%d", ics.curr[3]); break;
+
+				case UVC_VS_FRAME_UNCOMPRESSED:
+					uinfof("      VS_FRAME_UNCOMPRESSED, bFrameIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FRAME_MJPEG:
+					uinfof("      VS_FRAME_MJPEG, bFrameIndex=%d", ics.curr[3]); break;
+				case UVC_VS_FRAME_FRAME_BASED:
+					uinfof("      VS_FRAME_FRAME_BASED, bFrameIndex=%d", ics.curr[3]); break;
+
+				case UVC_VS_COLOR_FORMAT:
+					uinfo("      VS_COLOR_FORMAT"); break;
+				default:
+					uwarnf("Unknown video bDescriptorSubtype=%02x", ics.curr[2]);
+					break;
+				}
+			}
+			break;
+		default:
+			uwarnf("Unknown video bInterfaceSubClass=%02x", ifdesc->bInterfaceSubClass);
+			break;
+		}
+
+		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+
+			if ((ifdesc->bInterfaceSubClass == UVC_SC_VIDEOCONTROL)
+					&& ((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_INT)
+					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
+				/* found VC interrupt endpoint */
+				uinfof("  VC Interrupt endpoint; %02x, bInterval=%d",
+						epdesc->bEndpointAddress, epdesc->bInterval);
+				usbhEPObjectInit(&uvcdp->ep_int, dev, epdesc);
+				usbhEPSetName(&uvcdp->ep_int, "UVC[INT ]");
+			} else if ((ifdesc->bInterfaceSubClass == UVC_SC_VIDEOSTREAMING)
+					&& ((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_ISO)
+					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
+				/* found VS isochronous endpoint */
+				uinfof("  VS Isochronous endpoint; %02x, bInterval=%d, bmAttributes=%02x",
+						epdesc->bEndpointAddress, epdesc->bInterval, epdesc->bmAttributes);
+			} else {
+				/* unknown EP */
+				uwarnf("  <unknown endpoint>, bEndpointAddress=%02x, bmAttributes=%02x",
+									epdesc->bEndpointAddress, epdesc->bmAttributes);
+			}
+
+			for (cs_iter_init(&ics, &iep); ics.valid; cs_iter_next(&ics)) {
+				uinfof("    CS_ENDPOINT bLength=%d, bDescriptorType=%02X",
+						ics.curr[0], ics.curr[1]);
+			}
+		}
+	}
+
+	if ((uvcdp->ivc.curr == NULL) || (uvcdp->ivs.curr == NULL)) {
+		return NULL;
+	}
+
+//	uvcdp->dev = dev;
+
+	_set_vs_alternate(uvcdp, 0);
+
+	/* initialize the INT endpoint */
+	chPoolObjectInit(&uvcdp->mp_status, sizeof(usbhuvc_message_status_t), NULL);
+	for(i = 0; i < HAL_USBHUVC_STATUS_PACKETS_COUNT; i++)
+		chPoolFree(&uvcdp->mp_status, &uvcdp->mp_status_buffer[i]);
+
+	usbhEPOpen(&uvcdp->ep_int);
+
+	usbhuvc_message_status_t *const msg = (usbhuvc_message_status_t *)chPoolAlloc(&uvcdp->mp_status);
+	osalDbgCheck(msg);
+	usbhURBObjectInit(&uvcdp->urb_int, &uvcdp->ep_int, _cb_int, uvcdp, msg->data, USBHUVC_MAX_STATUS_PACKET_SZ);
+	osalSysLock();
+	usbhURBSubmitI(&uvcdp->urb_int);
+	uvcdp->state = USBHUVC_STATE_ACTIVE;
+	osalOsRescheduleS();
+	osalSysUnlock();
+
+	dev->keepFullCfgDesc++;
+	return (usbh_baseclassdriver_t *)uvcdp;
 }
 
 static void uvc_unload(usbh_baseclassdriver_t *drv) {
-	(void)drv;
+	USBHUVCDriver *const uvcdp = (USBHUVCDriver *)drv;
+
+	usbhuvcStreamStop(uvcdp);
+
+	usbhEPClose(&uvcdp->ep_int);
+
+	//TODO: free
+
+	if (drv->dev->keepFullCfgDesc)
+		drv->dev->keepFullCfgDesc--;
+
+	osalSysLock();
+	uvcdp->state = USBHUVC_STATE_STOP;
+	osalSysUnlock();
+}
+
+void usbhuvcInit(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHUVC_MAX_INSTANCES; i++) {
+		usbhuvcObjectInit(&USBHUVCD[i]);
+	}
 }
 
 #endif