Merge pull request #122 from dismirlian/usbh_devel

USBH: Various improvo

26 files changed, 1174 insertions, 914 deletions

diff --git a/os/hal/include/hal_usbh.h b/os/hal/include/hal_usbh.h
index b8c229a..1ed6416 100644
--- a/os/hal/include/hal_usbh.h
+++ b/os/hal/include/hal_usbh.h
@@ -48,6 +48,10 @@
 #define HAL_USBH_USE_HID FALSE
 #endif
 
+#ifndef HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
+#define HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS	FALSE
+#endif
+
 #define HAL_USBH_USE_IAD     HAL_USBH_USE_UVC
 
 #if (HAL_USE_USBH == TRUE) || defined(__DOXYGEN__)
@@ -56,13 +60,6 @@
 #include "usbh/list.h"
 #include "usbh/defs.h"
 
-/* TODO:
- *
- * - Integrate VBUS power switching functionality to the API.
- *
- */
-
-
 /*===========================================================================*/
 /* Derived constants and error checks.                                       */
 /*===========================================================================*/
@@ -79,6 +76,7 @@ enum usbh_status {
 	USBH_STATUS_SUSPENDED,
 };
 
+/* These correspond to the USB spec */
 enum usbh_devstatus {
 	USBH_DEVSTATUS_DISCONNECTED = 0,
 	USBH_DEVSTATUS_ATTACHED,
@@ -354,10 +352,8 @@ extern "C" {
 		osalDbgCheck(ep != 0);
 		osalDbgCheckClassS();
 		osalDbgAssert(ep->status != USBH_EPSTATUS_UNINITIALIZED, "invalid state");
-		if (ep->status == USBH_EPSTATUS_CLOSED) {
-			osalOsRescheduleS();
+		if (ep->status == USBH_EPSTATUS_CLOSED)
 			return;
-		}
 		usbh_lld_ep_close(ep);
 	}
 	static inline void usbhEPClose(usbh_ep_t *ep) {
@@ -388,6 +384,22 @@ extern "C" {
 	void usbhURBCancelAndWaitS(usbh_urb_t *urb);
 	msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout);
 
+	static inline void usbhURBSubmit(usbh_urb_t *urb) {
+		osalSysLock();
+		usbhURBSubmitI(urb);
+		osalOsRescheduleS();
+		osalSysUnlock();
+	}
+
+	static inline bool usbhURBCancel(usbh_urb_t *urb) {
+		bool ret;
+		osalSysLock();
+		ret = usbhURBCancelI(urb);
+		osalOsRescheduleS();
+		osalSysUnlock();
+		return ret;
+	}
+
 	/* Main loop */
 	void usbhMainLoop(USBHDriver *usbh);
 
@@ -402,14 +414,13 @@ extern "C" {
 
 typedef struct usbh_classdriver_vmt usbh_classdriver_vmt_t;
 struct usbh_classdriver_vmt {
+	void (*init)(void);
 	usbh_baseclassdriver_t *(*load)(usbh_device_t *dev,	const uint8_t *descriptor, uint16_t rem);
 	void (*unload)(usbh_baseclassdriver_t *drv);
+	/* TODO: add power control, suspend, etc */
 };
 
 struct usbh_classdriverinfo {
-	int16_t class;
-	int16_t subclass;
-	int16_t protocol;
 	const char *name;
 	const usbh_classdriver_vmt_t *vmt;
 };
@@ -423,13 +434,6 @@ struct usbh_baseclassdriver {
 	_usbh_base_classdriver_data
 };
 
-
-/*===========================================================================*/
-/* Helper functions.				                                         */
-/*===========================================================================*/
-#include <usbh/desciter.h>	/* descriptor iterators */
-#include <usbh/debug.h>		/* debug */
-
 #endif
 
 #endif /* HAL_USBH_H_ */
diff --git a/os/hal/include/usbh/dev/aoa.h b/os/hal/include/usbh/dev/aoa.h
index 636768a..a7f1c1b 100644
--- a/os/hal/include/usbh/dev/aoa.h
+++ b/os/hal/include/usbh/dev/aoa.h
@@ -140,12 +140,8 @@ extern USBHAOADriver USBHAOAD[HAL_USBHAOA_MAX_INSTANCES];
 extern "C" {
 #endif
 	/* AOA device driver */
-	void usbhaoaObjectInit(USBHAOADriver *aoap);
 	void usbhaoaChannelStart(USBHAOADriver *aoap);
 	void usbhaoaChannelStop(USBHAOADriver *aoap);
-
-	/* global initializer */
-	void usbhaoaInit(void);
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/include/usbh/dev/ftdi.h b/os/hal/include/usbh/dev/ftdi.h
index bfa3103..eedb056 100644
--- a/os/hal/include/usbh/dev/ftdi.h
+++ b/os/hal/include/usbh/dev/ftdi.h
@@ -138,16 +138,9 @@ extern USBHFTDIPortDriver FTDIPD[HAL_USBHFTDI_MAX_PORTS];
 #ifdef __cplusplus

 extern "C" {

 #endif

-	/* FTDI device driver */

-	void usbhftdiObjectInit(USBHFTDIDriver *ftdip);

-

 	/* FTDI port driver */

-	void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp);

 	void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config);

 	void usbhftdipStop(USBHFTDIPortDriver *ftdipp);

-

-	/* global initializer */

-	void usbhftdiInit(void);

 #ifdef __cplusplus

 }

 #endif

diff --git a/os/hal/include/usbh/dev/hid.h b/os/hal/include/usbh/dev/hid.h
index 0d6b894..c7371ee 100644
--- a/os/hal/include/usbh/dev/hid.h
+++ b/os/hal/include/usbh/dev/hid.h
@@ -95,6 +95,8 @@ struct USBHHIDDriver {
 	usbh_urb_t in_urb;
 
 	const USBHHIDConfig *config;
+
+	semaphore_t sem;
 };
 
 
@@ -112,9 +114,6 @@ extern USBHHIDDriver USBHHIDD[HAL_USBHHID_MAX_INSTANCES];
 #ifdef __cplusplus
 extern "C" {
 #endif
-	/* HID Driver */
-	void usbhHIDObjectInit(USBHHIDDriver *hidp);
-
 	/* HID Common API */
 	usbh_urbstatus_t usbhhidGetReport(USBHHIDDriver *hidp,
 			uint8_t report_id, usbhhid_reporttype_t report_type,
@@ -136,9 +135,6 @@ extern "C" {
 	}
 
 	void usbhhidStart(USBHHIDDriver *hidp, const USBHHIDConfig *cfg);
-
-	/* global initializer */
-	void usbhhidInit(void);
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/include/usbh/dev/hub.h b/os/hal/include/usbh/dev/hub.h
index 924ebec..406fbaf 100644
--- a/os/hal/include/usbh/dev/hub.h
+++ b/os/hal/include/usbh/dev/hub.h
@@ -88,10 +88,6 @@ static inline usbh_urbstatus_t usbhhubSetFeaturePort(usbh_port_t *port, uint8_t
 				0);

 }

 

-void usbhhubObjectInit(USBHHubDriver *hubdp);

-

-void usbhhubInit(void);

-

 #else

 

 static inline usbh_urbstatus_t usbhhubControlRequest(USBHDriver *host,

diff --git a/os/hal/include/usbh/dev/msd.h b/os/hal/include/usbh/dev/msd.h
index 84179c5..eedd474 100644
--- a/os/hal/include/usbh/dev/msd.h
+++ b/os/hal/include/usbh/dev/msd.h
@@ -58,30 +58,15 @@ struct USBHMassStorageLUNDriver {
 	const struct USBHMassStorageDriverVMT *vmt;

 	_base_block_device_data

 

+	/* for serializing access to the LUN driver */

+	semaphore_t sem;

+

 	BlockDeviceInfo info;

 	USBHMassStorageDriver *msdp;

 

 	USBHMassStorageLUNDriver *next;

 };

 

-struct USBHMassStorageDriver {

-	/* inherited from abstract class driver */

-	_usbh_base_classdriver_data

-

-	/* for LUN request serialization, can be removed

-	 * if the driver is configured to support only one LUN

-	 * per USBHMassStorageDriver instance */

-	mutex_t mtx;

-

-	usbh_ep_t epin;

-	usbh_ep_t epout;

-	uint8_t ifnum;

-	uint8_t max_lun;

-	uint32_t tag;

-

-	USBHMassStorageLUNDriver *luns;

-};

-

 

 /*===========================================================================*/

 /* Driver macros.                                                            */

@@ -93,18 +78,13 @@ struct USBHMassStorageDriver {
 /*===========================================================================*/

 

 extern USBHMassStorageLUNDriver MSBLKD[HAL_USBHMSD_MAX_LUNS];

-extern USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];

 

 #ifdef __cplusplus

 extern "C" {

 #endif

-	/* Mass Storage Driver */

-	void usbhmsdObjectInit(USBHMassStorageDriver *msdp);

-

 	/* Mass Storage LUN Driver (block driver) */

-	void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp);

-	void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp);

-	void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp);

+//	void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp);

+//	void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp);

 	bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp);

 	bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp);

 	bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

@@ -115,9 +95,6 @@ extern "C" {
 	bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip);

 	bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp);

 	bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp);

-

-	/* global initializer */

-	void usbhmsdInit(void);

 #ifdef __cplusplus

 }

 #endif

diff --git a/os/hal/include/usbh/dev/uvc.h b/os/hal/include/usbh/dev/uvc.h
index c68a082..817d465 100644
--- a/os/hal/include/usbh/dev/uvc.h
+++ b/os/hal/include/usbh/dev/uvc.h
@@ -22,11 +22,7 @@
 
 #if HAL_USE_USBH && HAL_USBH_USE_UVC
 
-/* TODO:
- *
- *
- */
-
+#include "usbh/desciter.h"
 
 /*===========================================================================*/
 /* Driver pre-compile time settings.                                         */
@@ -394,9 +390,6 @@ 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;
 	}
@@ -457,11 +450,6 @@ extern "C" {
 	static inline void usbhuvcFreeStatusMessage(USBHUVCDriver *uvcdp, usbhuvc_message_status_t *msg) {
 		chPoolFree(&uvcdp->mp_status, msg);
 	}
-
-
-	/* global initializer */
-	void usbhuvcInit(void);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/os/hal/include/usbh/internal.h b/os/hal/include/usbh/internal.h
index 70d2f7a..f6f17b7 100644
--- a/os/hal/include/usbh/internal.h
+++ b/os/hal/include/usbh/internal.h
@@ -54,6 +54,9 @@ void _usbh_urb_completeI(usbh_urb_t *urb, usbh_urbstatus_t status);
 bool _usbh_urb_abortI(usbh_urb_t *urb, usbh_urbstatus_t status);

 void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status);

 

+bool _usbh_match_vid_pid(usbh_device_t *dev, int32_t vid, int32_t pid);

+bool _usbh_match_descriptor(const uint8_t *descriptor, uint16_t rem,

+		int16_t type, int16_t _class, int16_t subclass, int16_t protocol);

 

 #define USBH_REQTYPE_CLASSIN(type)	\

 	(USBH_REQTYPE_DIR_IN | type | USBH_REQTYPE_TYPE_CLASS)

@@ -137,6 +140,9 @@ void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status);
 

 #define sizeof_array(x) 	(sizeof(x)/sizeof(*(x)))

 

+#include "usbh/desciter.h"	/* descriptor iterators */

+#include "usbh/debug.h"

+

 #endif

 

 #endif /* USBH_INTERNAL_H_ */

diff --git a/os/hal/include/usbh/list.h b/os/hal/include/usbh/list.h
index 317e51b..cdcca04 100644
--- a/os/hal/include/usbh/list.h
+++ b/os/hal/include/usbh/list.h
@@ -41,21 +41,15 @@ static inline void INIT_LIST_HEAD(struct list_head *list)
  * This is only for internal list manipulation where we know

  * the prev/next entries already!

  */

-#ifndef CONFIG_DEBUG_LIST

-static inline void __list_add(struct list_head *new,

+static inline void __list_add(struct list_head *_new,

                               struct list_head *prev,

                               struct list_head *next)

 {

-        next->prev = new;

-        new->next = next;

-        new->prev = prev;

-        prev->next = new;

+        next->prev = _new;

+        _new->next = next;

+        _new->prev = prev;

+        prev->next = _new;

 }

-#else

-extern void __list_add(struct list_head *new,

-                              struct list_head *prev,

-                              struct list_head *next);

-#endif

 

 /**

  * list_add - add a new entry

@@ -65,9 +59,9 @@ extern void __list_add(struct list_head *new,
  * Insert a new entry after the specified head.

  * This is good for implementing stacks.

  */

-static inline void list_add(struct list_head *new, struct list_head *head)

+static inline void list_add(struct list_head *_new, struct list_head *head)

 {

-        __list_add(new, head, head->next);

+        __list_add(_new, head, head->next);

 }

 

 

@@ -79,9 +73,9 @@ static inline void list_add(struct list_head *new, struct list_head *head)
  * Insert a new entry before the specified head.

  * This is useful for implementing queues.

  */

-static inline void list_add_tail(struct list_head *new, struct list_head *head)

+static inline void list_add_tail(struct list_head *_new, struct list_head *head)

 {

-        __list_add(new, head->prev, head);

+        __list_add(_new, head->prev, head);

 }

 

 /*

@@ -103,7 +97,7 @@ static inline void __list_del(struct list_head * prev, struct list_head * next)
  * Note: list_empty() on entry does not return true after this, the entry is

  * in an undefined state.

  */

-#ifndef CONFIG_DEBUG_LIST

+

 static inline void __list_del_entry(struct list_head *entry)

 {

         __list_del(entry->prev, entry->next);

@@ -112,35 +106,6 @@ static inline void __list_del_entry(struct list_head *entry)
 static inline void list_del(struct list_head *entry)

 {

         __list_del(entry->prev, entry->next);

-       // entry->next = LIST_POISON1;

-       // entry->prev = LIST_POISON2;

-}

-#else

-extern void __list_del_entry(struct list_head *entry);

-extern void list_del(struct list_head *entry);

-#endif

-

-/**

- * list_replace - replace old entry by new one

- * @old : the element to be replaced

- * @new : the new element to insert

- *

- * If @old was empty, it will be overwritten.

- */

-static inline void list_replace(struct list_head *old,

-                                struct list_head *new)

-{

-        new->next = old->next;

-        new->next->prev = new;

-        new->prev = old->prev;

-        new->prev->next = new;

-}

-

-static inline void list_replace_init(struct list_head *old,

-                                        struct list_head *new)

-{

-        list_replace(old, new);

-        INIT_LIST_HEAD(old);

 }

 

 /**

@@ -154,26 +119,159 @@ static inline void list_del_init(struct list_head *entry)
 }

 

 /**

- * list_move - delete from one list and add as another's head

+ * list_move_tail - delete from one list and add as another's tail

  * @list: the entry to move

- * @head: the head that will precede our entry

+ * @head: the head that will follow our entry

  */

-static inline void list_move(struct list_head *list, struct list_head *head)

+static inline void list_move_tail(struct list_head *list,

+                                  struct list_head *head)

 {

         __list_del_entry(list);

-        list_add(list, head);

+        list_add_tail(list, head);

 }

 

+

 /**

- * list_move_tail - delete from one list and add as another's tail

+ * list_empty - tests whether a list is empty

+ * @head: the list to test.

+ */

+static inline int list_empty(const struct list_head *head)

+{

+        return head->next == head;

+}

+

+/**

+ * list_entry - get the struct for this entry

+ * @ptr:        the &struct list_head pointer.

+ * @type:       the type of the struct this is embedded in.

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_entry(ptr, type, member) \

+        container_of(ptr, type, member)

+

+/**

+ * list_first_entry - get the first element from a list

+ * @ptr:        the list head to take the element from.

+ * @type:       the type of the struct this is embedded in.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Note, that list is expected to be not empty.

+ */

+#define list_first_entry(ptr, type, member) \

+        list_entry((ptr)->next, type, member)

+

+/**

+ * list_next_entry - get the next element in list

+ * @pos:        the type * to cursor

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_next_entry(pos, type, member) \

+        list_entry((pos)->member.next, type, member)

+

+/**

+ * list_for_each_entry  -       iterate over list of given type

+ * @pos:        the type * to use as a loop cursor.

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_for_each_entry(pos, type, head, member)                          \

+        for (pos = list_first_entry(head, type, member);        \

+             &pos->member != (head);                                    \

+             pos = list_next_entry(pos, type, member))

+

+

+/**

+ * list_for_each_entry_safe - iterate over list of given type safe against removal of list entry

+ * @pos:        the type * to use as a loop cursor.

+ * @n:          another type * to use as temporary storage

+ * @head:       the head for your list.

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_for_each_entry_safe(pos, type, n, head, member)                  \

+        for (pos = list_first_entry(head, type, member),        \

+                n = list_next_entry(pos, type, member);                       \

+             &pos->member != (head);                                    \

+             pos = n, n = list_next_entry(n, type, member))

+

+#if 0

+

+/**

+ * list_for_each        -       iterate over a list

+ * @pos:        the &struct list_head to use as a loop cursor.

+ * @head:       the head for your list.

+ */

+#define list_for_each(pos, head) \

+        for (pos = (head)->next; pos != (head); pos = pos->next)

+

+/**

+ * list_for_each_safe - iterate over a list safe against removal of list entry

+ * @pos:        the &struct list_head to use as a loop cursor.

+ * @n:          another &struct list_head to use as temporary storage

+ * @head:       the head for your list.

+ */

+#define list_for_each_safe(pos, n, head) \

+        for (pos = (head)->next, n = pos->next; pos != (head); \

+                pos = n, n = pos->next)

+

+/**

+ * list_prev_entry - get the prev element in list

+ * @pos:        the type * to cursor

+ * @member:     the name of the list_head within the struct.

+ */

+#define list_prev_entry(pos, type, member) \

+        list_entry((pos)->member.prev, type, member)

+

+/**

+ * list_for_each_prev   -       iterate over a list backwards

+ * @pos:        the &struct list_head to use as a loop cursor.

+ * @head:       the head for your list.

+ */

+#define list_for_each_prev(pos, head) \

+        for (pos = (head)->prev; pos != (head); pos = pos->prev)

+

+/**

+ * list_for_each_prev_safe - iterate over a list backwards safe against removal of list entry

+ * @pos:        the &struct list_head to use as a loop cursor.

+ * @n:          another &struct list_head to use as temporary storage

+ * @head:       the head for your list.

+ */

+#define list_for_each_prev_safe(pos, n, head) \

+        for (pos = (head)->prev, n = pos->prev; \

+             pos != (head); \

+             pos = n, n = pos->prev)

+

+/**

+ * list_last_entry - get the last element from a list

+ * @ptr:        the list head to take the element from.

+ * @type:       the type of the struct this is embedded in.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Note, that list is expected to be not empty.

+ */

+#define list_last_entry(ptr, type, member) \

+        list_entry((ptr)->prev, type, member)

+

+/**

+ * list_first_entry_or_null - get the first element from a list

+ * @ptr:        the list head to take the element from.

+ * @type:       the type of the struct this is embedded in.

+ * @member:     the name of the list_head within the struct.

+ *

+ * Note that if the list is empty, it returns NULL.

+ */

+#define list_first_entry_or_null(ptr, type, member) \

+        (!list_empty(ptr) ? list_first_entry(ptr, type, member) : NULL)

+

+

+/**

+ * list_move - delete from one list and add as another's head

  * @list: the entry to move

- * @head: the head that will follow our entry

+ * @head: the head that will precede our entry

  */

-static inline void list_move_tail(struct list_head *list,

-                                  struct list_head *head)

+static inline void list_move(struct list_head *list, struct list_head *head)

 {

         __list_del_entry(list);

-        list_add_tail(list, head);

+        list_add(list, head);

 }

 

 /**

@@ -188,15 +286,6 @@ static inline int list_is_last(const struct list_head *list,
 }

 

 /**

- * list_empty - tests whether a list is empty

- * @head: the list to test.

- */

-static inline int list_empty(const struct list_head *head)

-{

-        return head->next == head;

-}

-

-/**

  * list_empty_careful - tests whether a list is empty and not being modified

  * @head: the list to test

  *

@@ -350,110 +439,28 @@ static inline void list_splice_tail_init(struct list_head *list,
 }

 

 /**

- * list_entry - get the struct for this entry

- * @ptr:        the &struct list_head pointer.

- * @type:       the type of the struct this is embedded in.

- * @member:     the name of the list_head within the struct.

- */

-#define list_entry(ptr, type, member) \

-        container_of(ptr, type, member)

-

-/**

- * list_first_entry - get the first element from a list

- * @ptr:        the list head to take the element from.

- * @type:       the type of the struct this is embedded in.

- * @member:     the name of the list_head within the struct.

- *

- * Note, that list is expected to be not empty.

- */

-#define list_first_entry(ptr, type, member) \

-        list_entry((ptr)->next, type, member)

-

-/**

- * list_last_entry - get the last element from a list

- * @ptr:        the list head to take the element from.

- * @type:       the type of the struct this is embedded in.

- * @member:     the name of the list_head within the struct.

- *

- * Note, that list is expected to be not empty.

- */

-#define list_last_entry(ptr, type, member) \

-        list_entry((ptr)->prev, type, member)

-

-/**

- * list_first_entry_or_null - get the first element from a list

- * @ptr:        the list head to take the element from.

- * @type:       the type of the struct this is embedded in.

- * @member:     the name of the list_head within the struct.

+ * list_replace - replace old entry by new one

+ * @old : the element to be replaced

+ * @new : the new element to insert

  *

- * Note that if the list is empty, it returns NULL.

- */

-#define list_first_entry_or_null(ptr, type, member) \

-        (!list_empty(ptr) ? list_first_entry(ptr, type, member) : NULL)

-

-/**

- * list_next_entry - get the next element in list

- * @pos:        the type * to cursor

- * @member:     the name of the list_head within the struct.

- */

-#define list_next_entry(pos, type, member) \

-        list_entry((pos)->member.next, type, member)

-

-/**

- * list_prev_entry - get the prev element in list

- * @pos:        the type * to cursor

- * @member:     the name of the list_head within the struct.

- */

-#define list_prev_entry(pos, type, member) \

-        list_entry((pos)->member.prev, type, member)

-

-/**

- * list_for_each        -       iterate over a list

- * @pos:        the &struct list_head to use as a loop cursor.

- * @head:       the head for your list.

- */

-#define list_for_each(pos, head) \

-        for (pos = (head)->next; pos != (head); pos = pos->next)

-

-/**

- * list_for_each_prev   -       iterate over a list backwards

- * @pos:        the &struct list_head to use as a loop cursor.

- * @head:       the head for your list.

- */

-#define list_for_each_prev(pos, head) \

-        for (pos = (head)->prev; pos != (head); pos = pos->prev)

-

-/**

- * list_for_each_safe - iterate over a list safe against removal of list entry

- * @pos:        the &struct list_head to use as a loop cursor.

- * @n:          another &struct list_head to use as temporary storage

- * @head:       the head for your list.

+ * If @old was empty, it will be overwritten.

  */

-#define list_for_each_safe(pos, n, head) \

-        for (pos = (head)->next, n = pos->next; pos != (head); \

-                pos = n, n = pos->next)

+static inline void list_replace(struct list_head *old,

+                                struct list_head *_new)

+{

+        _new->next = old->next;

+        _new->next->prev = _new;

+        _new->prev = old->prev;

+        _new->prev->next = _new;

+}

 

-/**

- * list_for_each_prev_safe - iterate over a list backwards safe against removal of list entry

- * @pos:        the &struct list_head to use as a loop cursor.

- * @n:          another &struct list_head to use as temporary storage

- * @head:       the head for your list.

- */

-#define list_for_each_prev_safe(pos, n, head) \

-        for (pos = (head)->prev, n = pos->prev; \

-             pos != (head); \

-             pos = n, n = pos->prev)

+static inline void list_replace_init(struct list_head *old,

+                                        struct list_head *_new)

+{

+        list_replace(old, _new);

+        INIT_LIST_HEAD(old);

+}

 

-/**

- * list_for_each_entry  -       iterate over list of given type

- * @pos:        the type * to use as a loop cursor.

- * @head:       the head for your list.

- * @member:     the name of the list_head within the struct.

- */

-#define list_for_each_entry(pos, type, head, member)                          \

-        for (pos = list_first_entry(head, type, member);        \

-             &pos->member != (head);                                    \

-             pos = list_next_entry(pos, type, member))

 

 /**

  * list_for_each_entry_reverse - iterate backwards over list of given type.

@@ -518,19 +525,6 @@ static inline void list_splice_tail_init(struct list_head *list,
              pos = list_next_entry(pos, type, member))

 

 /**

- * list_for_each_entry_safe - iterate over list of given type safe against removal of list entry

- * @pos:        the type * to use as a loop cursor.

- * @n:          another type * to use as temporary storage

- * @head:       the head for your list.

- * @member:     the name of the list_head within the struct.

- */

-#define list_for_each_entry_safe(pos, type, n, head, member)                  \

-        for (pos = list_first_entry(head, type, member),        \

-                n = list_next_entry(pos, type, member);                       \

-             &pos->member != (head);                                    \

-             pos = n, n = list_next_entry(n, type, member))

-

-/**

  * list_for_each_entry_safe_continue - continue list iteration safe against removal

  * @pos:        the type * to use as a loop cursor.

  * @n:          another type * to use as temporary storage

@@ -591,5 +585,6 @@ static inline void list_splice_tail_init(struct list_head *list,
  */

 #define list_safe_reset_next(pos, type, n, member)                            \

         n = list_next_entry(pos, type, member)

+#endif

 

 #endif /* USBH_LIST_H_ */

diff --git a/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.c b/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.c
index 5954cb2..3944a79 100644
--- a/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.c
+++ b/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.c
@@ -105,6 +105,13 @@ static void _try_commit_np(USBHDriver *host);
 static void otg_rxfifo_flush(USBHDriver *usbp);
 static void otg_txfifo_flush(USBHDriver *usbp, uint32_t fifo);
 
+#if STM32_USBH_USE_OTG1
+USBHDriver USBHD1;
+#endif
+#if STM32_USBH_USE_OTG2
+USBHDriver USBHD2;
+#endif
+
 /*===========================================================================*/
 /* Little helper functions.                                                  */
 /*===========================================================================*/
@@ -120,16 +127,6 @@ static inline void _save_dt_mask(usbh_ep_t *ep, uint32_t hctsiz) {
 	ep->dt_mask = hctsiz & HCTSIZ_DPID_MASK;
 }
 
-#if 1
-#define _transfer_completed _transfer_completedI
-#else
-static inline void _transfer_completed(usbh_ep_t *ep, usbh_urb_t *urb, usbh_urbstatus_t status) {
-	osalSysLockFromISR();
-	_transfer_completedI(ep, urb, status);
-	osalSysUnlockFromISR();
-}
-#endif
-
 /*===========================================================================*/
 /* Functions called from many places.                                        */
 /*===========================================================================*/
@@ -459,7 +456,7 @@ static void _purge_queue(USBHDriver *host, struct list_head *list) {
 			_release_channel(host, hcm);
 			_update_urb(ep, hcm->hc->HCTSIZ, urb, FALSE);
 		}
-		_transfer_completed(ep, urb, USBH_URBSTATUS_DISCONNECTED);
+		_transfer_completedI(ep, urb, USBH_URBSTATUS_DISCONNECTED);
 	}
 }
 
@@ -595,7 +592,6 @@ void usbh_lld_ep_object_init(usbh_ep_t *ep) {
 void usbh_lld_ep_open(usbh_ep_t *ep) {
 	uinfof("\t%s: Open EP", ep->name);
 	ep->status = USBH_EPSTATUS_OPEN;
-	osalOsRescheduleS();
 }
 
 void usbh_lld_ep_close(usbh_ep_t *ep) {
@@ -607,7 +603,6 @@ void usbh_lld_ep_close(usbh_ep_t *ep) {
 	}
 	uinfof("\t%s: Closed", ep->name);
 	ep->status = USBH_EPSTATUS_CLOSED;
-	osalOsRescheduleS();
 }
 
 bool usbh_lld_ep_reset(usbh_ep_t *ep) {
@@ -617,6 +612,13 @@ bool usbh_lld_ep_reset(usbh_ep_t *ep) {
 
 void usbh_lld_urb_submit(usbh_urb_t *urb) {
 	usbh_ep_t *const ep = urb->ep;
+	USBHDriver *const host = ep->device->host;
+
+	if (!(host->otg->HPRT & HPRT_PENA)) {
+		uwarnf("\t%s: Can't submit URB, port disabled", ep->name);
+		_usbh_urb_completeI(urb, USBH_URBSTATUS_DISCONNECTED);
+		return;
+	}
 
 	/* add the URB to the EP's queue */
 	list_add_tail(&urb->node, &ep->urb_list);
@@ -628,7 +630,7 @@ void usbh_lld_urb_submit(usbh_urb_t *urb) {
 		_move_to_pending_queue(ep);
 
 		if (usbhEPIsPeriodic(ep)) {
-			ep->device->host->otg->GINTMSK |= GINTMSK_SOFM;
+			host->otg->GINTMSK |= GINTMSK_SOFM;
 		} else {
 			/* try to queue non-periodic transfers */
 			_try_commit_np(ep->device->host);
@@ -636,6 +638,7 @@ void usbh_lld_urb_submit(usbh_urb_t *urb) {
 	}
 }
 
+/* usbh_lld_urb_abort may require a reschedule if called from a S-locked state */
 bool usbh_lld_urb_abort(usbh_urb_t *urb, usbh_urbstatus_t status) {
 	osalDbgCheck(usbhURBIsBusy(urb));
 
@@ -660,8 +663,8 @@ bool usbh_lld_urb_abort(usbh_urb_t *urb, usbh_urbstatus_t status) {
 		return FALSE;
 	}
 
-	/* This URB is active, we can cancel it now */
-	uinfof("\t%s: usbh_lld_urb_abort: URB is not active", hcm->ep->name);
+	/* This URB is inactive, we can cancel it now */
+	uinfof("\t%s: usbh_lld_urb_abort: URB is not active", ep->name);
 	_transfer_completedI(ep, urb, status);
 
 	return TRUE;
@@ -760,7 +763,7 @@ static void _complete_bulk_int(USBHDriver *host, stm32_hc_management_t *hcm, usb
 	_save_dt_mask(ep, hctsiz);
 	if (_update_urb(ep, hctsiz, urb, TRUE)) {
 		udbgf("\t%s: done", ep->name);
-		_transfer_completed(ep, urb, USBH_URBSTATUS_OK);
+		_transfer_completedI(ep, urb, USBH_URBSTATUS_OK);
 	} else {
 		osalDbgCheck(urb->requestedLength > 0x7FFFF);
 		uwarnf("\t%s: incomplete", ep->name);
@@ -791,7 +794,7 @@ static void _complete_control(USBHDriver *host, stm32_hc_management_t *hcm, usbh
 	} else {
 		osalDbgCheck(ep->xfer.u.ctrl_phase == USBH_LLD_CTRLPHASE_STATUS);
 		udbgf("\t%s: STATUS done", ep->name);
-		_transfer_completed(ep, urb, USBH_URBSTATUS_OK);
+		_transfer_completedI(ep, urb, USBH_URBSTATUS_OK);
 	}
 	_try_commit_np(host);
 }
@@ -817,7 +820,7 @@ static void _complete_iso(USBHDriver *host, stm32_hc_management_t *hcm, usbh_ep_
 	udbgf("\t%s: done", hcm->ep->name);
 	_release_channel(host, hcm);
 	_update_urb(ep, hctsiz, urb, TRUE);
-	_transfer_completed(ep, urb, USBH_URBSTATUS_OK);
+	_transfer_completedI(ep, urb, USBH_URBSTATUS_OK);
 	_try_commit_p(host, FALSE);
 }
 
@@ -902,7 +905,7 @@ static inline void _chh_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_
 		switch (reason) {
 		case USBH_LLD_HALTREASON_NAK:
 			if ((ep->type == USBH_EPTYPE_INT) && ep->in) {
-				_transfer_completed(ep, urb, USBH_URBSTATUS_TIMEOUT);
+				_transfer_completedI(ep, urb, USBH_URBSTATUS_TIMEOUT);
 			} else {
 				ep->xfer.error_count = 0;
 				_move_to_pending_queue(ep);
@@ -917,12 +920,12 @@ static inline void _chh_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_
 			} else {
 				ep->status = USBH_EPSTATUS_HALTED;
 			}
-			_transfer_completed(ep, urb, USBH_URBSTATUS_STALL);
+			_transfer_completedI(ep, urb, USBH_URBSTATUS_STALL);
 			break;
 
 		case USBH_LLD_HALTREASON_ERROR:
 			if ((ep->type == USBH_EPTYPE_ISO) || done || (ep->xfer.error_count >= 3)) {
-				_transfer_completed(ep, urb, USBH_URBSTATUS_ERROR);
+				_transfer_completedI(ep, urb, USBH_URBSTATUS_ERROR);
 			} else {
 				uerrf("\t%s: err=%d, done=%d, retry", ep->name, ep->xfer.error_count, done);
 				_move_to_pending_queue(ep);
@@ -931,7 +934,7 @@ static inline void _chh_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_
 
 		case USBH_LLD_HALTREASON_ABORT:
 			uwarnf("\t%s: Abort", ep->name);
-			_transfer_completed(ep, urb, urb->status);
+			_transfer_completedI(ep, urb, urb->status);
 			break;
 
 		default:
@@ -1018,6 +1021,36 @@ static inline void _hcint_int(USBHDriver *host) {
 /* Host interrupts.                                                          */
 /*===========================================================================*/
 static inline void _sof_int(USBHDriver *host) {
+
+	/* this is part of the workaround to the LS bug in the OTG core */
+#undef HPRT_PLSTS_MASK
+#define HPRT_PLSTS_MASK (3U<<10)
+	if (host->check_ls_activity) {
+		uint16_t remaining = host->otg->HFNUM >> 16;
+		if (remaining < 5975) {
+			uwarnf("LS: ISR called too late (time=%d)", 6000 - remaining);
+			return;
+		}
+		/* 15us loop */
+		for (;;) {
+			uint32_t line_status = host->otg->HPRT & HPRT_PLSTS_MASK;
+			remaining = host->otg->HFNUM >> 16;
+			if (line_status != HPRT_PLSTS_DM) {
+				uinfof("LS: activity detected, line=%d, time=%d", line_status >> 10,  6000 - remaining);
+				host->check_ls_activity = FALSE;
+				host->otg->GINTMSK = (host->otg->GINTMSK & ~GINTMSK_SOFM) | (GINTMSK_HCM | GINTMSK_RXFLVLM);
+				host->rootport.lld_status |= USBH_PORTSTATUS_ENABLE;
+				host->rootport.lld_c_status |= USBH_PORTSTATUS_C_ENABLE;
+				return;
+			}
+			if (remaining < 5910) {
+				uwarn("LS: No activity detected");
+				return;
+			}
+		}
+	}
+
+	/* real SOF interrupt */
 	udbg("SOF");
 	_try_commit_p(host, TRUE);
 }
@@ -1137,17 +1170,19 @@ static inline void _ptxfe_int(USBHDriver *host) {
 	uinfo("PTXFE");
 }
 
-static inline void _discint_int(USBHDriver *host) {
-	uint32_t hprt = host->otg->HPRT;
-
-	uwarn("\tDISCINT");
+static void _disable(USBHDriver *host) {
+	host->rootport.lld_status &= ~(USBH_PORTSTATUS_CONNECTION | USBH_PORTSTATUS_ENABLE);
+	host->rootport.lld_c_status |= USBH_PORTSTATUS_C_CONNECTION | USBH_PORTSTATUS_C_ENABLE;
 
-	if (!(hprt & HPRT_PCSTS)) {
-		host->rootport.lld_status &= ~(USBH_PORTSTATUS_CONNECTION | USBH_PORTSTATUS_ENABLE);
-		host->rootport.lld_c_status |= USBH_PORTSTATUS_C_CONNECTION | USBH_PORTSTATUS_C_ENABLE;
-	}
 	_purge_active(host);
 	_purge_pending(host);
+
+	host->otg->GINTMSK &= ~(GINTMSK_HCM | GINTMSK_RXFLVLM);
+}
+
+static inline void _discint_int(USBHDriver *host) {
+	uinfo("DISCINT: Port disconnection detected");
+	_disable(host);
 }
 
 static inline void _hprtint_int(USBHDriver *host) {
@@ -1163,8 +1198,6 @@ static inline void _hprtint_int(USBHDriver *host) {
 			uinfo("\tHPRT: Port connection detected");
 			host->rootport.lld_status |= USBH_PORTSTATUS_CONNECTION;
 			host->rootport.lld_c_status |= USBH_PORTSTATUS_C_CONNECTION;
-		} else {
-			uinfo("\tHPRT: Port disconnection detected");
 		}
 	}
 
@@ -1172,9 +1205,32 @@ static inline void _hprtint_int(USBHDriver *host) {
 		hprt_clr |= HPRT_PENCHNG;
 		if (hprt & HPRT_PENA) {
 			uinfo("\tHPRT: Port enabled");
-			host->rootport.lld_status |= USBH_PORTSTATUS_ENABLE;
 			host->rootport.lld_status &= ~(USBH_PORTSTATUS_HIGH_SPEED | USBH_PORTSTATUS_LOW_SPEED);
 
+			/* configure FIFOs */
+#define HNPTXFSIZ						DIEPTXF0
+#if STM32_USBH_USE_OTG1
+#if STM32_USBH_USE_OTG2
+			if (&USBHD1 == host)
+#endif
+			{
+				otg->GRXFSIZ = GRXFSIZ_RXFD(STM32_OTG1_RXFIFO_SIZE / 4);
+				otg->HNPTXFSIZ = HPTXFSIZ_PTXSA(STM32_OTG1_RXFIFO_SIZE / 4) | HPTXFSIZ_PTXFD(STM32_OTG1_NPTXFIFO_SIZE / 4);
+				otg->HPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG1_RXFIFO_SIZE / 4) + (STM32_OTG1_NPTXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG1_PTXFIFO_SIZE / 4);
+			}
+#endif
+#if STM32_USBH_USE_OTG2
+#if STM32_USBH_USE_OTG1
+			if (&USBHD2 == host)
+#endif
+			{
+				otg->GRXFSIZ = GRXFSIZ_RXFD(STM32_OTG2_RXFIFO_SIZE / 4);
+				otg->HNPTXFSIZ = HPTXFSIZ_PTXSA(STM32_OTG2_RXFIFO_SIZE / 4) | HPTXFSIZ_PTXFD(STM32_OTG2_NPTXFIFO_SIZE / 4);
+				otg->HPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG2_RXFIFO_SIZE / 4) + (STM32_OTG2_NPTXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG2_PTXFIFO_SIZE / 4);
+			}
+#endif
+#undef HNPTXFSIZ
+
 			/* Make sure the FIFOs are flushed. */
 		    otg_txfifo_flush(host, 0x10);
 		    otg_rxfifo_flush(host);
@@ -1191,9 +1247,23 @@ static inline void _hprtint_int(USBHDriver *host) {
 				host->rootport.lld_status |= USBH_PORTSTATUS_LOW_SPEED;
 				otg->HFIR = 6000;
 				otg->HCFG = (otg->HCFG & ~HCFG_FSLSPCS_MASK) | HCFG_FSLSPCS_6;
+
+				/* Low speed devices connected to the STM32's internal transceiver sometimes
+				 * don't behave correctly. Although HPRT reports a port enable, really
+				 * no traffic is generated, and the core is non-functional. To avoid
+				 * this we won't report the port enable until we are sure that the
+				 * port is working. */
+				host->check_ls_activity = TRUE;
+				otg->GINTMSK |= GINTMSK_SOFM;
 			} else {
 				otg->HFIR = 48000;
 				otg->HCFG = (otg->HCFG & ~HCFG_FSLSPCS_MASK) | HCFG_FSLSPCS_48;
+				host->check_ls_activity = FALSE;
+
+				/* enable channel and rx interrupts */
+				otg->GINTMSK |= GINTMSK_HCM | GINTMSK_RXFLVLM;
+				host->rootport.lld_status |= USBH_PORTSTATUS_ENABLE;
+				host->rootport.lld_c_status |= USBH_PORTSTATUS_C_ENABLE;
 			}
 		} else {
 			if (hprt & HPRT_PCSTS) {
@@ -1205,13 +1275,8 @@ static inline void _hprtint_int(USBHDriver *host) {
 			} else {
 				uerr("\tHPRT: Port disabled due to disconnect");
 			}
-
-			_purge_active(host);
-			_purge_pending(host);
-
-			host->rootport.lld_status &= ~USBH_PORTSTATUS_ENABLE;
+			_disable(host);
 		}
-		host->rootport.lld_c_status |= USBH_PORTSTATUS_C_ENABLE;
 	}
 
 	if (hprt & HPRT_POCCHNG) {
@@ -1243,24 +1308,19 @@ static void usb_lld_serve_interrupt(USBHDriver *host) {
 	}
 
 	/* check mismatch */
-	if (gintsts & GINTSTS_MMIS) {
-		uerr("Mode Mismatch");
-		otg->GINTSTS = gintsts;
-		return;
-	}
+	osalDbgAssert((gintsts & GINTSTS_MMIS) == 0, "mode mismatch");
 
 	gintsts &= otg->GINTMSK;
-#if USBH_DEBUG_ENABLE_WARNINGS
 	if (!gintsts) {
+#if USBH_DEBUG_ENABLE_WARNINGS
 		uint32_t a, b;
 		a = otg->GINTSTS;
 		b = otg->GINTMSK;
-		uwarnf("GINTSTS=%08x, GINTMSK=%08x", a, b);
+		uwarnf("Masked bits caused an ISR: GINTSTS=%08x, GINTMSK=%08x (unhandled bits=%08x)", a, b, a & ~b);
+#endif
 		return;
 	}
-#endif
 
-	//	otg->GINTMSK &= ~(GINTMSK_NPTXFEM | GINTMSK_PTXFEM);
 	otg->GINTSTS = gintsts;
 
 	if (gintsts & GINTSTS_SOF)
@@ -1359,25 +1419,23 @@ static void _init(USBHDriver *host) {
 
 #if STM32_USBH_USE_OTG1
 #if STM32_USBH_USE_OTG2
-	if (&USBHD1 == host) {
+	if (&USBHD1 == host)
 #endif
+	{
 		host->otg = OTG_FS;
 		host->channels_number = STM32_OTG1_CHANNELS_NUMBER;
-#if STM32_USBH_USE_OTG2
 	}
 #endif
-#endif
 
 #if STM32_USBH_USE_OTG2
 #if STM32_USBH_USE_OTG1
-	if (&USBHD2 == host) {
+	if (&USBHD2 == host)
 #endif
+	{
 		host->otg = OTG_HS;
 		host->channels_number = STM32_OTG2_CHANNELS_NUMBER;
-#if STM32_USBH_USE_OTG1
 	}
 #endif
-#endif
 	INIT_LIST_HEAD(&host->ch_free[0]);
 	INIT_LIST_HEAD(&host->ch_free[1]);
 	for (i = 0; i < host->channels_number; i++) {
@@ -1411,8 +1469,9 @@ static void _usbh_start(USBHDriver *usbh) {
 	/* Clock activation.*/
 #if STM32_USBH_USE_OTG1
 #if STM32_USBH_USE_OTG2
-	if (&USBHD1 == usbh) {
+	if (&USBHD1 == usbh)
 #endif
+	{
 		/* OTG FS clock enable and reset.*/
 		rccEnableOTG_FS(FALSE);
 		rccResetOTG_FS();
@@ -1421,15 +1480,14 @@ static void _usbh_start(USBHDriver *usbh) {
 
 		/* Enables IRQ vector.*/
 		nvicEnableVector(STM32_OTG1_NUMBER, STM32_USB_OTG1_IRQ_PRIORITY);
-#if STM32_USBH_USE_OTG2
 	}
 #endif
-#endif
 
 #if STM32_USBH_USE_OTG2
 #if STM32_USBH_USE_OTG1
-	if (&USBHD2 == usbh) {
+	if (&USBHD2 == usbh)
 #endif
+	{
 		/* OTG HS clock enable and reset.*/
 		rccEnableOTG_HS(TRUE); // Enable HS clock when cpu is in sleep mode
 		rccDisableOTG_HSULPI(TRUE); // Disable HS ULPI clock when cpu is in sleep mode
@@ -1439,10 +1497,8 @@ static void _usbh_start(USBHDriver *usbh) {
 
 		/* Enables IRQ vector.*/
 		nvicEnableVector(STM32_OTG2_NUMBER, STM32_USB_OTG2_IRQ_PRIORITY);
-#if STM32_USBH_USE_OTG1
 	}
 #endif
-#endif
 
 	otgp->GUSBCFG = GUSBCFG_PHYSEL | GUSBCFG_TRDT(5);
 
@@ -1481,41 +1537,11 @@ static void _usbh_start(USBHDriver *usbh) {
 
 	otgp->HPRT |= HPRT_PPWR;
 
-	/* without this delay, the FIFO sizes are set INcorrectly */
-	osalThreadSleepS(MS2ST(200));
-
-#define HNPTXFSIZ						DIEPTXF0
-#if STM32_USBH_USE_OTG1
-#if STM32_USBH_USE_OTG2
-	if (&USBHD1 == usbh) {
-#endif
-		otgp->GRXFSIZ = GRXFSIZ_RXFD(STM32_OTG1_RXFIFO_SIZE / 4);
-		otgp->HNPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG1_RXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG1_NPTXFIFO_SIZE / 4);
-		otgp->HPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG1_RXFIFO_SIZE / 4) + (STM32_OTG1_NPTXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG1_PTXFIFO_SIZE / 4);
-#if STM32_USBH_USE_OTG2
-	}
-#endif
-#endif
-#if STM32_USBH_USE_OTG2
-#if STM32_USBH_USE_OTG1
-	if (&USBHD2 == usbh) {
-#endif
-		otgp->GRXFSIZ = GRXFSIZ_RXFD(STM32_OTG2_RXFIFO_SIZE / 4);
-		otgp->HNPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG2_RXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG2_NPTXFIFO_SIZE / 4);
-		otgp->HPTXFSIZ = HPTXFSIZ_PTXSA((STM32_OTG2_RXFIFO_SIZE / 4) + (STM32_OTG2_NPTXFIFO_SIZE / 4)) | HPTXFSIZ_PTXFD(STM32_OTG2_PTXFIFO_SIZE / 4);
-#if STM32_USBH_USE_OTG1
-	}
-#endif
-#endif
-#undef HNPTXFSIZ
-
 	otg_txfifo_flush(usbh, 0x10);
 	otg_rxfifo_flush(usbh);
 
 	otgp->GINTSTS = 0xffffffff;
-	otgp->GINTMSK = GINTMSK_DISCM /*| GINTMSK_PTXFEM*/ | GINTMSK_HCM | GINTMSK_HPRTM
-					/*| GINTMSK_IPXFRM | GINTMSK_NPTXFEM*/ | GINTMSK_RXFLVLM
-					/*| GINTMSK_SOFM */ | GINTMSK_MMISM;
+	otgp->GINTMSK = GINTMSK_DISCM | GINTMSK_HPRTM | GINTMSK_MMISM;
 
 	usbh->rootport.lld_status = USBH_PORTSTATUS_POWER;
 	usbh->rootport.lld_c_status = 0;
@@ -1580,30 +1606,18 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 			break;
 
 		case USBH_PORT_FEAT_C_OVERCURRENT:
-			usbh->rootport.lld_c_status &= USBH_PORTSTATUS_C_OVERCURRENT;
+			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_OVERCURRENT;
 			break;
 
 		default:
 			osalDbgAssert(0, "invalid wvalue");
 			break;
 		}
-		osalOsRescheduleS();
 		osalSysUnlock();
 		break;
 
 	case GetHubDescriptor:
-		/*dev_dbg(hsotg->dev, "GetHubDescriptor\n");
-		hub_desc = (struct usb_hub_descriptor *)buf;
-		hub_desc->bDescLength = 9;
-		hub_desc->bDescriptorType = USB_DT_HUB;
-		hub_desc->bNbrPorts = 1;
-		hub_desc->wHubCharacteristics =
-			cpu_to_le16(HUB_CHAR_COMMON_LPSM |
-					HUB_CHAR_INDV_PORT_OCPM);
-		hub_desc->bPwrOn2PwrGood = 1;
-		hub_desc->bHubContrCurrent = 0;
-		hub_desc->u.hs.DeviceRemovable[0] = 0;
-		hub_desc->u.hs.DeviceRemovable[1] = 0xff;*/
+		osalDbgAssert(0, "unsupported");
 		break;
 
 	case GetHubStatus:
@@ -1616,7 +1630,6 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 		osalDbgCheck(wlength >= 4);
 		osalSysLock();
 		*(uint32_t *)buf = usbh->rootport.lld_status | (usbh->rootport.lld_c_status << 16);
-		osalOsRescheduleS();
 		osalSysUnlock();
 		break;
 
@@ -1640,13 +1653,30 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 			uint32_t hprt;
 			otg->PCGCCTL = 0;
 			hprt = otg->HPRT;
+			if (hprt & HPRT_PENA) {
+				/* This can occur when the OTG core doesn't generate traffic
+				 * despite reporting a successful por enable  */
+				uerr("Detected enabled port; resetting OTG core");
+				otg->GAHBCFG = 0;
+				osalThreadSleepS(MS2ST(20));
+				_usbh_start(usbh);				/* this effectively resets the core */
+				osalThreadSleepS(MS2ST(100));	/* during this delay, the core generates connect ISR */
+				uinfo("OTG reset ended");
+				if (otg->HPRT & HPRT_PCSTS) {
+					/* if the device is still connected, don't report a C_CONNECTION flag, which would cause
+					 * the upper layer to abort enumeration */
+					uinfo("Clear connection change flag");
+					usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
+				}
+			}
 			/* note: writing PENA = 1 actually disables the port */
-			hprt &= ~(HPRT_PSUSP | HPRT_PENA | HPRT_PCDET | HPRT_PENCHNG | HPRT_POCCHNG );
+			hprt &= ~(HPRT_PSUSP | HPRT_PENA | HPRT_PCDET | HPRT_PENCHNG | HPRT_POCCHNG);
+			while ((otg->GRSTCTL & GRSTCTL_AHBIDL) == 0);
 			otg->HPRT = hprt | HPRT_PRST;
-			osalThreadSleepS(MS2ST(60));
+			osalThreadSleepS(MS2ST(15));
 			otg->HPRT = hprt;
+			osalThreadSleepS(MS2ST(10));
 			usbh->rootport.lld_c_status |= USBH_PORTSTATUS_C_RESET;
-			osalOsRescheduleS();
 			osalSysUnlock();
 		} 	break;
 
@@ -1669,16 +1699,7 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 }
 
 uint8_t usbh_lld_roothub_get_statuschange_bitmap(USBHDriver *usbh) {
-	osalSysLock();
-	if (usbh->rootport.lld_c_status) {
-		osalOsRescheduleS();
-		osalSysUnlock();
-		return 1 << 1;
-	}
-	osalOsRescheduleS();
-	osalSysUnlock();
-	return 0;
+	return usbh->rootport.lld_c_status ? (1 << 1) : 0;
 }
 
-
 #endif
diff --git a/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.h b/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.h
index a2b7628..fd7f4e0 100644
--- a/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.h
+++ b/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.h
@@ -63,6 +63,8 @@ typedef struct stm32_hc_management {
 
 #define _usbhdriver_ll_data											\
 	stm32_otg_t *otg;												\
+	/* low-speed port reset bug */									\
+	bool check_ls_activity;											\
 	/* channels */													\
 	uint8_t channels_number;										\
 	stm32_hc_management_t channels[STM32_OTG2_CHANNELS_NUMBER];		\
diff --git a/os/hal/src/hal_usbh.c b/os/hal/src/hal_usbh.c
index d242086..a7ba236 100644
--- a/os/hal/src/hal_usbh.c
+++ b/os/hal/src/hal_usbh.c
@@ -20,15 +20,8 @@
 #if HAL_USE_USBH
 
 #include "usbh/internal.h"
-#include <string.h>
-
-//devices
 #include "usbh/dev/hub.h"
-#include "usbh/dev/aoa.h"
-#include "usbh/dev/ftdi.h"
-#include "usbh/dev/msd.h"
-#include "usbh/dev/hid.h"
-#include "usbh/dev/uvc.h"
+#include <string.h>
 
 #if USBH_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
@@ -62,18 +55,15 @@
 #define uerr(f, ...)   do {} while(0)
 #endif
 
-#if STM32_USBH_USE_OTG1
-USBHDriver USBHD1;
-#endif
-#if STM32_USBH_USE_OTG2
-USBHDriver USBHD2;
-#endif
-
-
 static void _classdriver_process_device(usbh_device_t *dev);
-static bool _classdriver_load(usbh_device_t *dev, uint8_t class,
-		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem);
+static bool _classdriver_load(usbh_device_t *dev, uint8_t *descbuff, uint16_t rem);
 
+#if HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
+#include "usbh_additional_class_drivers.h"
+#ifndef HAL_USBH_ADDITIONAL_CLASS_DRIVERS
+#error "Must define HAL_USBH_ADDITIONAL_CLASS_DRIVERS"
+#endif
+#endif
 
 /*===========================================================================*/
 /* Checks.                                                                   */
@@ -113,28 +103,6 @@ void usbhObjectInit(USBHDriver *usbh) {
 #endif
 }
 
-void usbhInit(void) {
-#if HAL_USBH_USE_FTDI
-	usbhftdiInit();
-#endif
-#if HAL_USBH_USE_AOA
-	usbhaoaInit();
-#endif
-#if HAL_USBH_USE_MSD
-	usbhmsdInit();
-#endif
-#if HAL_USBH_USE_HID
-	usbhhidInit();
-#endif
-#if HAL_USBH_USE_UVC
-	usbhuvcInit();
-#endif
-#if HAL_USBH_USE_HUB
-	usbhhubInit();
-#endif
-	usbh_lld_init();
-}
-
 void usbhStart(USBHDriver *usbh) {
 	usbDbgInit(usbh);
 
@@ -143,7 +111,6 @@ void usbhStart(USBHDriver *usbh) {
 				"invalid state");
 	usbh_lld_start(usbh);
 	usbh->status = USBH_STATUS_STARTED;
-	osalOsRescheduleS();
 	osalSysUnlock();
 }
 
@@ -256,6 +223,7 @@ void usbhURBObjectResetI(usbh_urb_t *urb) {
 	usbh_lld_urb_object_reset(urb);
 }
 
+/* usbhURBSubmitI may require a reschedule if called from a S-locked state */
 void usbhURBSubmitI(usbh_urb_t *urb) {
 	osalDbgCheckClassI();
 	_check_urb(urb);
@@ -277,27 +245,21 @@ void usbhURBSubmitI(usbh_urb_t *urb) {
 	usbh_lld_urb_submit(urb);
 }
 
+/* _usbh_urb_abortI may require a reschedule if called from a S-locked state */
 bool _usbh_urb_abortI(usbh_urb_t *urb, usbh_urbstatus_t status) {
 	osalDbgCheckClassI();
 	_check_urb(urb);
+	osalDbgCheck(urb->status != USBH_URBSTATUS_UNINITIALIZED);
 
-	switch (urb->status) {
-/*	case USBH_URBSTATUS_UNINITIALIZED:
- * 	case USBH_URBSTATUS_INITIALIZED:
- *	case USBH_URBSTATUS_ERROR:
- *	case USBH_URBSTATUS_TIMEOUT:
- *	case USBH_URBSTATUS_CANCELLED:
- *	case USBH_URBSTATUS_STALL:
- *	case USBH_URBSTATUS_DISCONNECTED:
- *	case USBH_URBSTATUS_OK: */
-	default:
-		/* already finished */
-		_usbh_urb_completeI(urb, status);
-		return TRUE;
-
-	case USBH_URBSTATUS_PENDING:
+	if (urb->status == USBH_URBSTATUS_PENDING) {
 		return usbh_lld_urb_abort(urb, status);
 	}
+
+	/* already finished or never submitted:
+	 * USBH_URBSTATUS_INITIALIZED, USBH_URBSTATUS_ERROR, USBH_URBSTATUS_TIMEOUT,
+	 * USBH_URBSTATUS_CANCELLED, USBH_URBSTATUS_STALL, USBH_URBSTATUS_DISCONNECTED
+	 * USBH_URBSTATUS_OK */
+	return TRUE;
 }
 
 void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {
@@ -312,15 +274,18 @@ void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {
 	}
 #if !(USBH_DEBUG_ENABLE && USBH_DEBUG_ENABLE_WARNINGS)
 	else {
+		/* This call is necessary because _usbh_urb_abortI may require a reschedule */
 		osalOsRescheduleS();
 	}
 #else
 	uwarn("URB aborted");
 	osalOsRescheduleS();	/* debug printing functions call I-class functions inside
-	 	 	 	 	 	 	 which may cause a priority violation without this call */
+	 	 	 	 	 	 	 which may cause a priority violation without this call
+							Also, _usbh_urb_abortI may require a reschedule */
 #endif
 }
 
+/* usbhURBCancelI may require a reschedule if called from a S-locked state */
 bool usbhURBCancelI(usbh_urb_t *urb) {
 	return _usbh_urb_abortI(urb, USBH_URBSTATUS_CANCELLED);
 }
@@ -345,7 +310,6 @@ msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout) {
 
 	case USBH_URBSTATUS_OK:
 		ret = MSG_OK;
-		osalOsRescheduleS();
 		break;
 
 /*	case USBH_URBSTATUS_UNINITIALIZED:
@@ -356,7 +320,6 @@ msg_t usbhURBWaitTimeoutS(usbh_urb_t *urb, systime_t timeout) {
  *	case USBH_URBSTATUS_DISCONNECTED: */
 	default:
 		ret = MSG_RESET;
-		osalOsRescheduleS();
 		break;
 	}
 	return ret;
@@ -382,6 +345,7 @@ static inline msg_t _wakeup_message(usbh_urbstatus_t status) {
 	return MSG_RESET;
 }
 
+/* _usbh_urb_completeI may require a reschedule if called from a S-locked state */
 void _usbh_urb_completeI(usbh_urb_t *urb, usbh_urbstatus_t status) {
 	osalDbgCheckClassI();
 	_check_urb(urb);
@@ -459,7 +423,7 @@ usbh_urbstatus_t usbhControlRequest(usbh_device_t *dev,
 			wIndex,
 			wLength
 	};
-	return usbhControlRequestExtended(dev, &req, buff, NULL, MS2ST(1000));
+	return usbhControlRequestExtended(dev, &req, buff, NULL, HAL_USBH_CONTROL_REQUEST_DEFAULT_TIMEOUT);
 }
 
 /*===========================================================================*/
@@ -925,46 +889,48 @@ static void _port_process_status_change(usbh_port_t *port) {
 	_port_update_status(port);
 
 	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {
-		/* port connected status changed */
 		port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);
-		if ((port->status & (USBH_PORTSTATUS_CONNECTION | USBH_PORTSTATUS_ENABLE))
-				== USBH_PORTSTATUS_CONNECTION) {
-			if (port->device.status != USBH_DEVSTATUS_DISCONNECTED) {
+
+		if (port->device.status != USBH_DEVSTATUS_DISCONNECTED) {
+			if (!(port->status & USBH_PORTSTATUS_CONNECTION)) {
 				_usbh_port_disconnected(port);
 			}
+		}
+	}
 
-			/* connected, disabled */
+	if (port->device.status == USBH_DEVSTATUS_DISCONNECTED) {
+		if (port->status & USBH_PORTSTATUS_CONNECTION) {
 			_port_connected(port);
-		} else {
-			/* disconnected */
-			_usbh_port_disconnected(port);
 		}
 	}
 
 	if (port->c_status & USBH_PORTSTATUS_C_RESET) {
 		port->c_status &= ~USBH_PORTSTATUS_C_RESET;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);
+		uinfof("Port %d: reset=%d", port->number, port->status & USBH_PORTSTATUS_RESET ? 1 : 0);
 	}
 
 	if (port->c_status & USBH_PORTSTATUS_C_ENABLE) {
 		port->c_status &= ~USBH_PORTSTATUS_C_ENABLE;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_ENABLE);
+		uinfof("Port %d: enable=%d", port->number, port->status & USBH_PORTSTATUS_ENABLE ? 1 : 0);
 	}
 
 	if (port->c_status & USBH_PORTSTATUS_C_OVERCURRENT) {
 		port->c_status &= ~USBH_PORTSTATUS_C_OVERCURRENT;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_OVERCURRENT);
+		uwarnf("Port %d: overcurrent=%d", port->number, port->status & USBH_PORTSTATUS_OVERCURRENT ? 1 : 0);
 	}
 
 	if (port->c_status & USBH_PORTSTATUS_C_SUSPEND) {
 		port->c_status &= ~USBH_PORTSTATUS_C_SUSPEND;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_SUSPEND);
+		uinfof("Port %d: suspend=%d", port->number, port->status & USBH_PORTSTATUS_SUSPEND ? 1 : 0);
 	}
 
 }
 
-
 static void _port_connected(usbh_port_t *port) {
 	/* connected */
 
@@ -974,9 +940,8 @@ static void _port_connected(usbh_port_t *port) {
 	usbh_devspeed_t speed;
 	USBH_DEFINE_BUFFER(usbh_string_descriptor_t strdesc);
 
-	uinfof("Port %d connected, wait debounce...", port->number);
-
 	port->device.status = USBH_DEVSTATUS_ATTACHED;
+	uinfof("Port %d: attached, wait debounce...", port->number);
 
 	/* wait for attach de-bounce */
 	osalThreadSleepMilliseconds(HAL_USBH_PORT_DEBOUNCE_TIME);
@@ -984,16 +949,26 @@ static void _port_connected(usbh_port_t *port) {
 	/* check disconnection */
 	_port_update_status(port);
 	if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {
-		/* connection state changed; abort */
+		port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
+		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);
+		uwarnf("Port %d: connection state changed; abort #1", port->number);
 		goto abort;
 	}
 
+	/* make sure that the device is still connected */
+	if ((port->status & USBH_PORTSTATUS_CONNECTION) == 0) {
+		uwarnf("Port %d: device is disconnected", port->number);
+		goto abort;
+	}
+
+	uinfof("Port %d: connected", port->number);
 	port->device.status = USBH_DEVSTATUS_CONNECTED;
 	retries = 3;
 
 reset:
 	for (i = 0; i < 3; i++) {
-		uinfo("Try reset...");
+		uinfof("Port %d: Try reset...", port->number);
+		/* TODO: check that port is actually disabled */
 		port->c_status &= ~(USBH_PORTSTATUS_C_RESET | USBH_PORTSTATUS_C_ENABLE);
 		_port_reset(port);
 		osalThreadSleepMilliseconds(20);	/* give it some time to reset (min. 10ms) */
@@ -1002,8 +977,12 @@ reset:
 			_port_update_status(port);
 
 			/* check for disconnection */
-			if (port->c_status & USBH_PORTSTATUS_C_CONNECTION)
+			if (port->c_status & USBH_PORTSTATUS_C_CONNECTION) {
+				port->c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
+				usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_CONNECTION);
+				uwarnf("Port %d: connection state changed; abort #2", port->number);
 				goto abort;
+			}
 
 			/* check for reset completion */
 			if (port->c_status & USBH_PORTSTATUS_C_RESET) {
@@ -1017,7 +996,10 @@ reset:
 			}
 
 			/* check for timeout */
-			if (osalOsGetSystemTimeX() - start > HAL_USBH_PORT_RESET_TIMEOUT) break;
+			if (osalOsGetSystemTimeX() - start > HAL_USBH_PORT_RESET_TIMEOUT) {
+				uwarnf("Port %d: reset timeout", port->number);
+				break;
+			}
 		}
 	}
 
@@ -1025,8 +1007,7 @@ reset:
 	goto abort;
 
 reset_success:
-
-	uinfo("Reset OK, recovery...");
+	uinfof("Port %d: Reset OK, recovery...", port->number);
 
 	/* reset recovery */
 	osalThreadSleepMilliseconds(100);
@@ -1043,19 +1024,22 @@ reset_success:
 	usbhEPOpen(&port->device.ctrl);
 
 	/* device with default address (0), try enumeration */
-	if (_device_enumerate(&port->device)) {
+	if (_device_enumerate(&port->device) != HAL_SUCCESS) {
 		/* enumeration failed */
 		usbhEPClose(&port->device.ctrl);
 
-		if (!--retries)
+		if (!--retries) {
+			uwarnf("Port %d: enumeration failed; abort", port->number);
 			goto abort;
+		}
 
 		/* retry reset & enumeration */
+		uwarnf("Port %d: enumeration failed; retry reset & enumeration", port->number);
 		goto reset;
 	}
 
 	/* load the default language ID */
-	uinfo("Loading langID0...");
+	uinfof("Port %d: Loading langID0...", port->number);
 	if (!usbhStdReqGetStringDescriptor(&port->device, 0, 0,
 			USBH_DT_STRING_SIZE, (uint8_t *)&strdesc)
 		&& (strdesc.bLength >= 4)
@@ -1063,12 +1047,12 @@ reset_success:
 			4, (uint8_t *)&strdesc)) {
 
 		port->device.langID0 = strdesc.wData[0];
-		uinfof("langID0=%04x", port->device.langID0);
+		uinfof("Port %d: langID0=%04x", port->number, port->device.langID0);
 	}
 
 	/* check if the device has only one configuration */
 	if (port->device.devDesc.bNumConfigurations == 1) {
-		uinfo("Device has only one configuration");
+		uinfof("Port %d: device has only one configuration", port->number);
 		_device_configure(&port->device, 0);
 	}
 
@@ -1076,7 +1060,7 @@ reset_success:
 	return;
 
 abort:
-	uerr("Abort");
+	uerrf("Port %d: abort", port->number);
 	port->device.status = USBH_DEVSTATUS_DISCONNECTED;
 }
 
@@ -1084,14 +1068,14 @@ void _usbh_port_disconnected(usbh_port_t *port) {
 	if (port->device.status == USBH_DEVSTATUS_DISCONNECTED)
 		return;
 
-	uinfo("Port disconnected");
+	uinfof("Port %d: disconnected", port->number);
 
 	/* unload drivers */
 	while (port->device.drivers) {
 		usbh_baseclassdriver_t *drv = port->device.drivers;
 
 		/* unload */
-		uinfof("Unload driver %s", drv->info->name);
+		uinfof("Port %d: unload driver %s", port->number, drv->info->name);
 		drv->info->vmt->unload(drv);
 
 		/* unlink */
@@ -1100,9 +1084,7 @@ void _usbh_port_disconnected(usbh_port_t *port) {
 	}
 
 	/* close control endpoint */
-	osalSysLock();
-	usbhEPCloseS(&port->device.ctrl);
-	osalSysUnlock();
+	usbhEPClose(&port->device.ctrl);
 
 	/* free address */
 	if (port->device.address)
@@ -1142,7 +1124,7 @@ static void _hub_process_status_change(USBHDriver *host, USBHHubDriver *hub) {
 	uinfo("Hub status change. GET_STATUS.");
 	_hub_update_status(host, hub);
 
-	if (hub->c_status & USBH_HUBSTATUS_C_HUB_LOCAL_POWER) {
+		if (hub->c_status & USBH_HUBSTATUS_C_HUB_LOCAL_POWER) {
 		hub->c_status &= ~USBH_HUBSTATUS_C_HUB_LOCAL_POWER;
 		uinfo("Clear USBH_HUB_FEAT_C_HUB_LOCAL_POWER");
 		usbhhubClearFeatureHub(host, hub, USBH_HUB_FEAT_C_HUB_LOCAL_POWER);
@@ -1160,7 +1142,6 @@ static uint32_t _hub_get_status_change_bitmap(USBHDriver *host, USBHHubDriver *h
 		osalSysLock();
 		uint32_t ret = hub->statuschange;
 		hub->statuschange = 0;
-		osalOsRescheduleS();
 		osalSysUnlock();
 		return ret;
 	}
@@ -1226,8 +1207,8 @@ void usbhMainLoop(USBHDriver *usbh) {
 	_hub_process(usbh, NULL);
 
 	/* process connected hubs */
-	USBHHubDriver *hub;
-    list_for_each_entry(hub, USBHHubDriver, &usbh->hubs, node) {
+	USBHHubDriver *hub, *temp;
+    list_for_each_entry_safe(hub, USBHHubDriver, temp, &usbh->hubs, node) {
 		_hub_process(usbh, hub);
 	}
 #else
@@ -1236,75 +1217,78 @@ void usbhMainLoop(USBHDriver *usbh) {
 #endif
 }
 
-
 /*===========================================================================*/
-/* IAD class driver.                                                         */
+/* Class driver loader.                                                      */
 /*===========================================================================*/
-#if HAL_USBH_USE_IAD
-static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
-static void iad_unload(usbh_baseclassdriver_t *drv);
-static const usbh_classdriver_vmt_t usbhiadClassDriverVMT = {
-	iad_load,
-	iad_unload
-};
-static const usbh_classdriverinfo_t usbhiadClassDriverInfo = {
-	0xef, 0x02, 0x01, "IAD", &usbhiadClassDriverVMT
-};
 
-static usbh_baseclassdriver_t *iad_load(usbh_device_t *dev,
-		const uint8_t *descriptor, uint16_t rem) {
-	(void)rem;
+bool _usbh_match_vid_pid(usbh_device_t *dev, int32_t vid, int32_t pid) {
+	if (((vid < 0) || (dev->devDesc.idVendor == vid))
+		&& ((pid < 0) || (dev->devDesc.idProduct == pid)))
+		return HAL_SUCCESS;
 
-	if (descriptor[1] != USBH_DT_DEVICE)
-		return 0;
+	return HAL_FAILED;
+}
 
-	uinfo("Load a driver for each IF collection.");
+bool _usbh_match_descriptor(const uint8_t *descriptor, uint16_t rem,
+		int16_t type, int16_t _class, int16_t subclass, int16_t protocol) {
 
-	generic_iterator_t icfg;
-	if_iterator_t iif;
-	const usbh_ia_descriptor_t *last_iad = 0;
+	int16_t dclass, dsubclass, dprotocol;
 
-	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
-			dev->basicConfigDesc.wTotalLength);
-	if (!icfg.valid) {
-		uerr("Invalid configuration descriptor.");
-		return 0;
-	}
+	if ((rem < descriptor[0]) || (rem < 2))
+		return HAL_FAILED;
 
-	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
-		if (iif.iad && (iif.iad != last_iad)) {
-			last_iad = iif.iad;
-			if (_classdriver_load(dev, iif.iad->bFunctionClass,
-					iif.iad->bFunctionSubClass,
-					iif.iad->bFunctionProtocol,
-					(uint8_t *)iif.iad,
-					(uint8_t *)iif.curr - (uint8_t *)iif.iad + iif.rem) != HAL_SUCCESS) {
-				uwarnf("No drivers found for IF collection #%d:%d",
-						iif.iad->bFirstInterface,
-						iif.iad->bFirstInterface + iif.iad->bInterfaceCount - 1);
-			}
-		}
-	}
+	uint8_t dtype = descriptor[1];
 
-	return 0;
-}
+	if ((type >= 0) && (type != dtype))
+		return HAL_FAILED;
 
-static void iad_unload(usbh_baseclassdriver_t *drv) {
-	(void)drv;
-}
-#endif
+	switch (dtype) {
+	case USBH_DT_DEVICE: {
+		if (rem < USBH_DT_DEVICE_SIZE)
+			return HAL_FAILED;
+		const usbh_device_descriptor_t *const desc = (const usbh_device_descriptor_t *)descriptor;
+		dclass = desc->bDeviceClass;
+		dsubclass = desc->bDeviceSubClass;
+		dprotocol = desc->bDeviceProtocol;
+	}	break;
+	case USBH_DT_INTERFACE: {
+		if (rem < USBH_DT_INTERFACE_SIZE)
+			return HAL_FAILED;
+		const usbh_interface_descriptor_t *const desc = (const usbh_interface_descriptor_t *)descriptor;
+		dclass = desc->bInterfaceClass;
+		dsubclass = desc->bInterfaceSubClass;
+		dprotocol = desc->bInterfaceProtocol;
+	}	break;
+	case USBH_DT_INTERFACE_ASSOCIATION: {
+		if (rem < USBH_DT_INTERFACE_ASSOCIATION_SIZE)
+			return HAL_FAILED;
+		const usbh_ia_descriptor_t *const desc = (const usbh_ia_descriptor_t *)descriptor;
+		dclass = desc->bFunctionClass;
+		dsubclass = desc->bFunctionSubClass;
+		dprotocol = desc->bFunctionProtocol;
+	}	break;
+	default:
+		return HAL_FAILED;
+	}
 
+	if (((_class < 0) || (_class == dclass))
+		&& ((subclass < 0) || (subclass == dsubclass))
+		&& ((protocol < 0) || (protocol == dprotocol)))
+		return HAL_SUCCESS;
 
-/*===========================================================================*/
-/* Class driver loader.                                                      */
-/*===========================================================================*/
+	return HAL_FAILED;
+}
 
 static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {
+#if HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
+	/* user-defined out of tree class drivers */
+	HAL_USBH_ADDITIONAL_CLASS_DRIVERS
+#endif
 #if HAL_USBH_USE_FTDI
 	&usbhftdiClassDriverInfo,
 #endif
-#if HAL_USBH_USE_IAD
-	&usbhiadClassDriverInfo,
+#if HAL_USBH_USE_HUB
+	&usbhhubClassDriverInfo,
 #endif
 #if HAL_USBH_USE_UVC
 	&usbhuvcClassDriverInfo,
@@ -1318,44 +1302,24 @@ static const usbh_classdriverinfo_t *usbh_classdrivers_lookup[] = {
 #if HAL_USBH_USE_UVC
 	&usbhuvcClassDriverInfo,
 #endif
-#if HAL_USBH_USE_HUB
-	&usbhhubClassDriverInfo,
-#endif
 #if HAL_USBH_USE_AOA
 	&usbhaoaClassDriverInfo,	/* Leave always last */
 #endif
 };
 
-static bool _classdriver_load(usbh_device_t *dev, uint8_t class,
-		uint8_t subclass, uint8_t protocol, uint8_t *descbuff, uint16_t rem) {
+static bool _classdriver_load(usbh_device_t *dev, uint8_t *descbuff, uint16_t rem) {
 	uint8_t i;
 	usbh_baseclassdriver_t *drv = NULL;
 	for (i = 0; i < sizeof_array(usbh_classdrivers_lookup); i++) {
 		const usbh_classdriverinfo_t *const info = usbh_classdrivers_lookup[i];
-		if (class == 0xff) {
-			/* vendor specific */
-			if (info->class == 0xff) {
-				uinfof("Try load vendor-specific driver %s", info->name);
-				drv = info->vmt->load(dev, descbuff, rem);
-				if (drv != NULL)
-					goto success;
-			}
-		} else if ((info->class < 0) || ((info->class == class)
-			&& ((info->subclass < 0) || ((info->subclass == subclass)
-			&& ((info->protocol < 0) || (info->protocol == protocol)))))) {
-			uinfof("Try load driver %s", info->name);
-			drv = info->vmt->load(dev, descbuff, rem);
 
-#if HAL_USBH_USE_IAD
-			/* special case: */
-			if (info == &usbhiadClassDriverInfo)
-				goto success; //return HAL_SUCCESS;
-#endif
+		uinfof("Try load driver %s", info->name);
+		drv = info->vmt->load(dev, descbuff, rem);
 
-			if (drv != NULL)
-				goto success;
-		}
+		if (drv != NULL)
+			goto success;
 	}
+
 	return HAL_FAILED;
 
 success:
@@ -1396,13 +1360,16 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 	usbhDevicePrintConfiguration(dev->fullConfigurationDescriptor,
 			dev->basicConfigDesc.wTotalLength);
 
-	if (devdesc->bDeviceClass == 0) {
-		/* each interface defines its own device class/subclass/protocol */
-		uinfo("Load a driver for each IF.");
+#if HAL_USBH_USE_IAD
+	if (dev->devDesc.bDeviceClass == 0xef
+			&& dev->devDesc.bDeviceSubClass == 0x02
+			&& dev->devDesc.bDeviceProtocol == 0x01) {
+
+		uinfo("Load a driver for each IF collection.");
 
 		generic_iterator_t icfg;
 		if_iterator_t iif;
-		uint8_t last_if = 0xff;
+		const usbh_ia_descriptor_t *last_iad = 0;
 
 		cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
 				dev->basicConfigDesc.wTotalLength);
@@ -1412,24 +1379,49 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 		}
 
 		for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
-			const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
-			if (ifdesc->bInterfaceNumber != last_if) {
-				last_if = ifdesc->bInterfaceNumber;
-				if (_classdriver_load(dev, ifdesc->bInterfaceClass,
-						ifdesc->bInterfaceSubClass,
-						ifdesc->bInterfaceProtocol,
-						(uint8_t *)ifdesc, iif.rem) != HAL_SUCCESS) {
-					uwarnf("No drivers found for IF #%d", ifdesc->bInterfaceNumber);
+			if (iif.iad && (iif.iad != last_iad)) {
+				last_iad = iif.iad;
+				if (_classdriver_load(dev,
+						(uint8_t *)iif.iad,
+						(uint8_t *)iif.curr - (uint8_t *)iif.iad + iif.rem) != HAL_SUCCESS) {
+					uwarnf("No drivers found for IF collection #%d:%d",
+							iif.iad->bFirstInterface,
+							iif.iad->bFirstInterface + iif.iad->bInterfaceCount - 1);
 				}
 			}
 		}
 
-	} else {
-		if (_classdriver_load(dev, devdesc->bDeviceClass,
-				devdesc->bDeviceSubClass,
-				devdesc->bDeviceProtocol,
-				(uint8_t *)devdesc, USBH_DT_DEVICE_SIZE) != HAL_SUCCESS) {
-			uwarn("No drivers found.");
+	} else
+#endif
+	if (_classdriver_load(dev, (uint8_t *)devdesc, USBH_DT_DEVICE_SIZE) != HAL_SUCCESS) {
+		uinfo("No drivers found for device.");
+
+		if (devdesc->bDeviceClass == 0) {
+			/* each interface defines its own device class/subclass/protocol */
+			uinfo("Try load a driver for each IF.");
+
+			generic_iterator_t icfg;
+			if_iterator_t iif;
+			uint8_t last_if = 0xff;
+
+			cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
+					dev->basicConfigDesc.wTotalLength);
+			if (!icfg.valid) {
+				uerr("Invalid configuration descriptor.");
+				goto exit;
+			}
+
+			for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
+				const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+				if (ifdesc->bInterfaceNumber != last_if) {
+					last_if = ifdesc->bInterfaceNumber;
+					if (_classdriver_load(dev, (uint8_t *)ifdesc, iif.rem) != HAL_SUCCESS) {
+						uwarnf("No drivers found for IF #%d", ifdesc->bInterfaceNumber);
+					}
+				}
+			}
+		} else {
+			uwarn("Unable to load driver.");
 		}
 	}
 
@@ -1439,6 +1431,15 @@ exit:
 	}
 }
 
+void usbhInit(void) {
+	uint8_t i;
+	for (i = 0; i < sizeof_array(usbh_classdrivers_lookup); i++) {
+		if (usbh_classdrivers_lookup[i]->vmt->init) {
+			usbh_classdrivers_lookup[i]->vmt->init();
+		}
+	}
+	usbh_lld_init();
+}
 
 #endif
 
diff --git a/os/hal/src/usbh/TODO.txt b/os/hal/src/usbh/TODO.txt
new file mode 100644
index 0000000..87269be
--- /dev/null
+++ b/os/hal/src/usbh/TODO.txt
@@ -0,0 +1,21 @@
+In decreasing order of priority:
+
+Bugs:
+- Synchronization on driver unload between usbhMainLoop and driver APIs
+    - MSD: ok
+    - AOA: not done
+    - HUB: ok
+    - FTDI: not done
+    - HID: ok
+    - UVC: not done
+
+
+Enhancements:
+- Way to return error from the load() functions in order to stop the enumeration process
+- Event sources from the low-level driver, in order to know when to call usbhMainLoop (from the low-level driver and from the HUB driver status callback)
+- Possibility of internal main loop
+- Linked list for drivers for dynamic registration
+- A way to automate matching (similar to linux)
+- Hooks to override driver loading and to inform the user of problems
+- for STM32 LLD: think of a way to prevent Bulk IN NAK interrupt flood.
+- Integrate VBUS power switching functionality to the API.
diff --git a/os/hal/src/usbh/hal_usbh_aoa.c b/os/hal/src/usbh/hal_usbh_aoa.c
index 1fa49f8..85c3130 100644
--- a/os/hal/src/usbh/hal_usbh_aoa.c
+++ b/os/hal/src/usbh/hal_usbh_aoa.c
@@ -124,14 +124,16 @@ USBHAOADriver USBHAOAD[HAL_USBHAOA_MAX_INSTANCES];
 
 static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
 static void _aoa_unload(usbh_baseclassdriver_t *drv);
+static void _aoa_init(void);
 
 static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_aoa_init,
 	_aoa_load,
 	_aoa_unload
 };
 
 const usbh_classdriverinfo_t usbhaoaClassDriverInfo = {
-	0xff, 0xff, 0xff, "AOA", &class_driver_vmt
+	"AOA", &class_driver_vmt
 };
 
 #if defined(HAL_USBHAOA_FILTER_CALLBACK)
@@ -144,7 +146,7 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 
 	if (dev->devDesc.idVendor != AOA_GOOGLE_VID) {
 		uint16_t protocol;
-		static const USBHAOAConfig config = {
+		static USBHAOAConfig config = {
 			{
 				HAL_USBHAOA_DEFAULT_MANUFACTURER,
 				HAL_USBHAOA_DEFAULT_MODEL,
@@ -221,15 +223,9 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 		return NULL;
 	}
 
-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
-		return NULL;
-
 	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
-
-	if ((ifdesc->bInterfaceClass != 0xff)
-		|| (ifdesc->bInterfaceSubClass != 0xff)
-		|| (ifdesc->bInterfaceProtocol != 0x00)
-		|| (ifdesc->bNumEndpoints < 2)) {
+	if ((_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE, 0xFF, 0xFF, 0x00) != HAL_SUCCESS)
+			|| (ifdesc->bNumEndpoints < 2)) {
 		uerr("AOA: This IF is not the Accessory IF");
 		return NULL;
 	}
@@ -299,7 +295,6 @@ static void _aoa_unload(usbh_baseclassdriver_t *drv) {
 	_stop_channelS(&aoap->channel);
 	aoap->channel.state = USBHAOA_CHANNEL_STATE_STOP;
 	aoap->state = USBHAOA_STATE_STOP;
-	osalOsRescheduleS();
 	osalSysUnlock();
 }
 
@@ -341,15 +336,15 @@ static size_t _write_timeout(USBHAOAChannel *aoacp, const uint8_t *bp,
 	chDbgCheck(n > 0U);
 
 	size_t w = 0;
-	chSysLock();
+	osalSysLock();
 	while (true) {
 		if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
-			chSysUnlock();
+			osalSysUnlock();
 			return w;
 		}
 		while (usbhURBIsBusy(&aoacp->oq_urb)) {
 			if (chThdEnqueueTimeoutS(&aoacp->oq_waiting, timeout) != Q_OK) {
-				chSysUnlock();
+				osalSysUnlock();
 				return w;
 			}
 		}
@@ -357,30 +352,30 @@ static size_t _write_timeout(USBHAOAChannel *aoacp, const uint8_t *bp,
 		*aoacp->oq_ptr++ = *bp++;
 		if (--aoacp->oq_counter == 0) {
 			_submitOutI(aoacp, 64);
-			chSchRescheduleS();
+			osalOsRescheduleS();
 		}
-		chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
+		osalSysUnlock(); /* Gives a preemption chance in a controlled point.*/
 
 		w++;
 		if (--n == 0U)
 			return w;
 
-		chSysLock();
+		osalSysLock();
 	}
 }
 
 static msg_t _put_timeout(USBHAOAChannel *aoacp, uint8_t b, systime_t timeout) {
 
-	chSysLock();
+	osalSysLock();
 	if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
-		chSysUnlock();
+		osalSysUnlock();
 		return Q_RESET;
 	}
 
 	while (usbhURBIsBusy(&aoacp->oq_urb)) {
 		msg_t msg = chThdEnqueueTimeoutS(&aoacp->oq_waiting, timeout);
 		if (msg < Q_OK) {
-			chSysUnlock();
+			osalSysUnlock();
 			return msg;
 		}
 	}
@@ -388,9 +383,9 @@ static msg_t _put_timeout(USBHAOAChannel *aoacp, uint8_t b, systime_t timeout) {
 	*aoacp->oq_ptr++ = b;
 	if (--aoacp->oq_counter == 0) {
 		_submitOutI(aoacp, 64);
-		chSchRescheduleS();
+		osalOsRescheduleS();
 	}
-	chSysUnlock();
+	osalSysUnlock();
 	return Q_OK;
 }
 
@@ -439,41 +434,41 @@ static size_t _read_timeout(USBHAOAChannel *aoacp, uint8_t *bp,
 
 	chDbgCheck(n > 0U);
 
-	chSysLock();
+	osalSysLock();
 	while (true) {
 		if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
-			chSysUnlock();
+			osalSysUnlock();
 			return r;
 		}
 		while (aoacp->iq_counter == 0) {
 			if (!usbhURBIsBusy(&aoacp->iq_urb))
 				_submitInI(aoacp);
 			if (chThdEnqueueTimeoutS(&aoacp->iq_waiting, timeout) != Q_OK) {
-				chSysUnlock();
+				osalSysUnlock();
 				return r;
 			}
 		}
 		*bp++ = *aoacp->iq_ptr++;
 		if (--aoacp->iq_counter == 0) {
 			_submitInI(aoacp);
-			chSchRescheduleS();
+			osalOsRescheduleS();
 		}
-		chSysUnlock();
+		osalSysUnlock();
 
 		r++;
 		if (--n == 0U)
 			return r;
 
-		chSysLock();
+		osalSysLock();
 	}
 }
 
 static msg_t _get_timeout(USBHAOAChannel *aoacp, systime_t timeout) {
 	uint8_t b;
 
-	chSysLock();
+	osalSysLock();
 	if (aoacp->state != USBHAOA_CHANNEL_STATE_READY) {
-		chSysUnlock();
+		osalSysUnlock();
 		return Q_RESET;
 	}
 	while (aoacp->iq_counter == 0) {
@@ -481,16 +476,16 @@ static msg_t _get_timeout(USBHAOAChannel *aoacp, systime_t timeout) {
 			_submitInI(aoacp);
 		msg_t msg = chThdEnqueueTimeoutS(&aoacp->iq_waiting, timeout);
 		if (msg < Q_OK) {
-			chSysUnlock();
+			osalSysUnlock();
 			return msg;
 		}
 	}
 	b = *aoacp->iq_ptr++;
 	if (--aoacp->iq_counter == 0) {
 		_submitInI(aoacp);
-		chSchRescheduleS();
+		osalOsRescheduleS();
 	}
-	chSysUnlock();
+	osalSysUnlock();
 
 	return (msg_t)b;
 }
@@ -525,11 +520,12 @@ static void _stop_channelS(USBHAOAChannel *aoacp) {
 	chThdDequeueAllI(&aoacp->oq_waiting, Q_RESET);
 	chnAddFlagsI(aoacp, CHN_DISCONNECTED);
 	aoacp->state = USBHAOA_CHANNEL_STATE_ACTIVE;
+	osalOsRescheduleS();
 }
 
 static void _vt(void *p) {
 	USBHAOAChannel *const aoacp = (USBHAOAChannel *)p;
-	chSysLockFromISR();
+	osalSysLockFromISR();
 	uint32_t len = aoacp->oq_ptr - aoacp->oq_buff;
 	if (len && !usbhURBIsBusy(&aoacp->oq_urb)) {
 		_submitOutI(aoacp, len);
@@ -538,7 +534,7 @@ static void _vt(void *p) {
 		_submitInI(aoacp);
 	}
 	chVTSetI(&aoacp->vt, MS2ST(16), _vt, aoacp);
-	chSysUnlockFromISR();
+	osalSysUnlockFromISR();
 }
 
 void usbhaoaChannelStart(USBHAOADriver *aoap) {
@@ -566,9 +562,7 @@ void usbhaoaChannelStart(USBHAOADriver *aoap) {
 	aoacp->iq_counter = 0;
 	aoacp->iq_ptr = aoacp->iq_buff;
 	usbhEPOpen(&aoacp->epin);
-	osalSysLock();
-	usbhURBSubmitI(&aoacp->iq_urb);
-	osalSysUnlock();
+	usbhURBSubmit(&aoacp->iq_urb);
 
 	chVTObjectInit(&aoacp->vt);
 	chVTSet(&aoacp->vt, MS2ST(16), _vt, aoacp);
@@ -583,7 +577,6 @@ void usbhaoaChannelStop(USBHAOADriver *aoap) {
 			|| (aoap->channel.state == USBHAOA_CHANNEL_STATE_READY));
 	osalSysLock();
 	_stop_channelS(&aoap->channel);
-	osalOsRescheduleS();
 	osalSysUnlock();
 }
 
@@ -658,7 +651,7 @@ static bool _send_string(usbh_device_t *dev, uint8_t index, const char *string)
 	return HAL_SUCCESS;
 }
 
-void usbhaoaObjectInit(USBHAOADriver *aoap) {
+static void _object_init(USBHAOADriver *aoap) {
 	osalDbgCheck(aoap != NULL);
 	memset(aoap, 0, sizeof(*aoap));
 	aoap->info = &usbhaoaClassDriverInfo;
@@ -668,10 +661,10 @@ void usbhaoaObjectInit(USBHAOADriver *aoap) {
 	aoap->channel.state = USBHAOA_CHANNEL_STATE_STOP;
 }
 
-void usbhaoaInit(void) {
+static void _aoa_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHAOA_MAX_INSTANCES; i++) {
-		usbhaoaObjectInit(&USBHAOAD[i]);
+		_object_init(&USBHAOAD[i]);
 	}
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_debug.c b/os/hal/src/usbh/hal_usbh_debug.c
index 7c4ff7d..d32f1c6 100644
--- a/os/hal/src/usbh/hal_usbh_debug.c
+++ b/os/hal/src/usbh/hal_usbh_debug.c
@@ -106,17 +106,18 @@ static char *ftoa(char *p, double num, unsigned long precision, bool dot) {
 }
 #endif
 
-static inline void _put(char c) {
-	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
-
-	if (iqIsFullI(iqp))
-		return;
-
-	iqp->q_counter++;
+static inline void _wr(input_queue_t *iqp, char c) {
 	*iqp->q_wrptr++ = c;
 	if (iqp->q_wrptr >= iqp->q_top)
 		iqp->q_wrptr = iqp->q_buffer;
+}
 
+static inline void _put(char c) {
+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
+	if (sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter <= 1)
+		return;
+	iqp->q_counter++;
+	_wr(iqp, c);
 }
 
 int _dbg_printf(const char *fmt, va_list ap) {
@@ -341,19 +342,39 @@ unsigned_common:
 
 }
 
-static void _print_hdr(void)
-{
+static systime_t first, last;
+static bool ena;
+static uint32_t hdr[2];
+
+static void _build_hdr(void) {
 	uint32_t hfnum = USBH_DEBUG_USBHD.otg->HFNUM;
 	uint16_t hfir = USBH_DEBUG_USBHD.otg->HFIR;
+	last = osalOsGetSystemTimeX();
+	if (ena) {
+		first = last;
+	}
+
+	if (((hfnum & 0x3fff) == 0x3fff) && (hfir == (hfnum >> 16))) {
+		hdr[0] = 0xfeff;
+		hdr[1] = last - first;
+		ena = FALSE;
+	} else {
+		hdr[0] = 0xffff | (hfir << 16);
+		hdr[1] = hfnum;
+		ena = TRUE;
+	}
+}
 
-	_put(0xff);
-	_put(0xff);
-	_put(hfir & 0xff);
-	_put(hfir >> 8);
-	_put(hfnum & 0xff);
-	_put((hfnum >> 8) & 0xff);
-	_put((hfnum >> 16) & 0xff);
-	_put((hfnum >> 24) & 0xff);
+static void _print_hdr(void)
+{
+	_put(hdr[0] & 0xff);
+	_put((hdr[0] >> 8) & 0xff);
+	_put((hdr[0] >> 16) & 0xff);
+	_put((hdr[0] >> 24) & 0xff);
+	_put(hdr[1] & 0xff);
+	_put((hdr[1] >> 8) & 0xff);
+	_put((hdr[1] >> 16) & 0xff);
+	_put((hdr[1] >> 24) & 0xff);
 }
 
 void usbDbgPrintf(const char *fmt, ...)
@@ -361,10 +382,16 @@ void usbDbgPrintf(const char *fmt, ...)
 	va_list ap;
 	va_start(ap, fmt);
 	syssts_t sts = chSysGetStatusAndLockX();
-	_print_hdr();
-	_dbg_printf(fmt, ap);
-	_put(0);
-	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
+	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;
+	if (rem >= 9) {
+		_build_hdr();
+		_print_hdr();
+		_dbg_printf(fmt, ap);
+		iqp->q_counter++;
+		_wr(iqp, 0);
+		chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
+	}
 	chSysRestoreStatusX(sts);
 	va_end(ap);
 }
@@ -372,32 +399,28 @@ void usbDbgPrintf(const char *fmt, ...)
 
 void usbDbgPuts(const char *s)
 {
-	uint32_t buff[2] = {
-		0xffff | (USBH_DEBUG_USBHD.otg->HFIR << 16),
-		USBH_DEBUG_USBHD.otg->HFNUM
-	};
-	uint8_t *p = (uint8_t *)buff;
+	_build_hdr();
+	uint8_t *p = (uint8_t *)hdr;
 	uint8_t *top = p + 8;
 
 	syssts_t sts = chSysGetStatusAndLockX();
 	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
 	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;
-	while (rem) {
-		*iqp->q_wrptr++ = *p;
-		if (iqp->q_wrptr >= iqp->q_top)
-			iqp->q_wrptr = iqp->q_buffer;
-		rem--;
-		if (++p == top) break;
-	}
-	while (rem) {
-		*iqp->q_wrptr++ = *s;
-		if (iqp->q_wrptr >= iqp->q_top)
-			iqp->q_wrptr = iqp->q_buffer;
-		rem--;
-		if (!*s++) break;
+	if (rem >= 9) {
+		while (rem) {
+			_wr(iqp, *p);
+			if (++p == top) break;
+		}
+		rem -= 9;
+		while (rem && *s) {
+			_wr(iqp, *s);
+			rem--;
+			s++;
+		}
+		_wr(iqp, 0);
+		iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;
+		chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
 	}
-	iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;
-	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
 	chSysRestoreStatusX(sts);
 }
 
@@ -429,8 +452,8 @@ void usbDbgSystemHalted(void) {
 		if (!((bool)((USBH_DEBUG_SD.oqueue.q_wrptr == USBH_DEBUG_SD.oqueue.q_rdptr) && (USBH_DEBUG_SD.oqueue.q_counter != 0U))))
 			break;
 		USBH_DEBUG_SD.oqueue.q_counter++;
-		while (!(USART1->SR & USART_SR_TXE));
-		USART1->DR = *USBH_DEBUG_SD.oqueue.q_rdptr++;
+		while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
+		USBH_DEBUG_SD.usart->DR = *USBH_DEBUG_SD.oqueue.q_rdptr++;
 		if (USBH_DEBUG_SD.oqueue.q_rdptr >= USBH_DEBUG_SD.oqueue.q_top) {
 			USBH_DEBUG_SD.oqueue.q_rdptr = USBH_DEBUG_SD.oqueue.q_buffer;
 		}
@@ -456,15 +479,15 @@ void usbDbgSystemHalted(void) {
 			while (true) {
 				c = _get(); if (c < 0) return;
 				if (!c) {
-					while (!(USART1->SR & USART_SR_TXE));
-					USART1->DR = '\r';
-					while (!(USART1->SR & USART_SR_TXE));
-					USART1->DR = '\n';
+					while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
+					USBH_DEBUG_SD.usart->DR = '\r';
+					while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
+					USBH_DEBUG_SD.usart->DR = '\n';
 					state = 0;
 					break;
 				}
-				while (!(USART1->SR & USART_SR_TXE));
-				USART1->DR = c;
+				while (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
+				USBH_DEBUG_SD.usart->DR = c;
 			}
 		}
 	}
@@ -483,8 +506,9 @@ static void usb_debug_thread(void *arg) {
 			if (c == 0xff) state = 1;
 		} else if (state == 1) {
 			if (c == 0xff) state = 2;
+			else if (c == 0xfe) state = 3;
 			else (state = 0);
-		} else {
+		} else if (state == 2) {
 			uint16_t hfir;
 			uint32_t hfnum;
 
@@ -503,10 +527,26 @@ static void usb_debug_thread(void *arg) {
 
 			uint32_t f = hfnum & 0xffff;
 			uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));
-			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d  ", f, p);
+			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d ", f, p);
+			state = 4;
+		} else if (state == 3) {
+			uint32_t t;
 
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+
+			t = c;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+			t |= c << 8;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+			t |= c << 16;
+			c = iqGet(&host->iq); if (c < 0) goto reset;
+			t |= c << 24;
+
+			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "+%08d ", t);
+			state = 4;
+		} else {
 			while (true) {
-				c = iqGet(&host->iq); if (c < 0) goto reset;
 				if (!c) {
 					sdPut(&USBH_DEBUG_SD, '\r');
 					sdPut(&USBH_DEBUG_SD, '\n');
@@ -514,6 +554,7 @@ static void usb_debug_thread(void *arg) {
 					break;
 				}
 				sdPut(&USBH_DEBUG_SD, (uint8_t)c);
+				c = iqGet(&host->iq); if (c < 0) goto reset;
 			}
 		}
 
diff --git a/os/hal/src/usbh/hal_usbh_ftdi.c b/os/hal/src/usbh/hal_usbh_ftdi.c
index ce96958..6e33867 100644
--- a/os/hal/src/usbh/hal_usbh_ftdi.c
+++ b/os/hal/src/usbh/hal_usbh_ftdi.c
@@ -27,9 +27,6 @@
 #include "usbh/dev/ftdi.h"
 #include "usbh/internal.h"
 
-//#pragma GCC optimize("Og")
-
-
 #if USBHFTDI_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
 #define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
@@ -62,22 +59,25 @@
 #define uerr(f, ...)   do {} while(0)
 #endif
 
+static void _ftdip_object_init(USBHFTDIPortDriver *ftdipp);
 
 /*===========================================================================*/
 /* USB Class driver loader for FTDI								 		 	 */
 /*===========================================================================*/
 USBHFTDIDriver USBHFTDID[HAL_USBHFTDI_MAX_INSTANCES];
 
+static void _ftdi_init(void);
 static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
 static void _ftdi_unload(usbh_baseclassdriver_t *drv);
 
 static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_ftdi_init,
 	_ftdi_load,
 	_ftdi_unload
 };
 
 const usbh_classdriverinfo_t usbhftdiClassDriverInfo = {
-	0xff, 0xff, 0xff, "FTDI", &class_driver_vmt
+	"FTDI", &class_driver_vmt
 };
 
 static USBHFTDIPortDriver *_find_port(void) {
@@ -93,10 +93,8 @@ static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *des
 	int i;
 	USBHFTDIDriver *ftdip;
 
-	if (dev->devDesc.idVendor != 0x0403) {
-		uerr("FTDI: Unrecognized VID");
+	if (_usbh_match_vid_pid(dev, 0x0403, -1) != HAL_SUCCESS)
 		return NULL;
-	}
 
 	switch (dev->devDesc.idProduct) {
 	case 0x6001:
@@ -111,7 +109,8 @@ static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *des
 		return NULL;
 	}
 
-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE,
+			0xff, 0xff, 0xff) != HAL_SUCCESS)
 		return NULL;
 
 	if (((const usbh_interface_descriptor_t *)descriptor)->bInterfaceNumber != 0) {
@@ -220,7 +219,6 @@ static void _ftdi_unload(usbh_baseclassdriver_t *drv) {
 	while (ftdipp) {
 		osalSysLock();
 		_stopS(ftdipp);
-		osalOsRescheduleS();
 		osalSysUnlock();
 		ftdipp = ftdipp->next;
 	}
@@ -229,7 +227,7 @@ static void _ftdi_unload(usbh_baseclassdriver_t *drv) {
 	osalSysLock();
 	while (ftdipp) {
 		USBHFTDIPortDriver *next = ftdipp->next;
-		usbhftdipObjectInit(ftdipp);
+		_ftdip_object_init(ftdipp);
 		ftdipp = next;
 	}
 	osalSysUnlock();
@@ -432,15 +430,15 @@ static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,
 	chDbgCheck(n > 0U);
 
 	size_t w = 0;
-	chSysLock();
+	osalSysLock();
 	while (true) {
 		if (ftdipp->state != USBHFTDIP_STATE_READY) {
-			chSysUnlock();
+			osalSysUnlock();
 			return w;
 		}
 		while (usbhURBIsBusy(&ftdipp->oq_urb)) {
 			if (chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout) != Q_OK) {
-				chSysUnlock();
+				osalSysUnlock();
 				return w;
 			}
 		}
@@ -448,30 +446,30 @@ static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,
 		*ftdipp->oq_ptr++ = *bp++;
 		if (--ftdipp->oq_counter == 0) {
 			_submitOutI(ftdipp, 64);
-			chSchRescheduleS();
+			osalOsRescheduleS();
 		}
-		chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
+		osalSysUnlock(); /* Gives a preemption chance in a controlled point.*/
 
 		w++;
 		if (--n == 0U)
 			return w;
 
-		chSysLock();
+		osalSysLock();
 	}
 }
 
 static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeout) {
 
-	chSysLock();
+	osalSysLock();
 	if (ftdipp->state != USBHFTDIP_STATE_READY) {
-		chSysUnlock();
+		osalSysUnlock();
 		return Q_RESET;
 	}
 
 	while (usbhURBIsBusy(&ftdipp->oq_urb)) {
 		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout);
 		if (msg < Q_OK) {
-			chSysUnlock();
+			osalSysUnlock();
 			return msg;
 		}
 	}
@@ -479,9 +477,9 @@ static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeo
 	*ftdipp->oq_ptr++ = b;
 	if (--ftdipp->oq_counter == 0) {
 		_submitOutI(ftdipp, 64);
-		chSchRescheduleS();
+		osalOsRescheduleS();
 	}
-	chSysUnlock();
+	osalSysUnlock();
 	return Q_OK;
 }
 
@@ -538,41 +536,41 @@ static size_t _read_timeout(USBHFTDIPortDriver *ftdipp, uint8_t *bp,
 
 	chDbgCheck(n > 0U);
 
-	chSysLock();
+	osalSysLock();
 	while (true) {
 		if (ftdipp->state != USBHFTDIP_STATE_READY) {
-			chSysUnlock();
+			osalSysUnlock();
 			return r;
 		}
 		while (ftdipp->iq_counter == 0) {
 			if (!usbhURBIsBusy(&ftdipp->iq_urb))
 				_submitInI(ftdipp);
 			if (chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout) != Q_OK) {
-				chSysUnlock();
+				osalSysUnlock();
 				return r;
 			}
 		}
 		*bp++ = *ftdipp->iq_ptr++;
 		if (--ftdipp->iq_counter == 0) {
 			_submitInI(ftdipp);
-			chSchRescheduleS();
+			osalOsRescheduleS();
 		}
-		chSysUnlock();
+		osalSysUnlock();
 
 		r++;
 		if (--n == 0U)
 			return r;
 
-		chSysLock();
+		osalSysLock();
 	}
 }
 
 static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {
 	uint8_t b;
 
-	chSysLock();
+	osalSysLock();
 	if (ftdipp->state != USBHFTDIP_STATE_READY) {
-		chSysUnlock();
+		osalSysUnlock();
 		return Q_RESET;
 	}
 	while (ftdipp->iq_counter == 0) {
@@ -580,16 +578,16 @@ static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {
 			_submitInI(ftdipp);
 		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout);
 		if (msg < Q_OK) {
-			chSysUnlock();
+			osalSysUnlock();
 			return msg;
 		}
 	}
 	b = *ftdipp->iq_ptr++;
 	if (--ftdipp->iq_counter == 0) {
 		_submitInI(ftdipp);
-		chSchRescheduleS();
+		osalOsRescheduleS();
 	}
-	chSysUnlock();
+	osalSysUnlock();
 
 	return (msg_t)b;
 }
@@ -604,7 +602,7 @@ static size_t _read(USBHFTDIPortDriver *ftdipp, uint8_t *bp, size_t n) {
 
 static void _vt(void *p) {
 	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)p;
-	chSysLockFromISR();
+	osalSysLockFromISR();
 	uint32_t len = ftdipp->oq_ptr - ftdipp->oq_buff;
 	if (len && !usbhURBIsBusy(&ftdipp->oq_urb)) {
 		_submitOutI(ftdipp, len);
@@ -613,7 +611,7 @@ static void _vt(void *p) {
 		_submitInI(ftdipp);
 	}
 	chVTSetI(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
-	chSysUnlockFromISR();
+	osalSysUnlockFromISR();
 }
 
 static const struct FTDIPortDriverVMT async_channel_vmt = {
@@ -637,6 +635,7 @@ static void _stopS(USBHFTDIPortDriver *ftdipp) {
 	chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);
 	chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);
 	ftdipp->state = USBHFTDIP_STATE_ACTIVE;
+	osalOsRescheduleS();
 }
 
 void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {
@@ -647,7 +646,6 @@ void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {
 	chMtxLockS(&ftdipp->ftdip->mtx);
 	_stopS(ftdipp);
 	chMtxUnlockS(&ftdipp->ftdip->mtx);
-	osalOsRescheduleS();
 	osalSysUnlock();
 }
 
@@ -689,9 +687,7 @@ void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config
 	ftdipp->iq_counter = 0;
 	ftdipp->iq_ptr = ftdipp->iq_buff;
 	usbhEPOpen(&ftdipp->epin);
-	osalSysLock();
-	usbhURBSubmitI(&ftdipp->iq_urb);
-	osalSysUnlock();
+	usbhURBSubmit(&ftdipp->iq_urb);
 
 	chVTObjectInit(&ftdipp->vt);
 	chVTSet(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
@@ -700,27 +696,27 @@ void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config
 	osalMutexUnlock(&ftdipp->ftdip->mtx);
 }
 
-void usbhftdiObjectInit(USBHFTDIDriver *ftdip) {
+static void _ftdi_object_init(USBHFTDIDriver *ftdip) {
 	osalDbgCheck(ftdip != NULL);
 	memset(ftdip, 0, sizeof(*ftdip));
 	ftdip->info = &usbhftdiClassDriverInfo;
 	osalMutexObjectInit(&ftdip->mtx);
 }
 
-void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp) {
+static void _ftdip_object_init(USBHFTDIPortDriver *ftdipp) {
 	osalDbgCheck(ftdipp != NULL);
 	memset(ftdipp, 0, sizeof(*ftdipp));
 	ftdipp->vmt = &async_channel_vmt;
 	ftdipp->state = USBHFTDIP_STATE_STOP;
 }
 
-void usbhftdiInit(void) {
+static void _ftdi_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHFTDI_MAX_INSTANCES; i++) {
-		usbhftdiObjectInit(&USBHFTDID[i]);
+		_ftdi_object_init(&USBHFTDID[i]);
 	}
 	for (i = 0; i < HAL_USBHFTDI_MAX_PORTS; i++) {
-		usbhftdipObjectInit(&FTDIPD[i]);
+		_ftdip_object_init(&FTDIPD[i]);
 	}
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_hid.c b/os/hal/src/usbh/hal_usbh_hid.c
index e98dff7..2b2c5ce 100644
--- a/os/hal/src/usbh/hal_usbh_hid.c
+++ b/os/hal/src/usbh/hal_usbh_hid.c
@@ -69,28 +69,32 @@
 #define USBH_HID_REQ_SET_PROTOCOL	0x0B
 
 /*===========================================================================*/
-/* USB Class driver loader for MSD								 		 	 */
+/* USB Class driver loader for HID								 		 	 */
 /*===========================================================================*/
 
 USBHHIDDriver USBHHIDD[HAL_USBHHID_MAX_INSTANCES];
 
+static void _hid_init(void);
 static usbh_baseclassdriver_t *_hid_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
 static void _hid_unload(usbh_baseclassdriver_t *drv);
+static void _stop_locked(USBHHIDDriver *hidp);
 
 static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_hid_init,
 	_hid_load,
 	_hid_unload
 };
 
 const usbh_classdriverinfo_t usbhhidClassDriverInfo = {
-	0x03, -1, -1, "HID", &class_driver_vmt
+	"HID", &class_driver_vmt
 };
 
 static usbh_baseclassdriver_t *_hid_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
 	int i;
 	USBHHIDDriver *hidp;
 
-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE,
+			0x03, -1, -1) != HAL_SUCCESS)
 		return NULL;
 
 	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
@@ -180,7 +184,11 @@ deinit:
 }
 
 static void _hid_unload(usbh_baseclassdriver_t *drv) {
-	(void)drv;
+	USBHHIDDriver *const hidp = (USBHHIDDriver *)drv;
+	chSemWait(&hidp->sem);
+	_stop_locked(hidp);
+	hidp->state = USBHHID_STATE_STOP;
+	chSemSignal(&hidp->sem);
 }
 
 static void _in_cb(usbh_urb_t *urb) {
@@ -209,17 +217,22 @@ static void _in_cb(usbh_urb_t *urb) {
 void usbhhidStart(USBHHIDDriver *hidp, const USBHHIDConfig *cfg) {
 	osalDbgCheck(hidp && cfg);
 	osalDbgCheck(cfg->report_buffer && (cfg->protocol <= USBHHID_PROTOCOL_REPORT));
-	osalDbgCheck((hidp->state == USBHHID_STATE_ACTIVE)
-			|| (hidp->state == USBHHID_STATE_READY));
 
-	if (hidp->state == USBHHID_STATE_READY)
+	chSemWait(&hidp->sem);
+	if (hidp->state == USBHHID_STATE_READY) {
+		chSemSignal(&hidp->sem);
 		return;
+	}
+	osalDbgCheck(hidp->state == USBHHID_STATE_ACTIVE);
 
 	hidp->config = cfg;
 
 	/* init the URBs */
+	uint32_t report_len = hidp->epin.wMaxPacketSize;
+	if (report_len > cfg->report_len)
+		report_len = cfg->report_len;
 	usbhURBObjectInit(&hidp->in_urb, &hidp->epin, _in_cb, hidp,
-			cfg->report_buffer, cfg->report_len);
+			cfg->report_buffer, report_len);
 
 	/* open the int IN/OUT endpoints */
 	usbhEPOpen(&hidp->epin);
@@ -231,29 +244,31 @@ void usbhhidStart(USBHHIDDriver *hidp, const USBHHIDConfig *cfg) {
 
 	usbhhidSetProtocol(hidp, cfg->protocol);
 
-	osalSysLock();
-	usbhURBSubmitI(&hidp->in_urb);
-	osalSysUnlock();
+	usbhURBSubmit(&hidp->in_urb);
 
 	hidp->state = USBHHID_STATE_READY;
+	chSemSignal(&hidp->sem);
 }
 
-void usbhhidStop(USBHHIDDriver *hidp) {
-	osalDbgCheck((hidp->state == USBHHID_STATE_ACTIVE)
-			|| (hidp->state == USBHHID_STATE_READY));
-
-	if (hidp->state != USBHHID_STATE_READY)
+static void _stop_locked(USBHHIDDriver *hidp) {
+	if (hidp->state == USBHHID_STATE_ACTIVE)
 		return;
 
-	osalSysLock();
-	usbhEPCloseS(&hidp->epin);
+	osalDbgCheck(hidp->state == USBHHID_STATE_READY);
+
+	usbhEPClose(&hidp->epin);
 #if HAL_USBHHID_USE_INTERRUPT_OUT
 	if (hidp->epout.status != USBH_EPSTATUS_UNINITIALIZED) {
-		usbhEPCloseS(&hidp->epout);
+		usbhEPClose(&hidp->epout);
 	}
 #endif
 	hidp->state = USBHHID_STATE_ACTIVE;
-	osalSysUnlock();
+}
+
+void usbhhidStop(USBHHIDDriver *hidp) {
+	chSemWait(&hidp->sem);
+	_stop_locked(hidp);
+	chSemSignal(&hidp->sem);
 }
 
 usbh_urbstatus_t usbhhidGetReport(USBHHIDDriver *hidp,
@@ -305,17 +320,18 @@ usbh_urbstatus_t usbhhidSetProtocol(USBHHIDDriver *hidp, uint8_t protocol) {
 			protocol, hidp->ifnum, 0, NULL);
 }
 
-void usbhhidObjectInit(USBHHIDDriver *hidp) {
+static void _hid_object_init(USBHHIDDriver *hidp) {
 	osalDbgCheck(hidp != NULL);
 	memset(hidp, 0, sizeof(*hidp));
 	hidp->info = &usbhhidClassDriverInfo;
 	hidp->state = USBHHID_STATE_STOP;
+	chSemObjectInit(&hidp->sem, 1);
 }
 
-void usbhhidInit(void) {
+static void _hid_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHHID_MAX_INSTANCES; i++) {
-		usbhhidObjectInit(&USBHHIDD[i]);
+		_hid_object_init(&USBHHIDD[i]);
 	}
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_hub.c b/os/hal/src/usbh/hal_usbh_hub.c
index 3a84175..6a83c66 100644
--- a/os/hal/src/usbh/hal_usbh_hub.c
+++ b/os/hal/src/usbh/hal_usbh_hub.c
@@ -63,14 +63,17 @@
 USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];
 static usbh_port_t USBHPorts[HAL_USBHHUB_MAX_PORTS];
 
-static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
-static void hub_unload(usbh_baseclassdriver_t *drv);
+static void _hub_init(void);
+static usbh_baseclassdriver_t *_hub_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void _hub_unload(usbh_baseclassdriver_t *drv);
 static const usbh_classdriver_vmt_t usbhhubClassDriverVMT = {
-	hub_load,
-	hub_unload
+	_hub_init,
+	_hub_load,
+	_hub_unload
 };
+
 const usbh_classdriverinfo_t usbhhubClassDriverInfo = {
-	0x09, 0x00, -1, "HUB", &usbhhubClassDriverVMT
+	"HUB", &usbhhubClassDriverVMT
 };
 
 
@@ -104,7 +107,7 @@ static void _urb_complete(usbh_urb_t *urb) {
 	case USBH_URBSTATUS_TIMEOUT:
 		/* the device NAKed */
 		udbg("HUB: no info");
-		hubdp->statuschange = 0;
+		//hubdp->statuschange = 0;
 		break;
 	case USBH_URBSTATUS_OK: {
 		uint8_t len = hubdp->hubDesc.bNbrPorts / 8 + 1;
@@ -137,16 +140,14 @@ static void _urb_complete(usbh_urb_t *urb) {
 	usbhURBSubmitI(urb);
 }
 
-static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,
+static usbh_baseclassdriver_t *_hub_load(usbh_device_t *dev,
 		const uint8_t *descriptor, uint16_t rem) {
 	int i;
 
 	USBHHubDriver *hubdp;
 
-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_DEVICE))
-		return NULL;
-
-	if (dev->devDesc.bDeviceProtocol != 0)
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_DEVICE,
+			0x09, 0x00, 0x00) != HAL_SUCCESS)
 		return NULL;
 
 	generic_iterator_t iep, icfg;
@@ -158,12 +159,10 @@ static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,
 	if_iter_init(&iif, &icfg);
 	if (!iif.valid)
 		return NULL;
-	const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
-	if ((ifdesc->bInterfaceClass != 0x09)
-		|| (ifdesc->bInterfaceSubClass != 0x00)
-		|| (ifdesc->bInterfaceProtocol != 0x00)) {
+
+	if (_usbh_match_descriptor(iif.curr, iif.rem, USBH_DT_INTERFACE,
+			0x09, 0x00, 0x00) != HAL_SUCCESS)
 		return NULL;
-	}
 
 	ep_iter_init(&iep, &iif);
 	if (!iep.valid)
@@ -253,22 +252,18 @@ alloc_ok:
 			_urb_complete, hubdp, hubdp->scbuff,
 			(hubdesc->bNbrPorts + 8) / 8);
 
-	osalSysLock();
-	usbhURBSubmitI(&hubdp->urb);
-	osalOsRescheduleS();
-	osalSysUnlock();
+	usbhURBSubmit(&hubdp->urb);
 
+	hubdp->dev = NULL;
 	return (usbh_baseclassdriver_t *)hubdp;
 }
 
-static void hub_unload(usbh_baseclassdriver_t *drv) {
+static void _hub_unload(usbh_baseclassdriver_t *drv) {
 	osalDbgCheck(drv != NULL);
 	USBHHubDriver *const hubdp = (USBHHubDriver *)drv;
 
 	/* close the status change endpoint (this cancels ongoing URBs) */
-	osalSysLock();
-	usbhEPCloseS(&hubdp->epint);
-	osalSysUnlock();
+	usbhEPClose(&hubdp->epint);
 
 	/* de-alloc ports and unload drivers */
 	usbh_port_t *port = hubdp->ports;
@@ -283,16 +278,16 @@ static void hub_unload(usbh_baseclassdriver_t *drv) {
 
 }
 
-void usbhhubObjectInit(USBHHubDriver *hubdp) {
+static void _object_init(USBHHubDriver *hubdp) {
 	osalDbgCheck(hubdp != NULL);
 	memset(hubdp, 0, sizeof(*hubdp));
 	hubdp->info = &usbhhubClassDriverInfo;
 }
 
-void usbhhubInit(void) {
+static void _hub_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {
-		usbhhubObjectInit(&USBHHUBD[i]);
+		_object_init(&USBHHUBD[i]);
 	}
 }
 
diff --git a/os/hal/src/usbh/hal_usbh_msd.c b/os/hal/src/usbh/hal_usbh_msd.c
index b564e8e..069c47b 100644
--- a/os/hal/src/usbh/hal_usbh_msd.c
+++ b/os/hal/src/usbh/hal_usbh_msd.c
@@ -27,9 +27,6 @@
 #include "usbh/dev/msd.h"
 #include "usbh/internal.h"
 
-//#pragma GCC optimize("Og")
-
-
 #if USBHMSD_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
 #define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
@@ -62,26 +59,39 @@
 #define uerr(f, ...)   do {} while(0)
 #endif
 
-
-
-
+static void _lun_object_deinit(USBHMassStorageLUNDriver *lunp);
 
 /*===========================================================================*/
 /* USB Class driver loader for MSD                                           */
 /*===========================================================================*/
 
-USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];
+struct USBHMassStorageDriver {
+	/* inherited from abstract class driver */
+	_usbh_base_classdriver_data
 
+	usbh_ep_t epin;
+	usbh_ep_t epout;
+	uint8_t ifnum;
+	uint8_t max_lun;
+	uint32_t tag;
+
+	USBHMassStorageLUNDriver *luns;
+};
+
+static USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];
+
+static void _msd_init(void);
 static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
 static void _msd_unload(usbh_baseclassdriver_t *drv);
 
 static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_msd_init,
 	_msd_load,
 	_msd_unload
 };
 
 const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {
-	0x08, 0x06, 0x50, "MSD", &class_driver_vmt
+	"MSD", &class_driver_vmt
 };
 
 #define MSD_REQ_RESET							0xFF
@@ -93,15 +103,14 @@ static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *desc
 	uint8_t luns;
 	usbh_urbstatus_t stat;
 
-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE,
+			0x08, 0x06, 0x50) != HAL_SUCCESS)
 		return NULL;
 
 	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
 
 	if ((ifdesc->bAlternateSetting != 0)
-			|| (ifdesc->bNumEndpoints < 2)
-			|| (ifdesc->bInterfaceSubClass != 0x06)
-			|| (ifdesc->bInterfaceProtocol != 0x50)) {
+			|| (ifdesc->bNumEndpoints < 2)) {
 		return NULL;
 	}
 
@@ -187,15 +196,7 @@ alloc_ok:
 			MSBLKD[i].next = msdp->luns;
 			msdp->luns = &MSBLKD[i];
 			MSBLKD[i].msdp = msdp;
-
-			osalSysLock();
-			MSBLKD[i].state = BLK_ACTIVE;	/* transition directly to active, instead of BLK_STOP */
-			osalSysUnlock();
-
-			/* connect the LUN (TODO: review if it's best to leave the LUN disconnected) */
-			msdp->dev = dev;
-			usbhmsdLUNConnect(&MSBLKD[i]);
-			msdp->dev = NULL;
+			MSBLKD[i].state = BLK_ACTIVE;
 			luns--;
 		}
 	}
@@ -212,25 +213,15 @@ static void _msd_unload(usbh_baseclassdriver_t *drv) {
 	USBHMassStorageDriver *const msdp = (USBHMassStorageDriver *)drv;
 	USBHMassStorageLUNDriver *lunp = msdp->luns;
 
-	osalMutexLock(&msdp->mtx);
-	osalSysLock();
-	usbhEPCloseS(&msdp->epin);
-	usbhEPCloseS(&msdp->epout);
+	/* disconnect all LUNs */
 	while (lunp) {
-		lunp->state = BLK_STOP;
+		usbhmsdLUNDisconnect(lunp);
+		_lun_object_deinit(lunp);
 		lunp = lunp->next;
 	}
-	osalSysUnlock();
-	osalMutexUnlock(&msdp->mtx);
 
-	/* now that the LUNs are idle, deinit them */
-	lunp = msdp->luns;
-	osalSysLock();
-	while (lunp) {
-		usbhmsdLUNObjectInit(lunp);
-		lunp = lunp->next;
-	}
-	osalSysUnlock();
+	usbhEPClose(&msdp->epin);
+	usbhEPClose(&msdp->epout);
 }
 
 
@@ -238,8 +229,6 @@ static void _msd_unload(usbh_baseclassdriver_t *drv) {
 /* MSD Class driver operations (Bulk-Only transport)                         */
 /*===========================================================================*/
 
-
-
 /* USB Bulk Only Transport SCSI Command block wrapper */
 typedef PACKED_STRUCT {
 	uint32_t dCBWSignature;
@@ -704,11 +693,22 @@ static const struct USBHMassStorageDriverVMT blk_vmt = {
 	(bool (*)(void *, BlockDeviceInfo *))usbhmsdLUNGetInfo
 };
 
-void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {
+static void _lun_object_deinit(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	chSemWait(&lunp->sem);
+	lunp->msdp = NULL;
+	lunp->next = NULL;
+	memset(&lunp->info, 0, sizeof(lunp->info));
+	lunp->state = BLK_STOP;
+	chSemSignal(&lunp->sem);
+}
+
+static void _lun_object_init(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp != NULL);
 	memset(lunp, 0, sizeof(*lunp));
 	lunp->vmt = &blk_vmt;
 	lunp->state = BLK_STOP;
+	chSemObjectInit(&lunp->sem, 1);
 	/* Unnecessary because of the memset:
 		lunp->msdp = NULL;
 		lunp->next = NULL;
@@ -716,45 +716,18 @@ void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {
 	*/
 }
 
-void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp) {
-	osalDbgCheck(lunp != NULL);
-	osalSysLock();
-	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),
-			"invalid state");
-	//TODO: complete
-	//lunp->state = BLK_ACTIVE;
-	osalSysUnlock();
-}
-
-void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp) {
-	osalDbgCheck(lunp != NULL);
-	osalSysLock();
-	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),
-			"invalid state");
-	//TODO: complete
-	//lunp->state = BLK_STOP;
-	osalSysUnlock();
-}
-
 bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
-	USBHMassStorageDriver *const msdp = lunp->msdp;
+	osalDbgCheck(lunp != NULL);
+	osalDbgCheck(lunp->msdp != NULL);
 	msd_result_t res;
 
-	osalDbgCheck(msdp != NULL);
-	osalSysLock();
-	//osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),
-    //            "invalid state");
+	chSemWait(&lunp->sem);
+	osalDbgAssert((lunp->state == BLK_READY) || (lunp->state == BLK_ACTIVE), "invalid state");
 	if (lunp->state == BLK_READY) {
-	    osalSysUnlock();
+		chSemSignal(&lunp->sem);
 		return HAL_SUCCESS;
-	} else if (lunp->state != BLK_ACTIVE) {
-		osalSysUnlock();
-		return HAL_FAILED;
 	}
 	lunp->state = BLK_CONNECTING;
-    osalSysUnlock();
-
-    osalMutexLock(&msdp->mtx);
 
     {
 		USBH_DEFINE_BUFFER(scsi_inquiry_response_t inq);
@@ -820,41 +793,34 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 		(uint32_t)(((uint64_t)lunp->info.blk_size * lunp->info.blk_num) / (1024UL * 1024UL)));
 
 	uinfo("MSD Connected.");
-
-	osalMutexUnlock(&msdp->mtx);
-	osalSysLock();
 	lunp->state = BLK_READY;
-    osalSysUnlock();
-
+	chSemSignal(&lunp->sem);
 	return HAL_SUCCESS;
 
   /* Connection failed, state reset to BLK_ACTIVE.*/
 failed:
-	osalMutexUnlock(&msdp->mtx);
-	osalSysLock();
+	uinfo("MSD Connect failed.");
 	lunp->state = BLK_ACTIVE;
-    osalSysUnlock();
+	chSemSignal(&lunp->sem);
 	return HAL_FAILED;
 }
 
-
 bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp != NULL);
-	osalSysLock();
-	osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),
-				"invalid state");
+
+	chSemWait(&lunp->sem);
+	osalDbgAssert((lunp->state == BLK_READY) || (lunp->state == BLK_ACTIVE), "invalid state");
 	if (lunp->state == BLK_ACTIVE) {
-		osalSysUnlock();
+		chSemSignal(&lunp->sem);
 		return HAL_SUCCESS;
 	}
 	lunp->state = BLK_DISCONNECTING;
-	osalSysUnlock();
 
-	//TODO: complete
+	//TODO: complete: sync, etc.
 
-	osalSysLock();
 	lunp->state = BLK_ACTIVE;
-	osalSysUnlock();
+	chSemSignal(&lunp->sem);
+
 	return HAL_SUCCESS;
 }
 
@@ -867,15 +833,13 @@ bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	msd_result_t res;
 	uint32_t actual_len;
 
-	osalSysLock();
+	chSemWait(&lunp->sem);
 	if (lunp->state != BLK_READY) {
-		osalSysUnlock();
+		chSemSignal(&lunp->sem);
 		return ret;
 	}
 	lunp->state = BLK_READING;
-    osalSysUnlock();
 
-	osalMutexLock(&lunp->msdp->mtx);
 	while (n) {
 		if (n > 0xffff) {
 			blocks = 0xffff;
@@ -900,15 +864,8 @@ bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	ret = HAL_SUCCESS;
 
 exit:
-	osalMutexUnlock(&lunp->msdp->mtx);
-	osalSysLock();
-	if (lunp->state == BLK_READING) {
-		lunp->state = BLK_READY;
-	} else {
-		osalDbgCheck(lunp->state == BLK_STOP);
-		uwarn("MSD: State = BLK_STOP");
-	}
-    osalSysUnlock();
+	lunp->state = BLK_READY;
+	chSemSignal(&lunp->sem);
 	return ret;
 }
 
@@ -921,15 +878,13 @@ bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	msd_result_t res;
 	uint32_t actual_len;
 
-	osalSysLock();
+	chSemWait(&lunp->sem);
 	if (lunp->state != BLK_READY) {
-		osalSysUnlock();
+		chSemSignal(&lunp->sem);
 		return ret;
 	}
 	lunp->state = BLK_WRITING;
-    osalSysUnlock();
 
-	osalMutexLock(&lunp->msdp->mtx);
 	while (n) {
 		if (n > 0xffff) {
 			blocks = 0xffff;
@@ -954,15 +909,8 @@ bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
 	ret = HAL_SUCCESS;
 
 exit:
-	osalMutexUnlock(&lunp->msdp->mtx);
-	osalSysLock();
-	if (lunp->state == BLK_WRITING) {
-		lunp->state = BLK_READY;
-	} else {
-		osalDbgCheck(lunp->state == BLK_STOP);
-		uwarn("MSD: State = BLK_STOP");
-	}
-	osalSysUnlock();
+	lunp->state = BLK_READY;
+	chSemSignal(&lunp->sem);
 	return ret;
 }
 
@@ -976,38 +924,41 @@ bool usbhmsdLUNSync(USBHMassStorageLUNDriver *lunp) {
 bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip) {
 	osalDbgCheck(lunp != NULL);
 	osalDbgCheck(bdip != NULL);
-	*bdip = lunp->info;
-	return HAL_SUCCESS;
+
+	osalSysLock();
+	if (lunp->state >= BLK_READY) {
+		*bdip = lunp->info;
+		osalSysUnlock();
+		return HAL_SUCCESS;
+	}
+	osalSysUnlock();
+	return HAL_FAILED;
 }
 
 bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp != NULL);
-	blkstate_t state;
-	osalSysLock();
-	state = lunp->state;
-	osalSysUnlock();
-	return (state >= BLK_ACTIVE);
+	return (lunp->state >= BLK_ACTIVE);
 }
 
 bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp != NULL);
+	//TODO: Implement
 	return FALSE;
 }
 
-void usbhmsdObjectInit(USBHMassStorageDriver *msdp) {
+static void _msd_object_init(USBHMassStorageDriver *msdp) {
 	osalDbgCheck(msdp != NULL);
 	memset(msdp, 0, sizeof(*msdp));
 	msdp->info = &usbhmsdClassDriverInfo;
-	osalMutexObjectInit(&msdp->mtx);
 }
 
-void usbhmsdInit(void) {
+static void _msd_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHMSD_MAX_INSTANCES; i++) {
-		usbhmsdObjectInit(&USBHMSD[i]);
+		_msd_object_init(&USBHMSD[i]);
 	}
 	for (i = 0; i < HAL_USBHMSD_MAX_LUNS; i++) {
-		usbhmsdLUNObjectInit(&MSBLKD[i]);
+		_lun_object_init(&MSBLKD[i]);
 	}
 }
 #endif
diff --git a/os/hal/src/usbh/hal_usbh_uvc.c b/os/hal/src/usbh/hal_usbh_uvc.c
index b0507a0..a795cd8 100644
--- a/os/hal/src/usbh/hal_usbh_uvc.c
+++ b/os/hal/src/usbh/hal_usbh_uvc.c
@@ -27,9 +27,9 @@
 #error "USBHUVC needs HAL_USBH_USE_IAD"
 #endif
 
+#include <string.h>
 #include "usbh/dev/uvc.h"
 #include "usbh/internal.h"
-#include <string.h>
 
 #if USBHUVC_DEBUG_ENABLE_TRACE
 #define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
@@ -66,17 +66,18 @@
 
 USBHUVCDriver USBHUVCD[HAL_USBHUVC_MAX_INSTANCES];
 
-
-static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,
+static void _uvc_init(void);
+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);
+static void _uvc_unload(usbh_baseclassdriver_t *drv);
 
 static const usbh_classdriver_vmt_t class_driver_vmt = {
-	uvc_load,
-	uvc_unload
+	_uvc_init,
+	_uvc_load,
+	_uvc_unload
 };
 const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {
-	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt
+	"UVC", &class_driver_vmt
 };
 
 static bool _request(USBHUVCDriver *uvcdp,
@@ -360,10 +361,8 @@ bool usbhuvcStreamStart(USBHUVCDriver *uvcdp, uint16_t min_ep_sz) {
 		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();
+
+	usbhURBSubmit(&uvcdp->urb_iso);
 
 	ret = HAL_SUCCESS;
 	goto exit;
@@ -512,22 +511,13 @@ uint32_t usbhuvcEstimateRequiredEPSize(USBHUVCDriver *uvcdp, const uint8_t *form
 	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) {
+static usbh_baseclassdriver_t *_uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
 
 	USBHUVCDriver *uvcdp;
 	uint8_t i;
 
-	if (descriptor[1] != USBH_DT_INTERFACE_ASSOCIATION)
+	if (_usbh_match_descriptor(descriptor, rem, USBH_DT_INTERFACE_ASSOCIATION,
+			0x0e, 0x03, 0x00) != HAL_SUCCESS)
 		return NULL;
 
 	/* alloc driver */
@@ -703,14 +693,14 @@ alloc_ok:
 	osalSysLock();
 	usbhURBSubmitI(&uvcdp->urb_int);
 	uvcdp->state = USBHUVC_STATE_ACTIVE;
-	osalOsRescheduleS();
+	osalOsRescheduleS();	/* because of usbhURBSubmitI */
 	osalSysUnlock();
 
 	dev->keepFullCfgDesc++;
 	return (usbh_baseclassdriver_t *)uvcdp;
 }
 
-static void uvc_unload(usbh_baseclassdriver_t *drv) {
+static void _uvc_unload(usbh_baseclassdriver_t *drv) {
 	USBHUVCDriver *const uvcdp = (USBHUVCDriver *)drv;
 
 	usbhuvcStreamStop(uvcdp);
@@ -727,10 +717,19 @@ static void uvc_unload(usbh_baseclassdriver_t *drv) {
 	osalSysUnlock();
 }
 
-void usbhuvcInit(void) {
+static void _object_init(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 void _uvc_init(void) {
 	uint8_t i;
 	for (i = 0; i < HAL_USBHUVC_MAX_INSTANCES; i++) {
-		usbhuvcObjectInit(&USBHUVCD[i]);
+		_object_init(&USBHUVCD[i]);
 	}
 }
 
diff --git a/testhal/STM32/STM32F4xx/USB_HOST/Makefile b/testhal/STM32/STM32F4xx/USB_HOST/Makefile
index 0174baf..33d032a 100644
--- a/testhal/STM32/STM32F4xx/USB_HOST/Makefile
+++ b/testhal/STM32/STM32F4xx/USB_HOST/Makefile
@@ -120,7 +120,7 @@ CSRC = $(STARTUPSRC) \
        $(FATFSSRC) \

        $(STREAMSSRC) \

        $(SHELLSRC) \

-       main.c

+       main.c usbh_custom_class_example.c

 

 # C++ sources that can be compiled in ARM or THUMB mode depending on the global

 # setting.

diff --git a/testhal/STM32/STM32F4xx/USB_HOST/halconf_community.h b/testhal/STM32/STM32F4xx/USB_HOST/halconf_community.h
index 51e9a8d..9b22a79 100644
--- a/testhal/STM32/STM32F4xx/USB_HOST/halconf_community.h
+++ b/testhal/STM32/STM32F4xx/USB_HOST/halconf_community.h
@@ -89,6 +89,7 @@
 #define HAL_USBH_PORT_DEBOUNCE_TIME                   200

 #define HAL_USBH_PORT_RESET_TIMEOUT                   500

 #define HAL_USBH_DEVICE_ADDRESS_STABILIZATION         20

+#define HAL_USBH_CONTROL_REQUEST_DEFAULT_TIMEOUT	  MS2ST(1000)

 

 /* MSD */

 #define HAL_USBH_USE_MSD                              TRUE

@@ -141,6 +142,8 @@
 #define HAL_USBHHUB_MAX_INSTANCES                     1

 #define HAL_USBHHUB_MAX_PORTS                         6

 

+#define HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS		  TRUE

+

 /* debug */

 #define USBH_DEBUG_ENABLE                             TRUE

 #define USBH_DEBUG_USBHD                              USBHD1

diff --git a/testhal/STM32/STM32F4xx/USB_HOST/main.c b/testhal/STM32/STM32F4xx/USB_HOST/main.c
index 9652da3..595e454 100644
--- a/testhal/STM32/STM32F4xx/USB_HOST/main.c
+++ b/testhal/STM32/STM32F4xx/USB_HOST/main.c
@@ -18,8 +18,10 @@
 #include "hal.h"

 #include "ff.h"

 #include <string.h>

+#include "usbh/debug.h"		/* for usbDbgPuts/usbDbgPrintf */

 

-#define UVC_TO_MSD_PHOTOS_CAPTURE	TRUE

+

+#define UVC_TO_MSD_PHOTOS_CAPTURE	FALSE

 

 

 #if HAL_USBH_USE_FTDI || HAL_USBH_USE_AOA

@@ -343,7 +345,7 @@ static void ThreadTestMSD(void *p) {
     FATFS *fsp;

     DWORD clusters;

     FRESULT res;

-    blkstate_t state;

+

 #if !UVC_TO_MSD_PHOTOS_CAPTURE

     BaseSequentialStream * const chp = (BaseSequentialStream *)&USBH_DEBUG_SD;

     systime_t st, et;

@@ -354,11 +356,15 @@ start:
     for(;;) {

         chThdSleepMilliseconds(100);

 

-        chSysLock();

-        state = blkGetDriverState(&MSBLKD[0]);

-        chSysUnlock();

-        if (state != BLK_READY)

+        if (blkGetDriverState(&MSBLKD[0]) == BLK_ACTIVE) {

+        	usbDbgPuts("BLK: Active, connect....");

+        	usbhmsdLUNConnect(&MSBLKD[0]);

+        }

+        if (blkGetDriverState(&MSBLKD[0]) != BLK_READY) {

             continue;

+    	}

+

+    	usbDbgPuts("BLK: Ready.");

 

 #if !UVC_TO_MSD_PHOTOS_CAPTURE

         //raw read test

@@ -371,7 +377,8 @@ start:
             usbDbgPrintf("BLK: Raw read test (%dMB, %dB blocks)", RAW_READ_SZ_MB, sizeof(fbuff));

             st = chVTGetSystemTime();

             for (j = 0; j < NITERATIONS; j++) {

-                blkRead(&MSBLKD[0], start, fbuff, NBLOCKS);

+                if (blkRead(&MSBLKD[0], start, fbuff, NBLOCKS) != HAL_SUCCESS)

+                    goto start;

                 start += NBLOCKS;

             }

             et = chVTGetSystemTime();

@@ -914,6 +921,7 @@ int main(void) {
 

     //turn on USB power

     palClearPad(GPIOC, GPIOC_OTG_FS_POWER_ON);

+    chThdSleepMilliseconds(100);

 

     //start

 #if STM32_USBH_USE_OTG1

diff --git a/testhal/STM32/STM32F4xx/USB_HOST/usbh_additional_class_drivers.h b/testhal/STM32/STM32F4xx/USB_HOST/usbh_additional_class_drivers.h
new file mode 100644
index 0000000..ac9fc18
--- /dev/null
+++ b/testhal/STM32/STM32F4xx/USB_HOST/usbh_additional_class_drivers.h
@@ -0,0 +1,38 @@
+/*
+    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.
+*/
+
+#ifndef USBH_ADDITIONAL_H_
+#define USBH_ADDITIONAL_H_
+
+#include "hal_usbh.h"
+
+#if HAL_USE_USBH && HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
+
+/* Declarations */
+extern const usbh_classdriverinfo_t usbhCustomClassDriverInfo;
+
+
+
+/* Comma separated list of additional class drivers */
+#define HAL_USBH_ADDITIONAL_CLASS_DRIVERS	\
+	&usbhCustomClassDriverInfo,
+
+
+
+#endif
+
+#endif /* USBH_ADDITIONAL_H_ */
diff --git a/testhal/STM32/STM32F4xx/USB_HOST/usbh_custom_class_example.c b/testhal/STM32/STM32F4xx/USB_HOST/usbh_custom_class_example.c
new file mode 100644
index 0000000..4585a5c
--- /dev/null
+++ b/testhal/STM32/STM32F4xx/USB_HOST/usbh_custom_class_example.c
@@ -0,0 +1,144 @@
+/*
+    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.
+*/
+
+#include "hal.h"
+
+#if HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
+
+#include <string.h>
+#include "usbh_custom_class_example.h"
+#include "usbh/internal.h"
+
+#if USBH_DEBUG_ENABLE_TRACE
+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define udbgf(f, ...)  do {} while(0)
+#define udbg(f, ...)   do {} while(0)
+#endif
+
+#if USBH_DEBUG_ENABLE_INFO
+#define uinfof(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uinfo(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uinfof(f, ...)  do {} while(0)
+#define uinfo(f, ...)   do {} while(0)
+#endif
+
+#if USBH_DEBUG_ENABLE_WARNINGS
+#define uwarnf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uwarn(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uwarnf(f, ...)  do {} while(0)
+#define uwarn(f, ...)   do {} while(0)
+#endif
+
+#if USBH_DEBUG_ENABLE_ERRORS
+#define uerrf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define uerr(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#else
+#define uerrf(f, ...)  do {} while(0)
+#define uerr(f, ...)   do {} while(0)
+#endif
+
+/*===========================================================================*/
+/* USB Class driver loader for Custom Class Example				 		 	 */
+/*===========================================================================*/
+
+USBHCustomDriver USBHCUSTOMD[USBH_CUSTOM_CLASS_MAX_INSTANCES];
+
+static void _init(void);
+static usbh_baseclassdriver_t *_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void _unload(usbh_baseclassdriver_t *drv);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_init,
+	_load,
+	_unload
+};
+
+const usbh_classdriverinfo_t usbhCustomClassDriverInfo = {
+	"CUSTOM", &class_driver_vmt
+};
+
+static usbh_baseclassdriver_t *_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	int i;
+	USBHCustomDriver *custp;
+	(void)dev;
+
+	if (_usbh_match_vid_pid(dev, 0xABCD, 0x0123) != HAL_SUCCESS)
+		return NULL;
+
+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
+
+	/* alloc driver */
+	for (i = 0; i < USBH_CUSTOM_CLASS_MAX_INSTANCES; i++) {
+		if (USBHCUSTOMD[i].dev == NULL) {
+			custp = &USBHCUSTOMD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("Can't alloc CUSTOM driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	custp->ifnum = ifdesc->bInterfaceNumber;
+
+	/* parse the configuration descriptor */
+	if_iterator_t iif;
+	generic_iterator_t iep;
+	iif.iad = 0;
+	iif.curr = descriptor;
+	iif.rem = rem;
+	for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+		const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+		if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_INT)) {
+			/* ... */
+		} else {
+			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+					epdesc->bEndpointAddress, epdesc->bmAttributes);
+		}
+	}
+
+	custp->state = USBHCUSTOM_STATE_ACTIVE;
+
+	return (usbh_baseclassdriver_t *)custp;
+
+}
+
+static void _unload(usbh_baseclassdriver_t *drv) {
+	(void)drv;
+}
+
+static void _object_init(USBHCustomDriver *custp) {
+	osalDbgCheck(custp != NULL);
+	memset(custp, 0, sizeof(*custp));
+	custp->state = USBHCUSTOM_STATE_STOP;
+}
+
+static void _init(void) {
+	uint8_t i;
+	for (i = 0; i < USBH_CUSTOM_CLASS_MAX_INSTANCES; i++) {
+		_object_init(&USBHCUSTOMD[i]);
+	}
+}
+
+#endif
diff --git a/testhal/STM32/STM32F4xx/USB_HOST/usbh_custom_class_example.h b/testhal/STM32/STM32F4xx/USB_HOST/usbh_custom_class_example.h
new file mode 100644
index 0000000..3f00fe7
--- /dev/null
+++ b/testhal/STM32/STM32F4xx/USB_HOST/usbh_custom_class_example.h
@@ -0,0 +1,80 @@
+/*
+    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.
+*/
+
+#ifndef USBH_CUSTOM_H_
+#define USBH_CUSTOM_H_
+
+#include "hal_usbh.h"
+
+#if HAL_USE_USBH && HAL_USBH_USE_ADDITIONAL_CLASS_DRIVERS
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+#define USBH_CUSTOM_CLASS_MAX_INSTANCES		1
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+typedef enum {
+	USBHCUSTOM_STATE_UNINIT = 0,
+	USBHCUSTOM_STATE_STOP = 1,
+	USBHCUSTOM_STATE_ACTIVE = 2,
+	USBHCUSTOM_STATE_READY = 3
+} usbhcustom_state_t;
+
+typedef struct USBHCustomDriver USBHCustomDriver;
+
+struct USBHCustomDriver {
+	/* inherited from abstract class driver */
+	_usbh_base_classdriver_data
+
+	uint8_t ifnum;
+
+	usbhcustom_state_t state;
+};
+
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+extern USBHCustomDriver USBHCUSTOMD[USBH_CUSTOM_CLASS_MAX_INSTANCES];
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+	/* API goes here */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+#endif /* USBH_CUSTOM_H_ */