Merge pull request #4 from amirhammad/core-refactor

Major rework

14 files changed, 1721 insertions, 1034 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000..f8478f6
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,61 @@
+if (USE_USART_DEBUG)
+	set (USART_HELPERS
+		usart_helpers.c
+		tinyprintf.c
+	)
+else (USE_USART_DEBUG)
+	set (USART_HELPERS "")
+endif (USE_USART_DEBUG)
+
+set (inc ${CMAKE_SOURCE_DIR}/include)
+
+add_library (usbhost
+	${USART_HELPERS}
+	${inc}/usbh_core.h
+	${inc}/usbh_driver_ac_midi.h
+	${inc}/usbh_driver_gp_xbox.h
+	${inc}/usbh_driver_hid.h
+	${inc}/usbh_driver_hub.h
+	${inc}/usbh_lld_stm32f4.h
+	${inc}/driver/usbh_device_driver.h
+
+	usbh_core.c
+	usbh_driver_ac_midi.c
+	usbh_driver_ac_midi_private.h
+	usbh_driver_gp_xbox.c
+	usbh_driver_hid.c
+	usbh_driver_hub.c
+	usbh_driver_hub_private.h
+	usbh_lld_stm32f4.c
+)
+
+target_link_libraries (usbhost
+	${LIBOPENCM3_LIB}
+)
+
+add_executable (demo
+	demo.c
+)
+
+target_link_libraries (demo
+	usbhost
+)
+
+add_custom_command (TARGET demo
+	POST_BUILD
+	COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:demo> ${CMAKE_BINARY_DIR}/demo.hex
+	COMMENT "Generating output files: ${CMAKE_BINARY_DIR}/demo.hex"
+)
+
+add_custom_command (TARGET demo
+	POST_BUILD
+	COMMAND ${CMAKE_SIZE} $<TARGET_FILE:demo>
+	COMMENT "Calculating size of the binary"
+)
+
+add_custom_command (TARGET usbhost
+	POST_BUILD
+	COMMENT "Calculating size of the library"
+	COMMAND ${CMAKE_SIZE} $<TARGET_FILE:usbhost>
+)
+
diff --git a/src/demo.c b/src/demo.c
index 5d829ac..a6b4196 100644
--- a/src/demo.c
+++ b/src/demo.c
@@ -23,7 +23,7 @@
 #include "usart_helpers.h"			/// provides LOG_PRINTF macros used for debugging

 #include "usbh_core.h"				/// provides usbh_init() and usbh_poll()

 #include "usbh_lld_stm32f4.h"		/// provides low level usb host driver for stm32f4 platform

-#include "usbh_driver_hid_mouse.h"	/// provides usb device driver Human Interface Device - type mouse

+#include "usbh_driver_hid.h"		/// provides generic usb device driver for Human Interface Device (HID)

 #include "usbh_driver_hub.h"		/// provides usb full speed hub driver (Low speed devices on hub are not supported)

 #include "usbh_driver_gp_xbox.h"	/// provides usb device driver for Gamepad: Microsoft XBOX compatible Controller

 #include "usbh_driver_ac_midi.h"	/// provides usb device driver for midi class devices

@@ -116,12 +116,23 @@ static void gpio_setup(void)
 

 static const usbh_dev_driver_t *device_drivers[] = {

 	&usbh_hub_driver,

-	&usbh_hid_mouse_driver,

+	&usbh_hid_driver,

 	&usbh_gp_xbox_driver,

 	&usbh_midi_driver,

-	0

+	NULL

 };

 

+static const usbh_low_level_driver_t * const lld_drivers[] = {

+#ifdef USE_STM32F4_USBH_DRIVER_FS

+	&usbh_lld_stm32f4_driver_fs, // Make sure USE_STM32F4_USBH_DRIVER_FS is defined in usbh_config.h

+#endif

+

+#ifdef USE_STM32F4_USBH_DRIVER_HS

+	&usbh_lld_stm32f4_driver_hs, // Make sure USE_STM32F4_USBH_DRIVER_HS is defined in usbh_config.h

+#endif

+	NULL

+	};

+

 static void gp_xbox_update(uint8_t device_id, gp_xbox_packet_t packet)

 {

 	(void)device_id;

@@ -148,17 +159,29 @@ static const gp_xbox_config_t gp_xbox_config = {
 	.notify_disconnected = &gp_xbox_disconnected

 };

 

-static void mouse_in_message_handler(uint8_t device_id, const uint8_t *data)

+static void hid_in_message_handler(uint8_t device_id, const uint8_t *data, uint32_t length)

 {

 	(void)device_id;

 	(void)data;

+	if (length < 4) {

+		LOG_PRINTF("data too short, type=%d\n", hid_get_type(device_id));

+		return;

+	}

+

 	// print only first 4 bytes, since every mouse should have at least these four set.

 	// Report descriptors are not read by driver for now, so we do not know what each byte means

-	LOG_PRINTF("MOUSE EVENT %02X %02X %02X %02X \n", data[0], data[1], data[2], data[3]);

+	LOG_PRINTF("HID EVENT %02X %02X %02X %02X \n", data[0], data[1], data[2], data[3]);

+	if (hid_get_type(device_id) == HID_TYPE_KEYBOARD) {

+		static int x = 0;

+		if (x != data[2]) {

+			x = data[2];

+			hid_set_report(device_id, x);

+		}

+	}

 }

 

-static const hid_mouse_config_t mouse_config = {

-	.mouse_in_message_handler = &mouse_in_message_handler

+static const hid_config_t hid_config = {

+	.hid_in_message_handler = &hid_in_message_handler

 };

 

 static void midi_in_message_handler(int device_id, uint8_t *data)

@@ -200,13 +223,12 @@ int main(void)
 	 *

 	 * Pass configuration struct where the callbacks are defined

 	 */

-	hid_mouse_driver_init(&mouse_config);

+	hid_driver_init(&hid_config);

 	hub_driver_init();

 	gp_xbox_driver_init(&gp_xbox_config);

 	midi_driver_init(&midi_config);

 

 	gpio_set(GPIOD,  GPIO13);

-

 	/**

 	 * Pass array of supported low level drivers

 	 * In case of stm32f407, there are up to two supported OTG hosts on one chip.

@@ -214,11 +236,6 @@ int main(void)
 	 *

 	 * Pass array of supported device drivers

 	 */

-	const void *lld_drivers[] = {

-		usbh_lld_stm32f4_driver_fs, // Make sure USE_STM32F4_USBH_DRIVER_FS is defined in usbh_config.h

-//		usbh_lld_stm32f4_driver_hs, // Make sure USE_STM32F4_USBH_DRIVER_HS is defined in usbh_config.h

-		0

-	};

 	usbh_init(lld_drivers, device_drivers);

 	gpio_clear(GPIOD,  GPIO13);

 

diff --git a/src/tinyprintf.c b/src/tinyprintf.c
new file mode 100644
index 0000000..bb22700
--- /dev/null
+++ b/src/tinyprintf.c
@@ -0,0 +1,521 @@
+/*
+File: tinyprintf.c
+
+Copyright (C) 2004  Kustaa Nyholm
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+*/
+
+#include "tinyprintf.h"
+
+
+/*
+ * Configuration
+ */
+
+/* Enable long int support */
+#define PRINTF_LONG_SUPPORT
+
+/* Enable long long int support (implies long int support) */
+#define PRINTF_LONG_LONG_SUPPORT
+
+/* Enable %z (size_t) support */
+#define PRINTF_SIZE_T_SUPPORT
+
+/*
+ * Configuration adjustments
+ */
+#ifdef PRINTF_SIZE_T_SUPPORT
+#include <sys/types.h>
+#endif
+
+#ifdef PRINTF_LONG_LONG_SUPPORT
+# define PRINTF_LONG_SUPPORT
+#endif
+
+/* __SIZEOF_<type>__ defined at least by gcc */
+#ifdef __SIZEOF_POINTER__
+# define SIZEOF_POINTER __SIZEOF_POINTER__
+#endif
+#ifdef __SIZEOF_LONG_LONG__
+# define SIZEOF_LONG_LONG __SIZEOF_LONG_LONG__
+#endif
+#ifdef __SIZEOF_LONG__
+# define SIZEOF_LONG __SIZEOF_LONG__
+#endif
+#ifdef __SIZEOF_INT__
+# define SIZEOF_INT __SIZEOF_INT__
+#endif
+
+#ifdef __GNUC__
+# define _TFP_GCC_NO_INLINE_  __attribute__ ((noinline))
+#else
+# define _TFP_GCC_NO_INLINE_
+#endif
+
+/*
+ * Implementation
+ */
+struct param {
+    char lz:1;          /**<  Leading zeros */
+    char alt:1;         /**<  alternate form */
+    char uc:1;          /**<  Upper case (for base16 only) */
+    char align_left:1;  /**<  0 == align right (default), 1 == align left */
+    unsigned int width; /**<  field width */
+    char sign;          /**<  The sign to display (if any) */
+    unsigned int base;  /**<  number base (e.g.: 8, 10, 16) */
+    char *bf;           /**<  Buffer to output */
+};
+
+
+#ifdef PRINTF_LONG_LONG_SUPPORT
+static void _TFP_GCC_NO_INLINE_ ulli2a(
+    unsigned long long int num, struct param *p)
+{
+    int n = 0;
+    unsigned long long int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void lli2a(long long int num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    ulli2a(num, p);
+}
+#endif
+
+#ifdef PRINTF_LONG_SUPPORT
+static void uli2a(unsigned long int num, struct param *p)
+{
+    int n = 0;
+    unsigned long int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void li2a(long num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    uli2a(num, p);
+}
+#endif
+
+static void ui2a(unsigned int num, struct param *p)
+{
+    int n = 0;
+    unsigned int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void i2a(int num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    ui2a(num, p);
+}
+
+static int a2d(char ch)
+{
+    if (ch >= '0' && ch <= '9')
+        return ch - '0';
+    else if (ch >= 'a' && ch <= 'f')
+        return ch - 'a' + 10;
+    else if (ch >= 'A' && ch <= 'F')
+        return ch - 'A' + 10;
+    else
+        return -1;
+}
+
+static char a2u(char ch, const char **src, int base, unsigned int *nump)
+{
+    const char *p = *src;
+    unsigned int num = 0;
+    int digit;
+    while ((digit = a2d(ch)) >= 0) {
+        if (digit > base)
+            break;
+        num = num * base + digit;
+        ch = *p++;
+    }
+    *src = p;
+    *nump = num;
+    return ch;
+}
+
+static void putchw(void *putp, putcf putf, struct param *p)
+{
+    char ch;
+    int n = p->width;
+    char *bf = p->bf;
+
+    /* Number of filling characters */
+    while (*bf++ && n > 0)
+        n--;
+    if (p->sign)
+        n--;
+    if (p->alt && p->base == 16)
+        n -= 2;
+    else if (p->alt && p->base == 8)
+        n--;
+
+    /* Fill with space to align to the right, before alternate or sign */
+    if (!p->lz && !p->align_left) {
+        while (n-- > 0)
+            putf(putp, ' ');
+    }
+
+    /* print sign */
+    if (p->sign)
+        putf(putp, p->sign);
+
+    /* Alternate */
+    if (p->alt && p->base == 16) {
+        putf(putp, '0');
+        putf(putp, (p->uc ? 'X' : 'x'));
+    } else if (p->alt && p->base == 8) {
+        putf(putp, '0');
+    }
+
+    /* Fill with zeros, after alternate or sign */
+    if (p->lz) {
+        while (n-- > 0)
+            putf(putp, '0');
+    }
+
+    /* Put actual buffer */
+    bf = p->bf;
+    while ((ch = *bf++))
+        putf(putp, ch);
+
+    /* Fill with space to align to the left, after string */
+    if (!p->lz && p->align_left) {
+        while (n-- > 0)
+            putf(putp, ' ');
+    }
+}
+
+void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
+{
+    struct param p;
+#ifdef PRINTF_LONG_SUPPORT
+    char bf[23];  /* long = 64b on some architectures */
+#else
+    char bf[12];  /* int = 32b on some architectures */
+#endif
+    char ch;
+    p.bf = bf;
+
+    while ((ch = *(fmt++))) {
+        if (ch != '%') {
+            putf(putp, ch);
+        } else {
+#ifdef PRINTF_LONG_SUPPORT
+            char lng = 0;  /* 1 for long, 2 for long long */
+#endif
+            /* Init parameter struct */
+            p.lz = 0;
+            p.alt = 0;
+            p.width = 0;
+            p.align_left = 0;
+            p.sign = 0;
+
+            /* Flags */
+            while ((ch = *(fmt++))) {
+                switch (ch) {
+                case '-':
+                    p.align_left = 1;
+                    continue;
+                case '0':
+                    p.lz = 1;
+                    continue;
+                case '#':
+                    p.alt = 1;
+                    continue;
+                default:
+                    break;
+                }
+                break;
+            }
+
+            /* Width */
+            if (ch >= '0' && ch <= '9') {
+                ch = a2u(ch, &fmt, 10, &(p.width));
+            }
+
+            /* We accept 'x.y' format but don't support it completely:
+             * we ignore the 'y' digit => this ignores 0-fill
+             * size and makes it == width (ie. 'x') */
+            if (ch == '.') {
+              p.lz = 1;  /* zero-padding */
+              /* ignore actual 0-fill size: */
+              do {
+                ch = *(fmt++);
+              } while ((ch >= '0') && (ch <= '9'));
+            }
+
+#ifdef PRINTF_SIZE_T_SUPPORT
+# ifdef PRINTF_LONG_SUPPORT
+            if (ch == 'z') {
+                ch = *(fmt++);
+                if (sizeof(size_t) == sizeof(unsigned long int))
+                    lng = 1;
+#  ifdef PRINTF_LONG_LONG_SUPPORT
+                else if (sizeof(size_t) == sizeof(unsigned long long int))
+                    lng = 2;
+#  endif
+            } else
+# endif
+#endif
+
+#ifdef PRINTF_LONG_SUPPORT
+            if (ch == 'l') {
+                ch = *(fmt++);
+                lng = 1;
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (ch == 'l') {
+                  ch = *(fmt++);
+                  lng = 2;
+                }
+#endif
+            }
+#endif
+            switch (ch) {
+            case 0:
+                goto abort;
+            case 'u':
+                p.base = 10;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    ulli2a(va_arg(va, unsigned long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    uli2a(va_arg(va, unsigned long int), &p);
+                else
+#endif
+                    ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'd':
+            case 'i':
+                p.base = 10;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    lli2a(va_arg(va, long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    li2a(va_arg(va, long int), &p);
+                else
+#endif
+                    i2a(va_arg(va, int), &p);
+                putchw(putp, putf, &p);
+                break;
+#ifdef SIZEOF_POINTER
+            case 'p':
+                p.alt = 1;
+# if defined(SIZEOF_INT) && SIZEOF_POINTER <= SIZEOF_INT
+                lng = 0;
+# elif defined(SIZEOF_LONG) && SIZEOF_POINTER <= SIZEOF_LONG
+                lng = 1;
+# elif defined(SIZEOF_LONG_LONG) && SIZEOF_POINTER <= SIZEOF_LONG_LONG
+                lng = 2;
+# endif
+#endif
+            case 'x':
+            case 'X':
+                p.base = 16;
+                p.uc = (ch == 'X')?1:0;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    ulli2a(va_arg(va, unsigned long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    uli2a(va_arg(va, unsigned long int), &p);
+                else
+#endif
+                    ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'o':
+                p.base = 8;
+                ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'c':
+                putf(putp, (char)(va_arg(va, int)));
+                break;
+            case 's':
+                p.bf = va_arg(va, char *);
+                putchw(putp, putf, &p);
+                p.bf = bf;
+                break;
+            case '%':
+                putf(putp, ch);
+            default:
+                break;
+            }
+        }
+    }
+ abort:;
+}
+
+#if TINYPRINTF_DEFINE_TFP_PRINTF
+static putcf stdout_putf;
+static void *stdout_putp;
+
+void init_printf(void *putp, putcf putf)
+{
+    stdout_putf = putf;
+    stdout_putp = putp;
+}
+
+void tfp_printf(char *fmt, ...)
+{
+    va_list va;
+    va_start(va, fmt);
+    tfp_format(stdout_putp, stdout_putf, fmt, va);
+    va_end(va);
+}
+#endif
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+struct _vsnprintf_putcf_data
+{
+  size_t dest_capacity;
+  char *dest;
+  size_t num_chars;
+};
+
+static void _vsnprintf_putcf(void *p, char c)
+{
+  struct _vsnprintf_putcf_data *data = (struct _vsnprintf_putcf_data*)p;
+  if (data->num_chars < data->dest_capacity)
+    data->dest[data->num_chars] = c;
+  data->num_chars ++;
+}
+
+int tfp_vsnprintf(char *str, size_t size, const char *format, va_list ap)
+{
+  struct _vsnprintf_putcf_data data;
+
+  if (size < 1)
+    return 0;
+
+  data.dest = str;
+  data.dest_capacity = size-1;
+  data.num_chars = 0;
+  tfp_format(&data, _vsnprintf_putcf, format, ap);
+
+  if (data.num_chars < data.dest_capacity)
+    data.dest[data.num_chars] = '\0';
+  else
+    data.dest[data.dest_capacity] = '\0';
+
+  return data.num_chars;
+}
+
+int tfp_snprintf(char *str, size_t size, const char *format, ...)
+{
+  va_list ap;
+  int retval;
+
+  va_start(ap, format);
+  retval = tfp_vsnprintf(str, size, format, ap);
+  va_end(ap);
+  return retval;
+}
+
+struct _vsprintf_putcf_data
+{
+  char *dest;
+  size_t num_chars;
+};
+
+static void _vsprintf_putcf(void *p, char c)
+{
+  struct _vsprintf_putcf_data *data = (struct _vsprintf_putcf_data*)p;
+  data->dest[data->num_chars++] = c;
+}
+
+int tfp_vsprintf(char *str, const char *format, va_list ap)
+{
+  struct _vsprintf_putcf_data data;
+  data.dest = str;
+  data.num_chars = 0;
+  tfp_format(&data, _vsprintf_putcf, format, ap);
+  data.dest[data.num_chars] = '\0';
+  return data.num_chars;
+}
+
+int tfp_sprintf(char *str, const char *format, ...)
+{
+  va_list ap;
+  int retval;
+
+  va_start(ap, format);
+  retval = tfp_vsprintf(str, format, ap);
+  va_end(ap);
+  return retval;
+}
+#endif
diff --git a/src/tinyprintf.h b/src/tinyprintf.h
new file mode 100644
index 0000000..a769f4a
--- /dev/null
+++ b/src/tinyprintf.h
@@ -0,0 +1,186 @@
+/*
+File: tinyprintf.h
+
+Copyright (C) 2004  Kustaa Nyholm
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+This library is really just two files: 'tinyprintf.h' and 'tinyprintf.c'.
+
+They provide a simple and small (+400 loc) printf functionality to
+be used in embedded systems.
+
+I've found them so useful in debugging that I do not bother with a
+debugger at all.
+
+They are distributed in source form, so to use them, just compile them
+into your project.
+
+Two printf variants are provided: printf and the 'sprintf' family of
+functions ('snprintf', 'sprintf', 'vsnprintf', 'vsprintf').
+
+The formats supported by this implementation are:
+'c' 'd' 'i' 'o' 'p' 'u' 's' 'x' 'X'.
+
+Zero padding and field width are also supported.
+
+If the library is compiled with 'PRINTF_SUPPORT_LONG' defined, then
+the long specifier is also supported. Note that this will pull in some
+long math routines (pun intended!) and thus make your executable
+noticeably longer. Likewise with 'PRINTF_LONG_LONG_SUPPORT' for the
+long long specifier, and with 'PRINTF_SIZE_T_SUPPORT' for the size_t
+specifier.
+
+The memory footprint of course depends on the target CPU, compiler and
+compiler options, but a rough guesstimate (based on a H8S target) is about
+1.4 kB for code and some twenty 'int's and 'char's, say 60 bytes of stack space.
+Not too bad. Your mileage may vary. By hacking the source code you can
+get rid of some hundred bytes, I'm sure, but personally I feel the balance of
+functionality and flexibility versus  code size is close to optimal for
+many embedded systems.
+
+To use the printf, you need to supply your own character output function,
+something like :
+
+void putc ( void* p, char c)
+{
+    while (!SERIAL_PORT_EMPTY) ;
+    SERIAL_PORT_TX_REGISTER = c;
+}
+
+Before you can call printf, you need to initialize it to use your
+character output function with something like:
+
+init_printf(NULL,putc);
+
+Notice the 'NULL' in 'init_printf' and the parameter 'void* p' in 'putc',
+the NULL (or any pointer) you pass into the 'init_printf' will eventually be
+passed to your 'putc' routine. This allows you to pass some storage space (or
+anything really) to the character output function, if necessary.
+This is not often needed but it was implemented like that because it made
+implementing the sprintf function so neat (look at the source code).
+
+The code is re-entrant, except for the 'init_printf' function, so it is safe
+to call it from interrupts too, although this may result in mixed output.
+If you rely on re-entrancy, take care that your 'putc' function is re-entrant!
+
+The printf and sprintf functions are actually macros that translate to
+'tfp_printf' and 'tfp_sprintf' when 'TINYPRINTF_OVERRIDE_LIBC' is set
+(default). Setting it to 0 makes it possible to use them along with
+'stdio.h' printf's in a single source file. When
+'TINYPRINTF_OVERRIDE_LIBC' is set, please note that printf/sprintf are
+not function-like macros, so if you have variables or struct members
+with these names, things will explode in your face.  Without variadic
+macros this is the best we can do to wrap these function. If it is a
+problem, just give up the macros and use the functions directly, or
+rename them.
+
+It is also possible to avoid defining tfp_printf and/or tfp_sprintf by
+clearing 'TINYPRINTF_DEFINE_TFP_PRINTF' and/or
+'TINYPRINTF_DEFINE_TFP_SPRINTF' to 0. This allows for example to
+export only tfp_format, which is at the core of all the other
+functions.
+
+For further details see source code.
+
+regs Kusti, 23.10.2004
+*/
+
+#ifndef __TFP_PRINTF__
+#define __TFP_PRINTF__
+
+#include <stdarg.h>
+
+/* Global configuration */
+
+/* Set this to 0 if you do not want to provide tfp_printf */
+#ifndef TINYPRINTF_DEFINE_TFP_PRINTF
+# define TINYPRINTF_DEFINE_TFP_PRINTF 1
+#endif
+
+/* Set this to 0 if you do not want to provide
+   tfp_sprintf/snprintf/vsprintf/vsnprintf */
+#ifndef TINYPRINTF_DEFINE_TFP_SPRINTF
+# define TINYPRINTF_DEFINE_TFP_SPRINTF 1
+#endif
+
+/* Set this to 0 if you do not want tfp_printf and
+   tfp_{vsn,sn,vs,s}printf to be also available as
+   printf/{vsn,sn,vs,s}printf */
+#ifndef TINYPRINTF_OVERRIDE_LIBC
+# define TINYPRINTF_OVERRIDE_LIBC 1
+#endif
+
+/* Optional external types dependencies */
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+# include <sys/types.h>  /* size_t */
+#endif
+
+/* Declarations */
+
+#ifdef __GNUC__
+# define _TFP_SPECIFY_PRINTF_FMT(fmt_idx,arg1_idx) \
+    __attribute__((format (printf, fmt_idx, arg1_idx)))
+#else
+# define _TFP_SPECIFY_PRINTF_FMT(fmt_idx,arg1_idx)
+#endif
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+typedef void (*putcf) (void *, char);
+
+/*
+   'tfp_format' really is the central function for all tinyprintf. For
+   each output character after formatting, the 'putf' callback is
+   called with 2 args:
+     - an arbitrary void* 'putp' param defined by the user and
+       passed unmodified from 'tfp_format',
+     - the character.
+   The 'tfp_printf' and 'tfp_sprintf' functions simply define their own
+   callback and pass to it the right 'putp' it is expecting.
+*/
+void tfp_format(void *putp, putcf putf, const char *fmt, va_list va);
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+int tfp_vsnprintf(char *str, size_t size, const char *fmt, va_list ap);
+int tfp_snprintf(char *str, size_t size, const char *fmt, ...) \
+     _TFP_SPECIFY_PRINTF_FMT(3, 4);
+int tfp_vsprintf(char *str, const char *fmt, va_list ap);
+int tfp_sprintf(char *str, const char *fmt, ...) \
+    _TFP_SPECIFY_PRINTF_FMT(2, 3);
+# if TINYPRINTF_OVERRIDE_LIBC
+#  define vsnprintf tfp_vsnprintf
+#  define snprintf tfp_snprintf
+#  define vsprintf tfp_vsprintf
+#  define sprintf tfp_sprintf
+# endif
+#endif
+
+#if TINYPRINTF_DEFINE_TFP_PRINTF
+void init_printf(void *putp, putcf putf);
+void tfp_printf(char *fmt, ...) _TFP_SPECIFY_PRINTF_FMT(1, 2);
+# if TINYPRINTF_OVERRIDE_LIBC
+#  define printf tfp_printf
+# endif
+#endif
+
+#ifdef  __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/usart_helpers.c b/src/usart_helpers.c
index 5a9aa03..fc32333 100644
--- a/src/usart_helpers.c
+++ b/src/usart_helpers.c
@@ -22,14 +22,15 @@
 

 

 #include "usart_helpers.h"

+#define TINYPRINTF_OVERRIDE_LIBC 0

+#define TINYPRINTF_DEFINE_TFP_SPRINTF 0

+#include "tinyprintf.h"

 

 #include <string.h>

 #include <stdarg.h>

 #include <stdlib.h>

 #include <stdio.h>

 #include <libopencm3/stm32/usart.h>

-#include <libopencm3/stm32/gpio.h>

-

 

 #define USART_FIFO_OUT_SIZE (4096)

 uint8_t usart_fifo_out_data[USART_FIFO_OUT_SIZE];

@@ -103,35 +104,22 @@ static void usart_fifo_in_push(uint8_t aData)
 	usart_fifo_in_len++;

 }

 

-

-static void usart_write(const char * data, uint32_t len)

+static void putf(void *arg, char c)

 {

-    uint32_t i;

-    for(i = 0; i < len; i++)

-    {

-        usart_fifo_push(data[i]);

-    }

+	//unused argument

+	(void)arg;

+

+	usart_fifo_push(c);

 }

+

 void usart_printf(const char *str, ...)

 {

 	va_list va;

 	va_start(va, str);

-	usart_vprintf(str, va);

+	tfp_format(NULL, putf, str, va);

 	va_end(va);

-

-}

-

-void usart_vprintf(const char *str, va_list va)

-{

-	char databuffer[128];

-	int i = vsnprintf(databuffer, 128, str, va);

-	if (i > 0) {

-		usart_write(databuffer, i);

-	}

 }

 

-

-

 void usart_init(uint32_t arg_usart, uint32_t baudrate)

 {

 	usart_set_baudrate(arg_usart, baudrate);

@@ -145,6 +133,7 @@ void usart_init(uint32_t arg_usart, uint32_t baudrate)
 	usart_enable(arg_usart);

 	usart = arg_usart;

 }

+

 void usart_interrupt(void)

 {

 	if (usart_get_interrupt_source(usart, USART_SR_RXNE)) {

@@ -230,9 +219,7 @@ void usart_call_cmd(struct usart_commands * commands)
 		LOG_PRINTF("#2");

 		return;

 	}

-	//~ for (i = 0; i < command_len; i++) {

-		//~ LOG_PRINTF("%c", command[i]);

-	//~ }

+

 	i=0;

 	while(commands[i].cmd != NULL) {

 		if (!strcmp((char*)command, (char*)commands[i].cmd)) {

@@ -243,7 +230,7 @@ void usart_call_cmd(struct usart_commands * commands)
 					commands[i].callback(&command[command_argindex]);

 				}

 			}

-			usart_write("\n>>",4);

+			LOG_PRINTF("\n>>");

 			command_len = 0;

 			command_argindex = 0;

 			return;

diff --git a/src/usart_helpers.h b/src/usart_helpers.h
index d36f689..e62f67d 100644
--- a/src/usart_helpers.h
+++ b/src/usart_helpers.h
@@ -38,7 +38,6 @@ struct usart_commands{
 #ifdef USART_DEBUG

 void usart_init(uint32_t usart, uint32_t baudrate);

 void usart_printf(const char *str, ...);

-void usart_vprintf(const char *str, va_list va);

 void usart_fifo_send(void);

 

 void usart_call_cmd(struct usart_commands * commands);

diff --git a/src/usbh_core.c b/src/usbh_core.c
index e1076cd..1a60285 100644
--- a/src/usbh_core.c
+++ b/src/usbh_core.c
@@ -28,12 +28,14 @@
 #include <libopencm3/stm32/gpio.h>
 #include <libopencm3/usb/usbstd.h>
 
+#include <stddef.h>
+
 static struct {
 	bool enumeration_run;
 	const usbh_low_level_driver_t * const *lld_drivers;
 	const usbh_dev_driver_t * const *dev_drivers;
 	int8_t address_temporary;
-} usbh_data = {0};
+} usbh_data = {};
 
 static void set_enumeration(void)
 {
@@ -50,10 +52,20 @@ static bool enumeration(void)
 	return usbh_data.enumeration_run;
 }
 
+void device_remove(usbh_device_t *dev)
+{
+	if (dev->drv && dev->drvdata) {
+		dev->drv->remove(dev->drvdata);
+	}
+	dev->address = -1;
+	dev->drv = NULL;
+	dev->drvdata = NULL;
+}
+
 /**
  *
  */
-static const usbh_dev_driver_t *find_driver(const usbh_dev_driver_info_t * device_info)
+static bool find_driver(usbh_device_t *dev, const usbh_dev_driver_info_t * device_info)
 {
 
 #define CHECK_PARTIAL_COMPATIBILITY(what) \
@@ -63,7 +75,6 @@ static const usbh_dev_driver_t *find_driver(const usbh_dev_driver_info_t * devic
 		continue;\
 	}
 
-
 	int i = 0;
 
 	while (usbh_data.dev_drivers[i]) {
@@ -77,9 +88,16 @@ static const usbh_dev_driver_t *find_driver(const usbh_dev_driver_info_t * devic
 		CHECK_PARTIAL_COMPATIBILITY(idVendor);
 		CHECK_PARTIAL_COMPATIBILITY(idProduct);
 
-		return usbh_data.dev_drivers[i];
+		dev->drv = usbh_data.dev_drivers[i];
+		dev->drvdata = dev->drv->init(dev);
+		if (!dev->drvdata) {
+			LOG_PRINTF("Unable to initialize device driver at index %d\n", i);
+			i++;
+			continue;
+		}
+		return true;
 	}
-	return 0;
+	return false;
 #undef CHECK_PARTIAL_COMPATIBILITY
 }
 
@@ -87,11 +105,10 @@ static const usbh_dev_driver_t *find_driver(const usbh_dev_driver_info_t * devic
 static void device_register(void *descriptors, uint16_t descriptors_len, usbh_device_t *dev)
 {
 	uint32_t i = 0;
-	dev->drv = 0;
 	uint8_t *buf = (uint8_t *)descriptors;
 
-	dev->drv = 0;
-	dev->drvdata = 0;
+	dev->drv = NULL;
+	dev->drvdata = NULL;
 
 	uint8_t desc_len = buf[i];
 	uint8_t desc_type = buf[i + 1];
@@ -122,14 +139,27 @@ static void device_register(void *descriptors, uint16_t descriptors_len, usbh_de
 			device_info.ifaceClass = iface->bInterfaceClass;
 			device_info.ifaceSubClass = iface->bInterfaceSubClass;
 			device_info.ifaceProtocol = iface->bInterfaceProtocol;
-			const usbh_dev_driver_t *driver = find_driver(&device_info);
-			if (driver) {
-				dev->drv = driver;
-				dev->drvdata = dev->drv->init(dev);
-				if (!dev->drvdata) {
-					LOG_PRINTF("CANT TOUCH THIS");
+			if (find_driver(dev, &device_info)) {
+				int k = 0;
+				while (k < descriptors_len) {
+					desc_len = buf[k];
+					void *drvdata = dev->drvdata;
+					LOG_PRINTF("[%d]", buf[k+1]);
+					if (dev->drv->analyze_descriptor(drvdata, &buf[k])) {
+						LOG_PRINTF("Device Initialized\n");
+						return;
+					}
+
+					if (desc_len == 0) {
+						LOG_PRINTF("Problem occured while parsing complete configuration descriptor");
+						return;
+					}
+					k += desc_len;
 				}
-				break;
+				LOG_PRINTF("Device driver isn't compatible with this device\n");
+				device_remove(dev);
+			} else {
+				LOG_PRINTF("No compatible driver has been found for interface #%d\n", iface->bInterfaceNumber);
 			}
 		}
 			break;
@@ -142,28 +172,11 @@ static void device_register(void *descriptors, uint16_t descriptors_len, usbh_de
 			return;
 		}
 		i += desc_len;
-
-	}
-
-	if (dev->drv && dev->drvdata) {
-		// analyze descriptors
-		LOG_PRINTF("ANALYZE");
-		i = 0;
-		while (i < descriptors_len) {
-			desc_len = buf[i];
-			void *drvdata = dev->drvdata;
-			LOG_PRINTF("[%d]",buf[i+1]);
-			if (dev->drv->analyze_descriptor(drvdata, &buf[i])) {
-				LOG_PRINTF("Device Initialized\n");
-				return;
-			}
-			i += desc_len;
-		}
 	}
 	LOG_PRINTF("Device NOT Initialized\n");
 }
 
-void usbh_init(const void *low_level_drivers[], const usbh_dev_driver_t * const device_drivers[])
+void usbh_init(const usbh_low_level_driver_t * const low_level_drivers[], const usbh_dev_driver_t * const device_drivers[])
 {
 	if (!low_level_drivers) {
 		return;
@@ -172,10 +185,9 @@ void usbh_init(const void *low_level_drivers[], const usbh_dev_driver_t * const
 	usbh_data.lld_drivers = (const usbh_low_level_driver_t **)low_level_drivers;
 	usbh_data.dev_drivers = device_drivers;
 
-	// TODO: init structures
 	uint32_t k = 0;
 	while (usbh_data.lld_drivers[k]) {
-		LOG_PRINTF("DRIVER %d\n", k);
+		LOG_PRINTF("Initialization low-level driver with index=%d\n", k);
 
 		usbh_device_t * usbh_device =
 			((usbh_generic_data_t *)(usbh_data.lld_drivers[k])->driver_data)->usbh_device;
@@ -186,7 +198,6 @@ void usbh_init(const void *low_level_drivers[], const usbh_dev_driver_t * const
 			usbh_device[i].drv = 0;
 			usbh_device[i].drvdata = 0;
 		}
-		LOG_PRINTF("DRIVER %d", k);
 		usbh_data.lld_drivers[k]->init(usbh_data.lld_drivers[k]->driver_data);
 
 		k++;
@@ -194,15 +205,11 @@ void usbh_init(const void *low_level_drivers[], const usbh_dev_driver_t * const
 
 }
 
-/*
- * NEW ENUMERATE
- *
- */
-void device_xfer_control_write_setup(void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
+static void device_xfer_control_write_setup(const void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
 {
 	usbh_packet_t packet;
 
-	packet.data = data;
+	packet.data.out = data;
 	packet.datalen = datalen;
 	packet.address = dev->address;
 	packet.endpoint_address = 0;
@@ -218,11 +225,11 @@ void device_xfer_control_write_setup(void *data, uint16_t datalen, usbh_packet_c
 	LOG_PRINTF("WR-setup@device...%d \n", dev->address);
 }
 
-void device_xfer_control_write_data(void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
+static void device_xfer_control_write_data(const void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
 {
 	usbh_packet_t packet;
 
-	packet.data = data;
+	packet.data.out = data;
 	packet.datalen = datalen;
 	packet.address = dev->address;
 	packet.endpoint_address = 0;
@@ -238,11 +245,11 @@ void device_xfer_control_write_data(void *data, uint16_t datalen, usbh_packet_ca
 	LOG_PRINTF("WR-data@device...%d \n", dev->address);
 }
 
-void device_xfer_control_read(void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
+static void device_xfer_control_read(void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
 {
 	usbh_packet_t packet;
 
-	packet.data = data;
+	packet.data.in = data;
 	packet.datalen = datalen;
 	packet.address = dev->address;
 	packet.endpoint_address = 0;
@@ -258,6 +265,85 @@ void device_xfer_control_read(void *data, uint16_t datalen, usbh_packet_callback
 }
 
 
+static void control_state_machine(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
+{
+	switch (dev->control.state) {
+	case USBH_CONTROL_STATE_SETUP:
+		if (cb_data.status != USBH_PACKET_CALLBACK_STATUS_OK) {
+			dev->control.state = USBH_CONTROL_STATE_NONE;
+			// Unable to deliver setup control packet - this is a fatal error
+			usbh_packet_callback_data_t ret_data;
+			ret_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;
+			ret_data.transferred_length = 0;
+			dev->control.callback(dev, ret_data);
+			break;
+		}
+		if (dev->control.setup_data.bmRequestType & USB_REQ_TYPE_IN) {
+			dev->control.state = USBH_CONTROL_STATE_DATA;
+			device_xfer_control_read(dev->control.data.in, dev->control.data_length, control_state_machine, dev);
+		} else {
+			if (dev->control.data_length == 0) {
+				dev->control.state = USBH_CONTROL_STATE_STATUS;
+				device_xfer_control_read(NULL, 0, control_state_machine, dev);
+			} else {
+				dev->control.state = USBH_CONTROL_STATE_DATA;
+				device_xfer_control_write_data(dev->control.data.out, dev->control.data_length, control_state_machine, dev);
+			}
+		}
+		break;
+
+	case USBH_CONTROL_STATE_DATA:
+		if (dev->control.setup_data.bmRequestType & USB_REQ_TYPE_IN) {
+			dev->control.state = USBH_CONTROL_STATE_NONE;
+			dev->control.callback(dev, cb_data);
+		} else {
+			if (cb_data.status != USBH_PACKET_CALLBACK_STATUS_OK) {
+				dev->control.state = USBH_CONTROL_STATE_NONE;
+				// Unable to deliver data control packet - this is a fatal error
+				usbh_packet_callback_data_t ret_data;
+				ret_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;
+				ret_data.transferred_length = 0;
+				dev->control.callback(dev, ret_data);
+				break;
+			}
+
+			if (dev->control.data_length == 0) {
+				// we should be in status state when the length of data is zero
+				LOG_PRINTF("Control logic error\n");
+				dev->control.state = USBH_CONTROL_STATE_NONE;
+				dev->control.callback(dev, cb_data);
+			} else {
+				dev->control.state = USBH_CONTROL_STATE_STATUS;
+				device_xfer_control_read(NULL, 0, control_state_machine, dev);
+			}
+		}
+		break;
+
+	case USBH_CONTROL_STATE_STATUS:
+		dev->control.state = USBH_CONTROL_STATE_NONE;
+		dev->control.callback(dev, cb_data);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void device_control(usbh_device_t *dev, usbh_packet_callback_t callback, const struct usb_setup_data *setup_data, void *data)
+{
+	if (dev->control.state != USBH_CONTROL_STATE_NONE) {
+		LOG_PRINTF("ERROR: Use of control state machine while not idle\n");
+		return;
+	}
+
+	dev->control.state = USBH_CONTROL_STATE_SETUP;
+	dev->control.callback = callback;
+	dev->control.data.out = data;
+	dev->control.data_length = setup_data->wLength;
+	dev->control.setup_data = *setup_data;
+	device_xfer_control_write_setup(&dev->control.setup_data, sizeof(dev->control.setup_data), control_state_machine, dev);
+}
+
 
 bool usbh_enum_available(void)
 {
@@ -289,102 +375,62 @@ usbh_device_t *usbh_get_free_device(const usbh_device_t *dev)
 	return 0;
 }
 
-static void device_enumeration_terminate(usbh_device_t *dev)
+static void device_enumeration_finish(usbh_device_t *dev)
 {
 	reset_enumeration();
-	dev->state = 0;
+	dev->state = USBH_ENUM_STATE_FIRST;
+}
+
+static void device_enumeration_terminate(usbh_device_t *dev)
+{
 	dev->address = -1;
+	device_enumeration_finish(dev);
 }
 
-/* Do not call this function directly,
- *     only via callback passing into low-level function
- * If you must, call it carefully ;)
- */
+#define CONTINUE_WITH(en) \
+	dev->state = en;\
+	device_enumerate(dev, cb_data);
+
 static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 {
 	const usbh_low_level_driver_t *lld = dev->lld;
 	usbh_generic_data_t *lld_data = lld->driver_data;
 	uint8_t *usbh_buffer = lld_data->usbh_buffer;
-	uint8_t state_start = dev->state; // Detection of hang
 //	LOG_PRINTF("\nSTATE: %d\n", state);
 	switch (dev->state) {
-	case 1:
-		{
-			switch (cb_data.status) {
-			case USBH_PACKET_CALLBACK_STATUS_OK:
-				dev->state++;
-				LOG_PRINTF("::%d::", dev->address);
-				device_xfer_control_read(0, 0, device_enumerate, dev);
-				break;
-
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
-				device_enumeration_terminate(dev);
-				ERROR(cb_data.status);
-				break;
-			}
-		}
-		break;
-
-	case 2:
+	case USBH_ENUM_STATE_SET_ADDRESS:
 		switch (cb_data.status) {
 		case USBH_PACKET_CALLBACK_STATUS_OK:
 			if (dev->address == 0) {
 				dev->address = usbh_data.address_temporary;
-				LOG_PRINTF("ADDR: %d\n", dev->address);
+				LOG_PRINTF("Assigned address: %d\n", dev->address);
 			}
-
-			struct usb_setup_data setup_data;
-
-			setup_data.bmRequestType = 0b10000000;
-			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
-			setup_data.wValue = USB_DT_DEVICE << 8;
-			setup_data.wIndex = 0;
-			setup_data.wLength = USB_DT_DEVICE_SIZE;
-
-			dev->state++;
-			device_xfer_control_write_setup(&setup_data, sizeof(setup_data),
-				device_enumerate, dev);
+			CONTINUE_WITH(USBH_ENUM_STATE_DEVICE_DT_READ_SETUP);
 			break;
 
-		case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-		case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-		case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+		default:
 			device_enumeration_terminate(dev);
 			ERROR(cb_data.status);
 			break;
 		}
 		break;
 
-	case 3:
+	case USBH_ENUM_STATE_DEVICE_DT_READ_SETUP:
 		{
-			switch (cb_data.status) {
-			case USBH_PACKET_CALLBACK_STATUS_OK:
-				dev->state++;
-				device_xfer_control_read(&usbh_buffer[0], USB_DT_DEVICE_SIZE,
-					device_enumerate, dev);
-				break;
-
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-				dev->state = 2;
+			struct usb_setup_data setup_data;
 
-				// WARNING: Recursion
-				// .. but should work
-				cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;
-				device_enumerate(dev, cb_data);
-				break;
+			setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_DEVICE;
+			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
+			setup_data.wValue = USB_DT_DEVICE << 8;
+			setup_data.wIndex = 0;
+			setup_data.wLength = USB_DT_DEVICE_SIZE;
 
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
-				device_enumeration_terminate(dev);
-				ERROR(cb_data.status);
-				break;
-			}
+			dev->state = USBH_ENUM_STATE_DEVICE_DT_READ_COMPLETE;
+			device_control(dev, device_enumerate, &setup_data, &usbh_buffer[0]);
 		}
 		break;
 
-	case 4:
+	case USBH_ENUM_STATE_DEVICE_DT_READ_COMPLETE:
 		{
 			switch (cb_data.status) {
 			case USBH_PACKET_CALLBACK_STATUS_OK:
@@ -392,17 +438,9 @@ static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_
 					struct usb_device_descriptor *ddt =
 							(struct usb_device_descriptor *)&usbh_buffer[0];
 					dev->packet_size_max0 = ddt->bMaxPacketSize0;
-					struct usb_setup_data setup_data;
-
-					setup_data.bmRequestType = 0b10000000;
-					setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
-					setup_data.wValue = USB_DT_CONFIGURATION << 8;
-					setup_data.wIndex = 0;
-					setup_data.wLength = ddt->bMaxPacketSize0;//USB_DT_CONFIGURATION_SIZE;
-
-					dev->state++;
-					device_xfer_control_write_setup(&setup_data, sizeof(setup_data),
-						device_enumerate, dev);
+					LOG_PRINTF("Found device with vid=0x%04x pid=0x%04x\n", ddt->idVendor, ddt->idProduct);
+					LOG_PRINTF("class=0x%02x subclass=0x%02x protocol=0x%02x\n", ddt->bDeviceClass, ddt->bDeviceSubClass, ddt->bDeviceProtocol);
+					CONTINUE_WITH(USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ_SETUP)
 				}
 				break;
 
@@ -411,17 +449,14 @@ static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_
 					struct usb_device_descriptor *ddt =
 						(struct usb_device_descriptor *)&usbh_buffer[0];
 					dev->packet_size_max0 = ddt->bMaxPacketSize0;
-					dev->state = 2;
-
-					// WARNING: Recursion
-					// .. but should work
-					cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;
-					device_enumerate(dev, cb_data);
+					CONTINUE_WITH(USBH_ENUM_STATE_DEVICE_DT_READ_SETUP);
+				} else {
+					device_enumeration_terminate(dev);
+					ERROR(cb_data.status);
 				}
 				break;
 
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
+			default:
 				device_enumeration_terminate(dev);
 				ERROR(cb_data.status);
 				break;
@@ -429,18 +464,32 @@ static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_
 		}
 		break;
 
-	case 5:
+	case USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ_SETUP:
+		{
+			struct usb_setup_data setup_data;
+
+			setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_DEVICE;
+			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
+			setup_data.wValue = USB_DT_CONFIGURATION << 8;
+			setup_data.wIndex = 0;
+			setup_data.wLength = dev->packet_size_max0;
+
+			dev->state = USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ;
+			device_xfer_control_write_setup(&setup_data, sizeof(setup_data),
+				device_enumerate, dev);
+		}
+		break;
+
+	case USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ:
 		{
 			switch (cb_data.status) {
 			case USBH_PACKET_CALLBACK_STATUS_OK:
-				dev->state++;
+				dev->state = USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ_COMPLETE;
 				device_xfer_control_read(&usbh_buffer[USB_DT_DEVICE_SIZE],
 					dev->packet_size_max0, device_enumerate, dev);
 				break;
 
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+			default:
 				device_enumeration_terminate(dev);
 				ERROR(cb_data.status);
 				break;
@@ -448,44 +497,25 @@ static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_
 		}
 		break;
 
-	case 6:
+	case USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ_COMPLETE:
 		{
 			switch (cb_data.status) {
 			case USBH_PACKET_CALLBACK_STATUS_OK:
-				{
-					struct usb_config_descriptor *cdt =
-						(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
-					struct usb_setup_data setup_data;
-					LOG_PRINTF("WRITE: LEN: %d", cdt->wTotalLength);
-					setup_data.bmRequestType = 0b10000000;
-					setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
-					setup_data.wValue = USB_DT_CONFIGURATION << 8;
-					setup_data.wIndex = 0;
-					setup_data.wLength = cdt->wTotalLength;
-
-					dev->state++;
-					device_xfer_control_write_setup(&setup_data, sizeof(setup_data),
-						device_enumerate, dev);
-				}
+				CONTINUE_WITH(USBH_ENUM_STATE_CONFIGURATION_DT_READ_SETUP);
 				break;
 
 			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
 				if (cb_data.transferred_length >= USB_DT_CONFIGURATION_SIZE) {
 					struct usb_config_descriptor *cdt =
 						(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
-					if (cb_data.transferred_length <= cdt->wTotalLength) {
-						dev->state = 8;
-
-						// WARNING: Recursion
-						// .. but should work
-						cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;
-						device_enumerate(dev, cb_data);
+					if (cb_data.transferred_length == cdt->wTotalLength) {
+						LOG_PRINTF("Configuration descriptor read complete. length: %d\n", cdt->wTotalLength);
+						CONTINUE_WITH(USBH_ENUM_STATE_SET_CONFIGURATION_SETUP);
 					}
 				}
 				break;
 
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
+			default:
 				device_enumeration_terminate(dev);
 				ERROR(cb_data.status);
 				break;
@@ -493,22 +523,38 @@ static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_
 		}
 		break;
 
-	case 7:
+	case USBH_ENUM_STATE_CONFIGURATION_DT_READ_SETUP:
+		{
+			struct usb_config_descriptor *cdt =
+				(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+			struct usb_setup_data setup_data;
+			LOG_PRINTF("Getting complete configuration descriptor of length: %d bytes\n", cdt->wTotalLength);
+			setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_DEVICE;
+			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
+			setup_data.wValue = USB_DT_CONFIGURATION << 8;
+			setup_data.wIndex = 0;
+			setup_data.wLength = cdt->wTotalLength;
+
+			dev->state = USBH_ENUM_STATE_CONFIGURATION_DT_READ;
+			device_xfer_control_write_setup(&setup_data, sizeof(setup_data),
+				device_enumerate, dev);
+		}
+		break;
+
+	case USBH_ENUM_STATE_CONFIGURATION_DT_READ:
 		{
 			switch (cb_data.status) {
 			case USBH_PACKET_CALLBACK_STATUS_OK:
 				{
 					struct usb_config_descriptor *cdt =
 						(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
-					dev->state++;
+					dev->state = USBH_ENUM_STATE_CONFIGURATION_DT_READ_COMPLETE;
 					device_xfer_control_read(&usbh_buffer[USB_DT_DEVICE_SIZE],
 						cdt->wTotalLength, device_enumerate, dev);
 				}
 				break;
 
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+			default:
 				device_enumeration_terminate(dev);
 				ERROR(cb_data.status);
 				break;
@@ -516,29 +562,68 @@ static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_
 		}
 		break;
 
-	case 8:
+	case USBH_ENUM_STATE_CONFIGURATION_DT_READ_COMPLETE:
 		{
 			switch (cb_data.status) {
 			case USBH_PACKET_CALLBACK_STATUS_OK:
 				{
 					struct usb_config_descriptor *cdt =
 						(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
-					LOG_PRINTF("TOTAL_LENGTH: %d\n", cdt->wTotalLength);
-					device_register(usbh_buffer, cdt->wTotalLength + USB_DT_DEVICE_SIZE, dev);
-					dev->state++;
+					LOG_PRINTF("Configuration descriptor read complete. length: %d\n", cdt->wTotalLength);
+					CONTINUE_WITH(USBH_ENUM_STATE_SET_CONFIGURATION_SETUP);
 
-					reset_enumeration();
 				}
 				break;
 
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+			default:
+				device_enumeration_terminate(dev);
+				ERROR(cb_data.status);
+				break;
+			}
+
+		}
+		break;
+
+	case USBH_ENUM_STATE_SET_CONFIGURATION_SETUP:
+		{
+			struct usb_config_descriptor *cdt =
+				(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+
+			struct usb_setup_data setup_data;
+
+			setup_data.bmRequestType = USB_REQ_TYPE_STANDARD | USB_REQ_TYPE_DEVICE;
+			setup_data.bRequest = USB_REQ_SET_CONFIGURATION;
+			setup_data.wValue = cdt->bConfigurationValue;
+			setup_data.wIndex = 0;
+			setup_data.wLength = 0;
+
+			dev->state = USBH_ENUM_STATE_SET_CONFIGURATION_COMPLETE;
+			device_control(dev, device_enumerate, &setup_data, 0);
+		}
+		break;
+
+	case USBH_ENUM_STATE_SET_CONFIGURATION_COMPLETE:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				CONTINUE_WITH(USBH_ENUM_STATE_FIND_DRIVER);
+				break;
+
+			default:
 				device_enumeration_terminate(dev);
 				ERROR(cb_data.status);
 				break;
 			}
+		}
+		break;
+
+	case USBH_ENUM_STATE_FIND_DRIVER:
+		{
+			struct usb_config_descriptor *cdt =
+				(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+			device_register(usbh_buffer, cdt->wTotalLength + USB_DT_DEVICE_SIZE, dev);
 
+			device_enumeration_finish(dev);
 		}
 		break;
 
@@ -546,16 +631,11 @@ static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_
 		LOG_PRINTF("Error: Unknown state "__FILE__"/%d\n", __LINE__);
 		break;
 	}
-
-	if (state_start == dev->state) {
-		LOG_PRINTF("\n !HANG %d\n", state_start);
-	}
 }
 
 void device_enumeration_start(usbh_device_t *dev)
 {
 	set_enumeration();
-	dev->state = 1;
 
 	// save address
 	uint8_t address = dev->address;
@@ -571,16 +651,16 @@ void device_enumeration_start(usbh_device_t *dev)
 
 	LOG_PRINTF("\n\n\n ENUMERATION OF DEVICE@%d STARTED \n\n", address);
 
+	dev->state = USBH_ENUM_STATE_SET_ADDRESS;
 	struct usb_setup_data setup_data;
 
-	setup_data.bmRequestType = 0b00000000;
+	setup_data.bmRequestType = USB_REQ_TYPE_STANDARD | USB_REQ_TYPE_DEVICE;
 	setup_data.bRequest = USB_REQ_SET_ADDRESS;
 	setup_data.wValue = address;
 	setup_data.wIndex = 0;
 	setup_data.wLength = 0;
 
-	device_xfer_control_write_setup(&setup_data, sizeof(setup_data),
-		device_enumerate, dev);
+	device_control(dev, device_enumerate, &setup_data, 0);
 }
 
 /**
@@ -591,7 +671,7 @@ void usbh_poll(uint32_t time_curr_us)
 {
 	uint32_t k = 0;
 	while (usbh_data.lld_drivers[k]) {
-		usbh_device_t * usbh_device =
+		usbh_device_t *usbh_device =
 			((usbh_generic_data_t *)(usbh_data.lld_drivers[k]->driver_data))->usbh_device;
 		usbh_generic_data_t *lld_data = usbh_data.lld_drivers[k]->driver_data;
 
@@ -605,24 +685,17 @@ void usbh_poll(uint32_t time_curr_us)
 			usbh_device[0].lld = usbh_data.lld_drivers[k];
 			usbh_device[0].speed = usbh_data.lld_drivers[k]->root_speed(lld_data);
 			usbh_device[0].address = 1;
+			usbh_device[0].control.state = USBH_CONTROL_STATE_NONE;
 
 			device_enumeration_start(&usbh_device[0]);
 			break;
 
 		case USBH_POLL_STATUS_DEVICE_DISCONNECTED:
 			{
-				// Device disconnected
-				if (usbh_device[0].drv && usbh_device[0].drvdata) {
-					usbh_device[0].drv->remove(usbh_device[0].drvdata);
-				}
-				usbh_device[0].drv = 0;
-				usbh_device[0].drvdata = 0;
-
+				usbh_device[0].control.state = USBH_CONTROL_STATE_NONE;
 				uint32_t i;
-				for (i = 1; i < USBH_MAX_DEVICES; i++) {
-					usbh_device[i].address = -1;
-					usbh_device[i].drv = 0;
-					usbh_device[i].drvdata = 0;
+				for (i = 0; i < USBH_MAX_DEVICES; i++) {
+					device_remove(&usbh_device[i]);
 				}
 			}
 			break;
diff --git a/src/usbh_driver_ac_midi.c b/src/usbh_driver_ac_midi.c
index 413875f..96cf383 100644
--- a/src/usbh_driver_ac_midi.c
+++ b/src/usbh_driver_ac_midi.c
@@ -24,39 +24,16 @@
 #include "usbh_driver_ac_midi_private.h"
 #include "usart_helpers.h"
 
+#include <stddef.h>
+
 #include <libopencm3/usb/midi.h>
 #include <libopencm3/usb/audio.h>
 #include <libopencm3/usb/usbstd.h>
 
-
-static void *midi_init(void *usbh_dev);
-static bool midi_analyze_descriptor(void *drvdata, void *descriptor);
-static void midi_poll(void *drvdata, uint32_t tflp);
-static void midi_remove(void *drvdata);
-
 static midi_device_t midi_device[USBH_AC_MIDI_MAX_DEVICES];
-static const midi_config_t *midi_config = 0;
+static const midi_config_t *midi_config = NULL;
 static bool initialized = false;
 
-static const usbh_dev_driver_info_t usbh_midi_driver_info = {
-	.deviceClass = -1,
-	.deviceSubClass = -1,
-	.deviceProtocol = -1,
-	.idVendor = -1,
-	.idProduct = -1,
-	.ifaceClass = 0x01,
-	.ifaceSubClass = 0x03,
-	.ifaceProtocol = -1,
-};
-
-const usbh_dev_driver_t usbh_midi_driver = {
-	.init = midi_init,
-	.analyze_descriptor = midi_analyze_descriptor,
-	.poll = midi_poll,
-	.remove = midi_remove,
-	.info = &usbh_midi_driver_info
-};
-
 void midi_driver_init(const midi_config_t *config)
 {
 	uint32_t i;
@@ -70,14 +47,14 @@ void midi_driver_init(const midi_config_t *config)
  *
  *
  */
-static void *midi_init(void *usbh_dev)
+static void *init(usbh_device_t *usbh_dev)
 {
 	if (!midi_config || !initialized) {
 		LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);
 		return 0;
 	}
 	uint32_t i;
-	midi_device_t *drvdata = 0;
+	midi_device_t *drvdata = NULL;
 
 	// find free data space for midi device
 	for (i = 0; i < USBH_AC_MIDI_MAX_DEVICES; i++) {
@@ -89,7 +66,7 @@ static void *midi_init(void *usbh_dev)
 			drvdata->endpoint_in_toggle = 0;
 			drvdata->endpoint_out_toggle = 0;
 			drvdata->usbh_device = usbh_dev;
-			drvdata->write_callback_user = 0;
+			drvdata->write_callback_user = NULL;
 			drvdata->sending = false;
 			break;
 		}
@@ -101,7 +78,7 @@ static void *midi_init(void *usbh_dev)
 /**
  * Returns true if all needed data are parsed
  */
-static bool midi_analyze_descriptor(void *drvdata, void *descriptor)
+static bool analyze_descriptor(void *drvdata, void *descriptor)
 {
 	midi_device_t *midi = drvdata;
 	uint8_t desc_type = ((uint8_t *)descriptor)[1];
@@ -186,12 +163,13 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t status)
 				midi_in_message(midi, midi->endpoint_in_maxpacketsize);
 				midi->state = 25;
 				break;
+
 			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
 				midi_in_message(midi, status.transferred_length);
 				midi->state = 25;
 				break;
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
+
+			default:
 				LOG_PRINTF("FATAL ERROR, MIDI DRIVER DEAD \n");
 				//~ dev->drv->remove();
 				midi->state = 0;
@@ -206,30 +184,7 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t status)
 			LOG_PRINTF("\n CAN'T TOUCH THIS... ignoring data\n");
 		}
 		break;
-	case 2:
-		{
-			LOG_PRINTF("|empty packet read|");
-			if (status.status == USBH_PACKET_CALLBACK_STATUS_OK) {
-				midi->state++;
-				device_xfer_control_read(0, 0, event, dev);
-			}
-		}
-		break;
-	case 3: // Configured
-		{
-			if (status.status == USBH_PACKET_CALLBACK_STATUS_OK) {
-				midi->state = 100;
-
-				midi->endpoint_in_toggle = 0;
-				LOG_PRINTF("\nMIDI CONFIGURED\n");
 
-				// Notify user
-				if (midi_config->notify_connected) {
-					midi_config->notify_connected(midi->device_id);
-				}
-			}
-		}
-		break;
 	default:
 		break;
 	}
@@ -242,7 +197,7 @@ static void read_midi_in(void *drvdata, const uint8_t nextstate)
 	usbh_packet_t packet;
 
 	packet.address = midi->usbh_device->address;
-	packet.data = &midi->buffer[0];
+	packet.data.in = &midi->buffer[0];
 	packet.datalen = midi->endpoint_in_maxpacketsize;
 	packet.endpoint_address = midi->endpoint_in_address;
 	packet.endpoint_size_max = midi->endpoint_in_maxpacketsize;
@@ -260,12 +215,11 @@ static void read_midi_in(void *drvdata, const uint8_t nextstate)
  * 
  *  @param t_us global time us
  */
-static void midi_poll(void *drvdata, uint32_t t_us)
+static void poll(void *drvdata, uint32_t t_us)
 {
 	(void)drvdata;
 
 	midi_device_t *midi = drvdata;
-	usbh_device_t *dev = midi->usbh_device;
 	switch (midi->state) {
 
 	/// Upon configuration, some controllers send additional error data
@@ -276,11 +230,13 @@ static void midi_poll(void *drvdata, uint32_t t_us)
 			midi->state = 101;
 		}
 		break;
+
 	case 101:
 		{
 			read_midi_in(drvdata, 102);
 		}
 		break;
+
 	case 102:
 		{
 			// if elapsed MIDI initial delay microseconds
@@ -289,6 +245,7 @@ static void midi_poll(void *drvdata, uint32_t t_us)
 			}
 		}
 		break;
+
 	case 25:
 		{
 			read_midi_in(drvdata, 26);
@@ -297,18 +254,15 @@ static void midi_poll(void *drvdata, uint32_t t_us)
 
 	case 1:
 		{
-			//~ LOG_PRINTF("CFGVAL: %d\n", dev->config_val);
-			struct usb_setup_data setup_data;
-
-			setup_data.bmRequestType = 0b00000000;
-			setup_data.bRequest = USB_REQ_SET_CONFIGURATION;
-			setup_data.wValue = midi->buffer[0];
-			setup_data.wIndex = 0;
-			setup_data.wLength = 0;
+			midi->state = 100;
 
-			midi->state++;
+			midi->endpoint_in_toggle = 0;
+			LOG_PRINTF("\nMIDI CONFIGURED\n");
 
-			device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);
+			// Notify user
+			if (midi_config->notify_connected) {
+				midi_config->notify_connected(midi->device_id);
+			}
 		}
 		break;
 	}
@@ -362,7 +316,7 @@ void usbh_midi_write(uint8_t device_id, const void *data, uint32_t length, midi_
 	midi->sending = true;
 	midi->write_callback_user = callback;
 
-	midi->write_packet.data = (void*)data;	// it is safe cast since we are writing and function usbh_write cannot modify data
+	midi->write_packet.data.out = data;
 	midi->write_packet.datalen = length;
 	midi->write_packet.address = dev->address;
 	midi->write_packet.endpoint_address = midi->endpoint_out_address;
@@ -377,7 +331,7 @@ void usbh_midi_write(uint8_t device_id, const void *data, uint32_t length, midi_
 	usbh_write(dev, &midi->write_packet);
 }
 
-static void midi_remove(void *drvdata)
+static void remove(void *drvdata)
 {
 	midi_device_t *midi = drvdata;
 
@@ -389,3 +343,22 @@ static void midi_remove(void *drvdata)
 	midi->endpoint_in_address = 0;
 	midi->endpoint_out_address = 0;
 }
+
+static const usbh_dev_driver_info_t usbh_midi_driver_info = {
+	.deviceClass = -1,
+	.deviceSubClass = -1,
+	.deviceProtocol = -1,
+	.idVendor = -1,
+	.idProduct = -1,
+	.ifaceClass = 0x01,
+	.ifaceSubClass = 0x03,
+	.ifaceProtocol = -1,
+};
+
+const usbh_dev_driver_t usbh_midi_driver = {
+	.init = init,
+	.analyze_descriptor = analyze_descriptor,
+	.poll = poll,
+	.remove = remove,
+	.info = &usbh_midi_driver_info
+};
diff --git a/src/usbh_driver_gp_xbox.c b/src/usbh_driver_gp_xbox.c
index 01cebb5..957cb3e 100644
--- a/src/usbh_driver_gp_xbox.c
+++ b/src/usbh_driver_gp_xbox.c
@@ -30,11 +30,9 @@
 
 enum STATES {
 	STATE_INACTIVE,
-	STATE_READING_COMPLETE,
+	STATE_INITIAL,
 	STATE_READING_REQUEST,
-	STATE_SET_CONFIGURATION_REQUEST,
-	STATE_SET_CONFIGURATION_EMPTY_READ,
-	STATE_SET_CONFIGURATION_COMPLETE
+	STATE_READING_COMPLETE,
 };
 
 #define GP_XBOX_CORRECT_TRANSFERRED_LENGTH 20
@@ -74,7 +72,7 @@ void gp_xbox_driver_init(const gp_xbox_config_t *config)
  *
  *
  */
-static void *init(void *usbh_dev)
+static void *init(usbh_device_t *usbh_dev)
 {
 	if (!initialized) {
 		LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);
@@ -91,7 +89,7 @@ static void *init(void *usbh_dev)
 			drvdata->device_id = i;
 			drvdata->endpoint_in_address = 0;
 			drvdata->endpoint_in_toggle = 0;
-			drvdata->usbh_device = (usbh_device_t *)usbh_dev;
+			drvdata->usbh_device = usbh_dev;
 			break;
 		}
 	}
@@ -132,7 +130,7 @@ static bool analyze_descriptor(void *drvdata, void *descriptor)
 				}
 
 				if (gp_xbox->endpoint_in_address) {
-					gp_xbox->state_next = STATE_SET_CONFIGURATION_REQUEST;
+					gp_xbox->state_next = STATE_INITIAL;
 					return true;
 				}
 			}
@@ -259,47 +257,7 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 				gp_xbox->state_next = STATE_READING_REQUEST;
 				break;
 
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-				ERROR(cb_data.status);
-				gp_xbox->state_next = STATE_INACTIVE;
-				break;
-			}
-		}
-		break;
-
-	case STATE_SET_CONFIGURATION_EMPTY_READ:
-		{
-			LOG_PRINTF("|empty packet read|");
-			switch (cb_data.status) {
-			case USBH_PACKET_CALLBACK_STATUS_OK:
-				gp_xbox->state_next = STATE_SET_CONFIGURATION_COMPLETE;
-				device_xfer_control_read(0, 0, event, dev);
-				break;
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
-				ERROR(cb_data.status);
-				gp_xbox->state_next = STATE_INACTIVE;
-				break;
-			}
-		}
-		break;
-	case STATE_SET_CONFIGURATION_COMPLETE: // Configured
-		{
-			switch (cb_data.status) {
-			case USBH_PACKET_CALLBACK_STATUS_OK:
-				gp_xbox->state_next = STATE_READING_REQUEST;
-				gp_xbox->endpoint_in_toggle = 0;
-				LOG_PRINTF("\ngp_xbox CONFIGURED\n");
-				if (gp_xbox_config->notify_connected) {
-					gp_xbox_config->notify_connected(gp_xbox->device_id);
-				}
-				break;
-
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+			default:
 				ERROR(cb_data.status);
 				gp_xbox->state_next = STATE_INACTIVE;
 				break;
@@ -326,7 +284,7 @@ static void read_gp_xbox_in(gp_xbox_device_t *gp_xbox)
 	usbh_packet_t packet;
 
 	packet.address = gp_xbox->usbh_device->address;
-	packet.data = &gp_xbox->buffer[0];
+	packet.data.in = &gp_xbox->buffer[0];
 	packet.datalen = gp_xbox->endpoint_in_maxpacketsize;
 	packet.endpoint_address = gp_xbox->endpoint_in_address;
 	packet.endpoint_size_max = gp_xbox->endpoint_in_maxpacketsize;
@@ -351,7 +309,6 @@ static void poll(void *drvdata, uint32_t time_curr_us)
 	(void)time_curr_us;
 
 	gp_xbox_device_t *gp_xbox = (gp_xbox_device_t *)drvdata;
-	usbh_device_t *dev = gp_xbox->usbh_device;
 
 	switch (gp_xbox->state_next) {
 	case STATE_READING_REQUEST:
@@ -360,19 +317,14 @@ static void poll(void *drvdata, uint32_t time_curr_us)
 		}
 		break;
 
-	case STATE_SET_CONFIGURATION_REQUEST:
+	case STATE_INITIAL:
 		{
-			struct usb_setup_data setup_data;
-
-			setup_data.bmRequestType = 0b00000000;
-			setup_data.bRequest = USB_REQ_SET_CONFIGURATION;
-			setup_data.wValue = gp_xbox->configuration_value;
-			setup_data.wIndex = 0;
-			setup_data.wLength = 0;
-
-			gp_xbox->state_next = STATE_SET_CONFIGURATION_EMPTY_READ;
-
-			device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);
+			gp_xbox->state_next = STATE_READING_REQUEST;
+			gp_xbox->endpoint_in_toggle = 0;
+			LOG_PRINTF("\ngp_xbox CONFIGURED\n");
+			if (gp_xbox_config->notify_connected) {
+				gp_xbox_config->notify_connected(gp_xbox->device_id);
+			}
 		}
 		break;
 
diff --git a/src/usbh_driver_hid.c b/src/usbh_driver_hid.c
new file mode 100644
index 0000000..bb842db
--- /dev/null
+++ b/src/usbh_driver_hid.c
@@ -0,0 +1,412 @@
+/*
+ * This file is part of the libusbhost library
+ * hosted at http://github.com/libusbhost/libusbhost
+ *
+ * Copyright (C) 2016 Amir Hammad <amir.hammad@hotmail.com>
+ *
+ *
+ * libusbhost is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "usbh_core.h"
+#include "driver/usbh_device_driver.h"
+#include "usbh_driver_hid.h"
+#include "usart_helpers.h"
+
+#include <libopencm3/usb/usbstd.h>
+#include <libopencm3/usb/hid.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#define USB_HID_SET_REPORT 0x09
+#define USB_HID_SET_IDLE 0x0A
+
+enum STATES {
+	STATE_INACTIVE,
+	STATE_READING_REQUEST,
+	STATE_READING_COMPLETE_AND_CHECK_REPORT,
+	STATE_SET_REPORT_EMPTY_READ,
+	STATE_GET_REPORT_DESCRIPTOR_READ_SETUP,// configuration is complete at this point. We write request
+	STATE_GET_REPORT_DESCRIPTOR_READ_COMPLETE,// after the read finishes, we parse that descriptor
+	STATE_SET_IDLE,
+	STATE_SET_IDLE_COMPLETE,
+};
+
+enum REPORT_STATE {
+	REPORT_STATE_NULL,
+	REPORT_STATE_READY,
+	REPORT_STATE_PENDING,
+};
+
+struct _hid_device {
+	usbh_device_t *usbh_device;
+	uint8_t buffer[USBH_HID_BUFFER];
+	uint16_t endpoint_in_maxpacketsize;
+	uint8_t endpoint_in_address;
+	enum STATES state_next;
+	uint8_t endpoint_in_toggle;
+	uint8_t device_id;
+	uint8_t configuration_value;
+	uint16_t report0_length;
+	enum REPORT_STATE report_state;
+	uint8_t report_data[USBH_HID_REPORT_BUFFER];
+	uint8_t report_data_length;
+	enum HID_TYPE hid_type;
+	uint8_t interface_number;
+};
+typedef struct _hid_device hid_device_t;
+
+struct hid_report_decriptor {
+	struct usb_hid_descriptor header;
+	struct _report_descriptor_info {
+		uint8_t bDescriptorType;
+		uint16_t wDescriptorLength;
+	} __attribute__((packed)) report_descriptors_info[];
+} __attribute__((packed));
+
+static hid_device_t hid_device[USBH_HID_MAX_DEVICES];
+static hid_config_t hid_config;
+
+static bool initialized = false;
+
+void hid_driver_init(const hid_config_t *config)
+{
+	uint32_t i;
+
+	initialized = true;
+
+	hid_config = *config;
+	for (i = 0; i < USBH_HID_MAX_DEVICES; i++) {
+		hid_device[i].state_next = STATE_INACTIVE;
+	}
+}
+
+static void *init(usbh_device_t *usbh_dev)
+{
+	if (!initialized) {
+		LOG_PRINTF("\n%s/%d : driver not initialized\r\n", __FILE__, __LINE__);
+		return 0;
+	}
+
+	uint32_t i;
+	hid_device_t *drvdata = NULL;
+
+	// find free data space for HID device
+	for (i = 0; i < USBH_HID_MAX_DEVICES; i++) {
+		if (hid_device[i].state_next == STATE_INACTIVE) {
+			drvdata = &hid_device[i];
+			drvdata->device_id = i;
+			drvdata->endpoint_in_address = 0;
+			drvdata->endpoint_in_toggle = 0;
+			drvdata->report0_length = 0;
+			drvdata->usbh_device = usbh_dev;
+			drvdata->report_state = REPORT_STATE_NULL;
+			drvdata->hid_type = HID_TYPE_NONE;
+			break;
+		}
+	}
+
+	return drvdata;
+}
+
+static void parse_report_descriptor(hid_device_t *hid, const uint8_t *buffer, uint32_t length)
+{
+	// TODO
+	// Do some parsing!
+	// add some checks
+	hid->report_state = REPORT_STATE_READY;
+
+	// TODO: parse this from buffer!
+	hid->report_data_length = 1;
+	(void)buffer;
+	(void)length;
+}
+
+/**
+ * Returns true if all needed data are parsed
+ */
+static bool analyze_descriptor(void *drvdata, void *descriptor)
+{
+	hid_device_t *hid = (hid_device_t *)drvdata;
+	uint8_t desc_type = ((uint8_t *)descriptor)[1];
+	switch (desc_type) {
+	case USB_DT_CONFIGURATION:
+		{
+			const struct usb_config_descriptor * cfg = (const struct usb_config_descriptor*)descriptor;
+			hid->configuration_value = cfg->bConfigurationValue;
+		}
+		break;
+
+	case USB_DT_DEVICE:
+		{
+			const struct usb_device_descriptor *devDesc = (const struct usb_device_descriptor *)descriptor;
+			(void)devDesc;
+		}
+		break;
+
+	case USB_DT_INTERFACE:
+		{
+			const struct usb_interface_descriptor *ifDesc = (const struct usb_interface_descriptor *)descriptor;
+			if (ifDesc->bInterfaceProtocol == 0x01) {
+				hid->hid_type = HID_TYPE_KEYBOARD;
+				hid->interface_number = ifDesc->bInterfaceNumber;
+			} else if (ifDesc->bInterfaceProtocol == 0x02) {
+				hid->hid_type = HID_TYPE_MOUSE;
+				hid->interface_number = ifDesc->bInterfaceNumber;
+			}
+		}
+		break;
+
+	case USB_DT_ENDPOINT:
+		{
+			const struct usb_endpoint_descriptor *ep = (const struct usb_endpoint_descriptor *)descriptor;
+			if ((ep->bmAttributes&0x03) == USB_ENDPOINT_ATTR_INTERRUPT) {
+				uint8_t epaddr = ep->bEndpointAddress;
+				if (epaddr & (1<<7)) {
+					hid->endpoint_in_address = epaddr&0x7f;
+					if (ep->wMaxPacketSize < USBH_HID_BUFFER) {
+						hid->endpoint_in_maxpacketsize = ep->wMaxPacketSize;
+					} else {
+						hid->endpoint_in_maxpacketsize = USBH_HID_BUFFER;
+					}
+				}
+			}
+		}
+		break;
+
+	case USB_DT_HID:
+		{
+			const struct hid_report_decriptor *desc = (const struct hid_report_decriptor *)descriptor;
+			if (desc->header.bNumDescriptors > 0 && desc->report_descriptors_info[0].bDescriptorType == USB_DT_REPORT) {
+				hid->report0_length = desc->report_descriptors_info[0].wDescriptorLength;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	if (hid->endpoint_in_address && hid->report0_length) {
+		hid->state_next = STATE_GET_REPORT_DESCRIPTOR_READ_SETUP;
+		return true;
+	}
+
+	return false;
+}
+
+static void report_event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
+{
+	(void)cb_data;// UNUSED
+
+	hid_device_t *hid = (hid_device_t *)dev->drvdata;
+	hid->report_state = REPORT_STATE_READY;
+}
+
+static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
+{
+	hid_device_t *hid = (hid_device_t *)dev->drvdata;
+
+	switch (hid->state_next) {
+	case STATE_READING_COMPLETE_AND_CHECK_REPORT:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+				if (hid_config.hid_in_message_handler) {
+					hid_config.hid_in_message_handler(hid->device_id, hid->buffer, cb_data.transferred_length);
+				}
+				hid->state_next = STATE_READING_REQUEST;
+				break;
+
+			default:
+				ERROR(cb_data.status);
+				hid->state_next = STATE_INACTIVE;
+				break;
+			}
+		}
+		break;
+
+	case STATE_GET_REPORT_DESCRIPTOR_READ_COMPLETE: // read complete, SET_IDLE to 0
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				LOG_PRINTF("READ REPORT COMPLETE \n");
+				hid->state_next = STATE_READING_REQUEST;
+				hid->endpoint_in_toggle = 0;
+
+				parse_report_descriptor(hid, hid->buffer, cb_data.transferred_length);
+				break;
+
+			default:
+				ERROR(cb_data.status);
+				hid->state_next = STATE_INACTIVE;
+				break;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+static void read_hid_in_endpoint(void *drvdata)
+{
+	hid_device_t *hid = (hid_device_t *)drvdata;
+	usbh_packet_t packet;
+
+	packet.address = hid->usbh_device->address;
+	packet.data.in = &hid->buffer[0];
+	packet.datalen = hid->endpoint_in_maxpacketsize;
+	packet.endpoint_address = hid->endpoint_in_address;
+	packet.endpoint_size_max = hid->endpoint_in_maxpacketsize;
+	packet.endpoint_type = USBH_ENDPOINT_TYPE_INTERRUPT;
+	packet.speed = hid->usbh_device->speed;
+	packet.callback = event;
+	packet.callback_arg = hid->usbh_device;
+	packet.toggle = &hid->endpoint_in_toggle;
+
+	hid->state_next = STATE_READING_COMPLETE_AND_CHECK_REPORT;
+	usbh_read(hid->usbh_device, &packet);
+}
+
+/**
+ * @param time_curr_us - monotically rising time
+ *		unit is microseconds
+ * @see usbh_poll()
+ */
+static void poll(void *drvdata, uint32_t time_curr_us)
+{
+	(void)time_curr_us;
+
+	hid_device_t *hid = (hid_device_t *)drvdata;
+	usbh_device_t *dev = hid->usbh_device;
+	switch (hid->state_next) {
+	case STATE_READING_REQUEST:
+		{
+			read_hid_in_endpoint(drvdata);
+		}
+		break;
+
+	case STATE_GET_REPORT_DESCRIPTOR_READ_SETUP:
+		{
+			hid->endpoint_in_toggle = 0;
+			// We support only the first report descriptor with index 0
+
+			// limit the size of the report descriptor!
+			if (hid->report0_length > USBH_HID_BUFFER) {
+				hid->report0_length = USBH_HID_BUFFER;
+			}
+
+			struct usb_setup_data setup_data;
+
+			setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_INTERFACE;
+			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
+			setup_data.wValue = USB_DT_REPORT << 8;
+			setup_data.wIndex = 0;
+			setup_data.wLength = hid->report0_length;
+
+			hid->state_next = STATE_GET_REPORT_DESCRIPTOR_READ_COMPLETE;
+			device_control(dev, event, &setup_data, hid->buffer);
+		}
+		break;
+
+	default:
+		// do nothing - probably transfer is in progress
+		break;
+	}
+}
+
+static void remove(void *drvdata)
+{
+	hid_device_t *hid = (hid_device_t *)drvdata;
+	hid->state_next = STATE_INACTIVE;
+	hid->endpoint_in_address = 0;
+}
+
+bool hid_set_report(uint8_t device_id, uint8_t val)
+{
+	if (device_id >= USBH_HID_MAX_DEVICES) {
+		LOG_PRINTF("invalid device id");
+		return false;
+	}
+
+	hid_device_t *hid = &hid_device[device_id];
+	if (hid->report_state != REPORT_STATE_READY) {
+		LOG_PRINTF("reporting is not ready\n");
+		// store and update afterwards
+		return false;
+	}
+
+	if (hid->report_data_length == 0) {
+		LOG_PRINTF("reporting is not available (report len=0)\n");
+		return false;
+	}
+
+	struct usb_setup_data setup_data;
+	setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE;
+	setup_data.bRequest = USB_HID_SET_REPORT;
+	setup_data.wValue = 0x02 << 8;
+	setup_data.wIndex = hid->interface_number;
+	setup_data.wLength = hid->report_data_length;
+
+	hid->report_data[0] = val;
+
+	hid->report_state = REPORT_STATE_PENDING;
+	device_control(hid->usbh_device, report_event, &setup_data, &hid->report_data);
+	return true;
+}
+
+bool hid_is_connected(uint8_t device_id)
+{
+	if (device_id >= USBH_HID_MAX_DEVICES) {
+		LOG_PRINTF("is connected: invalid device id");
+		return false;
+	}
+	return hid_device[device_id].state_next == STATE_INACTIVE;
+}
+
+
+enum HID_TYPE hid_get_type(uint8_t device_id)
+{
+	if (hid_is_connected(device_id)) {
+		return HID_TYPE_NONE;
+	}
+	return hid_device[device_id].hid_type;
+}
+
+static const usbh_dev_driver_info_t driver_info = {
+	.deviceClass = -1,
+	.deviceSubClass = -1,
+	.deviceProtocol = -1,
+	.idVendor = -1,
+	.idProduct = -1,
+	.ifaceClass = 0x03, // HID class
+	.ifaceSubClass = -1,
+	.ifaceProtocol = -1, // Do not care
+};
+
+const usbh_dev_driver_t usbh_hid_driver = {
+	.init = init,
+	.analyze_descriptor = analyze_descriptor,
+	.poll = poll,
+	.remove = remove,
+	.info = &driver_info
+};
+
+
+
diff --git a/src/usbh_driver_hid_mouse.c b/src/usbh_driver_hid_mouse.c
deleted file mode 100644
index 9d438a9..0000000
--- a/src/usbh_driver_hid_mouse.c
+++ /dev/null
@@ -1,298 +0,0 @@
-/*
- * This file is part of the libusbhost library
- * hosted at http://github.com/libusbhost/libusbhost
- *
- * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>
- *
- *
- * libusbhost is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This library is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with this library.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-#include "usbh_core.h"
-#include "driver/usbh_device_driver.h"
-#include "usbh_driver_hid_mouse.h"
-#include "usart_helpers.h"
-
-#include <libopencm3/usb/usbstd.h>
-
-enum STATES {
-	STATE_INACTIVE,
-	STATE_READING_COMPLETE,
-	STATE_READING_REQUEST,
-	STATE_SET_CONFIGURATION_REQUEST,
-	STATE_SET_CONFIGURATION_EMPTY_READ,
-	STATE_SET_CONFIGURATION_COMPLETE
-};
-
-struct _hid_mouse_device {
-	usbh_device_t *usbh_device;
-	uint8_t buffer[USBH_HID_MOUSE_BUFFER];
-	uint16_t endpoint_in_maxpacketsize;
-	uint8_t endpoint_in_address;
-	enum STATES state_next;
-	uint8_t endpoint_in_toggle;
-	uint8_t device_id;
-	uint8_t configuration_value;
-};
-typedef struct _hid_mouse_device hid_mouse_device_t;
-
-static hid_mouse_device_t mouse_device[USBH_HID_MOUSE_MAX_DEVICES];
-static const hid_mouse_config_t *mouse_config;
-
-
-
-
-#include <stdint.h>
-
-static bool initialized = false;
-
-void hid_mouse_driver_init(const hid_mouse_config_t *config)
-{
-	uint32_t i;
-
-	initialized = true;
-
-	mouse_config = config;
-	for (i = 0; i < USBH_HID_MOUSE_MAX_DEVICES; i++) {
-		mouse_device[i].state_next = STATE_INACTIVE;
-	}
-}
-
-/**
- *
- *
- */
-static void *init(void *usbh_dev)
-{
-	if (!initialized) {
-		LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);
-		return 0;
-	}
-
-	uint32_t i;
-	hid_mouse_device_t *drvdata = 0;
-
-	// find free data space for mouse device
-	for (i = 0; i < USBH_HID_MOUSE_MAX_DEVICES; i++) {
-		if (mouse_device[i].state_next == STATE_INACTIVE) {
-			drvdata = &mouse_device[i];
-			drvdata->device_id = i;
-			drvdata->endpoint_in_address = 0;
-			drvdata->endpoint_in_toggle = 0;
-			drvdata->usbh_device = (usbh_device_t *)usbh_dev;
-			break;
-		}
-	}
-
-	return drvdata;
-}
-
-/**
- * Returns true if all needed data are parsed
- */
-static bool analyze_descriptor(void *drvdata, void *descriptor)
-{
-	hid_mouse_device_t *mouse = (hid_mouse_device_t *)drvdata;
-	uint8_t desc_type = ((uint8_t *)descriptor)[1];
-	switch (desc_type) {
-	case USB_DT_CONFIGURATION:
-		{
-			struct usb_config_descriptor *cfg = (struct usb_config_descriptor*)descriptor;
-			mouse->configuration_value = cfg->bConfigurationValue;
-		}
-		break;
-	case USB_DT_DEVICE:
-		break;
-	case USB_DT_INTERFACE:
-		break;
-	case USB_DT_ENDPOINT:
-		{
-			struct usb_endpoint_descriptor *ep = (struct usb_endpoint_descriptor*)descriptor;
-			if ((ep->bmAttributes&0x03) == USB_ENDPOINT_ATTR_INTERRUPT) {
-				uint8_t epaddr = ep->bEndpointAddress;
-				if (epaddr & (1<<7)) {
-					mouse->endpoint_in_address = epaddr&0x7f;
-					if (ep->wMaxPacketSize < USBH_HID_MOUSE_BUFFER) {
-						mouse->endpoint_in_maxpacketsize = ep->wMaxPacketSize;
-					} else {
-						mouse->endpoint_in_maxpacketsize = USBH_HID_MOUSE_BUFFER;
-					}
-				}
-
-				if (mouse->endpoint_in_address) {
-					mouse->state_next = STATE_SET_CONFIGURATION_REQUEST;
-					return true;
-				}
-			}
-		}
-		break;
-	// TODO Class Specific descriptors
-	default:
-		break;
-	}
-	return false;
-}
-
-static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
-{
-	hid_mouse_device_t *mouse = (hid_mouse_device_t *)dev->drvdata;
-	switch (mouse->state_next) {
-	case STATE_READING_COMPLETE:
-		{
-			switch (cb_data.status) {
-			case USBH_PACKET_CALLBACK_STATUS_OK:
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
-				mouse->state_next = STATE_READING_REQUEST;
-				break;
-
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-				ERROR(cb_data.status);
-				mouse->state_next = STATE_INACTIVE;
-				break;
-			}
-		}
-		break;
-
-	case STATE_SET_CONFIGURATION_EMPTY_READ:
-		{
-			LOG_PRINTF("|empty packet read|");
-			switch (cb_data.status) {
-			case USBH_PACKET_CALLBACK_STATUS_OK:
-				mouse->state_next = STATE_SET_CONFIGURATION_COMPLETE;
-				device_xfer_control_read(0, 0, event, dev);
-				break;
-
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-				ERROR(cb_data.status);
-				mouse->state_next = STATE_INACTIVE;
-				break;
-			}
-		}
-		break;
-	case STATE_SET_CONFIGURATION_COMPLETE: // Configured
-		{
-			switch (cb_data.status) {
-			case USBH_PACKET_CALLBACK_STATUS_OK:
-				mouse->state_next = STATE_READING_REQUEST;
-				mouse->endpoint_in_toggle = 0;
-				LOG_PRINTF("\nMOUSE CONFIGURED\n");
-				break;
-
-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:
-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:
-				ERROR(cb_data.status);
-				mouse->state_next = STATE_INACTIVE;
-				break;
-			}
-		}
-		break;
-	default:
-		break;
-	}
-}
-
-
-static void read_mouse_in(void *drvdata)
-{
-	hid_mouse_device_t *mouse = (hid_mouse_device_t *)drvdata;
-	usbh_packet_t packet;
-
-	packet.address = mouse->usbh_device->address;
-	packet.data = &mouse->buffer[0];
-	packet.datalen = mouse->endpoint_in_maxpacketsize;
-	packet.endpoint_address = mouse->endpoint_in_address;
-	packet.endpoint_size_max = mouse->endpoint_in_maxpacketsize;
-	packet.endpoint_type = USBH_ENDPOINT_TYPE_INTERRUPT;
-	packet.speed = mouse->usbh_device->speed;
-	packet.callback = event;
-	packet.callback_arg = mouse->usbh_device;
-	packet.toggle = &mouse->endpoint_in_toggle;
-
-	mouse->state_next = STATE_READING_COMPLETE;
-	usbh_read(mouse->usbh_device, &packet);
-
-	// LOG_PRINTF("@MOUSE EP1 |  \n");
-
-}
-
-/**
- * @param time_curr_us - monotically rising time
- *		unit is microseconds
- * @see usbh_poll()
- */
-static void poll(void *drvdata, uint32_t time_curr_us)
-{
-	(void)time_curr_us;
-
-	hid_mouse_device_t *mouse = (hid_mouse_device_t *)drvdata;
-	usbh_device_t *dev = mouse->usbh_device;
-	switch (mouse->state_next) {
-	case STATE_READING_REQUEST:
-		{
-			read_mouse_in(drvdata);
-		}
-		break;
-
-	case STATE_SET_CONFIGURATION_REQUEST:
-		{
-			struct usb_setup_data setup_data;
-
-			setup_data.bmRequestType = 0b00000000;
-			setup_data.bRequest = USB_REQ_SET_CONFIGURATION;
-			setup_data.wValue = mouse->configuration_value;
-			setup_data.wIndex = 0;
-			setup_data.wLength = 0;
-
-			mouse->state_next = STATE_SET_CONFIGURATION_EMPTY_READ;
-
-			device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);
-		}
-		break;
-
-	default:
-		// do nothing - probably transfer is in progress
-		break;
-	}
-}
-
-static void remove(void *drvdata)
-{
-	hid_mouse_device_t *mouse = (hid_mouse_device_t *)drvdata;
-	mouse->state_next = STATE_INACTIVE;
-	mouse->endpoint_in_address = 0;
-}
-
-static const usbh_dev_driver_info_t driver_info = {
-	.deviceClass = -1,
-	.deviceSubClass = -1,
-	.deviceProtocol = -1,
-	.idVendor = -1,
-	.idProduct = -1,
-	.ifaceClass = 0x03,
-	.ifaceSubClass = -1,
-	.ifaceProtocol = 0x02
-};
-
-const usbh_dev_driver_t usbh_hid_mouse_driver = {
-	.init = init,
-	.analyze_descriptor = analyze_descriptor,
-	.poll = poll,
-	.remove = remove,
-	.info = &driver_info
-};
diff --git a/src/usbh_driver_hub.c b/src/usbh_driver_hub.c
index a7448f5..3f51c68 100644
--- a/src/usbh_driver_hub.c
+++ b/src/usbh_driver_hub.c
@@ -25,9 +25,9 @@
 #include "usart_helpers.h"

 #include "usbh_config.h"

 

+#include <stddef.h>

 #include <stdint.h>

 

-

 static hub_device_t hub_device[USBH_MAX_HUBS];

 

 static bool initialized = false;

@@ -41,11 +41,11 @@ void hub_driver_init(void)
 	for (i = 0; i < USBH_MAX_HUBS; i++) {

 		hub_device[i].device[0] = 0;

 		hub_device[i].ports_num = 0;

-		hub_device[i].current_port = -1;

+		hub_device[i].current_port = CURRENT_PORT_NONE;

 	}

 }

 

-static void *init(void *usbh_dev)

+static void *init(usbh_device_t *usbh_dev)

 {

 	if (!initialized) {

 		LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);

@@ -53,24 +53,23 @@ static void *init(void *usbh_dev)
 	}

 

 	uint32_t i;

-	hub_device_t *drvdata = 0;

+	hub_device_t *drvdata = NULL;

 	// find free data space for hub device

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

 		if (hub_device[i].device[0] == 0) {

 			break;

 		}

 	}

-	LOG_PRINTF("%{%d}",i);

-    LOG_FLUSH();

+	LOG_PRINTF("{%d}",i);

 	if (i == USBH_MAX_HUBS) {

-		LOG_PRINTF("ERRRRRRR");

+		LOG_PRINTF("Unable to initialize hub driver");

 		return 0;

 	}

 

 	drvdata = &hub_device[i];

-	drvdata->state = 0;

+	drvdata->state = EVENT_STATE_NONE;

 	drvdata->ports_num = 0;

-	drvdata->device[0] = (usbh_device_t *)usbh_dev;

+	drvdata->device[0] = usbh_dev;

 	drvdata->busy = 0;

 	drvdata->endpoint_in_address = 0;

 	drvdata->endpoint_in_maxpacketsize = 0;

@@ -86,13 +85,6 @@ static bool analyze_descriptor(void *drvdata, void *descriptor)
 	hub_device_t *hub = (hub_device_t *)drvdata;

 	uint8_t desc_type = ((uint8_t *)descriptor)[1];

 	switch (desc_type) {

-	case USB_DT_CONFIGURATION:

-		{

-			struct usb_config_descriptor *cfg = (struct usb_config_descriptor*)descriptor;

-			hub->buffer[0] = cfg->bConfigurationValue;

-		}

-		break;

-

 	case USB_DT_ENDPOINT:

 		{

 			struct usb_endpoint_descriptor *ep = (struct usb_endpoint_descriptor *)descriptor;

@@ -110,7 +102,6 @@ static bool analyze_descriptor(void *drvdata, void *descriptor)
 	case USB_DT_HUB:

 		{

 			struct usb_hub_descriptor *desc = (struct usb_hub_descriptor *)descriptor;

-			//~ hub->ports_num = desc->head.bNbrPorts;

 			if ( desc->head.bNbrPorts <= USBH_HUB_MAX_DEVICES) {

 				hub->ports_num = desc->head.bNbrPorts;

 			} else {

@@ -127,7 +118,7 @@ static bool analyze_descriptor(void *drvdata, void *descriptor)
 	}

 

 	if (hub->endpoint_in_address) {

-		hub->state = 1;

+		hub->state = EVENT_STATE_INITIAL;

 		LOG_PRINTF("end enum");

 		return true;

 	}

@@ -141,7 +132,7 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 

 	LOG_PRINTF("\nHUB->STATE = %d\n", hub->state);

 	switch (hub->state) {

-	case 26:

+	case EVENT_STATE_POLL:

 		switch (cb_data.status) {

 		case USBH_PACKET_CALLBACK_STATUS_OK:

 			{

@@ -157,7 +148,7 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 				// Driver error... port not found

 				if (!psc) {

 					// Continue reading status change endpoint

-					hub->state = 25;

+					hub->state = EVENT_STATE_POLL_REQ;

 					break;

 				}

 

@@ -172,119 +163,45 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 				if (hub->current_port >= 1) {

 					if (hub->current_port != port) {

 						LOG_PRINTF("X");

-						hub->state = 25;

+						hub->state = EVENT_STATE_POLL_REQ;

 						break;

 					}

 				}

 				struct usb_setup_data setup_data;

 				// If regular port event, else hub event

 				if (port) {

-					setup_data.bmRequestType = 0b10100011;

+					setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

 				} else {

-					setup_data.bmRequestType = 0b10100000;

+					setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_DEVICE;

 				}

 

 				setup_data.bRequest = USB_REQ_GET_STATUS;

 				setup_data.wValue = 0;

 				setup_data.wIndex = port;

 				setup_data.wLength = 4;

-				hub->state = 31;

+				hub->state = EVENT_STATE_GET_STATUS_COMPLETE;

 

 				hub->current_port = port;

 				LOG_PRINTF("\n\nPORT FOUND: %d\n", port);

-				device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+				device_control(dev, event, &setup_data, &hub->hub_and_port_status[port]);

 			}

 			break;

 

-		case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-		case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

+		default:

 			ERROR(cb_data.status);

-			hub->state = 0;

+			hub->state = EVENT_STATE_NONE;

 			break;

 

 		case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

 

 			// In case of EAGAIN error, retry read on status endpoint

-			hub->state = 25;

+			hub->state = EVENT_STATE_POLL_REQ;

 			LOG_PRINTF("HUB: Retrying...\n");

 			break;

 		}

 		break;

 

-	case EMPTY_PACKET_READ_STATE:

-		{

-			LOG_PRINTF("|empty packet read|");

-			switch (cb_data.status) {

-			case USBH_PACKET_CALLBACK_STATUS_OK:

-				device_xfer_control_read(0, 0, event, dev);

-				hub->state = hub->state_after_empty_read;

-				hub->state_after_empty_read = 0;

-				break;

-

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

-				hub->state = hub->state_after_empty_read;

-				event(dev, cb_data);

-				break;

-			}

-		}

-		break;

-

-	case 3: // Get HUB Descriptor write

-		{

-			switch (cb_data.status) {

-			case USBH_PACKET_CALLBACK_STATUS_OK:

-				if (hub->ports_num) {

-					hub->index = 0;

-					hub->state = 6;

-					LOG_PRINTF("No need to get HUB DESC\n");

-					event(dev, cb_data);

-				} else {

-					hub->endpoint_in_toggle = 0;

-

-					struct usb_setup_data setup_data;

-					hub->desc_len = hub->device[0]->packet_size_max0;

-

-					setup_data.bmRequestType = 0b10100000;

-					setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;

-					setup_data.wValue = 0x29<<8;

-					setup_data.wIndex = 0;

-					setup_data.wLength = hub->desc_len;

-

-					hub->state++;

-					device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

-					LOG_PRINTF("DO Need to get HUB DESC\n");

-				}

-				break;

-

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

-				ERROR(cb_data.status);

-				break;

-			}

-		}

-		break;

-

-	case 4: // Get HUB Descriptor read

-		{

-			switch (cb_data.status) {

-			case USBH_PACKET_CALLBACK_STATUS_OK:

-				hub->state++;

-				device_xfer_control_read(hub->buffer, hub->desc_len, event, dev); // "error dynamic size" - bad comment, investigate

-				break;

-

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

-				ERROR(cb_data.status);

-				break;

-			}

-		}

-		break;

-

-	case 5:// Hub descriptor found

+	case EVENT_STATE_READ_HUB_DESCRIPTOR_COMPLETE:// Hub descriptor found

 		{

 			switch (cb_data.status) {

 			case USBH_PACKET_CALLBACK_STATUS_OK:

@@ -297,19 +214,19 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 						struct usb_setup_data setup_data;

 						hub->desc_len = hub_descriptor->head.bDescLength;

 

-						setup_data.bmRequestType = 0b10100000;

+						setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_DEVICE;

 						setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;

 						setup_data.wValue = 0x29<<8;

 						setup_data.wIndex = 0;

 						setup_data.wLength = hub->desc_len;

 

-						hub->state = 4;

-						device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+						hub->state = EVENT_STATE_READ_HUB_DESCRIPTOR_COMPLETE;

+						device_control(dev, event, &setup_data, hub->buffer);

 						break;

 					} else if (hub_descriptor->head.bDescLength == hub->desc_len) {

 						hub->ports_num = hub_descriptor->head.bNbrPorts;

 

-						hub->state++;

+						hub->state = EVENT_STATE_ENABLE_PORTS;

 						hub->index = 0;

 						cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;

 						event(dev, cb_data);

@@ -334,7 +251,7 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 								LOG_PRINTF("INCREASE NUMBER OF ENABLED PORTS\n");

 								hub->ports_num = USBH_HUB_MAX_DEVICES;

 							}

-							hub->state++;

+							hub->state = EVENT_STATE_ENABLE_PORTS;

 							hub->index = 0;

 

 							cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;

@@ -344,15 +261,14 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 				}

 				break;

 

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

+			default:

 				ERROR(cb_data.status);

 				break;

 			}

 		}

 		break;

 

-	case 6:// enable ports

+	case EVENT_STATE_ENABLE_PORTS:// enable ports

 		{

 			switch (cb_data.status) {

 			case USBH_PACKET_CALLBACK_STATUS_OK:

@@ -361,182 +277,76 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 					struct usb_setup_data setup_data;

 

 					LOG_PRINTF("[!%d!]",hub->index);

-					setup_data.bmRequestType = 0b00100011;

+					setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

 					setup_data.bRequest = HUB_REQ_SET_FEATURE;

 					setup_data.wValue = HUB_FEATURE_PORT_POWER;

 					setup_data.wIndex = hub->index;

 					setup_data.wLength = 0;

 

-					hub->state_after_empty_read = hub->state;

-					hub->state = EMPTY_PACKET_READ_STATE;

-

-					device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+					device_control(dev, event, &setup_data, 0);

 				} else {

-					hub->state++;

 					// TODO:

 					// Delay Based on hub descriptor field bPwr2PwrGood

 					// delay_ms_busy_loop(200);

 

 					LOG_PRINTF("\nHUB CONFIGURED & PORTS POWERED\n");

 

-					cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;

-					event(dev, cb_data);

-				}

-				break;

-

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

-				ERROR(cb_data.status);

-				break;

-			}

-		}

-		break;

-

-	case 7:

-		{

-			switch (cb_data.status) {

-			case USBH_PACKET_CALLBACK_STATUS_OK:

-				{

+					// get device status

 					struct usb_setup_data setup_data;

 

-					setup_data.bmRequestType = 0b10100000;

+					setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_DEVICE;

 					setup_data.bRequest = USB_REQ_GET_STATUS;

 					setup_data.wValue = 0;

 					setup_data.wIndex = 0;

 					setup_data.wLength = 4;

 

-					hub->state++;

-					device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+					hub->state = EVENT_STATE_GET_PORT_STATUS;

+					hub->index = 0;

+					device_control(dev, event, &setup_data, hub->buffer);

 				}

 				break;

 

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

-				ERROR(cb_data.status);

-				break;

-			}

-

-		}

-		break;

-	case 8:

-		{

-			switch (cb_data.status) {

-			case USBH_PACKET_CALLBACK_STATUS_OK:

-				device_xfer_control_read(hub->buffer, 4, event, dev);

-				hub->index = 0;

-				hub->state++;

-				break;

-

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

+			default:

 				ERROR(cb_data.status);

 				break;

 			}

 		}

 		break;

 

-	case 9:

+	case EVENT_STATE_GET_PORT_STATUS:

 		{

 			switch (cb_data.status) {

 			case USBH_PACKET_CALLBACK_STATUS_OK:

 				{

-					struct usb_setup_data setup_data;

-

-					setup_data.bmRequestType = 0b10100011;

-					setup_data.bRequest = USB_REQ_GET_STATUS;

-					setup_data.wValue = 0;

-					setup_data.wIndex = ++hub->index;

-					setup_data.wLength = 4;

-

-					hub->state++;

+					if (hub->index < hub->ports_num) {

+						//TODO: process data contained in hub->buffer

 

-					device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

-				}

-				break;

-

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

-				ERROR(cb_data.status);

-				break;

-			}

-		}

-		break;

+						struct usb_setup_data setup_data;

 

-	case 10:

-		{

-			switch (cb_data.status) {

-			case USBH_PACKET_CALLBACK_STATUS_OK:

-				device_xfer_control_read(hub->buffer, 4, event, dev);

-				hub->state++;

-				break;

+						setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

+						setup_data.bRequest = USB_REQ_GET_STATUS;

+						setup_data.wValue = 0;

+						setup_data.wIndex = ++hub->index;

+						setup_data.wLength = 4;

 

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

-				ERROR(cb_data.status);

-				break;

-			}

-		}

-		break;

+						hub->state = EVENT_STATE_GET_PORT_STATUS;

+						device_control(dev, event, &setup_data, hub->buffer);

+					} else {

+						hub->busy = 0;

+						hub->state = EVENT_STATE_POLL_REQ;

+					}

 

-	case 11:

-		{

-			switch (cb_data.status) {

-			case USBH_PACKET_CALLBACK_STATUS_OK:

-				if (hub->index < hub->ports_num) {

-					hub->state = 9;

-					// process data contained in hub->buffer

-					// TODO:

-					cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;

-					event(dev, cb_data);

-				} else {

-					hub->busy = 0;

-					hub->state = 25;

 				}

 				break;

 

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

+			default:

 				ERROR(cb_data.status);

 				break;

 			}

 		}

 		break;

 

-	case 31: // Read port status

-		{

-			switch (cb_data.status) {

-			case USBH_PACKET_CALLBACK_STATUS_OK:

-				{

-					int8_t port = hub->current_port;

-					hub->state++;

-

-					// TODO: rework to endianess aware,

-					// (maybe whole library is affected by this)

-					// Detail:

-					// 	Isn't universal. Here is endianess ok,

-					// 	but on another architecture may be incorrect

-					device_xfer_control_read(&hub->hub_and_port_status[port], 4, event, dev);

-				}

-				break;

-

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

-				ERROR(cb_data.status);

-				// continue

-				hub->state = 25;

-				break;

-			}

-

-		}

-		break;

-	case 32:

+	case EVENT_STATE_GET_STATUS_COMPLETE:

 		{

 			switch (cb_data.status) {

 			case USBH_PACKET_CALLBACK_STATUS_OK:

@@ -556,7 +366,7 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 							if (!hub->device[port]) {

 								if (!usbh_enum_available() || hub->busy) {

 									LOG_PRINTF("\n\t\t\tCannot enumerate %d %d\n", !usbh_enum_available(), hub->busy);

-									hub->state = 25;

+									hub->state = EVENT_STATE_POLL_REQ;

 									break;

 								}

 							}

@@ -564,54 +374,49 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 							// clear feature C_PORT_CONNECTION

 							struct usb_setup_data setup_data;

 

-							setup_data.bmRequestType = 0b00100011;

+							setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

 							setup_data.bRequest = HUB_REQ_CLEAR_FEATURE;

 							setup_data.wValue = HUB_FEATURE_C_PORT_CONNECTION;

 							setup_data.wIndex = port;

 							setup_data.wLength = 0;

 

-							hub->state_after_empty_read = 33;

-							hub->state = EMPTY_PACKET_READ_STATE;

-

-							device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+							hub->state = EVENT_STATE_PORT_RESET_REQ;

+							device_control(dev, event, &setup_data, 0);

 

 						} else if(stc & (1<<HUB_FEATURE_PORT_RESET)) {

 							// clear feature C_PORT_RESET

 							// Reset processing is complete, enumerate device

 							struct usb_setup_data setup_data;

 

-							setup_data.bmRequestType = 0b00100011;

+							setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

 							setup_data.bRequest = HUB_REQ_CLEAR_FEATURE;

 							setup_data.wValue = HUB_FEATURE_C_PORT_RESET;

 							setup_data.wIndex = port;

 							setup_data.wLength = 0;

 

-							hub->state_after_empty_read = 35;

-							hub->state = EMPTY_PACKET_READ_STATE;

+							hub->state = EVENT_STATE_PORT_RESET_COMPLETE;

 

 							LOG_PRINTF("RESET");

-							device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+							device_control(dev, event, &setup_data, 0);

 						} else {

 							LOG_PRINTF("another STC %d\n", stc);

 						}

 					} else {

-						hub->state = 25;

+						hub->state = EVENT_STATE_POLL_REQ;

 						LOG_PRINTF("HUB status change\n");

 					}

 				}

 				break;

 

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

+			default:

 				ERROR(cb_data.status);

 				// continue

-				hub->state = 25;

+				hub->state = EVENT_STATE_POLL_REQ;

 				break;

 			}

 		}

 		break;

-	case 33:

+	case EVENT_STATE_PORT_RESET_REQ:

 		{

 			switch (cb_data.status) {

 			case USBH_PACKET_CALLBACK_STATUS_OK:

@@ -622,48 +427,40 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 						if ((stc) & (1<<HUB_FEATURE_PORT_CONNECTION)) {

 							struct usb_setup_data setup_data;

 

-							setup_data.bmRequestType = 0b00100011;

+							setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

 							setup_data.bRequest = HUB_REQ_SET_FEATURE;

 							setup_data.wValue = HUB_FEATURE_PORT_RESET;

 							setup_data.wIndex = port;

 							setup_data.wLength = 0;

 

-							hub->state_after_empty_read = 11;

-							hub->state = EMPTY_PACKET_READ_STATE;

+							hub->state = EVENT_STATE_GET_PORT_STATUS;

 

 							LOG_PRINTF("CONN");

 

 							hub->busy = 1;

-							device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+							device_control(dev, event, &setup_data, 0);

 						}

 					} else {

 						LOG_PRINTF("\t\t\t\tDISCONNECT EVENT\n");

-						if (hub->device[port]->drv && hub->device[port]->drvdata) {

-							hub->device[port]->drv->remove(hub->device[port]->drvdata);

-						}

-						hub->device[port]->address = -1;

+						device_remove(hub->device[port]);

 

-						hub->device[port]->drv = 0;

-						hub->device[port]->drvdata = 0;

 						hub->device[port] = 0;

 						hub->current_port = CURRENT_PORT_NONE;

-						hub->state = 25;

+						hub->state = EVENT_STATE_POLL_REQ;

 						hub->busy = 0;

 					}

 				}

 				break;

 

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

+			default:

 				ERROR(cb_data.status);

 				// continue

-				hub->state = 25;

+				hub->state = EVENT_STATE_POLL_REQ;

 				break;

 			}

 		}

 		break;

-	case 35:	// RESET COMPLETE, start enumeration

+	case EVENT_STATE_PORT_RESET_COMPLETE:	// RESET COMPLETE, start enumeration

 		{

 			switch (cb_data.status) {

 			case USBH_PACKET_CALLBACK_STATUS_OK:

@@ -682,46 +479,52 @@ static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
 						}

 						if ((sts & (1<<(HUB_FEATURE_PORT_LOWSPEED))) &&

 							!(sts & (1<<(HUB_FEATURE_PORT_HIGHSPEED)))) {

+#define DISABLE_LOW_SPEED

+#ifdef DISABLE_LOW_SPEED

 							LOG_PRINTF("Low speed device");

 

 							// Disable Low speed device immediately

 							struct usb_setup_data setup_data;

 

-							setup_data.bmRequestType = 0b00100011;

+							setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

 							setup_data.bRequest = HUB_REQ_CLEAR_FEATURE;

 							setup_data.wValue = HUB_FEATURE_PORT_ENABLE;

 							setup_data.wIndex = port;

 							setup_data.wLength = 0;

 

 							// After write process another devices, poll for events

-							hub->state_after_empty_read = 11;//Expecting all ports are powered (constant/non-changeable after init)

-							hub->state = EMPTY_PACKET_READ_STATE;

+							//Expecting all ports are powered (constant/non-changeable after init)

+							hub->state = EVENT_STATE_GET_PORT_STATUS;

 

 							hub->current_port = CURRENT_PORT_NONE;

-							device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+							device_control(dev, event, &setup_data, 0);

+#else

+							hub->device[port]->speed = USBH_SPEED_LOW;

+							LOG_PRINTF("Low speed device");

+							hub->timestamp_us = hub->time_curr_us;

+							hub->state = EVENT_STATE_SLEEP_500_MS; // schedule wait for 500ms

+#endif

 						} else if (!(sts & (1<<(HUB_FEATURE_PORT_LOWSPEED))) &&

 							!(sts & (1<<(HUB_FEATURE_PORT_HIGHSPEED)))) {

 							hub->device[port]->speed = USBH_SPEED_FULL;

 							LOG_PRINTF("Full speed device");

 							hub->timestamp_us = hub->time_curr_us;

-							hub->state = 100; // schedule wait for 500ms

+							hub->state = EVENT_STATE_SLEEP_500_MS; // schedule wait for 500ms

 						}

 

 

 					} else {

 						LOG_PRINTF("%s:%d Do not know what to do, when device is disabled after reset\n", __FILE__, __LINE__);

-						hub->state = 25;

+						hub->state = EVENT_STATE_POLL_REQ;

 						return;

 					}

 				}

 				break;

 

-			case USBH_PACKET_CALLBACK_STATUS_EFATAL:

-			case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

-			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

+			default:

 				ERROR(cb_data.status);

 				// continue

-				hub->state = 25;

+				hub->state = EVENT_STATE_POLL_REQ;

 				break;

 			}

 		}

@@ -738,7 +541,7 @@ static void read_ep1(void *drvdata)
 	usbh_packet_t packet;

 

 	packet.address = hub->device[0]->address;

-	packet.data = hub->buffer;

+	packet.data.in = hub->buffer;

 	packet.datalen = hub->endpoint_in_maxpacketsize;

 	packet.endpoint_address = hub->endpoint_in_address;

 	packet.endpoint_size_max = hub->endpoint_in_maxpacketsize;

@@ -748,7 +551,7 @@ static void read_ep1(void *drvdata)
 	packet.callback_arg = hub->device[0];

 	packet.toggle = &hub->endpoint_in_toggle;

 

-	hub->state = 26;

+	hub->state = EVENT_STATE_POLL;

 	usbh_read(hub->device[0], &packet);

 	LOG_PRINTF("@hub %d/EP1 |  \n", hub->device[0]->address);

 

@@ -767,7 +570,7 @@ static void poll(void *drvdata, uint32_t time_curr_us)
 	hub->time_curr_us = time_curr_us;

 

 	switch (hub->state) {

-	case 25:

+	case EVENT_STATE_POLL_REQ:

 		{

 			if (usbh_enum_available()) {

 				read_ep1(hub);

@@ -777,28 +580,36 @@ static void poll(void *drvdata, uint32_t time_curr_us)
 		}

 		break;

 

-	case 1:

+	case EVENT_STATE_INITIAL:

 		{

-			LOG_PRINTF("CFGVAL: %d\n", hub->buffer[0]);

-			struct usb_setup_data setup_data;

+			if (hub->ports_num) {

+				hub->index = 0;

+				hub->state = EVENT_STATE_ENABLE_PORTS;

+				LOG_PRINTF("No need to get HUB DESC\n");

+				event(dev, (usbh_packet_callback_data_t){0, 0});

+			} else {

+				hub->endpoint_in_toggle = 0;

 

-			setup_data.bmRequestType = 0b00000000;

-			setup_data.bRequest = USB_REQ_SET_CONFIGURATION;

-			setup_data.wValue = hub->buffer[0];

-			setup_data.wIndex = 0;

-			setup_data.wLength = 0;

+				struct usb_setup_data setup_data;

+				hub->desc_len = hub->device[0]->packet_size_max0;

 

-			hub->state = EMPTY_PACKET_READ_STATE;

-			hub->state_after_empty_read = 3;

-			device_xfer_control_write_setup(&setup_data, sizeof(setup_data), event, dev);

+				setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_DEVICE;

+				setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;

+				setup_data.wValue = 0x29 << 8;

+				setup_data.wIndex = 0;

+				setup_data.wLength = hub->desc_len;

 

+				hub->state = EVENT_STATE_READ_HUB_DESCRIPTOR_COMPLETE;

+				device_control(dev, event, &setup_data, hub->buffer);

+				LOG_PRINTF("DO Need to get HUB DESC\n");

+			}

 		}

 		break;

-	case 100:

+	case EVENT_STATE_SLEEP_500_MS:

 		if (hub->time_curr_us - hub->timestamp_us > 500000) {

 			int8_t port = hub->current_port;

-			LOG_PRINTF("PORT: %d", port);

-			LOG_PRINTF("\nNEW device at address: %d\n", hub->device[port]->address);

+			LOG_PRINTF("PORT: %d\n", port);

+			LOG_PRINTF("NEW device at address: %d\n", hub->device[port]->address);

 			hub->device[port]->lld = hub->device[0]->lld;

 

 			device_enumeration_start(hub->device[port]);

@@ -812,7 +623,7 @@ static void poll(void *drvdata, uint32_t time_curr_us)
 			// Only one device on bus can have address==0

 			hub->busy = 0;

 

-			hub->state = 25;

+			hub->state = EVENT_STATE_POLL_REQ;

 		}

 		break;

 	default:

@@ -835,23 +646,11 @@ static void remove(void *drvdata)
 	hub_device_t *hub = (hub_device_t *)drvdata;

 	uint8_t i;

 

-	// Call fast... to avoid polling

-	hub->state = 0;

+	hub->state = EVENT_STATE_NONE;

 	hub->endpoint_in_address = 0;

 	hub->busy = 0;

-	for (i = 1; i < USBH_HUB_MAX_DEVICES + 1; i++) {

-		if (hub->device[i]) {

-			if (hub->device[i]->drv && hub->device[i]->drvdata) {

-				if (hub->device[i]->drv->remove != remove) {

-					LOG_PRINTF("\t\t\t\tHUB REMOVE %d\n",hub->device[i]->address);

-					hub->device[i]->drv->remove(hub->device[i]->drvdata);

-				}

-			}

-			hub->device[i] = 0;

-		}

-		hub->device[0]->drv = 0;

-		hub->device[0]->drvdata = 0;

-		hub->device[0] = 0;

+	for (i = 0; i < USBH_HUB_MAX_DEVICES + 1; i++) {

+		hub->device[i] = 0;

 

 	}

 }

diff --git a/src/usbh_driver_hub_private.h b/src/usbh_driver_hub_private.h
index df38243..8c9bc44 100644
--- a/src/usbh_driver_hub_private.h
+++ b/src/usbh_driver_hub_private.h
@@ -61,7 +61,19 @@
 
 #define CURRENT_PORT_NONE -1
 
-#define EMPTY_PACKET_READ_STATE 255
+enum EVENT_STATE {
+	EVENT_STATE_NONE,
+	EVENT_STATE_INITIAL,
+	EVENT_STATE_POLL_REQ,
+	EVENT_STATE_POLL,
+	EVENT_STATE_READ_HUB_DESCRIPTOR_COMPLETE,
+	EVENT_STATE_ENABLE_PORTS,
+	EVENT_STATE_GET_PORT_STATUS,
+	EVENT_STATE_PORT_RESET_REQ,
+	EVENT_STATE_PORT_RESET_COMPLETE,
+	EVENT_STATE_SLEEP_500_MS,
+	EVENT_STATE_GET_STATUS_COMPLETE,
+};
 
 struct _hub_device {
 	usbh_device_t *device[USBH_HUB_MAX_DEVICES + 1];
@@ -69,8 +81,7 @@ struct _hub_device {
 	uint16_t endpoint_in_maxpacketsize;
 	uint8_t endpoint_in_address;
 	uint8_t endpoint_in_toggle;
-	uint8_t state;
-	uint8_t state_after_empty_read;
+	enum EVENT_STATE state;
 
 	uint8_t desc_len;
 	uint16_t ports_num;
diff --git a/src/usbh_lld_stm32f4.c b/src/usbh_lld_stm32f4.c
index 9e81c09..0654d28 100644
--- a/src/usbh_lld_stm32f4.c
+++ b/src/usbh_lld_stm32f4.c
@@ -47,7 +47,6 @@ struct _channel {
 	enum CHANNEL_STATE state;

 	usbh_packet_t packet;

 	uint32_t data_index; //used in receive function

-	uint8_t error_count;

 };

 typedef struct _channel channel_t;

 

@@ -143,34 +142,31 @@ static void init(void *drvdata)
 	REBASE(OTG_GUSBCFG) |= OTG_GUSBCFG_PHYSEL;

 }

 

-static void stm32f4_usbh_port_channel_setup(

-	void *drvdata, uint32_t channel, uint32_t address,

-	uint32_t eptyp, uint32_t epnum, uint32_t epdir,

-	uint32_t max_packet_size)

+static uint32_t usbh_to_stm32_endpoint_type(enum USBH_ENDPOINT_TYPE usbh_eptyp)

 {

-	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

-	channel_t *channels = dev->channels;

-

-	// TODO: maybe to function

-	switch (eptyp) {

-	case USBH_ENDPOINT_TYPE_CONTROL:

-		eptyp = OTG_HCCHAR_EPTYP_CONTROL;

-		break;

-	case USBH_ENDPOINT_TYPE_BULK:

-		eptyp = OTG_HCCHAR_EPTYP_BULK;

-		break;

-	case USBH_ENDPOINT_TYPE_INTERRUPT:

-		// Use bulk transfer also for interrupt, since no difference is on protocol layer

-		// Except different behaviour of the core

-		eptyp = OTG_HCCHAR_EPTYP_BULK;

-		break;

-	case USBH_ENDPOINT_TYPE_ISOCHRONOUS:

-		eptyp = OTG_HCCHAR_EPTYP_ISOCHRONOUS;

-		break;

+	switch (usbh_eptyp) {

+	case USBH_ENDPOINT_TYPE_CONTROL: return OTG_HCCHAR_EPTYP_CONTROL;

+	case USBH_ENDPOINT_TYPE_BULK: return OTG_HCCHAR_EPTYP_BULK;

+

+	// Use bulk transfer also for interrupt, since no difference is on protocol layer

+	// Except different behaviour of the core

+	case USBH_ENDPOINT_TYPE_INTERRUPT: return OTG_HCCHAR_EPTYP_BULK;

+	case USBH_ENDPOINT_TYPE_ISOCHRONOUS: return OTG_HCCHAR_EPTYP_ISOCHRONOUS;

 	default:

 		LOG_PRINTF("\n\n\n\nWRONG EP TYPE\n\n\n\n\n");

-		return;

+		return OTG_HCCHAR_EPTYP_CONTROL;

 	}

+}

+

+static void stm32f4_usbh_port_channel_setup(

+	void *drvdata, uint32_t channel, uint32_t epdir)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	channel_t *channels = dev->channels;

+	uint32_t max_packet_size = channels[channel].packet.endpoint_size_max;

+	uint32_t address = channels[channel].packet.address;

+	uint32_t epnum = channels[channel].packet.endpoint_address;

+	uint32_t eptyp = usbh_to_stm32_endpoint_type(channels[channel].packet.endpoint_type);

 

 	uint32_t speed = 0;

 	if (channels[channel].packet.speed == USBH_SPEED_LOW) {

@@ -182,7 +178,7 @@ static void stm32f4_usbh_port_channel_setup(
 				OTG_HCCHAR_MCNT_1 |

 				(OTG_HCCHAR_EPTYP_MASK & (eptyp)) |

 				(speed) |

-				(epdir) |

+				(OTG_HCCHAR_EPDIR_MASK & epdir) |

 				(OTG_HCCHAR_EPNUM_MASK & (epnum << 11)) |

 				(OTG_HCCHAR_MPSIZ_MASK & max_packet_size);

 

@@ -227,12 +223,7 @@ static void read(void *drvdata, usbh_packet_t *packet)
 

 	REBASE_CH(OTG_HCTSIZ, channel) = dpid | (num_packets << 19) | packet->datalen;

 

-	stm32f4_usbh_port_channel_setup(dev, channel,

-									packet->address,

-									packet->endpoint_type,

-									packet->endpoint_address,

-									OTG_HCCHAR_EPDIR_IN,

-									packet->endpoint_size_max);

+	stm32f4_usbh_port_channel_setup(dev, channel, OTG_HCCHAR_EPDIR_IN);

 }

 

 /**

@@ -284,18 +275,13 @@ static void write(void *drvdata, const usbh_packet_t *packet)
 	}

 	REBASE_CH(OTG_HCTSIZ, channel) = dpid | (num_packets << 19) | packet->datalen;

 

-	stm32f4_usbh_port_channel_setup(dev, channel,

-									packet->address,

-									packet->endpoint_type,

-									packet->endpoint_address,

-									OTG_HCCHAR_EPDIR_OUT,

-									packet->endpoint_size_max);

+	stm32f4_usbh_port_channel_setup(dev, channel, OTG_HCCHAR_EPDIR_OUT);

 

 	if (packet->endpoint_type == USBH_ENDPOINT_TYPE_CONTROL ||

 		packet->endpoint_type == USBH_ENDPOINT_TYPE_BULK) {

 

 		volatile uint32_t *fifo = &REBASE_CH(OTG_FIFO, channel) + RX_FIFO_SIZE;

-		const uint32_t * buf32 = packet->data;

+		const uint32_t * buf32 = packet->data.out;

 		int i;

 		LOG_PRINTF("\nSending[%d]: ", packet->datalen);

 		for(i = packet->datalen; i >= 4; i-=4) {

@@ -317,7 +303,7 @@ static void write(void *drvdata, const usbh_packet_t *packet)
 	} else {

 		volatile uint32_t *fifo = &REBASE_CH(OTG_FIFO, channel) +

 			RX_FIFO_SIZE + TX_NP_FIFO_SIZE;

-		const uint32_t * buf32 = packet->data;

+		const uint32_t * buf32 = packet->data.out;

 		int i;

 		for(i = packet->datalen; i > 0; i-=4) {

 			*fifo++ = *buf32++;

@@ -334,7 +320,7 @@ static void rxflvl_handle(void *drvdata)
 	uint8_t channel = rxstsp&0xf;

 	uint32_t len = (rxstsp>>4) & 0x1ff;

 	if ((rxstsp&OTG_GRXSTSP_PKTSTS_MASK) == OTG_GRXSTSP_PKTSTS_IN) {

-		uint8_t *data = channels[channel].packet.data;

+		uint8_t *data = channels[channel].packet.data.in;

 		uint32_t *buf32 = (uint32_t *)&data[channels[channel].data_index];

 

 		int32_t i;

@@ -366,7 +352,7 @@ static void rxflvl_handle(void *drvdata)
 		uint32_t i;

 		LOG_PRINTF("\nDATA: ");

 		for (i = 0; i < channels[channel].data_index; i++) {

-			uint8_t *data = channels[channel].packet.data;

+			uint8_t *data = channels[channel].packet.data.in;

 			LOG_PRINTF("%02X ", data[i]);

 		}

 #endif

@@ -504,9 +490,17 @@ static enum USBH_POLL_STATUS poll_run(usbh_lld_stm32f4_driver_data_t *dev)
 

 				if (hcint & OTG_HCINT_NAK) {

 					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_NAK;

-					LOG_PRINTF("NAK");

+					LOG_PRINTF("NAK\n");

 

-					REBASE_CH(OTG_HCCHAR, channel) |= OTG_HCCHAR_CHENA;

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EAGAIN;

+					cb_data.transferred_length = channels[channel].data_index;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

 

 				}

 

@@ -540,6 +534,8 @@ static enum USBH_POLL_STATUS poll_run(usbh_lld_stm32f4_driver_data_t *dev)
 					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_FRMOR;

 					LOG_PRINTF("FRMOR");

 

+					free_channel(dev, channel);

+

 					usbh_packet_callback_data_t cb_data;

 					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;

 					cb_data.transferred_length = 0;

@@ -547,7 +543,6 @@ static enum USBH_POLL_STATUS poll_run(usbh_lld_stm32f4_driver_data_t *dev)
 					channels[channel].packet.callback(

 						channels[channel].packet.callback_arg,

 						cb_data);

-					free_channel(dev, channel);

 				}

 

 				if (hcint & OTG_HCINT_TXERR) {

@@ -922,7 +917,6 @@ static int8_t get_free_channel(void *drvdata)
 				OTG_HCINTMSK_CHHM | OTG_HCINTMSK_STALLM |

 				OTG_HCINTMSK_FRMORM;

 			REBASE(OTG_HAINTMSK) |= (1 << i);

-			dev->channels[i].error_count = 0;

 			return i;

 		}

 	}

@@ -1013,7 +1007,7 @@ static usbh_lld_stm32f4_driver_data_t driver_data_fs = {
 	.channels = channels_fs,

 	.num_channels = NUM_CHANNELS_FS

 };

-static const usbh_low_level_driver_t driver_fs = {

+const usbh_low_level_driver_t usbh_lld_stm32f4_driver_fs = {

 	.init = init,

 	.poll = poll,

 	.read = read,

@@ -1021,7 +1015,6 @@ static const usbh_low_level_driver_t driver_fs = {
 	.root_speed = root_speed,

 	.driver_data = &driver_data_fs

 };

-const void *usbh_lld_stm32f4_driver_fs = &driver_fs;

 #endif

 

 // USB High Speed - OTG_HS

@@ -1033,7 +1026,8 @@ static usbh_lld_stm32f4_driver_data_t driver_data_hs = {
 	.channels = channels_hs,

 	.num_channels = NUM_CHANNELS_HS

 };

-static const usbh_low_level_driver_t driver_hs = {

+

+const usbh_low_level_driver_t usbh_lld_stm32f4_driver_hs = {

 	.init = init,

 	.poll = poll,

 	.read = read,

@@ -1041,5 +1035,5 @@ static const usbh_low_level_driver_t driver_hs = {
 	.root_speed = root_speed,

 	.driver_data = &driver_data_hs

 };

-const void *usbh_lld_stm32f4_driver_hs = &driver_hs;

+

 #endif