Index: uspace/drv/bus/usb/xhci/streams.c
===================================================================
--- uspace/drv/bus/usb/xhci/streams.c	(revision 0f803831a7df5a03c2de637251d1bf306a4735b7)
+++ uspace/drv/bus/usb/xhci/streams.c	(revision 5dab9ef0eac60d28c2266de9c442a55ca7f6b721)
@@ -70,4 +70,49 @@
 }
 
+static int initialize_primary_structures(xhci_endpoint_t *xhci_ep, unsigned count)
+{
+	usb_log_debug2("Allocating primary stream context array of size %u for endpoint " XHCI_EP_FMT,
+		count, XHCI_EP_ARGS(*xhci_ep));
+
+	if ((dma_buffer_alloc(&xhci_ep->primary_stream_ctx_dma, count * sizeof(xhci_stream_ctx_t)))) {
+		return ENOMEM;
+	}
+
+	xhci_ep->primary_stream_ctx_array = xhci_ep->primary_stream_ctx_dma.virt;
+	xhci_ep->primary_stream_data_array = calloc(count, sizeof(xhci_stream_data_t));
+	if (!xhci_ep->primary_stream_data_array) {
+		dma_buffer_free(&xhci_ep->primary_stream_ctx_dma);
+		return ENOMEM;
+	}
+
+	xhci_ep->primary_stream_data_size = count;
+
+	return EOK;
+}
+
+static void clear_primary_structures(xhci_endpoint_t *xhci_ep)
+{
+	usb_log_debug2("Deallocating primary stream structures for endpoint " XHCI_EP_FMT, XHCI_EP_ARGS(*xhci_ep));
+
+	dma_buffer_free(&xhci_ep->primary_stream_ctx_dma);
+	free(xhci_ep->primary_stream_data_array);
+}
+
+static void clear_secondary_streams(xhci_endpoint_t *xhci_ep, unsigned index)
+{
+	xhci_stream_data_t *data = &xhci_ep->primary_stream_data_array[index];
+	if (!data->secondary_size) {
+		xhci_trb_ring_fini(&data->ring);
+		return;
+	}
+
+	for (size_t i = 0; i < data->secondary_size; ++i) {
+		xhci_trb_ring_fini(&data->secondary_data[i].ring);
+	}
+
+	dma_buffer_free(&data->secondary_stream_ctx_dma);
+	free(data->secondary_data);
+}
+
 void xhci_stream_free_ds(xhci_endpoint_t *xhci_ep)
 {
@@ -75,23 +120,13 @@
 
 	for (size_t index = 0; index < xhci_ep->primary_stream_data_size; ++index) {
-		xhci_stream_data_t *primary_data = xhci_ep->primary_stream_data_array + index;
-		if (primary_data->secondary_size > 0) {
-			for (size_t index2 = 0; index2 < primary_data->secondary_size; ++index2) {
-				xhci_stream_data_t *secondary_data = primary_data->secondary_data + index2;
-				xhci_trb_ring_fini(&secondary_data->ring);
-			}
-			dma_buffer_free(&primary_data->secondary_stream_ctx_dma);
-		}
-		else {
-			xhci_trb_ring_fini(&primary_data->ring);
-		}
-	}
-	dma_buffer_free(&xhci_ep->primary_stream_ctx_dma);
-}
-
-/** Initialize secondary streams of XHCI bulk endpoint.
+		clear_secondary_streams(xhci_ep, index);
+	}
+	clear_primary_structures(xhci_ep);
+}
+
+/** Initialize primary stream structure with given index.
  * @param[in] hc Host controller of the endpoint.
  * @param[in] xhci_epi XHCI bulk endpoint to use.
- * @param[in] index Index to primary stream array
+ * @param[in] index index of the initialized stream structure.
  */
 static int initialize_primary_stream(xhci_hc_t *hc, xhci_endpoint_t *xhci_ep, unsigned index) {
@@ -116,19 +151,24 @@
 /** Initialize primary streams of XHCI bulk endpoint.
  * @param[in] hc Host controller of the endpoint.
- * @param[in] xhci_epi XHCI bulk endpoint to use.
+ * @param[in] xhci_ep XHCI bulk endpoint to use.
  */
 static int initialize_primary_streams(xhci_hc_t *hc, xhci_endpoint_t *xhci_ep)
 {
 	int err = EOK;
-	for (size_t index = 0; index < xhci_ep->primary_stream_data_size; ++index) {
+	size_t index;
+	for (index = 0; index < xhci_ep->primary_stream_data_size; ++index) {
 		err = initialize_primary_stream(hc, xhci_ep, index);
 		if (err) {
-			return err;
+			goto err_clean;
 		}
 	}
 
-	// TODO: deinitialize if we got stuck in the middle
-
-	return EOK;
+	return EOK;
+
+err_clean:
+	for (size_t i = 0; i < index; ++i) {
+		xhci_trb_ring_fini(&xhci_ep->primary_stream_data_array[i].ring);
+	}
+	return err;
 }
 
@@ -136,11 +176,11 @@
  * @param[in] hc Host controller of the endpoint.
  * @param[in] xhci_epi XHCI bulk endpoint to use.
- * @param[in] index Index to primary stream array
+ * @param[in] idx Index to primary stream array
  * @param[in] count Number of secondary streams to initialize.
  */
-static int initialize_secondary_streams(xhci_hc_t *hc, xhci_endpoint_t *xhci_ep, unsigned index, unsigned count)
+static int initialize_secondary_streams(xhci_hc_t *hc, xhci_endpoint_t *xhci_ep, unsigned idx, unsigned count)
 {
 	if (count == 0) {
-		return initialize_primary_stream(hc, xhci_ep, index);
+		return initialize_primary_stream(hc, xhci_ep, idx);
 	}
 
@@ -150,6 +190,6 @@
 	}
 
-	xhci_stream_ctx_t *ctx = &xhci_ep->primary_stream_ctx_array[index];
-	xhci_stream_data_t *data = &xhci_ep->primary_stream_data_array[index];
+	xhci_stream_ctx_t *ctx = &xhci_ep->primary_stream_ctx_array[idx];
+	xhci_stream_data_t *data = &xhci_ep->primary_stream_data_array[idx];
 	memset(data, 0, sizeof(xhci_stream_data_t));
 
@@ -161,4 +201,5 @@
 
 	if ((dma_buffer_alloc(&data->secondary_stream_ctx_dma, count * sizeof(xhci_stream_ctx_t)))) {
+		free(data->secondary_data);
 		return ENOMEM;
 	}
@@ -169,22 +210,25 @@
 
 	int err = EOK;
-
-	for (size_t i = 0; i < count; ++i) {
-		xhci_stream_ctx_t *secondary_ctx = &data->secondary_stream_ctx_array[i];
-		xhci_stream_data_t *secondary_data = &data->secondary_data[i];
+	size_t index;
+	for (index = 0; index < count; ++index) {
+		xhci_stream_ctx_t *secondary_ctx = &data->secondary_stream_ctx_array[index];
+		xhci_stream_data_t *secondary_data = &data->secondary_data[index];
 		/* Init and register TRB ring for every secondary stream */
 		if ((err = xhci_trb_ring_init(&secondary_data->ring))) {
-			return err;
+			goto err_init;
 		}
 
 		XHCI_STREAM_DEQ_PTR_SET(*secondary_ctx, secondary_data->ring.dequeue);
-
-		/* Set to linear stream array */
+		/* Set to secondary stream array */
 		XHCI_STREAM_SCT_SET(*secondary_ctx, 0);
 	}
 
-	// TODO: deinitialize if we got stuck in the middle
-
-	return EOK;
+	return EOK;
+
+err_init:
+	for (size_t i = 0; i < index; ++i) {
+		xhci_trb_ring_fini(&data->secondary_data[i].ring);
+	}
+	return err;
 }
 
@@ -246,25 +290,4 @@
 }
 
-static int initialize_primary_structures(xhci_endpoint_t *xhci_ep, unsigned count)
-{
-	usb_log_debug2("Allocating primary stream context array of size %u for endpoint " XHCI_EP_FMT,
-		count, XHCI_EP_ARGS(*xhci_ep));
-
-	if ((dma_buffer_alloc(&xhci_ep->primary_stream_ctx_dma, count * sizeof(xhci_stream_ctx_t)))) {
-		return ENOMEM;
-	}
-
-	xhci_ep->primary_stream_ctx_array = xhci_ep->primary_stream_ctx_dma.virt;
-	xhci_ep->primary_stream_data_array = calloc(count, sizeof(xhci_stream_data_t));
-	if (!xhci_ep->primary_stream_data_array) {
-		dma_buffer_free(&xhci_ep->primary_stream_ctx_dma);
-		return ENOMEM;
-	}
-
-	xhci_ep->primary_stream_data_size = count;
-
-	return EOK;
-}
-
 /** Initialize primary streams
  */
@@ -283,5 +306,9 @@
 
 	memset(xhci_ep->primary_stream_ctx_array, 0, count * sizeof(xhci_stream_ctx_t));
-	initialize_primary_streams(hc, xhci_ep);
+	err = initialize_primary_streams(hc, xhci_ep);
+	if (err) {
+		clear_primary_structures(xhci_ep);
+		return err;
+	}
 
 	xhci_ep_ctx_t ep_ctx;
@@ -340,6 +367,10 @@
 
 	memset(xhci_ep->primary_stream_ctx_array, 0, count * sizeof(xhci_stream_ctx_t));
-	for (size_t index = 0; index < count; ++index) {
-		initialize_secondary_streams(hc, xhci_ep, index, *(sizes + index));
+	size_t index;
+	for (index = 0; index < count; ++index) {
+		err = initialize_secondary_streams(hc, xhci_ep, index, *(sizes + index));
+		if (err) {
+			goto err_init;
+		}
 	}
 
@@ -350,3 +381,10 @@
 	// FIXME: do we add endpoint? do we need to destroy previous configuration?
 	return hc_add_endpoint(hc, dev->slot_id, xhci_endpoint_index(xhci_ep), &ep_ctx);
-}
+
+err_init:
+	for (size_t i = 0; i < index; ++i) {
+		clear_secondary_streams(xhci_ep, i);
+	}
+	clear_primary_structures(xhci_ep);
+	return err;
+}