Index: HelenOS.config
===================================================================
--- HelenOS.config	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ HelenOS.config	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -573,5 +573,7 @@
 ! CONFIG_RUN_VIRTUAL_USB_HC (n/y)
 
-% Polling UHCI & OHCI (no interrupts)
-! [PLATFORM=ia32|PLATFORM=amd64] CONFIG_USBHC_NO_INTERRUPTS (n/y)
-
+% OHCI root hub port power switching
+@ "no" All root hub ports are always powered.
+@ "ganged" Root hub ports are all powered or all off.
+@ "per_port" Powered status of every root hub port is independent.
+![PLATFORM=ia32|PLATFORM=amd64] OHCI_POWER_SWITCH (choice)
Index: defaults/amd64/Makefile.config
===================================================================
--- defaults/amd64/Makefile.config	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ defaults/amd64/Makefile.config	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -65,2 +65,4 @@
 CONFIG_MOUNT_DATA = n
 
+# OHCI root hub power switch, ganged is enough
+OHCI_POWER_SWITCH = ganged
Index: defaults/ia32/Makefile.config
===================================================================
--- defaults/ia32/Makefile.config	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ defaults/ia32/Makefile.config	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -71,2 +71,4 @@
 CONFIG_MOUNT_DATA = n
 
+# OHCI root hub power switch, ganged is enough
+OHCI_POWER_SWITCH = ganged
Index: uspace/drv/bus/usb/ohci/hc.c
===================================================================
--- uspace/drv/bus/usb/ohci/hc.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/ohci/hc.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -46,8 +46,73 @@
 #define OHCI_USED_INTERRUPTS \
     (I_SO | I_WDH | I_UE | I_RHSC)
-static int interrupt_emulator(hc_t *instance);
+
+static const irq_cmd_t ohci_irq_commands[] =
+{
+	{ .cmd = CMD_MEM_READ_32, .dstarg = 1, .addr = NULL /*filled later*/ },
+	{ .cmd = CMD_BTEST, .srcarg = 1, .dstarg = 2, .value = OHCI_USED_INTERRUPTS },
+	{ .cmd = CMD_PREDICATE, .srcarg = 2, .value = 2 },
+	{ .cmd = CMD_MEM_WRITE_A_32, .srcarg = 1, .addr = NULL /*filled later*/ },
+	{ .cmd = CMD_ACCEPT },
+};
+
 static void hc_gain_control(hc_t *instance);
+static void hc_start(hc_t *instance);
 static int hc_init_transfer_lists(hc_t *instance);
 static int hc_init_memory(hc_t *instance);
+static int interrupt_emulator(hc_t *instance);
+
+/*----------------------------------------------------------------------------*/
+/** Get number of commands used in IRQ code.
+ * @return Number of commands.
+ */
+size_t hc_irq_cmd_count(void)
+{
+	return sizeof(ohci_irq_commands) / sizeof(irq_cmd_t);
+}
+/*----------------------------------------------------------------------------*/
+/** Generate IRQ code commands.
+ * @param[out] cmds Place to store the commands.
+ * @param[in] cmd_size Size of the place (bytes).
+ * @param[in] regs Physical address of device's registers.
+ * @param[in] reg_size Size of the register area (bytes).
+ *
+ * @return Error code.
+ */
+int hc_get_irq_commands(
+    irq_cmd_t cmds[], size_t cmd_size, uintptr_t regs, size_t reg_size)
+{
+	if (cmd_size < sizeof(ohci_irq_commands)
+	    || reg_size < sizeof(ohci_regs_t))
+		return EOVERFLOW;
+
+	/* Create register mapping to use in IRQ handler.
+	 * This mapping should be present in kernel only.
+	 * Remove it from here when kernel knows how to create mappings
+	 * and accepts physical addresses in IRQ code.
+	 * TODO: remove */
+	ohci_regs_t *registers;
+	const int ret = pio_enable((void*)regs, reg_size, (void**)&registers);
+	if (ret != EOK)
+		return ret;
+
+	/* Some bogus access to force create mapping. DO NOT remove,
+	 * unless whole virtual addresses in irq is replaced
+	 * NOTE: Compiler won't remove this as ohci_regs_t members
+	 * are declared volatile.
+	 *
+	 * Introducing CMD_MEM set of IRQ code commands broke
+	 * assumption that IRQ code does not cause page faults.
+	 * If this happens during idling (THREAD == NULL)
+	 * it causes kernel panic.
+	 */
+	registers->revision;
+
+	memcpy(cmds, ohci_irq_commands, sizeof(ohci_irq_commands));
+
+	void *address = (void*)&registers->interrupt_status;
+	cmds[0].addr = address;
+	cmds[3].addr = address;
+	return EOK;
+}
 /*----------------------------------------------------------------------------*/
 /** Announce OHCI root hub to the DDF
@@ -83,17 +148,21 @@
 	int ret = hc_add_endpoint(instance, hub_address, 0, USB_SPEED_FULL,
 	    USB_TRANSFER_CONTROL, USB_DIRECTION_BOTH, 64, 0, 0);
-	CHECK_RET_RELEASE(ret, "Failed(%d) to add OHCI rh endpoint 0.\n", ret);
+	CHECK_RET_RELEASE(ret,
+	    "Failed to add OHCI root hub endpoint 0: %s.\n", str_error(ret));
 
 	char *match_str = NULL;
-	/* DDF needs heap allocated string */
+	/* DDF needs heap allocated string. */
 	ret = asprintf(&match_str, "usb&class=hub");
 	ret = ret > 0 ? 0 : ret;
-	CHECK_RET_RELEASE(ret, "Failed(%d) to create match-id string.\n", ret);
+	CHECK_RET_RELEASE(ret,
+	    "Failed to create match-id string: %s.\n", str_error(ret));
 
 	ret = ddf_fun_add_match_id(hub_fun, match_str, 100);
-	CHECK_RET_RELEASE(ret, "Failed(%d) add root hub match-id.\n", ret);
+	CHECK_RET_RELEASE(ret,
+	    "Failed to add root hub match-id: %s.\n", str_error(ret));
 
 	ret = ddf_fun_bind(hub_fun);
-	CHECK_RET_RELEASE(ret, "Failed(%d) to bind root hub function.\n", ret);
+	CHECK_RET_RELEASE(ret,
+	    "Failed to bind root hub function: %s.\n", str_error(ret));
 
 	return EOK;
@@ -112,5 +181,5 @@
 {
 	assert(instance);
-	int ret = EOK;
+
 #define CHECK_RET_RETURN(ret, message...) \
 if (ret != EOK) { \
@@ -119,11 +188,12 @@
 } else (void)0
 
-	ret = pio_enable((void*)regs, reg_size, (void**)&instance->registers);
+	int ret =
+	    pio_enable((void*)regs, reg_size, (void**)&instance->registers);
 	CHECK_RET_RETURN(ret,
-	    "Failed(%d) to gain access to device registers: %s.\n",
-	    ret, str_error(ret));
+	    "Failed to gain access to device registers: %s.\n", str_error(ret));
 
 	list_initialize(&instance->pending_batches);
 	usb_device_keeper_init(&instance->manager);
+
 	ret = usb_endpoint_manager_init(&instance->ep_manager,
 	    BANDWIDTH_AVAILABLE_USB11);
@@ -137,7 +207,6 @@
 
 	fibril_mutex_initialize(&instance->guard);
+
 	hc_gain_control(instance);
-
-	rh_init(&instance->rh, instance->registers);
 
 	if (!interrupts) {
@@ -147,8 +216,11 @@
 	}
 
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-/** Create end register endpoint structures
+	rh_init(&instance->rh, instance->registers);
+	hc_start(instance);
+
+	return EOK;
+}
+/*----------------------------------------------------------------------------*/
+/** Create and register endpoint structures.
  *
  * @param[in] instance OHCI driver structure.
@@ -168,13 +240,8 @@
     size_t mps, size_t size, unsigned interval)
 {
-	endpoint_t *ep = malloc(sizeof(endpoint_t));
+	endpoint_t *ep =
+	    endpoint_get(address, endpoint, direction, type, speed, mps);
 	if (ep == NULL)
 		return ENOMEM;
-	int ret =
-	    endpoint_init(ep, address, endpoint, direction, type, speed, mps);
-	if (ret != EOK) {
-		free(ep);
-		return ret;
-	}
 
 	hcd_endpoint_t *hcd_ep = hcd_endpoint_assign(ep);
@@ -184,5 +251,6 @@
 	}
 
-	ret = usb_endpoint_manager_register_ep(&instance->ep_manager, ep, size);
+	int ret =
+	    usb_endpoint_manager_register_ep(&instance->ep_manager, ep, size);
 	if (ret != EOK) {
 		hcd_endpoint_clear(ep);
@@ -212,6 +280,4 @@
 		    &instance->lists[ep->transfer_type], hcd_ep);
 		instance->registers->control |= C_PLE | C_IE;
-		break;
-	default:
 		break;
 	}
@@ -312,5 +378,6 @@
 	/* Check for root hub communication */
 	if (batch->ep->address == instance->rh.address) {
-		return rh_request(&instance->rh, batch);
+		rh_request(&instance->rh, batch);
+		return EOK;
 	}
 
@@ -374,5 +441,5 @@
 
 	if (status & I_UE) {
-		hc_start_hw(instance);
+		hc_start(instance);
 	}
 
@@ -399,4 +466,7 @@
 /** Turn off any (BIOS)driver that might be in control of the device.
  *
+ * This function implements routines described in chapter 5.1.1.3 of the OHCI
+ * specification (page 40, pdf page 54).
+ *
  * @param[in] instance OHCI hc driver structure.
  */
@@ -404,14 +474,22 @@
 {
 	assert(instance);
+
 	usb_log_debug("Requesting OHCI control.\n");
-	/* Turn off legacy emulation */
-	volatile uint32_t *ohci_emulation_reg =
-	    (uint32_t*)((char*)instance->registers + 0x100);
-	usb_log_debug("OHCI legacy register %p: %x.\n",
-	    ohci_emulation_reg, *ohci_emulation_reg);
-	/* Do not change A20 state */
-	*ohci_emulation_reg &= 0x100;
-	usb_log_debug("OHCI legacy register %p: %x.\n",
-	    ohci_emulation_reg, *ohci_emulation_reg);
+	if (instance->registers->revision & R_LEGACY_FLAG) {
+		/* Turn off legacy emulation, it should be enough to zero
+		 * the lowest bit, but it caused problems. Thus clear all
+		 * except GateA20 (causes restart on some hw).
+		 * See page 145 of the specs for details.
+		 */
+		volatile uint32_t *ohci_emulation_reg =
+		(uint32_t*)((char*)instance->registers + LEGACY_REGS_OFFSET);
+		usb_log_debug("OHCI legacy register %p: %x.\n",
+		    ohci_emulation_reg, *ohci_emulation_reg);
+		/* Zero everything but A20State */
+		*ohci_emulation_reg &= 0x100;
+		usb_log_debug(
+		    "OHCI legacy register (should be 0 or 0x100) %p: %x.\n",
+		    ohci_emulation_reg, *ohci_emulation_reg);
+	}
 
 	/* Interrupt routing enabled => smm driver is active */
@@ -419,15 +497,15 @@
 		usb_log_debug("SMM driver: request ownership change.\n");
 		instance->registers->command_status |= CS_OCR;
+		/* Hope that SMM actually knows its stuff or we can hang here */
 		while (instance->registers->control & C_IR) {
 			async_usleep(1000);
 		}
 		usb_log_info("SMM driver: Ownership taken.\n");
-		instance->registers->control &= (C_HCFS_RESET << C_HCFS_SHIFT);
+		C_HCFS_SET(instance->registers->control, C_HCFS_RESET);
 		async_usleep(50000);
 		return;
 	}
 
-	const unsigned hc_status =
-	    (instance->registers->control >> C_HCFS_SHIFT) & C_HCFS_MASK;
+	const unsigned hc_status = C_HCFS_GET(instance->registers->control);
 	/* Interrupt routing disabled && status != USB_RESET => BIOS active */
 	if (hc_status != C_HCFS_RESET) {
@@ -437,6 +515,6 @@
 			return;
 		}
-		/* HC is suspended assert resume for 20ms */
-		instance->registers->control &= (C_HCFS_RESUME << C_HCFS_SHIFT);
+		/* HC is suspended assert resume for 20ms, */
+		C_HCFS_SET(instance->registers->control, C_HCFS_RESUME);
 		async_usleep(20000);
 		usb_log_info("BIOS driver: HC resumed.\n");
@@ -454,5 +532,5 @@
  * @param[in] instance OHCI hc driver structure.
  */
-void hc_start_hw(hc_t *instance)
+void hc_start(hc_t *instance)
 {
 	/* OHCI guide page 42 */
@@ -516,5 +594,5 @@
 	    instance->registers->periodic_start, frame_length);
 
-	instance->registers->control &= (C_HCFS_OPERATIONAL << C_HCFS_SHIFT);
+	C_HCFS_SET(instance->registers->control, C_HCFS_OPERATIONAL);
 	usb_log_debug("OHCI HC up and running (ctl_reg=0x%x).\n",
 	    instance->registers->control);
@@ -534,6 +612,6 @@
 	int ret = endpoint_list_init(&instance->lists[type], name); \
 	if (ret != EOK) { \
-		usb_log_error("Failed(%d) to setup %s endpoint list.\n", \
-		    ret, name); \
+		usb_log_error("Failed to setup %s endpoint list: %s.\n", \
+		    name, str_error(ret)); \
 		endpoint_list_fini(&instance->lists[USB_TRANSFER_ISOCHRONOUS]);\
 		endpoint_list_fini(&instance->lists[USB_TRANSFER_INTERRUPT]); \
@@ -587,38 +665,7 @@
 	    instance->lists[USB_TRANSFER_INTERRUPT].list_head_pa);
 
-	/* Init interrupt code */
-	instance->interrupt_code.cmds = instance->interrupt_commands;
-	instance->interrupt_code.cmdcount = OHCI_NEEDED_IRQ_COMMANDS;
-	{
-		/* Read status register */
-		instance->interrupt_commands[0].cmd = CMD_MEM_READ_32;
-		instance->interrupt_commands[0].dstarg = 1;
-		instance->interrupt_commands[0].addr =
-		    (void*)&instance->registers->interrupt_status;
-
-		/* Test whether we are the interrupt cause */
-		instance->interrupt_commands[1].cmd = CMD_BTEST;
-		instance->interrupt_commands[1].value =
-		    OHCI_USED_INTERRUPTS;
-		instance->interrupt_commands[1].srcarg = 1;
-		instance->interrupt_commands[1].dstarg = 2;
-
-		/* Predicate cleaning and accepting */
-		instance->interrupt_commands[2].cmd = CMD_PREDICATE;
-		instance->interrupt_commands[2].value = 2;
-		instance->interrupt_commands[2].srcarg = 2;
-
-		/* Write-clean status register */
-		instance->interrupt_commands[3].cmd = CMD_MEM_WRITE_A_32;
-		instance->interrupt_commands[3].srcarg = 1;
-		instance->interrupt_commands[3].addr =
-		    (void*)&instance->registers->interrupt_status;
-
-		/* Accept interrupt */
-		instance->interrupt_commands[4].cmd = CMD_ACCEPT;
-	}
-
-	return EOK;
-}
+	return EOK;
+}
+
 /**
  * @}
Index: uspace/drv/bus/usb/ohci/hc.h
===================================================================
--- uspace/drv/bus/usb/ohci/hc.h	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/ohci/hc.h	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -51,7 +51,5 @@
 #include "hw_struct/hcca.h"
 
-#define OHCI_NEEDED_IRQ_COMMANDS 5
-
-/** Main OHCI drier structure */
+/** Main OHCI driver structure */
 typedef struct hc {
 	/** USB bus driver, devices and addresses */
@@ -76,17 +74,13 @@
 	fibril_mutex_t guard;
 
-	/** Code to be executed in kernel interrupt handler */
-	irq_code_t interrupt_code;
-
-	/** Commands that form interrupt code */
-	irq_cmd_t interrupt_commands[OHCI_NEEDED_IRQ_COMMANDS];
-
 	/** USB hub emulation structure */
 	rh_t rh;
 } hc_t;
 
+size_t hc_irq_cmd_count(void);
+int hc_get_irq_commands(
+    irq_cmd_t cmds[], size_t cmd_size, uintptr_t regs, size_t reg_size);
+int hc_init(hc_t *instance, uintptr_t regs, size_t reg_size, bool interrupts);
 int hc_register_hub(hc_t *instance, ddf_fun_t *hub_fun);
-int hc_init(hc_t *instance, uintptr_t regs, size_t reg_size, bool interrupts);
-void hc_start_hw(hc_t *instance);
 
 /** Safely dispose host controller internal structures
Index: uspace/drv/bus/usb/ohci/ohci.c
===================================================================
--- uspace/drv/bus/usb/ohci/ohci.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/ohci/ohci.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -58,8 +58,6 @@
 {
 	assert(dev);
-	assert(dev->driver_data);
 	return dev->driver_data;
 }
-
 /** IRQ handling callback, identifies device
  *
@@ -70,8 +68,13 @@
 static void irq_handler(ddf_dev_t *dev, ipc_callid_t iid, ipc_call_t *call)
 {
-	hc_t *hc = &dev_to_ohci(dev)->hc;
-	assert(hc);
+	assert(dev);
+
+	ohci_t *ohci = dev_to_ohci(dev);
+	if (!ohci) {
+		usb_log_warning("Interrupt on device that is not ready.\n");
+		return;
+	}
 	const uint16_t status = IPC_GET_ARG1(*call);
-	hc_interrupt(hc, status);
+	hc_interrupt(&ohci->hc, status);
 }
 /*----------------------------------------------------------------------------*/
@@ -166,4 +169,5 @@
 	} \
 	free(instance); \
+	device->driver_data = NULL; \
 	usb_log_error(message); \
 	return ret; \
@@ -173,5 +177,6 @@
 	instance->hc_fun = ddf_fun_create(device, fun_exposed, "ohci_hc");
 	int ret = instance->hc_fun ? EOK : ENOMEM;
-	CHECK_RET_DEST_FREE_RETURN(ret, "Failed to create OHCI HC function.\n");
+	CHECK_RET_DEST_FREE_RETURN(ret,
+	    "Failed to create OHCI HC function: %s.\n", str_error(ret));
 	instance->hc_fun->ops = &hc_ops;
 	instance->hc_fun->driver_data = &instance->hc;
@@ -179,5 +184,6 @@
 	instance->rh_fun = ddf_fun_create(device, fun_inner, "ohci_rh");
 	ret = instance->rh_fun ? EOK : ENOMEM;
-	CHECK_RET_DEST_FREE_RETURN(ret, "Failed to create OHCI RH function.\n");
+	CHECK_RET_DEST_FREE_RETURN(ret,
+	    "Failed to create OHCI RH function: %s.\n", str_error(ret));
 	instance->rh_fun->ops = &rh_ops;
 
@@ -193,40 +199,48 @@
 	    (void *) reg_base, reg_size, irq);
 
+	const size_t cmd_count = hc_irq_cmd_count();
+	irq_cmd_t irq_cmds[cmd_count];
+	ret =
+	    hc_get_irq_commands(irq_cmds, sizeof(irq_cmds), reg_base, reg_size);
+	CHECK_RET_DEST_FREE_RETURN(ret,
+	    "Failed to generate IRQ commands: %s.\n", str_error(ret));
+
+	irq_code_t irq_code = { .cmdcount = cmd_count, .cmds = irq_cmds };
+
+	/* Register handler to avoid interrupt lockup */
+	ret = register_interrupt_handler(device, irq, irq_handler, &irq_code);
+	CHECK_RET_DEST_FREE_RETURN(ret,
+	    "Failed to register interrupt handler: %s.\n", str_error(ret));
+
+	/* Try to enable interrupts */
 	bool interrupts = false;
-#ifdef CONFIG_USBHC_NO_INTERRUPTS
-	usb_log_warning("Interrupts disabled in OS config, "
-	    "falling back to polling.\n");
-#else
 	ret = pci_enable_interrupts(device);
 	if (ret != EOK) {
-		usb_log_warning("Failed to enable interrupts: %s.\n",
-		    str_error(ret));
-		usb_log_info("HW interrupts not available, "
-		    "falling back to polling.\n");
+		usb_log_warning("Failed to enable interrupts: %s."
+		    " Falling back to polling\n", str_error(ret));
+		/* We don't need that handler */
+		unregister_interrupt_handler(device, irq);
 	} else {
 		usb_log_debug("Hw interrupts enabled.\n");
 		interrupts = true;
 	}
-#endif
 
 	ret = hc_init(&instance->hc, reg_base, reg_size, interrupts);
-	CHECK_RET_DEST_FREE_RETURN(ret, "Failed(%d) to init ohci_hcd.\n", ret);
+	CHECK_RET_DEST_FREE_RETURN(ret,
+	    "Failed to init ohci_hcd: %s.\n", str_error(ret));
+
+	device->driver_data = instance;
 
 #define CHECK_RET_FINI_RETURN(ret, message...) \
 if (ret != EOK) { \
+	unregister_interrupt_handler(device, irq); \
 	hc_fini(&instance->hc); \
 	CHECK_RET_DEST_FREE_RETURN(ret, message); \
 } else (void)0
 
-	/* It does no harm if we register this on polling */
-	ret = register_interrupt_handler(device, irq, irq_handler,
-	    &instance->hc.interrupt_code);
-	CHECK_RET_FINI_RETURN(ret,
-	    "Failed(%d) to register interrupt handler.\n", ret);
 
 	ret = ddf_fun_bind(instance->hc_fun);
 	CHECK_RET_FINI_RETURN(ret,
-	    "Failed(%d) to bind OHCI device function: %s.\n",
-	    ret, str_error(ret));
+	    "Failed to bind OHCI device function: %s.\n", str_error(ret));
 
 	ret = ddf_fun_add_to_class(instance->hc_fun, USB_HC_DDF_CLASS_NAME);
@@ -234,7 +248,4 @@
 	    "Failed to add OHCI to HC class: %s.\n", str_error(ret));
 
-	device->driver_data = instance;
-
-	hc_start_hw(&instance->hc);
 	hc_register_hub(&instance->hc, instance->rh_fun);
 	return EOK;
Index: uspace/drv/bus/usb/ohci/ohci_regs.h
===================================================================
--- uspace/drv/bus/usb/ohci/ohci_regs.h	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/ohci/ohci_regs.h	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -34,16 +34,22 @@
 #ifndef DRV_OHCI_OHCI_REGS_H
 #define DRV_OHCI_OHCI_REGS_H
-#include <stdint.h>
+#include <sys/types.h>
+
+#define LEGACY_REGS_OFFSET 0x100
 
 /** OHCI memory mapped registers structure */
 typedef struct ohci_regs {
-	const volatile uint32_t revision;
-	volatile uint32_t control;
-#define C_CSBR_MASK (0x3) /* Control-bulk service ratio */
-#define C_CSBR_1_1  (0x0)
-#define C_CSBR_1_2  (0x1)
-#define C_CSBR_1_3  (0x2)
-#define C_CSBR_1_4  (0x3)
-#define C_CSBR_SHIFT (0)
+	const ioport32_t revision;
+#define R_REVISION_MASK (0x3f)
+#define R_REVISION_SHIFT (0)
+#define R_LEGACY_FLAG   (0x80)
+
+	ioport32_t control;
+#define C_CBSR_MASK (0x3) /* Control-bulk service ratio */
+#define C_CBSR_1_1  (0x0)
+#define C_CBSR_1_2  (0x1)
+#define C_CBSR_1_3  (0x2)
+#define C_CBSR_1_4  (0x3)
+#define C_CBSR_SHIFT (0)
 
 #define C_PLE (1 << 2)   /* Periodic list enable */
@@ -59,9 +65,18 @@
 #define C_HCFS_SHIFT       (6)
 
+#define C_HCFS_GET(reg) \
+	((reg >> C_HCFS_SHIFT) & C_HCFS_MASK)
+#define C_HCFS_SET(reg, hcfs_state) \
+do { \
+	reg = (reg & ~(C_HCFS_MASK << C_HCFS_SHIFT)) \
+	    | ((hcfs_state & C_HCFS_MASK) << C_HCFS_SHIFT); \
+} while (0)
+
+
 #define C_IR  (1 << 8)   /* Interrupt routing, make sure it's 0 */
 #define C_RWC (1 << 9)   /* Remote wakeup connected, host specific */
 #define C_RWE (1 << 10)  /* Remote wakeup enable */
 
-	volatile uint32_t command_status;
+	ioport32_t command_status;
 #define CS_HCR (1 << 0)   /* Host controller reset */
 #define CS_CLF (1 << 1)   /* Control list filled */
@@ -75,7 +90,7 @@
 	 * writing causes enable/disable,
 	 * status is write-clean (writing 1 clears the bit*/
-	volatile uint32_t interrupt_status;
-	volatile uint32_t interrupt_enable;
-	volatile uint32_t interrupt_disable;
+	ioport32_t interrupt_status;
+	ioport32_t interrupt_enable;
+	ioport32_t interrupt_disable;
 #define I_SO   (1 << 0)   /* Scheduling overrun */
 #define I_WDH  (1 << 1)   /* Done head write-back */
@@ -89,27 +104,27 @@
 
 	/** HCCA pointer (see hw_struct hcca.h) */
-	volatile uint32_t hcca;
+	ioport32_t hcca;
 #define HCCA_PTR_MASK 0xffffff00 /* HCCA is 256B aligned */
 
 	/** Currently executed periodic endpoint */
-	const volatile uint32_t periodic_current;
+	const ioport32_t periodic_current;
 
 	/** The first control endpoint */
-	volatile uint32_t control_head;
+	ioport32_t control_head;
 
 	/** Currently executed control endpoint */
-	volatile uint32_t control_current;
+	ioport32_t control_current;
 
 	/** The first bulk endpoint */
-	volatile uint32_t bulk_head;
+	ioport32_t bulk_head;
 
 	/** Currently executed bulk endpoint */
-	volatile uint32_t bulk_current;
+	ioport32_t bulk_current;
 
 	/** Done TD list, this value is periodically written to HCCA */
-	const volatile uint32_t done_head;
+	const ioport32_t done_head;
 
 	/** Frame time and max packet size for all transfers */
-	volatile uint32_t fm_interval;
+	ioport32_t fm_interval;
 #define FMI_FI_MASK (0x3fff) /* Frame interval in bit times (should be 11999)*/
 #define FMI_FI_SHIFT (0)
@@ -119,5 +134,5 @@
 
 	/** Bit times remaining in current frame */
-	const volatile uint32_t fm_remaining;
+	const ioport32_t fm_remaining;
 #define FMR_FR_MASK FMI_FI_MASK
 #define FMR_FR_SHIFT FMI_FI_SHIFT
@@ -125,17 +140,17 @@
 
 	/** Frame number */
-	const volatile uint32_t fm_number;
+	const ioport32_t fm_number;
 #define FMN_NUMBER_MASK (0xffff)
 
 	/** Remaining bit time in frame to start periodic transfers */
-	volatile uint32_t periodic_start;
+	ioport32_t periodic_start;
 #define PS_PS_MASK (0x3fff) /* bit time when periodic get priority (0x3e67) */
 
 	/** Threshold for starting LS transaction */
-	volatile uint32_t ls_threshold;
+	ioport32_t ls_threshold;
 #define LST_LST_MASK (0x7fff)
 
 	/** The first root hub control register */
-	volatile uint32_t rh_desc_a;
+	ioport32_t rh_desc_a;
 #define RHDA_NDS_MASK (0xff) /* Number of downstream ports, max 15 */
 #define RHDA_NDS_SHIFT (0)
@@ -144,10 +159,10 @@
 #define RHDA_DT_FLAG   (1 << 10) /* 1-Compound device, must be 0 */
 #define RHDA_OCPM_FLAG (1 << 11) /* Over-current mode: 0-global, 1-per port */
-#define RHDA_NOCP      (1 << 12) /* OC control: 0-use OCPM, 1-OC off */
+#define RHDA_NOCP_FLAG (1 << 12) /* OC control: 0-use OCPM, 1-OC off */
 #define RHDA_POTPGT_MASK (0xff)  /* Power on to power good time */
 #define RHDA_POTPGT_SHIFT (24)
 
 	/** The other root hub control register */
-	volatile uint32_t rh_desc_b;
+	ioport32_t rh_desc_b;
 #define RHDB_DR_MASK (0xffff) /* Device removable mask */
 #define RHDB_DR_SHIFT (0)
@@ -161,5 +176,5 @@
 
 	/** Root hub status register */
-	volatile uint32_t rh_status;
+	ioport32_t rh_status;
 #define RHS_LPS_FLAG  (1 <<  0)/* read: 0,
                                 * write: 0-no effect,
@@ -167,5 +182,5 @@
                                 *        specified in PPCM(RHDB), or all ports,
                                 *        if power is set globally */
-#define RHS_CLEAR_PORT_POWER RHS_LPS_FLAG /* synonym for the above */
+#define RHS_CLEAR_GLOBAL_POWER RHS_LPS_FLAG /* synonym for the above */
 #define RHS_OCI_FLAG  (1 <<  1)/* Over-current indicator, if per-port: 0 */
 #define RHS_DRWE_FLAG (1 << 15)/* read: 0-connect status change does not wake HC
@@ -178,10 +193,10 @@
                                 *        specified in PPCM(RHDB), or all ports,
                                 *        if power is set globally */
-#define RHS_SET_PORT_POWER RHS_LPSC_FLAG /* synonym for the above */
+#define RHS_SET_GLOBAL_POWER RHS_LPSC_FLAG /* synonym for the above */
 #define RHS_OCIC_FLAG (1 << 17)/* Over-current indicator change   */
 #define RHS_CLEAR_DRWE (1 << 31)
 
 	/** Root hub per port status */
-	volatile uint32_t rh_port_status[];
+	ioport32_t rh_port_status[];
 #define RHPS_CCS_FLAG (1 << 0) /* r: current connect status,
                                 * w: 1-clear port enable, 0-nothing */
Index: uspace/drv/bus/usb/ohci/root_hub.c
===================================================================
--- uspace/drv/bus/usb/ohci/root_hub.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/ohci/root_hub.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -40,4 +40,5 @@
 #include "root_hub.h"
 #include <usb/classes/classes.h>
+#include <usb/classes/hub.h>
 #include <usb/dev/driver.h>
 #include "ohci_regs.h"
@@ -56,7 +57,7 @@
 	.device_subclass = 0,
 	.device_version = 0,
-	.length = sizeof (usb_standard_device_descriptor_t),
-	.max_packet_size = 8,
-	.vendor_id = 0x16db,
+	.length = sizeof(usb_standard_device_descriptor_t),
+	.max_packet_size = 64,
+	.vendor_id = 0x16db, /* HelenOS does not have USB vendor ID assigned.*/
 	.product_id = 0x0001,
 	.str_serial_number = 0,
@@ -73,6 +74,6 @@
 	.descriptor_type = USB_DESCTYPE_CONFIGURATION,
 	.interface_count = 1,
-	.length = sizeof (usb_standard_configuration_descriptor_t),
-	.max_power = 100,
+	.length = sizeof(usb_standard_configuration_descriptor_t),
+	.max_power = 0, /* root hubs don't need no power */
 	.str_configuration = 0,
 };
@@ -89,5 +90,5 @@
 	.interface_protocol = 0,
 	.interface_subclass = 0,
-	.length = sizeof (usb_standard_interface_descriptor_t),
+	.length = sizeof(usb_standard_interface_descriptor_t),
 	.str_interface = 0,
 };
@@ -100,345 +101,268 @@
 	.descriptor_type = USB_DESCTYPE_ENDPOINT,
 	.endpoint_address = 1 + (1 << 7),
-	.length = sizeof (usb_standard_endpoint_descriptor_t),
-	.max_packet_size = 8,
+	.length = sizeof(usb_standard_endpoint_descriptor_t),
+	.max_packet_size = 2,
 	.poll_interval = 255,
 };
 
-/**
- * bitmask of hub features that are valid to be cleared
- */
-static const uint32_t hub_clear_feature_valid_mask =
-    RHS_OCIC_FLAG |
-    RHS_CLEAR_PORT_POWER;
-
-/**
- * bitmask of hub features that are cleared by writing 1 (and not 0)
- */
-static const uint32_t hub_clear_feature_by_writing_one_mask =
-    RHS_CLEAR_PORT_POWER;
-
-/**
- * bitmask of hub features that are valid to be set
- */
-static const uint32_t hub_set_feature_valid_mask =
-    RHS_LPSC_FLAG |
-    RHS_OCIC_FLAG;
-
-/**
- * bitmask of hub features that are set by writing 1 and cleared by writing 0
- */
-static const uint32_t hub_set_feature_direct_mask =
-    RHS_SET_PORT_POWER;
-
-/**
- * bitmask of port features that are valid to be set
- */
-static const uint32_t port_set_feature_valid_mask =
-    RHPS_SET_PORT_ENABLE |
-    RHPS_SET_PORT_SUSPEND |
-    RHPS_SET_PORT_RESET |
-    RHPS_SET_PORT_POWER;
-
-/**
- * bitmask of port features that can be cleared
- */
-static const uint32_t port_clear_feature_valid_mask =
-    RHPS_CCS_FLAG |
-    RHPS_SET_PORT_SUSPEND |
-    RHPS_POCI_FLAG |
-    RHPS_SET_PORT_POWER |
-    RHPS_CSC_FLAG |
-    RHPS_PESC_FLAG |
-    RHPS_PSSC_FLAG |
-    RHPS_OCIC_FLAG |
-    RHPS_PRSC_FLAG;
-
-//note that USB_HUB_FEATURE_PORT_POWER bit is translated into
-//USB_HUB_FEATURE_PORT_LOW_SPEED for port set feature request
-
-/**
- * bitmask with port status changes
- */
-static const uint32_t port_status_change_mask = RHPS_CHANGE_WC_MASK;
-
-static int create_serialized_hub_descriptor(rh_t *instance);
-
-static int rh_init_descriptors(rh_t *instance);
-
-static int process_get_port_status_request(rh_t *instance, uint16_t port,
-    usb_transfer_batch_t * request);
-
-static int process_get_hub_status_request(rh_t *instance,
-    usb_transfer_batch_t * request);
-
-static int process_get_status_request(rh_t *instance,
-    usb_transfer_batch_t * request);
-
-static void create_interrupt_mask_in_instance(rh_t *instance);
-
-static int process_get_descriptor_request(rh_t *instance,
-    usb_transfer_batch_t *request);
-
-static int process_get_configuration_request(rh_t *instance,
-    usb_transfer_batch_t *request);
-
-static int process_hub_feature_set_request(rh_t *instance, uint16_t feature);
-
-static int process_hub_feature_clear_request(rh_t *instance,
-    uint16_t feature);
-
-static int process_port_feature_set_request(rh_t *instance,
-    uint16_t feature, uint16_t port);
-
-static int process_port_feature_clear_request(rh_t *instance,
-    uint16_t feature, uint16_t port);
-
-static int process_address_set_request(rh_t *instance,
-    uint16_t address);
-
-static int process_request_with_output(rh_t *instance,
-    usb_transfer_batch_t *request);
-
-static int process_request_with_input(rh_t *instance,
-    usb_transfer_batch_t *request);
-
-static int process_request_without_data(rh_t *instance,
-    usb_transfer_batch_t *request);
-
-static int process_ctrl_request(rh_t *instance, usb_transfer_batch_t *request);
-
-static int process_interrupt_mask_in_instance(rh_t *instance, usb_transfer_batch_t * request);
-
-static bool is_zeros(void * buffer, size_t size);
-
-/** Root hub initialization
- * @return Error code.
- */
-int rh_init(rh_t *instance, ohci_regs_t *regs) {
-	assert(instance);
+static void create_serialized_hub_descriptor(rh_t *instance);
+static void rh_init_descriptors(rh_t *instance);
+static uint16_t create_interrupt_mask(rh_t *instance);
+static int get_status(rh_t *instance, usb_transfer_batch_t *request);
+static int get_descriptor(rh_t *instance, usb_transfer_batch_t *request);
+static int set_feature(rh_t *instance, usb_transfer_batch_t *request);
+static int clear_feature(rh_t *instance, usb_transfer_batch_t *request);
+static int set_feature_port(rh_t *instance, uint16_t feature, uint16_t port);
+static int clear_feature_port(rh_t *instance, uint16_t feature, uint16_t port);
+static int control_request(rh_t *instance, usb_transfer_batch_t *request);
+static inline void interrupt_request(
+    usb_transfer_batch_t *request, uint16_t mask, size_t size)
+{
+	assert(request);
+
+	memcpy(request->data_buffer, &mask, size);
+	request->transfered_size = size;
+	usb_transfer_batch_finish_error(request, EOK);
+}
+
+#define TRANSFER_OK(bytes) \
+do { \
+	request->transfered_size = bytes; \
+	return EOK; \
+} while (0)
+
+/** Root Hub driver structure initialization.
+ *
+ * Reads info registers and prepares descriptors. Sets power mode.
+ */
+void rh_init(rh_t *instance, ohci_regs_t *regs)
+{
+	assert(instance);
+	assert(regs);
+
 	instance->registers = regs;
 	instance->port_count =
 	    (instance->registers->rh_desc_a >> RHDA_NDS_SHIFT) & RHDA_NDS_MASK;
-	int opResult = rh_init_descriptors(instance);
-	if (opResult != EOK) {
-		return opResult;
-	}
-	// set port power mode to no-power-switching
+	if (instance->port_count > 15) {
+		usb_log_warning("OHCI specification does not allow more than 15"
+		    " ports. Max 15 ports will be used");
+		instance->port_count = 15;
+	}
+
+	/* Don't forget the hub status bit and round up */
+	instance->interrupt_mask_size = 1 + (instance->port_count / 8);
+	instance->unfinished_interrupt_transfer = NULL;
+
+#ifdef OHCI_POWER_SWITCH_no
+	/* Set port power mode to no power-switching. (always on) */
 	instance->registers->rh_desc_a |= RHDA_NPS_FLAG;
-	instance->unfinished_interrupt_transfer = NULL;
-	instance->interrupt_mask_size = (instance->port_count + 8) / 8;
-	instance->interrupt_buffer = malloc(instance->interrupt_mask_size);
-	if (!instance->interrupt_buffer)
-		return ENOMEM;
+	/* Set to no over-current reporting */
+	instance->registers->rh_desc_a |= RHDA_NOCP_FLAG;
+#elif defined OHCI_POWER_SWITCH_ganged
+	/* Set port power mode to no ganged power-switching. */
+	instance->registers->rh_desc_a &= ~RHDA_NPS_FLAG;
+	instance->registers->rh_desc_a &= ~RHDA_PSM_FLAG;
+	instance->registers->rh_status = RHS_CLEAR_GLOBAL_POWER;
+	/* Set to global over-current */
+	instance->registers->rh_desc_a &= ~RHDA_NOCP_FLAG;
+	instance->registers->rh_desc_a &= ~RHDA_OCPM_FLAG;
+#else
+	/* Set port power mode to no per port power-switching. */
+	instance->registers->rh_desc_a &= ~RHDA_NPS_FLAG;
+	instance->registers->rh_desc_a |= RHDA_PSM_FLAG;
+
+	/* Control all ports by global switch and turn them off */
+	instance->registers->rh_desc_b &= (RHDB_PCC_MASK << RHDB_PCC_SHIFT);
+	instance->registers->rh_status = RHS_CLEAR_GLOBAL_POWER;
+	/* Return control to per port state */
+	instance->registers->rh_desc_b |=
+		((1 << (instance->port_count + 1)) - 1) << RHDB_PCC_SHIFT;
+	/* Set per port over-current */
+	instance->registers->rh_desc_a &= ~RHDA_NOCP_FLAG;
+	instance->registers->rh_desc_a |= RHDA_OCPM_FLAG;
+#endif
+
+	rh_init_descriptors(instance);
 
 	usb_log_info("Root hub (%zu ports) initialized.\n",
 	    instance->port_count);
-
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * process root hub request
- *
- * @param instance root hub instance
- * @param request structure containing both request and response information
- * @return error code
- */
-int rh_request(rh_t *instance, usb_transfer_batch_t *request) {
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * Process root hub request.
+ *
+ * @param instance Root hub instance
+ * @param request Structure containing both request and response information
+ * @return Error code
+ */
+void rh_request(rh_t *instance, usb_transfer_batch_t *request)
+{
 	assert(instance);
 	assert(request);
-	int opResult;
-	if (request->ep->transfer_type == USB_TRANSFER_CONTROL) {
+
+	switch (request->ep->transfer_type)
+	{
+	case USB_TRANSFER_CONTROL:
 		usb_log_debug("Root hub got CONTROL packet\n");
-		opResult = process_ctrl_request(instance, request);
-		usb_transfer_batch_finish_error(request, opResult);
-	} else if (request->ep->transfer_type == USB_TRANSFER_INTERRUPT) {
+		const int ret = control_request(instance, request);
+		usb_transfer_batch_finish_error(request, ret);
+		break;
+	case USB_TRANSFER_INTERRUPT:
 		usb_log_debug("Root hub got INTERRUPT packet\n");
-		create_interrupt_mask_in_instance(instance);
-		if (is_zeros(instance->interrupt_buffer,
-		    instance->interrupt_mask_size)) {
+		const uint16_t mask = create_interrupt_mask(instance);
+		if (mask == 0) {
 			usb_log_debug("No changes..\n");
+			assert(instance->unfinished_interrupt_transfer == NULL);
 			instance->unfinished_interrupt_transfer = request;
-			//will be finished later
-		} else {
-			usb_log_debug("Processing changes..\n");
-			process_interrupt_mask_in_instance(instance, request);
+			break;
 		}
-		opResult = EOK;
-	} else {
-
-		opResult = EINVAL;
-		usb_transfer_batch_finish_error(request, opResult);
-	}
-	return EOK;
-}
-
-/*----------------------------------------------------------------------------*/
-
-/**
- * process interrupt on a hub
+		usb_log_debug("Processing changes...\n");
+		interrupt_request(request, mask, instance->interrupt_mask_size);
+		break;
+
+	default:
+		usb_log_error("Root hub got unsupported request.\n");
+		usb_transfer_batch_finish_error(request, EINVAL);
+	}
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * Process interrupt on a hub device.
  *
  * If there is no pending interrupt transfer, nothing happens.
  * @param instance
  */
-void rh_interrupt(rh_t *instance) {
-	if (!instance->unfinished_interrupt_transfer) {
+void rh_interrupt(rh_t *instance)
+{
+	assert(instance);
+
+	if (!instance->unfinished_interrupt_transfer)
 		return;
-	}
+
 	usb_log_debug("Finalizing interrupt transfer\n");
-	create_interrupt_mask_in_instance(instance);
-	process_interrupt_mask_in_instance(instance,
-	    instance->unfinished_interrupt_transfer);
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * Create hub descriptor used in hub-driver <-> hub communication
- *
- * This means creating byt array from data in root hub registers. For more
- * info see usb hub specification.
- *
+	const uint16_t mask = create_interrupt_mask(instance);
+	interrupt_request(instance->unfinished_interrupt_transfer,
+	    mask, instance->interrupt_mask_size);
+
+	instance->unfinished_interrupt_transfer = NULL;
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * Create hub descriptor.
+ *
+ * For descriptor format see USB hub specification (chapter 11.15.2.1, pg. 263)
+ *
+ * @param instance Root hub instance
+ * @return Error code
+ */
+void create_serialized_hub_descriptor(rh_t *instance)
+{
+	assert(instance);
+
+	/* 7 bytes + 2 port bit fields (port count + global bit) */
+	const size_t size = 7 + (instance->interrupt_mask_size * 2);
+	assert(size <= HUB_DESCRIPTOR_MAX_SIZE);
+	instance->hub_descriptor_size = size;
+
+	const uint32_t hub_desc = instance->registers->rh_desc_a;
+	const uint32_t port_desc = instance->registers->rh_desc_b;
+
+	/* bDescLength */
+	instance->descriptors.hub[0] = size;
+	/* bDescriptorType */
+	instance->descriptors.hub[1] = USB_DESCTYPE_HUB;
+	/* bNmbrPorts */
+	instance->descriptors.hub[2] = instance->port_count;
+	/* wHubCharacteristics */
+	instance->descriptors.hub[3] = 0 |
+	    /* The lowest 2 bits indicate power switching mode */
+	    (((hub_desc & RHDA_PSM_FLAG)  ? 1 : 0) << 0) |
+	    (((hub_desc & RHDA_NPS_FLAG)  ? 1 : 0) << 1) |
+	    /* Bit 3 indicates device type (compound device) */
+	    (((hub_desc & RHDA_DT_FLAG)   ? 1 : 0) << 2) |
+	    /* Bits 4,5 indicate over-current protection mode */
+	    (((hub_desc & RHDA_OCPM_FLAG) ? 1 : 0) << 3) |
+	    (((hub_desc & RHDA_NOCP_FLAG) ? 1 : 0) << 4);
+
+	/* Reserved */
+	instance->descriptors.hub[4] = 0;
+	/* bPwrOn2PwrGood */
+	instance->descriptors.hub[5] =
+	    (hub_desc >> RHDA_POTPGT_SHIFT) & RHDA_POTPGT_MASK;
+	/* bHubContrCurrent, root hubs don't need no power. */
+	instance->descriptors.hub[6] = 0;
+
+	/* Device Removable and some legacy 1.0 stuff*/
+	instance->descriptors.hub[7] =
+	    (port_desc >> RHDB_DR_SHIFT) & RHDB_DR_MASK & 0xff;
+	instance->descriptors.hub[8] = 0xff;
+	if (instance->interrupt_mask_size == 2) {
+		instance->descriptors.hub[8] =
+		    (port_desc >> RHDB_DR_SHIFT) & RHDB_DR_MASK >> 8;
+		instance->descriptors.hub[9]  = 0xff;
+		instance->descriptors.hub[10] = 0xff;
+	}
+}
+/*----------------------------------------------------------------------------*/
+/** Initialize hub descriptors.
+ *
+ * A full configuration descriptor is assembled. The configuration and endpoint
+ * descriptors have local modifications.
+ * @param instance Root hub instance
+ * @return Error code
+ */
+void rh_init_descriptors(rh_t *instance)
+{
+	assert(instance);
+
+	instance->descriptors.configuration = ohci_rh_conf_descriptor;
+	instance->descriptors.interface = ohci_rh_iface_descriptor;
+	instance->descriptors.endpoint = ohci_rh_ep_descriptor;
+	create_serialized_hub_descriptor(instance);
+
+	instance->descriptors.endpoint.max_packet_size =
+	    instance->interrupt_mask_size;
+
+	instance->descriptors.configuration.total_length =
+	    sizeof(usb_standard_configuration_descriptor_t) +
+	    sizeof(usb_standard_endpoint_descriptor_t) +
+	    sizeof(usb_standard_interface_descriptor_t) +
+	    instance->hub_descriptor_size;
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * Create bitmap of changes to answer status interrupt.
+ *
+ * Result contains bitmap where bit 0 indicates change on hub and
+ * bit i indicates change on i`th port (i>0). For more info see
+ * Hub and Port status bitmap specification in USB specification
+ * (chapter 11.13.4).
  * @param instance root hub instance
- * @return error code
- */
-static int create_serialized_hub_descriptor(rh_t *instance) {
-	size_t size = 7 +
-	    ((instance->port_count + 7) / 8) * 2;
-	size_t var_size = (instance->port_count + 7) / 8;
-	uint8_t * result = (uint8_t*) malloc(size);
-	if (!result) return ENOMEM;
-
-	bzero(result, size);
-	//size
-	result[0] = size;
-	//descriptor type
-	result[1] = USB_DESCTYPE_HUB;
-	result[2] = instance->port_count;
-	uint32_t hub_desc_reg = instance->registers->rh_desc_a;
-	result[3] =
-	    ((hub_desc_reg >> 8) % 2) +
-	    (((hub_desc_reg >> 9) % 2) << 1) +
-	    (((hub_desc_reg >> 10) % 2) << 2) +
-	    (((hub_desc_reg >> 11) % 2) << 3) +
-	    (((hub_desc_reg >> 12) % 2) << 4);
-	result[4] = 0;
-	result[5] = /*descriptor->pwr_on_2_good_time*/ 50;
-	result[6] = 50;
-
-	size_t port;
-	for (port = 1; port <= instance->port_count; ++port) {
-		uint8_t is_non_removable =
-		    instance->registers->rh_desc_b >> port % 2;
-		result[7 + port / 8] +=
-		    is_non_removable << (port % 8);
-	}
-	size_t i;
-	for (i = 0; i < var_size; ++i) {
-		result[7 + var_size + i] = 255;
-	}
-	instance->hub_descriptor = result;
-	instance->descriptor_size = size;
-
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/** initialize hub descriptors
- *
- * Initialized are device and full configuration descriptor. These need to
- * be initialized only once per hub.
- * @instance root hub instance
- * @return error code
- */
-static int rh_init_descriptors(rh_t *instance) {
-	memcpy(&instance->descriptors.device, &ohci_rh_device_descriptor,
-	    sizeof (ohci_rh_device_descriptor)
-	    );
-	usb_standard_configuration_descriptor_t descriptor;
-	memcpy(&descriptor, &ohci_rh_conf_descriptor,
-	    sizeof (ohci_rh_conf_descriptor));
-
-	int opResult = create_serialized_hub_descriptor(instance);
-	if (opResult != EOK) {
-		return opResult;
-	}
-	descriptor.total_length =
-	    sizeof (usb_standard_configuration_descriptor_t) +
-	    sizeof (usb_standard_endpoint_descriptor_t) +
-	    sizeof (usb_standard_interface_descriptor_t) +
-	    instance->descriptor_size;
-
-	uint8_t * full_config_descriptor =
-	    (uint8_t*) malloc(descriptor.total_length);
-	if (!full_config_descriptor) {
-		return ENOMEM;
-	}
-	memcpy(full_config_descriptor, &descriptor, sizeof (descriptor));
-	memcpy(full_config_descriptor + sizeof (descriptor),
-	    &ohci_rh_iface_descriptor, sizeof (ohci_rh_iface_descriptor));
-	memcpy(full_config_descriptor + sizeof (descriptor) +
-	    sizeof (ohci_rh_iface_descriptor),
-	    &ohci_rh_ep_descriptor, sizeof (ohci_rh_ep_descriptor));
-	memcpy(full_config_descriptor + sizeof (descriptor) +
-	    sizeof (ohci_rh_iface_descriptor) +
-	    sizeof (ohci_rh_ep_descriptor),
-	    instance->hub_descriptor, instance->descriptor_size);
-
-	instance->descriptors.configuration = full_config_descriptor;
-	instance->descriptors.configuration_size = descriptor.total_length;
-
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * create answer to port status_request
- *
- * Copy content of corresponding port status register to answer buffer. The
- * format of the port status register and port status data is the same (
- * see OHCI root hub and USB hub documentation).
- *
- * @param instance root hub instance
- * @param port port number, counted from 1
- * @param request structure containing both request and response information
- * @return error code
- */
-static int process_get_port_status_request(rh_t *instance, uint16_t port,
-    usb_transfer_batch_t * request) {
-	if (port < 1 || port > instance->port_count)
-		return EINVAL;
-	request->transfered_size = 4;
-	uint32_t data = instance->registers->rh_port_status[port - 1];
-	memcpy(request->data_buffer, &data, 4);
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * create answer to port status_request
- *
- * This copies flags in hub status register into the buffer. The format of the
- * status register and status message is the same, according to USB hub
- * specification and OHCI root hub specification.
- *
- * @param instance root hub instance
- * @param request structure containing both request and response information
- * @return error code
- */
-static int process_get_hub_status_request(rh_t *instance,
-    usb_transfer_batch_t * request) {
-	request->transfered_size = 4;
-	//bits, 0,1,16,17
-	uint32_t mask = 1 | (1 << 1) | (1 << 16) | (1 << 17);
-	uint32_t data = mask & instance->registers->rh_status;
-	memcpy(request->data_buffer, &data, 4);
-
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * create answer to status request
+ * @return Mask of changes.
+ */
+uint16_t create_interrupt_mask(rh_t *instance)
+{
+	assert(instance);
+	uint16_t mask = 0;
+
+	/* Only local power source change and over-current change can happen */
+	if (instance->registers->rh_status & (RHS_LPSC_FLAG | RHS_OCIC_FLAG)) {
+		mask |= 1;
+	}
+	size_t port = 1;
+	for (; port <= instance->port_count; ++port) {
+		/* Write-clean bits are those that indicate change */
+		if (RHPS_CHANGE_WC_MASK
+		    & instance->registers->rh_port_status[port - 1]) {
+
+			mask |= (1 << port);
+		}
+	}
+	/* USB is little endian */
+	return host2uint32_t_le(mask);
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * Create answer to status request.
  *
  * This might be either hub status or port status request. If neither,
@@ -448,204 +372,119 @@
  * @return error code
  */
-static int process_get_status_request(rh_t *instance,
-    usb_transfer_batch_t * request) {
-	size_t buffer_size = request->buffer_size;
-	usb_device_request_setup_packet_t * request_packet =
-	    (usb_device_request_setup_packet_t*)
-	    request->setup_buffer;
-
-	usb_hub_bm_request_type_t request_type = request_packet->request_type;
-	if (buffer_size < 4) {
-		usb_log_warning("Requested more data than buffer size\n");
-		return EINVAL;
-	}
-
-	if (request_type == USB_HUB_REQ_TYPE_GET_HUB_STATUS)
-		return process_get_hub_status_request(instance, request);
-	if (request_type == USB_HUB_REQ_TYPE_GET_PORT_STATUS)
-		return process_get_port_status_request(instance,
-	    request_packet->index,
-	    request);
+int get_status(rh_t *instance, usb_transfer_batch_t *request)
+{
+	assert(instance);
+	assert(request);
+
+	const usb_device_request_setup_packet_t *request_packet =
+	    (usb_device_request_setup_packet_t*)request->setup_buffer;
+
+	if (request->buffer_size < 4) {
+		usb_log_error("Buffer too small for get status request.\n");
+		return EOVERFLOW;
+	}
+
+	/* Hub status: just filter relevant info from rh_status reg */
+	if (request_packet->request_type == USB_HUB_REQ_TYPE_GET_HUB_STATUS) {
+		const uint32_t data = instance->registers->rh_status &
+		    (RHS_LPS_FLAG | RHS_LPSC_FLAG | RHS_OCI_FLAG | RHS_OCIC_FLAG);
+		memcpy(request->data_buffer, &data, 4);
+		TRANSFER_OK(4);
+	}
+
+	/* Copy appropriate rh_port_status register, OHCI designers were
+	 * kind enough to make those bit values match USB specification */
+	if (request_packet->request_type == USB_HUB_REQ_TYPE_GET_PORT_STATUS) {
+		const unsigned port = request_packet->index;
+		if (port < 1 || port > instance->port_count)
+			return EINVAL;
+
+		const uint32_t data =
+		    instance->registers->rh_port_status[port - 1];
+		memcpy(request->data_buffer, &data, 4);
+		TRANSFER_OK(4);
+	}
 
 	return ENOTSUP;
 }
 /*----------------------------------------------------------------------------*/
-
-/**
- * create answer to status interrupt consisting of change bitmap
- *
- * Result contains bitmap where bit 0 indicates change on hub and
- * bit i indicates change on i`th port (i>0). For more info see
- * Hub and Port status bitmap specification in USB specification
- * (chapter 11.13.4).
- * Uses instance`s interrupt buffer to store the interrupt information.
- * @param instance root hub instance
- */
-static void create_interrupt_mask_in_instance(rh_t * instance) {
-	uint8_t * bitmap = (uint8_t*) (instance->interrupt_buffer);
-	uint32_t mask = (1 << (USB_HUB_FEATURE_C_HUB_LOCAL_POWER + 16))
-	    | (1 << (USB_HUB_FEATURE_C_HUB_OVER_CURRENT + 16));
-	bzero(bitmap, instance->interrupt_mask_size);
-	if ((instance->registers->rh_status & mask) != 0) {
-		bitmap[0] = 1;
-	}
-	size_t port;
-	mask = port_status_change_mask;
-	for (port = 1; port <= instance->port_count; ++port) {
-		if ((mask & instance->registers->rh_port_status[port - 1]) != 0) {
-
-			bitmap[(port) / 8] += 1 << (port % 8);
-		}
-	}
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * create answer to a descriptor request
+/**
+ * Create answer to a descriptor request.
  *
  * This might be a request for standard (configuration, device, endpoint or
  * interface) or device specific (hub) descriptor.
- * @param instance root hub instance
- * @param request structure containing both request and response information
- * @return error code
- */
-static int process_get_descriptor_request(rh_t *instance,
-    usb_transfer_batch_t *request) {
-	usb_device_request_setup_packet_t * setup_request =
-	    (usb_device_request_setup_packet_t*) request->setup_buffer;
+ * @param instance Root hub instance
+ * @param request Structure containing both request and response information
+ * @return Error code
+ */
+int get_descriptor(rh_t *instance, usb_transfer_batch_t *request)
+{
+	assert(instance);
+	assert(request);
+
+	const usb_device_request_setup_packet_t *setup_request =
+	    (usb_device_request_setup_packet_t *) request->setup_buffer;
 	size_t size;
-	const void * result_descriptor = NULL;
+	const void *descriptor = NULL;
 	const uint16_t setup_request_value = setup_request->value_high;
 	//(setup_request->value_low << 8);
-	switch (setup_request_value) {
-		case USB_DESCTYPE_HUB:
-		{
-			usb_log_debug2("USB_DESCTYPE_HUB\n");
-			result_descriptor = instance->hub_descriptor;
-			size = instance->descriptor_size;
-			break;
-		}
-		case USB_DESCTYPE_DEVICE:
-		{
-			usb_log_debug2("USB_DESCTYPE_DEVICE\n");
-			result_descriptor = &ohci_rh_device_descriptor;
-			size = sizeof (ohci_rh_device_descriptor);
-			break;
-		}
-		case USB_DESCTYPE_CONFIGURATION:
-		{
-			usb_log_debug2("USB_DESCTYPE_CONFIGURATION\n");
-			result_descriptor = instance->descriptors.configuration;
-			size = instance->descriptors.configuration_size;
-			break;
-		}
-		case USB_DESCTYPE_INTERFACE:
-		{
-			usb_log_debug2("USB_DESCTYPE_INTERFACE\n");
-			result_descriptor = &ohci_rh_iface_descriptor;
-			size = sizeof (ohci_rh_iface_descriptor);
-			break;
-		}
-		case USB_DESCTYPE_ENDPOINT:
-		{
-			usb_log_debug2("USB_DESCTYPE_ENDPOINT\n");
-			result_descriptor = &ohci_rh_ep_descriptor;
-			size = sizeof (ohci_rh_ep_descriptor);
-			break;
-		}
-		default:
-		{
-			usb_log_debug2("USB_DESCTYPE_EINVAL %d \n",
-			    setup_request->value);
-			usb_log_debug2("\ttype %d\n\trequest %d\n\tvalue "
-			    "%d\n\tindex %d\n\tlen %d\n ",
-			    setup_request->request_type,
-			    setup_request->request,
-			    setup_request_value,
-			    setup_request->index,
-			    setup_request->length
-			    );
-			return EINVAL;
-		}
+	switch (setup_request_value)
+	{
+	case USB_DESCTYPE_HUB:
+		usb_log_debug2("USB_DESCTYPE_HUB\n");
+		/* Hub descriptor was generated locally */
+		descriptor = instance->descriptors.hub;
+		size = instance->hub_descriptor_size;
+		break;
+
+	case USB_DESCTYPE_DEVICE:
+		usb_log_debug2("USB_DESCTYPE_DEVICE\n");
+		/* Device descriptor is shared (No one should ask for it)*/
+		descriptor = &ohci_rh_device_descriptor;
+		size = sizeof(ohci_rh_device_descriptor);
+		break;
+
+	case USB_DESCTYPE_CONFIGURATION:
+		usb_log_debug2("USB_DESCTYPE_CONFIGURATION\n");
+		/* Start with configuration and add others depending on
+		 * request size */
+		descriptor = &instance->descriptors;
+		size = instance->descriptors.configuration.total_length;
+		break;
+
+	case USB_DESCTYPE_INTERFACE:
+		usb_log_debug2("USB_DESCTYPE_INTERFACE\n");
+		/* Use local interface descriptor. There is one and it
+		 * might be modified */
+		descriptor = &instance->descriptors.interface;
+		size = sizeof(instance->descriptors.interface);
+		break;
+
+	case USB_DESCTYPE_ENDPOINT:
+		/* Use local endpoint descriptor. There is one
+		 * it might have max_packet_size field modified*/
+		usb_log_debug2("USB_DESCTYPE_ENDPOINT\n");
+		descriptor = &instance->descriptors.endpoint;
+		size = sizeof(instance->descriptors.endpoint);
+		break;
+
+	default:
+		usb_log_debug2("USB_DESCTYPE_EINVAL %d \n"
+		    "\ttype %d\n\trequest %d\n\tvalue "
+		    "%d\n\tindex %d\n\tlen %d\n ",
+		    setup_request->value,
+		    setup_request->request_type, setup_request->request,
+		    setup_request_value, setup_request->index,
+		    setup_request->length);
+		return EINVAL;
 	}
 	if (request->buffer_size < size) {
 		size = request->buffer_size;
 	}
-	request->transfered_size = size;
-	memcpy(request->data_buffer, result_descriptor, size);
-
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * answer to get configuration request
- *
- * Root hub works independently on the configuration.
- * @param instance root hub instance
- * @param request structure containing both request and response information
- * @return error code
- */
-static int process_get_configuration_request(rh_t *instance,
-    usb_transfer_batch_t *request) {
-	//set and get configuration requests do not have any meaning, only dummy
-	//values are returned
-	if (request->buffer_size != 1)
-		return EINVAL;
-	request->data_buffer[0] = 1;
-	request->transfered_size = 1;
-
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * process feature-enabling request on hub
- *
- * @param instance root hub instance
- * @param feature feature selector
- * @return error code
- */
-static int process_hub_feature_set_request(rh_t *instance,
-    uint16_t feature) {
-	if (!((1 << feature) & hub_set_feature_valid_mask))
-		return EINVAL;
-	if (feature == USB_HUB_FEATURE_C_HUB_LOCAL_POWER)
-		feature = USB_HUB_FEATURE_C_HUB_LOCAL_POWER << 16;
-	instance->registers->rh_status =
-	    (instance->registers->rh_status | (1 << feature))
-	    & (~hub_clear_feature_by_writing_one_mask);
-
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * process feature-disabling request on hub
- *
- * @param instance root hub instance
- * @param feature feature selector
- * @return error code
- */
-static int process_hub_feature_clear_request(rh_t *instance,
-    uint16_t feature) {
-	if (!((1 << feature) & hub_clear_feature_valid_mask))
-		return EINVAL;
-	//is the feature cleared directly?
-	if ((1 << feature) & hub_set_feature_direct_mask) {
-		instance->registers->rh_status =
-		    (instance->registers->rh_status & (~(1 << feature)))
-		    & (~hub_clear_feature_by_writing_one_mask);
-	} else {//the feature is cleared by writing '1'
-
-		instance->registers->rh_status =
-		    (instance->registers->rh_status
-		    & (~hub_clear_feature_by_writing_one_mask))
-		    | (1 << feature);
-	}
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
+
+	memcpy(request->data_buffer, descriptor, size);
+	TRANSFER_OK(size);
+}
+/*----------------------------------------------------------------------------*/
 /**
  * process feature-enabling request on hub
@@ -657,19 +496,36 @@
  * @return error code
  */
-static int process_port_feature_set_request(rh_t *instance,
-    uint16_t feature, uint16_t port) {
-	if (!((1 << feature) & port_set_feature_valid_mask))
-		return EINVAL;
+int set_feature_port(rh_t *instance, uint16_t feature, uint16_t port)
+{
+	assert(instance);
+
 	if (port < 1 || port > instance->port_count)
 		return EINVAL;
-	instance->registers->rh_port_status[port - 1] =
-	    (instance->registers->rh_port_status[port - 1] | (1 << feature))
-	    & (~port_clear_feature_valid_mask);
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * process feature-disabling request on hub
+
+	switch (feature)
+	{
+	case USB_HUB_FEATURE_PORT_POWER:   //8
+		/* No power switching */
+		if (instance->registers->rh_desc_a & RHDA_NPS_FLAG)
+			return EOK;
+		/* Ganged power switching */
+		if (!(instance->registers->rh_desc_a & RHDA_PSM_FLAG)) {
+			instance->registers->rh_status = RHS_SET_GLOBAL_POWER;
+			return EOK;
+		}
+	case USB_HUB_FEATURE_PORT_ENABLE:  //1
+	case USB_HUB_FEATURE_PORT_SUSPEND: //2
+	case USB_HUB_FEATURE_PORT_RESET:   //4
+		/* Nice thing is that these shifts correspond to the position
+		 * of control bits in register */
+		instance->registers->rh_port_status[port - 1] = (1 << feature);
+		return EOK;
+	default:
+		return ENOTSUP;
+	}
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * Process feature clear request.
  *
  * @param instance root hub instance
@@ -679,93 +535,52 @@
  * @return error code
  */
-static int process_port_feature_clear_request(rh_t *instance,
-    uint16_t feature, uint16_t port) {
-	if (!((1 << feature) & port_clear_feature_valid_mask))
-		return EINVAL;
+int clear_feature_port(rh_t *instance, uint16_t feature, uint16_t port)
+{
+	assert(instance);
+
 	if (port < 1 || port > instance->port_count)
 		return EINVAL;
-	if (feature == USB_HUB_FEATURE_PORT_POWER)
-		feature = USB_HUB_FEATURE_PORT_LOW_SPEED;
-	if (feature == USB_HUB_FEATURE_PORT_SUSPEND)
-		feature = USB_HUB_FEATURE_PORT_OVER_CURRENT;
-	instance->registers->rh_port_status[port - 1] =
-	    (instance->registers->rh_port_status[port - 1]
-	    & (~port_clear_feature_valid_mask))
-	    | (1 << feature);
-
-	return EOK;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * register address to this device
- *
- * @param instance root hub instance
- * @param address new address
- * @return error code
- */
-static int process_address_set_request(rh_t *instance,
-    uint16_t address) {
-	return ENOTSUP;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * process one of requests that requere output data
- *
- * Request can be one of USB_DEVREQ_GET_STATUS, USB_DEVREQ_GET_DESCRIPTOR or
- * USB_DEVREQ_GET_CONFIGURATION.
- * @param instance root hub instance
- * @param request structure containing both request and response information
- * @return error code
- */
-static int process_request_with_output(rh_t *instance,
-    usb_transfer_batch_t *request) {
-	usb_device_request_setup_packet_t * setup_request =
-	    (usb_device_request_setup_packet_t*) request->setup_buffer;
-	if (setup_request->request == USB_DEVREQ_GET_STATUS) {
-		usb_log_debug("USB_DEVREQ_GET_STATUS\n");
-		return process_get_status_request(instance, request);
-	}
-	if (setup_request->request == USB_DEVREQ_GET_DESCRIPTOR) {
-		usb_log_debug("USB_DEVREQ_GET_DESCRIPTOR\n");
-		return process_get_descriptor_request(instance, request);
-	}
-	if (setup_request->request == USB_DEVREQ_GET_CONFIGURATION) {
-		usb_log_debug("USB_DEVREQ_GET_CONFIGURATION\n");
-
-		return process_get_configuration_request(instance, request);
-	}
-	return ENOTSUP;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * process one of requests that carry input data
- *
- * Request can be one of USB_DEVREQ_SET_DESCRIPTOR or
- * USB_DEVREQ_SET_CONFIGURATION.
- * @param instance root hub instance
- * @param request structure containing both request and response information
- * @return error code
- */
-static int process_request_with_input(rh_t *instance,
-    usb_transfer_batch_t *request) {
-	usb_device_request_setup_packet_t * setup_request =
-	    (usb_device_request_setup_packet_t*) request->setup_buffer;
-	request->transfered_size = 0;
-	if (setup_request->request == USB_DEVREQ_SET_DESCRIPTOR) {
+
+	/* Enabled features to clear: see page 269 of USB specs */
+	switch (feature)
+	{
+	case USB_HUB_FEATURE_PORT_POWER:          //8
+		/* No power switching */
+		if (instance->registers->rh_desc_a & RHDA_NPS_FLAG)
+			return ENOTSUP;
+		/* Ganged power switching */
+		if (!(instance->registers->rh_desc_a & RHDA_PSM_FLAG)) {
+			instance->registers->rh_status = RHS_CLEAR_GLOBAL_POWER;
+			return EOK;
+		}
+		instance->registers->rh_port_status[port - 1] =
+			RHPS_CLEAR_PORT_POWER;
+		return EOK;
+
+	case USB_HUB_FEATURE_PORT_ENABLE:         //1
+		instance->registers->rh_port_status[port - 1] =
+			RHPS_CLEAR_PORT_ENABLE;
+		return EOK;
+
+	case USB_HUB_FEATURE_PORT_SUSPEND:        //2
+		instance->registers->rh_port_status[port - 1] =
+			RHPS_CLEAR_PORT_SUSPEND;
+		return EOK;
+
+	case USB_HUB_FEATURE_C_PORT_CONNECTION:   //16
+	case USB_HUB_FEATURE_C_PORT_ENABLE:       //17
+	case USB_HUB_FEATURE_C_PORT_SUSPEND:      //18
+	case USB_HUB_FEATURE_C_PORT_OVER_CURRENT: //19
+	case USB_HUB_FEATURE_C_PORT_RESET:        //20
+		/* Nice thing is that these shifts correspond to the position
+		 * of control bits in register */
+		instance->registers->rh_port_status[port - 1] = (1 << feature);
+		return EOK;
+
+	default:
 		return ENOTSUP;
 	}
-	if (setup_request->request == USB_DEVREQ_SET_CONFIGURATION) {
-		//set and get configuration requests do not have any meaning,
-		//only dummy values are returned
-
-		return EOK;
-	}
-	return ENOTSUP;
-}
-/*----------------------------------------------------------------------------*/
-
+}
+/*----------------------------------------------------------------------------*/
 /**
  * process one of requests that do not request nor carry additional data
@@ -777,55 +592,76 @@
  * @return error code
  */
-static int process_request_without_data(rh_t *instance,
-    usb_transfer_batch_t *request) {
-	usb_device_request_setup_packet_t * setup_request =
-	    (usb_device_request_setup_packet_t*) request->setup_buffer;
-	request->transfered_size = 0;
-	if (setup_request->request == USB_DEVREQ_CLEAR_FEATURE) {
-		if (setup_request->request_type == USB_HUB_REQ_TYPE_SET_HUB_FEATURE) {
-			usb_log_debug("USB_HUB_REQ_TYPE_SET_HUB_FEATURE\n");
-			return process_hub_feature_clear_request(instance,
-			    setup_request->value);
-		}
-		if (setup_request->request_type == USB_HUB_REQ_TYPE_SET_PORT_FEATURE) {
-			usb_log_debug2("USB_HUB_REQ_TYPE_SET_PORT_FEATURE\n");
-			return process_port_feature_clear_request(instance,
-			    setup_request->value,
-			    setup_request->index);
-		}
-		usb_log_debug("USB_HUB_REQ_TYPE_INVALID %d\n",
+int set_feature(rh_t *instance, usb_transfer_batch_t *request)
+{
+	assert(instance);
+	assert(request);
+
+	const usb_device_request_setup_packet_t *setup_request =
+	    (usb_device_request_setup_packet_t *) request->setup_buffer;
+	switch (setup_request->request_type)
+	{
+	case USB_HUB_REQ_TYPE_SET_PORT_FEATURE:
+		usb_log_debug("USB_HUB_REQ_TYPE_SET_PORT_FEATURE\n");
+		return set_feature_port(instance,
+		    setup_request->value, setup_request->index);
+
+	case USB_HUB_REQ_TYPE_SET_HUB_FEATURE:
+		/* Chapter 11.16.2 specifies that hub can be recipient
+		 * only for C_HUB_LOCAL_POWER and C_HUB_OVER_CURRENT
+		 * features. It makes no sense to SET either. */
+		usb_log_error("Invalid HUB set feature request.\n");
+		return ENOTSUP;
+	default:
+		usb_log_error("Invalid set feature request type: %d\n",
 		    setup_request->request_type);
 		return EINVAL;
 	}
-	if (setup_request->request == USB_DEVREQ_SET_FEATURE) {
-		if (setup_request->request_type == USB_HUB_REQ_TYPE_SET_HUB_FEATURE) {
-			usb_log_debug("USB_HUB_REQ_TYPE_SET_HUB_FEATURE\n");
-			return process_hub_feature_set_request(instance,
-			    setup_request->value);
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * process one of requests that do not request nor carry additional data
+ *
+ * Request can be one of USB_DEVREQ_CLEAR_FEATURE, USB_DEVREQ_SET_FEATURE or
+ * USB_DEVREQ_SET_ADDRESS.
+ * @param instance root hub instance
+ * @param request structure containing both request and response information
+ * @return error code
+ */
+int clear_feature(rh_t *instance, usb_transfer_batch_t *request)
+{
+	assert(instance);
+	assert(request);
+
+	const usb_device_request_setup_packet_t *setup_request =
+	    (usb_device_request_setup_packet_t *) request->setup_buffer;
+	request->transfered_size = 0;
+	switch (setup_request->request_type)
+	{
+	case USB_HUB_REQ_TYPE_CLEAR_PORT_FEATURE:
+		usb_log_debug("USB_HUB_REQ_TYPE_CLEAR_PORT_FEATURE\n");
+		return clear_feature_port(instance,
+		    setup_request->value, setup_request->index);
+
+	case USB_HUB_REQ_TYPE_CLEAR_HUB_FEATURE:
+		usb_log_debug("USB_HUB_REQ_TYPE_CLEAR_HUB_FEATURE\n");
+		/*
+		 * Chapter 11.16.2 specifies that only C_HUB_LOCAL_POWER and
+		 * C_HUB_OVER_CURRENT are supported. C_HUB_OVER_CURRENT is represented
+		 * by OHCI RHS_OCIC_FLAG. C_HUB_LOCAL_POWER is not supported
+		 * as root hubs do not support local power status feature.
+		 * (OHCI pg. 127) */
+		if (setup_request->value == USB_HUB_FEATURE_C_HUB_OVER_CURRENT) {
+			instance->registers->rh_status = RHS_OCIC_FLAG;
+			TRANSFER_OK(0);
 		}
-		if (setup_request->request_type == USB_HUB_REQ_TYPE_SET_PORT_FEATURE) {
-			usb_log_debug("USB_HUB_REQ_TYPE_SET_PORT_FEATURE\n");
-			return process_port_feature_set_request(instance,
-			    setup_request->value,
-			    setup_request->index);
-		}
-		usb_log_debug("USB_HUB_REQ_TYPE_INVALID %d\n",
+	default:
+		usb_log_error("Invalid clear feature request type: %d\n",
 		    setup_request->request_type);
 		return EINVAL;
 	}
-	if (setup_request->request == USB_DEVREQ_SET_ADDRESS) {
-		usb_log_debug("USB_DEVREQ_SET_ADDRESS\n");
-		return process_address_set_request(instance,
-		    setup_request->value);
-	}
-	usb_log_debug("USB_DEVREQ_SET_ENOTSUP %d\n",
-	    setup_request->request_type);
-
-	return ENOTSUP;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * process hub control request
+}
+/*----------------------------------------------------------------------------*/
+/**
+ * Process hub control request.
  *
  * If needed, writes answer into the request structure.
@@ -844,95 +680,65 @@
  * @return error code
  */
-static int process_ctrl_request(rh_t *instance, usb_transfer_batch_t *request) {
+int control_request(rh_t *instance, usb_transfer_batch_t *request)
+{
+	assert(instance);
+	assert(request);
+
 	if (!request->setup_buffer) {
-		usb_log_error("root hub received empty transaction?");
+		usb_log_error("Root hub received empty transaction!");
 		return EINVAL;
 	}
-	int opResult;
-	if (sizeof (usb_device_request_setup_packet_t) > request->setup_size) {
+
+	if (sizeof(usb_device_request_setup_packet_t) > request->setup_size) {
 		usb_log_error("Setup packet too small\n");
-		return EINVAL;
-	}
-	usb_log_debug("CTRL packet: %s.\n",
-	    usb_debug_str_buffer(
-	    (const uint8_t *) request->setup_buffer, 8, 8));
-	usb_device_request_setup_packet_t * setup_request =
-	    (usb_device_request_setup_packet_t*)
-	    request->setup_buffer;
-	switch (setup_request->request) {
-		case USB_DEVREQ_GET_STATUS:
-		case USB_DEVREQ_GET_DESCRIPTOR:
-		case USB_DEVREQ_GET_CONFIGURATION:
-			usb_log_debug2("Processing request with output\n");
-			opResult = process_request_with_output(
-			    instance, request);
-			break;
-		case USB_DEVREQ_CLEAR_FEATURE:
-		case USB_DEVREQ_SET_FEATURE:
-		case USB_DEVREQ_SET_ADDRESS:
-			usb_log_debug2("Processing request without "
-			    "additional data\n");
-			opResult = process_request_without_data(
-			    instance, request);
-			break;
-		case USB_DEVREQ_SET_DESCRIPTOR:
-		case USB_DEVREQ_SET_CONFIGURATION:
-			usb_log_debug2("Processing request with input\n");
-			opResult = process_request_with_input(
-			    instance, request);
-
-			break;
-		default:
-			usb_log_warning("Received unsupported request: %d.\n",
-			    setup_request->request);
-			opResult = ENOTSUP;
-	}
-	return opResult;
-}
-/*----------------------------------------------------------------------------*/
-
-/**
- * process hanging interrupt request
- *
- * If an interrupt transfer has been received and there was no change,
- * the driver stores the transfer information and waits for change to occcur.
- * This routine is called when that happens and it finalizes the interrupt
- * transfer.
- *
- * @param instance hub instance
- * @param request batch request to be processed
- *
- * @return
- */
-static int process_interrupt_mask_in_instance(rh_t *instance,
-    usb_transfer_batch_t * request) {
-	memcpy(request->data_buffer, instance->interrupt_buffer,
-	    instance->interrupt_mask_size);
-	request->transfered_size = instance->interrupt_mask_size;
-	instance->unfinished_interrupt_transfer = NULL;
-	usb_transfer_batch_finish_error(request, EOK);
-
-	return EOK;
-}
-
-/*----------------------------------------------------------------------------*/
-
-/**
- * return whether the buffer is full of zeros
- *
- * Convenience function.
- * @param buffer
- * @param size
- * @return
- */
-static bool is_zeros(void *buffer, size_t size) {
-	if (!buffer) return true;
-	if (!size) return true;
-	size_t i;
-	for (i = 0; i < size; ++i) {
-		if (((char*) buffer)[i])
-			return false;
-	}
-	return true;
+		return EOVERFLOW;
+	}
+
+	usb_log_debug2("CTRL packet: %s.\n",
+	    usb_debug_str_buffer((uint8_t *) request->setup_buffer, 8, 8));
+	const usb_device_request_setup_packet_t *setup_request =
+	    (usb_device_request_setup_packet_t *) request->setup_buffer;
+	switch (setup_request->request)
+	{
+	case USB_DEVREQ_GET_STATUS:
+		usb_log_debug("USB_DEVREQ_GET_STATUS\n");
+		return get_status(instance, request);
+
+	case USB_DEVREQ_GET_DESCRIPTOR:
+		usb_log_debug("USB_DEVREQ_GET_DESCRIPTOR\n");
+		return get_descriptor(instance, request);
+
+	case USB_DEVREQ_GET_CONFIGURATION:
+		usb_log_debug("USB_DEVREQ_GET_CONFIGURATION\n");
+		if (request->buffer_size != 1)
+			return EINVAL;
+		request->data_buffer[0] = 1;
+		TRANSFER_OK(1);
+
+	case USB_DEVREQ_CLEAR_FEATURE:
+		usb_log_debug2("Processing request without "
+		    "additional data\n");
+		return clear_feature(instance, request);
+	case USB_DEVREQ_SET_FEATURE:
+		usb_log_debug2("Processing request without "
+		    "additional data\n");
+		return set_feature(instance, request);
+
+	case USB_DEVREQ_SET_ADDRESS:
+		usb_log_debug("USB_DEVREQ_SET_ADDRESS\n");
+		instance->address = setup_request->value;
+		TRANSFER_OK(0);
+
+	case USB_DEVREQ_SET_CONFIGURATION:
+		usb_log_debug("USB_DEVREQ_SET_CONFIGURATION\n");
+		/* We don't need to do anything */
+		TRANSFER_OK(0);
+
+	case USB_DEVREQ_SET_DESCRIPTOR: /* Not supported by OHCI RH */
+	default:
+		usb_log_error("Received unsupported request: %d.\n",
+		    setup_request->request);
+		return ENOTSUP;
+	}
 }
 
Index: uspace/drv/bus/usb/ohci/root_hub.h
===================================================================
--- uspace/drv/bus/usb/ohci/root_hub.h	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/ohci/root_hub.h	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -41,4 +41,6 @@
 #include "batch.h"
 
+#define HUB_DESCRIPTOR_MAX_SIZE (7 + 2 + 2)
+
 /**
  * ohci root hub representation
@@ -51,28 +53,23 @@
 	/** hub port count */
 	size_t port_count;
-	/** hubs descriptors */
-	usb_device_descriptors_t descriptors;
 	/** interrupt transfer waiting for an actual interrupt to occur */
-	usb_transfer_batch_t * unfinished_interrupt_transfer;
-	/** pre-allocated interrupt mask
-	 *
-	 * This is allocated when initializing instance, so that memory
-	 * allocation is not needed when processing request. Buffer is used for
-	 * interrupt bitmask.
-	 */
-	uint8_t * interrupt_buffer;
+	usb_transfer_batch_t *unfinished_interrupt_transfer;
 	/** size of interrupt buffer */
 	size_t interrupt_mask_size;
-	/** instance`s descriptor*/
-	uint8_t * hub_descriptor;
+	/** Descriptors */
+	struct {
+		usb_standard_configuration_descriptor_t configuration;
+		usb_standard_interface_descriptor_t interface;
+		usb_standard_endpoint_descriptor_t endpoint;
+		uint8_t hub[HUB_DESCRIPTOR_MAX_SIZE];
+	} __attribute__ ((packed)) descriptors;
 	/** size of hub descriptor */
-	size_t descriptor_size;
-
+	size_t hub_descriptor_size;
 
 } rh_t;
 
-int rh_init(rh_t *instance, ohci_regs_t *regs);
+void rh_init(rh_t *instance, ohci_regs_t *regs);
 
-int rh_request(rh_t *instance, usb_transfer_batch_t *request);
+void rh_request(rh_t *instance, usb_transfer_batch_t *request);
 
 void rh_interrupt(rh_t *instance);
Index: uspace/drv/bus/usb/uhci/hc.c
===================================================================
--- uspace/drv/bus/usb/uhci/hc.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/uhci/hc.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -47,4 +47,13 @@
     (UHCI_STATUS_INTERRUPT | UHCI_STATUS_ERROR_INTERRUPT)
 
+static const irq_cmd_t uhci_irq_commands[] =
+{
+	{ .cmd = CMD_PIO_READ_16, .dstarg = 1, .addr = NULL/*filled later*/},
+	{ .cmd = CMD_BTEST, .srcarg = 1, .dstarg = 2,
+	  .value = UHCI_STATUS_USED_INTERRUPTS | UHCI_STATUS_NM_INTERRUPTS },
+	{ .cmd = CMD_PREDICATE, .srcarg = 2, .value = 2 },
+	{ .cmd = CMD_PIO_WRITE_A_16, .srcarg = 1, .addr = NULL/*filled later*/},
+	{ .cmd = CMD_ACCEPT },
+};
 
 static int hc_init_transfer_lists(hc_t *instance);
@@ -54,4 +63,37 @@
 static int hc_interrupt_emulator(void *arg);
 static int hc_debug_checker(void *arg);
+
+/*----------------------------------------------------------------------------*/
+/** Get number of commands used in IRQ code.
+ * @return Number of commands.
+ */
+size_t hc_irq_cmd_count(void)
+{
+	return sizeof(uhci_irq_commands) / sizeof(irq_cmd_t);
+}
+/*----------------------------------------------------------------------------*/
+/** Generate IRQ code commands.
+ * @param[out] cmds Place to store the commands.
+ * @param[in] cmd_size Size of the place (bytes).
+ * @param[in] regs Physical address of device's registers.
+ * @param[in] reg_size Size of the register area (bytes).
+ *
+ * @return Error code.
+ */
+int hc_get_irq_commands(
+    irq_cmd_t cmds[], size_t cmd_size, uintptr_t regs, size_t reg_size)
+{
+	if (cmd_size < sizeof(uhci_irq_commands)
+	    || reg_size < sizeof(uhci_regs_t))
+		return EOVERFLOW;
+
+	uhci_regs_t *registers = (uhci_regs_t*)regs;
+
+	memcpy(cmds, uhci_irq_commands, sizeof(uhci_irq_commands));
+
+	cmds[0].addr = (void*)&registers->usbsts;
+	cmds[3].addr = (void*)&registers->usbsts;
+	return EOK;
+}
 /*----------------------------------------------------------------------------*/
 /** Initialize UHCI hc driver structure
@@ -69,5 +111,5 @@
 int hc_init(hc_t *instance, void *regs, size_t reg_size, bool interrupts)
 {
-	assert(reg_size >= sizeof(regs_t));
+	assert(reg_size >= sizeof(uhci_regs_t));
 	int ret;
 
@@ -82,17 +124,16 @@
 
 	/* allow access to hc control registers */
-	regs_t *io;
+	uhci_regs_t *io;
 	ret = pio_enable(regs, reg_size, (void **)&io);
-	CHECK_RET_RETURN(ret,
-	    "Failed(%d) to gain access to registers at %p: %s.\n",
-	    ret, io, str_error(ret));
+	CHECK_RET_RETURN(ret, "Failed to gain access to registers at %p: %s.\n",
+	    io, str_error(ret));
 	instance->registers = io;
-	usb_log_debug("Device registers at %p (%zuB) accessible.\n",
-	    io, reg_size);
+	usb_log_debug(
+	    "Device registers at %p (%zuB) accessible.\n", io, reg_size);
 
 	ret = hc_init_mem_structures(instance);
 	CHECK_RET_RETURN(ret,
-	    "Failed(%d) to initialize UHCI memory structures: %s.\n",
-	    ret, str_error(ret));
+	    "Failed to initialize UHCI memory structures: %s.\n",
+	    str_error(ret));
 
 	hc_init_hw(instance);
@@ -116,12 +157,12 @@
 {
 	assert(instance);
-	regs_t *registers = instance->registers;
+	uhci_regs_t *registers = instance->registers;
 
 	/* Reset everything, who knows what touched it before us */
 	pio_write_16(&registers->usbcmd, UHCI_CMD_GLOBAL_RESET);
-	async_usleep(10000); /* 10ms according to USB spec */
+	async_usleep(50000); /* 50ms according to USB spec(root hub reset) */
 	pio_write_16(&registers->usbcmd, 0);
 
-	/* Reset hc, all states and counters */
+	/* Reset hc, all states and counters. Hope that hw is not broken */
 	pio_write_16(&registers->usbcmd, UHCI_CMD_HCRESET);
 	do { async_usleep(10); }
@@ -141,7 +182,7 @@
 	}
 
-	const uint16_t status = pio_read_16(&registers->usbcmd);
-	if (status != 0)
-		usb_log_warning("Previous command value: %x.\n", status);
+	const uint16_t cmd = pio_read_16(&registers->usbcmd);
+	if (cmd != 0)
+		usb_log_warning("Previous command value: %x.\n", cmd);
 
 	/* Start the hc with large(64B) packet FSBR */
@@ -170,60 +211,12 @@
 	} else (void) 0
 
-	/* Init interrupt code */
-	instance->interrupt_code.cmds = instance->interrupt_commands;
-	{
-		/* Read status register */
-		instance->interrupt_commands[0].cmd = CMD_PIO_READ_16;
-		instance->interrupt_commands[0].dstarg = 1;
-		instance->interrupt_commands[0].addr =
-		    &instance->registers->usbsts;
-
-		/* Test whether we are the interrupt cause */
-		instance->interrupt_commands[1].cmd = CMD_BTEST;
-		instance->interrupt_commands[1].value =
-		    UHCI_STATUS_USED_INTERRUPTS | UHCI_STATUS_NM_INTERRUPTS;
-		instance->interrupt_commands[1].srcarg = 1;
-		instance->interrupt_commands[1].dstarg = 2;
-
-		/* Predicate cleaning and accepting */
-		instance->interrupt_commands[2].cmd = CMD_PREDICATE;
-		instance->interrupt_commands[2].value = 2;
-		instance->interrupt_commands[2].srcarg = 2;
-
-		/* Write clean status register */
-		instance->interrupt_commands[3].cmd = CMD_PIO_WRITE_A_16;
-		instance->interrupt_commands[3].srcarg = 1;
-		instance->interrupt_commands[3].addr =
-		    &instance->registers->usbsts;
-
-		/* Accept interrupt */
-		instance->interrupt_commands[4].cmd = CMD_ACCEPT;
-
-		instance->interrupt_code.cmdcount = UHCI_NEEDED_IRQ_COMMANDS;
-	}
-
 	/* Init transfer lists */
 	int ret = hc_init_transfer_lists(instance);
-	CHECK_RET_RETURN(ret, "Failed to init transfer lists.\n");
+	CHECK_RET_RETURN(ret, "Failed to initialize transfer lists.\n");
 	usb_log_debug("Initialized transfer lists.\n");
-
-	/* Init USB frame list page*/
-	instance->frame_list = get_page();
-	ret = instance->frame_list ? EOK : ENOMEM;
-	CHECK_RET_RETURN(ret, "Failed to get frame list page.\n");
-	usb_log_debug("Initialized frame list at %p.\n", instance->frame_list);
-
-	/* Set all frames to point to the first queue head */
-	const uint32_t queue = LINK_POINTER_QH(
-	        addr_to_phys(instance->transfers_interrupt.queue_head));
-
-	unsigned i = 0;
-	for(; i < UHCI_FRAME_LIST_COUNT; ++i) {
-		instance->frame_list[i] = queue;
-	}
 
 	/* Init device keeper */
 	usb_device_keeper_init(&instance->manager);
-	usb_log_debug("Initialized device manager.\n");
+	usb_log_debug("Initialized device keeper.\n");
 
 	ret = usb_endpoint_manager_init(&instance->ep_manager,
@@ -232,4 +225,21 @@
 	    str_error(ret));
 
+	/* Init USB frame list page*/
+	instance->frame_list = get_page();
+	if (!instance->frame_list) {
+		usb_log_error("Failed to get frame list page.\n");
+		usb_endpoint_manager_destroy(&instance->ep_manager);
+		return ENOMEM;
+	}
+	usb_log_debug("Initialized frame list at %p.\n", instance->frame_list);
+
+	/* Set all frames to point to the first queue head */
+	const uint32_t queue = LINK_POINTER_QH(
+	        addr_to_phys(instance->transfers_interrupt.queue_head));
+	unsigned i = 0;
+	for(; i < UHCI_FRAME_LIST_COUNT; ++i) {
+		instance->frame_list[i] = queue;
+	}
+
 	return EOK;
 #undef CHECK_RET_RETURN
@@ -252,6 +262,6 @@
 	int ret = transfer_list_init(&instance->transfers_##type, name); \
 	if (ret != EOK) { \
-		usb_log_error("Failed(%d) to setup %s transfer list: %s.\n", \
-		    ret, name, str_error(ret)); \
+		usb_log_error("Failed to setup %s transfer list: %s.\n", \
+		    name, str_error(ret)); \
 		transfer_list_fini(&instance->transfers_bulk_full); \
 		transfer_list_fini(&instance->transfers_control_full); \
Index: uspace/drv/bus/usb/uhci/hc.h
===================================================================
--- uspace/drv/bus/usb/uhci/hc.h	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/uhci/hc.h	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -84,5 +84,5 @@
 	/** SOF modification to match external timers */
 	uint8_t sofmod;
-} regs_t;
+} uhci_regs_t;
 
 #define UHCI_FRAME_LIST_COUNT 1024
@@ -100,5 +100,5 @@
 
 	/** Addresses of I/O registers */
-	regs_t *registers;
+	uhci_regs_t *registers;
 
 	/** Frame List contains 1024 link pointers */
@@ -116,14 +116,6 @@
 	/** Pointer table to the above lists, helps during scheduling */
 	transfer_list_t *transfers[2][4];
-
-	/** Code to be executed in kernel interrupt handler */
-	irq_code_t interrupt_code;
-
-	/** Commands that form interrupt code */
-	irq_cmd_t interrupt_commands[UHCI_NEEDED_IRQ_COMMANDS];
-
 	/** Fibril periodically checking status register*/
 	fid_t interrupt_emulator;
-
 	/** Indicator of hw interrupts availability */
 	bool hw_interrupts;
@@ -132,9 +124,9 @@
 	unsigned hw_failures;
 } hc_t;
-
+size_t hc_irq_cmd_count(void);
+int hc_get_irq_commands(
+    irq_cmd_t cmds[], size_t cmd_size, uintptr_t regs, size_t reg_size);
 int hc_init(hc_t *instance, void *regs, size_t reg_size, bool interupts);
-
 int hc_schedule(hc_t *instance, usb_transfer_batch_t *batch);
-
 void hc_interrupt(hc_t *instance, uint16_t status);
 
Index: uspace/drv/bus/usb/uhci/root_hub.c
===================================================================
--- uspace/drv/bus/usb/uhci/root_hub.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/uhci/root_hub.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -55,7 +55,6 @@
 	int ret = asprintf(&match_str, "usb&uhci&root-hub");
 	if (ret < 0) {
-		usb_log_error(
-		    "Failed(%d) to create root hub match string: %s.\n",
-		    ret, str_error(ret));
+		usb_log_error("Failed to create root hub match string: %s.\n",
+		    str_error(ret));
 		return ret;
 	}
@@ -65,6 +64,6 @@
 	if (ret != EOK) {
 		free(match_str);
-		usb_log_error("Failed(%d) to add root hub match id: %s\n",
-		    ret, str_error(ret));
+		usb_log_error("Failed to add root hub match id: %s\n",
+		    str_error(ret));
 		return ret;
 	}
Index: uspace/drv/bus/usb/uhci/uhci.c
===================================================================
--- uspace/drv/bus/usb/uhci/uhci.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/uhci/uhci.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -64,5 +64,4 @@
 {
 	assert(dev);
-	assert(dev->driver_data);
 	return dev->driver_data;
 }
@@ -78,8 +77,10 @@
 	assert(dev);
 	uhci_t *uhci = dev_to_uhci(dev);
-	hc_t *hc = &uhci->hc;
+	if (!uhci) {
+		usb_log_error("Interrupt on not yet initialized device.\n");
+		return;
+	}
 	const uint16_t status = IPC_GET_ARG1(*call);
-	assert(hc);
-	hc_interrupt(hc, status);
+	hc_interrupt(&uhci->hc, status);
 }
 /*----------------------------------------------------------------------------*/
@@ -192,4 +193,5 @@
 	} \
 	free(instance); \
+	device->driver_data = NULL; \
 	usb_log_error(message); \
 	return ret; \
@@ -222,27 +224,35 @@
 	ret = pci_disable_legacy(device);
 	CHECK_RET_DEST_FREE_RETURN(ret,
-	    "Failed(%d) to disable legacy USB: %s.\n", ret, str_error(ret));
+	    "Failed to disable legacy USB: %s.\n", str_error(ret));
+
+	const size_t cmd_count = hc_irq_cmd_count();
+	irq_cmd_t irq_cmds[cmd_count];
+	ret =
+	    hc_get_irq_commands(irq_cmds, sizeof(irq_cmds), reg_base, reg_size);
+	CHECK_RET_DEST_FREE_RETURN(ret,
+	    "Failed to generate IRQ commands: %s.\n", str_error(ret));
+
+	irq_code_t irq_code = { .cmdcount = cmd_count, .cmds = irq_cmds };
+
+        /* Register handler to avoid interrupt lockup */
+        ret = register_interrupt_handler(device, irq, irq_handler, &irq_code);
+        CHECK_RET_DEST_FREE_RETURN(ret,
+            "Failed to register interrupt handler: %s.\n", str_error(ret));
 
 	bool interrupts = false;
-#ifdef CONFIG_USBHC_NO_INTERRUPTS
-	usb_log_warning("Interrupts disabled in OS config, " \
-	    "falling back to polling.\n");
-#else
 	ret = pci_enable_interrupts(device);
 	if (ret != EOK) {
-		usb_log_warning("Failed to enable interrupts: %s.\n",
-		    str_error(ret));
-		usb_log_info("HW interrupts not available, " \
-		    "falling back to polling.\n");
+		usb_log_warning("Failed to enable interrupts: %s."
+		    " Falling back to polling.\n", str_error(ret));
 	} else {
 		usb_log_debug("Hw interrupts enabled.\n");
 		interrupts = true;
 	}
-#endif
-
 
 	ret = hc_init(&instance->hc, (void*)reg_base, reg_size, interrupts);
 	CHECK_RET_DEST_FREE_RETURN(ret,
-	    "Failed(%d) to init uhci_hcd: %s.\n", ret, str_error(ret));
+	    "Failed to init uhci_hcd: %s.\n", str_error(ret));
+
+	device->driver_data = instance;
 
 #define CHECK_RET_FINI_RETURN(ret, message...) \
@@ -253,15 +263,7 @@
 } else (void)0
 
-	/* It does no harm if we register this on polling */
-	ret = register_interrupt_handler(device, irq, irq_handler,
-	    &instance->hc.interrupt_code);
-	CHECK_RET_FINI_RETURN(ret,
-	    "Failed(%d) to register interrupt handler: %s.\n",
-	    ret, str_error(ret));
-
 	ret = ddf_fun_bind(instance->hc_fun);
-	CHECK_RET_FINI_RETURN(ret,
-	    "Failed(%d) to bind UHCI device function: %s.\n",
-	    ret, str_error(ret));
+	CHECK_RET_FINI_RETURN(ret, "Failed to bind UHCI device function: %s.\n",
+	    str_error(ret));
 
 	ret = ddf_fun_add_to_class(instance->hc_fun, USB_HC_DDF_CLASS_NAME);
@@ -272,11 +274,10 @@
 	    (uintptr_t)instance->hc.registers + 0x10, 4);
 	CHECK_RET_FINI_RETURN(ret,
-	    "Failed(%d) to setup UHCI root hub: %s.\n", ret, str_error(ret));
+	    "Failed to setup UHCI root hub: %s.\n", str_error(ret));
 
 	ret = ddf_fun_bind(instance->rh_fun);
 	CHECK_RET_FINI_RETURN(ret,
-	    "Failed(%d) to register UHCI root hub: %s.\n", ret, str_error(ret));
-
-	device->driver_data = instance;
+	    "Failed to register UHCI root hub: %s.\n", str_error(ret));
+
 	return EOK;
 #undef CHECK_RET_FINI_RETURN
Index: uspace/drv/bus/usb/usbhub/usbhub.c
===================================================================
--- uspace/drv/bus/usb/usbhub/usbhub.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/usbhub/usbhub.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -220,9 +220,9 @@
  * @return error code
  */
-static int usb_hub_process_hub_specific_info(usb_hub_info_t *hub_info) {
+int usb_hub_process_hub_specific_info(usb_hub_info_t *hub_info)
+{
 	// get hub descriptor
-	usb_log_debug("Creating serialized descriptor\n");
+	usb_log_debug("Retrieving descriptor\n");
 	uint8_t serialized_descriptor[USB_HUB_MAX_DESCRIPTOR_SIZE];
-	usb_hub_descriptor_t * descriptor;
 	int opResult;
 
@@ -234,47 +234,53 @@
 
 	if (opResult != EOK) {
-		usb_log_error("Failed when receiving hub descriptor, "
-		    "%s\n",
-		    str_error(opResult));
-		free(serialized_descriptor);
+		usb_log_error("Failed to receive hub descriptor: %s.\n",
+		    str_error(opResult));
 		return opResult;
 	}
-	usb_log_debug2("Deserializing descriptor\n");
-	descriptor = usb_create_deserialized_hub_desriptor(
-	    serialized_descriptor);
-	if (descriptor == NULL) {
-		usb_log_warning("could not deserialize descriptor \n");
-		return ENOMEM;
-	}
-	usb_log_debug("setting port count to %d\n", descriptor->ports_count);
-	hub_info->port_count = descriptor->ports_count;
-	bool is_power_switched =
-	    ((descriptor->hub_characteristics & 1) == 0);
-	bool has_individual_port_powering =
-	    ((descriptor->hub_characteristics & 1) != 0);
-	hub_info->ports = malloc(
-	    sizeof (usb_hub_port_t) * (hub_info->port_count + 1));
+	usb_log_debug2("Parsing descriptor\n");
+	usb_hub_descriptor_t descriptor;
+	opResult = usb_deserialize_hub_desriptor(
+	        serialized_descriptor, received_size, &descriptor);
+	if (opResult != EOK) {
+		usb_log_error("Could not parse descriptor: %s\n",
+		    str_error(opResult));
+		return opResult;
+	}
+	usb_log_debug("Setting port count to %d.\n", descriptor.ports_count);
+	hub_info->port_count = descriptor.ports_count;
+
+	hub_info->ports =
+	    malloc(sizeof(usb_hub_port_t) * (hub_info->port_count + 1));
 	if (!hub_info->ports) {
 		return ENOMEM;
 	}
+
 	size_t port;
 	for (port = 0; port < hub_info->port_count + 1; ++port) {
 		usb_hub_port_init(&hub_info->ports[port]);
 	}
+
+	const bool is_power_switched =
+	    !(descriptor.hub_characteristics & HUB_CHAR_NO_POWER_SWITCH_FLAG);
 	if (is_power_switched) {
 		usb_log_debug("Hub power switched\n");
-
-		if (!has_individual_port_powering) {
-			//this setting actually makes no difference
-			usb_log_debug("Hub has global powering\n");
-		}
+		const bool per_port_power = descriptor.hub_characteristics
+		    & HUB_CHAR_POWER_PER_PORT_FLAG;
 
 		for (port = 1; port <= hub_info->port_count; ++port) {
 			usb_log_debug("Powering port %zu.\n", port);
-			opResult = usb_hub_set_port_feature(hub_info->control_pipe,
+			opResult = usb_hub_set_port_feature(
+			    hub_info->control_pipe,
 			    port, USB_HUB_FEATURE_PORT_POWER);
 			if (opResult != EOK) {
 				usb_log_error("Cannot power on port %zu: %s.\n",
 				    port, str_error(opResult));
+			} else {
+				if (!per_port_power) {
+					usb_log_debug(
+					    "Ganged power switching mode, "
+					    "one port is enough.\n");
+					break;
+				}
 			}
 		}
@@ -283,6 +289,4 @@
 		usb_log_debug("Power not switched, not going to be powered\n");
 	}
-	usb_log_debug2("Freeing data\n");
-	free(descriptor);
 	return EOK;
 }
Index: uspace/drv/bus/usb/usbhub/usbhub_private.h
===================================================================
--- uspace/drv/bus/usb/usbhub/usbhub_private.h	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/usbhub/usbhub_private.h	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -171,9 +171,6 @@
     void * serialized_descriptor);
 
-usb_hub_descriptor_t * usb_create_deserialized_hub_desriptor(
-    void * serialized_descriptor);
-
-void usb_deserialize_hub_desriptor(void * serialized_descriptor,
-    usb_hub_descriptor_t * descriptor);
+int usb_deserialize_hub_desriptor(
+    void *serialized_descriptor, size_t size, usb_hub_descriptor_t *descriptor);
 
 
Index: uspace/drv/bus/usb/usbhub/utils.c
===================================================================
--- uspace/drv/bus/usb/usbhub/utils.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/usbhub/utils.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -110,53 +110,45 @@
 }
 
+/*----------------------------------------------------------------------------*/
 /**
- * create deserialized desriptor structure out of serialized descriptor
+ * Deserialize descriptor into given pointer
  *
- * The serialized descriptor must be proper usb hub descriptor,
- * otherwise an eerror might occur.
- *
- * @param sdescriptor serialized descriptor
- * @return newly created deserialized descriptor pointer
- */
-usb_hub_descriptor_t * usb_create_deserialized_hub_desriptor(
-    void *serialized_descriptor) {
-	uint8_t * sdescriptor = serialized_descriptor;
-
-	if (sdescriptor[1] != USB_DESCTYPE_HUB) {
-		usb_log_warning("trying to deserialize wrong descriptor %x\n",
-		    sdescriptor[1]);
-		return NULL;
-	}
-
-	usb_hub_descriptor_t * result = malloc(sizeof (usb_hub_descriptor_t));
-	if (result)
-		usb_deserialize_hub_desriptor(serialized_descriptor, result);
-	return result;
-}
-
-/**
- * deserialize descriptor into given pointer
- * 
  * @param serialized_descriptor
  * @param descriptor
  * @return
  */
-void usb_deserialize_hub_desriptor(
-    void * serialized_descriptor, usb_hub_descriptor_t *descriptor) {
+int usb_deserialize_hub_desriptor(
+    void *serialized_descriptor, size_t size, usb_hub_descriptor_t *descriptor)
+{
 	uint8_t * sdescriptor = serialized_descriptor;
+
+	if (sdescriptor[1] != USB_DESCTYPE_HUB) {
+		usb_log_error("Trying to deserialize wrong descriptor %x\n",
+		    sdescriptor[1]);
+		return EINVAL;
+	}
+	if (size < 7) {
+		usb_log_error("Serialized descriptor too small.\n");
+		return EOVERFLOW;
+	}
+
 	descriptor->ports_count = sdescriptor[2];
-	/// @fixme handling of endianness??
-	descriptor->hub_characteristics = sdescriptor[4] + 256 * sdescriptor[3];
+	descriptor->hub_characteristics = sdescriptor[3] + 256 * sdescriptor[4];
 	descriptor->pwr_on_2_good_time = sdescriptor[5];
 	descriptor->current_requirement = sdescriptor[6];
-	size_t var_size = (descriptor->ports_count + 7) / 8;
+	const size_t var_size = (descriptor->ports_count + 7) / 8;
 	//descriptor->devices_removable = (uint8_t*) malloc(var_size);
 
-	size_t i;
-	for (i = 0; i < var_size; ++i) {
+	if (size < (7 + var_size)) {
+		usb_log_error("Serialized descriptor too small.\n");
+		return EOVERFLOW;
+	}
+	size_t i = 0;
+	for (; i < var_size; ++i) {
 		descriptor->devices_removable[i] = sdescriptor[7 + i];
 	}
+	return EOK;
 }
-
+/*----------------------------------------------------------------------------*/
 /**
  * @}
Index: uspace/drv/bus/usb/vhc/connhost.c
===================================================================
--- uspace/drv/bus/usb/vhc/connhost.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/drv/bus/usb/vhc/connhost.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -141,19 +141,14 @@
     size_t max_packet_size, unsigned int interval)
 {
-	VHC_DATA(vhc, fun);
-
-	endpoint_t *ep = malloc(sizeof(endpoint_t));
+	/* TODO: Use usb_endpoint_manager_add_ep */
+	VHC_DATA(vhc, fun);
+
+	endpoint_t *ep = endpoint_get(
+	    address, endpoint, direction, transfer_type, USB_SPEED_FULL, 1);
 	if (ep == NULL) {
 		return ENOMEM;
 	}
 
-	int rc = endpoint_init(ep, address, endpoint, direction, transfer_type,
-	    USB_SPEED_FULL, 1);
-	if (rc != EOK) {
-		free(ep);
-		return rc;
-	}
-
-	rc = usb_endpoint_manager_register_ep(&vhc->ep_manager, ep, 1);
+	int rc = usb_endpoint_manager_register_ep(&vhc->ep_manager, ep, 1);
 	if (rc != EOK) {
 		endpoint_destroy(ep);
Index: uspace/lib/usb/include/usb/classes/hub.h
===================================================================
--- uspace/lib/usb/include/usb/classes/hub.h	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/lib/usb/include/usb/classes/hub.h	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -119,4 +119,6 @@
      */
     uint16_t hub_characteristics;
+#define HUB_CHAR_POWER_PER_PORT_FLAG  (1 << 0)
+#define HUB_CHAR_NO_POWER_SWITCH_FLAG (1 << 1)
 
     /**
Index: uspace/lib/usbhost/include/usb/host/endpoint.h
===================================================================
--- uspace/lib/usbhost/include/usb/host/endpoint.h	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/lib/usbhost/include/usb/host/endpoint.h	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -61,7 +61,7 @@
 } endpoint_t;
 
-int endpoint_init(endpoint_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);
+endpoint_t * endpoint_get(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);
 
 void endpoint_destroy(endpoint_t *instance);
Index: uspace/lib/usbhost/include/usb/host/usb_endpoint_manager.h
===================================================================
--- uspace/lib/usbhost/include/usb/host/usb_endpoint_manager.h	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/lib/usbhost/include/usb/host/usb_endpoint_manager.h	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -82,21 +82,15 @@
     size_t data_size)
 {
-	endpoint_t *ep = malloc(sizeof(endpoint_t));
-	if (ep == NULL)
+	endpoint_t *ep = endpoint_get(
+	    address, endpoint, direction, type, speed, max_packet_size);
+	if (!ep)
 		return ENOMEM;
 
-	int ret = endpoint_init(ep, address, endpoint, direction, type, speed,
-	    max_packet_size);
-	if (ret != EOK) {
-		free(ep);
-		return ret;
-	}
-
-	ret = usb_endpoint_manager_register_ep(instance, ep, data_size);
+	const int ret =
+	    usb_endpoint_manager_register_ep(instance, ep, data_size);
 	if (ret != EOK) {
 		endpoint_destroy(ep);
-		return ret;
 	}
-	return EOK;
+	return ret;
 }
 #endif
Index: uspace/lib/usbhost/src/batch.c
===================================================================
--- uspace/lib/usbhost/src/batch.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/lib/usbhost/src/batch.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -109,4 +109,5 @@
 	assert(instance);
 	assert(instance->ep);
+	assert(instance->next_step);
 	endpoint_release(instance->ep);
 	instance->next_step(instance);
Index: uspace/lib/usbhost/src/endpoint.c
===================================================================
--- uspace/lib/usbhost/src/endpoint.c	(revision 3e5c48c98a9c3b9f430f7be2dc2213810c7f05ed)
+++ uspace/lib/usbhost/src/endpoint.c	(revision 159100a5b71a2aec1ea87131ae1ae38a533a493c)
@@ -39,21 +39,23 @@
 #include <usb/host/endpoint.h>
 
-int endpoint_init(endpoint_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)
+endpoint_t * endpoint_get(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)
 {
-	assert(instance);
-	instance->address = address;
-	instance->endpoint = endpoint;
-	instance->direction = direction;
-	instance->transfer_type = type;
-	instance->speed = speed;
-	instance->max_packet_size = max_packet_size;
-	instance->toggle = 0;
-	instance->active = false;
-	fibril_mutex_initialize(&instance->guard);
-	fibril_condvar_initialize(&instance->avail);
-	endpoint_clear_hc_data(instance);
-	return EOK;
+	endpoint_t *instance = malloc(sizeof(endpoint_t));
+	if (instance) {
+		instance->address = address;
+		instance->endpoint = endpoint;
+		instance->direction = direction;
+		instance->transfer_type = type;
+		instance->speed = speed;
+		instance->max_packet_size = max_packet_size;
+		instance->toggle = 0;
+		instance->active = false;
+		fibril_mutex_initialize(&instance->guard);
+		fibril_condvar_initialize(&instance->avail);
+		endpoint_clear_hc_data(instance);
+	}
+	return instance;
 }
 /*----------------------------------------------------------------------------*/
