Merge pull request #199 from dismirlian/usbh

USBH: improvements

23 files changed, 949 insertions, 1239 deletions

diff --git a/os/hal/hal.mk b/os/hal/hal.mk
index cd8420b..ae27ea5 100644
--- a/os/hal/hal.mk
+++ b/os/hal/hal.mk
@@ -28,10 +28,8 @@ endif
 ifneq ($(findstring HAL_USE_USBH TRUE,$(HALCONF)),)

 HALSRC_CONTRIB += ${CHIBIOS_CONTRIB}/os/hal/src/hal_usbh.c \

                   ${CHIBIOS_CONTRIB}/os/hal/src/usbh/hal_usbh_debug.c \

-                  ${CHIBIOS_CONTRIB}/os/hal/src/usbh/hal_usbh_desciter.c

-endif

-ifneq ($(findstring HAL_USBH_USE_HUB TRUE,$(HALCONF)),)

-HALSRC_CONTRIB += ${CHIBIOS_CONTRIB}/os/hal/src/usbh/hal_usbh_hub.c

+                  ${CHIBIOS_CONTRIB}/os/hal/src/usbh/hal_usbh_desciter.c \

+                  ${CHIBIOS_CONTRIB}/os/hal/src/usbh/hal_usbh_hub.c

 endif

 ifneq ($(findstring HAL_USBH_USE_MSD TRUE,$(HALCONF)),)

 HALSRC_CONTRIB += ${CHIBIOS_CONTRIB}/os/hal/src/usbh/hal_usbh_msd.c

diff --git a/os/hal/include/hal_usbh.h b/os/hal/include/hal_usbh.h
index aa49205..6e83a44 100644
--- a/os/hal/include/hal_usbh.h
+++ b/os/hal/include/hal_usbh.h
@@ -242,6 +242,31 @@ struct usbh_port {
 	_usbh_port_ll_data
 };
 
+
+#if USBH_DEBUG_ENABLE
+struct usbh_dq {
+	int rem;
+	int sz;
+	uint8_t *first;
+	uint8_t *next;
+	uint8_t *start;
+	uint8_t *end;
+};
+
+typedef struct usbh_dq usbh_dq_t;
+
+struct usbh_debug_helper {
+	uint8_t buff[USBH_DEBUG_BUFFER];
+	THD_WORKING_AREA(thd_wa, 512);
+	usbh_dq_t dq;
+	systime_t first;
+	systime_t last;
+	bool ena;
+	bool on;
+	thread_reference_t tr;
+};
+#endif
+
 struct USBHDriver {
 	usbh_status_t status;
 	uint8_t address_bitmap[(USBH_MAX_ADDRESSES + 7) / 8];
@@ -255,15 +280,16 @@ struct USBHDriver {
 	/* Low level part */
 	_usbhdriver_ll_data
 
-#if USBH_DEBUG_ENABLE
+#if USBH_DEBUG_ENABLE && USBH_DEBUG_MULTI_HOST
 	/* debug */
-	uint8_t dbg_buff[USBH_DEBUG_BUFFER];
-	THD_WORKING_AREA(waDebug, 512);
-	input_queue_t iq;
+	struct usbh_debug_helper debug;
 #endif
 };
 
-
+#if USBH_DEBUG_ENABLE && !USBH_DEBUG_MULTI_HOST
+/* debug */
+extern struct usbh_debug_helper usbh_debug;
+#endif
 
 /*===========================================================================*/
 /* External declarations.                                                    */
@@ -289,10 +315,10 @@ extern "C" {
 	/* Device-related */
 #if	USBH_DEBUG_ENABLE && USBH_DEBUG_ENABLE_INFO
 	void usbhDevicePrintInfo(usbh_device_t *dev);
-	void usbhDevicePrintConfiguration(const uint8_t *descriptor, uint16_t rem);
+	void usbhDevicePrintConfiguration(const usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
 #else
 #	define usbhDevicePrintInfo(dev) do {} while(0)
-#	define usbhDevicePrintConfiguration(descriptor, rem) do {} while(0)
+#	define usbhDevicePrintConfiguration(dev, descriptor, rem) do {} while(0)
 #endif
 	bool usbhDeviceReadString(usbh_device_t *dev, char *dest, uint8_t size,
 			uint8_t index, uint16_t langID);
diff --git a/os/hal/include/usbh/debug.h b/os/hal/include/usbh/debug.h
index 41319b9..f90a11a 100644
--- a/os/hal/include/usbh/debug.h
+++ b/os/hal/include/usbh/debug.h
@@ -24,18 +24,53 @@
 #if HAL_USE_USBH

 

 #if USBH_DEBUG_ENABLE

+#if USBH_DEBUG_MULTI_HOST

+	/* output callback */

+	void USBH_DEBUG_OUTPUT_CALLBACK(USBHDriver *host, const uint8_t *buff, size_t len);

+

+	/* printing functions */

+	void usbDbgPrintf(USBHDriver *host, const char *fmt, ...);

+	void usbDbgPuts(USBHDriver *host, const char *s);

+	void usbDbgInit(USBHDriver *host);

+#else

+	/* output callback */

+	void USBH_DEBUG_OUTPUT_CALLBACK(const uint8_t *buff, size_t len);

+

+	/* printing functions */

 	void usbDbgPrintf(const char *fmt, ...);

 	void usbDbgPuts(const char *s);

-	void usbDbgInit(USBHDriver *host);

+	void usbDbgInit(void);

+#endif

+

 	void usbDbgReset(void);

-	void usbDbgSystemHalted(void);

 #else

-#define usbDbgPrintf(fmt, ...) do {} while(0)

-#define usbDbgPuts(s) do {} while(0)

-#define usbDbgInit(host) do {} while(0)

-#define usbDbgReset() do {} while(0)

-#define usbDbgSystemHalted() do {} while(0)

+

+#if USBH_DEBUG_MULTI_HOST

+#	define usbDbgPrintf(host, fmt, ...) do {} while(0)

+#	define usbDbgPuts(host, s) do {} while(0)

+#	define usbDbgInit(host) do {} while(0)

+#else

+#	define usbDbgPrintf(fmt, ...) do {} while(0)

+#	define usbDbgPuts(s) do {} while(0)

+#	define usbDbgInit() do {} while(0)

+#endif

+#	define usbDbgReset() do {} while(0)

+#endif

+

+#if USBH_DEBUG_MULTI_HOST

+#define _usbh_dbg(host, s) usbDbgPuts(host, s)

+#define _usbh_dbgf(host, f, ...) usbDbgPrintf(host, f, ##__VA_ARGS__)

+#define _usbh_ldbg(host, lvl, n, s) do {if (lvl >= n) usbDbgPuts(host, s); } while(0)

+#define _usbh_ldbgf(host, lvl, n, f, ...) do {if (lvl >= n) usbDbgPrintf(host, f, ##__VA_ARGS__); } while(0)

+#else

+

+#define _usbh_dbg(host, s) usbDbgPuts(s)

+#define _usbh_dbgf(host, f, ...) usbDbgPrintf(f, ##__VA_ARGS__)

+#define _usbh_ldbg(host, lvl, n, s) do {if (lvl >= n) usbDbgPuts(s); } while(0)

+#define _usbh_ldbgf(host, lvl, n, f, ...) do {if (lvl >= n) usbDbgPrintf(f, ##__VA_ARGS__); } while(0)

+

 #endif

+
 

 #endif

 

diff --git a/os/hal/include/usbh/debug_helpers.h b/os/hal/include/usbh/debug_helpers.h
new file mode 100644
index 0000000..b09bd0a
--- /dev/null
+++ b/os/hal/include/usbh/debug_helpers.h
@@ -0,0 +1,163 @@
+#ifndef USBH_INTERNAL_H_

+#error "Internal use only"

+#endif

+

+#ifndef _USBH_DEBUG_HELPER_ENABLE_TRACE

+#error "_USBH_DEBUG_HELPER_ENABLE_TRACE must be set"

+#endif

+#ifndef _USBH_DEBUG_HELPER_ENABLE_INFO

+#error "_USBH_DEBUG_HELPER_ENABLE_INFO must be set"

+#endif

+#ifndef _USBH_DEBUG_HELPER_ENABLE_WARNINGS

+#error "_USBH_DEBUG_HELPER_ENABLE_WARNINGS must be set"

+#endif

+#ifndef _USBH_DEBUG_HELPER_ENABLE_ERRORS

+#error "_USBH_DEBUG_HELPER_ENABLE_ERRORS must be set"

+#endif

+

+#define _usbh_dbg_host(s)  _usbh_dbg(host, s)

+#define _usbh_dbg_port(s) _usbh_dbg(port->device.host, s)

+#define _usbh_dbg_dev(s) _usbh_dbg(dev->host, s)

+#define _usbh_dbg_ep(lvl, s) _usbh_ldbg(ep->device->host, ep->trace_level, lvl, s)

+#define _usbh_dbg_urb(lvl, s) _usbh_ldbg(urb->ep->device->host, urb->ep->trace_level, lvl, s)

+

+#define _usbh_dbgf_host(f, ...) _usbh_dbgf(host, f, ##__VA_ARGS__)

+#define _usbh_dbgf_port(f, ...) _usbh_dbgf(port->device.host, f, ##__VA_ARGS__)

+#define _usbh_dbgf_dev(f, ...)  _usbh_dbgf(dev->host, f, ##__VA_ARGS__)

+#define _usbh_dbgf_ep(f, lvl, ...) _usbh_ldbgf(ep->device->host, ep->trace_level, lvl, "\t%s: " f, ep->name, ##__VA_ARGS__)

+#define _usbh_dbgf_urb(f, lvl, ...) _usbh_ldbgf(urb->ep->device->host, urb->ep->trace_level, lvl, "\t%s: " f, urb->ep->name, ##__VA_ARGS__)

+

+#if defined(_USBH_DEBUG_HELPER_CLASS_DRIVER)

+#define _usbh_dbg_classdrv(drv, s) _usbh_dbg(drv->dev->host, s)

+#define _usbh_dbgf_classdrv(drv, f, ...)  _usbh_dbgf(drv->dev->host, f, ##__VA_ARGS__)

+#endif

+

+#define _usbh_dbg_dummy	do {} while(0)

+

+#if _USBH_DEBUG_HELPER_ENABLE_TRACE

+#define udbg(s) _usbh_dbg_host(s)

+#define uportdbg(s) _usbh_dbg_port(s)

+#define udevdbg(s) _usbh_dbg_dev(s)

+#define uepdbg(s) _usbh_dbg_ep(4, s)

+#define uurbdbg(s) _usbh_dbg_urb(4, s)

+#define udbgf(f, ...) _usbh_dbgf_host(f, ##__VA_ARGS__)

+#define uportdbgf(f, ...) _usbh_dbgf_port(f, ##__VA_ARGS__)

+#define udevdbgf(f, ...) _usbh_dbgf_dev(f, ##__VA_ARGS__)

+#define uepdbgf(f, ...) _usbh_dbgf_ep(f, 4, ##__VA_ARGS__)

+#define uurbdbgf(f, ...) _usbh_dbgf_urb(f, 4, ##__VA_ARGS__)

+#else

+#define udbg(s) _usbh_dbg_dummy

+#define uportdbg(s) _usbh_dbg_dummy

+#define udevdbg(s) _usbh_dbg_dummy

+#define uurbdbg(s) _usbh_dbg_dummy

+#define uepdbg(s) _usbh_dbg_dummy

+#define udbgf(f, ...) _usbh_dbg_dummy

+#define uportdbgf(f, ...) _usbh_dbg_dummy

+#define udevdbgf(f, ...) _usbh_dbg_dummy

+#define uepdbgf(f, ...) _usbh_dbg_dummy

+#define uurbdbgf(f, ...) _usbh_dbg_dummy

+#endif

+

+#if _USBH_DEBUG_HELPER_ENABLE_INFO

+#define uinfo(s) _usbh_dbg_host(s)

+#define uportinfo(s) _usbh_dbg_port(s)

+#define udevinfo(s) _usbh_dbg_dev(s)

+#define uepinfo(s) _usbh_dbg_ep(3, s)

+#define uurbinfo(s) _usbh_dbg_urb(3, s)

+#define uinfof(f, ...) _usbh_dbgf_host(f, ##__VA_ARGS__)

+#define uportinfof(f, ...) _usbh_dbgf_port(f, ##__VA_ARGS__)

+#define udevinfof(f, ...) _usbh_dbgf_dev(f, ##__VA_ARGS__)

+#define uepinfof(f, ...) _usbh_dbgf_ep(f, 3, ##__VA_ARGS__)

+#define uurbinfof(f, ...) _usbh_dbgf_urb(f, 3, ##__VA_ARGS__)

+#else

+#define uinfo(s) _usbh_dbg_dummy

+#define udevinfo(s) _usbh_dbg_dummy

+#define uportinfo(s) _usbh_dbg_dummy

+#define uepinfo(s) _usbh_dbg_dummy

+#define uurbinfo(s) _usbh_dbg_dummy

+#define uinfof(f, ...) _usbh_dbg_dummy

+#define uportinfof(f, ...) _usbh_dbg_dummy

+#define udevinfof(f, ...) _usbh_dbg_dummy

+#define uepinfof(f, ...) _usbh_dbg_dummy

+#define uurbinfof(f, ...) _usbh_dbg_dummy

+#endif

+

+#if _USBH_DEBUG_HELPER_ENABLE_WARNINGS

+#define uwarn(s) _usbh_dbg_host(s)

+#define uportwarn(s) _usbh_dbg_port(s)

+#define udevwarn(s) _usbh_dbg_dev(s)

+#define uepwarn(s) _usbh_dbg_ep(3, s)

+#define uurbwarn(s) _usbh_dbg_urb(3, s)

+#define uwarnf(f, ...) _usbh_dbgf_host(f, ##__VA_ARGS__)

+#define uportwarnf(f, ...) _usbh_dbgf_port(f, ##__VA_ARGS__)

+#define udevwarnf(f, ...) _usbh_dbgf_dev(f, ##__VA_ARGS__)

+#define uepwarnf(f, ...) _usbh_dbgf_ep(f, 3, ##__VA_ARGS__)

+#define uurbwarnf(f, ...) _usbh_dbgf_urb(f, 3, ##__VA_ARGS__)

+#else

+#define uwarn(s) _usbh_dbg_dummy

+#define udevwarn(s) _usbh_dbg_dummy

+#define uportwarn(s) _usbh_dbg_dummy

+#define uepwarn(s) _usbh_dbg_dummy

+#define uurbwarn(s) _usbh_dbg_dummy

+#define uwarnf(f, ...) _usbh_dbg_dummy

+#define uportwarnf(f, ...) _usbh_dbg_dummy

+#define udevwarnf(f, ...) _usbh_dbg_dummy

+#define uepwarnf(f, ...) _usbh_dbg_dummy

+#define uurbwarnf(f, ...) _usbh_dbg_dummy

+#endif

+

+

+#if _USBH_DEBUG_HELPER_ENABLE_ERRORS

+#define uerr(s) _usbh_dbg_host(s)

+#define uporterr(s) _usbh_dbg_port(s)

+#define udeverr(s) _usbh_dbg_dev(s)

+#define ueperr(s) _usbh_dbg_ep(3, s)

+#define uurberr(s) _usbh_dbg_urb(3, s)

+#define uerrf(f, ...) _usbh_dbgf_host(f, ##__VA_ARGS__)

+#define uporterrf(f, ...) _usbh_dbgf_port(f, ##__VA_ARGS__)

+#define udeverrf(f, ...) _usbh_dbgf_dev(f, ##__VA_ARGS__)

+#define ueperrf(f, ...) _usbh_dbgf_ep(f, 3, ##__VA_ARGS__)

+#define uurberrf(f, ...) _usbh_dbgf_urb(f, 3, ##__VA_ARGS__)

+#else

+#define uerr(s) _usbh_dbg_dummy

+#define udeverr(s) _usbh_dbg_dummy

+#define uporterr(s) _usbh_dbg_dummy

+#define ueperr(s) _usbh_dbg_dummy

+#define uurberr(s) _usbh_dbg_dummy

+#define uerrf(f, ...) _usbh_dbg_dummy

+#define uporterrf(f, ...) _usbh_dbg_dummy

+#define udeverrf(f, ...) _usbh_dbg_dummy

+#define ueperrf(f, ...) _usbh_dbg_dummy

+#define uurberrf(f, ...) _usbh_dbg_dummy

+#endif

+

+#if defined(_USBH_DEBUG_HELPER_CLASS_DRIVER)

+#if _USBH_DEBUG_HELPER_ENABLE_TRACE

+#define uclassdrvdbg(s) _usbh_dbg_classdrv(_USBH_DEBUG_HELPER_CLASS_DRIVER, s)

+#define uclassdrvdbgf(f, ...) _usbh_dbgf_classdrv(_USBH_DEBUG_HELPER_CLASS_DRIVER, f, ##__VA_ARGS__)

+#else

+#define uclassdrvdbg(s) _usbh_dbg_dummy

+#define uclassdrvdbgf(f, ...) _usbh_dbg_dummy

+#endif

+#if _USBH_DEBUG_HELPER_ENABLE_INFO

+#define uclassdrvinfo(s) _usbh_dbg_classdrv(_USBH_DEBUG_HELPER_CLASS_DRIVER, s)

+#define uclassdrvinfof(f, ...) _usbh_dbgf_classdrv(_USBH_DEBUG_HELPER_CLASS_DRIVER, f, ##__VA_ARGS__)

+#else

+#define uclassdrvinfo(s) _usbh_dbg_dummy

+#define uclassdrvinfof(f, ...) _usbh_dbg_dummy

+#endif

+#if _USBH_DEBUG_HELPER_ENABLE_WARNINGS

+#define uclassdrvwarn(s) _usbh_dbg_classdrv(_USBH_DEBUG_HELPER_CLASS_DRIVER, s)

+#define uclassdrvwarnf(f, ...) _usbh_dbgf_classdrv(_USBH_DEBUG_HELPER_CLASS_DRIVER, f, ##__VA_ARGS__)

+#else

+#define uclassdrvwarn(s) _usbh_dbg_dummy

+#define uclassdrvwarnf(f, ...) _usbh_dbg_dummy

+#endif

+#if _USBH_DEBUG_HELPER_ENABLE_ERRORS

+#define uclassdrverr(s) _usbh_dbg_classdrv(_USBH_DEBUG_HELPER_CLASS_DRIVER, s)

+#define uclassdrverrf(f, ...) _usbh_dbgf_classdrv(_USBH_DEBUG_HELPER_CLASS_DRIVER, f, ##__VA_ARGS__)

+#else

+#define uclassdrverr(s) _usbh_dbg_dummy

+#define uclassdrverrf(f, ...) _usbh_dbg_dummy

+#endif

+#endif

diff --git a/os/hal/include/usbh/dev/aoa.h b/os/hal/include/usbh/dev/aoa.h
index 8205dd5..1f36370 100644
--- a/os/hal/include/usbh/dev/aoa.h
+++ b/os/hal/include/usbh/dev/aoa.h
@@ -130,6 +130,7 @@ typedef bool (*usbhaoa_filter_callback_t)(usbh_device_t *dev, const uint8_t *des
 #define usbhaoaGetState(aoap) ((aoap)->state)
 
 #define usbhaoaGetChannelState(aoap) ((aoap)->channel.state)
+#define usbhaoaGetHost(aoap) ((aoap)->dev->host)
 
 /*===========================================================================*/
 /* External declarations.                                                    */
diff --git a/os/hal/include/usbh/dev/ftdi.h b/os/hal/include/usbh/dev/ftdi.h
index 84ea4bc..fdd767d 100644
--- a/os/hal/include/usbh/dev/ftdi.h
+++ b/os/hal/include/usbh/dev/ftdi.h
@@ -127,7 +127,7 @@ struct USBHFTDIDriver {
 /* Driver macros.                                                            */

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

 #define usbhftdipGetState(ftdipp) ((ftdipp)->state)

-

+#define usbhftdipGetHost(ftdipp) ((ftdipp)->ftdip->dev->host)

 

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

 /* External declarations.                                                    */

diff --git a/os/hal/include/usbh/dev/msd.h b/os/hal/include/usbh/dev/msd.h
index b245bfc..113f523 100644
--- a/os/hal/include/usbh/dev/msd.h
+++ b/os/hal/include/usbh/dev/msd.h
@@ -72,7 +72,6 @@ struct USBHMassStorageLUNDriver {
 /* Driver macros.                                                            */

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

 

-

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

 /* External declarations.                                                    */

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

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

 	bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp);

 	bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp);

+

+	USBHDriver *usbhmsdLUNGetHost(const USBHMassStorageLUNDriver *lunp);

 #ifdef __cplusplus

 }

 #endif

diff --git a/os/hal/include/usbh/dev/uvc.h b/os/hal/include/usbh/dev/uvc.h
index 713d16c..7116c91 100644
--- a/os/hal/include/usbh/dev/uvc.h
+++ b/os/hal/include/usbh/dev/uvc.h
@@ -408,9 +408,9 @@ extern "C" {
 			const uint8_t *framedesc, uint32_t dwFrameInterval);
 
 #if	USBH_DEBUG_ENABLE && USBHUVC_DEBUG_ENABLE_INFO
-	void usbhuvcPrintProbeCommit(const usbh_uvc_ctrl_vs_probecommit_data_t *pc);
+	void usbhuvcPrintProbeCommit(USBHUVCDriver *uvcdp, const usbh_uvc_ctrl_vs_probecommit_data_t *pc);
 #else
-#	define usbhuvcPrintProbeCommit(pc) do {} while(0)
+#	define usbhuvcPrintProbeCommit(uvcdp, pc) do {} while(0)
 #endif
 	bool usbhuvcProbe(USBHUVCDriver *uvcdp);
 	bool usbhuvcCommit(USBHUVCDriver *uvcdp);
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 787a831..2066b60 100644
--- a/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.c
+++ b/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.c
@@ -67,38 +67,11 @@
 #endif
 #endif
 
-
-#if USBH_LLD_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_LLD_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_LLD_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_LLD_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
+#define _USBH_DEBUG_HELPER_ENABLE_TRACE		USBH_LLD_DEBUG_ENABLE_TRACE
+#define _USBH_DEBUG_HELPER_ENABLE_INFO		USBH_LLD_DEBUG_ENABLE_INFO
+#define _USBH_DEBUG_HELPER_ENABLE_WARNINGS	USBH_LLD_DEBUG_ENABLE_WARNINGS
+#define _USBH_DEBUG_HELPER_ENABLE_ERRORS	USBH_LLD_DEBUG_ENABLE_ERRORS
+#include "usbh/debug_helpers.h"
 
 static void _transfer_completedI(usbh_ep_t *ep, usbh_urb_t *urb, usbh_urbstatus_t status);
 static void _try_commit_np(USBHDriver *host);
@@ -161,7 +134,10 @@ static void _halt_channel(USBHDriver *host, stm32_hc_management_t *hcm, usbh_lld
 	(void)host;
 
 	if (hcm->halt_reason != USBH_LLD_HALTREASON_NONE) {
-		uwarnf("\t%s: Repeated halt (original=%d, new=%d)", hcm->ep->name, hcm->halt_reason, reason);
+#if USBH_DEBUG_ENABLE && USBH_LLD_DEBUG_ENABLE_WARNINGS
+		usbh_ep_t *const ep = hcm->ep;
+		uepwarnf("Repeated halt (original=%d, new=%d)", hcm->halt_reason, reason);
+#endif
 		return;
 	}
 
@@ -178,17 +154,20 @@ static void _halt_channel(USBHDriver *host, stm32_hc_management_t *hcm, usbh_lld
 }
 
 static void _release_channel(USBHDriver *host, stm32_hc_management_t *hcm) {
-//	static const char *reason[] =  {"XFRC",	"XFRC",	"NAK", "STALL",	"ERROR", "ABORT"};
-//	udbgf("\t%s: release (%s)", hcm->ep->name, reason[hcm->halt_reason]);
+	usbh_ep_t *const ep = hcm->ep;
+#if USBH_DEBUG_ENABLE && USBH_LLD_DEBUG_ENABLE_TRACE
+	static const char *reason[] =  {"XFRC",	"XFRC",	"NAK", "STALL",	"ERROR", "ABORT"};
+	uepdbgf("release (%s)", reason[hcm->halt_reason]);
+#endif
 	hcm->hc->HCINTMSK = 0;
 	host->otg->HAINTMSK &= ~hcm->haintmsk;
 	hcm->halt_reason = USBH_LLD_HALTREASON_NONE;
-	if (usbhEPIsPeriodic(hcm->ep)) {
+	if (usbhEPIsPeriodic(ep)) {
 		list_add(&hcm->node, &host->ch_free[0]);
 	} else {
 		list_add(&hcm->node, &host->ch_free[1]);
 	}
-	hcm->ep->xfer.hcm = 0;
+	ep->xfer.hcm = 0;
 	hcm->ep = 0;
 }
 
@@ -207,12 +186,12 @@ static bool _activate_ep(USBHDriver *host, usbh_ep_t *ep) {
 	}
 
 	if (list_empty(list)) {
-		uwarnf("\t%s: No free %s channels", ep->name, usbhEPIsPeriodic(ep) ? "P" : "NP");
+		uepwarnf("No free %s channels", usbhEPIsPeriodic(ep) ? "P" : "NP");
 		return FALSE;
 	}
 
 	if (spc <= STM32_USBH_MIN_QSPACE) {
-		uwarnf("\t%s: No space in %s Queue (spc=%d)", ep->name, usbhEPIsPeriodic(ep) ? "P" : "NP", spc);
+		uepwarnf("No space in %s Queue (spc=%d)", usbhEPIsPeriodic(ep) ? "P" : "NP", spc);
 		return FALSE;
 	}
 
@@ -332,15 +311,15 @@ static bool _activate_ep(USBHDriver *host, usbh_ep_t *ep) {
 		if (--spc > STM32_USBH_MIN_QSPACE) {
 			hc->HCCHAR |= HCCHAR_CHENA;
 		} else {
-			uwarnf("\t%s: Could not queue back-to-back packets", ep->name);
+			uepwarnf("Could not queue back-to-back packets");
 		}
 	}
 
 	if (urb->queued == FALSE) {
 		urb->queued = TRUE;
-		udbgf("\t%s: Start (%dB)", ep->name, xfer_len);
+		uepdbgf("Start (%dB)", xfer_len);
 	} else {
-		udbgf("\t%s: Restart (%dB)", ep->name, xfer_len);
+		uepdbgf("Restart (%dB)", xfer_len);
 	}
 
 	ep->xfer.len = xfer_len;
@@ -387,7 +366,7 @@ static bool _update_urb(usbh_ep_t *ep, uint32_t hctsiz, usbh_urb_t *urb, bool co
 	osalDbgAssert(urb->actualLength + len <= urb->requestedLength, "what happened?");
 #else
 	if (urb->actualLength + len > urb->requestedLength) {
-		uerrf("\t%s: Trimming actualLength %u -> %u", ep->name, urb->actualLength + len, urb->requestedLength);
+		ueperrf("Trimming actualLength %u -> %u", urb->actualLength + len, urb->requestedLength);
 		urb->actualLength = urb->requestedLength;
 		return TRUE;
 	}
@@ -449,9 +428,9 @@ static void _purge_queue(USBHDriver *host, struct list_head *list) {
 	list_for_each_entry_safe(ep, usbh_ep_t, tmp, list, node) {
 		usbh_urb_t *const urb = _active_urb(ep);
 		stm32_hc_management_t *const hcm = ep->xfer.hcm;
-		uwarnf("\t%s: Abort URB, USBH_URBSTATUS_DISCONNECTED", ep->name);
+		uepwarnf("Abort URB, USBH_URBSTATUS_DISCONNECTED");
 		if (hcm) {
-			uwarnf("\t%s: URB had channel %d assigned, halt_reason = %d", ep->name, hcm - host->channels, hcm->halt_reason);
+			uepwarnf("URB had channel %d assigned, halt_reason = %d", hcm - host->channels, hcm->halt_reason);
 			_release_channel(host, hcm);
 			_update_urb(ep, hcm->hc->HCTSIZ, urb, FALSE);
 		}
@@ -514,7 +493,7 @@ static uint32_t _write_packet(struct list_head *list, uint32_t space_available)
 
 		volatile uint32_t *dest = ep->xfer.hcm->fifo;
 		uint32_t *src = (uint32_t *)ep->xfer.buf;
-		udbgf("\t%s: write %d words (%dB), partial=%d", ep->name, words, written, ep->xfer.partial);
+		uepdbgf("write %d words (%dB), partial=%d", words, written, ep->xfer.partial);
 		while (words--) {
 			*dest = *src++;
 		}
@@ -589,19 +568,19 @@ 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);
+	uepinfof("Open EP");
 	ep->status = USBH_EPSTATUS_OPEN;
 }
 
 void usbh_lld_ep_close(usbh_ep_t *ep) {
 	usbh_urb_t *urb;
-	uinfof("\t%s: Closing EP...", ep->name);
+	uepinfof("Closing EP...");
 	while (!list_empty(&ep->urb_list)) {
 		urb = list_first_entry(&ep->urb_list, usbh_urb_t, node);
-		uinfof("\t%s: Abort URB, USBH_URBSTATUS_DISCONNECTED", ep->name);
+		uepinfof("Abort URB, USBH_URBSTATUS_DISCONNECTED");
 		_usbh_urb_abort_and_waitS(urb, USBH_URBSTATUS_DISCONNECTED);
 	}
-	uinfof("\t%s: Closed", ep->name);
+	uepinfof("Closed");
 	ep->status = USBH_EPSTATUS_CLOSED;
 }
 
@@ -615,7 +594,7 @@ void usbh_lld_urb_submit(usbh_urb_t *urb) {
 	USBHDriver *const host = ep->device->host;
 
 	if (!(host->otg->HPRT & HPRT_PENA)) {
-		uwarnf("\t%s: Can't submit URB, port disabled", ep->name);
+		uepwarnf("Can't submit URB, port disabled");
 		_usbh_urb_completeI(urb, USBH_URBSTATUS_DISCONNECTED);
 		return;
 	}
@@ -651,20 +630,20 @@ bool usbh_lld_urb_abort(usbh_urb_t *urb, usbh_urbstatus_t status) {
 
 		if (hcm->halt_reason == USBH_LLD_HALTREASON_NONE) {
 			/* The channel is not being halted */
-			uinfof("\t%s: usbh_lld_urb_abort: channel is not being halted", hcm->ep->name);
+			uepinfof("usbh_lld_urb_abort: channel is not being halted");
 			urb->status = status;
 			_halt_channel(ep->device->host, hcm, USBH_LLD_HALTREASON_ABORT);
 		} else {
 			/* The channel is being halted, so we can't re-halt it. The CHH interrupt will
 			 * be in charge of completing the transfer, but the URB will not have the specified status.
 			 */
-			uinfof("\t%s: usbh_lld_urb_abort: channel is being halted", hcm->ep->name);
+			uepinfof("usbh_lld_urb_abort: channel is being halted");
 		}
 		return FALSE;
 	}
 
 	/* This URB is inactive, we can cancel it now */
-	uinfof("\t%s: usbh_lld_urb_abort: URB is not active", ep->name);
+	uepinfof("usbh_lld_urb_abort: URB is not active");
 	_transfer_completedI(ep, urb, status);
 
 	return TRUE;
@@ -679,94 +658,103 @@ bool usbh_lld_urb_abort(usbh_urb_t *urb, usbh_urbstatus_t status) {
 //	si			si			si			si			si			si			no			no		ep->type != ISO && !ep->in
 static inline void _ack_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {
 	(void)host;
-	osalDbgAssert(hcm->ep->type != USBH_EPTYPE_ISO, "ACK should not happen in ISO endpoints");
-	hcm->ep->xfer.error_count = 0;
+	usbh_ep_t *const ep = hcm->ep;
+	osalDbgAssert(ep->type != USBH_EPTYPE_ISO, "ACK should not happen in ISO endpoints");
+	ep->xfer.error_count = 0;
 	hc->HCINTMSK &= ~HCINTMSK_ACKM;
-	udbgf("\t%s: ACK", hcm->ep->name);
+	uepdbgf("ACK");
 }
 
 //CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)
 //	si			no			si			no			si			no			no			no		ep->type != ISO && ep->in
 static inline void _dterr_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {
 	(void)host;
-	osalDbgAssert(hcm->ep->in && (hcm->ep->type != USBH_EPTYPE_ISO), "DTERR should not happen in OUT or ISO endpoints");
+	usbh_ep_t *const ep = hcm->ep;
+	osalDbgAssert(ep->in && (ep->type != USBH_EPTYPE_ISO), "DTERR should not happen in OUT or ISO endpoints");
 #if 0
 	hc->HCINTMSK &= ~(HCINTMSK_DTERRM | HCINTMSK_ACKM);
-	hcm->ep->xfer.error_count = 0;
+	ep->xfer.error_count = 0;
 	_halt_channel(host, hcm, USBH_LLD_HALTREASON_ERROR);
 #else
 	/* restart directly, no need to halt it in this case */
-	hcm->ep->xfer.error_count = 0;
+	ep->xfer.error_count = 0;
 	hc->HCINTMSK &= ~HCINTMSK_ACKM;
 	hc->HCCHAR |= HCCHAR_CHENA;
 #endif
-	uerrf("\t%s: DTERR", hcm->ep->name);
+	ueperrf("DTERR");
 }
 
 //CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)
 //	si			no			si			no			si			no			si			no		ep->in
 static inline void _bberr_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {
-	osalDbgAssert(hcm->ep->in, "BBERR should not happen in OUT endpoints");
+	usbh_ep_t *const ep = hcm->ep;
+	osalDbgAssert(ep->in, "BBERR should not happen in OUT endpoints");
 	hc->HCINTMSK &= ~HCINTMSK_BBERRM;
-	hcm->ep->xfer.error_count = 3;
+	ep->xfer.error_count = 3;
 	_halt_channel(host, hcm, USBH_LLD_HALTREASON_ERROR);
-	uerrf("\t%s: BBERR", hcm->ep->name);
+	ueperrf("BBERR");
 }
 
 ///CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)
 //	si			si			si			si			si			si			si			no		ep->type != ISO || ep->in
 static inline void _trerr_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {
-	osalDbgAssert(hcm->ep->in || (hcm->ep->type != USBH_EPTYPE_ISO), "TRERR should not happen in ISO OUT endpoints");
+	usbh_ep_t *const ep = hcm->ep;
+	osalDbgAssert(ep->in || (ep->type != USBH_EPTYPE_ISO), "TRERR should not happen in ISO OUT endpoints");
 	hc->HCINTMSK &= ~HCINTMSK_TRERRM;
-	++hcm->ep->xfer.error_count;
+	++ep->xfer.error_count;
 	_halt_channel(host, hcm, USBH_LLD_HALTREASON_ERROR);
-	uerrf("\t%s: TRERR", hcm->ep->name);
+	ueperrf("TRERR");
 }
 
 //CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)
 //	no			no			si			si			no			no			si			si		ep->type = PERIODIC
 static inline void _frmor_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {
-	osalDbgAssert(usbhEPIsPeriodic(hcm->ep), "FRMOR should not happen in non-periodic endpoints");
+	usbh_ep_t *const ep = hcm->ep;
+	osalDbgAssert(usbhEPIsPeriodic(ep), "FRMOR should not happen in non-periodic endpoints");
 	hc->HCINTMSK &= ~HCINTMSK_FRMORM;
-	hcm->ep->xfer.error_count = 3;
+	ep->xfer.error_count = 3;
 	_halt_channel(host, hcm, USBH_LLD_HALTREASON_ERROR);
-	uerrf("\t%s: FRMOR", hcm->ep->name);
+	ueperrf("FRMOR");
 }
 
 //CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)
 //	si			si			si			si			si			si			no			no		ep->type != ISO
 static inline void _nak_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {
+	usbh_ep_t *const ep = hcm->ep;
 	osalDbgAssert(hcm->ep->type != USBH_EPTYPE_ISO, "NAK should not happen in ISO endpoints");
-	if (!hcm->ep->in || (hcm->ep->type == USBH_EPTYPE_INT)) {
+	if (!ep->in || (ep->type == USBH_EPTYPE_INT)) {
 		hc->HCINTMSK &= ~HCINTMSK_NAKM;
 		_halt_channel(host, hcm, USBH_LLD_HALTREASON_NAK);
 	} else {
 		/* restart directly, no need to halt it in this case */
-		hcm->ep->xfer.error_count = 0;
+		ep->xfer.error_count = 0;
 		hc->HCINTMSK &= ~HCINTMSK_ACKM;
 		hc->HCCHAR |= HCCHAR_CHENA;
 	}
-	udbgf("\t%s: NAK", hcm->ep->name);
+	uepdbgf("NAK");
 }
 
 //CTRL(IN)	CTRL(OUT)	INT(IN)		INT(OUT)	BULK(IN)	BULK(OUT)	ISO(IN)		ISO(OUT)
-//	si		sólo DAT/STAT	si			si			si			si			no			no		ep->type != ISO && (ep->type != CTRL || ctrlphase != SETUP)
+//	si		solo DAT/STAT	si			si			si			si			no			no		ep->type != ISO && (ep->type != CTRL || ctrlphase != SETUP)
 static inline void _stall_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_otg_host_chn_t *hc) {
 	osalDbgAssert(hcm->ep->type != USBH_EPTYPE_ISO, "STALL should not happen in ISO endpoints");
 	hc->HCINTMSK &= ~HCINTMSK_STALLM;
 	_halt_channel(host, hcm, USBH_LLD_HALTREASON_STALL);
-	uwarnf("\t%s: STALL", hcm->ep->name);
+#if USBH_DEBUG_ENABLE && USBH_LLD_DEBUG_ENABLE_WARNINGS
+	usbh_ep_t *const ep = hcm->ep;
+	uepwarnf("STALL");
+#endif
 }
 
 static void _complete_bulk_int(USBHDriver *host, stm32_hc_management_t *hcm, usbh_ep_t *ep, usbh_urb_t *urb, uint32_t hctsiz) {
 	_release_channel(host, hcm);
 	_save_dt_mask(ep, hctsiz);
 	if (_update_urb(ep, hctsiz, urb, TRUE)) {
-		udbgf("\t%s: done", ep->name);
+		uepdbgf("done");
 		_transfer_completedI(ep, urb, USBH_URBSTATUS_OK);
 	} else {
 		osalDbgCheck(urb->requestedLength > 0x7FFFF);
-		uwarnf("\t%s: incomplete", ep->name);
+		uepwarnf("incomplete");
 		_move_to_pending_queue(ep);
 	}
 	if (usbhEPIsPeriodic(ep)) {
@@ -782,18 +770,18 @@ static void _complete_control(USBHDriver *host, stm32_hc_management_t *hcm, usbh
 	_release_channel(host, hcm);
 	if (ep->xfer.u.ctrl_phase == USBH_LLD_CTRLPHASE_DATA) {
 		if (_update_urb(ep, hctsiz, urb, TRUE)) {
-			udbgf("\t%s: DATA done", ep->name);
+			uepdbgf("DATA done");
 			ep->xfer.u.ctrl_phase = USBH_LLD_CTRLPHASE_STATUS;
 			ep->in = !ep->in;
 		} else {
 			osalDbgCheck(urb->requestedLength > 0x7FFFF);
-			uwarnf("\t%s: DATA incomplete", ep->name);
+			uepwarnf("DATA incomplete");
 			_save_dt_mask(ep, hctsiz);
 		}
 		_move_to_pending_queue(ep);
 	} else {
 		osalDbgCheck(ep->xfer.u.ctrl_phase == USBH_LLD_CTRLPHASE_STATUS);
-		udbgf("\t%s: STATUS done", ep->name);
+		uepdbgf("STATUS done");
 		_transfer_completedI(ep, urb, USBH_URBSTATUS_OK);
 	}
 	_try_commit_np(host);
@@ -802,13 +790,13 @@ static void _complete_control(USBHDriver *host, stm32_hc_management_t *hcm, usbh
 static void _complete_control_setup(USBHDriver *host, stm32_hc_management_t *hcm, usbh_ep_t *ep, usbh_urb_t *urb) {
 	_release_channel(host, hcm);
 	if (urb->requestedLength) {
-		udbgf("\t%s: SETUP done -> DATA", ep->name);
+		uepdbgf("SETUP done -> DATA");
 		ep->xfer.u.ctrl_phase = USBH_LLD_CTRLPHASE_DATA;
 		ep->in = *((uint8_t *)urb->setup_buff) & 0x80 ? TRUE : FALSE;
 		ep->dt_mask = HCTSIZ_DPID_DATA1;
 		ep->xfer.error_count = 0;
 	} else {
-		udbgf("\t%s: SETUP done -> STATUS", ep->name);
+		uepdbgf("SETUP done -> STATUS");
 		ep->in = TRUE;
 		ep->xfer.u.ctrl_phase = USBH_LLD_CTRLPHASE_STATUS;
 	}
@@ -817,7 +805,7 @@ static void _complete_control_setup(USBHDriver *host, stm32_hc_management_t *hcm
 }
 
 static void _complete_iso(USBHDriver *host, stm32_hc_management_t *hcm, usbh_ep_t *ep, usbh_urb_t *urb, uint32_t hctsiz) {
-	udbgf("\t%s: done", hcm->ep->name);
+	uepdbgf("done");
 	_release_channel(host, hcm);
 	_update_urb(ep, hctsiz, urb, TRUE);
 	_transfer_completedI(ep, urb, USBH_URBSTATUS_OK);
@@ -915,7 +903,7 @@ static inline void _chh_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_
 		case USBH_LLD_HALTREASON_STALL:
 			if (ep->type == USBH_EPTYPE_CTRL) {
 				if (ep->xfer.u.ctrl_phase == USBH_LLD_CTRLPHASE_SETUP) {
-					uerrf("\t%s: Faulty device: STALLed SETUP phase", ep->name);
+					ueperrf("Faulty device: STALLed SETUP phase");
 				}
 			} else {
 				ep->status = USBH_EPSTATUS_HALTED;
@@ -927,13 +915,13 @@ static inline void _chh_int(USBHDriver *host, stm32_hc_management_t *hcm, stm32_
 			if ((ep->type == USBH_EPTYPE_ISO) || done || (ep->xfer.error_count >= 3)) {
 				_transfer_completedI(ep, urb, USBH_URBSTATUS_ERROR);
 			} else {
-				uerrf("\t%s: err=%d, done=%d, retry", ep->name, ep->xfer.error_count, done);
+				ueperrf("err=%d, done=%d, retry", ep->xfer.error_count, done);
 				_move_to_pending_queue(ep);
 			}
 			break;
 
 		case USBH_LLD_HALTREASON_ABORT:
-			uwarnf("\t%s: Abort", ep->name);
+			uepwarnf("Abort");
 			_transfer_completedI(ep, urb, urb->status);
 			break;
 
@@ -1094,8 +1082,7 @@ static inline void _rxflvl_int(USBHDriver *host) {
 				hcm->hc->HCCHAR |= HCCHAR_CHENA;
 			}
 
-			udbgf("\t%s: RXFLVL rx=%dB, rem=%dB (%dpkts)",
-					ep->name,
+			uepdbgf("RXFLVL rx=%dB, rem=%dB (%dpkts)",
 					(grxstsp & GRXSTSP_BCNT_MASK) >> 4,
 					(hctsiz & HCTSIZ_XFRSIZ_MASK),
 					(hctsiz & HCTSIZ_PKTCNT_MASK) >> 19);
@@ -1131,7 +1118,7 @@ static inline void _rxflvl_int(USBHDriver *host) {
 				uint32_t pkt = (hctsiz & HCTSIZ_PKTCNT_MASK) >> 19;
 				uint32_t siz = (hctsiz & HCTSIZ_XFRSIZ_MASK);
 				if (pkt * ep->wMaxPacketSize != siz) {
-					uerrf("\t%s: whatttt???", ep->name);
+					ueperrf("whatttt???");
 				}
 			}
 #endif
@@ -1479,13 +1466,13 @@ void usbh_lld_init(void) {
 #endif
 }
 
-static void _usbh_start(USBHDriver *usbh) {
-	stm32_otg_t *const otgp = usbh->otg;
+static void _usbh_start(USBHDriver *host) {
+	stm32_otg_t *const otgp = host->otg;
 
 	/* Clock activation.*/
 #if STM32_USBH_USE_OTG1
 #if STM32_USBH_USE_OTG2
-	if (&USBHD1 == usbh)
+	if (&USBHD1 == host)
 #endif
 	{
 		/* OTG FS clock enable and reset.*/
@@ -1501,7 +1488,7 @@ static void _usbh_start(USBHDriver *usbh) {
 
 #if STM32_USBH_USE_OTG2
 #if STM32_USBH_USE_OTG1
-	if (&USBHD2 == usbh)
+	if (&USBHD2 == host)
 #endif
 	{
 		/* OTG HS clock enable and reset.*/
@@ -1518,7 +1505,7 @@ static void _usbh_start(USBHDriver *usbh) {
 
 	otgp->GUSBCFG = GUSBCFG_PHYSEL | GUSBCFG_TRDT(5);
 
-	otg_core_reset(usbh);
+	otg_core_reset(host);
 
 	otgp->GCCFG = GCCFG_PWRDWN;
 
@@ -1553,28 +1540,28 @@ static void _usbh_start(USBHDriver *usbh) {
 
 	otgp->HPRT |= HPRT_PPWR;
 
-	otg_txfifo_flush(usbh, 0x10);
-	otg_rxfifo_flush(usbh);
+	otg_txfifo_flush(host, 0x10);
+	otg_rxfifo_flush(host);
 
 	otgp->GINTSTS = 0xffffffff;
 	otgp->GINTMSK = GINTMSK_DISCM | GINTMSK_HPRTM | GINTMSK_MMISM;
 
-	usbh->rootport.lld_status = USBH_PORTSTATUS_POWER;
-	usbh->rootport.lld_c_status = 0;
+	host->rootport.lld_status = USBH_PORTSTATUS_POWER;
+	host->rootport.lld_c_status = 0;
 
 	/* Global interrupts enable.*/
 	otgp->GAHBCFG |= GAHBCFG_GINTMSK;
 }
 
-void usbh_lld_start(USBHDriver *usbh) {
-	if (usbh->status != USBH_STATUS_STOPPED) return;
-	_usbh_start(usbh);
+void usbh_lld_start(USBHDriver *host) {
+	if (host->status != USBH_STATUS_STOPPED) return;
+	_usbh_start(host);
 }
 
 /*===========================================================================*/
 /* Root Hub request handler.                                                 */
 /*===========================================================================*/
-usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestType, uint8_t bRequest,
+usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *host, uint8_t bmRequestType, uint8_t bRequest,
 		uint16_t wvalue, uint16_t windex, uint16_t wlength, uint8_t *buf) {
 
 	uint16_t typereq = (bmRequestType << 8) | bRequest;
@@ -1606,23 +1593,23 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 			break;
 
 		case USBH_PORT_FEAT_C_CONNECTION:
-			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
+			host->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
 			break;
 
 		case USBH_PORT_FEAT_C_RESET:
-			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_RESET;
+			host->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_RESET;
 			break;
 
 		case USBH_PORT_FEAT_C_ENABLE:
-			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_ENABLE;
+			host->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_ENABLE;
 			break;
 
 		case USBH_PORT_FEAT_C_SUSPEND:
-			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_SUSPEND;
+			host->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_SUSPEND;
 			break;
 
 		case USBH_PORT_FEAT_C_OVERCURRENT:
-			usbh->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_OVERCURRENT;
+			host->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_OVERCURRENT;
 			break;
 
 		default:
@@ -1645,7 +1632,7 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 		osalDbgAssert(windex == 1, "invalid windex");
 		osalDbgCheck(wlength >= 4);
 		osalSysLock();
-		*(uint32_t *)buf = usbh->rootport.lld_status | (usbh->rootport.lld_c_status << 16);
+		*(uint32_t *)buf = host->rootport.lld_status | (host->rootport.lld_c_status << 16);
 		osalSysUnlock();
 		break;
 
@@ -1665,7 +1652,7 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 
 		case USBH_PORT_FEAT_RESET: {
 			osalSysLock();
-			stm32_otg_t *const otg = usbh->otg;
+			stm32_otg_t *const otg = host->otg;
 			uint32_t hprt;
 			otg->PCGCCTL = 0;
 			hprt = otg->HPRT;
@@ -1675,14 +1662,14 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 				uerr("Detected enabled port; resetting OTG core");
 				otg->GAHBCFG = 0;
 				osalThreadSleepS(OSAL_MS2I(20));
-				_usbh_start(usbh);				/* this effectively resets the core */
+				_usbh_start(host);				/* this effectively resets the core */
 				osalThreadSleepS(OSAL_MS2I(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;
+					host->rootport.lld_c_status &= ~USBH_PORTSTATUS_C_CONNECTION;
 				}
 			}
 			/* note: writing PENA = 1 actually disables the port */
@@ -1692,7 +1679,7 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 			osalThreadSleepS(OSAL_MS2I(15));
 			otg->HPRT = hprt;
 			osalThreadSleepS(OSAL_MS2I(10));
-			usbh->rootport.lld_c_status |= USBH_PORTSTATUS_C_RESET;
+			host->rootport.lld_c_status |= USBH_PORTSTATUS_C_RESET;
 			osalSysUnlock();
 		} 	break;
 
@@ -1714,8 +1701,8 @@ usbh_urbstatus_t usbh_lld_root_hub_request(USBHDriver *usbh, uint8_t bmRequestTy
 	return USBH_URBSTATUS_OK;
 }
 
-uint8_t usbh_lld_roothub_get_statuschange_bitmap(USBHDriver *usbh) {
-	return usbh->rootport.lld_c_status ? (1 << 1) : 0;
+uint8_t usbh_lld_roothub_get_statuschange_bitmap(USBHDriver *host) {
+	return host->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 bb92ede..7e0c1b6 100644
--- a/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.h
+++ b/os/hal/ports/STM32/LLD/USBHv1/hal_usbh_lld.h
@@ -83,6 +83,7 @@ typedef struct stm32_hc_management {
 		uint32_t 			hcintmsk;													\
 		uint32_t			hcchar;														\
 		uint32_t 			dt_mask;			/* data-toggle mask */					\
+		int32_t				trace_level;		/* enable tracing */					\
 		/* current transfer */															\
 		struct {																		\
 			stm32_hc_management_t *hcm;				/* assigned channel */				\
diff --git a/os/hal/src/hal_usbh.c b/os/hal/src/hal_usbh.c
index feaf956..2b9ae7c 100644
--- a/os/hal/src/hal_usbh.c
+++ b/os/hal/src/hal_usbh.c
@@ -23,36 +23,15 @@
 #include "usbh/dev/hub.h"
 #include <string.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)
+#define _USBH_DEBUG_HELPER_ENABLE_TRACE		USBH_DEBUG_ENABLE_TRACE
+#define _USBH_DEBUG_HELPER_ENABLE_INFO		USBH_DEBUG_ENABLE_INFO
+#define _USBH_DEBUG_HELPER_ENABLE_WARNINGS	USBH_DEBUG_ENABLE_WARNINGS
+#define _USBH_DEBUG_HELPER_ENABLE_ERRORS	USBH_DEBUG_ENABLE_ERRORS
+#include "usbh/debug_helpers.h"
+
+#if USBH_DEBUG_ENABLE && !USBH_DEBUG_MULTI_HOST
+/* debug */
+struct usbh_debug_helper usbh_debug;
 #endif
 
 static void _classdriver_process_device(usbh_device_t *dev);
@@ -104,7 +83,11 @@ void usbhObjectInit(USBHDriver *usbh) {
 }
 
 void usbhStart(USBHDriver *usbh) {
+#if USBH_DEBUG_MULTI_HOST
 	usbDbgInit(usbh);
+#else
+	usbDbgInit();
+#endif
 
 	osalSysLock();
 	osalDbgAssert((usbh->status == USBH_STATUS_STOPPED) || (usbh->status == USBH_STATUS_STARTED),
@@ -264,7 +247,7 @@ void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {
 	_check_urb(urb);
 
 	if (_usbh_urb_abortI(urb, status) == FALSE) {
-		uwarn("URB wasn't aborted immediately, suspend");
+		uurbwarn("URB wasn't aborted immediately, suspend");
 		osalThreadSuspendS(&urb->abortingThread);
 		osalDbgAssert(urb->abortingThread == 0, "maybe we should uncomment the line below");
 		//urb->abortingThread = 0;
@@ -272,7 +255,7 @@ void _usbh_urb_abort_and_waitS(usbh_urb_t *urb, usbh_urbstatus_t status) {
 		/* This call is necessary because _usbh_urb_abortI may require a reschedule */
 		osalOsRescheduleS();
 	}
-	uwarn("URB aborted");
+	uurbwarn("URB aborted");
 }
 
 /* usbhURBCancelI may require a reschedule if called from a S-locked state */
@@ -607,26 +590,26 @@ static bool _device_set_configuration(usbh_device_t *dev, uint8_t configuration)
 static bool _device_configure(usbh_device_t *dev, uint8_t bConfiguration) {
 	uint8_t i;
 
-	uinfof("Reading basic configuration descriptor %d", bConfiguration);
+	udevinfof("Reading basic configuration descriptor %d", bConfiguration);
 	for (i = 0; i < 3; i++) {
 		if (!_device_read_basic_cfgdesc(dev, bConfiguration))
 			break;
 	}
 
 	if (i == 3) {
-		uerrf("Could not read basic configuration descriptor %d; "
+		udeverrf("Could not read basic configuration descriptor %d; "
 					"won't configure device", bConfiguration);
 		return HAL_FAILED;
 	}
 
-	uinfof("Selecting configuration %d", bConfiguration);
+	udevinfof("Selecting configuration %d", bConfiguration);
 	for (i = 0; i < 3; i++) {
 		if (!_device_set_configuration(dev, dev->basicConfigDesc.bConfigurationValue)) {
 			/* TODO: check if correctly configured using GET_CONFIGURATION */
 			dev->status = USBH_DEVSTATUS_CONFIGURED;
 			dev->bConfiguration = bConfiguration;
 
-			uinfo("Device configured.");
+			udevinfo("Device configured.");
 			return HAL_SUCCESS;
 		}
 	}
@@ -636,16 +619,16 @@ static bool _device_configure(usbh_device_t *dev, uint8_t bConfiguration) {
 
 static bool _device_enumerate(usbh_device_t *dev) {
 
-	uinfo("Enumerate.");
-	uinfo("Get first 8 bytes of device descriptor");
+	udevinfo("Enumerate.");
+	udevinfo("Get first 8 bytes of device descriptor");
 
 	/* get first 8 bytes of device descriptor */
 	if (usbhStdReqGetDeviceDescriptor(dev, 8, (uint8_t *)&dev->devDesc)) {
-		uerr("Error");
+		udeverr("Error");
 		return HAL_FAILED;
 	}
 
-	uinfof("Configure bMaxPacketSize0 = %d", dev->devDesc.bMaxPacketSize0);
+	udevinfof("Configure bMaxPacketSize0 = %d", dev->devDesc.bMaxPacketSize0);
 	/* configure EP0 wMaxPacketSize */
 	usbhEPClose(&dev->ctrl);
 	_ep0_object_init(dev, dev->devDesc.bMaxPacketSize0);
@@ -653,14 +636,14 @@ static bool _device_enumerate(usbh_device_t *dev) {
 
 	uint8_t addr = _find_address(dev->host);
 	if (addr == 0) {
-		uerr("No free addresses found");
+		udeverr("No free addresses found");
 		return HAL_FAILED;
 	}
 
 	/* set device address */
-	uinfof("Set device address: %d", addr);
+	udevinfof("Set device address: %d", addr);
 	if (_device_setaddress(dev, addr)) {
-		uerr("Error");
+		udeverr("Error");
 		_free_address(dev->host, addr);
 		return HAL_FAILED;
 	}
@@ -670,23 +653,23 @@ static bool _device_enumerate(usbh_device_t *dev) {
 	_ep0_object_init(dev, dev->devDesc.bMaxPacketSize0);
 	usbhEPOpen(&dev->ctrl);
 
-	uinfof("Wait stabilization...");
+	udevinfof("Wait stabilization...");
 	osalThreadSleepMilliseconds(HAL_USBH_DEVICE_ADDRESS_STABILIZATION);
 
 	/* address is set */
 	dev->status = USBH_DEVSTATUS_ADDRESS;
 
-	uinfof("Get full device desc");
+	udevinfof("Get full device desc");
 	/* get full device descriptor */
 	if (usbhStdReqGetDeviceDescriptor(dev, sizeof(dev->devDesc),
 			(uint8_t *)&dev->devDesc)) {
-		uerr("Error");
+		udeverr("Error");
 		_device_setaddress(dev, 0);
 		_free_address(dev->host, addr);
 		return HAL_FAILED;
 	}
 
-	uinfof("Enumeration finished.");
+	udevinfof("Enumeration finished.");
 	return HAL_SUCCESS;
 }
 
@@ -695,93 +678,96 @@ void usbhDevicePrintInfo(usbh_device_t *dev) {
 	USBH_DEFINE_BUFFER(char str[64]);
 	usbh_device_descriptor_t *const desc = &dev->devDesc;
 
-	uinfo("----- Device info -----");
-	uinfo("Device descriptor:");
-	uinfof("\tUSBSpec=%04x, #configurations=%d, langID0=%04x",
+	udevinfo("----- Device info -----");
+	udevinfo("Device descriptor:");
+	udevinfof("\tUSBSpec=%04x, #configurations=%d, langID0=%04x",
 			desc->bcdUSB,
 			desc->bNumConfigurations,
 			dev->langID0);
 
-	uinfof("\tClass=%02x, Subclass=%02x, Protocol=%02x",
+	udevinfof("\tClass=%02x, Subclass=%02x, Protocol=%02x",
 			desc->bDeviceClass,
 			desc->bDeviceSubClass,
 			desc->bDeviceProtocol);
 
-	uinfof("\tVID=%04x, PID=%04x, Release=%04x",
+	udevinfof("\tVID=%04x, PID=%04x, Release=%04x",
 			desc->idVendor,
 			desc->idProduct,
 			desc->bcdDevice);
 
 	if (dev->langID0) {
 		usbhDeviceReadString(dev, str, sizeof(str), desc->iManufacturer, dev->langID0);
-		uinfof("\tManufacturer: %s", str);
+		udevinfof("\tManufacturer: %s", str);
 		usbhDeviceReadString(dev, str, sizeof(str), desc->iProduct, dev->langID0);
-		uinfof("\tProduct: %s", str);
+		udevinfof("\tProduct: %s", str);
 		usbhDeviceReadString(dev, str, sizeof(str), desc->iSerialNumber, dev->langID0);
-		uinfof("\tSerial Number: %s", str);
+		udevinfof("\tSerial Number: %s", str);
 	}
 
 	if (dev->status == USBH_DEVSTATUS_CONFIGURED) {
-		uinfo("Configuration descriptor (partial):");
+		udevinfo("Configuration descriptor (partial):");
 		usbh_config_descriptor_t *const cfg = &dev->basicConfigDesc;
-		uinfof("\tbConfigurationValue=%d, Length=%d, #interfaces=%d",
+		udevinfof("\tbConfigurationValue=%d, Length=%d, #interfaces=%d",
 				cfg->bConfigurationValue,
 				cfg->wTotalLength,
 				cfg->bNumInterfaces);
 
-		uinfof("\tCurrent=%dmA", cfg->bMaxPower * 2);
-		uinfof("\tSelfPowered=%d, RemoteWakeup=%d",
+		udevinfof("\tCurrent=%dmA", cfg->bMaxPower * 2);
+		udevinfof("\tSelfPowered=%d, RemoteWakeup=%d",
 				cfg->bmAttributes & 0x40 ? 1 : 0,
 				cfg->bmAttributes & 0x20 ? 1 : 0);
 		if (dev->langID0) {
 			usbhDeviceReadString(dev, str, sizeof(str), cfg->iConfiguration, dev->langID0);
-			uinfof("\tName: %s", str);
+			udevinfof("\tName: %s", str);
 		}
 	}
 
-	uinfo("----- End Device info -----");
+	udevinfo("----- End Device info -----");
 
 }
 
-void usbhDevicePrintConfiguration(const uint8_t *descriptor, uint16_t rem) {
+void usbhDevicePrintConfiguration(const usbh_device_t *dev,
+		const uint8_t *descriptor, uint16_t rem) {
+	(void)dev;
+
 	generic_iterator_t iep, icfg, ics;
 	if_iterator_t iif;
 
-	uinfo("----- Configuration info -----");
-	uinfo("Configuration descriptor:");
+	udevinfo("----- Configuration info -----");
+	udevinfo("Configuration descriptor:");
 	cfg_iter_init(&icfg, descriptor, rem);
 	const usbh_config_descriptor_t *const cfgdesc = cfg_get(&icfg);
-	uinfof("Configuration %d, #IFs=%d", cfgdesc->bConfigurationValue, cfgdesc->bNumInterfaces);
+	udevinfof("Configuration %d, #IFs=%d", cfgdesc->bConfigurationValue, cfgdesc->bNumInterfaces);
 
 	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
 		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
 
-		uinfof("  Interface %d, alt=%d, #EPs=%d, "
+		udevinfof("  Interface %d, alt=%d, #EPs=%d, "
 				"Class=%02x, Subclass=%02x, Protocol=%02x",
 				ifdesc->bInterfaceNumber, ifdesc->bAlternateSetting, ifdesc->bNumEndpoints,
 				ifdesc->bInterfaceClass, ifdesc->bInterfaceSubClass, ifdesc->bInterfaceProtocol);
 
 		for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {
-			uinfof("    Class-Specific descriptor, Length=%d, Type=%02x",
+			udevinfof("    Class-Specific descriptor, Length=%d, Type=%02x",
 					ics.curr[0], ics.curr[1]);
 		}
 
 		for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
 			const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
 
-			uinfof("    Endpoint descriptor, Address=%02x, Type=%d, MaxPacket=%d, Interval=%d",
+			udevinfof("    Endpoint descriptor, Address=%02x, Type=%d, MaxPacket=%d, Interval=%d",
 					epdesc->bEndpointAddress,
 					epdesc->bmAttributes & 3,
 					epdesc->wMaxPacketSize,
 					epdesc->bInterval);
 
 			for (cs_iter_init(&ics, &iep); ics.valid; cs_iter_next(&ics)) {
-				uinfof("    Class-Specific descriptor, Length=%d, Type=%02x",
+				udevinfof("    Class-Specific descriptor, Length=%d, Type=%02x",
 						ics.curr[0], ics.curr[1]);
 			}
 		}
 	}
-	uinfo("----- End Configuration info -----");
+	udevinfo("----- End Configuration info -----");
 }
 #endif
 
@@ -873,25 +859,25 @@ static void _port_process_status_change(usbh_port_t *port) {
 	if (port->c_status & USBH_PORTSTATUS_C_RESET) {
 		port->c_status &= ~USBH_PORTSTATUS_C_RESET;
 		usbhhubClearFeaturePort(port, USBH_PORT_FEAT_C_RESET);
-		udbgf("Port %d: reset=%d", port->number, port->status & USBH_PORTSTATUS_RESET ? 1 : 0);
+		uportdbgf("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);
-		udbgf("Port %d: enable=%d", port->number, port->status & USBH_PORTSTATUS_ENABLE ? 1 : 0);
+		uportdbgf("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);
+		uportwarnf("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);
+		uportinfof("Port %d: suspend=%d", port->number, port->status & USBH_PORTSTATUS_SUSPEND ? 1 : 0);
 	}
 
 }
@@ -906,7 +892,7 @@ static void _port_connected(usbh_port_t *port) {
 	USBH_DEFINE_BUFFER(usbh_string_descriptor_t strdesc);
 
 	port->device.status = USBH_DEVSTATUS_ATTACHED;
-	uinfof("Port %d: attached, wait debounce...", port->number);
+	uportinfof("Port %d: attached, wait debounce...", port->number);
 
 	/* wait for attach de-bounce */
 	osalThreadSleepMilliseconds(HAL_USBH_PORT_DEBOUNCE_TIME);
@@ -916,23 +902,23 @@ static void _port_connected(usbh_port_t *port) {
 	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 #1", port->number);
+		uportwarnf("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);
+		uportwarnf("Port %d: device is disconnected", port->number);
 		goto abort;
 	}
 
-	uinfof("Port %d: connected", port->number);
+	uportinfof("Port %d: connected", port->number);
 	port->device.status = USBH_DEVSTATUS_CONNECTED;
 	retries = 3;
 
 reset:
 	for (i = 0; i < 3; i++) {
-		uinfof("Port %d: Try reset...", port->number);
+		uportinfof("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);
@@ -945,7 +931,7 @@ reset:
 			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);
+				uportwarnf("Port %d: connection state changed; abort #2", port->number);
 				goto abort;
 			}
 
@@ -962,7 +948,7 @@ reset:
 
 			/* check for timeout */
 			if (osalOsGetSystemTimeX() - start > HAL_USBH_PORT_RESET_TIMEOUT) {
-				uwarnf("Port %d: reset timeout", port->number);
+				uportwarnf("Port %d: reset timeout", port->number);
 				break;
 			}
 		}
@@ -972,7 +958,7 @@ reset:
 	goto abort;
 
 reset_success:
-	uinfof("Port %d: Reset OK, recovery...", port->number);
+	uportinfof("Port %d: Reset OK, recovery...", port->number);
 
 	/* reset recovery */
 	osalThreadSleepMilliseconds(100);
@@ -994,17 +980,17 @@ reset_success:
 		usbhEPClose(&port->device.ctrl);
 
 		if (!--retries) {
-			uwarnf("Port %d: enumeration failed; abort", port->number);
+			uportwarnf("Port %d: enumeration failed; abort", port->number);
 			goto abort;
 		}
 
 		/* retry reset & enumeration */
-		uwarnf("Port %d: enumeration failed; retry reset & enumeration", port->number);
+		uportwarnf("Port %d: enumeration failed; retry reset & enumeration", port->number);
 		goto reset;
 	}
 
 	/* load the default language ID */
-	uinfof("Port %d: Loading langID0...", port->number);
+	uportinfof("Port %d: Loading langID0...", port->number);
 	if (!usbhStdReqGetStringDescriptor(&port->device, 0, 0,
 			USBH_DT_STRING_SIZE, (uint8_t *)&strdesc)
 		&& (strdesc.bLength >= 4)
@@ -1012,12 +998,12 @@ reset_success:
 			4, (uint8_t *)&strdesc)) {
 
 		port->device.langID0 = strdesc.wData[0];
-		uinfof("Port %d: langID0=%04x", port->number, port->device.langID0);
+		uportinfof("Port %d: langID0=%04x", port->number, port->device.langID0);
 	}
 
 	/* check if the device has only one configuration */
 	if (port->device.devDesc.bNumConfigurations == 1) {
-		uinfof("Port %d: device has only one configuration", port->number);
+		uportinfof("Port %d: device has only one configuration", port->number);
 		_device_configure(&port->device, 0);
 	}
 
@@ -1025,7 +1011,7 @@ reset_success:
 	return;
 
 abort:
-	uerrf("Port %d: abort", port->number);
+	uporterrf("Port %d: abort", port->number);
 	port->device.status = USBH_DEVSTATUS_DISCONNECTED;
 }
 
@@ -1033,14 +1019,14 @@ void _usbh_port_disconnected(usbh_port_t *port) {
 	if (port->device.status == USBH_DEVSTATUS_DISCONNECTED)
 		return;
 
-	uinfof("Port %d: disconnected", port->number);
+	uportinfof("Port %d: disconnected", port->number);
 
 	/* unload drivers */
 	while (port->device.drivers) {
 		usbh_baseclassdriver_t *drv = port->device.drivers;
 
 		/* unload */
-		uinfof("Port %d: unload driver %s", port->number, drv->info->name);
+		uportinfof("Port %d: unload driver %s", port->number, drv->info->name);
 		drv->info->vmt->unload(drv);
 
 		/* unlink */
@@ -1277,7 +1263,7 @@ static bool _classdriver_load(usbh_device_t *dev, uint8_t *descbuff, uint16_t re
 	for (i = 0; i < sizeof_array(usbh_classdrivers_lookup); i++) {
 		const usbh_classdriverinfo_t *const info = usbh_classdrivers_lookup[i];
 
-		uinfof("Try load driver %s", info->name);
+		udevinfof("Try load driver %s", info->name);
 		drv = info->vmt->load(dev, descbuff, rem);
 
 		if (drv != NULL)
@@ -1297,7 +1283,7 @@ success:
 }
 
 static void _classdriver_process_device(usbh_device_t *dev) {
-	uinfo("New device found.");
+	udevinfo("New device found.");
 	const usbh_device_descriptor_t *const devdesc = &dev->devDesc;
 
 	usbhDevicePrintInfo(dev);
@@ -1308,20 +1294,20 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 	 * will have multiple configurations.
 	 */
 	if (dev->status != USBH_DEVSTATUS_CONFIGURED) {
-		uwarn("Multiple configurations not supported, selecting configuration #0");
+		udevwarn("Multiple configurations not supported, selecting configuration #0");
 		if (_device_configure(dev, 0) != HAL_SUCCESS) {
-			uerr("Couldn't configure device; abort.");
+			udeverr("Couldn't configure device; abort.");
 			return;
 		}
 	}
 
 	_device_read_full_cfgdesc(dev, dev->bConfiguration);
 	if (dev->fullConfigurationDescriptor == NULL) {
-		uerr("Couldn't read full configuration descriptor; abort.");
+		udeverr("Couldn't read full configuration descriptor; abort.");
 		return;
 	}
 
-	usbhDevicePrintConfiguration(dev->fullConfigurationDescriptor,
+	usbhDevicePrintConfiguration(dev, dev->fullConfigurationDescriptor,
 			dev->basicConfigDesc.wTotalLength);
 
 #if HAL_USBH_USE_IAD
@@ -1329,7 +1315,7 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 			&& dev->devDesc.bDeviceSubClass == 0x02
 			&& dev->devDesc.bDeviceProtocol == 0x01) {
 
-		uinfo("Load a driver for each IF collection.");
+		udevinfo("Load a driver for each IF collection.");
 
 		generic_iterator_t icfg;
 		if_iterator_t iif;
@@ -1338,7 +1324,7 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 		cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
 				dev->basicConfigDesc.wTotalLength);
 		if (!icfg.valid) {
-			uerr("Invalid configuration descriptor.");
+			udeverr("Invalid configuration descriptor.");
 			goto exit;
 		}
 
@@ -1348,7 +1334,7 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 				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",
+					udevwarnf("No drivers found for IF collection #%d:%d",
 							iif.iad->bFirstInterface,
 							iif.iad->bFirstInterface + iif.iad->bInterfaceCount - 1);
 				}
@@ -1358,11 +1344,11 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 	} else
 #endif
 	if (_classdriver_load(dev, (uint8_t *)devdesc, USBH_DT_DEVICE_SIZE) != HAL_SUCCESS) {
-		uinfo("No drivers found for device.");
+		udevinfo("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.");
+			udevinfo("Try load a driver for each IF.");
 
 			generic_iterator_t icfg;
 			if_iterator_t iif;
@@ -1371,7 +1357,7 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 			cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
 					dev->basicConfigDesc.wTotalLength);
 			if (!icfg.valid) {
-				uerr("Invalid configuration descriptor.");
+				udeverr("Invalid configuration descriptor.");
 				goto exit;
 			}
 
@@ -1380,12 +1366,12 @@ static void _classdriver_process_device(usbh_device_t *dev) {
 				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);
+						udevwarnf("No drivers found for IF #%d", ifdesc->bInterfaceNumber);
 					}
 				}
 			}
 		} else {
-			uwarn("Unable to load driver.");
+			udevwarn("Unable to load driver.");
 		}
 	}
 
diff --git a/os/hal/src/usbh/hal_usbh_aoa.c b/os/hal/src/usbh/hal_usbh_aoa.c
index 0686179..77ac4e7 100644
--- a/os/hal/src/usbh/hal_usbh_aoa.c
+++ b/os/hal/src/usbh/hal_usbh_aoa.c
@@ -29,39 +29,12 @@
 
 //#pragma GCC optimize("Og")
 
-
-#if USBHAOA_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 USBHAOA_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 USBHAOA_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 USBHAOA_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
-
+#define _USBH_DEBUG_HELPER_CLASS_DRIVER		container_of(aoacp, USBHAOADriver, channel)
+#define _USBH_DEBUG_HELPER_ENABLE_TRACE		USBHAOA_DEBUG_ENABLE_TRACE
+#define _USBH_DEBUG_HELPER_ENABLE_INFO		USBHAOA_DEBUG_ENABLE_INFO
+#define _USBH_DEBUG_HELPER_ENABLE_WARNINGS	USBHAOA_DEBUG_ENABLE_WARNINGS
+#define _USBH_DEBUG_HELPER_ENABLE_ERRORS	USBHAOA_DEBUG_ENABLE_ERRORS
+#include "usbh/debug_helpers.h"
 
 /*===========================================================================*/
 /* Constants													 		 	 */
@@ -162,11 +135,11 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 		};
 
 		if (descriptor[1] != USBH_DT_DEVICE) {
-			uinfo("AOA: Won't try to detect Android device at interface level");
+			udevinfo("AOA: Won't try to detect Android device at interface level");
 			return NULL;
 		}
 
-		uinfo("AOA: Unrecognized VID");
+		udevinfo("AOA: Unrecognized VID");
 
 #if defined(HAL_USBHAOA_FILTER_CALLBACK)
 		if (!HAL_USBHAOA_FILTER_CALLBACK(dev, descriptor, rem, &config)) {
@@ -174,12 +147,12 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 		}
 #endif
 
-		uinfo("AOA: Try if it's an Android device");
+		udevinfo("AOA: Try if it's an Android device");
 		if (_get_protocol(dev, &protocol) != HAL_SUCCESS) {
-			uinfo("AOA: not an Android device");
+			udevinfo("AOA: not an Android device");
 			return NULL;
 		}
-		uinfof("AOA: Possible Android device found (protocol=%d)", protocol);
+		udevinfof("AOA: Possible Android device found (protocol=%d)", protocol);
 
 		if (config.channel.manufacturer != NULL) {
 			if ((_send_string(dev, USBHAOA_ACCESSORY_STRING_MANUFACTURER, config.channel.manufacturer) != HAL_SUCCESS)
@@ -188,22 +161,22 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 				|| (_send_string(dev, USBHAOA_ACCESSORY_STRING_VERSION, config.channel.version) != HAL_SUCCESS)
 				|| (_send_string(dev, USBHAOA_ACCESSORY_STRING_URI, config.channel.uri) != HAL_SUCCESS)
 				|| (_send_string(dev, USBHAOA_ACCESSORY_STRING_SERIAL, config.channel.serial) != HAL_SUCCESS)) {
-				uerr("AOA: Can't send string; abort start");
+				udeverr("AOA: Can't send string; abort start");
 				return NULL;
 			}
 		}
 
 		if (protocol > 1) {
 			if (_set_audio_mode(dev, (uint16_t)(config.audio.mode)) != HAL_SUCCESS) {
-				uerr("AOA: Can't set audio mode; abort channel start");
+				udeverr("AOA: Can't set audio mode; abort channel start");
 				return NULL;
 			}
 		}
 
 		if (_accessory_start(dev) != HAL_SUCCESS) {
-			uerr("AOA: Can't start accessory; abort channel start");
+			udeverr("AOA: Can't start accessory; abort channel start");
 		} else {
-			uinfo("AOA: Accessory started");
+			udevinfo("AOA: Accessory started");
 		}
 
 		return NULL;
@@ -227,18 +200,18 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 	case AOA_GOOGLE_PID_ACCESSORY_AUDIO_ABD:
 		break;
 	default:
-		uerr("AOA: Unrecognized PID");
+		udeverr("AOA: Unrecognized PID");
 		return NULL;
 	}
 
 	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
 	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");
+		udeverr("AOA: This IF is not the Accessory IF");
 		return NULL;
 	}
 
-	uinfof("AOA: Found Accessory Interface #%d", ifdesc->bInterfaceNumber);
+	udevinfof("AOA: Found Accessory Interface #%d", ifdesc->bInterfaceNumber);
 
 	for (i = 0; i < HAL_USBHAOA_MAX_INSTANCES; i++) {
 		if (USBHAOAD[i].dev == NULL) {
@@ -247,7 +220,7 @@ static usbh_baseclassdriver_t *_aoa_load(usbh_device_t *dev, const uint8_t *desc
 		}
 	}
 
-	uwarn("AOA: Can't alloc driver");
+	udevwarn("AOA: Can't alloc driver");
 
 	/* can't alloc */
 	return NULL;
@@ -269,30 +242,30 @@ alloc_ok:
 	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_BULK)) {
-			uinfof("AOA: BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			udevinfof("AOA: BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
 			usbhEPObjectInit(&aoap->channel.epin, dev, epdesc);
 			usbhEPSetName(&aoap->channel.epin, "AOA[BIN ]");
 		} else if (((epdesc->bEndpointAddress & 0x80) == 0)
 				&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
-			uinfof("AOA: BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			udevinfof("AOA: BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
 			usbhEPObjectInit(&aoap->channel.epout, dev, epdesc);
 			usbhEPSetName(&aoap->channel.epout, "AOA[BOUT]");
 		} else {
-			uinfof("AOA: unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+			udevinfof("AOA: unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
 				epdesc->bEndpointAddress, epdesc->bmAttributes);
 		}
 	}
 
 	if ((aoap->channel.epin.status != USBH_EPSTATUS_CLOSED)
 		|| (aoap->channel.epout.status != USBH_EPSTATUS_CLOSED)) {
-		uwarn("AOA: Couldn't find endpoints");
+		udevwarn("AOA: Couldn't find endpoints");
 		aoap->state = USBHAOA_STATE_STOP;
 		return NULL;
 	}
 
 	aoap->state = USBHAOA_STATE_READY;
 	aoap->channel.state = USBHAOA_CHANNEL_STATE_ACTIVE;
-	uwarn("AOA: Ready");
+	udevwarn("AOA: Ready");
 	return (usbh_baseclassdriver_t *)aoap;
 }
 
@@ -311,7 +284,7 @@ static void _aoa_unload(usbh_baseclassdriver_t *drv) {
 /* ------------------------------------ */
 
 static void _submitOutI(USBHAOAChannel *aoacp, uint32_t len) {
-	udbgf("AOA: Submit OUT %d", len);
+	uclassdrvdbgf("AOA: Submit OUT %d", len);
 	aoacp->oq_urb.requestedLength = len;
 	usbhURBObjectResetI(&aoacp->oq_urb);
 	usbhURBSubmitI(&aoacp->oq_urb);
@@ -327,12 +300,12 @@ static void _out_cb(usbh_urb_t *urb) {
 		chnAddFlagsI(aoacp, CHN_OUTPUT_EMPTY | CHN_TRANSMISSION_END);
 		return;
 	case USBH_URBSTATUS_DISCONNECTED:
-		uwarn("AOA: URB OUT disconnected");
+		uclassdrvwarn("AOA: URB OUT disconnected");
 		chThdDequeueAllI(&aoacp->oq_waiting, Q_RESET);
 		chnAddFlagsI(aoacp, CHN_OUTPUT_EMPTY);
 		return;
 	default:
-		uerrf("AOA: URB OUT status unexpected = %d", urb->status);
+		uclassdrverrf("AOA: URB OUT status unexpected = %d", urb->status);
 		break;
 	}
 	usbhURBObjectResetI(&aoacp->oq_urb);
@@ -406,7 +379,7 @@ static msg_t _put(USBHAOAChannel *aoacp, uint8_t b) {
 }
 
 static void _submitInI(USBHAOAChannel *aoacp) {
-	udbg("AOA: Submit IN");
+	uclassdrvdbg("AOA: Submit IN");
 	usbhURBObjectResetI(&aoacp->iq_urb);
 	usbhURBSubmitI(&aoacp->iq_urb);
 }
@@ -416,9 +389,9 @@ static void _in_cb(usbh_urb_t *urb) {
 	switch (urb->status) {
 	case USBH_URBSTATUS_OK:
 		if (urb->actualLength == 0) {
-			udbgf("AOA: URB IN no data");
+			uurbdbgf("AOA: URB IN no data");
 		} else {
-			udbgf("AOA: URB IN data len=%d", urb->actualLength);
+			uurbdbgf("AOA: URB IN data len=%d", urb->actualLength);
 			aoacp->iq_ptr = aoacp->iq_buff;
 			aoacp->iq_counter = urb->actualLength;
 			chThdDequeueNextI(&aoacp->iq_waiting, Q_OK);
@@ -426,14 +399,14 @@ static void _in_cb(usbh_urb_t *urb) {
 		}
 		break;
 	case USBH_URBSTATUS_DISCONNECTED:
-		uwarn("AOA: URB IN disconnected");
+		uurbwarn("AOA: URB IN disconnected");
 		chThdDequeueAllI(&aoacp->iq_waiting, Q_RESET);
 		chnAddFlagsI(aoacp, CHN_DISCONNECTED);
 		aoacp->state = USBHAOA_CHANNEL_STATE_ACTIVE;
 		container_of(aoacp, USBHAOADriver, channel)->state = USBHAOA_STATE_ACTIVE;
 		break;
 	default:
-		uerrf("AOA: URB IN status unexpected = %d", urb->status);
+		uurberrf("AOA: URB IN status unexpected = %d", urb->status);
 		_submitInI(aoacp);
 		break;
 	}
@@ -532,7 +505,7 @@ static const struct AOADriverVMT async_channel_vmt = {
 static void _stop_channelS(USBHAOAChannel *aoacp) {
 	if (aoacp->state != USBHAOA_CHANNEL_STATE_READY)
 		return;
-	uwarn("AOA: Stop channel");
+	uclassdrvwarn("AOA: Stop channel");
 	chVTResetI(&aoacp->vt);
 	usbhEPCloseS(&aoacp->epin);
 	usbhEPCloseS(&aoacp->epout);
diff --git a/os/hal/src/usbh/hal_usbh_debug.c b/os/hal/src/usbh/hal_usbh_debug.c
index 57df535..225489d 100644
--- a/os/hal/src/usbh/hal_usbh_debug.c
+++ b/os/hal/src/usbh/hal_usbh_debug.c
@@ -21,573 +21,199 @@
 
 #include "ch.h"
 #include "usbh/debug.h"
-#include <stdarg.h>
-#if 0
-#include "debug.h"
-#else
 #include "chprintf.h"
-#define dbg_lock()
-#define dbg_unlock()
-#endif
-
-#define MAX_FILLER 11
-#define FLOAT_PRECISION 9
-#define MPRINTF_USE_FLOAT 0
-
-static char *long_to_string_with_divisor(char *p, long num, unsigned radix, long divisor) 
-{
-	int i;
-	char *q;
-	long l, ll;
+#include <stdarg.h>
 
-	l = num;
-	if (divisor == 0) {
-		ll = num;
-	} else {
-		ll = divisor;
+#define TEMP_BUFF_LEN	255
+
+/* ************************ */
+/* Circular queue structure */
+/* ************************ */
+static int dq_append_string(usbh_dq_t *q, const uint8_t *s, int len) {
+	if (len <= 0) return 0;
+	if (len > TEMP_BUFF_LEN) len = TEMP_BUFF_LEN;
+	if (q->rem < len + 1) return -1;
+	q->rem -= len + 1;
+
+	uint8_t *d = q->next;
+	*d++ = len;
+	if (d == q->end) d = q->start;
+	while (len--) {
+		*d++ = *s++;
+		if (d == q->end) d = q->start;
 	}
-
-	q = p + MAX_FILLER;
-	do {
-		i = (int)(l % radix);
-		i += '0';
-		if (i > '9') {
-			i += 'A' - '0' - 10;
-		}
-		*--q = i;
-		l /= radix;
-	} while ((ll /= radix) != 0);
-
-	i = (int)(p + MAX_FILLER - q);
-	do {
-		*p++ = *q++;
-	} while (--i);
-
-	return p;
-}
-
-static char *ltoa(char *p, long num, unsigned radix) {
-
-	return long_to_string_with_divisor(p, num, radix, 0);
+	q->next = d;
+	return 0;
 }
 
-#if MPRINTF_USE_FLOAT
-static const long _pow10[FLOAT_PRECISION] = {10, 100, 1000, 10000, 100000, 1000000,
-	10000000, 100000000, 1000000000};
-static const double m10[FLOAT_PRECISION] = {5.0/100, 5.0/1000, 5.0/10000, 5.0/100000, 5.0/1000000,
-	5.0/10000000, 5.0/100000000, 5.0/1000000000, 5.0/10000000000};
-
-static char *ftoa(char *p, double num, unsigned long precision, bool dot) {
-	long l;
-	char *q;
-	double r;
-
-
-	if (precision == 0) {
-		l = (long)(num + 0.5);
-		return long_to_string_with_divisor(p, l, 10, 0);
-	} else {
-		if (precision > FLOAT_PRECISION) precision = FLOAT_PRECISION;
-		r = m10[precision - 1];
-		precision = _pow10[precision - 1];
-
-		l = (long)num;
-		p = long_to_string_with_divisor(p, l, 10, 0);
-		if (dot) *p++ = '.';
-		l = (long)((num - l + r) * precision);
-		q = long_to_string_with_divisor(p, l, 10, precision / 10) - 1;
-
-		while (q > p) {
-			if (*q != '0') {
-				break;
-			}
-			--q;
+static void dq_remove_oldest_string(usbh_dq_t *q) {
+	int len = *q->first;
+	if (len) {
+		++len;
+		q->rem += len;
+		q->first += len;
+		if (q->first >= q->end) {
+			q->first -= q->sz;
+		}
+		if (q->rem == q->sz) {
+			*q->first = 0;
 		}
-		return ++q;
 	}
-
-	
-
-
 }
-#endif
 
-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 int dq_read_oldest_string(usbh_dq_t *q, uint8_t *d) {
+	uint8_t *s = q->first;
+	int len;
+	int sz;
+	len = sz = *s++;
+	while (len--) {
+		*d++ = *s++;
+		if (d == q->end) d = q->start;
+	}
+	*d = 0;
+	return sz;
 }
 
-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);
+static void dq_init(usbh_dq_t *q, uint8_t *buff, int len) {
+	q->start = q->first = q->next = buff;
+	q->end = q->start + len;
+	q->sz = q->rem = len;
+	*buff = 0;
 }
 
-int _dbg_printf(const char *fmt, va_list ap) {
-	char *p, *s, c, filler;
-	int i, precision, width;
-	int n = 0;
-	bool is_long, left_align, sign;
-	long l;
-#if MPRINTF_USE_FLOAT
-	double f;
-	char tmpbuf[2*MAX_FILLER + 1];
-#else
-	char tmpbuf[MAX_FILLER + 1];
-#endif
-
-	for (;;) {
-
-		//agarrar nuevo caracter de formato
-		c = *fmt++;
 
-		//chequeo eos
-		if (c == 0) return n;
+static uint8_t buff[TEMP_BUFF_LEN + 1];
 
-		//copio los caracteres comunes
-		if (c != '%') {
-			_put(c);
-			n++;
-			continue;
-		}
-
-		//encontré un '%'
-		p = tmpbuf;
-		s = tmpbuf;
-
-		//left align
-		left_align = FALSE;
-		if (*fmt == '-') {
-			fmt++;
-			left_align = TRUE;
-		}
-		
-		sign = FALSE;
-		if (*fmt == '+') {
-			fmt++;
-			sign = TRUE;
-		}
-
-		//filler
-		filler = ' ';
-		if (*fmt == '0') {
-			fmt++;
-			filler = '0';
-		}
-
-		//width
-		width = 0;
-		while (TRUE) {
-			c = *fmt++;
-			if (c >= '0' && c <= '9')
-				c -= '0';
-			else if (c == '*')
-				c = va_arg(ap, int);
-			else
-				break;
-			width = width * 10 + c;
-		}
-
-		//precision
-		precision = 0;
-		if (c == '.') {
-
-			if (*fmt == 'n') {
-				fmt++;
-
-			}
-			while (TRUE) {
-				c = *fmt++;
-				if (c >= '0' && c <= '9')
-					c -= '0';
-				else if (c == '*')
-					c = va_arg(ap, int);
-				else
-					break;
-				precision = precision * 10 + c;
-			}
-		}
-
-		//long modifier
-		if (c == 'l' || c == 'L') {
-			is_long = TRUE;
-			if (*fmt)
-				c = *fmt++;
-		}
-		else
-			is_long = (c >= 'A') && (c <= 'Z');
-
-		/* Command decoding.*/
-		switch (c) {
-		//char
-		case 'c':
-			filler = ' ';
-			*p++ = va_arg(ap, int);
-			break;
-
-		//string
-		case 's':
-			filler = ' ';
-			if ((s = va_arg(ap, char *)) == 0)
-				s = (char *)"(null)";
-			if (precision == 0)
-				precision = 32767;
-
-			//strlen con límite hasta precision
-			for (p = s; *p && (--precision >= 0); p++)
-				;
-			break;
-
-
-
-		case 'D':
-		case 'd':
-		case 'I':
-		case 'i':
-			if (is_long)
-				l = va_arg(ap, long);
-			else
-				l = va_arg(ap, int);
-			if (l < 0) {
-				*p++ = '-';
-				l = -l;
-				sign = TRUE;
-			} else if (sign) {
-				*p++ = '+';
-			}
-			p = ltoa(p, l, 10);
-			break;
-
-#if MPRINTF_USE_FLOAT
-		case 'f':
-			f = va_arg(ap, double);
-			if (f < 0) {
-				*p++ = '-';
-				f = -f;
-				sign = TRUE;
-			} else if (sign) {
-				*p++ = '+';
-			}
-			if (prec == FALSE) precision = 6;
-			p = ftoa(p, f, precision, dot);
-			break;
-#endif
-
-
-		case 'X':
-		case 'x':
-			c = 16;
-			goto unsigned_common;
-		case 'U':
-		case 'u':
-			c = 10;
-			goto unsigned_common;
-		case 'O':
-		case 'o':
-			c = 8;
-
-unsigned_common:
-			if (is_long)
-				l = va_arg(ap, unsigned long);
-			else
-				l = va_arg(ap, unsigned int);
-			p = ltoa(p, l, c);
-			break;
-
-		//copiar
-		default:
-			*p++ = c;
-			break;
-		}
-
-		//longitud
-		i = (int)(p - s);
-
-		//calculo cuántos caracteres de filler debo poner
-		if ((width -= i) < 0)
-			width = 0;
-
-		if (left_align == FALSE)
-			width = -width;
-
-		if (width < 0) {
-			//alineado a la derecha
-
-			//poner el signo adelante
-			if (sign && filler == '0') {
-				_put(*s++);
-				n++;
-				i--;
-			}
-
-			//fill a la izquierda
-			do {
-				_put(filler);
-				n++;
-			} while (++width != 0);
-		}
-
-		//copiar los caracteres
-		while (--i >= 0) {
-			_put(*s++);
-			n++;
-		}
+static inline syssts_t _dbg_prologue(struct usbh_debug_helper *debug,
+		uint32_t hfnum, uint16_t hfir, const char *s, int *len) {
+	syssts_t sts = chSysGetStatusAndLockX();
 
-		//fill a la derecha
-		while (width) {
-			_put(filler);
-			n++;
-			width--;
-		}
+	debug->last = osalOsGetSystemTimeX();
+	if (debug->ena) {
+		debug->first = debug->last;
 	}
 
-	//return n; // can raise 'code is unreachable' warning
+	if (((hfnum & 0x3fff) == 0x3fff) && (hfir == (hfnum >> 16))) {
+		*len = chsnprintf((char *)buff, sizeof(buff), "+%08d ", debug->last - debug->first);
+		debug->ena = FALSE;
+	} else {
+		uint32_t f = hfnum & 0xffff;
+		uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));
+		*len = chsnprintf((char *)buff, sizeof(buff), "%05d.%03d %s", f, p, s);
+		debug->ena = TRUE;
+	}
 
+	return sts;
 }
 
-static systime_t first, last;
-static bool ena;
-static uint32_t hdr[2];
+static inline void dbg_epilogue(struct usbh_debug_helper *debug,
+		syssts_t sts, int len) {
 
-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;
+	while (dq_append_string(&debug->dq, buff, len) < 0) {
+		dq_remove_oldest_string(&debug->dq);
 	}
 
-	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;
+	if (debug->on) {
+		chThdResumeI(&debug->tr, MSG_OK);
 	}
-}
 
-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, ...)
-{
-	va_list ap;
-	va_start(ap, fmt);
-	syssts_t sts = chSysGetStatusAndLockX();
-	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);
-	if (!port_is_isr_context() && chSchIsPreemptionRequired()) {
-		chSchRescheduleS();
-	}
-	va_end(ap);
 }
 
+#if USBH_DEBUG_MULTI_HOST
+void usbDbgPrintf(USBHDriver *host, const char *fmt, ...) {
+	if (!host) return;
+	struct usbh_debug_helper *const debug = &host->debug;
+	uint32_t hfnum = host->otg->HFNUM;
+	uint16_t hfir = host->otg->HFIR;
+#else
+void usbDbgPrintf(const char *fmt, ...) {
+	struct usbh_debug_helper *const debug = &usbh_debug;
+	uint32_t hfnum = USBH_DEBUG_SINGLE_HOST_SELECTION.otg->HFNUM;
+	uint16_t hfir = USBH_DEBUG_SINGLE_HOST_SELECTION.otg->HFIR;
+#endif
+	int len;
 
-void usbDbgPuts(const char *s)
-{
-	_build_hdr();
-	uint8_t *p = (uint8_t *)hdr;
-	uint8_t *top = p + 8;
+	syssts_t sts = _dbg_prologue(debug, hfnum, hfir, "", &len);
 
-	syssts_t sts = chSysGetStatusAndLockX();
-	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
-	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;
-	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);
-	}
-	chSysRestoreStatusX(sts);
-	if (!port_is_isr_context() && chSchIsPreemptionRequired()) {
-		chSchRescheduleS();
-	}
-}
-
-void usbDbgReset(void) {
-	const char *msg = "\r\n\r\n==== DEBUG OUTPUT RESET ====\r\n";
+	va_list ap;
+	va_start(ap, fmt);
+	len += chvsnprintf((char *)buff + len, sizeof(buff) - len, fmt, ap);
+	va_end(ap);
 
-	syssts_t sts = chSysGetStatusAndLockX();
-	iqResetI(&USBH_DEBUG_USBHD.iq);
-	oqResetI(&USBH_DEBUG_SD.oqueue);
-	while (*msg) {
-		*USBH_DEBUG_SD.oqueue.q_wrptr++ = *msg++;
-		USBH_DEBUG_SD.oqueue.q_counter--;
-	}
-	chSysRestoreStatusX(sts);
-	if (!port_is_isr_context() && chSchIsPreemptionRequired()) {
-		chSchRescheduleS();
-	}
+	dbg_epilogue(debug, sts, len);
 }
 
-static int _get(void) {
-	if (!USBH_DEBUG_USBHD.iq.q_counter) return -1;
-	USBH_DEBUG_USBHD.iq.q_counter--;
-	uint8_t b = *USBH_DEBUG_USBHD.iq.q_rdptr++;
-	if (USBH_DEBUG_USBHD.iq.q_rdptr >= USBH_DEBUG_USBHD.iq.q_top) {
-		USBH_DEBUG_USBHD.iq.q_rdptr = USBH_DEBUG_USBHD.iq.q_buffer;
-	}
-	return b;
+#if USBH_DEBUG_MULTI_HOST
+void usbDbgPuts(USBHDriver *host, const char *s) {
+	if (!host) return;
+	struct usbh_debug_helper *const debug = &host->debug;
+	uint32_t hfnum = host->otg->HFNUM;
+	uint16_t hfir = host->otg->HFIR;
+#else
+void usbDbgPuts(const char *s) {
+	struct usbh_debug_helper *const debug = &usbh_debug;
+	uint32_t hfnum = USBH_DEBUG_SINGLE_HOST_SELECTION.otg->HFNUM;
+	uint16_t hfir = USBH_DEBUG_SINGLE_HOST_SELECTION.otg->HFIR;
+#endif
+	int len;
+	syssts_t sts = _dbg_prologue(debug, hfnum, hfir, s, &len);
+	dbg_epilogue(debug, sts, len);
 }
 
-void usbDbgSystemHalted(void) {
-	while (true) {
-		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 (!(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;
-		}
-	}
-
-	int c;
-	int state = 0;
-	for (;;) {
-		c = _get(); if (c < 0) break;
-
-		if (state == 0) {
-			if (c == 0xff) state = 1;
-		} else if (state == 1) {
-			if (c == 0xff) state = 2;
-			else (state = 0);
-		} else {
-			c = _get(); if (c < 0) return;
-			c = _get(); if (c < 0) return;
-			c = _get(); if (c < 0) return;
-			c = _get(); if (c < 0) return;
-			c = _get(); if (c < 0) return;
-
-			while (true) {
-				c = _get(); if (c < 0) return;
-				if (!c) {
-					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 (!(USBH_DEBUG_SD.usart->SR & USART_SR_TXE));
-				USBH_DEBUG_SD.usart->DR = c;
-			}
-		}
-	}
+#if USBH_DEBUG_MULTI_HOST
+void usbDbgEnable(USBHDriver *host, bool enable) {
+	struct usbh_debug_helper *const debug = &host->debug;
+#else
+void usbDbgEnable(bool enable) {
+	struct usbh_debug_helper *const debug = &usbh_debug;
+#endif
+	debug->on = enable;
 }
 
 static void usb_debug_thread(void *arg) {
-	USBHDriver *host = (USBHDriver *)arg;
-	uint8_t state = 0;
+#if USBH_DEBUG_MULTI_HOST
+	USBHDriver *const host = (USBHDriver *)arg;
+	struct usbh_debug_helper *const debug = &host->debug;
+#else
+	(void)arg;
+	struct usbh_debug_helper *const debug = &usbh_debug;
+#endif
+
+	uint8_t rdbuff[TEMP_BUFF_LEN + 1];
 
 	chRegSetThreadName("USBH_DBG");
 	while (true) {
-		msg_t c = iqGet(&host->iq);
-		if (c < 0) goto reset;
-
-		if (state == 0) {
-			if (c == 0xff) state = 1;
-		} else if (state == 1) {
-			if (c == 0xff) state = 2;
-			else if (c == 0xfe) state = 3;
-			else (state = 0);
-		} else if (state == 2) {
-			uint16_t hfir;
-			uint32_t hfnum;
-
-			hfir = c;
-			c = iqGet(&host->iq); if (c < 0) goto reset;
-			hfir |= c << 8;
-
-			c = iqGet(&host->iq); if (c < 0) goto reset;
-			hfnum = c;
-			c = iqGet(&host->iq); if (c < 0) goto reset;
-			hfnum |= c << 8;
-			c = iqGet(&host->iq); if (c < 0) goto reset;
-			hfnum |= c << 16;
-			c = iqGet(&host->iq); if (c < 0) goto reset;
-			hfnum |= c << 24;
-
-			uint32_t f = hfnum & 0xffff;
-			uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));
-			dbg_lock();
-			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;
-
-			dbg_lock();
-			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "+%08d ", t);
-			state = 4;
+		chSysLock();
+		int len = dq_read_oldest_string(&debug->dq, rdbuff);
+		if (!len) {
+			chThdSuspendS(&debug->tr);
+			chSysUnlock();
 		} else {
-			while (true) {
-				if (!c) {
-					sdPut(&USBH_DEBUG_SD, '\r');
-					sdPut(&USBH_DEBUG_SD, '\n');
-					goto reset;
-				}
-				sdPut(&USBH_DEBUG_SD, (uint8_t)c);
-				c = iqGet(&host->iq); if (c < 0) goto reset;
-			}
-		}
-
-		continue;
-reset:
-		if (state == 4) {
-			dbg_unlock();
+			dq_remove_oldest_string(&debug->dq);
+			chSysUnlock();
+#if USBH_DEBUG_MULTI_HOST
+			USBH_DEBUG_OUTPUT_CALLBACK(host, rdbuff, len);
+#else
+			USBH_DEBUG_OUTPUT_CALLBACK(rdbuff, len);
+#endif
 		}
-		state = 0;
 	}
 }
 
+#if USBH_DEBUG_MULTI_HOST
 void usbDbgInit(USBHDriver *host) {
-	if (host != &USBH_DEBUG_USBHD)
-		return;
-	iqObjectInit(&USBH_DEBUG_USBHD.iq, USBH_DEBUG_USBHD.dbg_buff, sizeof(USBH_DEBUG_USBHD.dbg_buff), 0, 0);
-	chThdCreateStatic(USBH_DEBUG_USBHD.waDebug, sizeof(USBH_DEBUG_USBHD.waDebug), NORMALPRIO, usb_debug_thread, &USBH_DEBUG_USBHD);
+	struct usbh_debug_helper *const debug = &host->debug;
+	void *param = host;
+#else
+void usbDbgInit(void) {
+	struct usbh_debug_helper *const debug = &usbh_debug;
+	void *param = NULL;
+#endif
+	dq_init(&debug->dq, debug->buff, sizeof(debug->buff));
+	debug->on = true;
+	chThdCreateStatic(debug->thd_wa, sizeof(debug->thd_wa),
+			NORMALPRIO, usb_debug_thread, param);
 }
 
 #endif
diff --git a/os/hal/src/usbh/hal_usbh_desciter.c b/os/hal/src/usbh/hal_usbh_desciter.c
index cfce62b..0ccf4e4 100644
--- a/os/hal/src/usbh/hal_usbh_desciter.c
+++ b/os/hal/src/usbh/hal_usbh_desciter.c
@@ -25,7 +25,7 @@
 void cfg_iter_init(generic_iterator_t *icfg, const uint8_t *buff, uint16_t rem) {
 	icfg->valid = 0;
 
-	if ((buff[0] < 2) || (rem < 2) || (rem < buff[0])
+	if ((rem < 2) || (buff[0] < 2) || (rem < buff[0])
 			|| (buff[0] < USBH_DT_CONFIG_SIZE)
 			|| (buff[1] != USBH_DT_CONFIG))
 		return;
@@ -45,14 +45,14 @@ void if_iter_next(if_iterator_t *iif) {
 
 	iif->valid = 0;
 
-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+	if ((rem < 2) || (curr[0] < 2) || (rem < curr[0]))
 		return;
 
 	for (;;) {
 		rem -= curr[0];
 		curr += curr[0];
 
-		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		if ((rem < 2) || (curr[0] < 2) || (rem < curr[0]))
 			return;
 
 		if (curr[1] == USBH_DT_INTERFACE_ASSOCIATION) {
@@ -92,14 +92,14 @@ void ep_iter_next(generic_iterator_t *iep) {
 
 	iep->valid = 0;
 
-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+	if ((rem < 2) || (curr[0] < 2) || (rem < curr[0]))
 		return;
 
 	for (;;) {
 		rem -= curr[0];
 		curr += curr[0];
 
-		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		if ((rem < 2) || (curr[0] < 2) || (rem < curr[0]))
 			return;
 
 		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)
@@ -131,13 +131,13 @@ void cs_iter_next(generic_iterator_t *ics) {
 
 	ics->valid = 0;
 
-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+	if ((rem < 2) || (curr[0] < 2) || (rem < curr[0]))
 		return;
 
 	rem -= curr[0];
 	curr += curr[0];
 
-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+	if ((rem < 2) || (curr[0] < 2) || (rem < curr[0]))
 		return;
 
 	if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)
diff --git a/os/hal/src/usbh/hal_usbh_ftdi.c b/os/hal/src/usbh/hal_usbh_ftdi.c
index b4b03df..1b06405 100644
--- a/os/hal/src/usbh/hal_usbh_ftdi.c
+++ b/os/hal/src/usbh/hal_usbh_ftdi.c
@@ -27,37 +27,13 @@
 #include "usbh/dev/ftdi.h"
 #include "usbh/internal.h"
 
-#if USBHFTDI_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 USBHFTDI_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
+#define _USBH_DEBUG_HELPER_CLASS_DRIVER		ftdipp->ftdip
+#define _USBH_DEBUG_HELPER_ENABLE_TRACE		USBHFTDI_DEBUG_ENABLE_TRACE
+#define _USBH_DEBUG_HELPER_ENABLE_INFO		USBHFTDI_DEBUG_ENABLE_INFO
+#define _USBH_DEBUG_HELPER_ENABLE_WARNINGS	USBHFTDI_DEBUG_ENABLE_WARNINGS
+#define _USBH_DEBUG_HELPER_ENABLE_ERRORS	USBHFTDI_DEBUG_ENABLE_ERRORS
+#include "usbh/debug_helpers.h"
 
-#if USBHFTDI_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 USBHFTDI_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
 
 static void _ftdip_object_init(USBHFTDIPortDriver *ftdipp);
 
@@ -105,7 +81,7 @@ static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *des
 	case 0xE2E6:
 		break;
 	default:
-		uerr("FTDI: Unrecognized PID");
+		udeverr("FTDI: Unrecognized PID");
 		return NULL;
 	}
 
@@ -114,7 +90,7 @@ static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *des
 		return NULL;
 
 	if (((const usbh_interface_descriptor_t *)descriptor)->bInterfaceNumber != 0) {
-		uwarn("FTDI: Will allocate driver along with IF #0");
+		udevwarn("FTDI: Will allocate driver along with IF #0");
 	}
 
 	/* alloc driver */
@@ -125,7 +101,7 @@ static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *des
 		}
 	}
 
-	uwarn("FTDI: Can't alloc driver");
+	udevwarn("FTDI: Can't alloc driver");
 
 	/* can't alloc */
 	return NULL;
@@ -135,24 +111,24 @@ alloc_ok:
 	ftdip->ports = 0;
 	switch (dev->devDesc.bcdDevice) {
 	case 0x200:		//AM
-		uinfo("FTDI: Type A chip");
+		udevinfo("FTDI: Type A chip");
 		ftdip->type = USBHFTDI_TYPE_A;
 		break;
 	case 0x400:		//BM
 	case 0x500:		//2232C
 	case 0x600:		//R
 	case 0x1000:	//230X
-		uinfo("FTDI: Type B chip");
+		udevinfo("FTDI: Type B chip");
 		ftdip->type = USBHFTDI_TYPE_B;
 		break;
 	case 0x700:		//2232H;
 	case 0x800:		//4232H;
 	case 0x900:		//232H;
-		uinfo("FTDI: Type H chip");
+		udevinfo("FTDI: Type H chip");
 		ftdip->type = USBHFTDI_TYPE_H;
 		break;
 	default:
-		uerr("FTDI: Unrecognized chip type");
+		udeverr("FTDI: Unrecognized chip type");
 		return NULL;
 	}
 	usbhEPSetName(&dev->ctrl, "FTD[CTRL]");
@@ -163,11 +139,11 @@ alloc_ok:
 	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor, dev->basicConfigDesc.wTotalLength);
 	for (if_iter_init(&iif, &icfg); iif.valid; if_iter_next(&iif)) {
 		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
-		uinfof("FTDI: Interface #%d", ifdesc->bInterfaceNumber);
+		udevinfof("FTDI: Interface #%d", ifdesc->bInterfaceNumber);
 
 		USBHFTDIPortDriver *const prt = _find_port();
 		if (prt == NULL) {
-			uwarn("\tCan't alloc port for this interface");
+			udevwarn("\tCan't alloc port for this interface");
 			break;
 		}
 
@@ -178,23 +154,23 @@ alloc_ok:
 		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_BULK)) {
-				uinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+				udevinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
 				usbhEPObjectInit(&prt->epin, dev, epdesc);
 				usbhEPSetName(&prt->epin, "FTD[BIN ]");
 			} else if (((epdesc->bEndpointAddress & 0x80) == 0)
 					&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
-				uinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+				udevinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
 				usbhEPObjectInit(&prt->epout, dev, epdesc);
 				usbhEPSetName(&prt->epout, "FTD[BOUT]");
 			} else {
-				uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+				udevinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
 						epdesc->bEndpointAddress, epdesc->bmAttributes);
 			}
 		}
 
 		if ((prt->epin.status != USBH_EPSTATUS_CLOSED)
 				|| (prt->epout.status != USBH_EPSTATUS_CLOSED)) {
-			uwarn("\tCouldn't find endpoints; can't alloc port for this interface");
+			udevwarn("\tCouldn't find endpoints; can't alloc port for this interface");
 			continue;
 		}
 
@@ -336,13 +312,14 @@ static usbh_urbstatus_t _ftdi_port_control(USBHFTDIPortDriver *ftdipp,
 	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, buff, NULL, OSAL_MS2I(1000));
 }
 
-static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t type) {
+static uint32_t _get_divisor(const USBHFTDIPortDriver *ftdipp, uint32_t baud) {
+	usbhftdi_type_t type = ftdipp->ftdip->type;
 	static const uint8_t divfrac[8] = {0, 3, 2, 4, 1, 5, 6, 7};
 	uint32_t divisor;
 
 	if (type == USBHFTDI_TYPE_A) {
 		uint32_t divisor3 = ((48000000UL / 2) + baud / 2) / baud;
-		uinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor3);
+		uclassdrvinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor3);
 		if ((divisor3 & 0x7) == 7)
 			divisor3++; /* round x.7/8 up to x+1 */
 
@@ -359,13 +336,13 @@ static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t type) {
 	} else {
 		if (type == USBHFTDI_TYPE_B) {
 			divisor = ((48000000UL / 2) + baud / 2) / baud;
-			uinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor);
+			uclassdrvinfof("FTDI: desired=%dbps, real=%dbps", baud, (48000000UL / 2) / divisor);
 		} else {
 			/* hi-speed baud rate is 10-bit sampling instead of 16-bit */
 			if (baud < 1200)
 				baud = 1200;
 			divisor = (120000000UL * 8 + baud * 5) / (baud * 10);
-			uinfof("FTDI: desired=%dbps, real=%dbps", baud, (120000000UL * 8) / divisor / 10);
+			uclassdrvinfof("FTDI: desired=%dbps, real=%dbps", baud, (120000000UL * 8) / divisor / 10);
 		}
 		divisor = (divisor >> 3) | (divfrac[divisor & 0x7] << 14);
 
@@ -382,7 +359,7 @@ static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t type) {
 }
 
 static usbh_urbstatus_t _set_baudrate(USBHFTDIPortDriver *ftdipp, uint32_t baudrate) {
-	uint32_t divisor = _get_divisor(baudrate, ftdipp->ftdip->type);
+	uint32_t divisor = _get_divisor(ftdipp, baudrate);
 	uint16_t wValue = (uint16_t)divisor;
 	uint16_t wIndex = (uint16_t)(divisor >> 16);
 	if (ftdipp->ftdip->dev->basicConfigDesc.bNumInterfaces > 1)
@@ -400,7 +377,7 @@ static usbh_urbstatus_t _set_baudrate(USBHFTDIPortDriver *ftdipp, uint32_t baudr
 
 
 static void _submitOutI(USBHFTDIPortDriver *ftdipp, uint32_t len) {
-	udbgf("FTDI: Submit OUT %d", len);
+	uclassdrvdbgf("FTDI: Submit OUT %d", len);
 	ftdipp->oq_urb.requestedLength = len;
 	usbhURBObjectResetI(&ftdipp->oq_urb);
 	usbhURBSubmitI(&ftdipp->oq_urb);
@@ -415,11 +392,11 @@ static void _out_cb(usbh_urb_t *urb) {
 		chThdDequeueNextI(&ftdipp->oq_waiting, Q_OK);
 		return;
 	case USBH_URBSTATUS_DISCONNECTED:
-		uwarn("FTDI: URB OUT disconnected");
+		uurbwarn("FTDI: URB OUT disconnected");
 		chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);
 		return;
 	default:
-		uerrf("FTDI: URB OUT status unexpected = %d", urb->status);
+		uurberrf("FTDI: URB OUT status unexpected = %d", urb->status);
 		break;
 	}
 	usbhURBObjectResetI(&ftdipp->oq_urb);
@@ -493,7 +470,7 @@ static msg_t _put(USBHFTDIPortDriver *ftdipp, uint8_t b) {
 }
 
 static void _submitInI(USBHFTDIPortDriver *ftdipp) {
-	udbg("FTDI: Submit IN");
+	uclassdrvdbg("FTDI: Submit IN");
 	usbhURBObjectResetI(&ftdipp->iq_urb);
 	usbhURBSubmitI(&ftdipp->iq_urb);
 }
@@ -503,9 +480,9 @@ static void _in_cb(usbh_urb_t *urb) {
 	switch (urb->status) {
 	case USBH_URBSTATUS_OK:
 		if (urb->actualLength < 2) {
-			uwarnf("FTDI: URB IN actualLength = %d, < 2", urb->actualLength);
+			uurbwarnf("FTDI: URB IN actualLength = %d, < 2", urb->actualLength);
 		} else if (urb->actualLength > 2) {
-			udbgf("FTDI: URB IN data len=%d, status=%02x %02x",
+			uurbdbgf("FTDI: URB IN data len=%d, status=%02x %02x",
 					urb->actualLength - 2,
 					((uint8_t *)urb->buff)[0],
 					((uint8_t *)urb->buff)[1]);
@@ -514,18 +491,18 @@ static void _in_cb(usbh_urb_t *urb) {
 			chThdDequeueNextI(&ftdipp->iq_waiting, Q_OK);
 			return;
 		} else {
-			udbgf("FTDI: URB IN no data, status=%02x %02x",
+			uurbdbgf("FTDI: URB IN no data, status=%02x %02x",
 					((uint8_t *)urb->buff)[0],
 					((uint8_t *)urb->buff)[1]);
 		//	return;
 		}
 		break;
 	case USBH_URBSTATUS_DISCONNECTED:
-		uwarn("FTDI: URB IN disconnected");
+		uurbwarn("FTDI: URB IN disconnected");
 		chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);
 		return;
 	default:
-		uerrf("FTDI: URB IN status unexpected = %d", urb->status);
+		uurberrf("FTDI: URB IN status unexpected = %d", urb->status);
 		break;
 	}
 	_submitInI(ftdipp);
diff --git a/os/hal/src/usbh/hal_usbh_hid.c b/os/hal/src/usbh/hal_usbh_hid.c
index a34325b..f381b90 100644
--- a/os/hal/src/usbh/hal_usbh_hid.c
+++ b/os/hal/src/usbh/hal_usbh_hid.c
@@ -27,39 +27,12 @@
 #include "usbh/dev/hid.h"
 #include "usbh/internal.h"
 
-#if USBHHID_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 USBHHID_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 USBHHID_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 USBHHID_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
-
-
+#define _USBH_DEBUG_HELPER_CLASS_DRIVER		hidp
+#define _USBH_DEBUG_HELPER_ENABLE_TRACE		USBHHID_DEBUG_ENABLE_TRACE
+#define _USBH_DEBUG_HELPER_ENABLE_INFO		USBHHID_DEBUG_ENABLE_INFO
+#define _USBH_DEBUG_HELPER_ENABLE_WARNINGS	USBHHID_DEBUG_ENABLE_WARNINGS
+#define _USBH_DEBUG_HELPER_ENABLE_ERRORS	USBHHID_DEBUG_ENABLE_ERRORS
+#include "usbh/debug_helpers.h"
 
 #define USBH_HID_REQ_GET_REPORT		0x01
 #define USBH_HID_REQ_GET_IDLE		0x02
@@ -113,7 +86,7 @@ static usbh_baseclassdriver_t *_hid_load(usbh_device_t *dev, const uint8_t *desc
 		}
 	}
 
-	uwarn("Can't alloc HID driver");
+	udevwarn("Can't alloc HID driver");
 
 	/* can't alloc */
 	return NULL;
@@ -136,7 +109,7 @@ alloc_ok:
 	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)) {
-			uinfof("INT IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			udevinfof("INT IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
 			usbhEPObjectInit(&hidp->epin, dev, epdesc);
 			usbhEPSetName(&hidp->epin, "HID[IIN ]");
 #if HAL_USBHHID_USE_INTERRUPT_OUT
@@ -147,7 +120,7 @@ alloc_ok:
 			usbhEPSetName(&hidp->epout, "HID[IOUT]");
 #endif
 		} else {
-			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+			udevinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
 					epdesc->bEndpointAddress, epdesc->bmAttributes);
 		}
 	}
@@ -157,19 +130,19 @@ alloc_ok:
 
 	if (ifdesc->bInterfaceSubClass != 0x01) {
 		hidp->type = USBHHID_DEVTYPE_GENERIC;
-		uinfof("HID: bInterfaceSubClass=%02x, generic HID", ifdesc->bInterfaceSubClass);
+		udevinfof("HID: bInterfaceSubClass=%02x, generic HID", ifdesc->bInterfaceSubClass);
 		if (ifdesc->bInterfaceSubClass != 0x00) {
-			uinfof("HID: bInterfaceSubClass=%02x is an invalid bInterfaceSubClass value",
+			udevinfof("HID: bInterfaceSubClass=%02x is an invalid bInterfaceSubClass value",
 					ifdesc->bInterfaceSubClass);
 		}
 	} else if (ifdesc->bInterfaceProtocol == 0x01) {
 		hidp->type = USBHHID_DEVTYPE_BOOT_KEYBOARD;
-		uinfo("HID: BOOT protocol keyboard found");
+		udevinfo("HID: BOOT protocol keyboard found");
 	} else if (ifdesc->bInterfaceProtocol == 0x02) {
 		hidp->type = USBHHID_DEVTYPE_BOOT_MOUSE;
-		uinfo("HID: BOOT protocol mouse found");
+		udevinfo("HID: BOOT protocol mouse found");
 	} else {
-		uerrf("HID: bInterfaceProtocol=%02x is an invalid boot protocol, abort",
+		udeverrf("HID: bInterfaceProtocol=%02x is an invalid boot protocol, abort",
 				ifdesc->bInterfaceProtocol);
 		goto deinit;
 	}
@@ -200,14 +173,14 @@ static void _in_cb(usbh_urb_t *urb) {
 		}
 		break;
 	case USBH_URBSTATUS_DISCONNECTED:
-		uwarn("HID: URB IN disconnected");
+		uurbwarn("HID: URB IN disconnected");
 
 		return;
 	case USBH_URBSTATUS_TIMEOUT:
 		//no data
 		break;
 	default:
-		uerrf("HID: URB IN status unexpected = %d", urb->status);
+		uurberrf("HID: URB IN status unexpected = %d", urb->status);
 		break;
 	}
 	usbhURBObjectResetI(&hidp->in_urb);
diff --git a/os/hal/src/usbh/hal_usbh_hub.c b/os/hal/src/usbh/hal_usbh_hub.c
index 900c6f8..b8937fa 100644
--- a/os/hal/src/usbh/hal_usbh_hub.c
+++ b/os/hal/src/usbh/hal_usbh_hub.c
@@ -27,38 +27,12 @@
 #include "usbh/dev/hub.h"
 #include "usbh/internal.h"
 
-#if USBHHUB_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 USBHHUB_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 USBHHUB_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 USBHHUB_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
-
+#define _USBH_DEBUG_HELPER_CLASS_DRIVER		hubp
+#define _USBH_DEBUG_HELPER_ENABLE_TRACE		USBHHUB_DEBUG_ENABLE_TRACE
+#define _USBH_DEBUG_HELPER_ENABLE_INFO		USBHHUB_DEBUG_ENABLE_INFO
+#define _USBH_DEBUG_HELPER_ENABLE_WARNINGS	USBHHUB_DEBUG_ENABLE_WARNINGS
+#define _USBH_DEBUG_HELPER_ENABLE_ERRORS	USBHHUB_DEBUG_ENABLE_ERRORS
+#include "usbh/debug_helpers.h"
 
 USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];
 static usbh_port_t USBHPorts[HAL_USBHHUB_MAX_PORTS];
@@ -106,13 +80,13 @@ static void _urb_complete(usbh_urb_t *urb) {
 	switch (urb->status) {
 	case USBH_URBSTATUS_TIMEOUT:
 		/* the device NAKed */
-		udbg("HUB: no info");
+		uurbdbg("HUB: no info");
 		//hubdp->statuschange = 0;
 		break;
 	case USBH_URBSTATUS_OK: {
 		uint8_t len = hubdp->hubDesc.bNbrPorts / 8 + 1;
 		if (urb->actualLength != len) {
-			uwarnf("Expected %d status change bytes but got %d", len, urb->actualLength);
+			uurbwarnf("Expected %d status change bytes but got %d", len, urb->actualLength);
 		}
 
 		if (urb->actualLength < len)
@@ -126,13 +100,13 @@ static void _urb_complete(usbh_urb_t *urb) {
 		while (len--)
 			*sc++ |= *r++;
 
-		uinfof("HUB: change, %08x", hubdp->statuschange);
+		uurbinfof("HUB: change, %08x", hubdp->statuschange);
 	}	break;
 	case USBH_URBSTATUS_DISCONNECTED:
-		uwarn("HUB: URB disconnected, aborting poll");
+		uurbwarn("HUB: URB disconnected, aborting poll");
 		return;
 	default:
-		uerrf("HUB: URB status unexpected = %d", urb->status);
+		uurberrf("HUB: URB status unexpected = %d", urb->status);
 		break;
 	}
 
@@ -181,7 +155,7 @@ static usbh_baseclassdriver_t *_hub_load(usbh_device_t *dev,
 		}
 	}
 
-	uwarn("Can't alloc HUB driver");
+	udevwarn("Can't alloc HUB driver");
 
 	/* can't alloc */
 	return NULL;
@@ -195,7 +169,7 @@ alloc_ok:
 	usbhEPSetName(&dev->ctrl, "HUB[CTRL]");
 
 	/* read Hub descriptor */
-	uinfo("Read Hub descriptor");
+	udevinfo("Read Hub descriptor");
 	if (usbhhubControlRequest(dev->host, hubdp,
 			USBH_REQTYPE_DIR_IN | USBH_REQTYPE_TYPE_CLASS | USBH_REQTYPE_RECIP_DEVICE,
 			USBH_REQ_GET_DESCRIPTOR,
@@ -207,7 +181,7 @@ alloc_ok:
 
 	const usbh_hub_descriptor_t *const hubdesc = &hubdp->hubDesc;
 
-	uinfof("Hub descriptor loaded; %d ports, wHubCharacteristics=%04x, bPwrOn2PwrGood=%d, bHubContrCurrent=%d",
+	udevinfof("Hub descriptor loaded; %d ports, wHubCharacteristics=%04x, bPwrOn2PwrGood=%d, bHubContrCurrent=%d",
 			hubdesc->bNbrPorts,
 			hubdesc->wHubCharacteristics,
 			hubdesc->bPwrOn2PwrGood,
@@ -217,7 +191,7 @@ alloc_ok:
 	uint8_t ports = hubdesc->bNbrPorts;
 	for (i = 0; (ports > 0) && (i < HAL_USBHHUB_MAX_PORTS); i++) {
 		if (USBHPorts[i].hub == NULL) {
-			uinfof("Alloc port %d", ports);
+			udevinfof("Alloc port %d", ports);
 			_usbhub_port_object_init(&USBHPorts[i], dev->host, hubdp, ports);
 			USBHPorts[i].next = hubdp->ports;
 			hubdp->ports = &USBHPorts[i];
@@ -226,7 +200,7 @@ alloc_ok:
 	}
 
 	if (ports) {
-		uwarn("Could not alloc all ports");
+		udevwarn("Could not alloc all ports");
 	}
 
 	/* link hub to the host's list */
@@ -235,7 +209,7 @@ alloc_ok:
 	/* enable power to ports */
 	usbh_port_t *port = hubdp->ports;
 	while (port) {
-		uinfof("Enable power for port %d", port->number);
+		udevinfof("Enable power for port %d", port->number);
 		usbhhubSetFeaturePort(port, USBH_PORT_FEAT_POWER);
 		port = port->next;
 	}
diff --git a/os/hal/src/usbh/hal_usbh_msd.c b/os/hal/src/usbh/hal_usbh_msd.c
index 230bf9a..081cccb 100644
--- a/os/hal/src/usbh/hal_usbh_msd.c
+++ b/os/hal/src/usbh/hal_usbh_msd.c
@@ -27,37 +27,12 @@
 #include "usbh/dev/msd.h"
 #include "usbh/internal.h"
 
-#if USBHMSD_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 USBHMSD_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 USBHMSD_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 USBHMSD_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
+#define _USBH_DEBUG_HELPER_CLASS_DRIVER		msdp
+#define _USBH_DEBUG_HELPER_ENABLE_TRACE		USBHMSD_DEBUG_ENABLE_TRACE
+#define _USBH_DEBUG_HELPER_ENABLE_INFO		USBHMSD_DEBUG_ENABLE_INFO
+#define _USBH_DEBUG_HELPER_ENABLE_WARNINGS	USBHMSD_DEBUG_ENABLE_WARNINGS
+#define _USBH_DEBUG_HELPER_ENABLE_ERRORS	USBHMSD_DEBUG_ENABLE_ERRORS
+#include "usbh/debug_helpers.h"
 
 static void _lun_object_deinit(USBHMassStorageLUNDriver *lunp);
 
@@ -122,7 +97,7 @@ static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *desc
 		}
 	}
 
-	uwarn("Can't alloc MSD driver");
+	udevwarn("Can't alloc MSD driver");
 
 	/* can't alloc */
 	return NULL;
@@ -146,16 +121,16 @@ alloc_ok:
 	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_BULK)) {
-			uinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			udevinfof("BULK IN endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
 			usbhEPObjectInit(&msdp->epin, dev, epdesc);
 			usbhEPSetName(&msdp->epin, "MSD[BIN ]");
 		} else if (((epdesc->bEndpointAddress & 0x80) == 0)
 				&& (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
-			uinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
+			udevinfof("BULK OUT endpoint found: bEndpointAddress=%02x", epdesc->bEndpointAddress);
 			usbhEPObjectInit(&msdp->epout, dev, epdesc);
 			usbhEPSetName(&msdp->epout, "MSD[BOUT]");
 		} else {
-			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+			udevinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
 					epdesc->bEndpointAddress, epdesc->bmAttributes);
 		}
 	}
@@ -164,23 +139,23 @@ alloc_ok:
 	}
 
 	/* read the number of LUNs */
-	uinfo("Reading Max LUN:");
+	udevinfo("Reading Max LUN:");
 	USBH_DEFINE_BUFFER(uint8_t buff[4]);
 	stat = usbhControlRequest(dev,
 			USBH_REQTYPE_CLASSIN(USBH_REQTYPE_RECIP_INTERFACE),
 			MSD_GET_MAX_LUN, 0, msdp->ifnum, 1, buff);
 	if (stat == USBH_URBSTATUS_OK) {
 		msdp->max_lun = buff[0] + 1;
-		uinfof("\tmax_lun = %d", msdp->max_lun);
+		udevinfof("\tmax_lun = %d", msdp->max_lun);
 		if (msdp->max_lun > HAL_USBHMSD_MAX_LUNS) {
 			msdp->max_lun = HAL_USBHMSD_MAX_LUNS;
-			uwarnf("\tUsing max_lun = %d", msdp->max_lun);
+			udevwarnf("\tUsing max_lun = %d", msdp->max_lun);
 		}
 	} else if (stat == USBH_URBSTATUS_STALL) {
-		uwarn("\tStall, max_lun = 1");
+		udevwarn("\tStall, max_lun = 1");
 		msdp->max_lun = 1;
 	} else {
-		uerr("\tError");
+		udeverr("\tError");
 		goto deinit;
 	}
 
@@ -293,28 +268,30 @@ static bool _msd_bot_reset(USBHMassStorageDriver *msdp) {
 
 static msd_bot_result_t _msd_bot_transaction(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp, void *data) {
 
+	USBHMassStorageDriver *const msdp = lunp->msdp;
+
 	uint32_t data_actual_len, actual_len;
 	usbh_urbstatus_t status;
 	USBH_DEFINE_BUFFER(msd_csw_t csw);
 
-	tran->cbw->bCBWLUN = (uint8_t)(lunp - &lunp->msdp->luns[0]);
+	tran->cbw->bCBWLUN = (uint8_t)(lunp - &msdp->luns[0]);
 	tran->cbw->dCBWSignature = MSD_CBW_SIGNATURE;
-	tran->cbw->dCBWTag = ++lunp->msdp->tag;
+	tran->cbw->dCBWTag = ++msdp->tag;
 	tran->data_processed = 0;
 
 	/* control phase */
-	status = usbhBulkTransfer(&lunp->msdp->epout, tran->cbw,
+	status = usbhBulkTransfer(&msdp->epout, tran->cbw,
 					sizeof(*tran->cbw), &actual_len, OSAL_MS2I(1000));
 
 	if (status == USBH_URBSTATUS_CANCELLED) {
-		uerr("\tMSD: Control phase: USBH_URBSTATUS_CANCELLED");
+		uclassdrverr("\tMSD: Control phase: USBH_URBSTATUS_CANCELLED");
 		return MSD_BOTRESULT_DISCONNECTED;
 	}
 
 	if ((status != USBH_URBSTATUS_OK) || (actual_len != sizeof(*tran->cbw))) {
-		uerrf("\tMSD: Control phase: status = %d (!= OK), actual_len = %d (expected to send %d)",
+		uclassdrverrf("\tMSD: Control phase: status = %d (!= OK), actual_len = %d (expected to send %d)",
 				status, actual_len, sizeof(*tran->cbw));
-		_msd_bot_reset(lunp->msdp);
+		_msd_bot_reset(msdp);
 		return MSD_BOTRESULT_ERROR;
 	}
 
@@ -322,7 +299,7 @@ static msd_bot_result_t _msd_bot_transaction(msd_transaction_t *tran, USBHMassSt
 	/* data phase */
 	data_actual_len = 0;
 	if (tran->cbw->dCBWDataTransferLength) {
-		usbh_ep_t *const ep = tran->cbw->bmCBWFlags & MSD_CBWFLAGS_D2H ? &lunp->msdp->epin : &lunp->msdp->epout;
+		usbh_ep_t *const ep = tran->cbw->bmCBWFlags & MSD_CBWFLAGS_D2H ? &msdp->epin : &msdp->epout;
 		status = usbhBulkTransfer(
 				ep,
 				data,
@@ -330,62 +307,62 @@ static msd_bot_result_t _msd_bot_transaction(msd_transaction_t *tran, USBHMassSt
 				&data_actual_len, OSAL_MS2I(20000));
 
 		if (status == USBH_URBSTATUS_CANCELLED) {
-			uerr("\tMSD: Data phase: USBH_URBSTATUS_CANCELLED");
+			uclassdrverr("\tMSD: Data phase: USBH_URBSTATUS_CANCELLED");
 			return MSD_BOTRESULT_DISCONNECTED;
 		}
 
 		if (status == USBH_URBSTATUS_STALL) {
-			uerrf("\tMSD: Data phase: USBH_URBSTATUS_STALL, clear halt");
+			uclassdrverrf("\tMSD: Data phase: USBH_URBSTATUS_STALL, clear halt");
 			status = (usbhEPReset(ep) == HAL_SUCCESS) ? USBH_URBSTATUS_OK : USBH_URBSTATUS_ERROR;
 		}
 
 		if (status != USBH_URBSTATUS_OK) {
-			uerrf("\tMSD: Data phase: status = %d (!= OK), resetting", status);
-			_msd_bot_reset(lunp->msdp);
+			uclassdrverrf("\tMSD: Data phase: status = %d (!= OK), resetting", status);
+			_msd_bot_reset(msdp);
 			return MSD_BOTRESULT_ERROR;
 		}
 	}
 
 
 	/* status phase */
-	status = usbhBulkTransfer(&lunp->msdp->epin, &csw,
+	status = usbhBulkTransfer(&msdp->epin, &csw,
 				sizeof(csw), &actual_len, OSAL_MS2I(1000));
 
 	if (status == USBH_URBSTATUS_STALL) {
-		uwarn("\tMSD: Status phase: USBH_URBSTATUS_STALL, clear halt and retry");
+		uclassdrvwarn("\tMSD: Status phase: USBH_URBSTATUS_STALL, clear halt and retry");
 
-		status = (usbhEPReset(&lunp->msdp->epin) == HAL_SUCCESS) ? USBH_URBSTATUS_OK : USBH_URBSTATUS_ERROR;
+		status = (usbhEPReset(&msdp->epin) == HAL_SUCCESS) ? USBH_URBSTATUS_OK : USBH_URBSTATUS_ERROR;
 
 		if (status == USBH_URBSTATUS_OK) {
-			status = usbhBulkTransfer(&lunp->msdp->epin, &csw,
+			status = usbhBulkTransfer(&msdp->epin, &csw,
 						sizeof(csw), &actual_len, OSAL_MS2I(1000));
 		}
 	}
 
 	if (status == USBH_URBSTATUS_CANCELLED) {
-		uerr("\tMSD: Status phase: USBH_URBSTATUS_CANCELLED");
+		uclassdrverr("\tMSD: Status phase: USBH_URBSTATUS_CANCELLED");
 		return MSD_BOTRESULT_DISCONNECTED;
 	}
 
 	if (status != USBH_URBSTATUS_OK) {
-		uerrf("\tMSD: Status phase: status = %d (!= OK), resetting", status);
-		_msd_bot_reset(lunp->msdp);
+		uclassdrverrf("\tMSD: Status phase: status = %d (!= OK), resetting", status);
+		_msd_bot_reset(msdp);
 		return MSD_BOTRESULT_ERROR;
 	}
 
 	/* validate CSW */
 	if ((actual_len != sizeof(csw))
 		|| (csw.dCSWSignature != MSD_CSW_SIGNATURE)
-		|| (csw.dCSWTag != lunp->msdp->tag)
+		|| (csw.dCSWTag != msdp->tag)
 		|| (csw.bCSWStatus >= CSW_STATUS_PHASE_ERROR)) {
 		/* CSW is not valid */
-		uerrf("\tMSD: Status phase: Invalid CSW: len=%d, dCSWSignature=%x, dCSWTag=%x (expected %x), bCSWStatus=%d, resetting",
+		uclassdrverrf("\tMSD: Status phase: Invalid CSW: len=%d, dCSWSignature=%x, dCSWTag=%x (expected %x), bCSWStatus=%d, resetting",
 				actual_len,
 				csw.dCSWSignature,
 				csw.dCSWTag,
-				lunp->msdp->tag,
+				msdp->tag,
 				csw.bCSWStatus);
-		_msd_bot_reset(lunp->msdp);
+		_msd_bot_reset(msdp);
 		return MSD_BOTRESULT_ERROR;
 	}
 
@@ -393,17 +370,17 @@ static msd_bot_result_t _msd_bot_transaction(msd_transaction_t *tran, USBHMassSt
 	if ((csw.bCSWStatus != CSW_STATUS_PHASE_ERROR)
 			&& (csw.dCSWDataResidue > tran->cbw->dCBWDataTransferLength)) {
 		/* CSW is not meaningful */
-		uerrf("\tMSD: Status phase: CSW not meaningful: bCSWStatus=%d, dCSWDataResidue=%u, dCBWDataTransferLength=%u, resetting",
+		uclassdrverrf("\tMSD: Status phase: CSW not meaningful: bCSWStatus=%d, dCSWDataResidue=%u, dCBWDataTransferLength=%u, resetting",
 				csw.bCSWStatus,
 				csw.dCSWDataResidue,
 				tran->cbw->dCBWDataTransferLength);
-		_msd_bot_reset(lunp->msdp);
+		_msd_bot_reset(msdp);
 		return MSD_BOTRESULT_ERROR;
 	}
 
 	if (csw.bCSWStatus == CSW_STATUS_PHASE_ERROR) {
-		uerr("\tMSD: Status phase: Phase error, resetting");
-		_msd_bot_reset(lunp->msdp);
+		uclassdrverr("\tMSD: Status phase: Phase error, resetting");
+		_msd_bot_reset(msdp);
 		return MSD_BOTRESULT_ERROR;
 	}
 
@@ -502,6 +479,9 @@ static msd_result_t scsi_requestsense(USBHMassStorageLUNDriver *lunp, scsi_sense
 static msd_result_t _scsi_perform_transaction(USBHMassStorageLUNDriver *lunp,
 		msd_transaction_t *transaction, void *data) {
 
+	USBHMassStorageDriver *const msdp = lunp->msdp;
+	(void)msdp;
+
 	msd_bot_result_t res;
 	res = _msd_bot_transaction(transaction, lunp, data);
 	if (res != MSD_BOTRESULT_OK) {
@@ -511,13 +491,11 @@ static msd_result_t _scsi_perform_transaction(USBHMassStorageLUNDriver *lunp,
 	if (transaction->csw_status == CSW_STATUS_FAILED) {
 		if (transaction->cbw->CBWCB[0] != SCSI_CMD_REQUEST_SENSE) {
 			/* do auto-sense (except for SCSI_CMD_REQUEST_SENSE!) */
-			uwarn("\tMSD: Command failed, auto-sense");
+			uclassdrvwarn("\tMSD: Command failed, auto-sense");
 			USBH_DEFINE_BUFFER(scsi_sense_response_t sense);
 			if (scsi_requestsense(lunp, &sense) == MSD_RESULT_OK) {
-				uwarnf("\tMSD: REQUEST SENSE: Sense key=%x, ASC=%02x, ASCQ=%02x",
+				uclassdrvwarnf("\tMSD: REQUEST SENSE: Sense key=%x, ASC=%02x, ASCQ=%02x",
 						sense.byte[2] & 0xf, sense.byte[12], sense.byte[13]);
-
-				return MSD_RESULT_OK;
 			}
 		}
 		return MSD_RESULT_FAILED;
@@ -724,6 +702,9 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 	osalDbgCheck(lunp->msdp != NULL);
 	msd_result_t res;
 
+	USBHMassStorageDriver *const msdp = lunp->msdp;
+	(void)msdp;
+
 	chSemWait(&lunp->sem);
 	osalDbgAssert((lunp->state == BLK_READY) || (lunp->state == BLK_ACTIVE), "invalid state");
 	if (lunp->state == BLK_READY) {
@@ -734,7 +715,7 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 
     {
 		USBH_DEFINE_BUFFER(scsi_inquiry_response_t inq);
-		uinfo("INQUIRY...");
+		uclassdrvinfo("INQUIRY...");
 		res = scsi_inquiry(lunp, &inq);
 		if (res == MSD_RESULT_DISCONNECTED) {
 			goto failed;
@@ -746,9 +727,9 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 			goto failed;
 		}
 
-		uinfof("\tPDT=%02x", inq.peripheral & 0x1f);
+		uclassdrvinfof("\tPDT=%02x", inq.peripheral & 0x1f);
 		if (inq.peripheral != 0) {
-			uerr("\tUnsupported PDT");
+			uclassdrverr("\tUnsupported PDT");
 			goto failed;
 		}
 	}
@@ -756,7 +737,7 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 	// Test if unit ready
     uint8_t i;
 	for (i = 0; i < 10; i++) {
-		uinfo("TEST UNIT READY...");
+		uclassdrvinfo("TEST UNIT READY...");
 		res = scsi_testunitready(lunp);
 		if (res == MSD_RESULT_DISCONNECTED) {
 			goto failed;
@@ -764,11 +745,11 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 			//retry?
 			goto failed;
 		} else if (res == MSD_RESULT_FAILED) {
-			uinfo("\tTEST UNIT READY: Command Failed, retry");
+			uclassdrvinfo("\tTEST UNIT READY: Command Failed, retry");
 			osalThreadSleepMilliseconds(200);
 			continue;
 		}
-		uinfo("\tReady.");
+		uclassdrvinfo("\tReady.");
 		break;
 	}
 	if (i == 10) goto failed;
@@ -776,7 +757,7 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 	{
 		USBH_DEFINE_BUFFER(scsi_readcapacity10_response_t cap);
 		// Read capacity
-		uinfo("READ CAPACITY(10)...");
+		uclassdrvinfo("READ CAPACITY(10)...");
 		res = scsi_readcapacity10(lunp, &cap);
 		if (res == MSD_RESULT_DISCONNECTED) {
 			goto failed;
@@ -792,17 +773,17 @@ bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
 		lunp->info.blk_num = __REV(cap.last_block_addr) + 1;
 	}
 
-	uinfof("\tBlock size=%dbytes, blocks=%u (~%u MB)", lunp->info.blk_size, lunp->info.blk_num,
+	uclassdrvinfof("\tBlock size=%dbytes, blocks=%u (~%u MB)", lunp->info.blk_size, lunp->info.blk_num,
 		(uint32_t)(((uint64_t)lunp->info.blk_size * lunp->info.blk_num) / (1024UL * 1024UL)));
 
-	uinfo("MSD Connected.");
+	uclassdrvinfo("MSD Connected.");
 	lunp->state = BLK_READY;
 	chSemSignal(&lunp->sem);
 	return HAL_SUCCESS;
 
   /* Connection failed, state reset to BLK_ACTIVE.*/
 failed:
-	uinfo("MSD Connect failed.");
+	uclassdrvinfo("MSD Connect failed.");
 	lunp->state = BLK_ACTIVE;
 	chSemSignal(&lunp->sem);
 	return HAL_FAILED;
@@ -949,6 +930,10 @@ bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp) {
 	return FALSE;
 }
 
+USBHDriver *usbhmsdLUNGetHost(const USBHMassStorageLUNDriver *lunp) {
+	return lunp->msdp->dev->host;
+}
+
 static void _msd_object_init(USBHMassStorageDriver *msdp) {
 	osalDbgCheck(msdp != NULL);
 	memset(msdp, 0, sizeof(*msdp));
diff --git a/os/hal/src/usbh/hal_usbh_uvc.c b/os/hal/src/usbh/hal_usbh_uvc.c
index ca16c66..3a8b488 100644
--- a/os/hal/src/usbh/hal_usbh_uvc.c
+++ b/os/hal/src/usbh/hal_usbh_uvc.c
@@ -31,38 +31,12 @@
 #include "usbh/dev/uvc.h"
 #include "usbh/internal.h"
 
-#if USBHUVC_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 USBHUVC_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 USBHUVC_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 USBHUVC_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
-
+#define _USBH_DEBUG_HELPER_CLASS_DRIVER		uvcdp
+#define _USBH_DEBUG_HELPER_ENABLE_TRACE		USBHUVC_DEBUG_ENABLE_TRACE
+#define _USBH_DEBUG_HELPER_ENABLE_INFO		USBHUVC_DEBUG_ENABLE_INFO
+#define _USBH_DEBUG_HELPER_ENABLE_WARNINGS	USBHUVC_DEBUG_ENABLE_WARNINGS
+#define _USBH_DEBUG_HELPER_ENABLE_ERRORS	USBHUVC_DEBUG_ENABLE_ERRORS
+#include "usbh/debug_helpers.h"
 
 USBHUVCDriver USBHUVCD[HAL_USBHUVC_MAX_INSTANCES];
 
@@ -124,7 +98,7 @@ bool usbhuvcVSRequest(USBHUVCDriver *uvcdp,
 static bool _set_vs_alternate(USBHUVCDriver *uvcdp, uint16_t min_ep_size) {
 
 	if (min_ep_size == 0) {
-		uinfo("Selecting Alternate setting 0");
+		uclassdrvinfo("Selecting Alternate setting 0");
 		return usbhStdReqSetInterface(uvcdp->dev, if_get(&uvcdp->ivs)->bInterfaceNumber, 0);
 	}
 
@@ -134,7 +108,7 @@ static bool _set_vs_alternate(USBHUVCDriver *uvcdp, uint16_t min_ep_size) {
 	uint8_t alt = 0;
 	uint16_t sz = 0xffff;
 
-	uinfof("Searching alternate setting with min_ep_size=%d", min_ep_size);
+	uclassdrvinfof("Searching alternate setting with min_ep_size=%d", min_ep_size);
 
 	for (; iif.valid; if_iter_next(&iif)) {
 		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
@@ -143,7 +117,7 @@ static bool _set_vs_alternate(USBHUVCDriver *uvcdp, uint16_t min_ep_size) {
 				|| (ifdesc->bInterfaceSubClass != UVC_SC_VIDEOSTREAMING))
 			continue;
 
-		uinfof("\tScanning alternate setting=%d", ifdesc->bAlternateSetting);
+		uclassdrvinfof("\tScanning alternate setting=%d", ifdesc->bAlternateSetting);
 
 		if (ifdesc->bNumEndpoints == 0)
 			continue;
@@ -153,11 +127,11 @@ static bool _set_vs_alternate(USBHUVCDriver *uvcdp, uint16_t min_ep_size) {
 			if (((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_ISO)
 					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
 
-				uinfof("\t  Endpoint wMaxPacketSize = %d", epdesc->wMaxPacketSize);
+				uclassdrvinfof("\t  Endpoint wMaxPacketSize = %d", epdesc->wMaxPacketSize);
 
 				if (epdesc->wMaxPacketSize >= min_ep_size) {
 					if (epdesc->wMaxPacketSize < sz) {
-						uinfo("\t    Found new optimal alternate setting");
+						uclassdrvinfo("\t    Found new optimal alternate setting");
 						sz = epdesc->wMaxPacketSize;
 						alt = ifdesc->bAlternateSetting;
 						ep = epdesc;
@@ -168,7 +142,7 @@ static bool _set_vs_alternate(USBHUVCDriver *uvcdp, uint16_t min_ep_size) {
 	}
 
 	if (ep && alt) {
-		uinfof("\tSelecting Alternate setting %d", alt);
+		uclassdrvinfof("\tSelecting Alternate setting %d", alt);
 		if (usbhStdReqSetInterface(uvcdp->dev, if_get(&uvcdp->ivs)->bInterfaceNumber, alt) == HAL_SUCCESS) {
 			usbhEPObjectInit(&uvcdp->ep_iso, uvcdp->dev, ep);
 			usbhEPSetName(&uvcdp->ep_iso, "UVC[ISO ]");
@@ -180,20 +154,22 @@ static bool _set_vs_alternate(USBHUVCDriver *uvcdp, uint16_t min_ep_size) {
 }
 
 #if	USBH_DEBUG_ENABLE && USBHUVC_DEBUG_ENABLE_INFO
-void usbhuvcPrintProbeCommit(const usbh_uvc_ctrl_vs_probecommit_data_t *pc) {
+void usbhuvcPrintProbeCommit(USBHUVCDriver *uvcdp,
+		const usbh_uvc_ctrl_vs_probecommit_data_t *pc) {
+	(void)uvcdp;
 
 	//uinfof("UVC: probe/commit data:");
-	uinfof("\tbmHint=%04x", pc->bmHint);
-	uinfof("\tbFormatIndex=%d, bFrameIndex=%d, dwFrameInterval=%u",
+	uclassdrvinfof("\tbmHint=%04x", pc->bmHint);
+	uclassdrvinfof("\tbFormatIndex=%d, bFrameIndex=%d, dwFrameInterval=%u",
 			pc->bFormatIndex, pc->bFrameIndex, pc->dwFrameInterval);
-	uinfof("\twKeyFrameRate=%d, wPFrameRate=%d, wCompQuality=%u, wCompWindowSize=%u",
+	uclassdrvinfof("\twKeyFrameRate=%d, wPFrameRate=%d, wCompQuality=%u, wCompWindowSize=%u",
 			pc->wKeyFrameRate, pc->wPFrameRate, pc->wCompQuality, pc->wCompWindowSize);
-	uinfof("\twDelay=%d", pc->wDelay);
-	uinfof("\tdwMaxVideoFrameSize=%u", pc->dwMaxVideoFrameSize);
-	uinfof("\tdwMaxPayloadTransferSize=%u", pc->dwMaxPayloadTransferSize);
-/*	uinfof("\tdwClockFrequency=%u", pc->dwClockFrequency);
-	uinfof("\tbmFramingInfo=%02x", pc->bmFramingInfo);
-	uinfof("\tbPreferedVersion=%d, bMinVersion=%d, bMaxVersion=%d",
+	uclassdrvinfof("\twDelay=%d", pc->wDelay);
+	uclassdrvinfof("\tdwMaxVideoFrameSize=%u", pc->dwMaxVideoFrameSize);
+	uclassdrvinfof("\tdwMaxPayloadTransferSize=%u", pc->dwMaxPayloadTransferSize);
+/*	uclassdrvinfof("\tdwClockFrequency=%u", pc->dwClockFrequency);
+	uclassdrvinfof("\tbmFramingInfo=%02x", pc->bmFramingInfo);
+	uclassdrvinfof("\tbPreferedVersion=%d, bMinVersion=%d, bMaxVersion=%d",
 			pc->bPreferedVersion, pc->bMinVersion, pc->bMaxVersion); */
 }
 #endif
@@ -205,7 +181,8 @@ static void _post(USBHUVCDriver *uvcdp, usbh_urb_t *urb, memory_pool_t *mp, uint
 	usbhuvc_message_base_t *const new_msg = (usbhuvc_message_base_t *)chPoolAllocI(mp);
 	if (new_msg != NULL) {
 		/* allocated the new buffer, now try to post the message to the mailbox */
-		if (chMBPostI(&uvcdp->mb, (msg_t)msg) == MSG_OK) {
+		msg_t r = chMBPostI(&uvcdp->mb, (msg_t)msg);
+		if (r == MSG_OK) {
 			/* everything OK, complete the missing fields */
 			msg->type = type;
 			msg->length = urb->actualLength;
@@ -213,12 +190,16 @@ static void _post(USBHUVCDriver *uvcdp, usbh_urb_t *urb, memory_pool_t *mp, uint
 			/* change the URB's buffer to the newly allocated one */
 			urb->buff = ((usbhuvc_message_data_t *)new_msg)->data;
 		} else {
+			if (r == MSG_RESET) {
+				uurbwarn("UVC: error, mailbox reset");
+			} else {
+				uurberr("UVC: error, mailbox overrun");
+			}
 			/* couldn't post the message, free the newly allocated buffer */
-			uerr("UVC: error, mailbox overrun");
 			chPoolFreeI(&uvcdp->mp_status, new_msg);
 		}
 	} else {
-		uerrf("UVC: error, %s pool overrun", mp == &uvcdp->mp_data ? "data" : "status");
+		uurberrf("UVC: error, %s pool overrun", mp == &uvcdp->mp_data ? "data" : "status");
 	}
 }
 
@@ -230,18 +211,18 @@ static void _cb_int(usbh_urb_t *urb) {
 		if (urb->actualLength >= 2) {
 			_post(uvcdp, urb, &uvcdp->mp_status, USBHUVC_MESSAGETYPE_STATUS);
 		} else {
-			uerrf("UVC: INT IN, actualLength=%d", urb->actualLength);
+			uurberrf("UVC: INT IN, actualLength=%d", urb->actualLength);
 		}
 		break;
 	case USBH_URBSTATUS_TIMEOUT:	/* the device NAKed */
-		udbg("UVC: INT IN no info");
+		uurbdbg("UVC: INT IN no info");
 		break;
 	case USBH_URBSTATUS_DISCONNECTED:
 	case USBH_URBSTATUS_CANCELLED:
-		uwarn("UVC: INT IN status = DISCONNECTED/CANCELLED, aborting");
+		uurbwarn("UVC: INT IN status = DISCONNECTED/CANCELLED, aborting");
 		return;
 	default:
-		uerrf("UVC: INT IN error, unexpected status = %d", urb->status);
+		uurberrf("UVC: INT IN error, unexpected status = %d", urb->status);
 		break;
 	}
 
@@ -254,20 +235,20 @@ static void _cb_iso(usbh_urb_t *urb) {
 
 	if ((urb->status == USBH_URBSTATUS_DISCONNECTED)
 			|| (urb->status == USBH_URBSTATUS_CANCELLED)) {
-		uwarn("UVC: ISO IN status = DISCONNECTED/CANCELLED, aborting");
+		uurbwarn("UVC: ISO IN status = DISCONNECTED/CANCELLED, aborting");
 		return;
 	}
 
 	if (urb->status != USBH_URBSTATUS_OK) {
-		uerrf("UVC: ISO IN error, unexpected status = %d", urb->status);
+		uurberrf("UVC: ISO IN error, unexpected status = %d", urb->status);
 	} else if (urb->actualLength >= 2) {
 		const uint8_t *const buff = (const uint8_t *)urb->buff;
 		if (buff[0] < 2) {
-			uerrf("UVC: ISO IN, bHeaderLength=%d", buff[0]);
+			uurberrf("UVC: ISO IN, bHeaderLength=%d", buff[0]);
 		} else if (buff[0] > urb->actualLength) {
-			uerrf("UVC: ISO IN, bHeaderLength=%d > actualLength=%d", buff[0], urb->actualLength);
+			uurberrf("UVC: ISO IN, bHeaderLength=%d > actualLength=%d", buff[0], urb->actualLength);
 		} else {
-			udbgf("UVC: ISO IN len=%d, hdr=%d, FID=%d, EOF=%d, ERR=%d, EOH=%d",
+			uurbdbgf("UVC: ISO IN len=%d, hdr=%d, FID=%d, EOF=%d, ERR=%d, EOH=%d",
 						urb->actualLength,
 						buff[0],
 						buff[1] & UVC_HDR_FID,
@@ -279,7 +260,7 @@ static void _cb_iso(usbh_urb_t *urb) {
 					|| (buff[1] & (UVC_HDR_EOF | UVC_HDR_ERR))) {
 				_post(uvcdp, urb, &uvcdp->mp_data, USBHUVC_MESSAGETYPE_DATA);
 			} else {
-				udbgf("UVC: ISO IN skip: len=%d, hdr=%d, FID=%d, EOF=%d, ERR=%d, EOH=%d",
+				uurbdbgf("UVC: ISO IN skip: len=%d, hdr=%d, FID=%d, EOF=%d, ERR=%d, EOH=%d",
 						urb->actualLength,
 						buff[0],
 						buff[1] & UVC_HDR_FID,
@@ -289,7 +270,7 @@ static void _cb_iso(usbh_urb_t *urb) {
 			}
 		}
 	} else if (urb->actualLength > 0) {
-		uerrf("UVC: ISO IN, actualLength=%d", urb->actualLength);
+		uurberrf("UVC: ISO IN, actualLength=%d", urb->actualLength);
 	}
 
 	usbhURBObjectResetI(urb);
@@ -327,20 +308,20 @@ bool usbhuvcStreamStart(USBHUVCDriver *uvcdp, uint16_t min_ep_sz) {
 	data_sz = (uvcdp->ep_iso.wMaxPacketSize + sizeof(usbhuvc_message_data_t) + 3) & ~3;
 	datapackets = HAL_USBHUVC_WORK_RAM_SIZE / data_sz;
 	if (datapackets == 0) {
-		uerr("Not enough work RAM");
+		uclassdrverr("Not enough work RAM");
 		goto failed;
 	}
 
 	workramsz = datapackets * data_sz;
-	uinfof("Reserving %u bytes of RAM (%d data packets of %d bytes)", workramsz, datapackets, data_sz);
+	uclassdrvinfof("Reserving %u bytes of RAM (%d data packets of %d bytes)", workramsz, datapackets, data_sz);
 	if (datapackets > (HAL_USBHUVC_MAX_MAILBOX_SZ - HAL_USBHUVC_STATUS_PACKETS_COUNT)) {
-		uwarn("Mailbox may overflow, use a larger HAL_USBHUVC_MAX_MAILBOX_SZ. UVC will under-utilize the assigned work RAM.");
+		uclassdrvwarn("Mailbox may overflow, use a larger HAL_USBHUVC_MAX_MAILBOX_SZ. UVC will under-utilize the assigned work RAM.");
 	}
 	chMBResumeX(&uvcdp->mb);
 
 	uvcdp->mp_data_buffer = chHeapAlloc(NULL, workramsz);
 	if (uvcdp->mp_data_buffer == NULL) {
-		uerr("Couldn't reserve RAM");
+		uclassdrverr("Couldn't reserve RAM");
 		goto failed;
 	}
 
@@ -497,7 +478,7 @@ uint32_t usbhuvcEstimateRequiredEPSize(USBHUVCDriver *uvcdp, const uint8_t *form
 		mul = div = 1;
 	}	break;
 	default:
-		uwarn("Unsupported format");
+		uclassdrvwarn("Unsupported format");
 		return 0xffffffff;
 	}
 
@@ -528,7 +509,7 @@ static usbh_baseclassdriver_t *_uvc_load(usbh_device_t *dev, const uint8_t *desc
 		}
 	}
 
-	uwarn("Can't alloc UVC driver");
+	udevwarn("Can't alloc UVC driver");
 
 	/* can't alloc */
 	return NULL;
@@ -553,12 +534,12 @@ alloc_ok:
 
 		const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
 		if (ifdesc->bInterfaceClass != UVC_CC_VIDEO) {
-			uwarnf("Skipping Interface %d (class != UVC_CC_VIDEO)",
+			udevwarnf("Skipping Interface %d (class != UVC_CC_VIDEO)",
 					ifdesc->bInterfaceNumber);
 			continue;
 		}
 
-		uinfof("Interface %d, Alt=%d, Class=UVC_CC_VIDEO, Subclass=%02x",
+		udevinfof("Interface %d, Alt=%d, Class=UVC_CC_VIDEO, Subclass=%02x",
 				ifdesc->bInterfaceNumber,
 				ifdesc->bAlternateSetting,
 				ifdesc->bInterfaceSubClass);
@@ -570,24 +551,24 @@ alloc_ok:
 			}
 			for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {
 				if (ics.curr[1] != UVC_CS_INTERFACE) {
-					uwarnf("Unknown descriptor=%02X", ics.curr[1]);
+					udevwarnf("Unknown descriptor=%02X", ics.curr[1]);
 					continue;
 				}
 				switch (ics.curr[2]) {
 				case UVC_VC_HEADER:
-					uinfo("  VC_HEADER"); break;
+					udevinfo("  VC_HEADER"); break;
 				case UVC_VC_INPUT_TERMINAL:
-					uinfof("    VC_INPUT_TERMINAL, ID=%d", ics.curr[3]); break;
+					udevinfof("    VC_INPUT_TERMINAL, ID=%d", ics.curr[3]); break;
 				case UVC_VC_OUTPUT_TERMINAL:
-					uinfof("    VC_OUTPUT_TERMINAL, ID=%d", ics.curr[3]); break;
+					udevinfof("    VC_OUTPUT_TERMINAL, ID=%d", ics.curr[3]); break;
 				case UVC_VC_SELECTOR_UNIT:
-					uinfof("    VC_SELECTOR_UNIT, ID=%d", ics.curr[3]); break;
+					udevinfof("    VC_SELECTOR_UNIT, ID=%d", ics.curr[3]); break;
 				case UVC_VC_PROCESSING_UNIT:
-					uinfof("    VC_PROCESSING_UNIT, ID=%d", ics.curr[3]); break;
+					udevinfof("    VC_PROCESSING_UNIT, ID=%d", ics.curr[3]); break;
 				case UVC_VC_EXTENSION_UNIT:
-					uinfof("    VC_EXTENSION_UNIT, ID=%d", ics.curr[3]); break;
+					udevinfof("    VC_EXTENSION_UNIT, ID=%d", ics.curr[3]); break;
 				default:
-					uwarnf("Unknown video bDescriptorSubtype=%02x", ics.curr[2]);
+					udevwarnf("Unknown video bDescriptorSubtype=%02x", ics.curr[2]);
 					break;
 				}
 			}
@@ -598,47 +579,47 @@ alloc_ok:
 			}
 			for (cs_iter_init(&ics, (generic_iterator_t *)&iif); ics.valid; cs_iter_next(&ics)) {
 				if (ics.curr[1] != UVC_CS_INTERFACE) {
-					uwarnf("Unknown descriptor=%02X", ics.curr[1]);
+					udevwarnf("Unknown descriptor=%02X", ics.curr[1]);
 					continue;
 				}
 				switch (ics.curr[2]) {
 				case UVC_VS_INPUT_HEADER:
-					uinfo("  VS_INPUT_HEADER"); break;
+					udevinfo("  VS_INPUT_HEADER"); break;
 				case UVC_VS_OUTPUT_HEADER:
-					uinfo("  VS_OUTPUT_HEADER"); break;
+					udevinfo("  VS_OUTPUT_HEADER"); break;
 				case UVC_VS_STILL_IMAGE_FRAME:
-					uinfo("    VS_STILL_IMAGE_FRAME"); break;
+					udevinfo("    VS_STILL_IMAGE_FRAME"); break;
 
 				case UVC_VS_FORMAT_UNCOMPRESSED:
-					uinfof("    VS_FORMAT_UNCOMPRESSED, bFormatIndex=%d", ics.curr[3]); break;
+					udevinfof("    VS_FORMAT_UNCOMPRESSED, bFormatIndex=%d", ics.curr[3]); break;
 				case UVC_VS_FORMAT_MPEG2TS:
-					uinfof("    VS_FORMAT_MPEG2TS, bFormatIndex=%d", ics.curr[3]); break;
+					udevinfof("    VS_FORMAT_MPEG2TS, bFormatIndex=%d", ics.curr[3]); break;
 				case UVC_VS_FORMAT_DV:
-					uinfof("    VS_FORMAT_DV, bFormatIndex=%d", ics.curr[3]); break;
+					udevinfof("    VS_FORMAT_DV, bFormatIndex=%d", ics.curr[3]); break;
 				case UVC_VS_FORMAT_MJPEG:
-					uinfof("    VS_FORMAT_MJPEG, bFormatIndex=%d", ics.curr[3]); break;
+					udevinfof("    VS_FORMAT_MJPEG, bFormatIndex=%d", ics.curr[3]); break;
 				case UVC_VS_FORMAT_FRAME_BASED:
-					uinfof("    VS_FORMAT_FRAME_BASED, bFormatIndex=%d", ics.curr[3]); break;
+					udevinfof("    VS_FORMAT_FRAME_BASED, bFormatIndex=%d", ics.curr[3]); break;
 				case UVC_VS_FORMAT_STREAM_BASED:
-					uinfof("    VS_FORMAT_STREAM_BASED, bFormatIndex=%d", ics.curr[3]); break;
+					udevinfof("    VS_FORMAT_STREAM_BASED, bFormatIndex=%d", ics.curr[3]); break;
 
 				case UVC_VS_FRAME_UNCOMPRESSED:
-					uinfof("      VS_FRAME_UNCOMPRESSED, bFrameIndex=%d", ics.curr[3]); break;
+					udevinfof("      VS_FRAME_UNCOMPRESSED, bFrameIndex=%d", ics.curr[3]); break;
 				case UVC_VS_FRAME_MJPEG:
-					uinfof("      VS_FRAME_MJPEG, bFrameIndex=%d", ics.curr[3]); break;
+					udevinfof("      VS_FRAME_MJPEG, bFrameIndex=%d", ics.curr[3]); break;
 				case UVC_VS_FRAME_FRAME_BASED:
-					uinfof("      VS_FRAME_FRAME_BASED, bFrameIndex=%d", ics.curr[3]); break;
+					udevinfof("      VS_FRAME_FRAME_BASED, bFrameIndex=%d", ics.curr[3]); break;
 
 				case UVC_VS_COLOR_FORMAT:
-					uinfo("      VS_COLOR_FORMAT"); break;
+					udevinfo("      VS_COLOR_FORMAT"); break;
 				default:
-					uwarnf("Unknown video bDescriptorSubtype=%02x", ics.curr[2]);
+					udevwarnf("Unknown video bDescriptorSubtype=%02x", ics.curr[2]);
 					break;
 				}
 			}
 			break;
 		default:
-			uwarnf("Unknown video bInterfaceSubClass=%02x", ifdesc->bInterfaceSubClass);
+			udevwarnf("Unknown video bInterfaceSubClass=%02x", ifdesc->bInterfaceSubClass);
 			break;
 		}
 
@@ -649,7 +630,7 @@ alloc_ok:
 					&& ((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_INT)
 					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
 				/* found VC interrupt endpoint */
-				uinfof("  VC Interrupt endpoint; %02x, bInterval=%d",
+				udevinfof("  VC Interrupt endpoint; %02x, bInterval=%d",
 						epdesc->bEndpointAddress, epdesc->bInterval);
 				usbhEPObjectInit(&uvcdp->ep_int, dev, epdesc);
 				usbhEPSetName(&uvcdp->ep_int, "UVC[INT ]");
@@ -657,16 +638,16 @@ alloc_ok:
 					&& ((epdesc->bmAttributes & 0x03) == USBH_EPTYPE_ISO)
 					&& ((epdesc->bEndpointAddress & 0x80) ==  USBH_EPDIR_IN)) {
 				/* found VS isochronous endpoint */
-				uinfof("  VS Isochronous endpoint; %02x, bInterval=%d, bmAttributes=%02x",
+				udevinfof("  VS Isochronous endpoint; %02x, bInterval=%d, bmAttributes=%02x",
 						epdesc->bEndpointAddress, epdesc->bInterval, epdesc->bmAttributes);
 			} else {
 				/* unknown EP */
-				uwarnf("  <unknown endpoint>, bEndpointAddress=%02x, bmAttributes=%02x",
+				udevwarnf("  <unknown endpoint>, bEndpointAddress=%02x, bmAttributes=%02x",
 									epdesc->bEndpointAddress, epdesc->bmAttributes);
 			}
 
 			for (cs_iter_init(&ics, &iep); ics.valid; cs_iter_next(&ics)) {
-				uinfof("    CS_ENDPOINT bLength=%d, bDescriptorType=%02X",
+				udevinfof("    CS_ENDPOINT bLength=%d, bDescriptorType=%02X",
 						ics.curr[0], ics.curr[1]);
 			}
 		}
diff --git a/testhal/STM32/STM32F4xx/USB_HOST/.settings/org.eclipse.cdt.core.prefs b/testhal/STM32/STM32F4xx/USB_HOST/.settings/org.eclipse.cdt.core.prefs
index 8ae48ef..bf2ed73 100644
--- a/testhal/STM32/STM32F4xx/USB_HOST/.settings/org.eclipse.cdt.core.prefs
+++ b/testhal/STM32/STM32F4xx/USB_HOST/.settings/org.eclipse.cdt.core.prefs
@@ -1,6 +1,6 @@
 eclipse.preferences.version=1
 environment/project/0.1003150841/PATH/delimiter=;
 environment/project/0.1003150841/PATH/operation=replace
-environment/project/0.1003150841/PATH/value=${PATH};D\:\\toolchains\\gcc-arm-none-eabi-8-2018-q4-major-win32\\bin;${PATH};D\:\\toolchains\\gcc-arm-none-eabi-4_9-2014q4-20141203-win32\\bin;D\:\\toolchains\\msys64\\usr\\bin
+environment/project/0.1003150841/PATH/value=${PATH};D\:\\toolchains\\gcc-arm-none-eabi-8-2019-q3-update-win32\\bin;D\:\\toolchains\\msys64\\usr\\bin
 environment/project/0.1003150841/append=true
 environment/project/0.1003150841/appendContributed=true
diff --git a/testhal/STM32/STM32F4xx/USB_HOST/chconf.h b/testhal/STM32/STM32F4xx/USB_HOST/chconf.h
index 12c6673..a01a37c 100644
--- a/testhal/STM32/STM32F4xx/USB_HOST/chconf.h
+++ b/testhal/STM32/STM32F4xx/USB_HOST/chconf.h
@@ -712,3 +712,4 @@
 #endif  /* CHCONF_H */
 
 /** @} */
+
diff --git a/testhal/STM32/STM32F4xx/USB_HOST/halconf_community.h b/testhal/STM32/STM32F4xx/USB_HOST/halconf_community.h
index fef417f..c76d1ef 100644
--- a/testhal/STM32/STM32F4xx/USB_HOST/halconf_community.h
+++ b/testhal/STM32/STM32F4xx/USB_HOST/halconf_community.h
@@ -177,7 +177,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           OSAL_MS2I(1000)

+#define HAL_USBH_CONTROL_REQUEST_DEFAULT_TIMEOUT	  OSAL_MS2I(1000)

 

 /* MSD */

 #define HAL_USBH_USE_MSD                              TRUE

@@ -202,7 +202,7 @@
 

 #define HAL_USBHAOA_MAX_INSTANCES                     1

 /* Uncomment this if you need a filter for AOA devices:

- * #define HAL_USBHAOA_FILTER_CALLBACK            _try_aoa

+ * #define HAL_USBHAOA_FILTER_CALLBACK                _try_aoa

  */

 #define HAL_USBHAOA_DEFAULT_MANUFACTURER              "Diego MFG & Co."

 #define HAL_USBHAOA_DEFAULT_MODEL                     "Diego's device"

@@ -235,9 +235,10 @@
 

 /* debug */

 #define USBH_DEBUG_ENABLE                             TRUE

-#define USBH_DEBUG_USBHD                              USBHD1

-#define USBH_DEBUG_SD                                 SD2

+#define USBH_DEBUG_MULTI_HOST                         FALSE

+#define USBH_DEBUG_SINGLE_HOST_SELECTION			  USBHD1

 #define USBH_DEBUG_BUFFER                             25000

+#define USBH_DEBUG_OUTPUT_CALLBACK                    usbh_debug_output

 

 #define USBH_DEBUG_ENABLE_TRACE                       FALSE

 #define USBH_DEBUG_ENABLE_INFO                        TRUE

diff --git a/testhal/STM32/STM32F4xx/USB_HOST/main.c b/testhal/STM32/STM32F4xx/USB_HOST/main.c
index c88880b..5efdcf7 100644
--- a/testhal/STM32/STM32F4xx/USB_HOST/main.c
+++ b/testhal/STM32/STM32F4xx/USB_HOST/main.c
@@ -18,7 +18,7 @@
 #include "hal.h"

 #include "ff.h"

 #include <string.h>

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

+#include "usbh/debug.h"		/* for _usbh_dbg/_usbh_dbgf */

 

 

 #define UVC_TO_MSD_PHOTOS_CAPTURE	FALSE

@@ -85,6 +85,8 @@ static const ShellConfig shell_cfg1 = {
 static void ThreadTestFTDI(void *p) {

     (void)p;

     USBHFTDIPortDriver *const ftdipp = &FTDIPD[0];

+    USBHDriver *host = NULL;

+    (void)host;

 

     chRegSetThreadName("FTDI");

 

@@ -95,7 +97,8 @@ start:
         chThdSleepMilliseconds(100);

     }

 

-    usbDbgPuts("FTDI: Connected");

+    host = usbhftdipGetHost(ftdipp);

+	_usbh_dbg(host, "FTDI: Connected");

 

     USBHFTDIPortConfig config = {

         115200,

@@ -112,7 +115,7 @@ start:
         for(;;) {

             msg_t m = streamGet(ftdipp);

             if (m < MSG_OK) {

-                usbDbgPuts("FTDI: Disconnected");

+                _usbh_dbg(host, "FTDI: Disconnected");

                 goto start;

             }

             streamPut(ftdipp, (uint8_t)m);

@@ -146,10 +149,10 @@ start:
         for(;;) {

             msg_t m = streamGet(ftdipp);

             if (m < MSG_OK) {

-                usbDbgPuts("FTDI: Disconnected");

+                _usbh_dbg(host, "FTDI: Disconnected");

                 goto start;

             }

-            sdPut(&USBH_DEBUG_SD, (uint8_t)m);

+            sdPut(&SD2, (uint8_t)m);

             if (m == 'q')

                 break;

         }

@@ -169,19 +172,19 @@ start:
             st = chVTGetSystemTimeX();

             while (times--) {

                 if (streamWrite(ftdipp, buf, 1024) < 1024) {

-                    usbDbgPuts("FTDI: Disconnected");

+                    _usbh_dbg(host, "FTDI: Disconnected");

                     goto start;

                 }

                 bytes -= 1024;

             }

             if (bytes) {

                 if (streamWrite(ftdipp, buf, bytes) < bytes) {

-                    usbDbgPuts("FTDI: Disconnected");

+                    _usbh_dbg(host, "FTDI: Disconnected");

                     goto start;

                 }

             }

             et = chVTGetSystemTimeX();

-            usbDbgPrintf("\tRate=%uB/s", (config.speed * 100) / (et - st));

+            _usbh_dbgf(host, "\tRate=%uB/s", (config.speed * 100) / (et - st));

         }

     }

 

@@ -189,7 +192,7 @@ start:
     if (0) {

         for (;;) {

             if (streamPut(ftdipp, 'A') != MSG_OK) {

-                usbDbgPuts("FTDI: Disconnected");

+                _usbh_dbg(host, "FTDI: Disconnected");

                 goto start;

             }

             chThdSleepMilliseconds(100);

@@ -198,7 +201,7 @@ start:
 

     usbhftdipStop(ftdipp);

 

-    usbDbgPuts("FTDI: Tests done, restarting in 3s");

+    _usbh_dbg(host, "FTDI: Tests done, restarting in 3s");

     chThdSleepMilliseconds(3000);

 

     goto start;

@@ -218,6 +221,8 @@ static void ThreadTestAOA(void *p) {
     (void)p;

     USBHAOADriver *const aoap = &USBHAOAD[0];

     USBHAOAChannel *const aoacp = &aoap->channel;

+    USBHDriver *host = NULL;

+    (void)host;

 

     chRegSetThreadName("AOA");

 

@@ -226,11 +231,12 @@ start:
         chThdSleepMilliseconds(100);

     }

 

-    usbDbgPuts("AOA: Connected");

+    host = usbhaoaGetHost(aoap);

+    _usbh_dbg(host, "AOA: Connected");

 

     if (usbhaoaGetChannelState(aoap) != USBHAOA_CHANNEL_STATE_READY) {

         usbhaoaChannelStart(aoap);

-        usbDbgPuts("AOA: Channel started");

+        _usbh_dbg(host, "AOA: Channel started");

     }

 

     //loopback

@@ -238,7 +244,7 @@ start:
         for(;;) {

             msg_t m = streamGet(aoacp);

             if (m < MSG_OK) {

-                usbDbgPuts("AOA: Disconnected");

+                _usbh_dbg(host, "AOA: Disconnected");

                 goto start;

             }

             streamPut(aoacp, (uint8_t)m);

@@ -258,19 +264,19 @@ start:
             st = chVTGetSystemTimeX();

             while (times--) {

                 if (streamWrite(aoacp, buf, 1024) < 1024) {

-                    usbDbgPuts("AOA: Disconnected");

+                    _usbh_dbg(host, "AOA: Disconnected");

                     goto start;

                 }

                 bytes -= 1024;

             }

             if (bytes) {

                 if (streamWrite(aoacp, buf, bytes) < bytes) {

-                    usbDbgPuts("AOA: Disconnected");

+                    _usbh_dbg(host, "AOA: Disconnected");

                     goto start;

                 }

             }

             et = chVTGetSystemTimeX();

-            usbDbgPrintf("\tRate=%uB/s", AOA_WRITE_SPEED_TEST_BYTES / (et - st) * 100);

+            _usbh_dbgf(host, "\tRate=%uB/s", AOA_WRITE_SPEED_TEST_BYTES / (et - st) * 100);

         }

     }

 

@@ -278,7 +284,7 @@ start:
     if (0) {

         for (;;) {

             if (streamPut(aoacp, 'A') != MSG_OK) {

-                usbDbgPuts("AOA: Disconnected");

+                _usbh_dbg(host, "AOA: Disconnected");

                 goto start;

             }

             chThdSleepMilliseconds(100);

@@ -287,7 +293,7 @@ start:
 

     usbhaoaChannelStop(aoap);

 

-    usbDbgPuts("AOA: Tests done, restarting in 3s");

+    _usbh_dbg(host, "AOA: Tests done, restarting in 3s");

     chThdSleepMilliseconds(3000);

 

     goto start;

@@ -305,7 +311,7 @@ static FATFS MSDLUN0FS;
 static uint8_t fbuff[10240];

 static FIL file;

 

-static FRESULT scan_files(BaseSequentialStream *chp, char *path) {

+static FRESULT scan_files(USBHDriver *host, BaseSequentialStream *chp, char *path) {

   FRESULT res;

   DIR dir;

   UINT i;

@@ -321,12 +327,12 @@ static FRESULT scan_files(BaseSequentialStream *chp, char *path) {
 		i = strlen(path);

         path[i++] = '/';

         strcpy(&path[i], fno.fname);

-        res = scan_files(chp, path);

+        res = scan_files(host, chp, path);

         if (res != FR_OK)

           break;

         path[--i] = 0;

       } else {

-          usbDbgPrintf("FS: %s/%s", path, fno.fname);

+          _usbh_dbgf(host, "FS: %s/%s", path, fno.fname);

       }

     }

   }

@@ -341,11 +347,12 @@ static void ThreadTestMSD(void *p) {
     FATFS *fsp;

     DWORD clusters;

     FRESULT res;

+    USBHDriver *host = NULL;

 

     chRegSetThreadName("MSD");

 

 #if !UVC_TO_MSD_PHOTOS_CAPTURE

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

+    BaseSequentialStream * const chp = (BaseSequentialStream *)&SD2;

     systime_t st, et;

     uint32_t j;

 #endif

@@ -355,14 +362,15 @@ start:
         chThdSleepMilliseconds(100);

 

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

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

+        	host = usbhmsdLUNGetHost(&MSBLKD[0]);

+        	_usbh_dbg(host, "BLK: Active, connect....");

         	usbhmsdLUNConnect(&MSBLKD[0]);

         }

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

             continue;

     	}

 

-    	usbDbgPuts("BLK: Ready.");

+    	_usbh_dbg(host, "BLK: Ready.");

 

 #if !UVC_TO_MSD_PHOTOS_CAPTURE

         //raw read test

@@ -372,7 +380,7 @@ start:
 #define NITERATIONS            ((RAW_READ_SZ_MB * 1024UL * 1024UL) / sizeof(fbuff))

             uint32_t start = 0;

             chThdSetPriority(HIGHPRIO);

-            usbDbgPrintf("BLK: Raw read test (%dMB, %dB blocks)", RAW_READ_SZ_MB, sizeof(fbuff));

+            _usbh_dbgf(host, "BLK: Raw read test (%dMB, %dB blocks)", RAW_READ_SZ_MB, sizeof(fbuff));

             st = chVTGetSystemTime();

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

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

@@ -380,29 +388,29 @@ start:
                 start += NBLOCKS;

             }

             et = chVTGetSystemTime();

-            usbDbgPrintf("BLK: Raw read in %d ms, %dkB/s",

+            _usbh_dbgf(host, "BLK: Raw read in %d ms, %dkB/s",

                     et - st,

                     (RAW_READ_SZ_MB * 1024UL * 1000) / (et - st));

             chThdSetPriority(NORMALPRIO);

         }

 #endif

 

-        usbDbgPuts("FS: Block driver ready, try mount...");

+        _usbh_dbg(host, "FS: Block driver ready, try mount...");

 

         res = f_mount(&MSDLUN0FS, FATFSDEV_MSD_DRIVE, 1);

         if (res != FR_OK) {

-            usbDbgPuts("FS: Can't mount. Check file system.");

+            _usbh_dbg(host, "FS: Can't mount. Check file system.");

             continue;

         }

-        usbDbgPuts("FS: Mounted.");

+        _usbh_dbg(host, "FS: Mounted.");

 

         res = f_getfree(FATFSDEV_MSD_DRIVE, &clusters, &fsp);

         if (res != FR_OK) {

-            usbDbgPuts("FS: f_getfree() failed");

+            _usbh_dbg(host, "FS: f_getfree() failed");

             continue;

         }

 

-        usbDbgPrintf("FS: %lu free clusters, %lu sectors per cluster, %lu bytes free",

+        _usbh_dbgf(host, "FS: %lu free clusters, %lu sectors per cluster, %lu bytes free",

                 clusters, (uint32_t)MSDLUN0FS.csize,

                 clusters * (uint32_t)MSDLUN0FS.csize * MSBLKD[0].info.blk_size);

 

@@ -419,7 +427,7 @@ start:
 

         //write test

         if (1) {

-            usbDbgPuts("FS: Write test (create file /test.dat, 1MB)");

+            _usbh_dbg(host, "FS: Write test (create file /test.dat, 1MB)");

             f_open(&file, FATFSDEV_MSD_DRIVE "/test.dat", FA_CREATE_ALWAYS | FA_WRITE);

             src = start;

             st = chVTGetSystemTime();

@@ -431,7 +439,7 @@ start:
                     src = start;

             }

             et = chVTGetSystemTime();

-            usbDbgPrintf("FS: Written 1MB in %d ms, %dkB/s",

+            _usbh_dbgf(host, "FS: Written 1MB in %d ms, %dkB/s",

                     et - st,

                     (1024UL*1000) / (et - st));

             f_close(&file);

@@ -439,7 +447,7 @@ start:
 

         //read test

         if (1) {

-            usbDbgPuts("FS: Read test (read file /test.dat, 1MB, compare)");

+            _usbh_dbg(host, "FS: Read test (read file /test.dat, 1MB, compare)");

             f_open(&file, FATFSDEV_MSD_DRIVE "/test.dat", FA_READ);

             src = start;

             st = chVTGetSystemTime();

@@ -447,7 +455,7 @@ start:
                 if (f_read(&file, fbuff, 512, &bw) != FR_OK)

                     goto start;

                 if (memcmp(src, fbuff, bw)) {

-                    usbDbgPrintf("Compare error @%08x", (uint32_t)src);

+                    _usbh_dbgf(host, "Compare error @%08x", (uint32_t)src);

                     goto start;

                 }

                 src += bw;

@@ -455,7 +463,7 @@ start:
                     src = start;

             }

             et = chVTGetSystemTime();

-            usbDbgPrintf("FS: Read 1MB in %d ms, %dkB/s",

+            _usbh_dbgf(host, "FS: Read 1MB in %d ms, %dkB/s",

                     et - st,

                     (1024UL*1000) / (et - st));

             f_close(&file);

@@ -463,14 +471,14 @@ start:
 

         //scan files test

         if (1) {

-            usbDbgPuts("FS: Scan files test");

+            _usbh_dbg(host, "FS: Scan files test");

             strcpy((char *)fbuff, FATFSDEV_MSD_DRIVE);

-            scan_files(chp, (char *)fbuff);

+            scan_files(host, chp, (char *)fbuff);

         }

     }

 #endif

 

-    usbDbgPuts("FS: Tests done, restarting in 3s");

+    _usbh_dbg(host, "FS: Tests done, restarting in 3s");

     chThdSleepMilliseconds(3000);

 

     goto start;

@@ -488,12 +496,12 @@ static void _hid_report_callback(USBHHIDDriver *hidp, uint16_t len) {
     uint8_t *report = (uint8_t *)hidp->config->report_buffer;

 

     if (hidp->type == USBHHID_DEVTYPE_BOOT_MOUSE) {

-        usbDbgPrintf("Mouse report: buttons=%02x, Dx=%d, Dy=%d",

+        _usbh_dbgf(hidp->dev->host, "Mouse report: buttons=%02x, Dx=%d, Dy=%d",

                 report[0],

                 (int8_t)report[1],

                 (int8_t)report[2]);

     } else if (hidp->type == USBHHID_DEVTYPE_BOOT_KEYBOARD) {

-        usbDbgPrintf("Keyboard report: modifier=%02x, keys=%02x %02x %02x %02x %02x %02x",

+        _usbh_dbgf(hidp->dev->host, "Keyboard report: modifier=%02x, keys=%02x %02x %02x %02x %02x %02x",

                 report[0],

                 report[2],

                 report[3],

@@ -502,7 +510,7 @@ static void _hid_report_callback(USBHHIDDriver *hidp, uint16_t len) {
                 report[6],

                 report[7]);

     } else {

-        usbDbgPrintf("Generic report, %d bytes", len);

+        _usbh_dbgf(hidp->dev->host, "Generic report, %d bytes", len);

     }

 }

 

@@ -525,21 +533,22 @@ static void ThreadTestHID(void *p) {
 

     for (;;) {

         for (i = 0; i < HAL_USBHHID_MAX_INSTANCES; i++) {

-            if (usbhhidGetState(&USBHHIDD[i]) == USBHHID_STATE_ACTIVE) {

-                usbDbgPrintf("HID: Connected, HID%d", i);

-                usbhhidStart(&USBHHIDD[i], &hidcfg[i]);

-                if (usbhhidGetType(&USBHHIDD[i]) != USBHHID_DEVTYPE_GENERIC) {

-                    usbhhidSetIdle(&USBHHIDD[i], 0, 0);

+        	USBHHIDDriver *const hidp = &USBHHIDD[i];

+            if (usbhhidGetState(hidp) == USBHHID_STATE_ACTIVE) {

+                _usbh_dbgf(hidp->dev->host, "HID: Connected, HID%d", i);

+                usbhhidStart(hidp, &hidcfg[i]);

+                if (usbhhidGetType(hidp) != USBHHID_DEVTYPE_GENERIC) {

+                    usbhhidSetIdle(hidp, 0, 0);

                 }

                 kbd_led_states[i] = 1;

-            } else if (usbhhidGetState(&USBHHIDD[i]) == USBHHID_STATE_READY) {

-                if (usbhhidGetType(&USBHHIDD[i]) == USBHHID_DEVTYPE_BOOT_KEYBOARD) {

+            } else if (usbhhidGetState(hidp) == USBHHID_STATE_READY) {

+                if (usbhhidGetType(hidp) == USBHHID_DEVTYPE_BOOT_KEYBOARD) {

                     USBH_DEFINE_BUFFER(uint8_t val);

                     val = kbd_led_states[i] << 1;

                     if (val == 0x08) {

                         val = 1;

                     }

-                    usbhhidSetReport(&USBHHIDD[i], 0, USBHHID_REPORTTYPE_OUTPUT, &val, 1);

+                    usbhhidSetReport(hidp, 0, USBHHID_REPORTTYPE_OUTPUT, &val, 1);

                     kbd_led_states[i] = val;

                 }

             }

@@ -637,12 +646,12 @@ static void ThreadTestUVC(void *p) {
         if (usbhuvcGetState(&USBHUVCD[0]) != USBHUVC_STATE_ACTIVE)

             continue;

 

-        usbDbgPuts("UVC: Webcam connected");

+        _usbh_dbg(uvcdp->dev->host, "UVC: Webcam connected");

 

         /* ************************************ */

         /*   Find best configuration            */

         /* ************************************ */

-        usbDbgPuts("UVC: Find best configuration");

+        _usbh_dbg(uvcdp->dev->host, "UVC: Find best configuration");

 

         generic_iterator_t ics;

         const usbh_uvc_format_mjpeg_t *format;

@@ -658,7 +667,7 @@ static void ThreadTestUVC(void *p) {
             goto failed;

 

         format = (const usbh_uvc_format_mjpeg_t *)ics.curr;

-        usbDbgPrintf("\tSelect bFormatIndex=%d", format->bFormatIndex);

+        _usbh_dbgf(uvcdp->dev->host, "\tSelect bFormatIndex=%d", format->bFormatIndex);

 

         //find the most suitable frame (largest one within the bandwidth requirements)

         if (usbhuvcFindVSDescriptor(uvcdp, &ics, UVC_VS_FRAME_MJPEG, TRUE) != HAL_SUCCESS)

@@ -668,18 +677,18 @@ static void ThreadTestUVC(void *p) {
             const usbh_uvc_frame_mjpeg_t *const frame = (usbh_uvc_frame_mjpeg_t *)ics.curr;

             uint32_t frame_sz = frame->wWidth * frame->wHeight;

 

-            usbDbgPrintf("\t\tbFrameIndex=%d", frame->bFrameIndex);

-            usbDbgPrintf("\t\t\twWidth=%d, wHeight=%d", frame->wWidth, frame->wHeight);

-            usbDbgPrintf("\t\t\tdwMinBitRate=%u, dwMaxBitRate=%u", frame->dwMinBitRate, frame->dwMaxBitRate);

-            usbDbgPrintf("\t\t\tdwMaxVideoFrameBufferSize=%u", frame->dwMaxVideoFrameBufferSize);

-            usbDbgPrintf("\t\t\tdwDefaultFrameInterval=%u", frame->dwDefaultFrameInterval);

+            _usbh_dbgf(uvcdp->dev->host, "\t\tbFrameIndex=%d", frame->bFrameIndex);

+            _usbh_dbgf(uvcdp->dev->host, "\t\t\twWidth=%d, wHeight=%d", frame->wWidth, frame->wHeight);

+            _usbh_dbgf(uvcdp->dev->host, "\t\t\tdwMinBitRate=%u, dwMaxBitRate=%u", frame->dwMinBitRate, frame->dwMaxBitRate);

+            _usbh_dbgf(uvcdp->dev->host, "\t\t\tdwMaxVideoFrameBufferSize=%u", frame->dwMaxVideoFrameBufferSize);

+            _usbh_dbgf(uvcdp->dev->host, "\t\t\tdwDefaultFrameInterval=%u", frame->dwDefaultFrameInterval);

 

             uint8_t j;

             for (j = 0; j < frame->bFrameIntervalType; j++) {

                 uint32_t ep_sz =

                         usbhuvcEstimateRequiredEPSize(uvcdp, (const uint8_t *)format, (const uint8_t *)frame, frame->dwFrameInterval[j]);

 

-                usbDbgPrintf("\t\t\tdwFrameInterval=%u, estimated EP size=%u", frame->dwFrameInterval[j], ep_sz);

+                _usbh_dbgf(uvcdp->dev->host, "\t\t\tdwFrameInterval=%u, estimated EP size=%u", frame->dwFrameInterval[j], ep_sz);

 

                 if (ep_sz > 310)

                     continue;

@@ -697,7 +706,7 @@ static void ThreadTestUVC(void *p) {
                 if (ep_sz < min_ep_sz) {

                     /* new best bitrate */

                     min_ep_sz = ep_sz;

-                    usbDbgPuts("\t\t\tNew best candidate found");

+                    _usbh_dbg(uvcdp->dev->host, "\t\t\tNew best candidate found");

                     best_frame_interval_index = j;

                     best_frame = frame;

                 }

@@ -706,14 +715,14 @@ static void ThreadTestUVC(void *p) {
 

 failed:

         if (best_frame == NULL) {

-            usbDbgPuts("\t\t\tCouldn't find suitable format/frame");

+            _usbh_dbg(uvcdp->dev->host, "\t\t\tCouldn't find suitable format/frame");

             continue;

         }

 

         /* ************************************ */

         /*   NEGOTIATION                        */

         /* ************************************ */

-        usbDbgPuts("UVC: Start negotiation");

+        _usbh_dbg(uvcdp->dev->host, "UVC: Start negotiation");

 

         usbhuvcResetPC(uvcdp);

         usbh_uvc_ctrl_vs_probecommit_data_t *const pc = usbhuvcGetPC(uvcdp);

@@ -723,42 +732,42 @@ failed:
         pc->bFrameIndex = best_frame->bFrameIndex;

         pc->dwFrameInterval = best_frame->dwFrameInterval[best_frame_interval_index];

 

-        usbDbgPrintf("\tFirst probe, selecting bFormatIndex=%d, bFrameIndex=%d, dwFrameInterval=%u",

+        _usbh_dbgf(uvcdp->dev->host, "\tFirst probe, selecting bFormatIndex=%d, bFrameIndex=%d, dwFrameInterval=%u",

                 pc->bFormatIndex, pc->bFrameIndex, pc->dwFrameInterval);

 

-        usbDbgPuts("SET_CUR (PROBE):"); usbhuvcPrintProbeCommit(&uvcdp->pc);

+        _usbh_dbg(uvcdp->dev->host, "SET_CUR (PROBE):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc);

         if (usbhuvcProbe(uvcdp) != HAL_SUCCESS) {

-            usbDbgPuts("\tFirst probe failed");

+            _usbh_dbg(uvcdp->dev->host, "\tFirst probe failed");

             continue;

         }

-        usbDbgPuts("GET_CUR (PROBE):"); usbhuvcPrintProbeCommit(&uvcdp->pc);

-        usbDbgPuts("GET_MIN (PROBE):"); usbhuvcPrintProbeCommit(&uvcdp->pc_min);

-        usbDbgPuts("GET_MAX (PROBE):"); usbhuvcPrintProbeCommit(&uvcdp->pc_max);

+        _usbh_dbg(uvcdp->dev->host, "GET_CUR (PROBE):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc);

+        _usbh_dbg(uvcdp->dev->host, "GET_MIN (PROBE):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc_min);

+        _usbh_dbg(uvcdp->dev->host, "GET_MAX (PROBE):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc_max);

 

         pc->bmHint = 0x0001;

         pc->wCompQuality = uvcdp->pc_min.wCompQuality;

 

-        usbDbgPuts("SET_CUR (PROBE):"); usbhuvcPrintProbeCommit(&uvcdp->pc);

-        usbDbgPrintf("\tSecond probe, selecting wCompQuality=%d", pc->wCompQuality);

+        _usbh_dbg(uvcdp->dev->host, "SET_CUR (PROBE):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc);

+        _usbh_dbgf(uvcdp->dev->host, "\tSecond probe, selecting wCompQuality=%d", pc->wCompQuality);

         if (usbhuvcProbe(uvcdp) != HAL_SUCCESS) {

-            usbDbgPuts("\tSecond probe failed");

+            _usbh_dbg(uvcdp->dev->host, "\tSecond probe failed");

             continue;

         }

-        usbDbgPuts("GET_CUR (PROBE):"); usbhuvcPrintProbeCommit(&uvcdp->pc);

-        usbDbgPuts("GET_MIN (PROBE):"); usbhuvcPrintProbeCommit(&uvcdp->pc_min);

-        usbDbgPuts("GET_MAX (PROBE):"); usbhuvcPrintProbeCommit(&uvcdp->pc_max);

+        _usbh_dbg(uvcdp->dev->host, "GET_CUR (PROBE):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc);

+        _usbh_dbg(uvcdp->dev->host, "GET_MIN (PROBE):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc_min);

+        _usbh_dbg(uvcdp->dev->host, "GET_MAX (PROBE):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc_max);

 

         /* ************************************ */

         /*   Commit negotiated parameters        */

         /* ************************************ */

-        usbDbgPuts("UVC: Commit negotiated parameters");

-        usbDbgPuts("SET_CUR (COMMIT):"); usbhuvcPrintProbeCommit(&uvcdp->pc);

+        _usbh_dbg(uvcdp->dev->host, "UVC: Commit negotiated parameters");

+        _usbh_dbg(uvcdp->dev->host, "SET_CUR (COMMIT):"); usbhuvcPrintProbeCommit(uvcdp, &uvcdp->pc);

         if (usbhuvcCommit(uvcdp) != HAL_SUCCESS) {

-            usbDbgPuts("\tCommit failed");

+            _usbh_dbg(uvcdp->dev->host, "\tCommit failed");

             continue;

         }

 

-        usbDbgPuts("UVC: Ready to start streaming");

+        _usbh_dbg(uvcdp->dev->host, "UVC: Ready to start streaming");

 

         uint32_t npackets = 0;

         uint32_t payload = 0;

@@ -774,7 +783,7 @@ failed:
             msg_t msg, ret;

             ret = usbhuvcLockAndFetch(uvcdp, &msg, TIME_INFINITE);

             if (ret == MSG_RESET) {

-                usbDbgPuts("UVC: Driver is unloading");

+                _usbh_dbg(uvcdp->dev->host, "UVC: Driver is unloading");

                 break;

             } else if (ret == MSG_TIMEOUT) {

                 continue;

@@ -784,7 +793,7 @@ failed:
                 usbhuvc_message_data_t *const data = (usbhuvc_message_data_t *)msg;

 

                 if (data->length < data->data[0]) {

-                    usbDbgPrintf("UVC: Length error!");

+                    _usbh_dbgf(uvcdp->dev->host, "UVC: Length error!");

                     goto free_data;

                 }

 

@@ -815,20 +824,20 @@ failed:
                     chsnprintf(fn, sizeof(fn), "/img%d.jpg", frame);

                     if (f_open(&fp, fn, FA_CREATE_ALWAYS | FA_WRITE) == FR_OK) {

                         if (with_dht && f_write(&fp, jpeg_header_plus_dht, sizeof(jpeg_header_plus_dht), &bw) != FR_OK) {

-                            usbDbgPuts("UVC->MSD: File write error");

+                            _usbh_dbg(uvcdp->dev->host, "UVC->MSD: File write error");

                             f_close(&fp);

                             state = 0;

                         }

                         state = 2;

                     } else {

-                        usbDbgPuts("UVC->MSD: File open error");

+                        _usbh_dbg(uvcdp->dev->host, "UVC->MSD: File open error");

                         state = 0;

                     }

                 }

 

                 if (state == 2) {

                     if (f_write(&fp, message_data, message_payload, &bw) != FR_OK) {

-                        usbDbgPuts("UVC->MSD: File write error");

+                        _usbh_dbg(uvcdp->dev->host, "UVC->MSD: File write error");

                         f_close(&fp);

                         state = 0;

                     }

@@ -837,7 +846,7 @@ failed:
 check_eof:

 #endif

                 if (data->data[1] & UVC_HDR_EOF) {

-                    usbDbgPrintf("UVC: FRAME #%d, delta=%03dticks, #packets=%d, useful_payload=%dbytes, total=%dbytes",

+                    _usbh_dbgf(uvcdp->dev->host, "UVC: FRAME #%d, delta=%03dticks, #packets=%d, useful_payload=%dbytes, total=%dbytes",

                             frame, data->timestamp - last , npackets, payload, total);

                     last = data->timestamp;

                     npackets = 0;

@@ -856,17 +865,17 @@ free_data:
                 const uint8_t *const stat = status->data;

                 switch (stat[0] & 0x0f) {

                 case 1:

-                    usbDbgPrintf("UVC: STATUS Control event, "

+                    _usbh_dbgf(uvcdp->dev->host, "UVC: STATUS Control event, "

                             "bOriginator=%d, bEvent=%d, bSelector=%d, bAttribute=%d",

                             stat[1], stat[2], stat[3], stat[4]);

                     break;

                 case 2:

-                    usbDbgPrintf("UVC: STATUS Streaming event, "

+                    _usbh_dbgf(uvcdp->dev->host, "UVC: STATUS Streaming event, "

                             "bOriginator=%d, bEvent=%d, bValue=%d",

                             stat[1], stat[2], stat[3]);

                     break;

                 default:

-                    usbDbgPrintf("UVC: STATUS unknown status report = %d", stat[0]);

+                    _usbh_dbgf(uvcdp->dev->host, "UVC: STATUS unknown status report = %d", stat[0]);

                     break;

                 }

                 usbhuvcFreeStatusMessage(uvcdp, status);

@@ -879,6 +888,16 @@ free_data:
 }

 #endif

 

+#if USBH_DEBUG_MULTI_HOST

+void USBH_DEBUG_OUTPUT_CALLBACK(USBHDriver *host, const uint8_t *buff, size_t len) {

+	(void)host;

+#else

+void USBH_DEBUG_OUTPUT_CALLBACK(const uint8_t *buff, size_t len) {

+#endif

+	sdWrite(&SD2, buff, len);

+	sdWrite(&SD2, (const uint8_t *)"\r\n", 2);

+}

+

 int main(void) {

 

     IWDG->KR = 0x5555;

@@ -928,9 +947,11 @@ int main(void) {
     //start

 #if STM32_USBH_USE_OTG1

     usbhStart(&USBHD1);

+    _usbh_dbgf(&USBHD1, "Started");

 #endif

 #if STM32_USBH_USE_OTG2

     usbhStart(&USBHD2);

+    _usbh_dbgf(&USBHD2, "Started");

 #endif

 

     for(;;) {