vioblk: properly handle multi-page scatter gather DMA

kernel-src/io/block/vioblk.c

@@ -7,8 +7,9 @@
 #include <kernel/block.h>
 #include <kernel/page.h>
 #include <string.h>
+#include <resource_allocator.h>
 
-#define QUEUE_MAX_SIZE 144
+#define QUEUE_MAX_SIZE 128
 
 typedef struct {
 	uint64_t capacity;
@@ -20,9 +21,10 @@ typedef struct {
 	int id;
 	vioqueue_t queue;
 	dpc_t queuedpc;
+	mutex_t buffers_mutex;
 	spinlock_t queuelock;
-	semaphore_t queuesem;
+	thread_t *queuewaiting[QUEUE_MAX_SIZE];
-	thread_t *queuewaiting[QUEUE_MAX_SIZE / 3];
+	resource_allocator_t allocator;
 } vioblkdev_t;
 
 typedef struct {
@@ -37,19 +39,30 @@ typedef struct {
 static void vioblk_dpc(context_t *context, dpcarg_t arg) {
 	vioblkdev_t *blkdev = arg;
 	volatile viobuffer_t *buffers = VIO_QUEUE_BUFFERS(&blkdev->queue);
-	spinlock_acquire(&blkdev->queuelock);
+
 	while (blkdev->queue.lastusedindex != VIO_QUEUE_DEV_IDX(&blkdev->queue)) {
-		int idx = blkdev->queue.lastusedindex++ % blkdev->queue.size;
+		int queue_idx = blkdev->queue.lastusedindex++ % blkdev->queue.size;
-		int buffidx = VIO_QUEUE_DEV_RING(&blkdev->queue)[idx].index;
+		int start_idx = VIO_QUEUE_DEV_RING(&blkdev->queue)[queue_idx].index;
-		__assert(blkdev->queuewaiting[buffidx / 3]);
+
-		sched_wakeup(blkdev->queuewaiting[buffidx / 3], SCHED_WAKEUP_REASON_NORMAL);
+		thread_t *thread = blkdev->queuewaiting[start_idx];
-		blkdev->queuewaiting[buffidx / 3] = NULL;
+		blkdev->queuewaiting[start_idx] = NULL;
-		buffers[buffidx].address = 0;
+		__assert(thread);
-		buffers[buffidx + 1].address = 0;
+
-		buffers[buffidx + 2].address = 0;
+		// Descriptors can be reused before the owner returns its allocator credit:
-		semaphore_signal(&blkdev->queuesem);
+		// a new request has already reserved an equal number of credits, so this
+		// just exchanges its reserved free slots for this completed chain.
+		int idx = start_idx;
+		for (;;) {
+			int next = buffers[idx].next;
+			int flags = buffers[idx].flags;
+			buffers[idx].address = 0;
+			if (!(flags & VIO_QUEUE_BUFFER_NEXT))
+				break;
+			idx = next;
+		}
+
+		sched_wakeup(thread, SCHED_WAKEUP_REASON_NORMAL);
 	}
-	spinlock_release(&blkdev->queuelock);
 }
 
 static void vioblk_irq(isr_t *isr, context_t *context) {
@@ -57,101 +70,185 @@ static void vioblk_irq(isr_t *isr, context_t *context) {
 	dpc_enqueue(&blkdev->queuedpc, blkdev);
 }
 
-// driver and device are physical addresses
+static void vioblk_enqueue(vioblkdev_t *blkdev, int index) {
-static void vioblk_enqueue(vioblkdev_t *blkdev, requestheader_t *headerphys, void *bufferphys, size_t bufferlen, uint8_t *statusphys, bool write) {
+	bool status = spinlock_acquire_irq_clear(&blkdev->queuelock);
-	bool intstatus = interrupt_set(false);
-	semaphore_wait(&blkdev->queuesem, false);
 
-	spinlock_acquire(&blkdev->queuelock);
+	blkdev->queuewaiting[index] = current_thread();
-	int idx = 0;
-	volatile viobuffer_t *buffers = VIO_QUEUE_BUFFERS(&blkdev->queue);
-	while (buffers[idx].address && idx < blkdev->queue.size)
-		idx += 3;
 
-	__assert(idx < blkdev->queue.size);
+	size_t driver_index = VIO_QUEUE_DRV_IDX(&blkdev->queue);
-
+	VIO_QUEUE_DRV_RING(&blkdev->queue)[driver_index % blkdev->queue.size] = index;
-	buffers[idx].address = (uint64_t)headerphys;
-	buffers[idx].length = sizeof(requestheader_t);
-	buffers[idx].flags = VIO_QUEUE_BUFFER_NEXT;
-	buffers[idx].next = idx + 1;
-
-	buffers[idx + 1].address = (uint64_t)bufferphys;
-	buffers[idx + 1].length = bufferlen;
-	buffers[idx + 1].flags = (write ? 0 : VIO_QUEUE_BUFFER_DEVICE) | VIO_QUEUE_BUFFER_NEXT;
-	buffers[idx + 1].next = idx + 2;
-
-	buffers[idx + 2].address = (uint64_t)statusphys;
-	buffers[idx + 2].length = 1;
-	buffers[idx + 2].flags = VIO_QUEUE_BUFFER_DEVICE;
-
-	size_t driveridx = VIO_QUEUE_DRV_IDX(&blkdev->queue);
-	VIO_QUEUE_DRV_RING(&blkdev->queue)[driveridx % blkdev->queue.size] = idx;
 
 	sched_prepare_sleep(false);
-	blkdev->queuewaiting[idx / 3] = current_thread();
 
 	++VIO_QUEUE_DRV_IDX(&blkdev->queue);
 	*blkdev->queue.notify = 0;
-	spinlock_release(&blkdev->queuelock);
 
+	spinlock_release(&blkdev->queuelock);
 	sched_yield();
 
-	interrupt_set(intstatus);
+	interrupt_set(status);
 }
 
-static int vioblk_rw(vioblkdev_t *blkdev, iovec_iterator_t *iovec_iterator, uintmax_t lba, size_t count, bool write) {
+static void release_and_unlock_pages(void **pages, size_t count) {
-	void *physpage = mm_alloc_page(MEMORY_SECTION_DEFAULT);
+	for (size_t i = 0; i < count; ++i)
-	if (physpage == NULL)
+		mm_unlock_and_release_page(pages[i]);
-		return ENOMEM;
+}
 
-	int err = 0;
+static inline size_t dma_max(vioblkdev_t *blkdev) {
-	requestheader_t *header = physpage;
+	return blkdev->queue.size / 2;
-	requestheader_t *headerhhdm = MAKE_HHDM(physpage);
+}
-	uint8_t *status = (uint8_t *)(header + 1);
-	uint8_t *statushhdm = MAKE_HHDM(status);
 
-	headerhhdm->type = write ? HEADER_TYPE_WRITE : HEADER_TYPE_READ;
+static int get_pages(vioblkdev_t *blkdev, iovec_iterator_t *iovec_iterator, void **pages, size_t *block_count, bool write, size_t *ret_count, int *page_blocks) {
-
+	int error;
-	int done = 0;
+	size_t blocks_done = 0;
-	while (done < count) {
+	size_t pages_done = 0;
-		void *page;
+	while (pages_done < dma_max(blkdev)) {
-		size_t page_offset, page_remaining;
+		if (blocks_done == *block_count)
-		err = iovec_iterator_next_page(iovec_iterator, &page_offset, &page_remaining, &page, !write);
-		if (err)
 			break;
 
-		// check that there is enough space to complete the write of this page
+		void *page;
-		__assert(page);
+		size_t page_offset, page_remaining;
-		__assert(page_remaining >= min((count - done) * 512, PAGE_SIZE));
+		error = iovec_iterator_next_page(iovec_iterator, &page_offset, &page_remaining, &page, !write);
+		if (error)
+			break;
 
-		// and that the space is aligned to the block size
+		if (page == NULL) {
-		__assert((page_remaining % 512) == 0);
+			error = EFAULT;
+			break;
+		}
 
-		headerhhdm->sector = lba + done;
+		pages[pages_done++] = page + page_offset;
-		size_t docount = page_remaining / 512;
-		docount = min(docount, count - done);
 
-		vioblk_enqueue(blkdev, header, (void *)((uintptr_t)page + page_offset), PAGE_SIZE, status, write);
+		if (page_remaining % 512) {
+			error = EINVAL;
+			break;
+		}
 
-		mm_unlock_and_release_page(page);
+		size_t blocks_in_page = page_remaining / 512;
+		size_t blocks = min(blocks_in_page, *block_count - blocks_done);
+
+		blocks_done += blocks;
+		page_blocks[pages_done - 1] = blocks;
 
 		// if we didnt use the whole space in the page, set the iterator back a bit
-		size_t diff_between_available_and_used = page_remaining - docount * 512;
+		size_t diff_between_available_and_used = page_remaining - blocks * 512;
 		if (diff_between_available_and_used) {
 			size_t iterator_offset = iovec_iterator_total_offset(iovec_iterator);
 			iovec_iterator_set(iovec_iterator, iterator_offset - diff_between_available_and_used);
 		}
+	}
 
-		if (*statushhdm) {
+	if (error) {
-			err = EIO;
+		release_and_unlock_pages(pages, pages_done);
+		return error;
+	}
+
+	*block_count = blocks_done;
+	*ret_count = pages_done;
+	return 0;
+}
+
+static int vioblk_rw(vioblkdev_t *blkdev, iovec_iterator_t *iovec_iterator, uintmax_t lba, size_t count, bool write) {
+	void *phys = mm_alloc_page(MEMORY_SECTION_DEFAULT);
+	if (phys == NULL)
+		return ENOMEM;
+
+	void **dma_pages = alloc(dma_max(blkdev) * sizeof(void *));
+	if (dma_pages == NULL) {
+		mm_release_page(phys);
+		return ENOMEM;
+	}
+
+	int *page_blocks = alloc(dma_max(blkdev) * sizeof(int));
+	if (page_blocks == NULL) {
+		free(dma_pages);
+		mm_release_page(phys);
+		return ENOMEM;
+	}
+
+	requestheader_t *header_phys = phys;
+	uint8_t *status_phys = (uint8_t *)(header_phys + 1);
+	requestheader_t *header = MAKE_HHDM(header_phys);
+	uint8_t *status = MAKE_HHDM(status_phys);
+
+	volatile viobuffer_t *buffers = VIO_QUEUE_BUFFERS(&blkdev->queue);
+
+	header->type = write ? HEADER_TYPE_WRITE : HEADER_TYPE_READ;
+
+	size_t done = 0;
+	int error = 0;
+	while (done < count) {
+		size_t page_count;
+		size_t do_count = count - done;
+		error = get_pages(blkdev, iovec_iterator, dma_pages, &do_count, write, &page_count, page_blocks);
+		if (error)
+			goto cleanup;
+
+		// TODO: it would be wise to allocate this before locking the pages
+		resource_allocate(&blkdev->allocator, page_count + 2);
+
+		MUTEX_ACQUIRE(&blkdev->buffers_mutex);
+
+		int first_buffer = -1;
+		int last_buffer;
+		size_t buffers_done = 0;
+		for (size_t i = 0; i < blkdev->queue.size; ++i) {
+			if (buffers[i].address)
+				continue;
+
+			++buffers_done;
+
+			if (first_buffer == -1) {
+				first_buffer = i;
+				buffers[i].address = (uint64_t)header_phys;
+				buffers[i].length = sizeof(requestheader_t);
+				buffers[i].flags = 0;
+				last_buffer = i;
+				continue;
+			}
+
+			buffers[last_buffer].next = i;
+			buffers[last_buffer].flags |= VIO_QUEUE_BUFFER_NEXT;
+			last_buffer = i;
+
+			if (buffers_done == page_count + 2) {
+				buffers[i].address = (uint64_t)status_phys;
+				buffers[i].length = 1;
+				buffers[i].flags = VIO_QUEUE_BUFFER_DEVICE;
 				break;
 			}
 
-		done += docount;
+			buffers[i].address = (uint64_t)dma_pages[buffers_done - 2];
+			buffers[i].length = page_blocks[buffers_done - 2] * 512;
+			buffers[i].flags = write ? 0 : VIO_QUEUE_BUFFER_DEVICE;
 		}
 
-	mm_release_page(physpage);
+		__assert(first_buffer != -1);
-	return err;
+		__assert(last_buffer != first_buffer);
+
+		MUTEX_RELEASE(&blkdev->buffers_mutex);
+
+		header->sector = lba + done;
+
+		vioblk_enqueue(blkdev, first_buffer);
+
+		resource_free(&blkdev->allocator, page_count + 2);
+
+		release_and_unlock_pages(dma_pages, page_count);
+
+		if (*status) {
+			error = EIO;
+			goto cleanup;
+		}
+
+		done += do_count;
+	}
+
+	cleanup:
+	free(page_blocks);
+	free(dma_pages);
+	mm_release_page(phys);
+	return error;
 }
 
 static int vioblk_write(void *private, iovec_iterator_t *iovec_iterator, uintmax_t lba, size_t count) {
@@ -194,11 +291,13 @@ int vioblk_newdevice(viodevice_t *viodevice) {
 	pci_msixsetmask(viodevice->e, 0);
 
 	// initialize queue
+	__assert(virtio_queuesize(viodevice, 0) > 2 && "Here's a nickel, kid. Go get yourself a better hypervisor.");
 	size_t size = min(QUEUE_MAX_SIZE, virtio_queuesize(viodevice, 0));
-	size = size - (size % 3);
 	virtio_createqueue(viodevice, &blkdev->queue, 0, size, 0);
-	SEMAPHORE_INIT(&blkdev->queuesem, blkdev->queue.size / 3);
 	SPINLOCK_INIT(blkdev->queuelock);
+	MUTEX_INIT(&blkdev->buffers_mutex);
+
+	resource_allocator_init(&blkdev->allocator, size, dma_max(blkdev) + 2);
 
 	virtio_enablequeue(viodevice, 0);
 	virtio_enabledevice(viodevice);