mirror of
https://github.com/Mathewnd/Astral.git
synced 2026-10-27 12:48:22 +00:00
vioblk: properly handle multi-page scatter gather DMA
This commit is contained in:
Matheus
+182
-83
@@ -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 / 3];
|
||||
thread_t *queuewaiting[QUEUE_MAX_SIZE];
|
||||
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 buffidx = VIO_QUEUE_DEV_RING(&blkdev->queue)[idx].index;
|
||||
__assert(blkdev->queuewaiting[buffidx / 3]);
|
||||
sched_wakeup(blkdev->queuewaiting[buffidx / 3], SCHED_WAKEUP_REASON_NORMAL);
|
||||
blkdev->queuewaiting[buffidx / 3] = NULL;
|
||||
buffers[buffidx].address = 0;
|
||||
buffers[buffidx + 1].address = 0;
|
||||
buffers[buffidx + 2].address = 0;
|
||||
semaphore_signal(&blkdev->queuesem);
|
||||
int queue_idx = blkdev->queue.lastusedindex++ % blkdev->queue.size;
|
||||
int start_idx = VIO_QUEUE_DEV_RING(&blkdev->queue)[queue_idx].index;
|
||||
|
||||
thread_t *thread = blkdev->queuewaiting[start_idx];
|
||||
blkdev->queuewaiting[start_idx] = NULL;
|
||||
__assert(thread);
|
||||
|
||||
// Descriptors can be reused before the owner returns its allocator credit:
|
||||
// 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, requestheader_t *headerphys, void *bufferphys, size_t bufferlen, uint8_t *statusphys, bool write) {
|
||||
bool intstatus = interrupt_set(false);
|
||||
semaphore_wait(&blkdev->queuesem, false);
|
||||
static void vioblk_enqueue(vioblkdev_t *blkdev, int index) {
|
||||
bool status = spinlock_acquire_irq_clear(&blkdev->queuelock);
|
||||
|
||||
spinlock_acquire(&blkdev->queuelock);
|
||||
int idx = 0;
|
||||
volatile viobuffer_t *buffers = VIO_QUEUE_BUFFERS(&blkdev->queue);
|
||||
while (buffers[idx].address && idx < blkdev->queue.size)
|
||||
idx += 3;
|
||||
blkdev->queuewaiting[index] = current_thread();
|
||||
|
||||
__assert(idx < blkdev->queue.size);
|
||||
|
||||
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;
|
||||
size_t driver_index = VIO_QUEUE_DRV_IDX(&blkdev->queue);
|
||||
VIO_QUEUE_DRV_RING(&blkdev->queue)[driver_index % blkdev->queue.size] = index;
|
||||
|
||||
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) {
|
||||
void *physpage = mm_alloc_page(MEMORY_SECTION_DEFAULT);
|
||||
if (physpage == NULL)
|
||||
return ENOMEM;
|
||||
static void release_and_unlock_pages(void **pages, size_t count) {
|
||||
for (size_t i = 0; i < count; ++i)
|
||||
mm_unlock_and_release_page(pages[i]);
|
||||
}
|
||||
|
||||
int err = 0;
|
||||
requestheader_t *header = physpage;
|
||||
requestheader_t *headerhhdm = MAKE_HHDM(physpage);
|
||||
uint8_t *status = (uint8_t *)(header + 1);
|
||||
uint8_t *statushhdm = MAKE_HHDM(status);
|
||||
static inline size_t dma_max(vioblkdev_t *blkdev) {
|
||||
return blkdev->queue.size / 2;
|
||||
}
|
||||
|
||||
headerhhdm->type = write ? HEADER_TYPE_WRITE : HEADER_TYPE_READ;
|
||||
|
||||
int done = 0;
|
||||
while (done < count) {
|
||||
void *page;
|
||||
size_t page_offset, page_remaining;
|
||||
err = iovec_iterator_next_page(iovec_iterator, &page_offset, &page_remaining, &page, !write);
|
||||
if (err)
|
||||
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;
|
||||
size_t blocks_done = 0;
|
||||
size_t pages_done = 0;
|
||||
while (pages_done < dma_max(blkdev)) {
|
||||
if (blocks_done == *block_count)
|
||||
break;
|
||||
|
||||
// check that there is enough space to complete the write of this page
|
||||
__assert(page);
|
||||
__assert(page_remaining >= min((count - done) * 512, PAGE_SIZE));
|
||||
void *page;
|
||||
size_t page_offset, page_remaining;
|
||||
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
|
||||
__assert((page_remaining % 512) == 0);
|
||||
if (page == NULL) {
|
||||
error = EFAULT;
|
||||
break;
|
||||
}
|
||||
|
||||
headerhhdm->sector = lba + done;
|
||||
size_t docount = page_remaining / 512;
|
||||
docount = min(docount, count - done);
|
||||
pages[pages_done++] = page + page_offset;
|
||||
|
||||
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) {
|
||||
err = EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
done += docount;
|
||||
}
|
||||
|
||||
mm_release_page(physpage);
|
||||
return err;
|
||||
if (error) {
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
__assert(first_buffer != -1);
|
||||
__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);
|
||||
|
||||
Reference in New Issue
Block a user