diff --git a/minix/drivers/usb/usbd/base/usbd.c b/minix/drivers/usb/usbd/base/usbd.c
index ee7299bc9..809bdce2a 100644
--- a/minix/drivers/usb/usbd/base/usbd.c
+++ b/minix/drivers/usb/usbd/base/usbd.c
@@ -38,7 +38,7 @@ main(int UNUSED(argc), char * UNUSED(argv[]))
 	int ret_val;
 
 	USB_MSG("Starting USBD");
-	USB_DBG("Built: %s %s", __DATE__, __TIME__);
+	USB_MSG("Built: %s %s", __DATE__, __TIME__);
 
 	/* Basic SEF,DDE,... initialization */
 	usbd_init();
@@ -75,7 +75,7 @@ main(int UNUSED(argc), char * UNUSED(argv[]))
 static int
 usbd_sef_handler(int type, sef_init_info_t * UNUSED(info))
 {
-	DEBUG_DUMP;
+	/* No DEBUG_DUMP, threading unavailable yet */
 
 	switch (type) {
 		case SEF_INIT_FRESH:
@@ -126,7 +126,7 @@ usbd_start(void)
 	DEBUG_DUMP;
 
 	/* Driver's "main loop" is within DDEKit server thread */
-	usbd_th = ddekit_thread_create(usbd_server_thread, NULL, "usbd");
+	usbd_th = ddekit_thread_create(usbd_server_thread, NULL, "ddekit_usb");
 
 	/* After spawning, allow server thread to work */
 	if (NULL != usbd_th) {
@@ -157,16 +157,18 @@ usbd_start(void)
 static void
 usbd_init(void)
 {
-	DEBUG_DUMP;
+	/* No DEBUG_DUMP, threading unavailable yet */
 
 	/* Set one handler for all messages */
 	sef_setcb_init_fresh(usbd_sef_handler);
 	sef_setcb_init_lu(usbd_sef_handler);
 	sef_setcb_init_restart(usbd_sef_handler);
-	sef_setcb_signal_handler(usbd_signal_handler);
 
 	/* Initialize DDEkit (involves sef_startup()) */
 	ddekit_init();
+
+	/* After threading initialization, add signal handler */
+	sef_setcb_signal_handler(usbd_signal_handler);
 }
 
 
diff --git a/minix/drivers/usb/usbd/hcd/hcd.c b/minix/drivers/usb/usbd/hcd/hcd.c
index db5fd5a88..018ceea1f 100644
--- a/minix/drivers/usb/usbd/hcd/hcd.c
+++ b/minix/drivers/usb/usbd/hcd/hcd.c
@@ -28,6 +28,8 @@ static hcd_device_state * hcd_get_child_for_ep(hcd_device_state *, hcd_reg1);
 /* For HCD level, hub handling */
 static void hcd_add_child(hcd_device_state *, hcd_reg1, hcd_speed);
 static void hcd_delete_child(hcd_device_state *, hcd_reg1);
+static void hcd_disconnect_tree(hcd_device_state *);
+static void hcd_dump_tree(hcd_device_state *, hcd_reg1);
 
 /* Typical USD device communication procedures */
 static int hcd_enumerate(hcd_device_state *);
@@ -36,6 +38,7 @@ static int hcd_set_address(hcd_device_state *);
 static int hcd_get_descriptor_tree(hcd_device_state *);
 static int hcd_set_configuration(hcd_device_state *, hcd_reg1);
 static void hcd_handle_urb(hcd_device_state *);
+static void hcd_complete_urb(hcd_device_state *);
 static int hcd_control_urb(hcd_device_state *, hcd_urb *);
 static int hcd_non_control_urb(hcd_device_state *, hcd_urb *);
 
@@ -69,13 +72,30 @@ hcd_handle_event(hcd_device_state * device, hcd_event event, hcd_reg1 val)
 {
 	DEBUG_DUMP;
 
-	/* No device may be supplied */
-	if (NULL == device) {
+	/* Invalid device may be supplied */
+	if (EXIT_SUCCESS != hcd_check_device(device)) {
 		USB_MSG("No device available for event: 0x%02X, value: 0x%02X",
 			event, val);
 		return;
 	}
 
+#ifdef HCD_DUMP_DEVICE_TREE
+	/* This can be unlocked to dump current USB device tree on event */
+	{
+		/* Go to the base of USB device tree and
+		 * print the current state of it */
+		hcd_device_state * base;
+
+		base = device;
+
+		while (NULL != base->parent)
+			base = base->parent;
+
+		USB_MSG("Current state of USB device tree:");
+		hcd_dump_tree(base, 0);
+	}
+#endif
+
 	/* Handle event and forward control to device thread when required */
 	switch (event) {
 		case HCD_EVENT_CONNECTED:
@@ -95,13 +115,9 @@ hcd_handle_event(hcd_device_state * device, hcd_event event, hcd_reg1 val)
 				"Device is marked as 'disconnected' "
 				"for 'disconnection' event");
 
-			/* If connect callback was used before, call
-			 * it's equivalent to signal disconnection */
-			if (HCD_STATE_CONNECTED == device->state)
-				hcd_disconnect_cb(device);
-
-			/* Handle device disconnection (freeing memory etc.) */
-			hcd_disconnect_device(device);
+			/* Make this device and all attached children
+			 * disconnect recursively */
+			hcd_disconnect_tree(device);
 
 			break;
 
@@ -143,10 +159,11 @@ hcd_handle_event(hcd_device_state * device, hcd_event event, hcd_reg1 val)
 				"Device is marked as 'disconnected' "
 				"for 'hub port detach' event");
 
+			hcd_delete_child(device, val);
+
 			USB_MSG("Device disconnected from "
 				"hub 0x%08X, port %u", device, val);
 
-			hcd_delete_child(device, val);
 			break;
 
 		case HCD_EVENT_ENDPOINT:
@@ -260,9 +277,6 @@ hcd_device_thread(void * thread_args)
 		/* Block and wait for something like 'submit URB' */
 		hcd_device_wait(this_device, HCD_EVENT_URB, HCD_UNUSED_VAL);
 		hcd_handle_urb(this_device);
-
-		/* Only external URBs should be submitted here */
-		hcd_completion_cb(this_device->urb);
 	}
 
 	/* Finish device handling to avoid leaving thread */
@@ -294,40 +308,48 @@ hcd_device_finish(hcd_device_state * this_device, const char * finish_msg)
 static hcd_device_state *
 hcd_get_child_for_ep(hcd_device_state * device, hcd_reg1 ep)
 {
-	hcd_device_state * d;
-	hcd_device_state * found;
-	int child_num;
+	hcd_device_state * child_found;
+	hcd_device_state * final_found;
+	hcd_device_state * child;
+	hcd_reg1 child_num;
 
 	DEBUG_DUMP;
 
-	/* Start with parent */
-	d = device;
-	child_num = 0;
+	/* Nothing yet */
+	final_found = NULL;
 
-	/* Nothing found yet */
-	found = NULL;
+	/* Check if any children (and their children) wait for EP event */
+	/* Every device in tree is checked every time so errors can be found */
+	for (child_num = 0; child_num < HCD_CHILDREN; child_num++) {
+		/* Device, to be checked for EP event recursively... */
+		child = device->child[child_num];
 
-	/* When device has multiple children (hub), this allows
-	 * routing interrupt login downstream */
-	do {
-		/* Must exist and match requirements */
-		if ((NULL != d) &&
-		    (HCD_EVENT_ENDPOINT == d->wait_event) &&
-		    (ep == d->wait_ep)) {
+		/* ...but only if attached */
+		if (NULL != child) {
+			/* Look deeper first */
+			child_found = hcd_get_child_for_ep(child, ep);
 
-			/* Waiting for EP by multiple devices is mutually
-			 * exclusive, as long as scheduler works correctly */
-			USB_ASSERT(NULL == found, "More than one device waits "
-						"for given endpoint interrupt");
-			found = d;
+			if (NULL != child_found) {
+				/* Only one device can wait for EP event */
+				USB_ASSERT((NULL == final_found),
+					"More than one device waits for EP");
+				/* Remember what was found */
+				final_found = child_found;
+			}
 		}
+	}
 
-		/* Check next child */
-		d = device->child[child_num++];
+	/* Check this device last */
+	if ((HCD_EVENT_ENDPOINT == device->wait_event) &&
+	    (ep == device->wait_ep)) {
+		/* Only one device can wait for EP event */
+		USB_ASSERT((NULL == final_found),
+			"More than one device waits for EP");
+		/* Remember what was found */
+		final_found = device;
+	}
 
-	} while(child_num <= HCD_CHILDREN);
-
-	return found;
+	return final_found;
 }
 
 
@@ -365,27 +387,85 @@ hcd_add_child(hcd_device_state * parent, hcd_reg1 port, hcd_speed speed)
 static void
 hcd_delete_child(hcd_device_state * parent, hcd_reg1 port)
 {
+	hcd_device_state * child;
+
 	DEBUG_DUMP;
 
 	USB_ASSERT(port < HCD_CHILDREN, "Port number too high");
-	USB_ASSERT(NULL != parent->child[port], "Child device does not exist");
 
-	/* If connect callback was used before, call
-	 * it's equivalent to signal disconnection */
-	if (HCD_STATE_CONNECTED == parent->child[port]->state)
-		hcd_disconnect_cb(parent->child[port]);
+	child = parent->child[port]; /* Child to be detached */
 
-	/* Disconnect to release fields */
-	hcd_disconnect_device(parent->child[port]);
+	USB_ASSERT(NULL != child, "Child device does not exist");
+
+	/* Make this child device and all its attached children
+	 * disconnect recursively */
+	hcd_disconnect_tree(child);
 
 	/* Delete to release device itself */
-	hcd_delete_device(parent->child[port]);
+	hcd_delete_device(child);
 
 	/* Mark as released */
 	parent->child[port] = NULL;
 }
 
 
+/*===========================================================================*
+ *    hcd_disconnect_tree                                                    *
+ *===========================================================================*/
+static void
+hcd_disconnect_tree(hcd_device_state * device)
+{
+	hcd_reg1 child_num;
+
+	DEBUG_DUMP;
+
+	/* Generate disconnect event for all children */
+	for (child_num = 0; child_num < HCD_CHILDREN; child_num++) {
+		if (NULL != device->child[child_num])
+			hcd_handle_event(device, HCD_EVENT_PORT_DISCONNECTED,
+					child_num);
+	}
+
+	/* If this device was detached during URB handling, some steps must be
+	 * taken to ensure that no process/thread is waiting for completion */
+	if (NULL != device->urb) {
+		USB_MSG("Unplugged device had unhandled URB");
+		/* Tell device driver that device was detached */
+		/* TODO: ENODEV selected for that */
+		device->urb->inout_status = ENODEV;
+		hcd_complete_urb(device);
+	}
+
+	/* If connect callback was used before, call
+	 * it's equivalent to signal disconnection */
+	if (HCD_STATE_CONNECTED == device->state)
+		hcd_disconnect_cb(device);
+
+	/* Handle device disconnection (freeing memory etc.) */
+	hcd_disconnect_device(device);
+}
+
+
+/*===========================================================================*
+ *    hcd_dump_tree                                                          *
+ *===========================================================================*/
+static void
+hcd_dump_tree(hcd_device_state * device, hcd_reg1 level)
+{
+	hcd_reg1 child_num;
+
+	/* DEBUG_DUMP; */ /* Let's keep tree output cleaner */
+
+	USB_MSG("Device on level %03u: 0x%08X", level, device);
+
+	/* Traverse device tree recursively */
+	for (child_num = 0; child_num < HCD_CHILDREN; child_num++) {
+		if (NULL != device->child[child_num])
+			hcd_dump_tree(device->child[child_num], level + 1);
+	}
+}
+
+
 /*===========================================================================*
  *    hcd_enumerate                                                          *
  *===========================================================================*/
@@ -803,10 +883,27 @@ hcd_handle_urb(hcd_device_state * this_device)
 	} else
 		USB_MSG("Invalid URB supplied");
 
+	/* Perform completion routine */
+	hcd_complete_urb(this_device);
+}
+
+
+/*===========================================================================*
+ *    hcd_complete_urb                                                       *
+ *===========================================================================*/
+static void
+hcd_complete_urb(hcd_device_state * this_device)
+{
+	DEBUG_DUMP;
+
 	/* Signal scheduler that URB was handled */
-	/* TODO: This works based on the fact that device thread has higher
-	 * priority than scheduler and won't change context within this call */
-	urb->handled(urb);
+	this_device->urb->handled(this_device->urb);
+
+	/* Use this callback in case it is an external URB */
+	hcd_completion_cb(this_device->urb);
+
+	/* Make device forget about this URB */
+	this_device->urb = NULL;
 }
 
 
@@ -916,7 +1013,14 @@ hcd_non_control_urb(hcd_device_state * this_device, hcd_urb * urb)
 	request.direction = urb->direction;
 	request.data_left = (int)urb->in_size;
 	request.data = urb->inout_data;
+	/* TODO: This was changed to allow software scheduler to work correctly
+	 * by switching URBs when they NAK, rather than waiting forever if URB
+	 * which requires such waiting, was issued */
+#if 0
 	request.interval = urb->interval;
+#else
+	request.interval = HCD_DEFAULT_NAKLIMIT;
+#endif
 
 	/* Assign to let know how much data can be transfered at a time */
 	request.max_packet_size = UGETW(e->descriptor.wMaxPacketSize);
@@ -967,7 +1071,8 @@ hcd_setup_packet(hcd_device_state * this_device, hcd_ctrlrequest * setup,
 	this_device->control_len = 0;		/* Nothing read yet */
 
 	/* Set parameters for further communication */
-	d->setup_device(d->private_data, ep, this_device->current_address);
+	d->setup_device(d->private_data, ep, this_device->current_address,
+			NULL, NULL);
 
 	/* Send setup packet */
 	d->setup_stage(d->private_data, setup);
@@ -1130,7 +1235,9 @@ hcd_data_transfer(hcd_device_state * this_device, hcd_datarequest * request)
 
 	/* Set parameters for further communication */
 	d->setup_device(d->private_data, request->endpoint,
-			this_device->current_address);
+			this_device->current_address,
+			&(this_device->ep_tx_tog[request->endpoint]),
+			&(this_device->ep_rx_tog[request->endpoint]));
 
 	/* Check transfer direction first */
 	if (HCD_DIRECTION_IN == request->direction) {
diff --git a/minix/drivers/usb/usbd/hcd/hcd_common.c b/minix/drivers/usb/usbd/hcd/hcd_common.c
index 59301fc09..d6b6f798c 100644
--- a/minix/drivers/usb/usbd/hcd/hcd_common.c
+++ b/minix/drivers/usb/usbd/hcd/hcd_common.c
@@ -31,6 +31,13 @@ static hcd_reg1 hcd_reserve_addr(hcd_driver_state *);
 static void hcd_release_addr(hcd_driver_state *, hcd_reg1);
 
 
+/*===========================================================================*
+ *    Local definitions                                                      *
+ *===========================================================================*/
+/* List of all allocated devices */
+static hcd_device_state * dev_list = NULL;
+
+
 /*===========================================================================*
  *    hcd_os_interrupt_attach                                                *
  *===========================================================================*/
@@ -282,7 +289,7 @@ hcd_device_wait(hcd_device_state * device, hcd_event event, hcd_reg1 ep)
 {
 	DEBUG_DUMP;
 
-	USB_DBG("Device 0x%08X wait (0x%02X, 0x%02X)", device, event, ep);
+	USB_DBG("0x%08X wait (0x%02X, 0x%02X)", device, event, ep);
 
 	device->wait_event = event;
 	device->wait_ep = ep;
@@ -299,7 +306,7 @@ hcd_device_continue(hcd_device_state * device, hcd_event event, hcd_reg1 ep)
 {
 	DEBUG_DUMP;
 
-	USB_DBG("Device 0x%08X continue (0x%02X, 0x%02X)", device, event, ep);
+	USB_DBG("0x%08X continue (0x%02X, 0x%02X)", device, event, ep);
 
 	USB_ASSERT(device->wait_event == event, "Unexpected event");
 	USB_ASSERT(device->wait_ep == ep, "Unexpected endpoint");
@@ -323,6 +330,18 @@ hcd_new_device(void)
 
 	USB_ASSERT(NULL != d, "Failed to allocate device");
 
+	if (NULL == dev_list) {
+		dev_list = d;
+	} else {
+		d->_next = dev_list;
+		dev_list = d;
+	}
+
+#ifdef HCD_DUMP_DEVICE_LIST
+	/* Dump updated state of device list */
+	hcd_dump_devices();
+#endif
+
 	return d;
 }
 
@@ -333,9 +352,78 @@ hcd_new_device(void)
 void
 hcd_delete_device(hcd_device_state * d)
 {
+	hcd_device_state * temp;
+
 	DEBUG_DUMP;
 
+	if (d == dev_list) {
+		dev_list = dev_list->_next;
+	} else {
+		temp = dev_list;
+
+		/* Find the device and ... */
+		while (temp->_next != d) {
+			USB_ASSERT(NULL != temp->_next,
+				"Invalid state of device list");
+			temp = temp->_next;
+		}
+
+		/* ...make device list forget about it */
+		temp->_next = temp->_next->_next;
+	}
+
 	free(d);
+
+#ifdef HCD_DUMP_DEVICE_LIST
+	/* Dump updated state of device list */
+	hcd_dump_devices();
+#endif
+}
+
+
+/*===========================================================================*
+ *    hcd_dump_devices                                                       *
+ *===========================================================================*/
+void
+hcd_dump_devices(void)
+{
+	hcd_device_state * temp;
+
+	DEBUG_DUMP;
+
+	temp = dev_list;
+
+	USB_MSG("Allocated devices:");
+
+	while (NULL != temp) {
+		USB_MSG("0x%08X", (int)temp);
+		temp = temp->_next;
+	}
+}
+
+
+/*===========================================================================*
+ *    hcd_check_device                                                       *
+ *===========================================================================*/
+int
+hcd_check_device(hcd_device_state * d)
+{
+	hcd_device_state * temp;
+
+	DEBUG_DUMP;
+
+	temp = dev_list;
+
+	/* Traverse the list of allocated devices
+	 * to determine validity of this one */
+	while (NULL != temp) {
+		if (temp == d)
+			return EXIT_SUCCESS; /* Device found within the list */
+		temp = temp->_next;
+	}
+
+	/* Device was not found, may have been removed earlier */
+	return EXIT_FAILURE;
 }
 
 
diff --git a/minix/drivers/usb/usbd/hcd/hcd_ddekit.c b/minix/drivers/usb/usbd/hcd/hcd_ddekit.c
index 9c9684663..c9862c865 100644
--- a/minix/drivers/usb/usbd/hcd/hcd_ddekit.c
+++ b/minix/drivers/usb/usbd/hcd/hcd_ddekit.c
@@ -308,15 +308,17 @@ hcd_completion_cb(hcd_urb * urb)
 	/* Recollect original URB */
 	d_urb = (struct ddekit_usb_urb *)urb->original_urb;
 
-	USB_ASSERT(NULL != d_urb, "Original DDEKit URB missing");
+	/* Original URB will not be NULL if URB
+	 * was external (from device driver) */
+	if (NULL != d_urb) {
+		/* Turn HCD URB format to one handled by DDEKit */
+		hcd_encode_urb(urb, d_urb);
 
-	/* Turn HCD URB format to one handled by DDEKit */
-	hcd_encode_urb(urb, d_urb);
+		/* No need for this URB anymore */
+		hcd_free_urb(urb);
 
-	/* No need for this URB anymore */
-	hcd_free_urb(urb);
-
-	completion_cb(d_urb->priv);
+		completion_cb(d_urb->priv);
+	}
 }
 
 
diff --git a/minix/drivers/usb/usbd/hcd/hcd_schedule.c b/minix/drivers/usb/usbd/hcd/hcd_schedule.c
index 49906bf32..feacd214e 100644
--- a/minix/drivers/usb/usbd/hcd/hcd_schedule.c
+++ b/minix/drivers/usb/usbd/hcd/hcd_schedule.c
@@ -68,7 +68,8 @@ usbd_init_scheduler(void)
 	num_stored_urbs = 0;
 	memset(stored_urb, 0, sizeof(stored_urb));
 
-	urb_thread = ddekit_thread_create(hcd_urb_scheduler_task, NULL, "urb");
+	urb_thread = ddekit_thread_create(hcd_urb_scheduler_task, NULL,
+					"scheduler");
 	if (NULL == urb_thread)
 		goto ERR1;
 
@@ -170,6 +171,7 @@ hcd_unschedule_urb(hcd_urb * urb)
 static void
 hcd_urb_scheduler_task(void * UNUSED(arg))
 {
+	hcd_device_state * current_device;
 	hcd_urb * current_urb;
 
 	DEBUG_DUMP;
@@ -181,18 +183,37 @@ hcd_urb_scheduler_task(void * UNUSED(arg))
 		/* Get URB */
 		current_urb = hcd_get_urb();
 
+		/* Get URB's target device */
+		current_device = current_urb->target_device;
+
 		/* Check for mismatch */
 		USB_ASSERT(NULL != current_urb, "URB missing after URB unlock");
 
-		/* Tell device that this is its URB */
-		current_urb->target_device->urb = current_urb;
+		/* Check if URB's device is still allocated */
+		if (EXIT_SUCCESS == hcd_check_device(current_device)) {
+			/* Tell device that this is its URB */
+			current_device->urb = current_urb;
 
-		/* Start handling URB event */
-		hcd_handle_event(current_urb->target_device,
-				HCD_EVENT_URB, HCD_UNUSED_VAL);
+			/* Start handling URB event */
+			hcd_handle_event(current_device, HCD_EVENT_URB,
+					HCD_UNUSED_VAL);
 
-		/* Wait for completion */
-		ddekit_sem_down(handled_lock);
+			/* Wait for completion */
+			ddekit_sem_down(handled_lock);
+
+			/* TODO: Not enough DDEKit thread priorities
+			 * for a better solution */
+			/* Yield, to allow unlocking thread, to continue
+			 * before next URB is used */
+			ddekit_yield();
+
+			/* Makes thread debugging easier */
+			USB_DBG("URB handled, scheduler unlocked");
+		} else {
+			USB_MSG("Device 0x%08X for URB 0x%08X, is unavailable",
+				(int)current_device,
+				(int)current_urb);
+		}
 	}
 }
 
@@ -206,8 +227,6 @@ hcd_urb_handled(hcd_urb * urb)
 	DEBUG_DUMP;
 
 	/* This URB will be scheduled no more */
-	/* TODO: It would be better if this was connected
-	 * to setting urb_lock down */
 	hcd_unschedule_urb(urb);
 
 	/* Handling completed */
diff --git a/minix/drivers/usb/usbd/hcd/musb/musb_core.c b/minix/drivers/usb/usbd/hcd/musb/musb_core.c
index 2232e1584..37417d5e2 100644
--- a/minix/drivers/usb/usbd/hcd/musb/musb_core.c
+++ b/minix/drivers/usb/usbd/hcd/musb/musb_core.c
@@ -345,13 +345,16 @@ musb_core_stop(void * cfg)
  *    musb_setup_device                                                      *
  *===========================================================================*/
 void
-musb_setup_device(void * cfg, hcd_reg1 ep, hcd_reg1 addr)
+musb_setup_device(void * cfg, hcd_reg1 ep, hcd_reg1 addr,
+		hcd_datatog * tx_tog, hcd_datatog * rx_tog)
 {
 	DEBUG_DUMP;
 
 	/* Assign  */
 	((musb_core_config *)cfg)->ep = ep;
 	((musb_core_config *)cfg)->addr = addr;
+	((musb_core_config *)cfg)->datatog_tx = tx_tog;
+	((musb_core_config *)cfg)->datatog_rx = rx_tog;
 }
 
 
@@ -372,7 +375,7 @@ musb_reset_device(void * cfg, hcd_speed * speed)
 	r = core->regs;
 
 	/* Set initial parameters */
-	musb_setup_device(core, HCD_DEFAULT_EP, HCD_DEFAULT_ADDR);
+	musb_setup_device(core, HCD_DEFAULT_EP, HCD_DEFAULT_ADDR, NULL, NULL);
 
 	/* Set EP and device address to be used in this command */
 	musb_set_state(core);
@@ -534,16 +537,16 @@ musb_rx_stage(void * cfg, hcd_datarequest * request)
 
 	/* Make controller reconfigure */
 	host_rxcsr = HCD_RD2(r, MUSB_REG_HOST_RXCSR);
-	if (MUSB_DATATOG_UNKNOWN == core->datatog_rx[core->ep]) {
-		/* Reset DATA toggle on first transfer */
-		HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_CLRDATATOG);
-		core->datatog_rx[core->ep] = MUSB_DATATOG_INIT;
-	}
+	HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_DATATOGWREN); /* Enable first */
 	HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_FLUSHFIFO);
 	HCD_WR2(r, MUSB_REG_HOST_RXCSR, host_rxcsr);
 
-	/* Request packet */
+	/* Set data toggle and start receiving */
 	host_rxcsr = HCD_RD2(r, MUSB_REG_HOST_RXCSR);
+	if (HCD_DATATOG_DATA0 == *(core->datatog_rx))
+		HCD_CLR(host_rxcsr, MUSB_VAL_HOST_RXCSR_DATATOG);
+	else
+		HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_DATATOG);
 	HCD_SET(host_rxcsr, MUSB_VAL_HOST_RXCSR_REQPKT);
 	HCD_WR2(r, MUSB_REG_HOST_RXCSR, host_rxcsr);
 }
@@ -630,19 +633,20 @@ musb_tx_stage(void * cfg, hcd_datarequest * request)
 	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_MODE);
 	HCD_CLR(host_txcsr, MUSB_VAL_HOST_TXCSR_ISO);
 	HCD_CLR(host_txcsr, MUSB_VAL_HOST_TXCSR_AUTOSET);
-	if (MUSB_DATATOG_UNKNOWN == core->datatog_tx[core->ep]) {
-		/* Reset DATA toggle on first transfer */
-		HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_CLRDATATOG);
-		core->datatog_tx[core->ep] = MUSB_DATATOG_INIT;
-	}
+	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_DATATOGWREN); /* Enable first */
+	/* TODO: May have no effect */
 	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_FLUSHFIFO);
 	HCD_WR2(r, MUSB_REG_HOST_TXCSR, host_txcsr);
 
 	/* Put data in FIFO */
 	musb_write_fifo(cfg, request->data, request->data_left, core->ep);
 
-	/* Request packet */
+	/* Set data toggle and start transmitting */
 	host_txcsr = HCD_RD2(r, MUSB_REG_HOST_TXCSR);
+	if (HCD_DATATOG_DATA0 == *(core->datatog_tx))
+		HCD_CLR(host_txcsr, MUSB_VAL_HOST_TXCSR_DATATOG);
+	else
+		HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_DATATOG);
 	HCD_SET(host_txcsr, MUSB_VAL_HOST_TXCSR_TXPKTRDY);
 	HCD_WR2(r, MUSB_REG_HOST_TXCSR, host_txcsr);
 }
@@ -790,6 +794,7 @@ musb_check_error(void * cfg, hcd_transfer xfer, hcd_reg1 ep, hcd_direction dir)
 	}
 	musb_error_case;
 
+	musb_core_config * core;
 	void * r;
 	hcd_reg2 host_csr;
 	musb_error_case error_case;
@@ -800,7 +805,8 @@ musb_check_error(void * cfg, hcd_transfer xfer, hcd_reg1 ep, hcd_direction dir)
 	USB_ASSERT(HCD_TRANSFER_ISOCHRONOUS != xfer,
 		"ISO transfer not supported");
 
-	r = ((musb_core_config *)cfg)->regs;
+	core = (musb_core_config *)cfg;
+	r = core->regs;
 
 	/* Set EP and device address to be used in this command */
 	musb_set_state((musb_core_config *)cfg);
@@ -859,6 +865,13 @@ musb_check_error(void * cfg, hcd_transfer xfer, hcd_reg1 ep, hcd_direction dir)
 		/* Get TX status register */
 		host_csr = HCD_RD2(r, MUSB_REG_HOST_TXCSR);
 
+		/* Check for completion */
+		if (!(host_csr & MUSB_VAL_HOST_TXCSR_TXPKTRDY)) {
+			/* ACK received update data toggle */
+			*(core->datatog_tx) ^= HCD_DATATOG_DATA1;
+			return EXIT_SUCCESS;
+		}
+
 		/* Check for common errors */
 		if (host_csr & MUSB_VAL_HOST_TXCSR_ERROR) {
 			USB_MSG("HOST_TXCSR ERROR: %04X", host_csr);
@@ -876,18 +889,30 @@ musb_check_error(void * cfg, hcd_transfer xfer, hcd_reg1 ep, hcd_direction dir)
 
 		if (host_csr & MUSB_VAL_HOST_TXCSR_NAK_TIMEOUT) {
 			USB_MSG("HOST_TXCSR NAK_TIMEOUT: %04X", host_csr);
+			/* Flush FIFO before clearing NAKTIMEOUT
+			 * to abort transfer */
+			HCD_SET(host_csr, MUSB_VAL_HOST_TXCSR_FLUSHFIFO);
+			HCD_WR2(r, MUSB_REG_HOST_TXCSR, host_csr);
+			host_csr = HCD_RD2(r, MUSB_REG_HOST_TXCSR);
 			HCD_CLR(host_csr, MUSB_VAL_HOST_TXCSR_NAK_TIMEOUT);
 			HCD_WR2(r, MUSB_REG_HOST_TXCSR, host_csr);
 			return EXIT_FAILURE;
 		}
 
-		return EXIT_SUCCESS;
+		USB_ASSERT(0, "Invalid state of HOST_TXCSR");
 	}
 
 	if (MUSB_IN_ERROR_CASE == error_case) {
 		/* Get RX status register */
 		host_csr = HCD_RD2(r, MUSB_REG_HOST_RXCSR);
 
+		/* Check for completion */
+		if (host_csr & MUSB_VAL_HOST_RXCSR_RXPKTRDY) {
+			/* ACK received update data toggle */
+			*(core->datatog_rx) ^= HCD_DATATOG_DATA1;
+			return EXIT_SUCCESS;
+		}
+
 		/* Check for common errors */
 		if (host_csr & MUSB_VAL_HOST_RXCSR_ERROR) {
 			USB_MSG("HOST_RXCSR ERROR: %04X", host_csr);
@@ -905,12 +930,16 @@ musb_check_error(void * cfg, hcd_transfer xfer, hcd_reg1 ep, hcd_direction dir)
 
 		if (host_csr & MUSB_VAL_HOST_RXCSR_NAKTIMEOUT) {
 			USB_MSG("HOST_RXCSR NAK_TIMEOUT: %04X", host_csr);
+			/* Clear REQPKT before NAKTIMEOUT to abort transfer */
+			HCD_CLR(host_csr, MUSB_VAL_HOST_RXCSR_REQPKT);
+			HCD_WR2(r, MUSB_REG_HOST_RXCSR, host_csr);
+			host_csr = HCD_RD2(r, MUSB_REG_HOST_RXCSR);
 			HCD_CLR(host_csr, MUSB_VAL_HOST_RXCSR_NAKTIMEOUT);
 			HCD_WR2(r, MUSB_REG_HOST_RXCSR, host_csr);
 			return EXIT_FAILURE;
 		}
 
-		return EXIT_SUCCESS;
+		USB_ASSERT(0, "Invalid state of HOST_RXCSR");
 	}
 
 	USB_MSG("Invalid USB transfer error check: 0x%X, 0x%X, 0x%X",
diff --git a/minix/drivers/usb/usbd/hcd/musb/musb_core.h b/minix/drivers/usb/usbd/hcd/musb/musb_core.h
index 09bd174b6..f8f2987e7 100644
--- a/minix/drivers/usb/usbd/hcd/musb/musb_core.h
+++ b/minix/drivers/usb/usbd/hcd/musb/musb_core.h
@@ -11,15 +11,6 @@
 /*===========================================================================*
  *    Types and constants                                                    *
  *===========================================================================*/
-/* Holds info on DATA toggle (DATA0/DATA1) initialization,
- * required by bulk transfers */
-typedef enum {
-
-	MUSB_DATATOG_UNKNOWN = 0,	/* Default with memset 0 */
-	MUSB_DATATOG_INIT
-}
-musb_datatog;
-
 /* Structure to hold Mentor USB core configuration
  * May be more than one on a single chip
  * Should be initialized by MUSB's variant specific code (like AM335x) */
@@ -28,8 +19,8 @@ typedef struct {
 	void * regs;	/* Points to beginning of memory mapped registers */
 	hcd_reg1 ep;	/* Currently used endpoint */
 	hcd_reg1 addr;	/* Currently used address */
-	musb_datatog datatog_tx[HCD_TOTAL_EP];
-	musb_datatog datatog_rx[HCD_TOTAL_EP];
+	hcd_datatog * datatog_tx; /* Should point at currently used TX toggle */
+	hcd_datatog * datatog_rx; /* Should point at currently used RX toggle */
 }
 musb_core_config;
 
@@ -43,7 +34,8 @@ void musb_core_stop(void *);
 
 
 /* For HCD interface */
-void musb_setup_device(void *, hcd_reg1, hcd_reg1);
+void musb_setup_device(void *, hcd_reg1, hcd_reg1,
+			hcd_datatog *, hcd_datatog *);
 int musb_reset_device(void *, hcd_speed *);
 void musb_setup_stage(void *, hcd_ctrlrequest *);
 void musb_rx_stage(void *, hcd_datarequest *);
diff --git a/minix/drivers/usb/usbd/include/usbd/hcd_common.h b/minix/drivers/usb/usbd/include/usbd/hcd_common.h
index 5feeef6a1..d89c449aa 100644
--- a/minix/drivers/usb/usbd/include/usbd/hcd_common.h
+++ b/minix/drivers/usb/usbd/include/usbd/hcd_common.h
@@ -154,6 +154,14 @@ typedef enum {
 }
 hcd_speed;
 
+/* Possible data toggle values (at least for bulk transfer) */
+typedef enum {
+
+	HCD_DATATOG_DATA0 = 0,
+	HCD_DATATOG_DATA1 = 1
+}
+hcd_datatog;
+
 
 /*===========================================================================*
  *    HCD threading/device/URB types                                         *
@@ -166,12 +174,15 @@ typedef struct usb_ctrlrequest		hcd_ctrlrequest;
 
 /* Largest value that can be transfered by this driver at a time
  * see MAXPAYLOAD in TXMAXP/RXMAXP */
-#define MAX_WTOTALLENGTH 		1024
+#define MAX_WTOTALLENGTH 		1024u
 
 /* TODO: This has corresponding redefinition in hub driver */
 /* Limit of child devices for each parent */
 #define HCD_CHILDREN			8u
 
+/* Total number of endpoints available in USB 2.0 */
+#define HCD_TOTAL_EP			16u
+
 /* Forward declarations */
 typedef struct hcd_datarequest		hcd_datarequest;
 typedef struct hcd_urb			hcd_urb;
@@ -219,6 +230,7 @@ struct hcd_device_state {
 
 	hcd_device_state * parent;		/* In case of hub attachment */
 	hcd_device_state * child[HCD_CHILDREN];	/* In case of being hub */
+	hcd_device_state * _next;		/* To allow device lists */
 	hcd_driver_state * driver;		/* Specific HCD driver object */
 	hcd_thread * thread;
 	hcd_lock * lock;
@@ -234,6 +246,8 @@ struct hcd_device_state {
 	hcd_state state;
 	hcd_reg1 reserved_address;
 	hcd_reg1 current_address;
+	hcd_datatog ep_tx_tog[HCD_TOTAL_EP];
+	hcd_datatog ep_rx_tog[HCD_TOTAL_EP];
 
 	/*
 	 * Control transfer's local data:
@@ -266,8 +280,9 @@ struct hcd_device_state {
 #define HCD_LAST_ADDR			0x7Fu
 #define HCD_TOTAL_ADDR			0x80u
 #define HCD_LAST_EP			0x0Fu
-#define HCD_TOTAL_EP			0x10u
 #define HCD_UNUSED_VAL			0xFFu	/* When number not needed */
+#define HCD_DEFAULT_NAKLIMIT		0x10u
+
 
 /* Legal interval values */
 #define HCD_LOWEST_INTERVAL		0x00u
@@ -332,6 +347,8 @@ void hcd_device_continue(hcd_device_state *, hcd_event, hcd_reg1);
 /* Allocation/deallocation of device structures */
 hcd_device_state * hcd_new_device(void);
 void hcd_delete_device(hcd_device_state *);
+void hcd_dump_devices(void);
+int hcd_check_device(hcd_device_state *);
 
 
 /*===========================================================================*
diff --git a/minix/drivers/usb/usbd/include/usbd/hcd_interface.h b/minix/drivers/usb/usbd/include/usbd/hcd_interface.h
index ed7486808..be225aded 100644
--- a/minix/drivers/usb/usbd/include/usbd/hcd_interface.h
+++ b/minix/drivers/usb/usbd/include/usbd/hcd_interface.h
@@ -32,7 +32,8 @@ hcd_addr_state;
  *===========================================================================*/
 struct hcd_driver_state {
 	/* Standard USB controller procedures */
-	void	(*setup_device)		(void *, hcd_reg1, hcd_reg1);
+	void	(*setup_device)		(void *, hcd_reg1, hcd_reg1,
+					hcd_datatog *, hcd_datatog *);
 	int	(*reset_device)		(void *, hcd_speed *);
 	void	(*setup_stage)		(void *, hcd_ctrlrequest *);
 	void	(*rx_stage)		(void *, hcd_datarequest *);
diff --git a/minix/drivers/usb/usbd/include/usbd/usbd_common.h b/minix/drivers/usb/usbd/include/usbd/usbd_common.h
index 2cc6aa589..5d1bfc074 100644
--- a/minix/drivers/usb/usbd/include/usbd/usbd_common.h
+++ b/minix/drivers/usb/usbd/include/usbd/usbd_common.h
@@ -16,16 +16,23 @@
 #define DEBUG
 #endif
 
+/* This allows us to analyze thread context in
+ * consecutive function calls (DEBUG_DUMP) */
+#include <ddekit/thread.h>
+
+/* Represents current thread's name string */
+#define HCD_THREAD_NAME ddekit_thread_get_name(ddekit_thread_myself())
+
 
 /*===========================================================================*
  *    Standard output message                                                *
  *===========================================================================*/
 #define USB_MSG(fmt, ...)						\
-		do {							\
-			printf("USBD: ");				\
-			printf(fmt, ##__VA_ARGS__);			\
-			printf("\n");					\
-		} while(0)
+	do {								\
+		printf("USBD: ");					\
+		printf(fmt, ##__VA_ARGS__);				\
+		printf("\n");						\
+	} while(0)
 
 
 /*===========================================================================*
@@ -33,16 +40,16 @@
  *===========================================================================*/
 #ifdef DEBUG
 #define DEBUG_DUMP							\
-		do {							\
-			printf("USBD (DEBUG %s)\n", __func__);		\
-		} while(0)
+	do {								\
+		printf("USBD: [%s -> %s]\n", HCD_THREAD_NAME, __func__);\
+	} while(0)
 
 #define USB_DBG(fmt, ...)						\
-		do {							\
-			printf("USBD (DEBUG %s): ", __func__);		\
-			printf(fmt, ##__VA_ARGS__);			\
-			printf("\n");					\
-		} while(0)
+	do {								\
+		printf("USBD: [%s -> %s] ", HCD_THREAD_NAME, __func__);	\
+		printf(fmt, ##__VA_ARGS__);				\
+		printf("\n");						\
+	} while(0)
 
 #else
 #define DEBUG_DUMP		((void)0)
@@ -54,13 +61,14 @@
  *    Assert for USB code                                                    *
  *===========================================================================*/
 #define USB_ASSERT(cond, otherwise)					\
-		do {							\
-			if(!(cond)) {					\
-				USB_MSG("ASSERTION ERROR (%s:%d) - "	\
-					otherwise, __func__, __LINE__);	\
-				exit(EXIT_FAILURE);			\
-			}						\
-		} while(0)
+	do {								\
+		if (!(cond)) {						\
+			USB_MSG("ASSERTION ERROR (%s -> %s:%d) - "	\
+				otherwise, HCD_THREAD_NAME,		\
+				__func__, __LINE__);			\
+			exit(EXIT_FAILURE);				\
+		}							\
+	} while(0)
 
 
 #endif /* !_USBD_COMMON_H_ */
diff --git a/minix/lib/libddekit/src/irq.c b/minix/lib/libddekit/src/irq.c
index b60981aeb..da0a949ea 100644
--- a/minix/lib/libddekit/src/irq.c
+++ b/minix/lib/libddekit/src/irq.c
@@ -288,13 +288,13 @@ void _ddekit_interrupt_trigger(int irq_id)
 	irq_s = find_by_irq_id(irq_id);
 
 	if (irq_s)	{
-		DDEBUG_MSG_VERBOSE("Triggering IRQ %d", irq);
+		DDEBUG_MSG_VERBOSE("Triggering IRQ %d", irq_s->irq);
 		ddekit_sem_up(irq_s->sem);
 		if (0 != (err_code = sys_irqenable(&irq_s->irq_hook)))
 			ddekit_panic("Failed to enable interrupt "
 					"(ERROR %d)", err_code);
 	} else {
-		DDEBUG_MSG_WARN("no handler for IRQ %d", irq);
+		DDEBUG_MSG_WARN("no handler for IRQ %d", irq_s->irq);
 	}
 }