Index: uspace/drv/bus/usb/xhci/transfers.c
===================================================================
--- uspace/drv/bus/usb/xhci/transfers.c	(revision a1ce9bd75a1611b36c1d882e27a09e58da89755f)
+++ uspace/drv/bus/usb/xhci/transfers.c	(revision 3038d518c8964eb6ceeb13d82eafc07e08a0d630)
@@ -133,5 +133,5 @@
 
 static void trb_set_buffer(xhci_transfer_t *transfer, xhci_trb_t *trb,
-	size_t i, size_t total)
+	size_t i, size_t total, size_t *remaining)
 {
 	const uintptr_t ptr = dma_buffer_phys(&transfer->hc_buffer,
@@ -140,10 +140,11 @@
 	trb->parameter = host2xhci(64, ptr);
 	TRB_CTRL_SET_TD_SIZE(*trb, max(31, total - i - 1));
-	if (i < total - 1) {
+	if (*remaining > PAGE_SIZE) {
 		TRB_CTRL_SET_XFER_LEN(*trb, PAGE_SIZE);
+		*remaining -= PAGE_SIZE;
 	}
 	else {
-		const size_t size = ((transfer->batch.buffer_size - 1) % PAGE_SIZE) + 1;
-		TRB_CTRL_SET_XFER_LEN(*trb, size);
+		TRB_CTRL_SET_XFER_LEN(*trb, *remaining);
+		*remaining = 0;
 	}
 }
@@ -156,8 +157,12 @@
 	usb_device_request_setup_packet_t* setup = &batch->setup.packet;
 
-	xhci_trb_t trbs[3];
-	int trbs_used = 0;
-
-	xhci_trb_t *trb_setup = trbs + trbs_used++;
+	size_t buffer_count = 0;
+	if (setup->length > 0) {
+		buffer_count = calculate_trb_count(transfer);
+	}
+
+	xhci_trb_t trbs[buffer_count + 2];
+
+	xhci_trb_t *trb_setup = trbs;
 	xhci_trb_clean(trb_setup);
 
@@ -178,26 +183,25 @@
 
 	/* Data stage */
-	xhci_trb_t *trb_data = NULL;
 	if (setup->length > 0) {
-		trb_data = trbs + trbs_used++;
-		xhci_trb_clean(trb_data);
-
-		trb_data->parameter = host2xhci(64, transfer->hc_buffer.phys);
-
-		// data size (sent for OUT, or buffer size)
-		TRB_CTRL_SET_XFER_LEN(*trb_data, batch->buffer_size);
-		// FIXME: TD size 4.11.2.4
-		TRB_CTRL_SET_TD_SIZE(*trb_data, 1);
-
-		// Some more fields here, no idea what they mean
-		TRB_CTRL_SET_TRB_TYPE(*trb_data, XHCI_TRB_TYPE_DATA_STAGE);
-
 		int stage_dir = REQUEST_TYPE_IS_DEVICE_TO_HOST(setup->request_type)
 					? STAGE_IN : STAGE_OUT;
-		TRB_CTRL_SET_DIR(*trb_data, stage_dir);
+		size_t remaining = transfer->batch.buffer_size;
+
+		for (size_t i = 0; i < buffer_count; ++i) {
+			xhci_trb_clean(&trbs[i + 1]);
+			trb_set_buffer(transfer, &trbs[i + 1], i, buffer_count, &remaining);
+
+			TRB_CTRL_SET_DIR(trbs[i + 1], stage_dir);
+			TRB_CTRL_SET_TRB_TYPE(trbs[i + 1], XHCI_TRB_TYPE_DATA_STAGE);
+
+			if (i == buffer_count - 1) break;
+
+			/* Set the chain bit as this is not the last TRB */
+			TRB_CTRL_SET_CHAIN(trbs[i], 1);
+		}
 	}
 
 	/* Status stage */
-	xhci_trb_t *trb_status = trbs + trbs_used++;
+	xhci_trb_t *trb_status = trbs + buffer_count + 1;
 	xhci_trb_clean(trb_status);
 
@@ -215,5 +219,5 @@
 
 	return xhci_trb_ring_enqueue_multiple(get_ring(transfer), trbs,
-	    trbs_used, &transfer->interrupt_trb_phys);
+	    buffer_count + 2, &transfer->interrupt_trb_phys);
 }
 
@@ -224,10 +228,10 @@
 	if (!ep->primary_stream_data_size) {
 		const size_t buffer_count = calculate_trb_count(transfer);
-		xhci_trb_ring_t* ring = get_ring(transfer);
 		xhci_trb_t trbs[buffer_count];
+		size_t remaining = transfer->batch.buffer_size;
 
 		for (size_t i = 0; i < buffer_count; ++i) {
 			xhci_trb_clean(&trbs[i]);
-			trb_set_buffer(transfer, &trbs[i], i, buffer_count);
+			trb_set_buffer(transfer, &trbs[i], i, buffer_count, &remaining);
 			TRB_CTRL_SET_TRB_TYPE(trbs[i], XHCI_TRB_TYPE_NORMAL);
 
@@ -239,19 +243,10 @@
 		/* Set the interrupt bit for last TRB */
 		TRB_CTRL_SET_IOC(trbs[buffer_count - 1], 1);
+
+		xhci_trb_ring_t* ring = get_ring(transfer);
 		return xhci_trb_ring_enqueue_multiple(ring, &trbs[0], buffer_count,
 			&transfer->interrupt_trb_phys);
 	}
 	else {
-		xhci_trb_t trb;
-		xhci_trb_clean(&trb);
-		trb.parameter = host2xhci(64, transfer->hc_buffer.phys);
-
-		// data size (sent for OUT, or buffer size)
-		TRB_CTRL_SET_XFER_LEN(trb, transfer->batch.buffer_size);
-		TRB_CTRL_SET_TD_SIZE(trb, 2);
-		TRB_CTRL_SET_TRB_TYPE(trb, XHCI_TRB_TYPE_NORMAL);
-		TRB_CTRL_SET_CHAIN(trb, 1);
-		TRB_CTRL_SET_ENT(trb, 1);
-
 		xhci_trb_ring_t* ring = get_ring(transfer);
 		if (!ring) {
@@ -259,16 +254,23 @@
 		}
 
-		int err = xhci_trb_ring_enqueue(ring, &trb, &transfer->interrupt_trb_phys);
-
-		if (err) {
-			return err;
-		}
-
-		xhci_trb_clean(&trb);
-		trb.parameter = host2xhci(64, (uintptr_t) transfer);
-		TRB_CTRL_SET_TRB_TYPE(trb, XHCI_TRB_TYPE_EVENT_DATA);
-		TRB_CTRL_SET_IOC(trb, 1);
-
-		return xhci_trb_ring_enqueue(ring, &trb, &transfer->interrupt_trb_phys);
+		const size_t buffer_count = calculate_trb_count(transfer);
+		xhci_trb_t trbs[buffer_count + 1];
+		size_t remaining = transfer->batch.buffer_size;
+
+		for (size_t i = 0; i < buffer_count; ++i) {
+			xhci_trb_clean(&trbs[i]);
+			trb_set_buffer(transfer, &trbs[i], i, buffer_count + 1, &remaining);
+			TRB_CTRL_SET_TRB_TYPE(trbs[i], XHCI_TRB_TYPE_NORMAL);
+			TRB_CTRL_SET_CHAIN(trbs[i], 1);
+		}
+		TRB_CTRL_SET_ENT(trbs[buffer_count - 1], 1);
+
+		xhci_trb_clean(&trbs[buffer_count]);
+		trbs[buffer_count].parameter = host2xhci(64, (uintptr_t) transfer);
+		TRB_CTRL_SET_TRB_TYPE(trbs[buffer_count], XHCI_TRB_TYPE_EVENT_DATA);
+		TRB_CTRL_SET_IOC(trbs[buffer_count], 1);
+
+		return xhci_trb_ring_enqueue_multiple(ring, &trbs[0], buffer_count + 1,
+			&transfer->interrupt_trb_phys);
 	}
 }
@@ -277,10 +279,10 @@
 {
 	const size_t buffer_count = calculate_trb_count(transfer);
-	xhci_trb_ring_t* ring = get_ring(transfer);
 	xhci_trb_t trbs[buffer_count];
+	size_t remaining = transfer->batch.buffer_size;
 
 	for (size_t i = 0; i < buffer_count; ++i) {
 		xhci_trb_clean(&trbs[i]);
-		trb_set_buffer(transfer, &trbs[i], i, buffer_count);
+		trb_set_buffer(transfer, &trbs[i], i, buffer_count, &remaining);
 		TRB_CTRL_SET_TRB_TYPE(trbs[i], XHCI_TRB_TYPE_NORMAL);
 
@@ -292,4 +294,6 @@
 	/* Set the interrupt bit for last TRB */
 	TRB_CTRL_SET_IOC(trbs[buffer_count - 1], 1);
+
+	xhci_trb_ring_t* ring = get_ring(transfer);
 	return xhci_trb_ring_enqueue_multiple(ring, &trbs[0], buffer_count,
 		&transfer->interrupt_trb_phys);
