STM32 mass update to current naming convention in ChibiOS

6 files changed, 2748 insertions, 2748 deletions

diff --git a/os/hal/src/usbh/usbh_debug.c b/os/hal/src/usbh/hal_usbh_debug.c
index 63505aa..9f17189 100644
--- a/os/hal/src/usbh/usbh_debug.c
+++ b/os/hal/src/usbh/hal_usbh_debug.c
@@ -1,536 +1,536 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-

-#if HAL_USE_USBH

-

-#include "ch.h"

-#include "usbh/debug.h"

-#include <stdarg.h>

-#include "chprintf.h"

-

-#if USBH_DEBUG_ENABLE

-

-#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;

-

-	l = num;

-	if (divisor == 0) {

-		ll = num;

-	} else {

-		ll = divisor;

-	}

-

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

-}

-

-#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;

-		}

-		return ++q;

-	}

-

-	

-

-

-}

-#endif

-

-static inline void _put(char c) {

-	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;

-

-	if (chIQIsFullI(iqp))

-		return;

-

-	iqp->q_counter++;

-	*iqp->q_wrptr++ = c;

-	if (iqp->q_wrptr >= iqp->q_top)

-		iqp->q_wrptr = iqp->q_buffer;

-

-}

-

-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;

-

-		//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++;

-		}

-

-		//fill a la derecha

-		while (width) {

-			_put(filler);

-			n++;

-			width--;

-		}

-	}

-

-	//return n; // can raise 'code is unreachable' warning

-

-}

-

-static void _print_hdr(void)

-{

-	uint32_t hfnum = USBH_DEBUG_USBHD.otg->HFNUM;

-	uint16_t hfir = USBH_DEBUG_USBHD.otg->HFIR;

-

-	_put(0xff);

-	_put(0xff);

-	_put(hfir & 0xff);

-	_put(hfir >> 8);

-	_put(hfnum & 0xff);

-	_put((hfnum >> 8) & 0xff);

-	_put((hfnum >> 16) & 0xff);

-	_put((hfnum >> 24) & 0xff);

-}

-

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

-{

-	va_list ap;

-	va_start(ap, fmt);

-	syssts_t sts = chSysGetStatusAndLockX();

-	_print_hdr();

-	_dbg_printf(fmt, ap);

-	_put(0);

-	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);

-	chSysRestoreStatusX(sts);

-	va_end(ap);

-}

-

-

-void usbDbgPuts(const char *s)

-{

-	uint32_t buff[2] = {

-		0xffff | (USBH_DEBUG_USBHD.otg->HFIR << 16),

-		USBH_DEBUG_USBHD.otg->HFNUM

-	};

-	uint8_t *p = (uint8_t *)buff;

-	uint8_t *top = p + 8;

-

-	syssts_t sts = chSysGetStatusAndLockX();

-	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;

-	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;

-	while (rem) {

-		*iqp->q_wrptr++ = *p;

-		if (iqp->q_wrptr >= iqp->q_top)

-			iqp->q_wrptr = iqp->q_buffer;

-		rem--;

-		if (++p == top) break;

-	}

-	while (rem) {

-		*iqp->q_wrptr++ = *s;

-		if (iqp->q_wrptr >= iqp->q_top)

-			iqp->q_wrptr = iqp->q_buffer;

-		rem--;

-		if (!*s++) break;

-	}

-	iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;

-	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);

-	chSysRestoreStatusX(sts);

-}

-

-void usbDbgReset(void) {

-	const char *msg = "\r\n\r\n==== DEBUG OUTPUT RESET ====\r\n";

-

-	syssts_t sts = chSysGetStatusAndLockX();

-	chIQResetI(&USBH_DEBUG_USBHD.iq);

-	chOQResetI(&USBH_DEBUG_SD.oqueue);

-	while (*msg) {

-		*USBH_DEBUG_SD.oqueue.q_wrptr++ = *msg++;

-		USBH_DEBUG_SD.oqueue.q_counter--;

-	}

-	chSysRestoreStatusX(sts);

-}

-

-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;

-}

-

-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 (!(USART1->SR & USART_SR_TXE));

-		USART1->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 (!(USART1->SR & USART_SR_TXE));

-					USART1->DR = '\r';

-					while (!(USART1->SR & USART_SR_TXE));

-					USART1->DR = '\n';

-					state = 0;

-					break;

-				}

-				while (!(USART1->SR & USART_SR_TXE));

-				USART1->DR = c;

-			}

-		}

-	}

-}

-

-static void usb_debug_thread(void *p) {

-	USBHDriver *host = (USBHDriver *)p;

-	uint8_t state = 0;

-

-	chRegSetThreadName("USBH_DBG");

-	while (true) {

-		msg_t c = chIQGet(&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 (state = 0);

-		} else {

-			uint16_t hfir;

-			uint32_t hfnum;

-

-			hfir = c;

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfir |= c << 8;

-

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfnum = c;

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfnum |= c << 8;

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfnum |= c << 16;

-			c = chIQGet(&host->iq); if (c < 0) goto reset;

-			hfnum |= c << 24;

-

-			uint32_t f = hfnum & 0xffff;

-			uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));

-			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d  ", f, p);

-

-			while (true) {

-				c = chIQGet(&host->iq); if (c < 0) goto reset;

-				if (!c) {

-					sdPut(&USBH_DEBUG_SD, '\r');

-					sdPut(&USBH_DEBUG_SD, '\n');

-					state = 0;

-					break;

-				}

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

-			}

-		}

-

-		continue;

-reset:

-		state = 0;

-	}

-}

-

-void usbDbgInit(USBHDriver *host) {

-	if (host != &USBH_DEBUG_USBHD)

-		return;

-	chIQObjectInit(&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);

-}

-#endif

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+
+#if HAL_USE_USBH
+
+#include "ch.h"
+#include "usbh/debug.h"
+#include <stdarg.h>
+#include "chprintf.h"
+
+#if USBH_DEBUG_ENABLE
+
+#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;
+
+	l = num;
+	if (divisor == 0) {
+		ll = num;
+	} else {
+		ll = divisor;
+	}
+
+	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);
+}
+
+#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;
+		}
+		return ++q;
+	}
+
+	
+
+
+}
+#endif
+
+static inline void _put(char c) {
+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
+
+	if (chIQIsFullI(iqp))
+		return;
+
+	iqp->q_counter++;
+	*iqp->q_wrptr++ = c;
+	if (iqp->q_wrptr >= iqp->q_top)
+		iqp->q_wrptr = iqp->q_buffer;
+
+}
+
+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;
+
+		//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++;
+		}
+
+		//fill a la derecha
+		while (width) {
+			_put(filler);
+			n++;
+			width--;
+		}
+	}
+
+	//return n; // can raise 'code is unreachable' warning
+
+}
+
+static void _print_hdr(void)
+{
+	uint32_t hfnum = USBH_DEBUG_USBHD.otg->HFNUM;
+	uint16_t hfir = USBH_DEBUG_USBHD.otg->HFIR;
+
+	_put(0xff);
+	_put(0xff);
+	_put(hfir & 0xff);
+	_put(hfir >> 8);
+	_put(hfnum & 0xff);
+	_put((hfnum >> 8) & 0xff);
+	_put((hfnum >> 16) & 0xff);
+	_put((hfnum >> 24) & 0xff);
+}
+
+void usbDbgPrintf(const char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	syssts_t sts = chSysGetStatusAndLockX();
+	_print_hdr();
+	_dbg_printf(fmt, ap);
+	_put(0);
+	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
+	chSysRestoreStatusX(sts);
+	va_end(ap);
+}
+
+
+void usbDbgPuts(const char *s)
+{
+	uint32_t buff[2] = {
+		0xffff | (USBH_DEBUG_USBHD.otg->HFIR << 16),
+		USBH_DEBUG_USBHD.otg->HFNUM
+	};
+	uint8_t *p = (uint8_t *)buff;
+	uint8_t *top = p + 8;
+
+	syssts_t sts = chSysGetStatusAndLockX();
+	input_queue_t *iqp = &USBH_DEBUG_USBHD.iq;
+	int rem = sizeof(USBH_DEBUG_USBHD.dbg_buff) - iqp->q_counter;
+	while (rem) {
+		*iqp->q_wrptr++ = *p;
+		if (iqp->q_wrptr >= iqp->q_top)
+			iqp->q_wrptr = iqp->q_buffer;
+		rem--;
+		if (++p == top) break;
+	}
+	while (rem) {
+		*iqp->q_wrptr++ = *s;
+		if (iqp->q_wrptr >= iqp->q_top)
+			iqp->q_wrptr = iqp->q_buffer;
+		rem--;
+		if (!*s++) break;
+	}
+	iqp->q_counter = sizeof(USBH_DEBUG_USBHD.dbg_buff) - rem;
+	chThdDequeueNextI(&USBH_DEBUG_USBHD.iq.q_waiting, Q_OK);
+	chSysRestoreStatusX(sts);
+}
+
+void usbDbgReset(void) {
+	const char *msg = "\r\n\r\n==== DEBUG OUTPUT RESET ====\r\n";
+
+	syssts_t sts = chSysGetStatusAndLockX();
+	chIQResetI(&USBH_DEBUG_USBHD.iq);
+	chOQResetI(&USBH_DEBUG_SD.oqueue);
+	while (*msg) {
+		*USBH_DEBUG_SD.oqueue.q_wrptr++ = *msg++;
+		USBH_DEBUG_SD.oqueue.q_counter--;
+	}
+	chSysRestoreStatusX(sts);
+}
+
+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;
+}
+
+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 (!(USART1->SR & USART_SR_TXE));
+		USART1->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 (!(USART1->SR & USART_SR_TXE));
+					USART1->DR = '\r';
+					while (!(USART1->SR & USART_SR_TXE));
+					USART1->DR = '\n';
+					state = 0;
+					break;
+				}
+				while (!(USART1->SR & USART_SR_TXE));
+				USART1->DR = c;
+			}
+		}
+	}
+}
+
+static void usb_debug_thread(void *p) {
+	USBHDriver *host = (USBHDriver *)p;
+	uint8_t state = 0;
+
+	chRegSetThreadName("USBH_DBG");
+	while (true) {
+		msg_t c = chIQGet(&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 (state = 0);
+		} else {
+			uint16_t hfir;
+			uint32_t hfnum;
+
+			hfir = c;
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfir |= c << 8;
+
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfnum = c;
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfnum |= c << 8;
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfnum |= c << 16;
+			c = chIQGet(&host->iq); if (c < 0) goto reset;
+			hfnum |= c << 24;
+
+			uint32_t f = hfnum & 0xffff;
+			uint32_t p = 1000 - ((hfnum >> 16) / (hfir / 1000));
+			chprintf((BaseSequentialStream *)&USBH_DEBUG_SD, "%05d.%03d  ", f, p);
+
+			while (true) {
+				c = chIQGet(&host->iq); if (c < 0) goto reset;
+				if (!c) {
+					sdPut(&USBH_DEBUG_SD, '\r');
+					sdPut(&USBH_DEBUG_SD, '\n');
+					state = 0;
+					break;
+				}
+				sdPut(&USBH_DEBUG_SD, (uint8_t)c);
+			}
+		}
+
+		continue;
+reset:
+		state = 0;
+	}
+}
+
+void usbDbgInit(USBHDriver *host) {
+	if (host != &USBH_DEBUG_USBHD)
+		return;
+	chIQObjectInit(&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);
+}
+#endif
+
+#endif
diff --git a/os/hal/src/usbh/usbh_desciter.c b/os/hal/src/usbh/hal_usbh_desciter.c
index 80e4728..63137d4 100644
--- a/os/hal/src/usbh/usbh_desciter.c
+++ b/os/hal/src/usbh/hal_usbh_desciter.c
@@ -1,165 +1,165 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-

-#if HAL_USE_USBH

-

-#include "usbh/defs.h"

-#include "usbh/desciter.h"

-

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

-			|| (buff[0] < USBH_DT_CONFIG_SIZE)

-			|| (buff[1] != USBH_DT_CONFIG))

-		return;

-

-	if (rem > ((usbh_config_descriptor_t *)buff)->wTotalLength) {

-		rem = ((usbh_config_descriptor_t *)buff)->wTotalLength;

-	}

-

-	icfg->valid = 1;

-	icfg->rem = rem;

-	icfg->curr = buff;

-}

-

-void if_iter_next(if_iterator_t *iif) {

-	const uint8_t *curr = iif->curr;

-	uint16_t rem = iif->rem;

-

-	iif->valid = 0;

-

-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-		return;

-

-	for (;;) {

-		rem -= curr[0];

-		curr += curr[0];

-

-		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-			return;

-

-		if (curr[1] == USBH_DT_INTERFACE_ASSOCIATION) {

-			if (curr[0] < USBH_DT_INTERFACE_ASSOCIATION_SIZE)

-				return;

-

-			iif->iad = (usbh_ia_descriptor_t *)curr;

-

-		} else if (curr[1] == USBH_DT_INTERFACE) {

-			if (curr[0] < USBH_DT_INTERFACE_SIZE)

-				return;

-

-			if (iif->iad) {

-				if ((curr[2] < iif->iad->bFirstInterface)

-					|| (curr[2] >= (iif->iad->bFirstInterface + iif->iad->bInterfaceCount)))

-					iif->iad = 0;

-			}

-			break;

-		}

-	}

-

-	iif->valid = 1;

-	iif->rem = rem;

-	iif->curr = curr;

-}

-

-void if_iter_init(if_iterator_t *iif, const generic_iterator_t *icfg) {

-	iif->iad = 0;

-	iif->curr = icfg->curr;

-	iif->rem = icfg->rem;

-	if_iter_next(iif);

-}

-

-void ep_iter_next(generic_iterator_t *iep) {

-	const uint8_t *curr = iep->curr;

-	uint16_t rem = iep->rem;

-

-	iep->valid = 0;

-

-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-		return;

-

-	for (;;) {

-		rem -= curr[0];

-		curr += curr[0];

-

-		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-			return;

-

-		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)

-				|| (curr[1] == USBH_DT_INTERFACE)

-				|| (curr[1] == USBH_DT_CONFIG)) {

-			return;

-		} else if (curr[1] == USBH_DT_ENDPOINT) {

-			if (curr[0] < USBH_DT_ENDPOINT_SIZE)

-				return;

-

-			break;

-		}

-	}

-

-	iep->valid = 1;

-	iep->rem = rem;

-	iep->curr = curr;

-}

-

-void ep_iter_init(generic_iterator_t *iep, const if_iterator_t *iif) {

-	iep->curr = iif->curr;

-	iep->rem = iif->rem;

-	ep_iter_next(iep);

-}

-

-void cs_iter_next(generic_iterator_t *ics) {

-	const uint8_t *curr = ics->curr;

-	uint16_t rem = ics->rem;

-

-	ics->valid = 0;

-

-	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-		return;

-

-	//for (;;) {

-		rem -= curr[0];

-		curr += curr[0];

-

-		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))

-			return;

-

-		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)

-				|| (curr[1] == USBH_DT_INTERFACE)

-				|| (curr[1] == USBH_DT_CONFIG)

-				|| (curr[1] == USBH_DT_ENDPOINT)) {

-			return;

-		}

-

-	//	break;

-	//}

-

-	ics->valid = 1;

-	ics->rem = rem;

-	ics->curr = curr;

-}

-

-void cs_iter_init(generic_iterator_t *ics, const generic_iterator_t *iter) {

-	ics->curr = iter->curr;

-	ics->rem = iter->rem;

-	cs_iter_next(ics);

-}

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+
+#if HAL_USE_USBH
+
+#include "usbh/defs.h"
+#include "usbh/desciter.h"
+
+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])
+			|| (buff[0] < USBH_DT_CONFIG_SIZE)
+			|| (buff[1] != USBH_DT_CONFIG))
+		return;
+
+	if (rem > ((usbh_config_descriptor_t *)buff)->wTotalLength) {
+		rem = ((usbh_config_descriptor_t *)buff)->wTotalLength;
+	}
+
+	icfg->valid = 1;
+	icfg->rem = rem;
+	icfg->curr = buff;
+}
+
+void if_iter_next(if_iterator_t *iif) {
+	const uint8_t *curr = iif->curr;
+	uint16_t rem = iif->rem;
+
+	iif->valid = 0;
+
+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		return;
+
+	for (;;) {
+		rem -= curr[0];
+		curr += curr[0];
+
+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+			return;
+
+		if (curr[1] == USBH_DT_INTERFACE_ASSOCIATION) {
+			if (curr[0] < USBH_DT_INTERFACE_ASSOCIATION_SIZE)
+				return;
+
+			iif->iad = (usbh_ia_descriptor_t *)curr;
+
+		} else if (curr[1] == USBH_DT_INTERFACE) {
+			if (curr[0] < USBH_DT_INTERFACE_SIZE)
+				return;
+
+			if (iif->iad) {
+				if ((curr[2] < iif->iad->bFirstInterface)
+					|| (curr[2] >= (iif->iad->bFirstInterface + iif->iad->bInterfaceCount)))
+					iif->iad = 0;
+			}
+			break;
+		}
+	}
+
+	iif->valid = 1;
+	iif->rem = rem;
+	iif->curr = curr;
+}
+
+void if_iter_init(if_iterator_t *iif, const generic_iterator_t *icfg) {
+	iif->iad = 0;
+	iif->curr = icfg->curr;
+	iif->rem = icfg->rem;
+	if_iter_next(iif);
+}
+
+void ep_iter_next(generic_iterator_t *iep) {
+	const uint8_t *curr = iep->curr;
+	uint16_t rem = iep->rem;
+
+	iep->valid = 0;
+
+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		return;
+
+	for (;;) {
+		rem -= curr[0];
+		curr += curr[0];
+
+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+			return;
+
+		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)
+				|| (curr[1] == USBH_DT_INTERFACE)
+				|| (curr[1] == USBH_DT_CONFIG)) {
+			return;
+		} else if (curr[1] == USBH_DT_ENDPOINT) {
+			if (curr[0] < USBH_DT_ENDPOINT_SIZE)
+				return;
+
+			break;
+		}
+	}
+
+	iep->valid = 1;
+	iep->rem = rem;
+	iep->curr = curr;
+}
+
+void ep_iter_init(generic_iterator_t *iep, const if_iterator_t *iif) {
+	iep->curr = iif->curr;
+	iep->rem = iif->rem;
+	ep_iter_next(iep);
+}
+
+void cs_iter_next(generic_iterator_t *ics) {
+	const uint8_t *curr = ics->curr;
+	uint16_t rem = ics->rem;
+
+	ics->valid = 0;
+
+	if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+		return;
+
+	//for (;;) {
+		rem -= curr[0];
+		curr += curr[0];
+
+		if ((curr[0] < 2) || (rem < 2) || (rem < curr[0]))
+			return;
+
+		if ((curr[1] == USBH_DT_INTERFACE_ASSOCIATION)
+				|| (curr[1] == USBH_DT_INTERFACE)
+				|| (curr[1] == USBH_DT_CONFIG)
+				|| (curr[1] == USBH_DT_ENDPOINT)) {
+			return;
+		}
+
+	//	break;
+	//}
+
+	ics->valid = 1;
+	ics->rem = rem;
+	ics->curr = curr;
+}
+
+void cs_iter_init(generic_iterator_t *ics, const generic_iterator_t *iter) {
+	ics->curr = iter->curr;
+	ics->rem = iter->rem;
+	cs_iter_next(ics);
+}
+
+#endif
diff --git a/os/hal/src/usbh/usbh_ftdi.c b/os/hal/src/usbh/hal_usbh_ftdi.c
index cdf3410..4bd7296 100644
--- a/os/hal/src/usbh/usbh_ftdi.c
+++ b/os/hal/src/usbh/hal_usbh_ftdi.c
@@ -1,717 +1,717 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-#include "usbh.h"

-

-#if HAL_USBH_USE_FTDI

-

-#if !HAL_USE_USBH

-#error "USBHFTDI needs USBH"

-#endif

-

-#include <string.h>

-#include "usbh/dev/ftdi.h"

-#include "usbh/internal.h"

-

-//#pragma GCC optimize("Og")

-

-

-#if USBHFTDI_DEBUG_ENABLE_TRACE

-#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#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

-

-#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

-

-

-/*===========================================================================*/

-/* USB Class driver loader for FTDI								 		 	 */

-/*===========================================================================*/

-USBHFTDIDriver USBHFTDID[HAL_USBHFTDI_MAX_INSTANCES];

-

-static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

-static void _ftdi_unload(usbh_baseclassdriver_t *drv);

-

-static const usbh_classdriver_vmt_t class_driver_vmt = {

-	_ftdi_load,

-	_ftdi_unload

-};

-

-const usbh_classdriverinfo_t usbhftdiClassDriverInfo = {

-	0xff, 0xff, 0xff, "FTDI", &class_driver_vmt

-};

-

-static USBHFTDIPortDriver *_find_port(void) {

-	uint8_t i;

-	for (i = 0; i < HAL_USBHFTDI_MAX_PORTS; i++) {

-		if (FTDIPD[i].ftdip == NULL)

-			return &FTDIPD[i];

-	}

-	return NULL;

-}

-

-static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

-	int i;

-	USBHFTDIDriver *ftdip;

-

-	if (dev->devDesc.idVendor != 0x0403) {

-		uerr("FTDI: Unrecognized VID");

-		return NULL;

-	}

-

-	switch (dev->devDesc.idProduct) {

-	case 0x6001:

-	case 0x6010:

-	case 0x6011:

-	case 0x6014:

-	case 0x6015:

-		break;

-	default:

-		uerr("FTDI: Unrecognized PID");

-		return NULL;

-	}

-

-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))

-		return NULL;

-

-	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t * const)descriptor;

-	if (ifdesc->bInterfaceNumber != 0) {

-		uwarn("FTDI: Will allocate driver along with IF #0");

-	}

-

-	/* alloc driver */

-	for (i = 0; i < HAL_USBHFTDI_MAX_INSTANCES; i++) {

-		if (USBHFTDID[i].dev == NULL) {

-			ftdip = &USBHFTDID[i];

-			goto alloc_ok;

-		}

-	}

-

-	uwarn("FTDI: Can't alloc driver");

-

-	/* can't alloc */

-	return NULL;

-

-alloc_ok:

-	/* initialize the driver's variables */

-	ftdip->ports = 0;

-	switch (dev->devDesc.bcdDevice) {

-	case 0x200:		//AM

-		uinfo("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");

-		ftdip->type = USBHFTDI_TYPE_B;

-		break;

-	case 0x700:		//2232H;

-	case 0x800:		//4232H;

-	case 0x900:		//232H;

-		uinfo("FTDI: Type H chip");

-		ftdip->type = USBHFTDI_TYPE_H;

-	default:

-		uerr("FTDI: Unrecognized chip type");

-		return NULL;

-	}

-	usbhEPSetName(&dev->ctrl, "FTD[CTRL]");

-

-	/* parse the configuration descriptor */

-	generic_iterator_t iep, icfg;

-	if_iterator_t iif;

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

-

-		USBHFTDIPortDriver *const prt = _find_port();

-		if (prt == NULL) {

-			uwarn("\tCan't alloc port for this interface");

-			break;

-		}

-

-		prt->ifnum = ifdesc->bInterfaceNumber;

-		prt->epin.status = USBH_EPSTATUS_UNINITIALIZED;

-		prt->epout.status = USBH_EPSTATUS_UNINITIALIZED;

-

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

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

-				usbhEPObjectInit(&prt->epout, dev, epdesc);

-				usbhEPSetName(&prt->epout, "FTD[BOUT]");

-			} else {

-				uinfof("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");

-			continue;

-		}

-

-		/* link the new block driver to the list */

-		prt->next = ftdip->ports;

-		ftdip->ports = prt;

-		prt->ftdip = ftdip;

-

-		prt->state = USBHFTDIP_STATE_ACTIVE;

-	}

-

-	return (usbh_baseclassdriver_t *)ftdip;

-

-}

-

-static void _stop(USBHFTDIPortDriver *ftdipp);

-static void _ftdi_unload(usbh_baseclassdriver_t *drv) {

-	osalDbgCheck(drv != NULL);

-	USBHFTDIDriver *const ftdip = (USBHFTDIDriver *)drv;

-	USBHFTDIPortDriver *ftdipp = ftdip->ports;

-

-	osalMutexLock(&ftdip->mtx);

-	while (ftdipp) {

-		_stop(ftdipp);

-		ftdipp = ftdipp->next;

-	}

-

-	ftdipp = ftdip->ports;

-	osalSysLock();

-	while (ftdipp) {

-		USBHFTDIPortDriver *next = ftdipp->next;

-		usbhftdipObjectInit(ftdipp);

-		ftdipp = next;

-	}

-	osalSysUnlock();

-	osalMutexUnlock(&ftdip->mtx);

-}

-

-

-USBHFTDIPortDriver FTDIPD[HAL_USBHFTDI_MAX_PORTS];

-

-

-#define FTDI_COMMAND_RESET		0

-#define FTDI_RESET_ALL 			0

-#define FTDI_RESET_PURGE_RX 	1

-#define FTDI_RESET_PURGE_TX 	2

-

-#define FTDI_COMMAND_SETFLOW    2

-

-#define FTDI_COMMAND_SETBAUD    3

-

-#define FTDI_COMMAND_SETDATA    4

-#define FTDI_SETDATA_BREAK      (0x1 << 14)

-

-#if 0

-#define FTDI_COMMAND_MODEMCTRL  	1

-#define FTDI_COMMAND_GETMODEMSTATUS       5 /* Retrieve current value of modem status register */

-#define FTDI_COMMAND_SETEVENTCHAR         6 /* Set the event character */

-#define FTDI_COMMAND_SETERRORCHAR         7 /* Set the error character */

-#define FTDI_COMMAND_SETLATENCYTIMER      9 /* Set the latency timer */

-#define FTDI_COMMAND_GETLATENCYTIMER      10 /* Get the latency timer */

-#endif

-

-/*

- * DATA FORMAT

- *

- * IN Endpoint

- *

- * The device reserves the first two bytes of data on this endpoint to contain

- * the current values of the modem and line status registers. In the absence of

- * data, the device generates a message consisting of these two status bytes

- * every 40 ms

- *

- * Byte 0: Modem Status

- *

- * Offset       Description

- * B0   Reserved - must be 1

- * B1   Reserved - must be 0

- * B2   Reserved - must be 0

- * B3   Reserved - must be 0

- * B4   Clear to Send (CTS)

- * B5   Data Set Ready (DSR)

- * B6   Ring Indicator (RI)

- * B7   Receive Line Signal Detect (RLSD)

- *

- * Byte 1: Line Status

- *

- * Offset       Description

- * B0   Data Ready (DR)

- * B1   Overrun Error (OE)

- * B2   Parity Error (PE)

- * B3   Framing Error (FE)

- * B4   Break Interrupt (BI)

- * B5   Transmitter Holding Register (THRE)

- * B6   Transmitter Empty (TEMT)

- * B7   Error in RCVR FIFO

- *

- */

-#define FTDI_RS0_CTS    (1 << 4)

-#define FTDI_RS0_DSR    (1 << 5)

-#define FTDI_RS0_RI     (1 << 6)

-#define FTDI_RS0_RLSD   (1 << 7)

-

-#define FTDI_RS_DR      1

-#define FTDI_RS_OE      (1<<1)

-#define FTDI_RS_PE      (1<<2)

-#define FTDI_RS_FE      (1<<3)

-#define FTDI_RS_BI      (1<<4)

-#define FTDI_RS_THRE    (1<<5)

-#define FTDI_RS_TEMT    (1<<6)

-#define FTDI_RS_FIFO    (1<<7)

-

-

-static usbh_urbstatus_t _ftdi_port_control(USBHFTDIPortDriver *ftdipp,

-		uint8_t bRequest, uint8_t wValue, uint8_t bHIndex, uint16_t wLength,

-		uint8_t *buff) {

-

-	static const uint8_t bmRequestType[] = {

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //0 FTDI_COMMAND_RESET

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //1 FTDI_COMMAND_MODEMCTRL

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //2 FTDI_COMMAND_SETFLOW

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //3 FTDI_COMMAND_SETBAUD

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //4 FTDI_COMMAND_SETDATA

-	};

-

-	osalDbgCheck(bRequest < sizeof_array(bmRequestType));

-	osalDbgCheck(bRequest != 1);

-

-	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {

-			bmRequestType[bRequest],

-			bRequest,

-			wValue,

-			(bHIndex << 8) | (ftdipp->ifnum + 1),

-			wLength

-	};

-

-	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, buff, NULL, MS2ST(1000));

-}

-

-static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t 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);

-		if ((divisor3 & 0x7) == 7)

-			divisor3++; /* round x.7/8 up to x+1 */

-

-		divisor = divisor3 >> 3;

-	    divisor3 &= 0x7;

-		if (divisor3 == 1)

-			divisor |= 0xc000;

-		else if (divisor3 >= 4)

-			divisor |= 0x4000;

-		else if (divisor3 != 0)

-			divisor |= 0x8000;

-		else if (divisor == 1)

-			divisor = 0;    /* special case for maximum baud rate */

-	} else {

-		if (type == USBHFTDI_TYPE_B) {

-			divisor = ((48000000UL / 2) + baud / 2) / baud;

-			uinfof("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);

-		}

-		divisor = (divisor >> 3) | (divfrac[divisor & 0x7] << 14);

-

-		/* Deal with special cases for highest baud rates. */

-		if (divisor == 1)

-			divisor = 0;

-		else if (divisor == 0x4001)

-			divisor = 1;

-

-		if (type == USBHFTDI_TYPE_H)

-			divisor |= 0x00020000;

-	}

-	return divisor;

-}

-

-static usbh_urbstatus_t _set_baudrate(USBHFTDIPortDriver *ftdipp, uint32_t baudrate) {

-	uint32_t divisor = _get_divisor(baudrate, ftdipp->ftdip->type);

-	uint16_t wValue = (uint16_t)divisor;

-	uint16_t wIndex = (uint16_t)(divisor >> 16);

-	if (ftdipp->ftdip->dev->basicConfigDesc.bNumInterfaces > 1)

-		wIndex = (wIndex << 8) | (ftdipp->ifnum + 1);

-

-	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {

-		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE,

-		FTDI_COMMAND_SETBAUD,

-		wValue,

-		wIndex,

-		0

-	};

-	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, NULL, NULL, MS2ST(1000));

-}

-

-

-static void _submitOutI(USBHFTDIPortDriver *ftdipp, uint32_t len) {

-	udbgf("FTDI: Submit OUT %d", len);

-	ftdipp->oq_urb.requestedLength = len;

-	usbhURBObjectResetI(&ftdipp->oq_urb);

-	usbhURBSubmitI(&ftdipp->oq_urb);

-}

-

-static void _out_cb(usbh_urb_t *urb) {

-	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;

-	switch (urb->status) {

-	case USBH_URBSTATUS_OK:

-		ftdipp->oq_ptr = ftdipp->oq_buff;

-		ftdipp->oq_counter = 64;

-		chThdDequeueNextI(&ftdipp->oq_waiting, Q_OK);

-		return;

-	case USBH_URBSTATUS_DISCONNECTED:

-		uwarn("FTDI: URB OUT disconnected");

-		chThdDequeueNextI(&ftdipp->oq_waiting, Q_RESET);

-		return;

-	default:

-		uerrf("FTDI: URB OUT status unexpected = %d", urb->status);

-		break;

-	}

-	usbhURBObjectResetI(&ftdipp->oq_urb);

-	usbhURBSubmitI(&ftdipp->oq_urb);

-}

-

-static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,

-		size_t n, systime_t timeout) {

-	chDbgCheck(n > 0U);

-

-	size_t w = 0;

-	chSysLock();

-	while (true) {

-		if (ftdipp->state != USBHFTDIP_STATE_READY) {

-			chSysUnlock();

-			return w;

-		}

-		while (usbhURBIsBusy(&ftdipp->oq_urb)) {

-			if (chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout) != Q_OK) {

-				chSysUnlock();

-				return w;

-			}

-		}

-

-		*ftdipp->oq_ptr++ = *bp++;

-		if (--ftdipp->oq_counter == 0) {

-			_submitOutI(ftdipp, 64);

-			chSchRescheduleS();

-		}

-		chSysUnlock(); /* Gives a preemption chance in a controlled point.*/

-

-		w++;

-		if (--n == 0U)

-			return w;

-

-		chSysLock();

-	}

-}

-

-static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeout) {

-

-	chSysLock();

-	if (ftdipp->state != USBHFTDIP_STATE_READY) {

-		chSysUnlock();

-		return Q_RESET;

-	}

-

-	while (usbhURBIsBusy(&ftdipp->oq_urb)) {

-		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout);

-		if (msg < Q_OK) {

-			chSysUnlock();

-			return msg;

-		}

-	}

-

-	*ftdipp->oq_ptr++ = b;

-	if (--ftdipp->oq_counter == 0) {

-		_submitOutI(ftdipp, 64);

-		chSchRescheduleS();

-	}

-	chSysUnlock();

-	return Q_OK;

-}

-

-static size_t _write(USBHFTDIPortDriver *ftdipp, const uint8_t *bp, size_t n) {

-	return _write_timeout(ftdipp, bp, n, TIME_INFINITE);

-}

-

-static msg_t _put(USBHFTDIPortDriver *ftdipp, uint8_t b) {

-	return _put_timeout(ftdipp, b, TIME_INFINITE);

-}

-

-static void _submitInI(USBHFTDIPortDriver *ftdipp) {

-	udbg("FTDI: Submit IN");

-	usbhURBObjectResetI(&ftdipp->iq_urb);

-	usbhURBSubmitI(&ftdipp->iq_urb);

-}

-

-static void _in_cb(usbh_urb_t *urb) {

-	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;

-	switch (urb->status) {

-	case USBH_URBSTATUS_OK:

-		if (urb->actualLength < 2) {

-			uwarnf("FTDI: URB IN actualLength = %d, < 2", urb->actualLength);

-		} else if (urb->actualLength > 2) {

-			udbgf("FTDI: URB IN data len=%d, status=%02x %02x",

-					urb->actualLength - 2,

-					((uint8_t *)urb->buff)[0],

-					((uint8_t *)urb->buff)[1]);

-			ftdipp->iq_ptr = ftdipp->iq_buff + 2;

-			ftdipp->iq_counter = urb->actualLength - 2;

-			chThdDequeueNextI(&ftdipp->iq_waiting, Q_OK);

-			return;

-		} else {

-			udbgf("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");

-		chThdDequeueNextI(&ftdipp->iq_waiting, Q_RESET);

-		return;

-	default:

-		uerrf("FTDI: URB IN status unexpected = %d", urb->status);

-		break;

-	}

-	_submitInI(ftdipp);

-}

-

-static size_t _read_timeout(USBHFTDIPortDriver *ftdipp, uint8_t *bp,

-		size_t n, systime_t timeout) {

-	size_t r = 0;

-

-	chDbgCheck(n > 0U);

-

-	chSysLock();

-	while (true) {

-		if (ftdipp->state != USBHFTDIP_STATE_READY) {

-			chSysUnlock();

-			return r;

-		}

-		while (ftdipp->iq_counter == 0) {

-			if (!usbhURBIsBusy(&ftdipp->iq_urb))

-				_submitInI(ftdipp);

-			if (chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout) != Q_OK) {

-				chSysUnlock();

-				return r;

-			}

-		}

-		*bp++ = *ftdipp->iq_ptr++;

-		if (--ftdipp->iq_counter == 0) {

-			_submitInI(ftdipp);

-			chSchRescheduleS();

-		}

-		chSysUnlock();

-

-		r++;

-		if (--n == 0U)

-			return r;

-

-		chSysLock();

-	}

-}

-

-static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {

-	uint8_t b;

-

-	chSysLock();

-	if (ftdipp->state != USBHFTDIP_STATE_READY) {

-		chSysUnlock();

-		return Q_RESET;

-	}

-	while (ftdipp->iq_counter == 0) {

-		if (!usbhURBIsBusy(&ftdipp->iq_urb))

-			_submitInI(ftdipp);

-		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout);

-		if (msg < Q_OK) {

-			chSysUnlock();

-			return msg;

-		}

-	}

-	b = *ftdipp->iq_ptr++;

-	if (--ftdipp->iq_counter == 0) {

-		_submitInI(ftdipp);

-		chSchRescheduleS();

-	}

-	chSysUnlock();

-

-	return (msg_t)b;

-}

-

-static msg_t _get(USBHFTDIPortDriver *ftdipp) {

-	return _get_timeout(ftdipp, TIME_INFINITE);

-}

-

-static size_t _read(USBHFTDIPortDriver *ftdipp, uint8_t *bp, size_t n) {

-	return _read_timeout(ftdipp, bp, n, TIME_INFINITE);

-}

-

-static void _vt(void *p) {

-	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)p;

-	chSysLockFromISR();

-	uint32_t len = ftdipp->oq_ptr - ftdipp->oq_buff;

-	if (len && !usbhURBIsBusy(&ftdipp->oq_urb)) {

-		_submitOutI(ftdipp, len);

-	}

-	if ((ftdipp->iq_counter == 0) && !usbhURBIsBusy(&ftdipp->iq_urb)) {

-		_submitInI(ftdipp);

-	}

-	chVTSetI(&ftdipp->vt, MS2ST(16), _vt, ftdipp);

-	chSysUnlockFromISR();

-}

-

-static const struct FTDIPortDriverVMT async_channel_vmt = {

-	(size_t (*)(void *, const uint8_t *, size_t))_write,

-	(size_t (*)(void *, uint8_t *, size_t))_read,

-	(msg_t (*)(void *, uint8_t))_put,

-	(msg_t (*)(void *))_get,

-	(msg_t (*)(void *, uint8_t, systime_t))_put_timeout,

-	(msg_t (*)(void *, systime_t))_get_timeout,

-	(size_t (*)(void *, const uint8_t *, size_t, systime_t))_write_timeout,

-	(size_t (*)(void *, uint8_t *, size_t, systime_t))_read_timeout

-};

-

-

-static void _stop(USBHFTDIPortDriver *ftdipp) {

-	osalSysLock();

-	chVTResetI(&ftdipp->vt);

-	usbhEPCloseS(&ftdipp->epin);

-	usbhEPCloseS(&ftdipp->epout);

-	chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);

-	chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);

-	osalOsRescheduleS();

-	ftdipp->state = USBHFTDIP_STATE_ACTIVE;

-	osalSysUnlock();

-}

-

-void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {

-	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)

-			|| (ftdipp->state == USBHFTDIP_STATE_READY));

-

-	if (ftdipp->state == USBHFTDIP_STATE_ACTIVE) {

-		return;

-	}

-

-	osalMutexLock(&ftdipp->ftdip->mtx);

-	_stop(ftdipp);

-	osalMutexUnlock(&ftdipp->ftdip->mtx);

-}

-

-void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config) {

-	static const USBHFTDIPortConfig default_config = {

-		HAL_USBHFTDI_DEFAULT_SPEED,

-		HAL_USBHFTDI_DEFAULT_FRAMING,

-		HAL_USBHFTDI_DEFAULT_HANDSHAKE,

-		HAL_USBHFTDI_DEFAULT_XON,

-		HAL_USBHFTDI_DEFAULT_XOFF

-	};

-

-	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)

-			|| (ftdipp->state == USBHFTDIP_STATE_READY));

-

-	if (ftdipp->state == USBHFTDIP_STATE_READY)

-		return;

-

-	osalMutexLock(&ftdipp->ftdip->mtx);

-	if (config == NULL)

-		config = &default_config;

-

-	uint16_t wValue = 0;

-	_ftdi_port_control(ftdipp, FTDI_COMMAND_RESET, FTDI_RESET_ALL, 0, 0, NULL);

-	_set_baudrate(ftdipp, config->speed);

-	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETDATA, config->framing, 0, 0, NULL);

-	if (config->handshake & USBHFTDI_HANDSHAKE_XON_XOFF)

-		wValue = (config->xoff_character << 8) | config->xon_character;

-	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETFLOW, wValue, config->handshake, 0, NULL);

-

-	usbhURBObjectInit(&ftdipp->oq_urb, &ftdipp->epout, _out_cb, ftdipp, ftdipp->oq_buff, 0);

-	chThdQueueObjectInit(&ftdipp->oq_waiting);

-	ftdipp->oq_counter = 64;

-	ftdipp->oq_ptr = ftdipp->oq_buff;

-	usbhEPOpen(&ftdipp->epout);

-

-	usbhURBObjectInit(&ftdipp->iq_urb, &ftdipp->epin, _in_cb, ftdipp, ftdipp->iq_buff, 64);

-	chThdQueueObjectInit(&ftdipp->iq_waiting);

-	ftdipp->iq_counter = 0;

-	ftdipp->iq_ptr = ftdipp->iq_buff;

-	usbhEPOpen(&ftdipp->epin);

-	osalSysLock();

-	usbhURBSubmitI(&ftdipp->iq_urb);

-	osalSysUnlock();

-

-	chVTObjectInit(&ftdipp->vt);

-	chVTSet(&ftdipp->vt, MS2ST(16), _vt, ftdipp);

-

-	ftdipp->state = USBHFTDIP_STATE_READY;

-	osalMutexUnlock(&ftdipp->ftdip->mtx);

-}

-

-void usbhftdiObjectInit(USBHFTDIDriver *ftdip) {

-	osalDbgCheck(ftdip != NULL);

-	memset(ftdip, 0, sizeof(*ftdip));

-	ftdip->info = &usbhftdiClassDriverInfo;

-	osalMutexObjectInit(&ftdip->mtx);

-}

-

-void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp) {

-	osalDbgCheck(ftdipp != NULL);

-	memset(ftdipp, 0, sizeof(*ftdipp));

-	ftdipp->vmt = &async_channel_vmt;

-	ftdipp->state = USBHFTDIP_STATE_STOP;

-}

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+#include "hal_usbh.h"
+
+#if HAL_USBH_USE_FTDI
+
+#if !HAL_USE_USBH
+#error "USBHFTDI needs USBH"
+#endif
+
+#include <string.h>
+#include "usbh/dev/ftdi.h"
+#include "usbh/internal.h"
+
+//#pragma GCC optimize("Og")
+
+
+#if USBHFTDI_DEBUG_ENABLE_TRACE
+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#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
+
+#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
+
+
+/*===========================================================================*/
+/* USB Class driver loader for FTDI								 		 	 */
+/*===========================================================================*/
+USBHFTDIDriver USBHFTDID[HAL_USBHFTDI_MAX_INSTANCES];
+
+static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void _ftdi_unload(usbh_baseclassdriver_t *drv);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_ftdi_load,
+	_ftdi_unload
+};
+
+const usbh_classdriverinfo_t usbhftdiClassDriverInfo = {
+	0xff, 0xff, 0xff, "FTDI", &class_driver_vmt
+};
+
+static USBHFTDIPortDriver *_find_port(void) {
+	uint8_t i;
+	for (i = 0; i < HAL_USBHFTDI_MAX_PORTS; i++) {
+		if (FTDIPD[i].ftdip == NULL)
+			return &FTDIPD[i];
+	}
+	return NULL;
+}
+
+static usbh_baseclassdriver_t *_ftdi_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	int i;
+	USBHFTDIDriver *ftdip;
+
+	if (dev->devDesc.idVendor != 0x0403) {
+		uerr("FTDI: Unrecognized VID");
+		return NULL;
+	}
+
+	switch (dev->devDesc.idProduct) {
+	case 0x6001:
+	case 0x6010:
+	case 0x6011:
+	case 0x6014:
+	case 0x6015:
+		break;
+	default:
+		uerr("FTDI: Unrecognized PID");
+		return NULL;
+	}
+
+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+		return NULL;
+
+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t * const)descriptor;
+	if (ifdesc->bInterfaceNumber != 0) {
+		uwarn("FTDI: Will allocate driver along with IF #0");
+	}
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHFTDI_MAX_INSTANCES; i++) {
+		if (USBHFTDID[i].dev == NULL) {
+			ftdip = &USBHFTDID[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("FTDI: Can't alloc driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	ftdip->ports = 0;
+	switch (dev->devDesc.bcdDevice) {
+	case 0x200:		//AM
+		uinfo("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");
+		ftdip->type = USBHFTDI_TYPE_B;
+		break;
+	case 0x700:		//2232H;
+	case 0x800:		//4232H;
+	case 0x900:		//232H;
+		uinfo("FTDI: Type H chip");
+		ftdip->type = USBHFTDI_TYPE_H;
+	default:
+		uerr("FTDI: Unrecognized chip type");
+		return NULL;
+	}
+	usbhEPSetName(&dev->ctrl, "FTD[CTRL]");
+
+	/* parse the configuration descriptor */
+	generic_iterator_t iep, icfg;
+	if_iterator_t iif;
+	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);
+
+		USBHFTDIPortDriver *const prt = _find_port();
+		if (prt == NULL) {
+			uwarn("\tCan't alloc port for this interface");
+			break;
+		}
+
+		prt->ifnum = ifdesc->bInterfaceNumber;
+		prt->epin.status = USBH_EPSTATUS_UNINITIALIZED;
+		prt->epout.status = USBH_EPSTATUS_UNINITIALIZED;
+
+		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);
+				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);
+				usbhEPObjectInit(&prt->epout, dev, epdesc);
+				usbhEPSetName(&prt->epout, "FTD[BOUT]");
+			} else {
+				uinfof("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");
+			continue;
+		}
+
+		/* link the new block driver to the list */
+		prt->next = ftdip->ports;
+		ftdip->ports = prt;
+		prt->ftdip = ftdip;
+
+		prt->state = USBHFTDIP_STATE_ACTIVE;
+	}
+
+	return (usbh_baseclassdriver_t *)ftdip;
+
+}
+
+static void _stop(USBHFTDIPortDriver *ftdipp);
+static void _ftdi_unload(usbh_baseclassdriver_t *drv) {
+	osalDbgCheck(drv != NULL);
+	USBHFTDIDriver *const ftdip = (USBHFTDIDriver *)drv;
+	USBHFTDIPortDriver *ftdipp = ftdip->ports;
+
+	osalMutexLock(&ftdip->mtx);
+	while (ftdipp) {
+		_stop(ftdipp);
+		ftdipp = ftdipp->next;
+	}
+
+	ftdipp = ftdip->ports;
+	osalSysLock();
+	while (ftdipp) {
+		USBHFTDIPortDriver *next = ftdipp->next;
+		usbhftdipObjectInit(ftdipp);
+		ftdipp = next;
+	}
+	osalSysUnlock();
+	osalMutexUnlock(&ftdip->mtx);
+}
+
+
+USBHFTDIPortDriver FTDIPD[HAL_USBHFTDI_MAX_PORTS];
+
+
+#define FTDI_COMMAND_RESET		0
+#define FTDI_RESET_ALL 			0
+#define FTDI_RESET_PURGE_RX 	1
+#define FTDI_RESET_PURGE_TX 	2
+
+#define FTDI_COMMAND_SETFLOW    2
+
+#define FTDI_COMMAND_SETBAUD    3
+
+#define FTDI_COMMAND_SETDATA    4
+#define FTDI_SETDATA_BREAK      (0x1 << 14)
+
+#if 0
+#define FTDI_COMMAND_MODEMCTRL  	1
+#define FTDI_COMMAND_GETMODEMSTATUS       5 /* Retrieve current value of modem status register */
+#define FTDI_COMMAND_SETEVENTCHAR         6 /* Set the event character */
+#define FTDI_COMMAND_SETERRORCHAR         7 /* Set the error character */
+#define FTDI_COMMAND_SETLATENCYTIMER      9 /* Set the latency timer */
+#define FTDI_COMMAND_GETLATENCYTIMER      10 /* Get the latency timer */
+#endif
+
+/*
+ * DATA FORMAT
+ *
+ * IN Endpoint
+ *
+ * The device reserves the first two bytes of data on this endpoint to contain
+ * the current values of the modem and line status registers. In the absence of
+ * data, the device generates a message consisting of these two status bytes
+ * every 40 ms
+ *
+ * Byte 0: Modem Status
+ *
+ * Offset       Description
+ * B0   Reserved - must be 1
+ * B1   Reserved - must be 0
+ * B2   Reserved - must be 0
+ * B3   Reserved - must be 0
+ * B4   Clear to Send (CTS)
+ * B5   Data Set Ready (DSR)
+ * B6   Ring Indicator (RI)
+ * B7   Receive Line Signal Detect (RLSD)
+ *
+ * Byte 1: Line Status
+ *
+ * Offset       Description
+ * B0   Data Ready (DR)
+ * B1   Overrun Error (OE)
+ * B2   Parity Error (PE)
+ * B3   Framing Error (FE)
+ * B4   Break Interrupt (BI)
+ * B5   Transmitter Holding Register (THRE)
+ * B6   Transmitter Empty (TEMT)
+ * B7   Error in RCVR FIFO
+ *
+ */
+#define FTDI_RS0_CTS    (1 << 4)
+#define FTDI_RS0_DSR    (1 << 5)
+#define FTDI_RS0_RI     (1 << 6)
+#define FTDI_RS0_RLSD   (1 << 7)
+
+#define FTDI_RS_DR      1
+#define FTDI_RS_OE      (1<<1)
+#define FTDI_RS_PE      (1<<2)
+#define FTDI_RS_FE      (1<<3)
+#define FTDI_RS_BI      (1<<4)
+#define FTDI_RS_THRE    (1<<5)
+#define FTDI_RS_TEMT    (1<<6)
+#define FTDI_RS_FIFO    (1<<7)
+
+
+static usbh_urbstatus_t _ftdi_port_control(USBHFTDIPortDriver *ftdipp,
+		uint8_t bRequest, uint8_t wValue, uint8_t bHIndex, uint16_t wLength,
+		uint8_t *buff) {
+
+	static const uint8_t bmRequestType[] = {
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //0 FTDI_COMMAND_RESET
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //1 FTDI_COMMAND_MODEMCTRL
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //2 FTDI_COMMAND_SETFLOW
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //3 FTDI_COMMAND_SETBAUD
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE, //4 FTDI_COMMAND_SETDATA
+	};
+
+	osalDbgCheck(bRequest < sizeof_array(bmRequestType));
+	osalDbgCheck(bRequest != 1);
+
+	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {
+			bmRequestType[bRequest],
+			bRequest,
+			wValue,
+			(bHIndex << 8) | (ftdipp->ifnum + 1),
+			wLength
+	};
+
+	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, buff, NULL, MS2ST(1000));
+}
+
+static uint32_t _get_divisor(uint32_t baud, usbhftdi_type_t 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);
+		if ((divisor3 & 0x7) == 7)
+			divisor3++; /* round x.7/8 up to x+1 */
+
+		divisor = divisor3 >> 3;
+	    divisor3 &= 0x7;
+		if (divisor3 == 1)
+			divisor |= 0xc000;
+		else if (divisor3 >= 4)
+			divisor |= 0x4000;
+		else if (divisor3 != 0)
+			divisor |= 0x8000;
+		else if (divisor == 1)
+			divisor = 0;    /* special case for maximum baud rate */
+	} else {
+		if (type == USBHFTDI_TYPE_B) {
+			divisor = ((48000000UL / 2) + baud / 2) / baud;
+			uinfof("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);
+		}
+		divisor = (divisor >> 3) | (divfrac[divisor & 0x7] << 14);
+
+		/* Deal with special cases for highest baud rates. */
+		if (divisor == 1)
+			divisor = 0;
+		else if (divisor == 0x4001)
+			divisor = 1;
+
+		if (type == USBHFTDI_TYPE_H)
+			divisor |= 0x00020000;
+	}
+	return divisor;
+}
+
+static usbh_urbstatus_t _set_baudrate(USBHFTDIPortDriver *ftdipp, uint32_t baudrate) {
+	uint32_t divisor = _get_divisor(baudrate, ftdipp->ftdip->type);
+	uint16_t wValue = (uint16_t)divisor;
+	uint16_t wIndex = (uint16_t)(divisor >> 16);
+	if (ftdipp->ftdip->dev->basicConfigDesc.bNumInterfaces > 1)
+		wIndex = (wIndex << 8) | (ftdipp->ifnum + 1);
+
+	const USBH_DEFINE_BUFFER(usbh_control_request_t, req) = {
+		USBH_REQTYPE_VENDOR | USBH_REQTYPE_OUT | USBH_REQTYPE_DEVICE,
+		FTDI_COMMAND_SETBAUD,
+		wValue,
+		wIndex,
+		0
+	};
+	return usbhControlRequestExtended(ftdipp->ftdip->dev, &req, NULL, NULL, MS2ST(1000));
+}
+
+
+static void _submitOutI(USBHFTDIPortDriver *ftdipp, uint32_t len) {
+	udbgf("FTDI: Submit OUT %d", len);
+	ftdipp->oq_urb.requestedLength = len;
+	usbhURBObjectResetI(&ftdipp->oq_urb);
+	usbhURBSubmitI(&ftdipp->oq_urb);
+}
+
+static void _out_cb(usbh_urb_t *urb) {
+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		ftdipp->oq_ptr = ftdipp->oq_buff;
+		ftdipp->oq_counter = 64;
+		chThdDequeueNextI(&ftdipp->oq_waiting, Q_OK);
+		return;
+	case USBH_URBSTATUS_DISCONNECTED:
+		uwarn("FTDI: URB OUT disconnected");
+		chThdDequeueNextI(&ftdipp->oq_waiting, Q_RESET);
+		return;
+	default:
+		uerrf("FTDI: URB OUT status unexpected = %d", urb->status);
+		break;
+	}
+	usbhURBObjectResetI(&ftdipp->oq_urb);
+	usbhURBSubmitI(&ftdipp->oq_urb);
+}
+
+static size_t _write_timeout(USBHFTDIPortDriver *ftdipp, const uint8_t *bp,
+		size_t n, systime_t timeout) {
+	chDbgCheck(n > 0U);
+
+	size_t w = 0;
+	chSysLock();
+	while (true) {
+		if (ftdipp->state != USBHFTDIP_STATE_READY) {
+			chSysUnlock();
+			return w;
+		}
+		while (usbhURBIsBusy(&ftdipp->oq_urb)) {
+			if (chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout) != Q_OK) {
+				chSysUnlock();
+				return w;
+			}
+		}
+
+		*ftdipp->oq_ptr++ = *bp++;
+		if (--ftdipp->oq_counter == 0) {
+			_submitOutI(ftdipp, 64);
+			chSchRescheduleS();
+		}
+		chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
+
+		w++;
+		if (--n == 0U)
+			return w;
+
+		chSysLock();
+	}
+}
+
+static msg_t _put_timeout(USBHFTDIPortDriver *ftdipp, uint8_t b, systime_t timeout) {
+
+	chSysLock();
+	if (ftdipp->state != USBHFTDIP_STATE_READY) {
+		chSysUnlock();
+		return Q_RESET;
+	}
+
+	while (usbhURBIsBusy(&ftdipp->oq_urb)) {
+		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->oq_waiting, timeout);
+		if (msg < Q_OK) {
+			chSysUnlock();
+			return msg;
+		}
+	}
+
+	*ftdipp->oq_ptr++ = b;
+	if (--ftdipp->oq_counter == 0) {
+		_submitOutI(ftdipp, 64);
+		chSchRescheduleS();
+	}
+	chSysUnlock();
+	return Q_OK;
+}
+
+static size_t _write(USBHFTDIPortDriver *ftdipp, const uint8_t *bp, size_t n) {
+	return _write_timeout(ftdipp, bp, n, TIME_INFINITE);
+}
+
+static msg_t _put(USBHFTDIPortDriver *ftdipp, uint8_t b) {
+	return _put_timeout(ftdipp, b, TIME_INFINITE);
+}
+
+static void _submitInI(USBHFTDIPortDriver *ftdipp) {
+	udbg("FTDI: Submit IN");
+	usbhURBObjectResetI(&ftdipp->iq_urb);
+	usbhURBSubmitI(&ftdipp->iq_urb);
+}
+
+static void _in_cb(usbh_urb_t *urb) {
+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_OK:
+		if (urb->actualLength < 2) {
+			uwarnf("FTDI: URB IN actualLength = %d, < 2", urb->actualLength);
+		} else if (urb->actualLength > 2) {
+			udbgf("FTDI: URB IN data len=%d, status=%02x %02x",
+					urb->actualLength - 2,
+					((uint8_t *)urb->buff)[0],
+					((uint8_t *)urb->buff)[1]);
+			ftdipp->iq_ptr = ftdipp->iq_buff + 2;
+			ftdipp->iq_counter = urb->actualLength - 2;
+			chThdDequeueNextI(&ftdipp->iq_waiting, Q_OK);
+			return;
+		} else {
+			udbgf("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");
+		chThdDequeueNextI(&ftdipp->iq_waiting, Q_RESET);
+		return;
+	default:
+		uerrf("FTDI: URB IN status unexpected = %d", urb->status);
+		break;
+	}
+	_submitInI(ftdipp);
+}
+
+static size_t _read_timeout(USBHFTDIPortDriver *ftdipp, uint8_t *bp,
+		size_t n, systime_t timeout) {
+	size_t r = 0;
+
+	chDbgCheck(n > 0U);
+
+	chSysLock();
+	while (true) {
+		if (ftdipp->state != USBHFTDIP_STATE_READY) {
+			chSysUnlock();
+			return r;
+		}
+		while (ftdipp->iq_counter == 0) {
+			if (!usbhURBIsBusy(&ftdipp->iq_urb))
+				_submitInI(ftdipp);
+			if (chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout) != Q_OK) {
+				chSysUnlock();
+				return r;
+			}
+		}
+		*bp++ = *ftdipp->iq_ptr++;
+		if (--ftdipp->iq_counter == 0) {
+			_submitInI(ftdipp);
+			chSchRescheduleS();
+		}
+		chSysUnlock();
+
+		r++;
+		if (--n == 0U)
+			return r;
+
+		chSysLock();
+	}
+}
+
+static msg_t _get_timeout(USBHFTDIPortDriver *ftdipp, systime_t timeout) {
+	uint8_t b;
+
+	chSysLock();
+	if (ftdipp->state != USBHFTDIP_STATE_READY) {
+		chSysUnlock();
+		return Q_RESET;
+	}
+	while (ftdipp->iq_counter == 0) {
+		if (!usbhURBIsBusy(&ftdipp->iq_urb))
+			_submitInI(ftdipp);
+		msg_t msg = chThdEnqueueTimeoutS(&ftdipp->iq_waiting, timeout);
+		if (msg < Q_OK) {
+			chSysUnlock();
+			return msg;
+		}
+	}
+	b = *ftdipp->iq_ptr++;
+	if (--ftdipp->iq_counter == 0) {
+		_submitInI(ftdipp);
+		chSchRescheduleS();
+	}
+	chSysUnlock();
+
+	return (msg_t)b;
+}
+
+static msg_t _get(USBHFTDIPortDriver *ftdipp) {
+	return _get_timeout(ftdipp, TIME_INFINITE);
+}
+
+static size_t _read(USBHFTDIPortDriver *ftdipp, uint8_t *bp, size_t n) {
+	return _read_timeout(ftdipp, bp, n, TIME_INFINITE);
+}
+
+static void _vt(void *p) {
+	USBHFTDIPortDriver *const ftdipp = (USBHFTDIPortDriver *)p;
+	chSysLockFromISR();
+	uint32_t len = ftdipp->oq_ptr - ftdipp->oq_buff;
+	if (len && !usbhURBIsBusy(&ftdipp->oq_urb)) {
+		_submitOutI(ftdipp, len);
+	}
+	if ((ftdipp->iq_counter == 0) && !usbhURBIsBusy(&ftdipp->iq_urb)) {
+		_submitInI(ftdipp);
+	}
+	chVTSetI(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
+	chSysUnlockFromISR();
+}
+
+static const struct FTDIPortDriverVMT async_channel_vmt = {
+	(size_t (*)(void *, const uint8_t *, size_t))_write,
+	(size_t (*)(void *, uint8_t *, size_t))_read,
+	(msg_t (*)(void *, uint8_t))_put,
+	(msg_t (*)(void *))_get,
+	(msg_t (*)(void *, uint8_t, systime_t))_put_timeout,
+	(msg_t (*)(void *, systime_t))_get_timeout,
+	(size_t (*)(void *, const uint8_t *, size_t, systime_t))_write_timeout,
+	(size_t (*)(void *, uint8_t *, size_t, systime_t))_read_timeout
+};
+
+
+static void _stop(USBHFTDIPortDriver *ftdipp) {
+	osalSysLock();
+	chVTResetI(&ftdipp->vt);
+	usbhEPCloseS(&ftdipp->epin);
+	usbhEPCloseS(&ftdipp->epout);
+	chThdDequeueAllI(&ftdipp->iq_waiting, Q_RESET);
+	chThdDequeueAllI(&ftdipp->oq_waiting, Q_RESET);
+	osalOsRescheduleS();
+	ftdipp->state = USBHFTDIP_STATE_ACTIVE;
+	osalSysUnlock();
+}
+
+void usbhftdipStop(USBHFTDIPortDriver *ftdipp) {
+	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)
+			|| (ftdipp->state == USBHFTDIP_STATE_READY));
+
+	if (ftdipp->state == USBHFTDIP_STATE_ACTIVE) {
+		return;
+	}
+
+	osalMutexLock(&ftdipp->ftdip->mtx);
+	_stop(ftdipp);
+	osalMutexUnlock(&ftdipp->ftdip->mtx);
+}
+
+void usbhftdipStart(USBHFTDIPortDriver *ftdipp, const USBHFTDIPortConfig *config) {
+	static const USBHFTDIPortConfig default_config = {
+		HAL_USBHFTDI_DEFAULT_SPEED,
+		HAL_USBHFTDI_DEFAULT_FRAMING,
+		HAL_USBHFTDI_DEFAULT_HANDSHAKE,
+		HAL_USBHFTDI_DEFAULT_XON,
+		HAL_USBHFTDI_DEFAULT_XOFF
+	};
+
+	osalDbgCheck((ftdipp->state == USBHFTDIP_STATE_ACTIVE)
+			|| (ftdipp->state == USBHFTDIP_STATE_READY));
+
+	if (ftdipp->state == USBHFTDIP_STATE_READY)
+		return;
+
+	osalMutexLock(&ftdipp->ftdip->mtx);
+	if (config == NULL)
+		config = &default_config;
+
+	uint16_t wValue = 0;
+	_ftdi_port_control(ftdipp, FTDI_COMMAND_RESET, FTDI_RESET_ALL, 0, 0, NULL);
+	_set_baudrate(ftdipp, config->speed);
+	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETDATA, config->framing, 0, 0, NULL);
+	if (config->handshake & USBHFTDI_HANDSHAKE_XON_XOFF)
+		wValue = (config->xoff_character << 8) | config->xon_character;
+	_ftdi_port_control(ftdipp, FTDI_COMMAND_SETFLOW, wValue, config->handshake, 0, NULL);
+
+	usbhURBObjectInit(&ftdipp->oq_urb, &ftdipp->epout, _out_cb, ftdipp, ftdipp->oq_buff, 0);
+	chThdQueueObjectInit(&ftdipp->oq_waiting);
+	ftdipp->oq_counter = 64;
+	ftdipp->oq_ptr = ftdipp->oq_buff;
+	usbhEPOpen(&ftdipp->epout);
+
+	usbhURBObjectInit(&ftdipp->iq_urb, &ftdipp->epin, _in_cb, ftdipp, ftdipp->iq_buff, 64);
+	chThdQueueObjectInit(&ftdipp->iq_waiting);
+	ftdipp->iq_counter = 0;
+	ftdipp->iq_ptr = ftdipp->iq_buff;
+	usbhEPOpen(&ftdipp->epin);
+	osalSysLock();
+	usbhURBSubmitI(&ftdipp->iq_urb);
+	osalSysUnlock();
+
+	chVTObjectInit(&ftdipp->vt);
+	chVTSet(&ftdipp->vt, MS2ST(16), _vt, ftdipp);
+
+	ftdipp->state = USBHFTDIP_STATE_READY;
+	osalMutexUnlock(&ftdipp->ftdip->mtx);
+}
+
+void usbhftdiObjectInit(USBHFTDIDriver *ftdip) {
+	osalDbgCheck(ftdip != NULL);
+	memset(ftdip, 0, sizeof(*ftdip));
+	ftdip->info = &usbhftdiClassDriverInfo;
+	osalMutexObjectInit(&ftdip->mtx);
+}
+
+void usbhftdipObjectInit(USBHFTDIPortDriver *ftdipp) {
+	osalDbgCheck(ftdipp != NULL);
+	memset(ftdipp, 0, sizeof(*ftdipp));
+	ftdipp->vmt = &async_channel_vmt;
+	ftdipp->state = USBHFTDIP_STATE_STOP;
+}
+
+#endif
diff --git a/os/hal/src/usbh/usbh_hub.c b/os/hal/src/usbh/hal_usbh_hub.c
index eb585d2..7fdcef1 100644
--- a/os/hal/src/usbh/usbh_hub.c
+++ b/os/hal/src/usbh/hal_usbh_hub.c
@@ -1,302 +1,302 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-#include "usbh.h"

-#include "usbh/internal.h"

-

-#if HAL_USBH_USE_HUB

-

-#if !HAL_USE_USBH

-#error "USBHHUB needs HAL_USE_USBH"

-#endif

-

-#include <string.h>

-#include "usbh/dev/hub.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

-

-

-USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];

-usbh_port_t USBHPorts[HAL_USBHHUB_MAX_PORTS];

-

-static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

-static void hub_unload(usbh_baseclassdriver_t *drv);

-static const usbh_classdriver_vmt_t usbhhubClassDriverVMT = {

-	hub_load,

-	hub_unload

-};

-const usbh_classdriverinfo_t usbhhubClassDriverInfo = {

-	0x09, 0x00, -1, "HUB", &usbhhubClassDriverVMT

-};

-

-

-void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh,

-		USBHHubDriver *hub, uint8_t number) {

-	memset(port, 0, sizeof(*port));

-	port->number = number;

-	port->device.host = usbh;

-	port->hub = hub;

-}

-

-usbh_urbstatus_t usbhhubControlRequest(USBHDriver *host, USBHHubDriver *hub,

-											uint8_t bmRequestType,

-											uint8_t bRequest,

-											uint16_t wValue,

-											uint16_t wIndex,

-											uint16_t wLength,

-											uint8_t *buf) {

-	if (hub == NULL)

-		return usbh_lld_root_hub_request(host, bmRequestType, bRequest, wValue, wIndex, wLength, buf);

-

-	return usbhControlRequest(hub->dev,

-			bmRequestType, bRequest, wValue, wIndex, wLength, buf);

-}

-

-

-static void _urb_complete(usbh_urb_t *urb) {

-

-	USBHHubDriver *const hubdp = (USBHHubDriver *)urb->userData;

-	switch (urb->status) {

-	case USBH_URBSTATUS_TIMEOUT:

-		/* the device NAKed */

-		udbg("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);

-		}

-

-		if (urb->actualLength < len)

-			len = urb->actualLength;

-

-		if (len > 4)

-			len = 4;

-

-		uint8_t *sc = (uint8_t *)&hubdp->statuschange;

-		uint8_t *r = hubdp->scbuff;

-		while (len--)

-			*sc++ |= *r++;

-

-		uinfof("HUB: change, %08x", hubdp->statuschange);

-	}	break;

-	case USBH_URBSTATUS_DISCONNECTED:

-		uwarn("HUB: URB disconnected, aborting poll");

-		return;

-	default:

-		uerrf("HUB: URB status unexpected = %d", urb->status);

-		break;

-	}

-

-	usbhURBObjectResetI(urb);

-	usbhURBSubmitI(urb);

-}

-

-static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,

-		const uint8_t *descriptor, uint16_t rem) {

-	int i;

-

-	USBHHubDriver *hubdp;

-

-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_DEVICE))

-		return NULL;

-

-	if (dev->devDesc.bDeviceProtocol != 0)

-		return NULL;

-

-	generic_iterator_t iep, icfg;

-	if_iterator_t iif;

-

-	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,

-			dev->basicConfigDesc.wTotalLength);

-

-	if_iter_init(&iif, &icfg);

-	if (!iif.valid)

-		return NULL;

-	const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);

-	if ((ifdesc->bInterfaceClass != 0x09)

-		|| (ifdesc->bInterfaceSubClass != 0x00)

-		|| (ifdesc->bInterfaceProtocol != 0x00)) {

-		return NULL;

-	}

-

-	ep_iter_init(&iep, &iif);

-	if (!iep.valid)

-		return NULL;

-	const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

-	if ((epdesc->bmAttributes & 0x03) != USBH_EPTYPE_INT) {

-		return NULL;

-	}

-

-

-	/* alloc driver */

-	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {

-		if (USBHHUBD[i].dev == NULL) {

-			hubdp = &USBHHUBD[i];

-			goto alloc_ok;

-		}

-	}

-

-	uwarn("Can't alloc HUB driver");

-

-	/* can't alloc */

-	return NULL;

-

-alloc_ok:

-	/* initialize the driver's variables */

-	hubdp->epint.status = USBH_EPSTATUS_UNINITIALIZED;

-	hubdp->dev = dev;

-	hubdp->ports = 0;

-

-	usbhEPSetName(&dev->ctrl, "HUB[CTRL]");

-

-	/* read Hub descriptor */

-	uinfo("Read Hub descriptor");

-	if (usbhhubControlRequest(dev->host, hubdp,

-			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,

-			USBH_REQ_GET_DESCRIPTOR,

-			(USBH_DT_HUB << 8), 0, sizeof(hubdp->hubDesc),

-			(uint8_t *)&hubdp->hubDesc) != USBH_URBSTATUS_OK) {

-		hubdp->dev = NULL;

-		return NULL;

-	}

-

-	const usbh_hub_descriptor_t *const hubdesc = &hubdp->hubDesc;

-

-	uinfof("Hub descriptor loaded; %d ports, wHubCharacteristics=%04x, bPwrOn2PwrGood=%d, bHubContrCurrent=%d",

-			hubdesc->bNbrPorts,

-			hubdesc->wHubCharacteristics,

-			hubdesc->bPwrOn2PwrGood,

-			hubdesc->bHubContrCurrent);

-

-	/* Alloc ports */

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

-			_usbhub_port_object_init(&USBHPorts[i], dev->host, hubdp, ports);

-			USBHPorts[i].next = hubdp->ports;

-			hubdp->ports = &USBHPorts[i];

-			--ports;

-		}

-	}

-

-	if (ports) {

-		uwarn("Could not alloc all ports");

-	}

-

-	/* link hub to the host's list */

-	list_add_tail(&hubdp->node, &dev->host->hubs);

-

-	/* enable power to ports */

-	usbh_port_t *port = hubdp->ports;

-	while (port) {

-		uinfof("Enable power for port %d", port->number);

-		usbhhubSetFeaturePort(port, USBH_PORT_FEAT_POWER);

-		port = port->next;

-	}

-

-	if (hubdesc->bPwrOn2PwrGood)

-		osalThreadSleepMilliseconds(2 * hubdesc->bPwrOn2PwrGood);

-

-	/* initialize the status change endpoint and trigger the first transfer */

-	usbhEPObjectInit(&hubdp->epint, dev, epdesc);

-	usbhEPSetName(&hubdp->epint, "HUB[INT ]");

-	usbhEPOpen(&hubdp->epint);

-

-	usbhURBObjectInit(&hubdp->urb, &hubdp->epint,

-			_urb_complete, hubdp, hubdp->scbuff,

-			(hubdesc->bNbrPorts + 8) / 8);

-

-	osalSysLock();

-	usbhURBSubmitI(&hubdp->urb);

-	osalOsRescheduleS();

-	osalSysUnlock();

-

-	return (usbh_baseclassdriver_t *)hubdp;

-}

-

-static void hub_unload(usbh_baseclassdriver_t *drv) {

-	osalDbgCheck(drv != NULL);

-	USBHHubDriver *const hubdp = (USBHHubDriver *)drv;

-

-	/* close the status change endpoint (this cancels ongoing URBs) */

-	osalSysLock();

-	usbhEPCloseS(&hubdp->epint);

-	osalSysUnlock();

-

-	/* de-alloc ports and unload drivers */

-	usbh_port_t *port = hubdp->ports;

-	while (port) {

-		_usbh_port_disconnected(port);

-		port->hub = NULL;

-		port = port->next;

-	}

-

-	/* unlink the hub from the host's list */

-	list_del(&hubdp->node);

-

-}

-

-void usbhhubObjectInit(USBHHubDriver *hubdp) {

-	osalDbgCheck(hubdp != NULL);

-	memset(hubdp, 0, sizeof(*hubdp));

-	hubdp->info = &usbhhubClassDriverInfo;

-}

-#else

-

-#if HAL_USE_USBH

-void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh, uint8_t number) {

-	memset(port, 0, sizeof(*port));

-	port->number = number;

-	port->device.host = usbh;

-}

-#endif

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+#include "hal_usbh.h"
+#include "usbh/internal.h"
+
+#if HAL_USBH_USE_HUB
+
+#if !HAL_USE_USBH
+#error "USBHHUB needs HAL_USE_USBH"
+#endif
+
+#include <string.h>
+#include "usbh/dev/hub.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
+
+
+USBHHubDriver USBHHUBD[HAL_USBHHUB_MAX_INSTANCES];
+usbh_port_t USBHPorts[HAL_USBHHUB_MAX_PORTS];
+
+static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void hub_unload(usbh_baseclassdriver_t *drv);
+static const usbh_classdriver_vmt_t usbhhubClassDriverVMT = {
+	hub_load,
+	hub_unload
+};
+const usbh_classdriverinfo_t usbhhubClassDriverInfo = {
+	0x09, 0x00, -1, "HUB", &usbhhubClassDriverVMT
+};
+
+
+void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh,
+		USBHHubDriver *hub, uint8_t number) {
+	memset(port, 0, sizeof(*port));
+	port->number = number;
+	port->device.host = usbh;
+	port->hub = hub;
+}
+
+usbh_urbstatus_t usbhhubControlRequest(USBHDriver *host, USBHHubDriver *hub,
+											uint8_t bmRequestType,
+											uint8_t bRequest,
+											uint16_t wValue,
+											uint16_t wIndex,
+											uint16_t wLength,
+											uint8_t *buf) {
+	if (hub == NULL)
+		return usbh_lld_root_hub_request(host, bmRequestType, bRequest, wValue, wIndex, wLength, buf);
+
+	return usbhControlRequest(hub->dev,
+			bmRequestType, bRequest, wValue, wIndex, wLength, buf);
+}
+
+
+static void _urb_complete(usbh_urb_t *urb) {
+
+	USBHHubDriver *const hubdp = (USBHHubDriver *)urb->userData;
+	switch (urb->status) {
+	case USBH_URBSTATUS_TIMEOUT:
+		/* the device NAKed */
+		udbg("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);
+		}
+
+		if (urb->actualLength < len)
+			len = urb->actualLength;
+
+		if (len > 4)
+			len = 4;
+
+		uint8_t *sc = (uint8_t *)&hubdp->statuschange;
+		uint8_t *r = hubdp->scbuff;
+		while (len--)
+			*sc++ |= *r++;
+
+		uinfof("HUB: change, %08x", hubdp->statuschange);
+	}	break;
+	case USBH_URBSTATUS_DISCONNECTED:
+		uwarn("HUB: URB disconnected, aborting poll");
+		return;
+	default:
+		uerrf("HUB: URB status unexpected = %d", urb->status);
+		break;
+	}
+
+	usbhURBObjectResetI(urb);
+	usbhURBSubmitI(urb);
+}
+
+static usbh_baseclassdriver_t *hub_load(usbh_device_t *dev,
+		const uint8_t *descriptor, uint16_t rem) {
+	int i;
+
+	USBHHubDriver *hubdp;
+
+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_DEVICE))
+		return NULL;
+
+	if (dev->devDesc.bDeviceProtocol != 0)
+		return NULL;
+
+	generic_iterator_t iep, icfg;
+	if_iterator_t iif;
+
+	cfg_iter_init(&icfg, dev->fullConfigurationDescriptor,
+			dev->basicConfigDesc.wTotalLength);
+
+	if_iter_init(&iif, &icfg);
+	if (!iif.valid)
+		return NULL;
+	const usbh_interface_descriptor_t *const ifdesc = if_get(&iif);
+	if ((ifdesc->bInterfaceClass != 0x09)
+		|| (ifdesc->bInterfaceSubClass != 0x00)
+		|| (ifdesc->bInterfaceProtocol != 0x00)) {
+		return NULL;
+	}
+
+	ep_iter_init(&iep, &iif);
+	if (!iep.valid)
+		return NULL;
+	const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+	if ((epdesc->bmAttributes & 0x03) != USBH_EPTYPE_INT) {
+		return NULL;
+	}
+
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHHUB_MAX_INSTANCES; i++) {
+		if (USBHHUBD[i].dev == NULL) {
+			hubdp = &USBHHUBD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("Can't alloc HUB driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	hubdp->epint.status = USBH_EPSTATUS_UNINITIALIZED;
+	hubdp->dev = dev;
+	hubdp->ports = 0;
+
+	usbhEPSetName(&dev->ctrl, "HUB[CTRL]");
+
+	/* read Hub descriptor */
+	uinfo("Read Hub descriptor");
+	if (usbhhubControlRequest(dev->host, hubdp,
+			USBH_REQTYPE_IN | USBH_REQTYPE_CLASS | USBH_REQTYPE_DEVICE,
+			USBH_REQ_GET_DESCRIPTOR,
+			(USBH_DT_HUB << 8), 0, sizeof(hubdp->hubDesc),
+			(uint8_t *)&hubdp->hubDesc) != USBH_URBSTATUS_OK) {
+		hubdp->dev = NULL;
+		return NULL;
+	}
+
+	const usbh_hub_descriptor_t *const hubdesc = &hubdp->hubDesc;
+
+	uinfof("Hub descriptor loaded; %d ports, wHubCharacteristics=%04x, bPwrOn2PwrGood=%d, bHubContrCurrent=%d",
+			hubdesc->bNbrPorts,
+			hubdesc->wHubCharacteristics,
+			hubdesc->bPwrOn2PwrGood,
+			hubdesc->bHubContrCurrent);
+
+	/* Alloc ports */
+	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);
+			_usbhub_port_object_init(&USBHPorts[i], dev->host, hubdp, ports);
+			USBHPorts[i].next = hubdp->ports;
+			hubdp->ports = &USBHPorts[i];
+			--ports;
+		}
+	}
+
+	if (ports) {
+		uwarn("Could not alloc all ports");
+	}
+
+	/* link hub to the host's list */
+	list_add_tail(&hubdp->node, &dev->host->hubs);
+
+	/* enable power to ports */
+	usbh_port_t *port = hubdp->ports;
+	while (port) {
+		uinfof("Enable power for port %d", port->number);
+		usbhhubSetFeaturePort(port, USBH_PORT_FEAT_POWER);
+		port = port->next;
+	}
+
+	if (hubdesc->bPwrOn2PwrGood)
+		osalThreadSleepMilliseconds(2 * hubdesc->bPwrOn2PwrGood);
+
+	/* initialize the status change endpoint and trigger the first transfer */
+	usbhEPObjectInit(&hubdp->epint, dev, epdesc);
+	usbhEPSetName(&hubdp->epint, "HUB[INT ]");
+	usbhEPOpen(&hubdp->epint);
+
+	usbhURBObjectInit(&hubdp->urb, &hubdp->epint,
+			_urb_complete, hubdp, hubdp->scbuff,
+			(hubdesc->bNbrPorts + 8) / 8);
+
+	osalSysLock();
+	usbhURBSubmitI(&hubdp->urb);
+	osalOsRescheduleS();
+	osalSysUnlock();
+
+	return (usbh_baseclassdriver_t *)hubdp;
+}
+
+static void hub_unload(usbh_baseclassdriver_t *drv) {
+	osalDbgCheck(drv != NULL);
+	USBHHubDriver *const hubdp = (USBHHubDriver *)drv;
+
+	/* close the status change endpoint (this cancels ongoing URBs) */
+	osalSysLock();
+	usbhEPCloseS(&hubdp->epint);
+	osalSysUnlock();
+
+	/* de-alloc ports and unload drivers */
+	usbh_port_t *port = hubdp->ports;
+	while (port) {
+		_usbh_port_disconnected(port);
+		port->hub = NULL;
+		port = port->next;
+	}
+
+	/* unlink the hub from the host's list */
+	list_del(&hubdp->node);
+
+}
+
+void usbhhubObjectInit(USBHHubDriver *hubdp) {
+	osalDbgCheck(hubdp != NULL);
+	memset(hubdp, 0, sizeof(*hubdp));
+	hubdp->info = &usbhhubClassDriverInfo;
+}
+#else
+
+#if HAL_USE_USBH
+void _usbhub_port_object_init(usbh_port_t *port, USBHDriver *usbh, uint8_t number) {
+	memset(port, 0, sizeof(*port));
+	port->number = number;
+	port->device.host = usbh;
+}
+#endif
+
+#endif
diff --git a/os/hal/src/usbh/usbh_msd.c b/os/hal/src/usbh/hal_usbh_msd.c
index 8db68a4..6869a74 100644
--- a/os/hal/src/usbh/usbh_msd.c
+++ b/os/hal/src/usbh/hal_usbh_msd.c
@@ -1,939 +1,939 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-#include "usbh.h"

-

-#if HAL_USBH_USE_MSD

-

-#if !HAL_USE_USBH

-#error "USBHMSD needs USBH"

-#endif

-

-#include <string.h>

-#include "usbh/dev/msd.h"

-#include "usbh/internal.h"

-

-//#pragma GCC optimize("Og")

-

-

-#if USBHMSD_DEBUG_ENABLE_TRACE

-#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)

-#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)

-#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

-

-

-

-

-

-/*===========================================================================*/

-/* USB Class driver loader for MSD								 		 	 */

-/*===========================================================================*/

-

-USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];

-

-static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);

-static void _msd_unload(usbh_baseclassdriver_t *drv);

-

-static const usbh_classdriver_vmt_t class_driver_vmt = {

-	_msd_load,

-	_msd_unload

-};

-

-const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {

-	0x08, 0x06, 0x50, "MSD", &class_driver_vmt

-};

-

-#define MSD_REQ_RESET							0xFF

-#define MSD_GET_MAX_LUN							0xFE

-

-static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

-	int i;

-	USBHMassStorageDriver *msdp;

-	uint8_t luns; // should declare it here to eliminate 'control bypass initialization' warning

-	usbh_urbstatus_t stat;  // should declare it here to eliminate 'control bypass initialization' warning

-

-	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))

-		return NULL;

-

-	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;

-

-	if ((ifdesc->bAlternateSetting != 0)

-			|| (ifdesc->bNumEndpoints < 2)

-			|| (ifdesc->bInterfaceSubClass != 0x06)

-			|| (ifdesc->bInterfaceProtocol != 0x50)) {

-		return NULL;

-	}

-

-	/* alloc driver */

-	for (i = 0; i < HAL_USBHMSD_MAX_INSTANCES; i++) {

-		if (USBHMSD[i].dev == NULL) {

-			msdp = &USBHMSD[i];

-			goto alloc_ok;

-		}

-	}

-

-	uwarn("Can't alloc MSD driver");

-

-	/* can't alloc */

-	return NULL;

-

-alloc_ok:

-	/* initialize the driver's variables */

-	msdp->epin.status = USBH_EPSTATUS_UNINITIALIZED;

-	msdp->epout.status = USBH_EPSTATUS_UNINITIALIZED;

-	msdp->max_lun = 0;

-	msdp->tag = 0;

-	msdp->luns = 0;

-	msdp->ifnum = ifdesc->bInterfaceNumber;

-	usbhEPSetName(&dev->ctrl, "MSD[CTRL]");

-

-	/* parse the configuration descriptor */

-	if_iterator_t iif;

-	generic_iterator_t iep;

-	iif.iad = 0;

-	iif.curr = descriptor;

-	iif.rem = rem;

-	for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {

-		const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);

-		if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {

-			uinfof("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);

-			usbhEPObjectInit(&msdp->epout, dev, epdesc);

-			usbhEPSetName(&msdp->epout, "MSD[BOUT]");

-		} else {

-			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",

-					epdesc->bEndpointAddress, epdesc->bmAttributes);

-		}

-	}

-	if ((msdp->epin.status != USBH_EPSTATUS_CLOSED) || (msdp->epout.status != USBH_EPSTATUS_CLOSED)) {

-		goto deinit;

-	}

-

-	/* read the number of LUNs */

-	uinfo("Reading Max LUN:");

-	USBH_DEFINE_BUFFER(uint8_t, buff[4]);

-	stat = usbhControlRequest(dev,

-			USBH_CLASSIN(USBH_REQTYPE_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);

-		if (msdp->max_lun > HAL_USBHMSD_MAX_LUNS) {

-			msdp->max_lun = HAL_USBHMSD_MAX_LUNS;

-			uwarnf("\tUsing max_lun = %d", msdp->max_lun);

-		}

-	} else if (stat == USBH_URBSTATUS_STALL) {

-		uwarn("\tStall, max_lun = 1");

-		msdp->max_lun = 1;

-	} else {

-		uerr("\tError");

-		goto deinit;

-	}

-

-	/* open the bulk IN/OUT endpoints */

-	usbhEPOpen(&msdp->epin);

-	usbhEPOpen(&msdp->epout);

-

-	/* Alloc one block device per logical unit found */

-	luns = msdp->max_lun;

-	for (i = 0; (luns > 0) && (i < HAL_USBHMSD_MAX_LUNS); i++) {

-		if (MSBLKD[i].msdp == NULL) {

-			/* link the new block driver to the list */

-			MSBLKD[i].next = msdp->luns;

-			msdp->luns = &MSBLKD[i];

-			MSBLKD[i].msdp = msdp;

-

-			osalSysLock();

-			MSBLKD[i].state = BLK_ACTIVE;	/* transition directly to active, instead of BLK_STOP */

-			osalSysUnlock();

-

-			/* connect the LUN (TODO: review if it's best to leave the LUN disconnected) */

-			usbhmsdLUNConnect(&MSBLKD[i]);

-			luns--;

-		}

-	}

-

-	return (usbh_baseclassdriver_t *)msdp;

-

-deinit:

-	/* Here, the enpoints are closed, and the driver is unlinked */

-	return NULL;

-}

-

-static void _msd_unload(usbh_baseclassdriver_t *drv) {

-	osalDbgCheck(drv != NULL);

-	USBHMassStorageDriver *const msdp = (USBHMassStorageDriver *)drv;

-	USBHMassStorageLUNDriver *lunp = msdp->luns;

-

-	osalMutexLock(&msdp->mtx);

-	osalSysLock();

-	usbhEPCloseS(&msdp->epin);

-	usbhEPCloseS(&msdp->epout);

-	while (lunp) {

-		lunp->state = BLK_STOP;

-		lunp = lunp->next;

-	}

-	osalSysUnlock();

-	osalMutexUnlock(&msdp->mtx);

-

-	/* now that the LUNs are idle, deinit them */

-	lunp = msdp->luns;

-	osalSysLock();

-	while (lunp) {

-		usbhmsdLUNObjectInit(lunp);

-		lunp = lunp->next;

-	}

-	osalSysUnlock();

-}

-

-

-/*===========================================================================*/

-/* MSD Class driver operations (Bulk-Only transport)			 		 	 */

-/*===========================================================================*/

-

-

-

-/* USB Bulk Only Transport SCSI Command block wrapper */

-PACKED_STRUCT {

-	uint32_t dCBWSignature;

-	uint32_t dCBWTag;

-	uint32_t dCBWDataTransferLength;

-	uint8_t bmCBWFlags;

-	uint8_t bCBWLUN;

-	uint8_t bCBWCBLength;

-	uint8_t CBWCB[16];

-} msd_cbw_t;

-#define MSD_CBW_SIGNATURE						0x43425355

-#define MSD_CBWFLAGS_D2H						0x80

-#define MSD_CBWFLAGS_H2D						0x00

-

-

-/* USB Bulk Only Transport SCSI Command status wrapper */

-PACKED_STRUCT {

-	uint32_t dCSWSignature;

-	uint32_t dCSWTag;

-	uint32_t dCSWDataResidue;

-	uint8_t bCSWStatus;

-} msd_csw_t;

-#define MSD_CSW_SIGNATURE						0x53425355

-

-

-typedef union {

-	msd_cbw_t cbw;

-	msd_csw_t csw;

-} msd_transaction_t;

-

-typedef enum {

-	MSD_TRANSACTIONRESULT_OK,

-	MSD_TRANSACTIONRESULT_DISCONNECTED,

-	MSD_TRANSACTIONRESULT_STALL,

-	MSD_TRANSACTIONRESULT_BUS_ERROR,

-	MSD_TRANSACTIONRESULT_SYNC_ERROR

-} msd_transaction_result_t;

-

-typedef enum {

-	MSD_COMMANDRESULT_PASSED = 0,

-	MSD_COMMANDRESULT_FAILED = 1,

-	MSD_COMMANDRESULT_PHASE_ERROR = 2

-} msd_command_result_t;

-

-typedef struct {

-	msd_transaction_result_t tres;

-	msd_command_result_t cres;

-} msd_result_t;

-

-

-/* ----------------------------------------------------- */

-/* 			SCSI Commands 								*/

-/* ----------------------------------------------------- */

-

-/* Read 10 and Write 10 */

-#define SCSI_CMD_READ_10 						0x28

-#define SCSI_CMD_WRITE_10						0x2A

-

-/* Request sense */

-#define SCSI_CMD_REQUEST_SENSE 					0x03

-PACKED_STRUCT {

-	uint8_t byte[18];

-} scsi_sense_response_t;

-

-#define SCSI_SENSE_KEY_GOOD                     0x00

-#define SCSI_SENSE_KEY_RECOVERED_ERROR          0x01

-#define SCSI_SENSE_KEY_NOT_READY                0x02

-#define SCSI_SENSE_KEY_MEDIUM_ERROR             0x03

-#define SCSI_SENSE_KEY_HARDWARE_ERROR           0x04

-#define SCSI_SENSE_KEY_ILLEGAL_REQUEST          0x05

-#define SCSI_SENSE_KEY_UNIT_ATTENTION           0x06

-#define SCSI_SENSE_KEY_DATA_PROTECT             0x07

-#define SCSI_SENSE_KEY_BLANK_CHECK              0x08

-#define SCSI_SENSE_KEY_VENDOR_SPECIFIC          0x09

-#define SCSI_SENSE_KEY_COPY_ABORTED             0x0A

-#define SCSI_SENSE_KEY_ABORTED_COMMAND          0x0B

-#define SCSI_SENSE_KEY_VOLUME_OVERFLOW          0x0D

-#define SCSI_SENSE_KEY_MISCOMPARE               0x0E

-#define SCSI_ASENSE_NO_ADDITIONAL_INFORMATION   0x00

-#define SCSI_ASENSE_LOGICAL_UNIT_NOT_READY      0x04

-#define SCSI_ASENSE_INVALID_FIELD_IN_CDB        0x24

-#define SCSI_ASENSE_NOT_READY_TO_READY_CHANGE   0x28

-#define SCSI_ASENSE_WRITE_PROTECTED             0x27

-#define SCSI_ASENSE_FORMAT_ERROR                0x31

-#define SCSI_ASENSE_INVALID_COMMAND             0x20

-#define SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE 0x21

-#define SCSI_ASENSE_MEDIUM_NOT_PRESENT                 0x3A

-#define SCSI_ASENSEQ_NO_QUALIFIER                      0x00

-#define SCSI_ASENSEQ_FORMAT_COMMAND_FAILED             0x01

-#define SCSI_ASENSEQ_INITIALIZING_COMMAND_REQUIRED     0x02

-#define SCSI_ASENSEQ_OPERATION_IN_PROGRESS             0x07

-

-/* Inquiry */

-#define SCSI_CMD_INQUIRY 						0x12

-PACKED_STRUCT {

-	uint8_t peripheral;

-	uint8_t removable;

-	uint8_t version;

-	uint8_t response_data_format;

-	uint8_t additional_length;

-	uint8_t sccstp;

-	uint8_t bqueetc;

-	uint8_t cmdque;

-	uint8_t vendorID[8];

-	uint8_t productID[16];

-	uint8_t productRev[4];

-} scsi_inquiry_response_t;

-

-/* Read Capacity 10 */

-#define SCSI_CMD_READ_CAPACITY_10				0x25

-PACKED_STRUCT {

-	uint32_t last_block_addr;

-	uint32_t block_size;

-} scsi_readcapacity10_response_t;

-

-/* Start/Stop Unit */

-#define SCSI_CMD_START_STOP_UNIT				0x1B

-PACKED_STRUCT {

-	uint8_t op_code;

-	uint8_t lun_immed;

-	uint8_t res1;

-	uint8_t res2;

-	uint8_t loej_start;

-	uint8_t control;

-} scsi_startstopunit_request_t;

-

-/* test unit ready */

-#define SCSI_CMD_TEST_UNIT_READY				0x00

-

-/* Other commands, TODO: use or remove them

-#define SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL	0x1E

-#define SCSI_CMD_VERIFY_10						0x2F

-#define SCSI_CMD_SEND_DIAGNOSTIC				0x1D

-#define SCSI_CMD_MODE_SENSE_6                   0x1A

-*/

-

-static inline void _prepare_cbw(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp) {

-	tran->cbw.bCBWLUN = (uint8_t)(lunp - &lunp->msdp->luns[0]);

-	memset(&tran->cbw.CBWCB, 0, sizeof(tran->cbw.CBWCB));

-}

-

-static msd_transaction_result_t _msd_transaction(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp, void *data) {

-

-	uint32_t actual_len;

-	usbh_urbstatus_t status;

-

-	tran->cbw.dCBWSignature = MSD_CBW_SIGNATURE;

-	tran->cbw.dCBWTag = ++lunp->msdp->tag;

-

-	/* control phase */

-	status = usbhBulkTransfer(&lunp->msdp->epout, &tran->cbw,

-					sizeof(tran->cbw), &actual_len, MS2ST(1000));

-

-	if (status == USBH_URBSTATUS_CANCELLED) {

-		uerr("\tMSD: Control phase: USBH_URBSTATUS_CANCELLED");

-		return MSD_TRANSACTIONRESULT_DISCONNECTED;

-	} else if (status == USBH_URBSTATUS_STALL) {

-		uerr("\tMSD: Control phase: USBH_URBSTATUS_STALL");

-		return MSD_TRANSACTIONRESULT_STALL;

-	} else if (status != USBH_URBSTATUS_OK) {

-		uerrf("\tMSD: Control phase: status = %d, != OK", status);

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	} else if (actual_len != sizeof(tran->cbw)) {

-		uerrf("\tMSD: Control phase: wrong actual_len = %d", actual_len);

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	}

-

-

-	/* data phase */

-	if (tran->cbw.dCBWDataTransferLength) {

-		status = usbhBulkTransfer(

-				tran->cbw.bmCBWFlags & MSD_CBWFLAGS_D2H ? &lunp->msdp->epin : &lunp->msdp->epout,

-				data,

-				tran->cbw.dCBWDataTransferLength,

-				&actual_len, MS2ST(20000));

-

-		if (status == USBH_URBSTATUS_CANCELLED) {

-			uerr("\tMSD: Data phase: USBH_URBSTATUS_CANCELLED");

-			return MSD_TRANSACTIONRESULT_DISCONNECTED;

-		} else if (status == USBH_URBSTATUS_STALL) {

-			uerr("\tMSD: Data phase: USBH_URBSTATUS_STALL");

-			return MSD_TRANSACTIONRESULT_STALL;

-		} else if (status != USBH_URBSTATUS_OK) {

-			uerrf("\tMSD: Data phase: status = %d, != OK", status);

-			return MSD_TRANSACTIONRESULT_BUS_ERROR;

-		} else if (actual_len != tran->cbw.dCBWDataTransferLength) {

-			uerrf("\tMSD: Data phase: wrong actual_len = %d", actual_len);

-			return MSD_TRANSACTIONRESULT_BUS_ERROR;

-		}

-	}

-

-

-	/* status phase */

-	status = usbhBulkTransfer(&lunp->msdp->epin, &tran->csw,

-			sizeof(tran->csw), &actual_len, MS2ST(1000));

-

-	if (status == USBH_URBSTATUS_CANCELLED) {

-		uerr("\tMSD: Status phase: USBH_URBSTATUS_CANCELLED");

-		return MSD_TRANSACTIONRESULT_DISCONNECTED;

-	} else if (status == USBH_URBSTATUS_STALL) {

-		uerr("\tMSD: Status phase: USBH_URBSTATUS_STALL");

-		return MSD_TRANSACTIONRESULT_STALL;

-	} else if (status != USBH_URBSTATUS_OK) {

-		uerrf("\tMSD: Status phase: status = %d, != OK", status);

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	} else if (actual_len != sizeof(tran->csw)) {

-		uerrf("\tMSD: Status phase: wrong actual_len = %d", actual_len);

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	} else if (tran->csw.dCSWSignature != MSD_CSW_SIGNATURE) {

-		uerr("\tMSD: Status phase: wrong signature");

-		return MSD_TRANSACTIONRESULT_BUS_ERROR;

-	} else if (tran->csw.dCSWTag != lunp->msdp->tag) {

-		uerrf("\tMSD: Status phase: wrong tag (expected %d, got %d)",

-				lunp->msdp->tag, tran->csw.dCSWTag);

-		return MSD_TRANSACTIONRESULT_SYNC_ERROR;

-	}

-

-	if (tran->csw.dCSWDataResidue) {

-		uwarnf("\tMSD: Residue=%d", tran->csw.dCSWDataResidue);

-	}

-

-	return MSD_TRANSACTIONRESULT_OK;

-}

-

-

-static msd_result_t scsi_inquiry(USBHMassStorageLUNDriver *lunp, scsi_inquiry_response_t *resp) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_inquiry_response_t);

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 6;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_INQUIRY;

-	transaction.cbw.CBWCB[4] = sizeof(scsi_inquiry_response_t);

-

-	res.tres = _msd_transaction(&transaction, lunp, resp);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-static msd_result_t scsi_requestsense(USBHMassStorageLUNDriver *lunp, scsi_sense_response_t *resp) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_sense_response_t);

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 12;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_REQUEST_SENSE;

-	transaction.cbw.CBWCB[4] = sizeof(scsi_sense_response_t);

-

-	res.tres = _msd_transaction(&transaction, lunp, resp);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-static msd_result_t scsi_testunitready(USBHMassStorageLUNDriver *lunp) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = 0;

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 6;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_TEST_UNIT_READY;

-

-	res.tres = _msd_transaction(&transaction, lunp, NULL);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-static msd_result_t scsi_readcapacity10(USBHMassStorageLUNDriver *lunp, scsi_readcapacity10_response_t *resp) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_readcapacity10_response_t);

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 12;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_CAPACITY_10;

-

-	res.tres = _msd_transaction(&transaction, lunp, resp);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-

-static msd_result_t scsi_read10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, uint8_t *data) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;

-	transaction.cbw.bCBWCBLength = 10;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_10;

-	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);

-	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);

-	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);

-	transaction.cbw.CBWCB[5] = (uint8_t)(lba);

-	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);

-	transaction.cbw.CBWCB[8] = (uint8_t)(n);

-

-	res.tres = _msd_transaction(&transaction, lunp, data);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-static msd_result_t scsi_write10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, const uint8_t *data) {

-	msd_transaction_t transaction;

-	msd_result_t res;

-

-	_prepare_cbw(&transaction, lunp);

-	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;

-	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_H2D;

-	transaction.cbw.bCBWCBLength = 10;

-	transaction.cbw.CBWCB[0] = SCSI_CMD_WRITE_10;

-	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);

-	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);

-	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);

-	transaction.cbw.CBWCB[5] = (uint8_t)(lba);

-	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);

-	transaction.cbw.CBWCB[8] = (uint8_t)(n);

-

-	res.tres = _msd_transaction(&transaction, lunp, (uint8_t *)data);

-	if (res.tres == MSD_TRANSACTIONRESULT_OK) {

-		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;

-	}

-	return res;

-}

-

-

-

-/*===========================================================================*/

-/* Block driver data/functions								 		 	 	 */

-/*===========================================================================*/

-

-USBHMassStorageLUNDriver MSBLKD[HAL_USBHMSD_MAX_LUNS];

-

-static const struct USBHMassStorageDriverVMT blk_vmt = {

-	(bool (*)(void *))usbhmsdLUNIsInserted,

-	(bool (*)(void *))usbhmsdLUNIsProtected,

-	(bool (*)(void *))usbhmsdLUNConnect,

-	(bool (*)(void *))usbhmsdLUNDisconnect,

-	(bool (*)(void *, uint32_t, uint8_t *, uint32_t))usbhmsdLUNRead,

-	(bool (*)(void *, uint32_t,	const uint8_t *, uint32_t))usbhmsdLUNWrite,

-	(bool (*)(void *))usbhmsdLUNSync,

-	(bool (*)(void *, BlockDeviceInfo *))usbhmsdLUNGetInfo

-};

-

-

-

-static uint32_t _requestsense(USBHMassStorageLUNDriver *lunp) {

-	scsi_sense_response_t sense;

-	msd_result_t res;

-

-	res = scsi_requestsense(lunp, &sense);

-	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-		uerr("\tREQUEST SENSE: Transaction error");

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-		uerr("\tREQUEST SENSE: Command Failed");

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-		//TODO: Do reset, etc.

-		uerr("\tREQUEST SENSE: Command Phase Error");

-		goto failed;

-	}

-

-	uerrf("\tREQUEST SENSE: Sense key=%x, ASC=%02x, ASCQ=%02x",

-			sense.byte[2] & 0xf, sense.byte[12], sense.byte[13]);

-

-	return (sense.byte[2] & 0xf) | (sense.byte[12] << 8) | (sense.byte[13] << 16);

-

-failed:

-	return 0xffffffff;

-}

-

-void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	memset(lunp, 0, sizeof(*lunp));

-	lunp->vmt = &blk_vmt;

-	lunp->state = BLK_STOP;

-	/* Unnecessary because of the memset:

-		lunp->msdp = NULL;

-		lunp->next = NULL;

-		lunp->info.* = 0;

-	*/

-}

-

-void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	osalSysLock();

-	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),

-			"invalid state");

-	//TODO: complete

-	//lunp->state = BLK_ACTIVE;

-	osalSysUnlock();

-}

-

-void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	osalSysLock();

-	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),

-			"invalid state");

-	//TODO: complete

-	//lunp->state = BLK_STOP;

-	osalSysUnlock();

-}

-

-bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {

-	USBHMassStorageDriver *const msdp = lunp->msdp;

-	msd_result_t res;

-

-	osalDbgCheck(msdp != NULL);

-	osalSysLock();

-	//osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),

-    //            "invalid state");

-	if (lunp->state == BLK_READY) {

-	    osalSysUnlock();

-		return HAL_SUCCESS;

-	} else if (lunp->state != BLK_ACTIVE) {

-		osalSysUnlock();

-		return HAL_FAILED;

-	}

-	lunp->state = BLK_CONNECTING;

-    osalSysUnlock();

-

-    osalMutexLock(&msdp->mtx);

-

-    USBH_DEFINE_BUFFER(union {

-    		scsi_inquiry_response_t inq;

-    		scsi_readcapacity10_response_t cap;	}, u);

-

-	uinfo("INQUIRY...");

-	res = scsi_inquiry(lunp, &u.inq);

-	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-		uerr("\tINQUIRY: Transaction error");

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-		uerr("\tINQUIRY: Command Failed");

-		_requestsense(lunp);

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-		//TODO: Do reset, etc.

-		uerr("\tINQUIRY: Command Phase Error");

-		goto failed;

-	}

-

-	uinfof("\tPDT=%02x", u.inq.peripheral & 0x1f);

-	if (u.inq.peripheral != 0) {

-		uerr("\tUnsupported PDT");

-		goto failed;

-	}

-

-	// Test if unit ready

-	uint8_t i;

-	for (i = 0; i < 10; i++) {

-		uinfo("TEST UNIT READY...");

-		res = scsi_testunitready(lunp);

-		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-			uerr("\tTEST UNIT READY: Transaction error");

-			goto failed;

-		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-			uerr("\tTEST UNIT READY: Command Failed");

-			_requestsense(lunp);

-			continue;

-		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-			//TODO: Do reset, etc.

-			uerr("\tTEST UNIT READY: Command Phase Error");

-			goto failed;

-		}

-		uinfo("\tReady.");

-		break;

-		// osalThreadSleepMilliseconds(200); // will raise 'code is unreachable' warning

-	}

-	if (i == 10) goto failed;

-

-	// Read capacity

-	uinfo("READ CAPACITY(10)...");

-	res = scsi_readcapacity10(lunp, &u.cap);

-	if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-		uerr("\tREAD CAPACITY(10): Transaction error");

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-		uerr("\tREAD CAPACITY(10): Command Failed");

-		_requestsense(lunp);

-		goto failed;

-	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-		//TODO: Do reset, etc.

-		uerr("\tREAD CAPACITY(10): Command Phase Error");

-		goto failed;

-	}

-	lunp->info.blk_size = __REV(u.cap.block_size);

-	lunp->info.blk_num = __REV(u.cap.last_block_addr) + 1;

-	uinfof("\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.");

-

-	osalMutexUnlock(&msdp->mtx);

-	osalSysLock();

-	lunp->state = BLK_READY;

-    osalSysUnlock();

-

-	return HAL_SUCCESS;

-

-  /* Connection failed, state reset to BLK_ACTIVE.*/

-failed:

-	osalMutexUnlock(&msdp->mtx);

-	osalSysLock();

-	lunp->state = BLK_ACTIVE;

-    osalSysUnlock();

-	return HAL_FAILED;

-}

-

-

-bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	osalSysLock();

-	osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),

-				"invalid state");

-	if (lunp->state == BLK_ACTIVE) {

-		osalSysUnlock();

-		return HAL_SUCCESS;

-	}

-	lunp->state = BLK_DISCONNECTING;

-	osalSysUnlock();

-

-	//TODO: complete

-

-	osalSysLock();

-	lunp->state = BLK_ACTIVE;

-	osalSysUnlock();

-	return HAL_SUCCESS;

-}

-

-bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

-                uint8_t *buffer, uint32_t n) {

-

-	osalDbgCheck(lunp != NULL);

-	bool ret = HAL_FAILED;

-	uint16_t blocks;

-	msd_result_t res;

-

-	osalSysLock();

-	if (lunp->state != BLK_READY) {

-		osalSysUnlock();

-		return ret;

-	}

-	lunp->state = BLK_READING;

-    osalSysUnlock();

-

-	osalMutexLock(&lunp->msdp->mtx);

-	while (n) {

-		if (n > 0xffff) {

-			blocks = 0xffff;

-		} else {

-			blocks = (uint16_t)n;

-		}

-		res = scsi_read10(lunp, startblk, blocks, buffer);

-		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-			uerr("\tREAD (10): Transaction error");

-			goto exit;

-		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-			//TODO: request sense, and act appropriately

-			uerr("\tREAD (10): Command Failed");

-			_requestsense(lunp);

-			goto exit;

-		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-			//TODO: Do reset, etc.

-			uerr("\tREAD (10): Command Phase Error");

-			goto exit;

-		}

-		n -= blocks;

-		startblk += blocks;

-		buffer += blocks * lunp->info.blk_size;

-	}

-

-	ret = HAL_SUCCESS;

-

-exit:

-	osalMutexUnlock(&lunp->msdp->mtx);

-	osalSysLock();

-	if (lunp->state == BLK_READING) {

-		lunp->state = BLK_READY;

-	} else {

-		osalDbgCheck(lunp->state == BLK_STOP);

-		uwarn("MSD: State = BLK_STOP");

-	}

-    osalSysUnlock();

-	return ret;

-}

-

-bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,

-                const uint8_t *buffer, uint32_t n) {

-

-	osalDbgCheck(lunp != NULL);

-	bool ret = HAL_FAILED;

-	uint16_t blocks;

-	msd_result_t res;

-

-	osalSysLock();

-	if (lunp->state != BLK_READY) {

-		osalSysUnlock();

-		return ret;

-	}

-	lunp->state = BLK_WRITING;

-    osalSysUnlock();

-

-	osalMutexLock(&lunp->msdp->mtx);

-	while (n) {

-		if (n > 0xffff) {

-			blocks = 0xffff;

-		} else {

-			blocks = (uint16_t)n;

-		}

-		res = scsi_write10(lunp, startblk, blocks, buffer);

-		if (res.tres != MSD_TRANSACTIONRESULT_OK) {

-			uerr("\tWRITE (10): Transaction error");

-			goto exit;

-		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {

-			//TODO: request sense, and act appropriately

-			uerr("\tWRITE (10): Command Failed");

-			_requestsense(lunp);

-			goto exit;

-		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {

-			//TODO: Do reset, etc.

-			uerr("\tWRITE (10): Command Phase Error");

-			goto exit;

-		}

-		n -= blocks;

-		startblk += blocks;

-		buffer += blocks * lunp->info.blk_size;

-	}

-

-	ret = HAL_SUCCESS;

-

-exit:

-	osalMutexUnlock(&lunp->msdp->mtx);

-	osalSysLock();

-	if (lunp->state == BLK_WRITING) {

-		lunp->state = BLK_READY;

-	} else {

-		osalDbgCheck(lunp->state == BLK_STOP);

-		uwarn("MSD: State = BLK_STOP");

-	}

-	osalSysUnlock();

-	return ret;

-}

-

-bool usbhmsdLUNSync(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	(void)lunp;

-	//TODO: Do SCSI Sync

-	return HAL_SUCCESS;

-}

-

-bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip) {

-	osalDbgCheck(lunp != NULL);

-	osalDbgCheck(bdip != NULL);

-	*bdip = lunp->info;

-	return HAL_SUCCESS;

-}

-

-bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	blkstate_t state;

-	osalSysLock();

-	state = lunp->state;

-	osalSysUnlock();

-	return (state >= BLK_ACTIVE);

-}

-

-bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp) {

-	osalDbgCheck(lunp != NULL);

-	return FALSE;

-}

-

-void usbhmsdObjectInit(USBHMassStorageDriver *msdp) {

-	osalDbgCheck(msdp != NULL);

-	memset(msdp, 0, sizeof(*msdp));

-	msdp->info = &usbhmsdClassDriverInfo;

-	osalMutexObjectInit(&msdp->mtx);

-}

-

-#endif

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+#include "hal_usbh.h"
+
+#if HAL_USBH_USE_MSD
+
+#if !HAL_USE_USBH
+#error "USBHMSD needs USBH"
+#endif
+
+#include <string.h>
+#include "usbh/dev/msd.h"
+#include "usbh/internal.h"
+
+//#pragma GCC optimize("Og")
+
+
+#if USBHMSD_DEBUG_ENABLE_TRACE
+#define udbgf(f, ...)  usbDbgPrintf(f, ##__VA_ARGS__)
+#define udbg(f, ...)  usbDbgPuts(f, ##__VA_ARGS__)
+#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
+
+
+
+
+
+/*===========================================================================*/
+/* USB Class driver loader for MSD								 		 	 */
+/*===========================================================================*/
+
+USBHMassStorageDriver USBHMSD[HAL_USBHMSD_MAX_INSTANCES];
+
+static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem);
+static void _msd_unload(usbh_baseclassdriver_t *drv);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	_msd_load,
+	_msd_unload
+};
+
+const usbh_classdriverinfo_t usbhmsdClassDriverInfo = {
+	0x08, 0x06, 0x50, "MSD", &class_driver_vmt
+};
+
+#define MSD_REQ_RESET							0xFF
+#define MSD_GET_MAX_LUN							0xFE
+
+static usbh_baseclassdriver_t *_msd_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	int i;
+	USBHMassStorageDriver *msdp;
+	uint8_t luns; // should declare it here to eliminate 'control bypass initialization' warning
+	usbh_urbstatus_t stat;  // should declare it here to eliminate 'control bypass initialization' warning
+
+	if ((rem < descriptor[0]) || (descriptor[1] != USBH_DT_INTERFACE))
+		return NULL;
+
+	const usbh_interface_descriptor_t * const ifdesc = (const usbh_interface_descriptor_t *)descriptor;
+
+	if ((ifdesc->bAlternateSetting != 0)
+			|| (ifdesc->bNumEndpoints < 2)
+			|| (ifdesc->bInterfaceSubClass != 0x06)
+			|| (ifdesc->bInterfaceProtocol != 0x50)) {
+		return NULL;
+	}
+
+	/* alloc driver */
+	for (i = 0; i < HAL_USBHMSD_MAX_INSTANCES; i++) {
+		if (USBHMSD[i].dev == NULL) {
+			msdp = &USBHMSD[i];
+			goto alloc_ok;
+		}
+	}
+
+	uwarn("Can't alloc MSD driver");
+
+	/* can't alloc */
+	return NULL;
+
+alloc_ok:
+	/* initialize the driver's variables */
+	msdp->epin.status = USBH_EPSTATUS_UNINITIALIZED;
+	msdp->epout.status = USBH_EPSTATUS_UNINITIALIZED;
+	msdp->max_lun = 0;
+	msdp->tag = 0;
+	msdp->luns = 0;
+	msdp->ifnum = ifdesc->bInterfaceNumber;
+	usbhEPSetName(&dev->ctrl, "MSD[CTRL]");
+
+	/* parse the configuration descriptor */
+	if_iterator_t iif;
+	generic_iterator_t iep;
+	iif.iad = 0;
+	iif.curr = descriptor;
+	iif.rem = rem;
+	for (ep_iter_init(&iep, &iif); iep.valid; ep_iter_next(&iep)) {
+		const usbh_endpoint_descriptor_t *const epdesc = ep_get(&iep);
+		if ((epdesc->bEndpointAddress & 0x80) && (epdesc->bmAttributes == USBH_EPTYPE_BULK)) {
+			uinfof("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);
+			usbhEPObjectInit(&msdp->epout, dev, epdesc);
+			usbhEPSetName(&msdp->epout, "MSD[BOUT]");
+		} else {
+			uinfof("unsupported endpoint found: bEndpointAddress=%02x, bmAttributes=%02x",
+					epdesc->bEndpointAddress, epdesc->bmAttributes);
+		}
+	}
+	if ((msdp->epin.status != USBH_EPSTATUS_CLOSED) || (msdp->epout.status != USBH_EPSTATUS_CLOSED)) {
+		goto deinit;
+	}
+
+	/* read the number of LUNs */
+	uinfo("Reading Max LUN:");
+	USBH_DEFINE_BUFFER(uint8_t, buff[4]);
+	stat = usbhControlRequest(dev,
+			USBH_CLASSIN(USBH_REQTYPE_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);
+		if (msdp->max_lun > HAL_USBHMSD_MAX_LUNS) {
+			msdp->max_lun = HAL_USBHMSD_MAX_LUNS;
+			uwarnf("\tUsing max_lun = %d", msdp->max_lun);
+		}
+	} else if (stat == USBH_URBSTATUS_STALL) {
+		uwarn("\tStall, max_lun = 1");
+		msdp->max_lun = 1;
+	} else {
+		uerr("\tError");
+		goto deinit;
+	}
+
+	/* open the bulk IN/OUT endpoints */
+	usbhEPOpen(&msdp->epin);
+	usbhEPOpen(&msdp->epout);
+
+	/* Alloc one block device per logical unit found */
+	luns = msdp->max_lun;
+	for (i = 0; (luns > 0) && (i < HAL_USBHMSD_MAX_LUNS); i++) {
+		if (MSBLKD[i].msdp == NULL) {
+			/* link the new block driver to the list */
+			MSBLKD[i].next = msdp->luns;
+			msdp->luns = &MSBLKD[i];
+			MSBLKD[i].msdp = msdp;
+
+			osalSysLock();
+			MSBLKD[i].state = BLK_ACTIVE;	/* transition directly to active, instead of BLK_STOP */
+			osalSysUnlock();
+
+			/* connect the LUN (TODO: review if it's best to leave the LUN disconnected) */
+			usbhmsdLUNConnect(&MSBLKD[i]);
+			luns--;
+		}
+	}
+
+	return (usbh_baseclassdriver_t *)msdp;
+
+deinit:
+	/* Here, the enpoints are closed, and the driver is unlinked */
+	return NULL;
+}
+
+static void _msd_unload(usbh_baseclassdriver_t *drv) {
+	osalDbgCheck(drv != NULL);
+	USBHMassStorageDriver *const msdp = (USBHMassStorageDriver *)drv;
+	USBHMassStorageLUNDriver *lunp = msdp->luns;
+
+	osalMutexLock(&msdp->mtx);
+	osalSysLock();
+	usbhEPCloseS(&msdp->epin);
+	usbhEPCloseS(&msdp->epout);
+	while (lunp) {
+		lunp->state = BLK_STOP;
+		lunp = lunp->next;
+	}
+	osalSysUnlock();
+	osalMutexUnlock(&msdp->mtx);
+
+	/* now that the LUNs are idle, deinit them */
+	lunp = msdp->luns;
+	osalSysLock();
+	while (lunp) {
+		usbhmsdLUNObjectInit(lunp);
+		lunp = lunp->next;
+	}
+	osalSysUnlock();
+}
+
+
+/*===========================================================================*/
+/* MSD Class driver operations (Bulk-Only transport)			 		 	 */
+/*===========================================================================*/
+
+
+
+/* USB Bulk Only Transport SCSI Command block wrapper */
+PACKED_STRUCT {
+	uint32_t dCBWSignature;
+	uint32_t dCBWTag;
+	uint32_t dCBWDataTransferLength;
+	uint8_t bmCBWFlags;
+	uint8_t bCBWLUN;
+	uint8_t bCBWCBLength;
+	uint8_t CBWCB[16];
+} msd_cbw_t;
+#define MSD_CBW_SIGNATURE						0x43425355
+#define MSD_CBWFLAGS_D2H						0x80
+#define MSD_CBWFLAGS_H2D						0x00
+
+
+/* USB Bulk Only Transport SCSI Command status wrapper */
+PACKED_STRUCT {
+	uint32_t dCSWSignature;
+	uint32_t dCSWTag;
+	uint32_t dCSWDataResidue;
+	uint8_t bCSWStatus;
+} msd_csw_t;
+#define MSD_CSW_SIGNATURE						0x53425355
+
+
+typedef union {
+	msd_cbw_t cbw;
+	msd_csw_t csw;
+} msd_transaction_t;
+
+typedef enum {
+	MSD_TRANSACTIONRESULT_OK,
+	MSD_TRANSACTIONRESULT_DISCONNECTED,
+	MSD_TRANSACTIONRESULT_STALL,
+	MSD_TRANSACTIONRESULT_BUS_ERROR,
+	MSD_TRANSACTIONRESULT_SYNC_ERROR
+} msd_transaction_result_t;
+
+typedef enum {
+	MSD_COMMANDRESULT_PASSED = 0,
+	MSD_COMMANDRESULT_FAILED = 1,
+	MSD_COMMANDRESULT_PHASE_ERROR = 2
+} msd_command_result_t;
+
+typedef struct {
+	msd_transaction_result_t tres;
+	msd_command_result_t cres;
+} msd_result_t;
+
+
+/* ----------------------------------------------------- */
+/* 			SCSI Commands 								*/
+/* ----------------------------------------------------- */
+
+/* Read 10 and Write 10 */
+#define SCSI_CMD_READ_10 						0x28
+#define SCSI_CMD_WRITE_10						0x2A
+
+/* Request sense */
+#define SCSI_CMD_REQUEST_SENSE 					0x03
+PACKED_STRUCT {
+	uint8_t byte[18];
+} scsi_sense_response_t;
+
+#define SCSI_SENSE_KEY_GOOD                     0x00
+#define SCSI_SENSE_KEY_RECOVERED_ERROR          0x01
+#define SCSI_SENSE_KEY_NOT_READY                0x02
+#define SCSI_SENSE_KEY_MEDIUM_ERROR             0x03
+#define SCSI_SENSE_KEY_HARDWARE_ERROR           0x04
+#define SCSI_SENSE_KEY_ILLEGAL_REQUEST          0x05
+#define SCSI_SENSE_KEY_UNIT_ATTENTION           0x06
+#define SCSI_SENSE_KEY_DATA_PROTECT             0x07
+#define SCSI_SENSE_KEY_BLANK_CHECK              0x08
+#define SCSI_SENSE_KEY_VENDOR_SPECIFIC          0x09
+#define SCSI_SENSE_KEY_COPY_ABORTED             0x0A
+#define SCSI_SENSE_KEY_ABORTED_COMMAND          0x0B
+#define SCSI_SENSE_KEY_VOLUME_OVERFLOW          0x0D
+#define SCSI_SENSE_KEY_MISCOMPARE               0x0E
+#define SCSI_ASENSE_NO_ADDITIONAL_INFORMATION   0x00
+#define SCSI_ASENSE_LOGICAL_UNIT_NOT_READY      0x04
+#define SCSI_ASENSE_INVALID_FIELD_IN_CDB        0x24
+#define SCSI_ASENSE_NOT_READY_TO_READY_CHANGE   0x28
+#define SCSI_ASENSE_WRITE_PROTECTED             0x27
+#define SCSI_ASENSE_FORMAT_ERROR                0x31
+#define SCSI_ASENSE_INVALID_COMMAND             0x20
+#define SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE 0x21
+#define SCSI_ASENSE_MEDIUM_NOT_PRESENT                 0x3A
+#define SCSI_ASENSEQ_NO_QUALIFIER                      0x00
+#define SCSI_ASENSEQ_FORMAT_COMMAND_FAILED             0x01
+#define SCSI_ASENSEQ_INITIALIZING_COMMAND_REQUIRED     0x02
+#define SCSI_ASENSEQ_OPERATION_IN_PROGRESS             0x07
+
+/* Inquiry */
+#define SCSI_CMD_INQUIRY 						0x12
+PACKED_STRUCT {
+	uint8_t peripheral;
+	uint8_t removable;
+	uint8_t version;
+	uint8_t response_data_format;
+	uint8_t additional_length;
+	uint8_t sccstp;
+	uint8_t bqueetc;
+	uint8_t cmdque;
+	uint8_t vendorID[8];
+	uint8_t productID[16];
+	uint8_t productRev[4];
+} scsi_inquiry_response_t;
+
+/* Read Capacity 10 */
+#define SCSI_CMD_READ_CAPACITY_10				0x25
+PACKED_STRUCT {
+	uint32_t last_block_addr;
+	uint32_t block_size;
+} scsi_readcapacity10_response_t;
+
+/* Start/Stop Unit */
+#define SCSI_CMD_START_STOP_UNIT				0x1B
+PACKED_STRUCT {
+	uint8_t op_code;
+	uint8_t lun_immed;
+	uint8_t res1;
+	uint8_t res2;
+	uint8_t loej_start;
+	uint8_t control;
+} scsi_startstopunit_request_t;
+
+/* test unit ready */
+#define SCSI_CMD_TEST_UNIT_READY				0x00
+
+/* Other commands, TODO: use or remove them
+#define SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL	0x1E
+#define SCSI_CMD_VERIFY_10						0x2F
+#define SCSI_CMD_SEND_DIAGNOSTIC				0x1D
+#define SCSI_CMD_MODE_SENSE_6                   0x1A
+*/
+
+static inline void _prepare_cbw(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp) {
+	tran->cbw.bCBWLUN = (uint8_t)(lunp - &lunp->msdp->luns[0]);
+	memset(&tran->cbw.CBWCB, 0, sizeof(tran->cbw.CBWCB));
+}
+
+static msd_transaction_result_t _msd_transaction(msd_transaction_t *tran, USBHMassStorageLUNDriver *lunp, void *data) {
+
+	uint32_t actual_len;
+	usbh_urbstatus_t status;
+
+	tran->cbw.dCBWSignature = MSD_CBW_SIGNATURE;
+	tran->cbw.dCBWTag = ++lunp->msdp->tag;
+
+	/* control phase */
+	status = usbhBulkTransfer(&lunp->msdp->epout, &tran->cbw,
+					sizeof(tran->cbw), &actual_len, MS2ST(1000));
+
+	if (status == USBH_URBSTATUS_CANCELLED) {
+		uerr("\tMSD: Control phase: USBH_URBSTATUS_CANCELLED");
+		return MSD_TRANSACTIONRESULT_DISCONNECTED;
+	} else if (status == USBH_URBSTATUS_STALL) {
+		uerr("\tMSD: Control phase: USBH_URBSTATUS_STALL");
+		return MSD_TRANSACTIONRESULT_STALL;
+	} else if (status != USBH_URBSTATUS_OK) {
+		uerrf("\tMSD: Control phase: status = %d, != OK", status);
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	} else if (actual_len != sizeof(tran->cbw)) {
+		uerrf("\tMSD: Control phase: wrong actual_len = %d", actual_len);
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	}
+
+
+	/* data phase */
+	if (tran->cbw.dCBWDataTransferLength) {
+		status = usbhBulkTransfer(
+				tran->cbw.bmCBWFlags & MSD_CBWFLAGS_D2H ? &lunp->msdp->epin : &lunp->msdp->epout,
+				data,
+				tran->cbw.dCBWDataTransferLength,
+				&actual_len, MS2ST(20000));
+
+		if (status == USBH_URBSTATUS_CANCELLED) {
+			uerr("\tMSD: Data phase: USBH_URBSTATUS_CANCELLED");
+			return MSD_TRANSACTIONRESULT_DISCONNECTED;
+		} else if (status == USBH_URBSTATUS_STALL) {
+			uerr("\tMSD: Data phase: USBH_URBSTATUS_STALL");
+			return MSD_TRANSACTIONRESULT_STALL;
+		} else if (status != USBH_URBSTATUS_OK) {
+			uerrf("\tMSD: Data phase: status = %d, != OK", status);
+			return MSD_TRANSACTIONRESULT_BUS_ERROR;
+		} else if (actual_len != tran->cbw.dCBWDataTransferLength) {
+			uerrf("\tMSD: Data phase: wrong actual_len = %d", actual_len);
+			return MSD_TRANSACTIONRESULT_BUS_ERROR;
+		}
+	}
+
+
+	/* status phase */
+	status = usbhBulkTransfer(&lunp->msdp->epin, &tran->csw,
+			sizeof(tran->csw), &actual_len, MS2ST(1000));
+
+	if (status == USBH_URBSTATUS_CANCELLED) {
+		uerr("\tMSD: Status phase: USBH_URBSTATUS_CANCELLED");
+		return MSD_TRANSACTIONRESULT_DISCONNECTED;
+	} else if (status == USBH_URBSTATUS_STALL) {
+		uerr("\tMSD: Status phase: USBH_URBSTATUS_STALL");
+		return MSD_TRANSACTIONRESULT_STALL;
+	} else if (status != USBH_URBSTATUS_OK) {
+		uerrf("\tMSD: Status phase: status = %d, != OK", status);
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	} else if (actual_len != sizeof(tran->csw)) {
+		uerrf("\tMSD: Status phase: wrong actual_len = %d", actual_len);
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	} else if (tran->csw.dCSWSignature != MSD_CSW_SIGNATURE) {
+		uerr("\tMSD: Status phase: wrong signature");
+		return MSD_TRANSACTIONRESULT_BUS_ERROR;
+	} else if (tran->csw.dCSWTag != lunp->msdp->tag) {
+		uerrf("\tMSD: Status phase: wrong tag (expected %d, got %d)",
+				lunp->msdp->tag, tran->csw.dCSWTag);
+		return MSD_TRANSACTIONRESULT_SYNC_ERROR;
+	}
+
+	if (tran->csw.dCSWDataResidue) {
+		uwarnf("\tMSD: Residue=%d", tran->csw.dCSWDataResidue);
+	}
+
+	return MSD_TRANSACTIONRESULT_OK;
+}
+
+
+static msd_result_t scsi_inquiry(USBHMassStorageLUNDriver *lunp, scsi_inquiry_response_t *resp) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_inquiry_response_t);
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 6;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_INQUIRY;
+	transaction.cbw.CBWCB[4] = sizeof(scsi_inquiry_response_t);
+
+	res.tres = _msd_transaction(&transaction, lunp, resp);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+static msd_result_t scsi_requestsense(USBHMassStorageLUNDriver *lunp, scsi_sense_response_t *resp) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_sense_response_t);
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 12;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_REQUEST_SENSE;
+	transaction.cbw.CBWCB[4] = sizeof(scsi_sense_response_t);
+
+	res.tres = _msd_transaction(&transaction, lunp, resp);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+static msd_result_t scsi_testunitready(USBHMassStorageLUNDriver *lunp) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = 0;
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 6;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_TEST_UNIT_READY;
+
+	res.tres = _msd_transaction(&transaction, lunp, NULL);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+static msd_result_t scsi_readcapacity10(USBHMassStorageLUNDriver *lunp, scsi_readcapacity10_response_t *resp) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = sizeof(scsi_readcapacity10_response_t);
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 12;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_CAPACITY_10;
+
+	res.tres = _msd_transaction(&transaction, lunp, resp);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+
+static msd_result_t scsi_read10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, uint8_t *data) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_D2H;
+	transaction.cbw.bCBWCBLength = 10;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_READ_10;
+	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);
+	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);
+	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);
+	transaction.cbw.CBWCB[5] = (uint8_t)(lba);
+	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);
+	transaction.cbw.CBWCB[8] = (uint8_t)(n);
+
+	res.tres = _msd_transaction(&transaction, lunp, data);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+static msd_result_t scsi_write10(USBHMassStorageLUNDriver *lunp, uint32_t lba, uint16_t n, const uint8_t *data) {
+	msd_transaction_t transaction;
+	msd_result_t res;
+
+	_prepare_cbw(&transaction, lunp);
+	transaction.cbw.dCBWDataTransferLength = n * lunp->info.blk_size;
+	transaction.cbw.bmCBWFlags = MSD_CBWFLAGS_H2D;
+	transaction.cbw.bCBWCBLength = 10;
+	transaction.cbw.CBWCB[0] = SCSI_CMD_WRITE_10;
+	transaction.cbw.CBWCB[2] = (uint8_t)(lba >> 24);
+	transaction.cbw.CBWCB[3] = (uint8_t)(lba >> 16);
+	transaction.cbw.CBWCB[4] = (uint8_t)(lba >> 8);
+	transaction.cbw.CBWCB[5] = (uint8_t)(lba);
+	transaction.cbw.CBWCB[7] = (uint8_t)(n >> 8);
+	transaction.cbw.CBWCB[8] = (uint8_t)(n);
+
+	res.tres = _msd_transaction(&transaction, lunp, (uint8_t *)data);
+	if (res.tres == MSD_TRANSACTIONRESULT_OK) {
+		res.cres = (msd_command_result_t) transaction.csw.bCSWStatus;
+	}
+	return res;
+}
+
+
+
+/*===========================================================================*/
+/* Block driver data/functions								 		 	 	 */
+/*===========================================================================*/
+
+USBHMassStorageLUNDriver MSBLKD[HAL_USBHMSD_MAX_LUNS];
+
+static const struct USBHMassStorageDriverVMT blk_vmt = {
+	(bool (*)(void *))usbhmsdLUNIsInserted,
+	(bool (*)(void *))usbhmsdLUNIsProtected,
+	(bool (*)(void *))usbhmsdLUNConnect,
+	(bool (*)(void *))usbhmsdLUNDisconnect,
+	(bool (*)(void *, uint32_t, uint8_t *, uint32_t))usbhmsdLUNRead,
+	(bool (*)(void *, uint32_t,	const uint8_t *, uint32_t))usbhmsdLUNWrite,
+	(bool (*)(void *))usbhmsdLUNSync,
+	(bool (*)(void *, BlockDeviceInfo *))usbhmsdLUNGetInfo
+};
+
+
+
+static uint32_t _requestsense(USBHMassStorageLUNDriver *lunp) {
+	scsi_sense_response_t sense;
+	msd_result_t res;
+
+	res = scsi_requestsense(lunp, &sense);
+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+		uerr("\tREQUEST SENSE: Transaction error");
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+		uerr("\tREQUEST SENSE: Command Failed");
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+		//TODO: Do reset, etc.
+		uerr("\tREQUEST SENSE: Command Phase Error");
+		goto failed;
+	}
+
+	uerrf("\tREQUEST SENSE: Sense key=%x, ASC=%02x, ASCQ=%02x",
+			sense.byte[2] & 0xf, sense.byte[12], sense.byte[13]);
+
+	return (sense.byte[2] & 0xf) | (sense.byte[12] << 8) | (sense.byte[13] << 16);
+
+failed:
+	return 0xffffffff;
+}
+
+void usbhmsdLUNObjectInit(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	memset(lunp, 0, sizeof(*lunp));
+	lunp->vmt = &blk_vmt;
+	lunp->state = BLK_STOP;
+	/* Unnecessary because of the memset:
+		lunp->msdp = NULL;
+		lunp->next = NULL;
+		lunp->info.* = 0;
+	*/
+}
+
+void usbhmsdLUNStart(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	osalSysLock();
+	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),
+			"invalid state");
+	//TODO: complete
+	//lunp->state = BLK_ACTIVE;
+	osalSysUnlock();
+}
+
+void usbhmsdLUNStop(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	osalSysLock();
+	osalDbgAssert((lunp->state == BLK_STOP) || (lunp->state == BLK_ACTIVE),
+			"invalid state");
+	//TODO: complete
+	//lunp->state = BLK_STOP;
+	osalSysUnlock();
+}
+
+bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
+	USBHMassStorageDriver *const msdp = lunp->msdp;
+	msd_result_t res;
+
+	osalDbgCheck(msdp != NULL);
+	osalSysLock();
+	//osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),
+    //            "invalid state");
+	if (lunp->state == BLK_READY) {
+	    osalSysUnlock();
+		return HAL_SUCCESS;
+	} else if (lunp->state != BLK_ACTIVE) {
+		osalSysUnlock();
+		return HAL_FAILED;
+	}
+	lunp->state = BLK_CONNECTING;
+    osalSysUnlock();
+
+    osalMutexLock(&msdp->mtx);
+
+    USBH_DEFINE_BUFFER(union {
+    		scsi_inquiry_response_t inq;
+    		scsi_readcapacity10_response_t cap;	}, u);
+
+	uinfo("INQUIRY...");
+	res = scsi_inquiry(lunp, &u.inq);
+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+		uerr("\tINQUIRY: Transaction error");
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+		uerr("\tINQUIRY: Command Failed");
+		_requestsense(lunp);
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+		//TODO: Do reset, etc.
+		uerr("\tINQUIRY: Command Phase Error");
+		goto failed;
+	}
+
+	uinfof("\tPDT=%02x", u.inq.peripheral & 0x1f);
+	if (u.inq.peripheral != 0) {
+		uerr("\tUnsupported PDT");
+		goto failed;
+	}
+
+	// Test if unit ready
+	uint8_t i;
+	for (i = 0; i < 10; i++) {
+		uinfo("TEST UNIT READY...");
+		res = scsi_testunitready(lunp);
+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+			uerr("\tTEST UNIT READY: Transaction error");
+			goto failed;
+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+			uerr("\tTEST UNIT READY: Command Failed");
+			_requestsense(lunp);
+			continue;
+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+			//TODO: Do reset, etc.
+			uerr("\tTEST UNIT READY: Command Phase Error");
+			goto failed;
+		}
+		uinfo("\tReady.");
+		break;
+		// osalThreadSleepMilliseconds(200); // will raise 'code is unreachable' warning
+	}
+	if (i == 10) goto failed;
+
+	// Read capacity
+	uinfo("READ CAPACITY(10)...");
+	res = scsi_readcapacity10(lunp, &u.cap);
+	if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+		uerr("\tREAD CAPACITY(10): Transaction error");
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+		uerr("\tREAD CAPACITY(10): Command Failed");
+		_requestsense(lunp);
+		goto failed;
+	} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+		//TODO: Do reset, etc.
+		uerr("\tREAD CAPACITY(10): Command Phase Error");
+		goto failed;
+	}
+	lunp->info.blk_size = __REV(u.cap.block_size);
+	lunp->info.blk_num = __REV(u.cap.last_block_addr) + 1;
+	uinfof("\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.");
+
+	osalMutexUnlock(&msdp->mtx);
+	osalSysLock();
+	lunp->state = BLK_READY;
+    osalSysUnlock();
+
+	return HAL_SUCCESS;
+
+  /* Connection failed, state reset to BLK_ACTIVE.*/
+failed:
+	osalMutexUnlock(&msdp->mtx);
+	osalSysLock();
+	lunp->state = BLK_ACTIVE;
+    osalSysUnlock();
+	return HAL_FAILED;
+}
+
+
+bool usbhmsdLUNDisconnect(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	osalSysLock();
+	osalDbgAssert((lunp->state == BLK_ACTIVE) || (lunp->state == BLK_READY),
+				"invalid state");
+	if (lunp->state == BLK_ACTIVE) {
+		osalSysUnlock();
+		return HAL_SUCCESS;
+	}
+	lunp->state = BLK_DISCONNECTING;
+	osalSysUnlock();
+
+	//TODO: complete
+
+	osalSysLock();
+	lunp->state = BLK_ACTIVE;
+	osalSysUnlock();
+	return HAL_SUCCESS;
+}
+
+bool usbhmsdLUNRead(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
+                uint8_t *buffer, uint32_t n) {
+
+	osalDbgCheck(lunp != NULL);
+	bool ret = HAL_FAILED;
+	uint16_t blocks;
+	msd_result_t res;
+
+	osalSysLock();
+	if (lunp->state != BLK_READY) {
+		osalSysUnlock();
+		return ret;
+	}
+	lunp->state = BLK_READING;
+    osalSysUnlock();
+
+	osalMutexLock(&lunp->msdp->mtx);
+	while (n) {
+		if (n > 0xffff) {
+			blocks = 0xffff;
+		} else {
+			blocks = (uint16_t)n;
+		}
+		res = scsi_read10(lunp, startblk, blocks, buffer);
+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+			uerr("\tREAD (10): Transaction error");
+			goto exit;
+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+			//TODO: request sense, and act appropriately
+			uerr("\tREAD (10): Command Failed");
+			_requestsense(lunp);
+			goto exit;
+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+			//TODO: Do reset, etc.
+			uerr("\tREAD (10): Command Phase Error");
+			goto exit;
+		}
+		n -= blocks;
+		startblk += blocks;
+		buffer += blocks * lunp->info.blk_size;
+	}
+
+	ret = HAL_SUCCESS;
+
+exit:
+	osalMutexUnlock(&lunp->msdp->mtx);
+	osalSysLock();
+	if (lunp->state == BLK_READING) {
+		lunp->state = BLK_READY;
+	} else {
+		osalDbgCheck(lunp->state == BLK_STOP);
+		uwarn("MSD: State = BLK_STOP");
+	}
+    osalSysUnlock();
+	return ret;
+}
+
+bool usbhmsdLUNWrite(USBHMassStorageLUNDriver *lunp, uint32_t startblk,
+                const uint8_t *buffer, uint32_t n) {
+
+	osalDbgCheck(lunp != NULL);
+	bool ret = HAL_FAILED;
+	uint16_t blocks;
+	msd_result_t res;
+
+	osalSysLock();
+	if (lunp->state != BLK_READY) {
+		osalSysUnlock();
+		return ret;
+	}
+	lunp->state = BLK_WRITING;
+    osalSysUnlock();
+
+	osalMutexLock(&lunp->msdp->mtx);
+	while (n) {
+		if (n > 0xffff) {
+			blocks = 0xffff;
+		} else {
+			blocks = (uint16_t)n;
+		}
+		res = scsi_write10(lunp, startblk, blocks, buffer);
+		if (res.tres != MSD_TRANSACTIONRESULT_OK) {
+			uerr("\tWRITE (10): Transaction error");
+			goto exit;
+		} else if (res.cres == MSD_COMMANDRESULT_FAILED) {
+			//TODO: request sense, and act appropriately
+			uerr("\tWRITE (10): Command Failed");
+			_requestsense(lunp);
+			goto exit;
+		} else if (res.cres == MSD_COMMANDRESULT_PHASE_ERROR) {
+			//TODO: Do reset, etc.
+			uerr("\tWRITE (10): Command Phase Error");
+			goto exit;
+		}
+		n -= blocks;
+		startblk += blocks;
+		buffer += blocks * lunp->info.blk_size;
+	}
+
+	ret = HAL_SUCCESS;
+
+exit:
+	osalMutexUnlock(&lunp->msdp->mtx);
+	osalSysLock();
+	if (lunp->state == BLK_WRITING) {
+		lunp->state = BLK_READY;
+	} else {
+		osalDbgCheck(lunp->state == BLK_STOP);
+		uwarn("MSD: State = BLK_STOP");
+	}
+	osalSysUnlock();
+	return ret;
+}
+
+bool usbhmsdLUNSync(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	(void)lunp;
+	//TODO: Do SCSI Sync
+	return HAL_SUCCESS;
+}
+
+bool usbhmsdLUNGetInfo(USBHMassStorageLUNDriver *lunp, BlockDeviceInfo *bdip) {
+	osalDbgCheck(lunp != NULL);
+	osalDbgCheck(bdip != NULL);
+	*bdip = lunp->info;
+	return HAL_SUCCESS;
+}
+
+bool usbhmsdLUNIsInserted(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	blkstate_t state;
+	osalSysLock();
+	state = lunp->state;
+	osalSysUnlock();
+	return (state >= BLK_ACTIVE);
+}
+
+bool usbhmsdLUNIsProtected(USBHMassStorageLUNDriver *lunp) {
+	osalDbgCheck(lunp != NULL);
+	return FALSE;
+}
+
+void usbhmsdObjectInit(USBHMassStorageDriver *msdp) {
+	osalDbgCheck(msdp != NULL);
+	memset(msdp, 0, sizeof(*msdp));
+	msdp->info = &usbhmsdClassDriverInfo;
+	osalMutexObjectInit(&msdp->mtx);
+}
+
+#endif
diff --git a/os/hal/src/usbh/usbh_uvc.c b/os/hal/src/usbh/hal_usbh_uvc.c
index 2fb2563..09a0f1d 100644
--- a/os/hal/src/usbh/usbh_uvc.c
+++ b/os/hal/src/usbh/hal_usbh_uvc.c
@@ -1,89 +1,89 @@
-/*

-    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

-              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)

-

-    Licensed under the Apache License, Version 2.0 (the "License");

-    you may not use this file except in compliance with the License.

-    You may obtain a copy of the License at

-

-        http://www.apache.org/licenses/LICENSE-2.0

-

-    Unless required by applicable law or agreed to in writing, software

-    distributed under the License is distributed on an "AS IS" BASIS,

-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

-    See the License for the specific language governing permissions and

-    limitations under the License.

-*/

-

-#include "hal.h"

-#include "usbh.h"

-

-#if HAL_USBH_USE_UVC

-

-#if !HAL_USE_USBH

-#error "USBHUVC needs HAL_USE_USBH"

-#endif

-

-#if !HAL_USBH_USE_IAD

-#error "USBHUVC needs HAL_USBH_USE_IAD"

-#endif

-

-#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

-

-

-static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,

-		const uint8_t *descriptor, uint16_t rem);

-static void uvc_unload(usbh_baseclassdriver_t *drv);

-

-static const usbh_classdriver_vmt_t class_driver_vmt = {

-	uvc_load,

-	uvc_unload

-};

-const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {

-	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt

-};

-

-

-static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {

-	(void)dev;

-	(void)descriptor;

-	(void)rem;

-	return NULL;

-}

-

-static void uvc_unload(usbh_baseclassdriver_t *drv) {

-	(void)drv;

-}

-

-#endif

-

+/*
+    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio
+              Copyright (C) 2015 Diego Ismirlian, TISA, (dismirlian (at) google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "hal.h"
+#include "hal_usbh.h"
+
+#if HAL_USBH_USE_UVC
+
+#if !HAL_USE_USBH
+#error "USBHUVC needs HAL_USE_USBH"
+#endif
+
+#if !HAL_USBH_USE_IAD
+#error "USBHUVC needs HAL_USBH_USE_IAD"
+#endif
+
+#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
+
+
+static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev,
+		const uint8_t *descriptor, uint16_t rem);
+static void uvc_unload(usbh_baseclassdriver_t *drv);
+
+static const usbh_classdriver_vmt_t class_driver_vmt = {
+	uvc_load,
+	uvc_unload
+};
+const usbh_classdriverinfo_t usbhuvcClassDriverInfo = {
+	0x0e, 0x03, 0x00, "UVC", &class_driver_vmt
+};
+
+
+static usbh_baseclassdriver_t *uvc_load(usbh_device_t *dev, const uint8_t *descriptor, uint16_t rem) {
+	(void)dev;
+	(void)descriptor;
+	(void)rem;
+	return NULL;
+}
+
+static void uvc_unload(usbh_baseclassdriver_t *drv) {
+	(void)drv;
+}
+
+#endif
+