Merge branch 'master' into segger_systemview

9 files changed, 486 insertions, 1011 deletions

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]);
 			}
 		}