Index: uspace/drv/bus/usb/xhci/endpoint.c
===================================================================
--- uspace/drv/bus/usb/xhci/endpoint.c	(revision 25251bb053fa524ed4b8076c0a1430bfca92c892)
+++ uspace/drv/bus/usb/xhci/endpoint.c	(revision 70108616235ce221aaa3a66cd84292e390a11def)
@@ -66,13 +66,36 @@
 }
 
-static bool endpoint_uses_streams(xhci_endpoint_t *xhci_ep)
-{
-	return xhci_ep->base.transfer_type == USB_TRANSFER_BULK
-	    && xhci_ep->max_streams;
-}
-
-static size_t primary_stream_ctx_array_size(xhci_endpoint_t *xhci_ep)
-{
-	if (!endpoint_uses_streams(xhci_ep))
+static int xhci_endpoint_type(xhci_endpoint_t *ep)
+{
+	const bool in = ep->base.direction == USB_DIRECTION_IN;
+
+	switch (ep->base.transfer_type) {
+	case USB_TRANSFER_CONTROL:
+		return EP_TYPE_CONTROL;
+
+	case USB_TRANSFER_ISOCHRONOUS:
+		return in ? EP_TYPE_ISOCH_IN
+			  : EP_TYPE_ISOCH_OUT;
+
+	case USB_TRANSFER_BULK:
+		return in ? EP_TYPE_BULK_IN
+			  : EP_TYPE_BULK_OUT;
+
+	case USB_TRANSFER_INTERRUPT:
+		return in ? EP_TYPE_INTERRUPT_IN
+			  : EP_TYPE_INTERRUPT_OUT;
+	}
+
+	return EP_TYPE_INVALID;
+}
+
+static bool endpoint_using_streams(xhci_endpoint_t *xhci_ep)
+{
+	return xhci_ep->primary_stream_ctx_array != NULL;
+}
+
+static size_t primary_stream_ctx_array_max_size(xhci_endpoint_t *xhci_ep)
+{
+	if (!xhci_ep->max_streams)
 		return 0;
 
@@ -81,37 +104,96 @@
 }
 
-static bool primary_stream_ctx_has_secondary_array(xhci_stream_ctx_t *primary_ctx) {
-	/* Section 6.2.4.1, SCT values */
-	return XHCI_STREAM_SCT(*primary_ctx) >= 2;
-}
-
-static size_t secondary_stream_ctx_array_size(xhci_stream_ctx_t *primary_ctx) {
-	if (XHCI_STREAM_SCT(*primary_ctx) < 2) return 0;
-	return 2 << XHCI_STREAM_SCT(*primary_ctx);
-}
-
-int xhci_endpoint_alloc_transfer_ds(xhci_endpoint_t *xhci_ep)
-{
-	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 " XHCI_EP_FMT,
-		    size, XHCI_EP_ARGS(*xhci_ep));
-
-		xhci_ep->primary_stream_ctx_array = malloc32(size * sizeof(xhci_stream_ctx_t));
+// static bool primary_stream_ctx_has_secondary_array(xhci_stream_ctx_t *primary_ctx) {
+// 	/* Section 6.2.4.1, SCT values */
+// 	return XHCI_STREAM_SCT(*primary_ctx) >= 2;
+// }
+//
+// static size_t secondary_stream_ctx_array_size(xhci_stream_ctx_t *primary_ctx) {
+// 	if (XHCI_STREAM_SCT(*primary_ctx) < 2) return 0;
+// 	return 2 << XHCI_STREAM_SCT(*primary_ctx);
+// }
+
+static void initialize_primary_streams(xhci_hc_t *hc, xhci_endpoint_t *xhci_ep, unsigned count) {
+	for (size_t index = 0; index < count; ++index) {
+		// Create trb ring for every primary stream
+		// Store it somewhere
+		// Set the dequeue pointer in stream context structure
+
+		// Set to linear stream array
+		XHCI_STREAM_SCT_SET(xhci_ep->primary_stream_ctx_array[index], 1);
+	}
+}
+
+static void setup_stream_context(xhci_endpoint_t *xhci_ep, xhci_ep_ctx_t *ctx, unsigned pstreams) {
+	XHCI_EP_TYPE_SET(*ctx, xhci_endpoint_type(xhci_ep));
+	XHCI_EP_MAX_PACKET_SIZE_SET(*ctx, xhci_ep->base.max_packet_size);
+	XHCI_EP_MAX_BURST_SIZE_SET(*ctx, xhci_ep->max_burst);
+	XHCI_EP_ERROR_COUNT_SET(*ctx, 3);
+
+	XHCI_EP_MAX_P_STREAMS_SET(*ctx, pstreams);
+	XHCI_EP_TR_DPTR_SET(*ctx, addr_to_phys(xhci_ep->primary_stream_ctx_array));
+	// TODO: set HID?
+	XHCI_EP_LSA_SET(*ctx, 1);
+}
+
+int xhci_endpoint_request_streams(xhci_hc_t *hc, xhci_device_t *dev, xhci_endpoint_t *xhci_ep, unsigned count) {
+	if (xhci_ep->base.transfer_type != USB_TRANSFER_BULK
+		|| xhci_ep->base.speed != USB_SPEED_SUPER) {
+		usb_log_error("Streams are only supported by superspeed bulk endpoints.");
+		return EINVAL;
+	}
+
+	if (!primary_stream_ctx_array_max_size(xhci_ep)) {
+		usb_log_error("Streams are not supported by endpoint " XHCI_EP_FMT, XHCI_EP_ARGS(*xhci_ep));
+		return EINVAL;
+	}
+
+	uint8_t max_psa_size = 2 << XHCI_REG_RD(hc->cap_regs, XHCI_CAP_MAX_PSA_SIZE);
+	if (count > max_psa_size) {
+		// We don't support secondary stream arrays yet, so we just give up for this
+		return ENOTSUP;
+	}
+
+	if (count > (unsigned) (1 << xhci_ep->max_streams)) {
+		usb_log_error("Endpoint " XHCI_EP_FMT " supports only %u streams.",
+			XHCI_EP_ARGS(*xhci_ep), (1 << xhci_ep->max_streams));
+		return EINVAL;
+	}
+
+	if (count <= 1024) {
+		usb_log_debug2("Allocating primary stream context array of size %u for endpoint " XHCI_EP_FMT,
+			count, XHCI_EP_ARGS(*xhci_ep));
+		xhci_ep->primary_stream_ctx_array = malloc32(count * sizeof(xhci_stream_ctx_t));
 		if (!xhci_ep->primary_stream_ctx_array) {
 			return ENOMEM;
 		}
 
-		memset(xhci_ep->primary_stream_ctx_array, 0, size * sizeof(xhci_stream_ctx_t));
-	} else {
-		usb_log_debug2("Allocating main transfer ring for endpoint " XHCI_EP_FMT, XHCI_EP_ARGS(*xhci_ep));
-
-		xhci_ep->primary_stream_ctx_array = NULL;
-
-		int err;
-		if ((err = xhci_trb_ring_init(&xhci_ep->ring))) {
-			return err;
-		}
+		// FIXME: count should be rounded to nearest power of 2 for xHC, workaround for now
+		count = 1024;
+		// FIXME: pstreams are "log2(count) - 1"
+		const size_t pstreams = 9;
+		xhci_ep->primary_stream_ctx_array_size = count;
+
+		memset(xhci_ep->primary_stream_ctx_array, 0, count * sizeof(xhci_stream_ctx_t));
+		initialize_primary_streams(hc, xhci_ep, count);
+
+		xhci_ep_ctx_t ep_ctx;
+		setup_stream_context(xhci_ep, &ep_ctx, pstreams);
+		return hc_add_endpoint(hc, dev->slot_id, xhci_endpoint_index(xhci_ep), &ep_ctx);
+	}
+	// Complex stuff not supported yet
+	return ENOTSUP;
+}
+
+int xhci_endpoint_alloc_transfer_ds(xhci_endpoint_t *xhci_ep)
+{
+
+	usb_log_debug2("Allocating main transfer ring for endpoint " XHCI_EP_FMT, XHCI_EP_ARGS(*xhci_ep));
+
+	xhci_ep->primary_stream_ctx_array = NULL;
+
+	int err;
+	if ((err = xhci_trb_ring_init(&xhci_ep->ring))) {
+		return err;
 	}
 
@@ -121,15 +203,18 @@
 int xhci_endpoint_free_transfer_ds(xhci_endpoint_t *xhci_ep)
 {
-	if (endpoint_uses_streams(xhci_ep)) {
+	if (endpoint_using_streams(xhci_ep)) {
 		usb_log_debug2("Freeing primary stream context array for endpoint " XHCI_EP_FMT, XHCI_EP_ARGS(*xhci_ep));
 
 		// maybe check if LSA, then skip?
-		for (size_t index = 0; index < primary_stream_ctx_array_size(xhci_ep); ++index) {
-			xhci_stream_ctx_t *primary_ctx = xhci_ep->primary_stream_ctx_array + index;
-			if (primary_stream_ctx_has_secondary_array(primary_ctx)) {
-				// uintptr_t phys = XHCI_STREAM_DEQ_PTR(*primary_ctx);
-				/* size_t size = */ secondary_stream_ctx_array_size(primary_ctx);
-				// TODO: somehow map the address to virtual and free the secondary array
-			}
+		// for (size_t index = 0; index < primary_stream_ctx_array_size(xhci_ep); ++index) {
+		// 	xhci_stream_ctx_t *primary_ctx = xhci_ep->primary_stream_ctx_array + index;
+		// 	if (primary_stream_ctx_has_secondary_array(primary_ctx)) {
+		// 		// uintptr_t phys = XHCI_STREAM_DEQ_PTR(*primary_ctx);
+		// 		/* size_t size = */ secondary_stream_ctx_array_size(primary_ctx);
+		// 		// TODO: somehow map the address to virtual and free the secondary array
+		// 	}
+		// }
+		for (size_t index = 0; index < xhci_ep->primary_stream_ctx_array_size; ++index) {
+			// FIXME: Get the trb ring associated with stream [index] and fini it
 		}
 		free32(xhci_ep->primary_stream_ctx_array);
@@ -170,28 +255,4 @@
 }
 
-static int xhci_endpoint_type(xhci_endpoint_t *ep)
-{
-	const bool in = ep->base.direction == USB_DIRECTION_IN;
-
-	switch (ep->base.transfer_type) {
-	case USB_TRANSFER_CONTROL:
-		return EP_TYPE_CONTROL;
-
-	case USB_TRANSFER_ISOCHRONOUS:
-		return in ? EP_TYPE_ISOCH_IN
-			  : EP_TYPE_ISOCH_OUT;
-
-	case USB_TRANSFER_BULK:
-		return in ? EP_TYPE_BULK_IN
-			  : EP_TYPE_BULK_OUT;
-
-	case USB_TRANSFER_INTERRUPT:
-		return in ? EP_TYPE_INTERRUPT_IN
-			  : EP_TYPE_INTERRUPT_OUT;
-	}
-
-	return EP_TYPE_INVALID;
-}
-
 static void setup_control_ep_ctx(xhci_endpoint_t *ep, xhci_ep_ctx_t *ctx)
 {
@@ -212,14 +273,7 @@
 	XHCI_EP_ERROR_COUNT_SET(*ctx, 3);
 
-	if (endpoint_uses_streams(ep)) {
-		XHCI_EP_MAX_P_STREAMS_SET(*ctx, ep->max_streams);
-		XHCI_EP_TR_DPTR_SET(*ctx, addr_to_phys(ep->primary_stream_ctx_array));
-		// TODO: set HID
-		// TODO: set LSA
-	} else {
-		XHCI_EP_MAX_P_STREAMS_SET(*ctx, 0);
-		XHCI_EP_TR_DPTR_SET(*ctx, ep->ring.dequeue);
-		XHCI_EP_DCS_SET(*ctx, 1);
-	}
+	XHCI_EP_MAX_P_STREAMS_SET(*ctx, 0);
+	XHCI_EP_TR_DPTR_SET(*ctx, ep->ring.dequeue);
+	XHCI_EP_DCS_SET(*ctx, 1);
 }
 
