Index: uspace/drv/bus/usb/xhci/endpoint.c
===================================================================
--- uspace/drv/bus/usb/xhci/endpoint.c	(revision 81487c4a97e762fee2b77d428ca04d094c33dc25)
+++ uspace/drv/bus/usb/xhci/endpoint.c	(revision 31cca4f32e9a51af49d12c4a23a2adfb951e750c)
@@ -81,9 +81,9 @@
 int xhci_endpoint_alloc_transfer_ds(xhci_endpoint_t *xhci_ep)
 {
-	int err;
-
 	if (endpoint_uses_streams(xhci_ep)) {
 		/* Set up primary stream context array if needed. */
 		const size_t size = primary_stream_ctx_array_size(xhci_ep);
+		usb_log_debug2("Allocating primary stream context array of size %lu for endpoint %d:%d.",
+		    size, xhci_ep->base.target.address, xhci_ep->base.target.endpoint);
 
 		xhci_ep->primary_stream_ctx_array = malloc32(size * sizeof(xhci_stream_ctx_t));
@@ -94,5 +94,10 @@
 		memset(xhci_ep->primary_stream_ctx_array, 0, size * sizeof(xhci_stream_ctx_t));
 	} else {
+		usb_log_debug2("Allocating main transfer ring for endpoint %d:%d.",
+		    xhci_ep->base.target.address, xhci_ep->base.target.endpoint);
+
 		xhci_ep->primary_stream_ctx_array = NULL;
+
+		int err;
 		if ((err = xhci_trb_ring_init(&xhci_ep->ring))) {
 			return err;
@@ -105,10 +110,15 @@
 int xhci_endpoint_free_transfer_ds(xhci_endpoint_t *xhci_ep)
 {
-	int err;
-
 	if (endpoint_uses_streams(xhci_ep)) {
+		usb_log_debug2("Freeing primary stream context array for endpoint %d:%d.",
+		    xhci_ep->base.target.address, xhci_ep->base.target.endpoint);
+
 		// TODO: What about secondaries?
 		free32(xhci_ep->primary_stream_ctx_array);
 	} else {
+		usb_log_debug2("Freeing main transfer ring for endpoint %d:%d.",
+		    xhci_ep->base.target.address, xhci_ep->base.target.endpoint);
+
+		int err;
 		if ((err = xhci_trb_ring_fini(&xhci_ep->ring))) {
 			return err;
@@ -232,40 +242,10 @@
 };
 
-int xhci_device_add_endpoint(xhci_device_t *dev, xhci_endpoint_t *ep)
-{
-	assert(dev);
-	assert(ep);
-
-	/* Offline devices don't create new endpoints other than EP0. */
-	if (!dev->online) {
-		return EAGAIN;
-	}
-
-	int err = ENOMEM;
-	const usb_endpoint_t ep_num = ep->base.target.endpoint;
-
-	assert(&dev->base == ep->base.device);
-	assert(dev->base.address == ep->base.target.address);
-	assert(!dev->endpoints[ep_num]);
-
-	dev->endpoints[ep_num] = ep;
-	++dev->active_endpoint_count;
-
-	if (ep_num == 0)
-		/* EP 0 is initialized while setting up the device,
-		 * so we must not issue the command now. */
-		return EOK;
-
-	// FIXME: Set these from usb_superspeed_endpoint_companion_descriptor_t:
-	ep->max_streams = 0;
-	ep->max_burst = 0;
-	ep->mult = 0;
-
-	xhci_endpoint_alloc_transfer_ds(ep);
-
-	// Prepare input context.
+static int create_valid_input_ctx(xhci_input_ctx_t **out_ictx)
+{
 	xhci_input_ctx_t *ictx = malloc32(sizeof(xhci_input_ctx_t));
-	if (!ictx)
-		goto err;
+	if (!ictx) {
+		return ENOMEM;
+	}
 
 	memset(ictx, 0, sizeof(xhci_input_ctx_t));
@@ -277,18 +257,69 @@
 	XHCI_INPUT_CTRL_CTX_ADD_SET(ictx->ctrl_ctx, 0);
 
+	if (out_ictx) {
+		*out_ictx = ictx;
+	}
+
+	return EOK;
+}
+
+int xhci_device_add_endpoint(xhci_device_t *dev, xhci_endpoint_t *ep)
+{
+	assert(dev);
+	assert(ep);
+
+	/* Offline devices don't create new endpoints other than EP0. */
+	if (!dev->online) {
+		return EAGAIN;
+	}
+
+	int err = ENOMEM;
+	const usb_endpoint_t ep_num = ep->base.target.endpoint;
+
+	assert(&dev->base == ep->base.device);
+	assert(dev->base.address == ep->base.target.address);
+	assert(!dev->endpoints[ep_num]);
+
+	dev->endpoints[ep_num] = ep;
+	++dev->active_endpoint_count;
+
+	if (ep_num == 0) {
+		/* EP 0 is initialized while setting up the device,
+		 * so we must not issue the command now. */
+		return EOK;
+	}
+
+	// FIXME: Set these from usb_superspeed_endpoint_companion_descriptor_t:
+	ep->max_streams = 0;
+	ep->max_burst = 0;
+	ep->mult = 0;
+
+	/* Set up TRB ring / PSA. */
+	if ((err = xhci_endpoint_alloc_transfer_ds(ep))) {
+		goto err;
+	}
+
+	/* Issue configure endpoint command (sec 4.3.5). */
+	xhci_input_ctx_t *ictx;
+	if ((err = create_valid_input_ctx(&ictx))) {
+		goto err_ds;
+	}
+
 	const unsigned ep_idx = xhci_endpoint_index(ep);
 	XHCI_INPUT_CTRL_CTX_ADD_SET(ictx->ctrl_ctx, ep_idx + 1); /* Preceded by slot ctx */
-
-	xhci_ep_ctx_t *ep_ctx = &ictx->endpoint_ctx[ep_idx];
-	setup_ep_ctx_helpers[ep->base.transfer_type](ep, ep_ctx);
-
-	// Issue configure endpoint command (sec 4.3.5).
+	setup_ep_ctx_helpers[ep->base.transfer_type](ep, &ictx->endpoint_ctx[ep_idx]);
+
 	xhci_cmd_t cmd;
 	xhci_cmd_init(&cmd);
 
 	cmd.slot_id = dev->slot_id;
-	xhci_send_configure_endpoint_command(dev->hc, &cmd, ictx);
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT)) != EOK)
+
+	if ((err = xhci_send_configure_endpoint_command(dev->hc, &cmd, ictx))) {
 		goto err_ictx;
+	}
+
+	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT))) {
+		goto err_ictx;
+	}
 
 	xhci_cmd_fini(&cmd);
@@ -299,4 +330,6 @@
 err_ictx:
 	free32(ictx);
+err_ds:
+	xhci_endpoint_free_transfer_ds(ep);
 err:
 	dev->endpoints[ep_num] = NULL;
@@ -311,53 +344,96 @@
 	assert(dev->endpoints[ep->base.target.endpoint]);
 
-	// TODO: Issue configure endpoint command to drop this endpoint.
-
-	// FIXME: Ignoring return code.
-	xhci_endpoint_free_transfer_ds(ep);
+	int err = ENOMEM;
+	const usb_endpoint_t ep_num = ep->base.target.endpoint;
 
 	dev->endpoints[ep->base.target.endpoint] = NULL;
 	--dev->active_endpoint_count;
-	return EOK;
-}
-
-xhci_endpoint_t * xhci_device_get_endpoint(xhci_device_t *dev, usb_endpoint_t ep)
-{
-	return dev->endpoints[ep];
-}
-
-int xhci_device_configure(xhci_device_t *dev, xhci_hc_t *hc)
-{
-	int err;
-
-	// Prepare input context.
-	xhci_input_ctx_t *ictx = malloc(sizeof(xhci_input_ctx_t));
-	if (!ictx) {
-		return ENOMEM;
-	}
-
-	memset(ictx, 0, sizeof(xhci_input_ctx_t));
-
-	// Quoting sec. 4.6.6: A1, D0, D1 are down, A0 is up.
-	XHCI_INPUT_CTRL_CTX_ADD_CLEAR(ictx->ctrl_ctx, 1);
-	XHCI_INPUT_CTRL_CTX_DROP_CLEAR(ictx->ctrl_ctx, 0);
-	XHCI_INPUT_CTRL_CTX_DROP_CLEAR(ictx->ctrl_ctx, 1);
-	XHCI_INPUT_CTRL_CTX_ADD_SET(ictx->ctrl_ctx, 0);
-
-	// TODO: Set slot context and other flags. (probably forgot a lot of 'em)
-
-	// Issue configure endpoint command (sec 4.3.5).
+
+	if (ep_num == 0) {
+		/* EP 0 is finalized while releasing the device,
+		 * so we must not issue the command now. */
+		return EOK;
+	}
+
+	/* Issue configure endpoint command to drop this endpoint. */
+	xhci_input_ctx_t *ictx;
+	if ((err = create_valid_input_ctx(&ictx))) {
+		goto err;
+	}
+
+	const unsigned ep_idx = xhci_endpoint_index(ep);
+	XHCI_INPUT_CTRL_CTX_DROP_SET(ictx->ctrl_ctx, ep_idx + 1); /* Preceded by slot ctx */
+
 	xhci_cmd_t cmd;
 	xhci_cmd_init(&cmd);
 
 	cmd.slot_id = dev->slot_id;
-	xhci_send_configure_endpoint_command(hc, &cmd, ictx);
-	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT)) != EOK)
+
+	if ((err = xhci_send_configure_endpoint_command(dev->hc, &cmd, ictx))) {
+		goto err_ictx;
+	}
+
+	if ((err = xhci_cmd_wait(&cmd, XHCI_DEFAULT_TIMEOUT))) {
+		goto err_ictx;
+	}
+
+	xhci_cmd_fini(&cmd);
+
+	/* Tear down TRB ring / PSA. */
+	/* FIXME: For some reason, this causes crash at xhci_trb_ring_fini.
+	if ((err = xhci_endpoint_free_transfer_ds(ep))) {
 		goto err_cmd;
+	}
+	*/
+
+	free32(ictx);
+	return EOK;
+
+err_ictx:
+	free32(ictx);
+err:
+	dev->endpoints[ep_num] = ep;
+	++dev->active_endpoint_count;
+	return err;
+}
+
+xhci_endpoint_t *xhci_device_get_endpoint(xhci_device_t *dev, usb_endpoint_t ep)
+{
+	return dev->endpoints[ep];
+}
+
+int xhci_device_configure(xhci_device_t *dev, xhci_hc_t *hc)
+{
+	int err;
+
+	/* Issue configure endpoint command (sec 4.3.5). */
+	xhci_input_ctx_t *ictx;
+	if ((err = create_valid_input_ctx(&ictx))) {
+		goto 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 = dev->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:
-	free(ictx);
+	free32(ictx);
+err:
 	return err;
 }
