Index: uspace/drv/bus/usb/xhci/commands.c
===================================================================
--- uspace/drv/bus/usb/xhci/commands.c	(revision 0206d350511d5ad74999ecd3b4f1abcea0463ba8)
+++ uspace/drv/bus/usb/xhci/commands.c	(revision c3d926f3ce91a6e5d315f2628cac95ccdcbebeff)
@@ -63,4 +63,6 @@
 #define TRB_GET_PHYS(trb) (XHCI_QWORD_EXTRACT((trb).parameter, 63, 4) << 4)
 
+/* Control functions */
+
 int xhci_init_commands(xhci_hc_t *hc)
 {
@@ -80,50 +82,32 @@
 }
 
-int xhci_cmd_wait(xhci_cmd_t *cmd, suseconds_t timeout)
-{
-	int rv = EOK;
-
-	fibril_mutex_lock(&cmd->completed_mtx);
-	while (!cmd->completed) {
-		usb_log_debug2("Waiting for event completion: going to sleep.");
-		rv = fibril_condvar_wait_timeout(&cmd->completed_cv, &cmd->completed_mtx, timeout);
-
-		usb_log_debug2("Waiting for event completion: woken: %s", str_error(rv));
-		if (rv == ETIMEOUT)
-			break;
-	}
-	fibril_mutex_unlock(&cmd->completed_mtx);
-
-	return rv;
-}
-
-xhci_cmd_t *xhci_cmd_alloc(void)
-{
-	xhci_cmd_t *cmd = malloc(sizeof(xhci_cmd_t));
-	xhci_cmd_init(cmd);
-
-	usb_log_debug2("Allocating cmd on the heap. Don't forget to deallocate it!");
-	return cmd;
-}
-
-void xhci_cmd_init(xhci_cmd_t *cmd)
+void xhci_cmd_init(xhci_cmd_t *cmd, xhci_cmd_type_t type)
 {
 	memset(cmd, 0, sizeof(*cmd));
 
-	link_initialize(&cmd->link);
-
-	fibril_mutex_initialize(&cmd->completed_mtx);
-	fibril_condvar_initialize(&cmd->completed_cv);
+	link_initialize(&cmd->_header.link);
+
+	fibril_mutex_initialize(&cmd->_header.completed_mtx);
+	fibril_condvar_initialize(&cmd->_header.completed_cv);
+
+	cmd->_header.cmd = type;
+	cmd->_header.timeout = XHCI_DEFAULT_TIMEOUT;
 }
 
 void xhci_cmd_fini(xhci_cmd_t *cmd)
 {
-	list_remove(&cmd->link);
-}
-
-void xhci_cmd_free(xhci_cmd_t *cmd)
-{
-	xhci_cmd_fini(cmd);
-	free(cmd);
+	list_remove(&cmd->_header.link);
+
+	if (cmd->input_ctx) {
+		free32(cmd->input_ctx);
+	};
+
+	if (cmd->bandwidth_ctx) {
+		free32(cmd->bandwidth_ctx);
+	}
+
+	if (cmd->_header.async) {
+		free(cmd);
+	}
 }
 
@@ -135,7 +119,7 @@
 
 	while (cmd_link != NULL) {
-		xhci_cmd_t *cmd = list_get_instance(cmd_link, xhci_cmd_t, link);
-
-		if (cmd->trb_phys == phys)
+		xhci_cmd_t *cmd = list_get_instance(cmd_link, xhci_cmd_t, _header.link);
+
+		if (cmd->_header.trb_phys == phys)
 			break;
 
@@ -147,5 +131,5 @@
 		fibril_mutex_unlock(&hc->commands_mtx);
 
-		return list_get_instance(cmd_link, xhci_cmd_t, link);
+		return list_get_instance(cmd_link, xhci_cmd_t, _header.link);
 	}
 
@@ -160,12 +144,12 @@
 
 	fibril_mutex_lock(&hc->commands_mtx);
-	list_append(&cmd->link, &hc->commands);
+	list_append(&cmd->_header.link, &hc->commands);
 	fibril_mutex_unlock(&hc->commands_mtx);
 
-	xhci_trb_ring_enqueue(&hc->command_ring, &cmd->trb, &cmd->trb_phys);
+	xhci_trb_ring_enqueue(&hc->command_ring, &cmd->_header.trb, &cmd->_header.trb_phys);
 	hc_ring_doorbell(hc, doorbell, target);
 
 	usb_log_debug2("HC(%p): Sent command:", hc);
-	xhci_dump_trb(&cmd->trb);
+	xhci_dump_trb(&cmd->_header.trb);
 
 	return EOK;
@@ -253,45 +237,126 @@
 }
 
-int xhci_send_no_op_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
-{
-	assert(hc);
-
-	xhci_trb_clean(&cmd->trb);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_NO_OP_CMD);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_send_enable_slot_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
-{
-	assert(hc);
-
-	xhci_trb_clean(&cmd->trb);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_ENABLE_SLOT_CMD);
-	cmd->trb.control |= host2xhci(32, XHCI_REG_RD(hc->xecp, XHCI_EC_SP_SLOT_TYPE) << 16);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_send_disable_slot_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
-{
-	assert(hc);
-	assert(cmd);
-
-	xhci_trb_clean(&cmd->trb);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_DISABLE_SLOT_CMD);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_send_address_device_command(xhci_hc_t *hc, xhci_cmd_t *cmd, xhci_input_ctx_t *ictx)
-{
-	assert(hc);
-	assert(cmd);
-	assert(ictx);
+int xhci_handle_command_completion(xhci_hc_t *hc, xhci_trb_t *trb)
+{
+	// TODO: Update dequeue ptrs.
+	assert(hc);
+	assert(trb);
+
+	usb_log_debug2("HC(%p) Command completed.", hc);
+
+	int code;
+	uint64_t phys;
+	xhci_cmd_t *command;
+
+	code = TRB_GET_CODE(*trb);
+	phys = TRB_GET_PHYS(*trb);;
+	command = get_command(hc, phys);
+	if (command == NULL) {
+		// TODO: STOP & ABORT may not have command structs in the list!
+		usb_log_warning("No command struct for this completion event found.");
+
+		if (code != XHCI_TRBC_SUCCESS)
+			report_error(code);
+
+		return EOK;
+	}
+
+	command->status = code;
+	command->slot_id = TRB_GET_SLOT(*trb);
+
+	usb_log_debug2("Completed command trb: %s", xhci_trb_str_type(TRB_TYPE(command->_header.trb)));
+	if (TRB_TYPE(command->_header.trb) != XHCI_TRB_TYPE_NO_OP_CMD) {
+		if (code != XHCI_TRBC_SUCCESS) {
+			report_error(code);
+			xhci_dump_trb(&command->_header.trb);
+		}
+	}
+
+	switch (TRB_TYPE(command->_header.trb)) {
+	case XHCI_TRB_TYPE_NO_OP_CMD:
+		assert(code == XHCI_TRBC_TRB_ERROR);
+		break;
+	case XHCI_TRB_TYPE_ENABLE_SLOT_CMD:
+		break;
+	case XHCI_TRB_TYPE_DISABLE_SLOT_CMD:
+		break;
+	case XHCI_TRB_TYPE_ADDRESS_DEVICE_CMD:
+		break;
+	case XHCI_TRB_TYPE_CONFIGURE_ENDPOINT_CMD:
+		break;
+	case XHCI_TRB_TYPE_EVALUATE_CONTEXT_CMD:
+		break;
+	case XHCI_TRB_TYPE_RESET_ENDPOINT_CMD:
+		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!
+		break;
+	case XHCI_TRB_TYPE_RESET_DEVICE_CMD:
+		break;
+	default:
+		usb_log_debug2("Unsupported command trb: %s", xhci_trb_str_type(TRB_TYPE(command->_header.trb)));
+
+		command->_header.completed = true;
+		return ENAK;
+	}
+
+	fibril_mutex_lock(&command->_header.completed_mtx);
+	command->_header.completed = true;
+	fibril_condvar_broadcast(&command->_header.completed_cv);
+	fibril_mutex_unlock(&command->_header.completed_mtx);
+
+	if (command->_header.async) {
+		/* Free the command and other DS upon completion. */
+		xhci_cmd_fini(command);
+	}
+
+	return EOK;
+}
+
+/* Command-issuing functions */
+
+static int no_op_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+
+	xhci_trb_clean(&cmd->_header.trb);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_NO_OP_CMD);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int enable_slot_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+
+	xhci_trb_clean(&cmd->_header.trb);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_ENABLE_SLOT_CMD);
+	cmd->_header.trb.control |= host2xhci(32, XHCI_REG_RD(hc->xecp, XHCI_EC_SP_SLOT_TYPE) << 16);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int disable_slot_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+
+	xhci_trb_clean(&cmd->_header.trb);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_DISABLE_SLOT_CMD);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int address_device_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+	assert(cmd->input_ctx);
 
 	/**
@@ -302,8 +367,8 @@
 	 */
 
-	xhci_trb_clean(&cmd->trb);
-
-	uint64_t phys_addr = (uint64_t) addr_to_phys(ictx);
-	TRB_SET_ICTX(cmd->trb, phys_addr);
+	xhci_trb_clean(&cmd->_header.trb);
+
+	uint64_t phys_addr = (uint64_t) addr_to_phys(cmd->input_ctx);
+	TRB_SET_ICTX(cmd->_header.trb, phys_addr);
 
 	/**
@@ -314,37 +379,37 @@
 	 *       that require their device descriptor to be read before such request.
 	 */
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_ADDRESS_DEVICE_CMD);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_send_configure_endpoint_command(xhci_hc_t *hc, xhci_cmd_t *cmd, xhci_input_ctx_t *ictx)
-{
-	assert(hc);
-	assert(cmd);
-
-	xhci_trb_clean(&cmd->trb);
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_ADDRESS_DEVICE_CMD);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int configure_endpoint_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+
+	xhci_trb_clean(&cmd->_header.trb);
 
 	if (!cmd->deconfigure) {
 		/* If the DC flag is on, input context is not evaluated. */
-		assert(ictx);
-
-		uint64_t phys_addr = (uint64_t) addr_to_phys(ictx);
-		TRB_SET_ICTX(cmd->trb, phys_addr);
-	}
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_CONFIGURE_ENDPOINT_CMD);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-	TRB_SET_DC(cmd->trb, cmd->deconfigure);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_send_evaluate_context_command(xhci_hc_t *hc, xhci_cmd_t *cmd, xhci_input_ctx_t *ictx)
-{
-	assert(hc);
-	assert(cmd);
-	assert(ictx);
+		assert(cmd->input_ctx);
+
+		uint64_t phys_addr = (uint64_t) addr_to_phys(cmd->input_ctx);
+		TRB_SET_ICTX(cmd->_header.trb, phys_addr);
+	}
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_CONFIGURE_ENDPOINT_CMD);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+	TRB_SET_DC(cmd->_header.trb, cmd->deconfigure);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int evaluate_context_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+	assert(cmd->input_ctx);
 
 	/**
@@ -354,16 +419,16 @@
 	 *       Refer to sections 6.2.2.3 and 6.3.3.3 for further info.
 	 */
-	xhci_trb_clean(&cmd->trb);
-
-	uint64_t phys_addr = (uint64_t) addr_to_phys(ictx);
-	TRB_SET_ICTX(cmd->trb, phys_addr);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_EVALUATE_CONTEXT_CMD);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_send_reset_endpoint_command(xhci_hc_t *hc, xhci_cmd_t *cmd, uint32_t ep_id, uint8_t tcs)
+	xhci_trb_clean(&cmd->_header.trb);
+
+	uint64_t phys_addr = (uint64_t) addr_to_phys(cmd->input_ctx);
+	TRB_SET_ICTX(cmd->_header.trb, phys_addr);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_EVALUATE_CONTEXT_CMD);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int reset_endpoint_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
 {
 	assert(hc);
@@ -374,43 +439,41 @@
 	 *       information about this flag.
 	 */
-	xhci_trb_clean(&cmd->trb);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_RESET_ENDPOINT_CMD);
-	TRB_SET_TCS(cmd->trb, tcs);
-	TRB_SET_EP(cmd->trb, ep_id);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-
-	return enqueue_command(hc, cmd, 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_clean(&cmd->trb);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_STOP_ENDPOINT_CMD);
-	TRB_SET_EP(cmd->trb, ep_id);
-	TRB_SET_SUSP(cmd->trb, susp);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_send_set_dequeue_ptr_command(xhci_hc_t *hc, xhci_cmd_t *cmd,
-				      uintptr_t dequeue_ptr, uint16_t stream_id,
-				      uint32_t ep_id)
-{
-	assert(hc);
-	assert(cmd);
-
-	xhci_trb_clean(&cmd->trb);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_SET_TR_DEQUEUE_POINTER_CMD);
-	TRB_SET_EP(cmd->trb, ep_id);
-	TRB_SET_STREAM(cmd->trb, stream_id);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-	TRB_SET_DEQUEUE_PTR(cmd->trb, dequeue_ptr);
+	xhci_trb_clean(&cmd->_header.trb);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_RESET_ENDPOINT_CMD);
+	TRB_SET_TCS(cmd->_header.trb, cmd->tcs);
+	TRB_SET_EP(cmd->_header.trb, cmd->endpoint_id);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int stop_endpoint_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+
+	xhci_trb_clean(&cmd->_header.trb);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_STOP_ENDPOINT_CMD);
+	TRB_SET_EP(cmd->_header.trb, cmd->endpoint_id);
+	TRB_SET_SUSP(cmd->_header.trb, cmd->susp);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int set_tr_dequeue_pointer_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+
+	xhci_trb_clean(&cmd->_header.trb);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_SET_TR_DEQUEUE_POINTER_CMD);
+	TRB_SET_EP(cmd->_header.trb, cmd->endpoint_id);
+	TRB_SET_STREAM(cmd->_header.trb, cmd->stream_id);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+	TRB_SET_DEQUEUE_PTR(cmd->_header.trb, cmd->dequeue_ptr);
 
 	/**
@@ -421,109 +484,157 @@
 }
 
-int xhci_send_reset_device_command(xhci_hc_t *hc, xhci_cmd_t *cmd)
-{
-	assert(hc);
-	assert(cmd);
-
-	xhci_trb_clean(&cmd->trb);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_RESET_DEVICE_CMD);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_get_port_bandwidth_command(xhci_hc_t *hc, xhci_cmd_t *cmd,
-	xhci_port_bandwidth_ctx_t *ctx, uint8_t device_speed)
-{
-	assert(hc);
-	assert(cmd);
-
-	xhci_trb_clean(&cmd->trb);
-
-	uint64_t phys_addr = (uint64_t) addr_to_phys(ctx);
-	TRB_SET_ICTX(cmd->trb, phys_addr);
-
-	TRB_SET_TYPE(cmd->trb, XHCI_TRB_TYPE_GET_PORT_BANDWIDTH_CMD);
-	TRB_SET_SLOT(cmd->trb, cmd->slot_id);
-	TRB_SET_DEV_SPEED(cmd->trb, device_speed);
-
-	return enqueue_command(hc, cmd, 0, 0);
-}
-
-int xhci_handle_command_completion(xhci_hc_t *hc, xhci_trb_t *trb)
-{
-	// TODO: Update dequeue ptrs.
-	assert(hc);
-	assert(trb);
-
-	usb_log_debug2("HC(%p) Command completed.", hc);
-
-	int code;
-	uint64_t phys;
-	xhci_cmd_t *command;
-
-	code = TRB_GET_CODE(*trb);
-	phys = TRB_GET_PHYS(*trb);;
-	command = get_command(hc, phys);
-	if (command == NULL) {
-		// TODO: STOP & ABORT may not have command structs in the list!
-		usb_log_warning("No command struct for this completion event found.");
-
-		if (code != XHCI_TRBC_SUCCESS)
-			report_error(code);
-
-		return EOK;
-	}
-
-	command->status = code;
-	command->slot_id = TRB_GET_SLOT(*trb);
-
-	usb_log_debug2("Completed command trb: %s", xhci_trb_str_type(TRB_TYPE(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->trb);
+static int reset_device_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+
+	xhci_trb_clean(&cmd->_header.trb);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_RESET_DEVICE_CMD);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+static int get_port_bandwidth_cmd(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+
+	xhci_trb_clean(&cmd->_header.trb);
+
+	uint64_t phys_addr = (uint64_t) addr_to_phys(cmd->bandwidth_ctx);
+	TRB_SET_ICTX(cmd->_header.trb, phys_addr);
+
+	TRB_SET_TYPE(cmd->_header.trb, XHCI_TRB_TYPE_GET_PORT_BANDWIDTH_CMD);
+	TRB_SET_SLOT(cmd->_header.trb, cmd->slot_id);
+	TRB_SET_DEV_SPEED(cmd->_header.trb, cmd->device_speed);
+
+	return enqueue_command(hc, cmd, 0, 0);
+}
+
+/* The table of command-issuing functions. */
+
+typedef int (*cmd_handler) (xhci_hc_t *hc, xhci_cmd_t *cmd);
+
+static cmd_handler cmd_handlers [] = {
+	[XHCI_CMD_ENABLE_SLOT] = enable_slot_cmd,
+	[XHCI_CMD_DISABLE_SLOT] = disable_slot_cmd,
+	[XHCI_CMD_ADDRESS_DEVICE] = address_device_cmd,
+	[XHCI_CMD_CONFIGURE_ENDPOINT] = configure_endpoint_cmd,
+	[XHCI_CMD_EVALUATE_CONTEXT] = evaluate_context_cmd,
+	[XHCI_CMD_RESET_ENDPOINT] = reset_endpoint_cmd,
+	[XHCI_CMD_STOP_ENDPOINT] = stop_endpoint_cmd,
+	[XHCI_CMD_SET_TR_DEQUEUE_POINTER] = set_tr_dequeue_pointer_cmd,
+	[XHCI_CMD_RESET_DEVICE] = reset_device_cmd,
+	// TODO: Force event (optional normative, for VMM, section 4.6.12).
+	[XHCI_CMD_FORCE_EVENT] = NULL,
+	// TODO: Negotiate bandwidth (optional normative, section 4.6.13).
+	[XHCI_CMD_NEGOTIATE_BANDWIDTH] = NULL,
+	// TODO: Set latency tolerance value (optional normative, section 4.6.14).
+	[XHCI_CMD_SET_LATENCY_TOLERANCE_VALUE] = NULL,
+	// TODO: Get port bandwidth (mandatory, but needs root hub implementation, section 4.6.15).
+	[XHCI_CMD_GET_PORT_BANDWIDTH] = get_port_bandwidth_cmd,
+	// TODO: Force header (mandatory, but needs root hub implementation, section 4.6.16).
+	[XHCI_CMD_FORCE_HEADER] = NULL,
+	[XHCI_CMD_NO_OP] = no_op_cmd
+};
+
+static int wait_for_cmd_completion(xhci_cmd_t *cmd)
+{
+	int rv = EOK;
+
+	fibril_mutex_lock(&cmd->_header.completed_mtx);
+	while (!cmd->_header.completed) {
+		usb_log_debug2("Waiting for event completion: going to sleep.");
+		rv = fibril_condvar_wait_timeout(&cmd->_header.completed_cv, &cmd->_header.completed_mtx, cmd->_header.timeout);
+
+		usb_log_debug2("Waiting for event completion: woken: %s", str_error(rv));
+		if (rv == ETIMEOUT) {
+			break;
 		}
 	}
-
-	switch (TRB_TYPE(command->trb)) {
-	case XHCI_TRB_TYPE_NO_OP_CMD:
-		assert(code == XHCI_TRBC_TRB_ERROR);
-		break;
-	case XHCI_TRB_TYPE_ENABLE_SLOT_CMD:
-		break;
-	case XHCI_TRB_TYPE_DISABLE_SLOT_CMD:
-		break;
-	case XHCI_TRB_TYPE_ADDRESS_DEVICE_CMD:
-		break;
-	case XHCI_TRB_TYPE_CONFIGURE_ENDPOINT_CMD:
-		break;
-	case XHCI_TRB_TYPE_EVALUATE_CONTEXT_CMD:
-		break;
-	case XHCI_TRB_TYPE_RESET_ENDPOINT_CMD:
-		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!
-		break;
-	case XHCI_TRB_TYPE_RESET_DEVICE_CMD:
-		break;
-	default:
-		usb_log_debug2("Unsupported command trb: %s", xhci_trb_str_type(TRB_TYPE(command->trb)));
-
-		command->completed = true;
-		return ENAK;
-	}
-
-	fibril_mutex_lock(&command->completed_mtx);
-	command->completed = true;
-	fibril_condvar_broadcast(&command->completed_cv);
-	fibril_mutex_unlock(&command->completed_mtx);
+	fibril_mutex_unlock(&cmd->_header.completed_mtx);
+
+	return rv;
+}
+
+/** Issue command and block the current fibril until it is completed or timeout
+ *  expires. Nothing is deallocated. Caller should always execute `xhci_cmd_fini`.
+ */
+int xhci_cmd_sync(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	assert(hc);
+	assert(cmd);
+
+	int err;
+
+	if (!cmd_handlers[cmd->_header.cmd]) {
+		/* Handler not implemented. */
+		return ENOTSUP;
+	}
+
+	if ((err = cmd_handlers[cmd->_header.cmd](hc, cmd))) {
+		/* Command could not be issued. */
+		return err;
+	}
+
+	if ((err = wait_for_cmd_completion(cmd))) {
+		/* Timeout expired or command failed. */
+		return err;
+	}
 
 	return EOK;
 }
 
+/** Does the same thing as `xhci_cmd_sync` and executes `xhci_cmd_fini`. This
+ *  is a useful shorthand for issuing commands without out parameters.
+ */
+int xhci_cmd_sync_fini(xhci_hc_t *hc, xhci_cmd_t *cmd)
+{
+	const int err = xhci_cmd_sync(hc, cmd);
+	xhci_cmd_fini(cmd);
+
+	return err;
+}
+
+/** Does the same thing as `xhci_cmd_sync_fini` without blocking the current
+ *  fibril. The command is copied to stack memory and `fini` is called upon its completion.
+ */
+int xhci_cmd_async_fini(xhci_hc_t *hc, xhci_cmd_t *stack_cmd)
+{
+	assert(hc);
+	assert(stack_cmd);
+
+	/* Save the command for later. */
+	xhci_cmd_t *heap_cmd = (xhci_cmd_t *) malloc(sizeof(xhci_cmd_t));
+	if (!heap_cmd) {
+		return ENOMEM;
+	}
+
+	/* TODO: Is this good for the mutex and the condvar? */
+	memcpy(heap_cmd, stack_cmd, sizeof(xhci_cmd_t));
+	heap_cmd->_header.async = true;
+
+	/* Issue the command. */
+	int err;
+
+	if (!cmd_handlers[heap_cmd->_header.cmd]) {
+		/* Handler not implemented. */
+		err = ENOTSUP;
+		goto err_heap_cmd;
+	}
+
+	if ((err = cmd_handlers[heap_cmd->_header.cmd](hc, heap_cmd))) {
+		/* Command could not be issued. */
+		goto err_heap_cmd;
+	}
+
+	return EOK;
+
+err_heap_cmd:
+	free(heap_cmd);
+	return err;
+}
 
 /**
Index: uspace/drv/bus/usb/xhci/commands.h
===================================================================
--- uspace/drv/bus/usb/xhci/commands.h	(revision 0206d350511d5ad74999ecd3b4f1abcea0463ba8)
+++ uspace/drv/bus/usb/xhci/commands.h	(revision c3d926f3ce91a6e5d315f2628cac95ccdcbebeff)
@@ -42,7 +42,6 @@
 #include "hw_struct/trb.h"
 
-/* Useful timeouts for `xhci_cmd_wait()` */
-#define XHCI_DEFAULT_TIMEOUT 		 1000000
-#define XHCI_BLOCK_INDEFINITELY 	     0
+#define XHCI_DEFAULT_TIMEOUT       1000000
+#define XHCI_BLOCK_INDEFINITELY    0
 
 typedef struct xhci_hc xhci_hc_t;
@@ -50,29 +49,62 @@
 typedef struct xhci_port_bandwidth_ctx xhci_port_bandwidth_ctx_t;
 
+typedef enum xhci_cmd_type {
+	XHCI_CMD_ENABLE_SLOT,
+	XHCI_CMD_DISABLE_SLOT,
+	XHCI_CMD_ADDRESS_DEVICE,
+	XHCI_CMD_CONFIGURE_ENDPOINT,
+	XHCI_CMD_EVALUATE_CONTEXT,
+	XHCI_CMD_RESET_ENDPOINT,
+	XHCI_CMD_STOP_ENDPOINT,
+	XHCI_CMD_SET_TR_DEQUEUE_POINTER,
+	XHCI_CMD_RESET_DEVICE,
+	XHCI_CMD_FORCE_EVENT,
+	XHCI_CMD_NEGOTIATE_BANDWIDTH,
+	XHCI_CMD_SET_LATENCY_TOLERANCE_VALUE,
+	XHCI_CMD_GET_PORT_BANDWIDTH,
+	XHCI_CMD_FORCE_HEADER,
+	XHCI_CMD_NO_OP,
+} xhci_cmd_type_t;
+
 typedef struct xhci_command {
-	link_t link;
+	/** Internal fields used for bookkeeping. Need not worry about these. */
+	struct {
+		link_t link;
 
-	xhci_trb_t trb;
-	uintptr_t trb_phys;
+		xhci_cmd_type_t cmd;
+		suseconds_t timeout;
+
+		xhci_trb_t trb;
+		uintptr_t trb_phys;
+
+		bool async;
+		bool completed;
+
+		/* Will broadcast after command completes. */
+		fibril_mutex_t completed_mtx;
+		fibril_condvar_t completed_cv;
+	} _header;
+
+	/** Below are arguments of all commands mixed together.
+	 *  Be sure to know which command accepts what arguments. */
 
 	uint32_t slot_id;
+	uint32_t endpoint_id;
+	uint16_t stream_id;
+
+	xhci_input_ctx_t *input_ctx;
+	xhci_port_bandwidth_ctx_t *bandwidth_ctx;
+	uintptr_t dequeue_ptr;
+
+	uint8_t tcs;
+	uint8_t susp;
+	uint8_t device_speed;
 	uint32_t status;
 	bool deconfigure;
-
-	bool completed;
-
-	/* Will broadcast after command completes. */
-	fibril_mutex_t completed_mtx;
-	fibril_condvar_t completed_cv;
 } xhci_cmd_t;
 
+/* Command handling control */
 int xhci_init_commands(xhci_hc_t *);
 void xhci_fini_commands(xhci_hc_t *);
-
-xhci_cmd_t *xhci_cmd_alloc(void);
-void xhci_cmd_init(xhci_cmd_t *);
-int xhci_cmd_wait(xhci_cmd_t *, suseconds_t);
-void xhci_cmd_fini(xhci_cmd_t *);
-void xhci_cmd_free(xhci_cmd_t *);
 
 void xhci_stop_command_ring(xhci_hc_t *);
@@ -80,22 +112,47 @@
 void xhci_start_command_ring(xhci_hc_t *);
 
-int xhci_send_no_op_command(xhci_hc_t *, xhci_cmd_t *);
-int xhci_send_enable_slot_command(xhci_hc_t *, xhci_cmd_t *);
-int xhci_send_disable_slot_command(xhci_hc_t *, xhci_cmd_t *);
-int xhci_send_address_device_command(xhci_hc_t *, xhci_cmd_t *, xhci_input_ctx_t *);
-int xhci_send_configure_endpoint_command(xhci_hc_t *, xhci_cmd_t *, xhci_input_ctx_t *);
-int xhci_send_evaluate_context_command(xhci_hc_t *, xhci_cmd_t *, xhci_input_ctx_t *);
-int xhci_send_reset_endpoint_command(xhci_hc_t *, xhci_cmd_t *, uint32_t, uint8_t);
-int xhci_send_stop_endpoint_command(xhci_hc_t *, xhci_cmd_t *, uint32_t, uint8_t);
-int xhci_send_set_dequeue_ptr_command(xhci_hc_t *, xhci_cmd_t *, uintptr_t, uint16_t, uint32_t);
-int xhci_send_reset_device_command(xhci_hc_t *, xhci_cmd_t *);
-// TODO: Force event (optional normative, for VMM, section 4.6.12).
-// TODO: Negotiate bandwidth (optional normative, section 4.6.13).
-// TODO: Set latency tolerance value (optional normative, section 4.6.14).
-int xhci_get_port_bandwidth_command(xhci_hc_t *, xhci_cmd_t *, xhci_port_bandwidth_ctx_t *, uint8_t);
-// TODO: Get port bandwidth (mandatory, but needs root hub implementation, section 4.6.15).
-// TODO: Force header (mandatory, but needs root hub implementation, section 4.6.16).
+int xhci_handle_command_completion(xhci_hc_t *, xhci_trb_t *);
 
-int xhci_handle_command_completion(xhci_hc_t *, xhci_trb_t *);
+/* Command lifecycle */
+void xhci_cmd_init(xhci_cmd_t *, xhci_cmd_type_t);
+void xhci_cmd_fini(xhci_cmd_t *);
+
+/* Issuing commands */
+int xhci_cmd_sync(xhci_hc_t *, xhci_cmd_t *);
+int xhci_cmd_sync_fini(xhci_hc_t *, xhci_cmd_t *);
+int xhci_cmd_async_fini(xhci_hc_t *, xhci_cmd_t *);
+
+static inline int xhci_cmd_sync_inline_wrapper(xhci_hc_t *hc, xhci_cmd_t cmd)
+{
+	/* Poor man's xhci_cmd_init (everything else is zeroed) */
+	link_initialize(&cmd._header.link);
+	fibril_mutex_initialize(&cmd._header.completed_mtx);
+	fibril_condvar_initialize(&cmd._header.completed_cv);
+
+	if (!cmd._header.timeout) {
+		cmd._header.timeout = XHCI_DEFAULT_TIMEOUT;
+	}
+
+	/* Issue the command */
+	const int err = xhci_cmd_sync(hc, &cmd);
+	xhci_cmd_fini(&cmd);
+
+	return err;
+}
+
+/** The inline macro expects:
+ *    - hc      - HC to schedule command on (xhci_hc_t *).
+ *    - command - Member of `xhci_cmd_type_t` without the "XHCI_CMD_" prefix.
+ *    - VA_ARGS - (optional) Command arguments in struct initialization notation.
+ *
+ *  The return code and semantics matches those of `xhci_cmd_sync_fini`.
+ *
+ *  Example:
+ *    int err = xhci_cmd_sync_inline(hc, DISABLE_SLOT, .slot_id = 42);
+ */
+
+#define xhci_cmd_sync_inline(hc, command, ...) \
+	xhci_cmd_sync_inline_wrapper(hc, \
+	(xhci_cmd_t) { ._header.cmd = XHCI_CMD_##command, ##__VA_ARGS__ })
 
 #endif
Index: uspace/drv/bus/usb/xhci/hc.c
===================================================================
--- uspace/drv/bus/usb/xhci/hc.c	(revision 0206d350511d5ad74999ecd3b4f1abcea0463ba8)
+++ uspace/drv/bus/usb/xhci/hc.c	(revision c3d926f3ce91a6e5d315f2628cac95ccdcbebeff)
@@ -600,17 +600,17 @@
 	int err;
 	xhci_cmd_t cmd;
-	xhci_cmd_init(&cmd);
-
-	if ((err = xhci_send_enable_slot_command(hc, &cmd)) != EOK)
-		return err;
-
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT)) != EOK)
-		return err;
-
-	if (slot_id)
+	xhci_cmd_init(&cmd, XHCI_CMD_ENABLE_SLOT);
+
+	if ((err = xhci_cmd_sync(hc, &cmd))) {
+		goto end;
+	}
+
+	if (slot_id) {
 		*slot_id = cmd.slot_id;
-
+	}
+
+end:
 	xhci_cmd_fini(&cmd);
-	return EOK;
+	return err;
 }
 
@@ -620,16 +620,8 @@
 
 	int err;
-	xhci_cmd_t cmd;
-	xhci_cmd_init(&cmd);
-
-	cmd.slot_id = slot_id;
-
-	if ((err = xhci_send_disable_slot_command(hc, &cmd)) != EOK)
-		return err;
-
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT)) != EOK)
-		return err;
-
-	xhci_cmd_fini(&cmd);
+	if ((err = xhci_cmd_sync_inline(hc, DISABLE_SLOT, .slot_id = slot_id))) {
+		return err;
+	}
+
 	return EOK;
 }
@@ -688,14 +680,8 @@
 	xhci_setup_endpoint_context(ep0, &ictx->endpoint_ctx[0]);
 
-	xhci_cmd_t cmd;
-	xhci_cmd_init(&cmd);
-
-	cmd.slot_id = dev->slot_id;
-
-	if ((err = xhci_send_address_device_command(hc, &cmd, ictx)) != EOK)
-		goto err_cmd;
-
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT)) != EOK)
-		goto err_cmd;
+	/* Issue Address Device command. */
+	if ((err = xhci_cmd_sync_inline(hc, ADDRESS_DEVICE, .slot_id = dev->slot_id, .input_ctx = ictx))) {
+		goto err_dev_ctx;
+	}
 
 	dev->base.address = XHCI_SLOT_DEVICE_ADDRESS(dev->dev_ctx->slot_ctx);
@@ -707,11 +693,6 @@
 	fibril_mutex_unlock(&dev->base.guard);
 
-	xhci_cmd_fini(&cmd);
-	free32(ictx);
-	return EOK;
-
-err_cmd:
-	xhci_cmd_fini(&cmd);
-	free32(ictx);
+	return EOK;
+
 err_dev_ctx:
 	free32(dev->dev_ctx);
@@ -728,31 +709,13 @@
 	xhci_input_ctx_t *ictx;
 	if ((err = create_valid_input_ctx(&ictx))) {
-		goto err;
-	}
-
+		return err;
+	}
 	// TODO: Set slot context and other flags. (probably forgot a lot of 'em)
 
-	xhci_cmd_t cmd;
-	xhci_cmd_init(&cmd);
-
-	cmd.slot_id = slot_id;
-
-	if ((err = xhci_send_configure_endpoint_command(hc, &cmd, ictx))) {
-		goto err_cmd;
-	}
-
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT))) {
-		goto err_cmd;
-	}
-
-	xhci_cmd_fini(&cmd);
-
-	free32(ictx);
-	return EOK;
-
-err_cmd:
-	free32(ictx);
-err:
-	return err;
+	if ((err = xhci_cmd_sync_inline(hc, CONFIGURE_ENDPOINT, .slot_id = slot_id, .input_ctx = ictx))) {
+		return err;
+	}
+
+	return EOK;
 }
 
@@ -762,19 +725,7 @@
 
 	/* Issue configure endpoint command (sec 4.3.5) with the DC flag. */
-	xhci_cmd_t cmd;
-	xhci_cmd_init(&cmd);
-
-	cmd.slot_id = slot_id;
-	cmd.deconfigure = true;
-
-	if ((err = xhci_send_configure_endpoint_command(hc, &cmd, NULL))) {
-		return err;
-	}
-
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT))) {
-		return err;
-	}
-
-	xhci_cmd_fini(&cmd);
+	if ((err = xhci_cmd_sync_inline(hc, CONFIGURE_ENDPOINT, .slot_id = slot_id, .deconfigure = true))) {
+		return err;
+	}
 
 	return EOK;
@@ -788,34 +739,16 @@
 	xhci_input_ctx_t *ictx;
 	if ((err = create_valid_input_ctx(&ictx))) {
-		goto err;
+		return err;
 	}
 
 	XHCI_INPUT_CTRL_CTX_ADD_SET(ictx->ctrl_ctx, ep_idx + 1); /* Preceded by slot ctx */
 	memcpy(&ictx->endpoint_ctx[ep_idx], ep_ctx, sizeof(xhci_ep_ctx_t));
-
 	// TODO: Set slot context and other flags. (probably forgot a lot of 'em)
 
-	xhci_cmd_t cmd;
-	xhci_cmd_init(&cmd);
-
-	cmd.slot_id = slot_id;
-
-	if ((err = xhci_send_configure_endpoint_command(hc, &cmd, ictx))) {
-		goto err_cmd;
-	}
-
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT))) {
-		goto err_cmd;
-	}
-
-	xhci_cmd_fini(&cmd);
-
-	free32(ictx);
-	return EOK;
-
-err_cmd:
-	free32(ictx);
-err:
-	return err;
+	if ((err = xhci_cmd_sync_inline(hc, CONFIGURE_ENDPOINT, .slot_id = slot_id, .input_ctx = ictx))) {
+		return err;
+	}
+
+	return EOK;
 }
 
@@ -827,33 +760,15 @@
 	xhci_input_ctx_t *ictx;
 	if ((err = create_valid_input_ctx(&ictx))) {
-		goto err;
+		return err;
 	}
 
 	XHCI_INPUT_CTRL_CTX_DROP_SET(ictx->ctrl_ctx, ep_idx + 1); /* Preceded by slot ctx */
-
 	// TODO: Set slot context and other flags. (probably forgot a lot of 'em)
 
-	xhci_cmd_t cmd;
-	xhci_cmd_init(&cmd);
-
-	cmd.slot_id = slot_id;
-
-	if ((err = xhci_send_configure_endpoint_command(hc, &cmd, ictx))) {
-		goto err_cmd;
-	}
-
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT))) {
-		goto err_cmd;
-	}
-
-	xhci_cmd_fini(&cmd);
-
-	free32(ictx);
-	return EOK;
-
-err_cmd:
-	free32(ictx);
-err:
-	return err;
+	if ((err = xhci_cmd_sync_inline(hc, CONFIGURE_ENDPOINT, .slot_id = slot_id, .input_ctx = ictx))) {
+		return err;
+	}
+
+	return EOK;
 }
 
Index: uspace/drv/bus/usb/xhci/rh.c
===================================================================
--- uspace/drv/bus/usb/xhci/rh.c	(revision 0206d350511d5ad74999ecd3b4f1abcea0463ba8)
+++ uspace/drv/bus/usb/xhci/rh.c	(revision c3d926f3ce91a6e5d315f2628cac95ccdcbebeff)
@@ -374,20 +374,26 @@
 	// ctx is an out parameter as of now
 	assert(dev);
-
-	ctx = malloc(sizeof(xhci_port_bandwidth_ctx_t));
-	if(!ctx)
+	assert(ctx);
+
+	xhci_port_bandwidth_ctx_t *in_ctx = malloc32(sizeof(xhci_port_bandwidth_ctx_t));
+	if (!in_ctx) {
 		return ENOMEM;
+	}
 
 	xhci_cmd_t cmd;
-	xhci_cmd_init(&cmd);
-
-	xhci_get_port_bandwidth_command(dev->hc, &cmd, ctx, speed);
-
-	int err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT);
-	if(err != EOK) {
-		free(ctx);
-		ctx = NULL;
-	}
-
+	xhci_cmd_init(&cmd, XHCI_CMD_GET_PORT_BANDWIDTH);
+
+	cmd.bandwidth_ctx = in_ctx;
+	cmd.device_speed = speed;
+
+	int err;
+	if ((err = xhci_cmd_sync(dev->hc, &cmd))) {
+		goto end;
+	}
+
+	memcpy(ctx, in_ctx, sizeof(xhci_port_bandwidth_ctx_t));
+
+end:
+	xhci_cmd_fini(&cmd);
 	return EOK;
 }