Index: uspace/drv/bus/usb/xhci/commands.c
===================================================================
--- uspace/drv/bus/usb/xhci/commands.c	(revision c058a388719cf7bf6599a0c394ad933ba0702271)
+++ uspace/drv/bus/usb/xhci/commands.c	(revision 110d7951a92abed55ef5aab651f32aa7733ab100)
@@ -44,4 +44,65 @@
 #include "hw_struct/trb.h"
 
+int xhci_init_commands(xhci_hc_t *hc)
+{
+	assert(hc);
+
+	list_initialize(&hc->commands);
+	return EOK;
+}
+
+int xhci_wait_for_command(xhci_hc_t *hc, xhci_cmd_t *cmd, uint32_t timeout)
+{
+	uint32_t time = 0;
+	while (!cmd->completed) {
+		async_usleep(1000);
+		time += 1000;
+
+		if (time > timeout)
+			return ETIMEOUT;
+	}
+
+	return EOK;
+}
+
+xhci_cmd_t *xhci_alloc_command(void)
+{
+	xhci_cmd_t *cmd = malloc32(sizeof(xhci_cmd_t));
+	memset(cmd, 0, sizeof(xhci_cmd_t));
+
+	link_initialize(&cmd->link);
+
+	/**
+	 * Internal functions will set this to false, other are implicit
+	 * owners unless they overwrite this field.
+	 * TODO: Is this wise?
+	 */
+	cmd->has_owner = true;
+
+	return cmd;
+}
+
+void xhci_free_command(xhci_cmd_t *cmd)
+{
+	// TODO: If we decide to copy trb, free it here.
+	if (cmd->ictx)
+		free32(cmd->ictx);
+
+	free32(cmd);
+}
+
+static inline xhci_cmd_t *get_next_command(xhci_hc_t *hc)
+{
+	link_t *cmd_link = list_first(&hc->commands);
+
+	if (cmd_link != NULL) {
+		list_remove(cmd_link);
+		
+		return list_get_instance(cmd_link, xhci_cmd_t, link);
+	}
+
+	return NULL;
+}
+
 static inline int ring_doorbell(xhci_hc_t *hc, unsigned doorbell, unsigned target)
 {
@@ -65,4 +126,20 @@
 
 	return EOK;
+}
+
+static inline xhci_cmd_t *add_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{ 
+	if (cmd == NULL) {
+		cmd = xhci_alloc_command();
+		if (cmd == NULL)
+			return cmd;
+
+		cmd->has_owner = false;
+	}
+
+	list_append(&cmd->link, &hc->commands);
+	cmd->trb = hc->command_ring.enqueue_trb;
+
+	return cmd;
 }
 
@@ -118,5 +195,5 @@
 }
 
-int xhci_send_no_op_command(xhci_hc_t *hc)
+int xhci_send_no_op_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
 {
 	assert(hc);
@@ -127,8 +204,10 @@
 	trb.control = host2xhci(32, XHCI_TRB_TYPE_NO_OP_CMD << 10);
 
-	return enqueue_trb(hc, &trb, 0, 0);
-}
-
-int xhci_send_enable_slot_command(xhci_hc_t *hc)
+	cmd = add_cmd(hc, cmd);
+
+	return enqueue_trb(hc, &trb, 0, 0);
+}
+
+int xhci_send_enable_slot_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
 {
 	assert(hc);
@@ -141,10 +220,13 @@
 	trb.control |= host2xhci(32, hc->command_ring.pcs);
 
-	return enqueue_trb(hc, &trb, 0, 0);
-}
-
-int xhci_send_disable_slot_command(xhci_hc_t *hc, uint32_t slot_id)
-{
-	assert(hc);
+	cmd = add_cmd(hc, cmd);
+
+	return enqueue_trb(hc, &trb, 0, 0);
+}
+
+int xhci_send_disable_slot_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
 
 	xhci_trb_t trb;
@@ -153,14 +235,16 @@
 	trb.control = host2xhci(32, XHCI_TRB_TYPE_DISABLE_SLOT_CMD << 10);
 	trb.control |= host2xhci(32, hc->command_ring.pcs);
-	trb.control |= host2xhci(32, slot_id << 24);
-
-	return enqueue_trb(hc, &trb, 0, 0);
-}
-
-int xhci_send_address_device_command(xhci_hc_t *hc, uint32_t slot_id,
-				     xhci_input_ctx_t *ictx)
-{
-	assert(hc);
-	assert(ictx);
+	trb.control |= host2xhci(32, cmd->slot_id << 24);
+
+	add_cmd(hc, cmd);
+
+	return enqueue_trb(hc, &trb, 0, 0);
+}
+
+int xhci_send_address_device_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+	assert(cmd->ictx);
 
 	/**
@@ -173,5 +257,5 @@
 	memset(&trb, 0, sizeof(trb));
 
-	uint64_t phys_addr = (uint64_t) addr_to_phys(ictx);
+	uint64_t phys_addr = (uint64_t) addr_to_phys(cmd->ictx);
 	trb.parameter = host2xhci(32, phys_addr & (~0xF));
 
@@ -185,33 +269,37 @@
 	trb.control = host2xhci(32, XHCI_TRB_TYPE_ADDRESS_DEVICE_CMD << 10);
 	trb.control |= host2xhci(32, hc->command_ring.pcs);
-	trb.control |= host2xhci(32, slot_id << 24);
-
-	return enqueue_trb(hc, &trb, 0, 0);
-}
-
-int xhci_send_configure_endpoint_command(xhci_hc_t *hc, uint32_t slot_id,
-					 xhci_input_ctx_t *ictx)
-{
-	assert(hc);
-	assert(ictx);
-
-	xhci_trb_t trb;
-	memset(&trb, 0, sizeof(trb));
-
-	uint64_t phys_addr = (uint64_t) addr_to_phys(ictx);
+	trb.control |= host2xhci(32, cmd->slot_id << 24);
+
+	cmd = add_cmd(hc, cmd);
+
+	return enqueue_trb(hc, &trb, 0, 0);
+}
+
+int xhci_send_configure_endpoint_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+	assert(cmd->ictx);
+
+	xhci_trb_t trb;
+	memset(&trb, 0, sizeof(trb));
+
+	uint64_t phys_addr = (uint64_t) addr_to_phys(cmd->ictx);
 	trb.parameter = host2xhci(32, phys_addr & (~0xF));
 
 	trb.control = host2xhci(32, XHCI_TRB_TYPE_CONFIGURE_ENDPOINT_CMD << 10);
 	trb.control |= host2xhci(32, hc->command_ring.pcs);
-	trb.control |= host2xhci(32, slot_id << 24);
-
-	return enqueue_trb(hc, &trb, 0, 0);
-}
-
-int xhci_send_evaluate_context_command(xhci_hc_t *hc, uint32_t slot_id,
-				       xhci_input_ctx_t *ictx)
-{
-	assert(hc);
-	assert(ictx);
+	trb.control |= host2xhci(32, cmd->slot_id << 24);
+
+	cmd = add_cmd(hc, cmd);
+
+	return enqueue_trb(hc, &trb, 0, 0);
+}
+
+int xhci_send_evaluate_context_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+	assert(cmd->ictx);
 
 	/**
@@ -224,17 +312,20 @@
 	memset(&trb, 0, sizeof(trb));
 
-	uint64_t phys_addr = (uint64_t) addr_to_phys(ictx);
+	uint64_t phys_addr = (uint64_t) addr_to_phys(cmd->ictx);
 	trb.parameter = host2xhci(32, phys_addr & (~0xF));
 
 	trb.control = host2xhci(32, XHCI_TRB_TYPE_EVALUATE_CONTEXT_CMD << 10);
 	trb.control |= host2xhci(32, hc->command_ring.pcs);
-	trb.control |= host2xhci(32, slot_id << 24);
-
-	return enqueue_trb(hc, &trb, 0, 0);
-}
-
-int xhci_send_reset_endpoint_command(xhci_hc_t *hc, uint32_t slot_id, uint32_t ep_id, uint8_t tcs)
-{
-	assert(hc);
+	trb.control |= host2xhci(32, cmd->slot_id << 24);
+
+	cmd = add_cmd(hc, cmd);
+
+	return enqueue_trb(hc, &trb, 0, 0);
+}
+
+int xhci_send_reset_endpoint_command(xhci_hc_t *hc, xhci_cmd_t *cmd, uint32_t ep_id, uint8_t tcs)
+{
+	assert(hc);
+	assert(cmd);
 
 	/**
@@ -249,12 +340,13 @@
 	trb.control |= host2xhci(32, (tcs & 0x1) << 9);
 	trb.control |= host2xhci(32, (ep_id & 0x5) << 16);
-	trb.control |= host2xhci(32, slot_id << 24);
-
-	return enqueue_trb(hc, &trb, 0, 0);
-}
-
-int xhci_send_stop_endpoint_command(xhci_hc_t *hc, uint32_t slot_id, uint32_t ep_id, uint8_t susp)
-{
-	assert(hc);
+	trb.control |= host2xhci(32, cmd->slot_id << 24);
+
+	return enqueue_trb(hc, &trb, 0, 0);
+}
+
+int xhci_send_stop_endpoint_command(xhci_hc_t *hc, xhci_cmd_t *cmd, uint32_t ep_id, uint8_t susp)
+{
+	assert(hc);
+	assert(cmd);
 
 	xhci_trb_t trb;
@@ -265,12 +357,15 @@
 	trb.control |= host2xhci(32, (ep_id & 0x5) << 16);
 	trb.control |= host2xhci(32, (susp & 0x1) << 23);
-	trb.control |= host2xhci(32, slot_id << 24);
-
-	return enqueue_trb(hc, &trb, 0, 0);
-}
-
-int xhci_send_reset_device_command(xhci_hc_t *hc, uint32_t slot_id)
-{
-	assert(hc);
+	trb.control |= host2xhci(32, cmd->slot_id << 24);
+
+	cmd = add_cmd(hc, cmd);
+
+	return enqueue_trb(hc, &trb, 0, 0);
+}
+
+int xhci_send_reset_device_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
 
 	xhci_trb_t trb;
@@ -279,5 +374,5 @@
 	trb.control = host2xhci(32, XHCI_TRB_TYPE_RESET_DEVICE_CMD << 10);
 	trb.control |= host2xhci(32, hc->command_ring.pcs);
-	trb.control |= host2xhci(32, slot_id << 24);
+	trb.control |= host2xhci(32, cmd->slot_id << 24);
 
 	return enqueue_trb(hc, &trb, 0, 0);
@@ -286,54 +381,78 @@
 int xhci_handle_command_completion(xhci_hc_t *hc, xhci_trb_t *trb)
 {
+	// TODO: Update dequeue ptrs.
+	// TODO: Possibly clone command trb, as it may get overwritten before
+	//       it is processed (if somebody polls the command completion).
 	assert(hc);
 	assert(trb);
 
 	usb_log_debug("HC(%p) Command completed.", hc);
-	xhci_dump_trb(trb);
 
 	int code;
 	uint32_t slot_id;
-	xhci_trb_t *command;
+	xhci_cmd_t *command;
+	xhci_trb_t *command_trb;
 
 	code = XHCI_DWORD_EXTRACT(trb->status, 31, 24);
-	command = (xhci_trb_t *) XHCI_QWORD_EXTRACT(trb->parameter, 63, 4);
 	slot_id = XHCI_DWORD_EXTRACT(trb->control, 31, 24);
 	(void) slot_id;
 
-	if (TRB_TYPE(*command) != XHCI_TRB_TYPE_NO_OP_CMD) {
+	command = get_next_command(hc);
+	assert(command);
+
+	command_trb = command->trb;
+
+	code = XHCI_DWORD_EXTRACT(trb->status, 31, 24);
+	command->status = code;
+
+	slot_id = XHCI_DWORD_EXTRACT(trb->control, 31, 24);
+	command->slot_id = slot_id;
+
+	usb_log_debug2("Completed command trb:");
+	xhci_dump_trb(command_trb);
+	if (TRB_TYPE(*command_trb) != XHCI_TRB_TYPE_NO_OP_CMD) {
 		if (code != XHCI_TRBC_SUCCESS) {
 			report_error(code);
-			xhci_dump_trb(command);
+			xhci_dump_trb(command_trb);
 		}
 	}
 
-	switch (TRB_TYPE(*command)) {
+	switch (TRB_TYPE(*command_trb)) {
 	case XHCI_TRB_TYPE_NO_OP_CMD:
 		assert(code = XHCI_TRBC_TRB_ERROR);
-		return EOK;
+		break;
 	case XHCI_TRB_TYPE_ENABLE_SLOT_CMD:
-		return EOK;
+		break;
 	case XHCI_TRB_TYPE_DISABLE_SLOT_CMD:
-		return EOK;
+		break;
 	case XHCI_TRB_TYPE_ADDRESS_DEVICE_CMD:
-		return EOK;
+		break;
 	case XHCI_TRB_TYPE_CONFIGURE_ENDPOINT_CMD:
-		return EOK;
+		break;
 	case XHCI_TRB_TYPE_EVALUATE_CONTEXT_CMD:
-		return EOK;
+		break;
 	case XHCI_TRB_TYPE_RESET_ENDPOINT_CMD:
-		return EOK;
+		break;
 	case XHCI_TRB_TYPE_STOP_ENDPOINT_CMD:
 		// Note: If the endpoint was in the middle of a transfer, then the xHC
 		//       will add a Transfer TRB before the Event TRB, research that and
 		//       handle it appropriately!
-		return EOK;
+		break;
 	case XHCI_TRB_TYPE_RESET_DEVICE_CMD:
-		return EOK;
+		break;
 	default:
 		usb_log_debug2("Unsupported command trb.");
-		xhci_dump_trb(command);
+		xhci_dump_trb(command_trb);
+
+		command->completed = true;
 		return ENAK;
 	}
+
+	command->completed = true;
+
+	if (!command->has_owner)
+		xhci_free_command(command);
+
+	return EOK;
 }
 
