1 files changed, 266 insertions, 193 deletions

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;