Index: uspace/lib/usbhost/src/usb_endpoint_manager.c
===================================================================
--- uspace/lib/usbhost/src/usb_endpoint_manager.c	(revision feeac0d8cd6e0ce736779bc5f1dc701c5cf42ba6)
+++ uspace/lib/usbhost/src/usb_endpoint_manager.c	(revision 93488625b5baeeeee9e1065454fa8734da4f68d8)
@@ -36,4 +36,5 @@
 #include <assert.h>
 #include <errno.h>
+#include <macros.h>
 
 #include <usb/debug.h>
@@ -71,5 +72,5 @@
 	assert(instance);
 	assert(addr >= 0);
-	return &instance->endpoint_lists[addr % ENDPOINT_LIST_COUNT];
+	return &instance->devices[addr % ARRAY_SIZE(instance->devices)].endpoint_list;
 }
 
@@ -95,4 +96,28 @@
 	}
 	return NULL;
+}
+
+/** Get a free USB address
+ *
+ * @param[in] instance Device manager structure to use.
+ * @return Free address, or error code.
+ */
+static usb_address_t usb_endpoint_manager_get_free_address(
+    usb_endpoint_manager_t *instance)
+{
+
+	usb_address_t new_address = instance->last_address;
+	do {
+		new_address = (new_address + 1) % USB_ADDRESS_COUNT;
+		if (new_address == USB_ADDRESS_DEFAULT)
+			new_address = 1;
+		if (new_address == instance->last_address)
+			return ENOSPC;
+	} while (instance->devices[new_address].occupied);
+
+	assert(new_address != USB_ADDRESS_DEFAULT);
+	instance->last_address = new_address;
+
+	return new_address;
 }
 
@@ -155,6 +180,5 @@
  */
 int usb_endpoint_manager_init(usb_endpoint_manager_t *instance,
-    size_t available_bandwidth,
-    size_t (*bw_count)(usb_speed_t, usb_transfer_type_t, size_t, size_t))
+    size_t available_bandwidth, bw_count_func_t bw_count, usb_speed_t max_speed)
 {
 	assert(instance);
@@ -162,67 +186,12 @@
 	instance->free_bw = available_bandwidth;
 	instance->bw_count = bw_count;
-	for (unsigned i = 0; i < ENDPOINT_LIST_COUNT; ++i) {
-		list_initialize(&instance->endpoint_lists[i]);
+	instance->last_address = 0;
+	instance->max_speed = max_speed;
+	for (unsigned i = 0; i < ARRAY_SIZE(instance->devices); ++i) {
+		list_initialize(&instance->devices[i].endpoint_list);
+		instance->devices[i].speed = USB_SPEED_MAX;
+		instance->devices[i].occupied = false;
 	}
 	return EOK;
-}
-
-/** Check setup packet data for signs of toggle reset.
- *
- * @param[in] instance usb_endpoint_manager structure, non-null.
- * @param[in] target Device to receive setup packet.
- * @param[in] data Setup packet data.
- *
- * Really ugly one. Resets toggle bit on all endpoints that need it.
- * @TODO Use tools from libusbdev requests.h
- */
-void usb_endpoint_manager_reset_eps_if_need(usb_endpoint_manager_t *instance,
-    usb_target_t target, const uint8_t data[8])
-{
-	assert(instance);
-	if (!usb_target_is_valid(target)) {
-		usb_log_error("Invalid data when checking for toggle reset.\n");
-		return;
-	}
-
-	assert(data);
-	switch (data[1])
-	{
-	case 0x01: /* Clear Feature -- resets only cleared ep */
-		/* Recipient is endpoint, value is zero (ENDPOINT_STALL) */
-		// TODO Use macros in libusbdev requests.h
-		if (((data[0] & 0xf) == 1) && ((data[2] | data[3]) == 0)) {
-			fibril_mutex_lock(&instance->guard);
-			/* endpoint number is < 16, thus first byte is enough */
-			list_foreach(*get_list(instance, target.address),
-			    link, endpoint_t, ep) {
-				if ((ep->address == target.address)
-				    && (ep->endpoint = data[4])) {
-					endpoint_toggle_set(ep,0);
-				}
-			}
-			fibril_mutex_unlock(&instance->guard);
-		}
-	break;
-
-	case 0x9: /* Set Configuration */
-	case 0x11: /* Set Interface */
-		/* Recipient must be device, this resets all endpoints,
-		 * In fact there should be no endpoints but EP 0 registered
-		 * as different interfaces use different endpoints,
-		 * unless you're changing configuration or alternative
-		 * interface of an already setup device. */
-		if ((data[0] & 0xf) == 0) {
-			fibril_mutex_lock(&instance->guard);
-			list_foreach(*get_list(instance, target.address),
-			    link, endpoint_t, ep) {
-				if (ep->address == target.address) {
-					endpoint_toggle_set(ep,0);
-				}
-			}
-			fibril_mutex_unlock(&instance->guard);
-		}
-	break;
-	}
 }
 
@@ -316,21 +285,20 @@
 int usb_endpoint_manager_add_ep(usb_endpoint_manager_t *instance,
     usb_address_t address, usb_endpoint_t endpoint, usb_direction_t direction,
-    usb_transfer_type_t type, usb_speed_t speed, size_t max_packet_size,
-    size_t data_size, int (*callback)(endpoint_t *, void *), void *arg)
+    usb_transfer_type_t type, size_t max_packet_size, size_t data_size,
+    ep_add_callback_t callback, void *arg, usb_address_t tt_address,
+    unsigned tt_port)
 {
 	assert(instance);
 	if (instance->bw_count == NULL)
 		return ENOTSUP;
-	if (address < 0)
-		return EINVAL;
-
-	const size_t bw =
-	    instance->bw_count(speed, type, data_size, max_packet_size);
-
-	fibril_mutex_lock(&instance->guard);
-	/* Check for available bandwidth */
-	if (bw > instance->free_bw) {
-		fibril_mutex_unlock(&instance->guard);
-		return ENOSPC;
+	if (!usb_address_is_valid(address))
+		return EINVAL;
+
+
+	fibril_mutex_lock(&instance->guard);
+	/* Check for speed and address */
+	if (!instance->devices[address].occupied) {
+		fibril_mutex_unlock(&instance->guard);
+		return ENOENT;
 	}
 
@@ -342,6 +310,16 @@
 	}
 
-	ep = endpoint_create(
-	    address, endpoint, direction, type, speed, max_packet_size, bw);
+	const usb_speed_t speed = instance->devices[address].speed;
+	const size_t bw =
+	    instance->bw_count(speed, type, data_size, max_packet_size);
+
+	/* Check for available bandwidth */
+	if (bw > instance->free_bw) {
+		fibril_mutex_unlock(&instance->guard);
+		return ENOSPC;
+	}
+
+	ep = endpoint_create(address, endpoint, direction, type, speed,
+	    max_packet_size, bw, tt_address, tt_port);
 	if (!ep) {
 		fibril_mutex_unlock(&instance->guard);
@@ -375,5 +353,5 @@
 int usb_endpoint_manager_remove_ep(usb_endpoint_manager_t *instance,
     usb_address_t address, usb_endpoint_t endpoint, usb_direction_t direction,
-    void (*callback)(endpoint_t *, void *), void *arg)
+    ep_remove_callback_t callback, void *arg)
 {
 	assert(instance);
@@ -393,4 +371,25 @@
 	endpoint_destroy(ep);
 	return EOK;
+}
+
+int usb_endpoint_manager_reset_toggle(usb_endpoint_manager_t *instance,
+    usb_target_t target, bool all)
+{
+	assert(instance);
+	if (!usb_target_is_valid(target))
+		return EINVAL;
+
+	int ret = ENOENT;
+
+	fibril_mutex_lock(&instance->guard);
+	list_foreach(*get_list(instance, target.address), link, endpoint_t, ep) {
+		if ((ep->address == target.address)
+		    && (all || ep->endpoint == target.endpoint)) {
+			endpoint_toggle_set(ep, 0);
+			ret = EOK;
+		}
+	}
+	fibril_mutex_unlock(&instance->guard);
+	return ret;
 }
 
@@ -404,21 +403,20 @@
  * @return Error code.
  */
-void usb_endpoint_manager_remove_address(usb_endpoint_manager_t *instance,
-    usb_address_t address, void (*callback)(endpoint_t *, void *), void *arg)
-{
-	list_t *list;
-	link_t *link;
-	link_t *next;
-
-	assert(address >= 0);
-	assert(instance);
-	fibril_mutex_lock(&instance->guard);
-
-	list = get_list(instance, address);
-	link = list_first(list);
-	while (link != NULL) {
+int usb_endpoint_manager_remove_address(usb_endpoint_manager_t *instance,
+    usb_address_t address, ep_remove_callback_t callback, void *arg)
+{
+	assert(instance);
+	if (!usb_address_is_valid(address))
+		return EINVAL;
+
+	fibril_mutex_lock(&instance->guard);
+
+	const int ret = instance->devices[address].occupied ? EOK : ENOENT;
+	instance->devices[address].occupied = false;
+
+	list_t *list = get_list(instance, address);
+	for (link_t *link = list_first(list); link != NULL; ) {
 		endpoint_t *ep = list_get_instance(link, endpoint_t, link);
-		next = list_next(link, list);
-
+		link = list_next(link, list);
 		if (ep->address == address) {
 			list_remove(&ep->link);
@@ -427,7 +425,80 @@
 			endpoint_destroy(ep);
 		}
-		link = next;
-	}
-	fibril_mutex_unlock(&instance->guard);
+	}
+	fibril_mutex_unlock(&instance->guard);
+	return ret;
+}
+
+/** Request USB address.
+ * @param instance usb_device_manager
+ * @param address Pointer to requested address value, place to store new address
+ * @parma strict Fail if the requested address is not available.
+ * @return Error code.
+ * @note Default address is only available in strict mode.
+ */
+int usb_endpoint_manager_request_address(usb_endpoint_manager_t *instance,
+    usb_address_t *address, bool strict, usb_speed_t speed)
+{
+	assert(instance);
+	assert(address);
+	if (speed > instance->max_speed)
+		return ENOTSUP;
+
+	if (!usb_address_is_valid(*address))
+		return EINVAL;
+
+	usb_address_t addr = *address;
+
+	fibril_mutex_lock(&instance->guard);
+	/* Only grant default address to strict requests */
+	if ((addr == USB_ADDRESS_DEFAULT) && !strict) {
+		addr = usb_endpoint_manager_get_free_address(instance);
+	}
+
+	if (instance->devices[addr].occupied) {
+		if (strict) {
+			fibril_mutex_unlock(&instance->guard);
+			return ENOENT;
+		}
+		addr = usb_endpoint_manager_get_free_address(instance);
+	}
+	if (usb_address_is_valid(addr)) {
+		assert(instance->devices[addr].occupied == false);
+		assert(addr != USB_ADDRESS_DEFAULT || strict);
+
+		instance->devices[addr].occupied = true;
+		instance->devices[addr].speed = speed;
+		*address = addr;
+		addr = 0;
+	}
+
+	fibril_mutex_unlock(&instance->guard);
+	return addr;
+}
+
+/** Get speed assigned to USB address.
+ *
+ * @param[in] instance Device manager structure to use.
+ * @param[in] address Address the caller wants to find.
+ * @param[out] speed Assigned speed.
+ * @return Error code.
+ */
+int usb_endpoint_manager_get_speed(usb_endpoint_manager_t *instance,
+    usb_address_t address, usb_speed_t *speed)
+{
+	assert(instance);
+	if (!usb_address_is_valid(address)) {
+		return EINVAL;
+	}
+
+	fibril_mutex_lock(&instance->guard);
+
+	const int ret = instance->devices[address].occupied ? EOK : ENOENT;
+	if (speed && instance->devices[address].occupied) {
+		*speed = instance->devices[address].speed;
+	}
+
+	fibril_mutex_unlock(&instance->guard);
+	return ret;
 }
 /**
