Index: uspace/drv/bus/usb/usbhub/port.c
===================================================================
--- uspace/drv/bus/usb/usbhub/port.c	(revision d2c3dcd880ba1b8f4263eddeaae18bcc6c072bd9)
+++ uspace/drv/bus/usb/usbhub/port.c	(revision c4e84ed699e365187054b23f19f7c0c3c431291d)
@@ -48,412 +48,229 @@
 #include "status.h"
 
-/** Information for fibril for device discovery. */
-struct add_device_phase1 {
-	usb_hub_dev_t *hub;
-	usb_hub_port_t *port;
-	usb_speed_t speed;
-};
-
-static int usb_hub_port_device_gone(usb_hub_port_t *port, usb_hub_dev_t *hub);
-static void usb_hub_port_reset_completed(usb_hub_port_t *port,
-    usb_hub_dev_t *hub, usb_port_status_t status);
-static int get_port_status(usb_hub_port_t *port, usb_port_status_t *status);
-static int add_device_phase1_worker_fibril(void *arg);
-static int create_add_device_fibril(usb_hub_port_t *port, usb_hub_dev_t *hub,
-    usb_speed_t speed);
-
-int usb_hub_port_fini(usb_hub_port_t *port, usb_hub_dev_t *hub)
+#define port_log(lvl, port, fmt, ...) do { usb_log_##lvl("(%p-%u): " fmt, (port->hub), (port->port_number), ##__VA_ARGS__); } while (0)
+
+/** Initialize hub port information.
+ *
+ * @param port Port to be initialized.
+ */
+void usb_hub_port_init(usb_hub_port_t *port, usb_hub_dev_t *hub, unsigned int port_number)
 {
 	assert(port);
-	if (port->device_attached)
-		return usb_hub_port_device_gone(port, hub);
-	return EOK;
-}
-
-/**
- * Clear feature on hub port.
- *
- * @param port Port structure.
- * @param feature Feature selector.
- * @return Operation result
- */
-int usb_hub_port_clear_feature(
-    usb_hub_port_t *port, usb_hub_class_feature_t feature)
+	memset(port, 0, sizeof(*port));
+	fibril_mutex_initialize(&port->guard);
+	fibril_condvar_initialize(&port->state_cv);
+	port->hub = hub;
+	port->port_number = port_number;
+}
+
+/**
+ * Utility method to change current port state and notify waiters.
+ */
+static void port_change_state(usb_hub_port_t *port, port_state_t state)
+{
+	assert(fibril_mutex_is_locked(&port->guard));
+#define S(STATE) [PORT_##STATE] = #STATE
+	static const char *state_names [] = {
+	    S(DISABLED), S(CONNECTED), S(ERROR), S(IN_RESET), S(ENABLED),
+	};
+#undef S
+	port_log(debug, port, "%s ->%s", state_names[port->state], state_names[state]);
+	port->state = state;
+	fibril_condvar_broadcast(&port->state_cv);
+}
+
+/**
+ * Utility method to wait for a particular state.
+ *
+ * @warning Some states might not be reached because of an external error
+ * condition (PORT_ENABLED).
+ */
+static void port_wait_state(usb_hub_port_t *port, port_state_t state)
+{
+	assert(fibril_mutex_is_locked(&port->guard));
+	while (port->state != state)
+		fibril_condvar_wait(&port->state_cv, &port->guard);
+}
+
+/**
+ * Inform the HC that the device on port is gone.
+ */
+static int usb_hub_port_device_gone(usb_hub_port_t *port)
 {
 	assert(port);
-	const usb_device_request_setup_packet_t clear_request = {
-		.request_type = USB_HUB_REQ_TYPE_CLEAR_PORT_FEATURE,
-		.request = USB_DEVREQ_CLEAR_FEATURE,
-		.value = feature,
-		.index = port->port_number,
-		.length = 0,
-	};
-	return usb_pipe_control_write(port->control_pipe, &clear_request,
-	    sizeof(clear_request), NULL, 0);
-}
-
-/**
- * Set feature on hub port.
- *
- * @param port Port structure.
- * @param feature Feature selector.
- * @return Operation result
- */
-int usb_hub_port_set_feature(
-    usb_hub_port_t *port, usb_hub_class_feature_t feature)
-{
-	assert(port);
-	const usb_device_request_setup_packet_t clear_request = {
-		.request_type = USB_HUB_REQ_TYPE_SET_PORT_FEATURE,
-		.request = USB_DEVREQ_SET_FEATURE,
-		.index = port->port_number,
-		.value = feature,
-		.length = 0,
-	};
-	return usb_pipe_control_write(port->control_pipe, &clear_request,
-	    sizeof(clear_request), NULL, 0);
-}
-
-/**
- * Mark reset process as failed due to external reasons
- *
- * @param port Port structure
- */
-void usb_hub_port_reset_fail(usb_hub_port_t *port)
-{
-	assert(port);
-	fibril_mutex_lock(&port->mutex);
-	if (port->reset_status == IN_RESET)
-		port->reset_status = RESET_FAIL;
-	fibril_condvar_broadcast(&port->reset_cv);
-	fibril_mutex_unlock(&port->mutex);
-}
-
-/**
- * Process interrupts on given port
- *
- * Accepts connection, over current and port reset change.
- * @param port port structure
- * @param hub hub representation
- */
-void usb_hub_port_process_interrupt(usb_hub_port_t *port, usb_hub_dev_t *hub)
-{
-	assert(port);
-	assert(hub);
-	usb_log_debug2("(%p-%u): Interrupt.", hub, port->port_number);
-
-	usb_port_status_t status = 0;
-	const int opResult = get_port_status(port, &status);
-	if (opResult != EOK) {
-		usb_log_error("(%p-%u): Failed to get port status: %s.", hub,
-		    port->port_number, str_error(opResult));
-		return;
-	}
-
-	/* Connection change */
-	if (status & USB_HUB_PORT_C_STATUS_CONNECTION) {
-		const bool connected =
-		    (status & USB_HUB_PORT_STATUS_CONNECTION) != 0;
-		usb_log_debug("(%p-%u): Connection change: device %s.", hub,
-		    port->port_number, connected ? "attached" : "removed");
-
-		/* ACK the change */
-		const int opResult = usb_hub_port_clear_feature(port,
-		    USB_HUB_FEATURE_C_PORT_CONNECTION);
-		if (opResult != EOK) {
-			usb_log_warning("(%p-%u): Failed to clear "
-			    "port-change-connection flag: %s.\n", hub,
-			    port->port_number, str_error(opResult));
-		}
-
-		if (connected) {
-			const int opResult = create_add_device_fibril(port, hub,
-			    usb_port_speed(status));
-			if (opResult != EOK) {
-				usb_log_error("(%p-%u): Cannot handle change on"
-				   " port: %s.\n", hub, port->port_number,
-				   str_error(opResult));
-			}
-		} else {
-			/* Handle the case we were in reset */
-			// FIXME: usb_hub_port_reset_fail(port);
-			/* If enabled change was reported leave the removal
-			 * to that handler, it shall ACK the change too. */
-			if (!(status & USB_HUB_PORT_C_STATUS_ENABLED)) {
-				usb_hub_port_device_gone(port, hub);
-			}
-		}
-	}
-
-	/* Enable change, ports are automatically disabled on errors. */
-	if (status & USB_HUB_PORT_C_STATUS_ENABLED) {
-		// TODO: maybe HS reset failed?
-		usb_log_info("(%p-%u): Port disabled because of errors.", hub,
-		   port->port_number);
-		usb_hub_port_device_gone(port, hub);
-		const int rc = usb_hub_port_clear_feature(port,
-		        USB_HUB_FEATURE_C_PORT_ENABLE);
-		if (rc != EOK) {
-			usb_log_error("(%p-%u): Failed to clear port enable "
-			    "change feature: %s.", hub, port->port_number,
-			    str_error(rc));
-		}
-
-	}
-
-	/* Suspend change */
-	if (status & USB_HUB_PORT_C_STATUS_SUSPEND) {
-		usb_log_error("(%p-%u): Port went to suspend state, this should"
-		    " NOT happen as we do not support suspend state!", hub,
-		    port->port_number);
-		const int rc = usb_hub_port_clear_feature(port,
-		        USB_HUB_FEATURE_C_PORT_SUSPEND);
-		if (rc != EOK) {
-			usb_log_error("(%p-%u): Failed to clear port suspend "
-			    "change feature: %s.", hub, port->port_number,
-			    str_error(rc));
-		}
-	}
-
-	/* Over current */
-	if (status & USB_HUB_PORT_C_STATUS_OC) {
-		usb_log_debug("(%p-%u): Port OC reported!.", hub,
-		    port->port_number);
-		/* According to the USB specs:
-		 * 11.13.5 Over-current Reporting and Recovery
-		 * Hub device is responsible for putting port in power off
-		 * mode. USB system software is responsible for powering port
-		 * back on when the over-current condition is gone */
-		const int rc = usb_hub_port_clear_feature(port,
-		    USB_HUB_FEATURE_C_PORT_OVER_CURRENT);
-		if (rc != EOK) {
-			usb_log_error("(%p-%u): Failed to clear port OC change "
-			    "feature: %s.\n", hub, port->port_number,
-			    str_error(rc));
-		}
-		if (!(status & ~USB_HUB_PORT_STATUS_OC)) {
-			const int rc = usb_hub_port_set_feature(
-			    port, USB_HUB_FEATURE_PORT_POWER);
-			if (rc != EOK) {
-				usb_log_error("(%p-%u): Failed to set port "
-				    "power after OC: %s.", hub,
-				    port->port_number, str_error(rc));
-			}
-		}
-	}
-
-	/* Port reset, set on port reset complete. */
-	if (status & USB_HUB_PORT_C_STATUS_RESET) {
-		usb_hub_port_reset_completed(port, hub, status);
-	}
-
-	usb_log_debug2("(%p-%u): Port status %#08" PRIx32, hub,
-	    port->port_number, status);
-}
-
-/**
- * routine called when a device on port has been removed
- *
- * If the device on port had default address, it releases default address.
- * Otherwise does not do anything, because DDF does not allow to remove device
- * from it`s device tree.
- * @param port port structure
- * @param hub hub representation
- */
-int usb_hub_port_device_gone(usb_hub_port_t *port, usb_hub_dev_t *hub)
-{
-	assert(port);
-	assert(hub);
-	async_exch_t *exch = usb_device_bus_exchange_begin(hub->usb_device);
+	assert(fibril_mutex_is_locked(&port->guard));
+	assert(port->state == PORT_ENABLED);
+
+	async_exch_t *exch = usb_device_bus_exchange_begin(port->hub->usb_device);
 	if (!exch)
 		return ENOMEM;
 	const int rc = usbhc_device_remove(exch, port->port_number);
 	usb_device_bus_exchange_end(exch);
-	if (rc == EOK)
-		port->device_attached = false;
 	return rc;
-
-}
-
-/**
- * Process port reset change
- *
- * After this change port should be enabled, unless some problem occurred.
- * This functions triggers second phase of enabling new device.
- * @param port Port structure
- * @param status Port status mask
- */
-void usb_hub_port_reset_completed(usb_hub_port_t *port, usb_hub_dev_t *hub,
-    usb_port_status_t status)
+}
+
+/**
+ * Teardown a device on port, no matter which state it was in.
+ */
+static void port_make_disabled(usb_hub_port_t *port)
+{
+	assert(fibril_mutex_is_locked(&port->guard));
+
+	port_log(debug, port, "Making device offline.");
+
+	switch (port->state) {
+	case PORT_ENABLED:
+		port_log(debug, port, "Enabled ->");
+		if (usb_hub_port_device_gone(port))
+			port_log(error, port, "Failed to remove the device node from HC. Continuing anyway.");
+		port_change_state(port, PORT_DISABLED);
+		break;
+
+	case PORT_CONNECTED:
+		port_log(debug, port, "Connected ->");
+		/* fallthrough */
+	case PORT_IN_RESET:
+		port_log(debug, port, "In reset ->");
+		port_change_state(port, PORT_ERROR);
+		/* fallthrough */
+	case PORT_ERROR:
+		port_log(debug, port, "Error ->");
+		port_wait_state(port, PORT_DISABLED);
+		/* fallthrough */
+	case PORT_DISABLED:
+		port_log(debug, port, "Disabled.");
+		break;
+	}
+
+	assert(port->state == PORT_DISABLED);
+}
+
+/**
+ * Finalize a port. Make sure no fibril is managing its state,
+ * and that the HC is aware the device is no longer there.
+ */
+void usb_hub_port_fini(usb_hub_port_t *port)
 {
 	assert(port);
-	fibril_mutex_lock(&port->mutex);
-	const bool enabled = (status & USB_HUB_PORT_STATUS_ENABLED) != 0;
-	/* Finalize device adding. */
-
-	if (enabled) {
-		port->reset_status = RESET_OK;
-		usb_log_debug("(%p-%u): Port reset complete.", hub,
-		    port->port_number);
-	} else {
-		port->reset_status = RESET_FAIL;
-		usb_log_warning("(%p-%u): Port reset complete but port not "
-		    "enabled.", hub, port->port_number);
-	}
-	fibril_condvar_broadcast(&port->reset_cv);
-	fibril_mutex_unlock(&port->mutex);
-
-	/* Clear the port reset change. */
-	int rc = usb_hub_port_clear_feature(port, USB_HUB_FEATURE_C_PORT_RESET);
+	fibril_mutex_lock(&port->guard);
+	port_make_disabled(port);
+	port_log(debug, port, "Finalized.");
+	fibril_mutex_unlock(&port->guard);
+}
+
+static int port_reset_sync(usb_hub_port_t *port)
+{
+	assert(fibril_mutex_is_locked(&port->guard));
+	assert(port->state == PORT_CONNECTED);
+
+	port_change_state(port, PORT_IN_RESET);
+	port_log(debug2, port, "Issuing reset.");
+	int rc = usb_hub_set_port_feature(port->hub, port->port_number, USB_HUB_FEATURE_PORT_RESET);
 	if (rc != EOK) {
-		usb_log_error("(%p-%u): Failed to clear port reset change: %s.",
-		    hub, port->port_number, str_error(rc));
-	}
-}
-
-/** Retrieve port status.
- *
- * @param[in] port Port structure
- * @param[out] status Where to store the port status.
- * @return Error code.
- */
-static int get_port_status(usb_hub_port_t *port, usb_port_status_t *status)
-{
-	assert(port);
-	/* USB hub specific GET_PORT_STATUS request. See USB Spec 11.16.2.6
-	 * Generic GET_STATUS request cannot be used because of the difference
-	 * in status data size (2B vs. 4B)*/
-	const usb_device_request_setup_packet_t request = {
-		.request_type = USB_HUB_REQ_TYPE_GET_PORT_STATUS,
-		.request = USB_HUB_REQUEST_GET_STATUS,
-		.value = 0,
-		.index = uint16_host2usb(port->port_number),
-		.length = sizeof(usb_port_status_t),
-	};
-	size_t recv_size;
-	usb_port_status_t status_tmp;
-
-	const int rc = usb_pipe_control_read(port->control_pipe,
-	    &request, sizeof(usb_device_request_setup_packet_t),
-	    &status_tmp, sizeof(status_tmp), &recv_size);
-	if (rc != EOK) {
+		port_log(warning, port, "Port reset request failed: %s", str_error(rc));
 		return rc;
 	}
 
-	if (recv_size != sizeof (status_tmp)) {
-		return ELIMIT;
-	}
-
-	if (status != NULL) {
-		*status = status_tmp;
-	}
-
+	fibril_condvar_wait_timeout(&port->state_cv, &port->guard, 2000000);
+	return port->state == PORT_ENABLED ? EOK : ESTALL;
+}
+
+/**
+ * Routine for adding a new device.
+ *
+ * Separate fibril is needed because the operation blocks on waiting for
+ * requesting default address and resetting port, and we must not block the
+ * control pipe.
+ */
+static void setup_device(usb_hub_port_t *port)
+{
+	int err;
+
+	fibril_mutex_lock(&port->guard);
+
+	if (port->state == PORT_ERROR) {
+		/* 
+		 * The device was removed faster than this fibril acquired the
+		 * mutex.
+		 */
+		port_change_state(port, PORT_DISABLED);
+		goto out;
+	}
+
+	if (port->state != PORT_CONNECTED) {
+		/*
+		 * Another fibril already took care of the device.
+		 * This may happen for example when the connection is unstable
+		 * and a sequence of connect, disconnect and connect come
+		 * faster the first fibril manages to request the default
+		 * address.
+		 */
+		goto out;
+	}
+
+	port_log(debug, port, "Setting up new device.");
+
+	async_exch_t *exch = usb_device_bus_exchange_begin(port->hub->usb_device);
+	if (!exch) {
+		port_log(error, port, "Failed to create exchange.");
+		goto out;
+	}
+
+	/* Reserve default address
+	 * TODO: Make the request synchronous.
+	 */
+	while ((err = usbhc_reserve_default_address(exch, port->speed)) == EAGAIN) {
+		fibril_condvar_wait_timeout(&port->state_cv, &port->guard, 500000);
+		if (port->state != PORT_CONNECTED) {
+			assert(port->state == PORT_ERROR);
+			port_change_state(port, PORT_DISABLED);
+			goto out_exch;
+		}
+	}
+	if (err != EOK) {
+		port_log(error, port, "Failed to reserve default address: %s", str_error(err));
+		goto out_exch;
+	}
+
+	port_log(debug, port, "Got default address. Resetting port.");
+
+	if ((err = port_reset_sync(port))) {
+		port_log(error, port, "Failed to reset port.");
+		port_change_state(port, PORT_DISABLED);
+		goto out_address;
+	}
+
+	assert(port->state == PORT_ENABLED);
+
+	port_log(debug, port, "Port reset, enumerating device.");
+
+	if ((err = usbhc_device_enumerate(exch, port->port_number))) {
+		port_log(error, port, "Failed to enumerate device: %s", str_error(err));
+		port_change_state(port, PORT_DISABLED);
+		goto out_port;
+	}
+
+	port_log(debug, port, "Device enumerated");
+
+out_port:
+	if (port->state != PORT_ENABLED)
+		usb_hub_clear_port_feature(port->hub, port->port_number, USB_HUB_FEATURE_C_PORT_ENABLE);
+
+out_address:
+	if ((err = usbhc_release_default_address(exch)))
+		port_log(error, port, "Failed to release default address: %s", str_error(err));
+
+out_exch:
+	usb_device_bus_exchange_end(exch);
+
+out:
+	assert(port->state == PORT_ENABLED || port->state == PORT_DISABLED);
+	fibril_mutex_unlock(&port->guard);
+}
+
+static int setup_device_worker(void *arg)
+{
+	setup_device(arg);
 	return EOK;
-}
-
-static int port_enable(usb_hub_port_t *port, usb_hub_dev_t *hub, bool enable)
-{
-	if (enable) {
-		int rc =
-		    usb_hub_port_set_feature(port, USB_HUB_FEATURE_PORT_RESET);
-		if (rc != EOK) {
-			usb_log_error("(%p-%u): Port reset request failed: %s.",
-			    hub, port->port_number, str_error(rc));
-			return rc;
-		}
-		/* Wait until reset completes. */
-		fibril_mutex_lock(&port->mutex);
-		port->reset_status = IN_RESET;
-		while (port->reset_status == IN_RESET)
-			fibril_condvar_wait(&port->reset_cv, &port->mutex);
-		rc = port->reset_status == RESET_OK ? EOK : ESTALL;
-		fibril_mutex_unlock(&port->mutex);
-		return rc;
-	} else {
-		return usb_hub_port_clear_feature(port,
-				USB_HUB_FEATURE_PORT_ENABLE);
-	}
-}
-
-/** Fibril for adding a new device.
- *
- * Separate fibril is needed because the port reset completion is announced
- * via interrupt pipe and thus we cannot block here.
- *
- * @param arg Pointer to struct add_device_phase1.
- * @return 0 Always.
- */
-int add_device_phase1_worker_fibril(void *arg)
-{
-	struct add_device_phase1 *params = arg;
-	assert(params);
-
-	bool release_default_address = false;
-
-	int ret = EOK;
-	usb_hub_dev_t *hub = params->hub;
-	usb_hub_port_t *port = params->port;
-	const usb_speed_t speed = params->speed;
-	free(arg);
-
-	usb_log_debug("(%p-%u): New device sequence.", hub, port->port_number);
-
-	async_exch_t *exch = usb_device_bus_exchange_begin(hub->usb_device);
-	if (!exch) {
-		usb_log_error("(%p-%u): Failed to begin bus exchange.", hub,
-		    port->port_number);
-		ret = ENOMEM;
-		goto out;
-	}
-
-	/* Reserve default address */
-	while ((ret = usbhc_reserve_default_address(exch, speed)) == EAGAIN) {
-		async_usleep(1000000);
-	}
-	if (ret != EOK) {
-		usb_log_error("(%p-%u): Failed to reserve default address: %s", hub, port->port_number, str_error(ret));
-		goto out;
-	}
-	release_default_address = true;
-
-	usb_log_debug("(%p-%u): Got default address. Reseting port.", hub, port->port_number);
-
-	/* Reset port */
-	if ((ret = port_enable(port, hub, true))) {
-		usb_log_error("(%p-%u): Failed to reset port.", hub, port->port_number);
-		ret = EIO;
-		goto out_address;
-	}
-
-	usb_log_debug("(%p-%u): Port reset, enumerating device", hub, port->port_number);
-
-	if ((ret = usbhc_device_enumerate(exch, port->port_number))) {
-		usb_log_error("(%p-%u): Failed to enumerate device: %s", hub, port->port_number, str_error(ret));
-		goto out_port;
-	}
-
-	usb_log_debug("(%p-%u): Device enumerated", hub, port->port_number);
-	port->device_attached = true;
-
-out_port:
-	if (!port->device_attached && (ret = port_enable(port, hub, false))) {
-		usb_log_warning("(%p-%u)Failed to disable port (%s), NOT releasing default address.", hub, port->port_number, str_error(ret));
-		release_default_address = false;
-	}
-
-out_address:
-	if (release_default_address && (ret = usbhc_release_default_address(exch)))
-		usb_log_warning("(%p-%u): Failed to release default address: %s", hub, port->port_number, str_error(ret));
-
-out:
-	usb_device_bus_exchange_end(exch);
-
-	fibril_mutex_lock(&hub->pending_ops_mutex);
-	assert(hub->pending_ops_count > 0);
-	--hub->pending_ops_count;
-	fibril_condvar_signal(&hub->pending_ops_cv);
-	fibril_mutex_unlock(&hub->pending_ops_mutex);
-
-	return ret;
 }
 
@@ -467,30 +284,133 @@
  * @return Error code.
  */
-static int create_add_device_fibril(usb_hub_port_t *port, usb_hub_dev_t *hub,
-    usb_speed_t speed)
-{
+static int create_setup_device_fibril(usb_hub_port_t *port)
+{
+	assert(port);
+
+	fid_t fibril = fibril_create(setup_device_worker, port);
+	if (!fibril)
+		return ENOMEM;
+
+	fibril_add_ready(fibril);
+	return EOK;
+}
+
+static void port_changed_connection(usb_hub_port_t *port, usb_port_status_t status)
+{
+	const bool connected = !!(status & USB_HUB_PORT_STATUS_CONNECTION);
+	port_log(debug, port, "Connection change: device %s.", connected ? "attached" : "removed");
+
+	if (connected) {
+		if (port->state == PORT_ENABLED)
+			port_log(warning, port, "Connection detected on port that is currently enabled. Resetting.");
+
+		port_make_disabled(port);
+		port_change_state(port, PORT_CONNECTED);
+		port->speed = usb_port_speed(status);
+		create_setup_device_fibril(port);
+	} else {
+		port_make_disabled(port);
+	}
+}
+
+static void port_changed_enabled(usb_hub_port_t *port, usb_port_status_t status)
+{
+	const bool enabled = !!(status & USB_HUB_PORT_STATUS_ENABLED);
+	if (enabled) {
+		port_log(warning, port, "Port unexpectedly changed to enabled.");
+	} else {
+		port_make_disabled(port);
+	}
+}
+
+static void port_changed_suspend(usb_hub_port_t *port, usb_port_status_t status)
+{
+	port_log(error, port, "Port unexpectedly suspend. Weird, we do not support suspending!");
+}
+
+static void port_changed_overcurrent(usb_hub_port_t *port, usb_port_status_t status)
+{
+	const bool overcurrent = !!(status & USB_HUB_PORT_STATUS_OC);
+
+	/* According to the USB specs:
+	 * 11.13.5 Over-current Reporting and Recovery
+	 * Hub device is responsible for putting port in power off
+	 * mode. USB system software is responsible for powering port
+	 * back on when the over-current condition is gone */
+
+	port_make_disabled(port);
+
+	if (!overcurrent) {
+		const int err = usb_hub_set_port_feature(port->hub, port->port_number, USB_HUB_FEATURE_PORT_POWER);
+		if (err)
+			port_log(error, port, "Failed to set port power after OC: %s.", str_error(err));
+	}
+}
+
+static void port_changed_reset(usb_hub_port_t *port, usb_port_status_t status)
+{
+	const bool enabled = !!(status & USB_HUB_PORT_STATUS_ENABLED);
+
+	/* Check if someone is waiting for the result */
+	if (port->state != PORT_IN_RESET)
+		return;
+
+	port_change_state(port, enabled ? PORT_ENABLED : PORT_ERROR);
+}
+
+typedef void (*change_handler_t)(usb_hub_port_t *, usb_port_status_t);
+
+static const change_handler_t port_change_handlers [] = {
+	[USB_HUB_FEATURE_C_PORT_CONNECTION] = &port_changed_connection,
+	[USB_HUB_FEATURE_C_PORT_ENABLE] = &port_changed_enabled,
+	[USB_HUB_FEATURE_C_PORT_SUSPEND] = &port_changed_suspend,
+	[USB_HUB_FEATURE_C_PORT_OVER_CURRENT] = &port_changed_overcurrent,
+	[USB_HUB_FEATURE_C_PORT_RESET] = &port_changed_reset,
+	[sizeof(usb_port_status_t) * 8] = NULL,
+};
+
+/**
+ * Process interrupts on given port
+ *
+ * Accepts connection, over current and port reset change.
+ * @param port port structure
+ * @param hub hub representation
+ */
+void usb_hub_port_process_interrupt(usb_hub_port_t *port, usb_hub_dev_t *hub)
+{
+	assert(port);
 	assert(hub);
-	assert(port);
-	struct add_device_phase1 *data
-	    = malloc(sizeof(struct add_device_phase1));
-	if (data == NULL) {
-		return ENOMEM;
-	}
-	data->hub = hub;
-	data->port = port;
-	data->speed = speed;
-
-	fid_t fibril = fibril_create(add_device_phase1_worker_fibril, data);
-	if (fibril == 0) {
-		free(data);
-		return ENOMEM;
-	}
-	fibril_mutex_lock(&hub->pending_ops_mutex);
-	++hub->pending_ops_count;
-	fibril_mutex_unlock(&hub->pending_ops_mutex);
-	fibril_add_ready(fibril);
-
-	return EOK;
-}
+	port_log(debug2, port, "Interrupt.");
+
+	usb_port_status_t status = 0;
+	const int err = usb_hub_get_port_status(port->hub, port->port_number, &status);
+	if (err != EOK) {
+		port_log(error, port, "Failed to get port status: %s.", str_error(err));
+		return;
+	}
+
+	fibril_mutex_lock(&port->guard);
+
+	for (uint32_t feature = 0; feature < sizeof(usb_port_status_t) * 8; ++feature) {
+		uint32_t mask = 1 << feature;
+
+		if ((status & mask) == 0)
+			continue;
+
+		if (!port_change_handlers[feature])
+			continue;
+
+		/* ACK this change */
+		status &= ~mask;
+		usb_hub_clear_port_feature(port->hub, port->port_number, feature);
+
+		port_change_handlers[feature](port, status);
+	}
+
+	fibril_mutex_unlock(&port->guard);
+
+	port_log(debug2, port, "Port status after handling: %#08" PRIx32, status);
+}
+
 
 /**
Index: uspace/drv/bus/usb/usbhub/port.h
===================================================================
--- uspace/drv/bus/usb/usbhub/port.h	(revision d2c3dcd880ba1b8f4263eddeaae18bcc6c072bd9)
+++ uspace/drv/bus/usb/usbhub/port.h	(revision c4e84ed699e365187054b23f19f7c0c3c431291d)
@@ -2,4 +2,5 @@
  * Copyright (c) 2011 Vojtech Horky
  * Copyright (c) 2011 Jan Vesely
+ * Copyright (c) 2017 Ondra Hlavaty
  * All rights reserved.
  *
@@ -32,5 +33,5 @@
  */
 /** @file
- * Hub ports related functions.
+ * Hub port state machine.
  */
 
@@ -43,49 +44,31 @@
 typedef struct usb_hub_dev usb_hub_dev_t;
 
+typedef enum {
+	PORT_DISABLED,	/* No device connected. */
+	PORT_CONNECTED,	/* A device connected, not yet initialized. */
+	PORT_IN_RESET,	/* An initial port reset in progress. */
+	PORT_ENABLED,	/* Port reset complete, port enabled. Device announced to the HC. */
+	PORT_ERROR,	/* An error occured. There is still a fibril that needs to know it. */
+} port_state_t;
+
 /** Information about single port on a hub. */
 typedef struct {
+	/* Parenting hub */
+	usb_hub_dev_t *hub;
+	/** Guarding all fields */
+	fibril_mutex_t guard;
+	/** Current state of the port */
+	port_state_t state;
+	/** A speed of the device connected (if any). Valid unless state == PORT_DISABLED. */
+	usb_speed_t speed;
 	/** Port number as reported in descriptors. */
 	unsigned int port_number;
-	/** Device communication pipe. */
-	usb_pipe_t *control_pipe;
-	/** Mutex needed not only by CV for checking port reset. */
-	fibril_mutex_t mutex;
 	/** CV for waiting to port reset completion. */
-	fibril_condvar_t reset_cv;
-	/** Port reset status.
-	 * Guarded by @c reset_mutex.
-	 */
-	enum {
-		NO_RESET,
-		IN_RESET,
-		RESET_OK,
-		RESET_FAIL,
-	} reset_status;
-	/** Device reported to USB bus driver */
-	bool device_attached;
+	fibril_condvar_t state_cv;
 } usb_hub_port_t;
 
-/** Initialize hub port information.
- *
- * @param port Port to be initialized.
- */
-static inline void usb_hub_port_init(usb_hub_port_t *port,
-    unsigned int port_number, usb_pipe_t *control_pipe)
-{
-	assert(port);
-	port->port_number = port_number;
-	port->control_pipe = control_pipe;
-	port->reset_status = NO_RESET;
-	port->device_attached = false;
-	fibril_mutex_initialize(&port->mutex);
-	fibril_condvar_initialize(&port->reset_cv);
-}
+void usb_hub_port_init(usb_hub_port_t *, usb_hub_dev_t *, unsigned int);
+void usb_hub_port_fini(usb_hub_port_t *);
 
-int usb_hub_port_fini(usb_hub_port_t *port, usb_hub_dev_t *hub);
-int usb_hub_port_clear_feature(
-    usb_hub_port_t *port, usb_hub_class_feature_t feature);
-int usb_hub_port_set_feature(
-    usb_hub_port_t *port, usb_hub_class_feature_t feature);
-void usb_hub_port_reset_fail(usb_hub_port_t *port);
 void usb_hub_port_process_interrupt(usb_hub_port_t *port, usb_hub_dev_t *hub);
 
Index: uspace/drv/bus/usb/usbhub/usbhub.c
===================================================================
--- uspace/drv/bus/usb/usbhub/usbhub.c	(revision d2c3dcd880ba1b8f4263eddeaae18bcc6c072bd9)
+++ uspace/drv/bus/usb/usbhub/usbhub.c	(revision c4e84ed699e365187054b23f19f7c0c3c431291d)
@@ -86,6 +86,4 @@
     usb_hub_status_t status);
 static void usb_hub_global_interrupt(const usb_hub_dev_t *hub_dev);
-static void usb_hub_polling_terminated_callback(usb_device_t *device,
-    bool was_error, void *data);
 
 static bool usb_hub_polling_error_callback(usb_device_t *dev, int err_code, void *arg)
@@ -93,11 +91,8 @@
 	assert(dev);
 	assert(arg);
-	usb_hub_dev_t *hub = arg;
-
-	usb_log_error("Device %s polling error: %s", usb_device_get_name(dev),
-	    str_error(err_code));
-
-	/* Continue polling until the device is about to be removed. */
-	return hub->running;
+
+	usb_log_error("Device %s polling error: %s", usb_device_get_name(dev), str_error(err_code));
+
+	return true;
 }
 
@@ -121,8 +116,4 @@
 	}
 	hub_dev->usb_device = usb_dev;
-	hub_dev->pending_ops_count = 0;
-	hub_dev->running = false;
-	fibril_mutex_initialize(&hub_dev->pending_ops_mutex);
-	fibril_condvar_initialize(&hub_dev->pending_ops_cv);
 
 	/* Set hub's first configuration. (There should be only one) */
@@ -178,5 +169,4 @@
 	polling->buffer = malloc(polling->request_size);
 	polling->on_data = hub_port_changes_callback;
-	polling->on_polling_end = usb_hub_polling_terminated_callback;
 	polling->on_error = usb_hub_polling_error_callback;
 	polling->arg = hub_dev;
@@ -194,5 +184,4 @@
 	}
 
-	hub_dev->running = true;
 	usb_log_info("Controlling hub '%s' (%p: %zu ports).",
 	    usb_device_get_name(hub_dev->usb_device), hub_dev,
@@ -204,13 +193,9 @@
 static int usb_hub_cleanup(usb_hub_dev_t *hub)
 {
-	assert(!hub->running);
-
 	free(hub->polling.buffer);
 	usb_polling_fini(&hub->polling);
 
 	for (size_t port = 0; port < hub->port_count; ++port) {
-		const int ret = usb_hub_port_fini(&hub->ports[port], hub);
-		if (ret != EOK)
-			return ret;
+		usb_hub_port_fini(&hub->ports[port]);
 	}
 	free(hub->ports);
@@ -343,4 +328,5 @@
 	    descriptor.port_count);
 	hub_dev->port_count = descriptor.port_count;
+	hub_dev->control_pipe = control_pipe;
 
 	hub_dev->ports = calloc(hub_dev->port_count, sizeof(usb_hub_port_t));
@@ -350,6 +336,5 @@
 
 	for (size_t port = 0; port < hub_dev->port_count; ++port) {
-		usb_hub_port_init(
-		    &hub_dev->ports[port], port + 1, control_pipe);
+		usb_hub_port_init(&hub_dev->ports[port], hub_dev, port + 1);
 	}
 
@@ -370,6 +355,5 @@
 	for (unsigned int port = 0; port < hub_dev->port_count; ++port) {
 		usb_log_debug("(%p): Powering port %u.", hub_dev, port);
-		const int ret = usb_hub_port_set_feature(
-		    &hub_dev->ports[port], USB_HUB_FEATURE_PORT_POWER);
+		const int ret = usb_hub_set_port_feature(hub_dev, port, USB_HUB_FEATURE_PORT_POWER);
 
 		if (ret != EOK) {
@@ -459,6 +443,5 @@
 	/* Over-current condition is gone, it is safe to turn the ports on. */
 	for (size_t port = 0; port < hub_dev->port_count; ++port) {
-		const int ret = usb_hub_port_set_feature(
-		    &hub_dev->ports[port], USB_HUB_FEATURE_PORT_POWER);
+		const int ret = usb_hub_set_port_feature(hub_dev, port, USB_HUB_FEATURE_PORT_POWER);
 		if (ret != EOK) {
 			usb_log_warning("(%p-%u): HUB OVER-CURRENT GONE: Cannot"
@@ -470,5 +453,80 @@
 		}
 	}
-
+}
+
+/**
+ * Set feature on the real hub port.
+ *
+ * @param port Port structure.
+ * @param feature Feature selector.
+ */
+int usb_hub_set_port_feature(const usb_hub_dev_t *hub, size_t port_number, usb_hub_class_feature_t feature)
+{
+	assert(hub);
+	const usb_device_request_setup_packet_t clear_request = {
+		.request_type = USB_HUB_REQ_TYPE_SET_PORT_FEATURE,
+		.request = USB_DEVREQ_SET_FEATURE,
+		.index = uint16_host2usb(port_number),
+		.value = feature,
+		.length = 0,
+	};
+	return usb_pipe_control_write(hub->control_pipe, &clear_request,
+	    sizeof(clear_request), NULL, 0);
+}
+
+/**
+ * Clear feature on the real hub port.
+ *
+ * @param port Port structure.
+ * @param feature Feature selector.
+ */
+int usb_hub_clear_port_feature(const usb_hub_dev_t *hub, size_t port_number, usb_hub_class_feature_t feature)
+{
+	assert(hub);
+	const usb_device_request_setup_packet_t clear_request = {
+		.request_type = USB_HUB_REQ_TYPE_CLEAR_PORT_FEATURE,
+		.request = USB_DEVREQ_CLEAR_FEATURE,
+		.value = feature,
+		.index = uint16_host2usb(port_number),
+		.length = 0,
+	};
+	return usb_pipe_control_write(hub->control_pipe,
+	    &clear_request, sizeof(clear_request), NULL, 0);
+}
+
+/**
+ * Retrieve port status.
+ *
+ * @param[in] port Port structure
+ * @param[out] status Where to store the port status.
+ * @return Error code.
+ */
+int usb_hub_get_port_status(const usb_hub_dev_t *hub, size_t port_number, usb_port_status_t *status)
+{
+	assert(hub);
+	assert(status);
+
+	/* USB hub specific GET_PORT_STATUS request. See USB Spec 11.16.2.6
+	 * Generic GET_STATUS request cannot be used because of the difference
+	 * in status data size (2B vs. 4B)*/
+	const usb_device_request_setup_packet_t request = {
+		.request_type = USB_HUB_REQ_TYPE_GET_PORT_STATUS,
+		.request = USB_HUB_REQUEST_GET_STATUS,
+		.value = 0,
+		.index = uint16_host2usb(port_number),
+		.length = sizeof(usb_port_status_t),
+	};
+	size_t recv_size;
+
+	const int rc = usb_pipe_control_read(hub->control_pipe,
+	    &request, sizeof(usb_device_request_setup_packet_t),
+	    status, sizeof(*status), &recv_size);
+	if (rc != EOK)
+		return rc;
+
+	if (recv_size != sizeof(*status))
+		return ELIMIT;
+
+	return EOK;
 }
 
@@ -545,42 +603,4 @@
 
 /**
- * callback called from hub polling fibril when the fibril terminates
- *
- * Does not perform cleanup, just marks the hub as not running.
- * @param device usb device afected
- * @param was_error indicates that the fibril is stoped due to an error
- * @param data pointer to usb_hub_dev_t structure
- */
-static void usb_hub_polling_terminated_callback(usb_device_t *device,
-    bool was_error, void *data)
-{
-	usb_hub_dev_t *hub = data;
-	assert(hub);
-
-	fibril_mutex_lock(&hub->pending_ops_mutex);
-
-	/* The device is dead. However there might be some pending operations
-	 * that we need to wait for.
-	 * One of them is device adding in progress.
-	 * The respective fibril is probably waiting for status change
-	 * in port reset (port enable) callback.
-	 * Such change would never come (otherwise we would not be here).
-	 * Thus, we would flush all pending port resets.
-	 */
-	if (hub->pending_ops_count > 0) {
-		for (size_t port = 0; port < hub->port_count; ++port) {
-			usb_hub_port_reset_fail(&hub->ports[port]);
-		}
-	}
-	/* And now wait for them. */
-	while (hub->pending_ops_count > 0) {
-		fibril_condvar_wait(&hub->pending_ops_cv,
-		    &hub->pending_ops_mutex);
-	}
-	fibril_mutex_unlock(&hub->pending_ops_mutex);
-	hub->running = false;
-}
-
-/**
  * @}
  */
Index: uspace/drv/bus/usb/usbhub/usbhub.h
===================================================================
--- uspace/drv/bus/usb/usbhub/usbhub.h	(revision d2c3dcd880ba1b8f4263eddeaae18bcc6c072bd9)
+++ uspace/drv/bus/usb/usbhub/usbhub.h	(revision c4e84ed699e365187054b23f19f7c0c3c431291d)
@@ -50,4 +50,5 @@
 
 #include "port.h"
+#include "status.h"
 
 /** Information about attached hub. */
@@ -61,19 +62,8 @@
 	/** Data polling handle. */
 	usb_polling_t polling;
-	/** Number of pending operations on the mutex to prevent shooting
-	 * ourselves in the foot.
-	 * When the hub is disconnected but we are in the middle of some
-	 * operation, we cannot destroy this structure right away because
-	 * the pending operation might use it.
-	 */
-	size_t pending_ops_count;
-	/** Guard for pending_ops_count. */
-	fibril_mutex_t pending_ops_mutex;
-	/** Condition variable for pending_ops_count. */
-	fibril_condvar_t pending_ops_cv;
 	/** Pointer to usbhub function. */
 	ddf_fun_t *hub_fun;
-	/** Status indicator */
-	volatile bool running;
+	/** Device communication pipe. */
+	usb_pipe_t *control_pipe;
 	/** Hub supports port power switching. */
 	bool power_switched;
@@ -84,10 +74,13 @@
 extern const usb_endpoint_description_t hub_status_change_endpoint_description;
 
-extern int usb_hub_device_add(usb_device_t *);
-extern int usb_hub_device_remove(usb_device_t *);
-extern int usb_hub_device_gone(usb_device_t *);
+int usb_hub_device_add(usb_device_t *);
+int usb_hub_device_remove(usb_device_t *);
+int usb_hub_device_gone(usb_device_t *);
 
-extern bool hub_port_changes_callback(usb_device_t *, uint8_t *, size_t,
-    void *);
+int usb_hub_get_port_status(const usb_hub_dev_t *, size_t, usb_port_status_t *);
+int usb_hub_set_port_feature(const usb_hub_dev_t *, size_t, usb_hub_class_feature_t);
+int usb_hub_clear_port_feature(const usb_hub_dev_t *, size_t, usb_hub_class_feature_t);
+
+bool hub_port_changes_callback(usb_device_t *, uint8_t *, size_t, void *);
 
 #endif