diff --git a/kernel-src/io/block/vioblk.c b/kernel-src/io/block/vioblk.c new file mode 100644 index 00000000..886c80d4 --- /dev/null +++ b/kernel-src/io/block/vioblk.c @@ -0,0 +1,211 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define QUEUE_MAX_SIZE 128 + +typedef struct { + uint64_t capacity; +} __attribute__((packed)) blkdevconfig_t; + +typedef struct { + viodevice_t *viodevice; + size_t capacity; + int id; + void *queue; + size_t queuesize; + dpc_t queuedpc; + spinlock_t queuelock; + semaphore_t queuesem; + event_t *queuewaiting[QUEUE_MAX_SIZE / 2]; + uint16_t *queuenotify; + uint16_t queuelastindex; +} vioblkdev_t; + +typedef struct { + uint32_t type; + uint32_t reserved; + uint64_t sector; +} requestheader_t; + +#define HEADER_TYPE_READ 0 +#define HEADER_TYPE_WRITE 1 + +static hashtable_t inttable; // int id -> vioblkdev +static scache_t *headercache; + +static void vioblk_dpc(context_t *context, dpcarg_t arg) { + vioblkdev_t *blkdev = arg; + volatile viobuffer_t *buffers = VIO_QUEUE_BUFFERS(blkdev->queue, blkdev->queuesize); + spinlock_acquire(&blkdev->queuelock); + while (blkdev->queuelastindex != VIO_QUEUE_DEV_IDX(blkdev->queue, blkdev->queuesize)) { + int idx = blkdev->queuelastindex++ % blkdev->queuesize; + int buffidx = VIO_QUEUE_DEV_RING(blkdev->queue, blkdev->queuesize)[idx].index; + __assert(blkdev->queuewaiting[buffidx / 2]); + event_signal(blkdev->queuewaiting[buffidx / 2]); + blkdev->queuewaiting[buffidx / 2] = NULL; + buffers[buffidx].address = 0; + buffers[buffidx + 1].address = 0; + semaphore_signal(&blkdev->queuesem); + } + spinlock_release(&blkdev->queuelock); +} + +static void vioblk_irq(isr_t *isr, context_t *context) { + void *v; + __assert(hashtable_get(&inttable, &v, &isr->id, sizeof(isr->id)) == 0); + vioblkdev_t *blkdev = v; + dpc_enqueue(&blkdev->queuedpc, vioblk_dpc, blkdev); +} + +// driver and device are physical addresses +static void vioblk_enqueue(vioblkdev_t *blkdev, void *driver, size_t driverlen, void *device, size_t devicelen) { + event_t event; + EVENT_INIT(&event); + + bool intstatus = interrupt_set(false); + semaphore_wait(&blkdev->queuesem, false); + + spinlock_acquire(&blkdev->queuelock); + int idx = 0; + volatile viobuffer_t *buffers = VIO_QUEUE_BUFFERS(blkdev->queue, blkdev->queuesize); + while (buffers[idx].address && idx < QUEUE_MAX_SIZE) + ++idx; + + __assert(idx < QUEUE_MAX_SIZE); + + buffers[idx].address = (uint64_t)driver; + buffers[idx].length = driverlen; + buffers[idx].flags = VIO_QUEUE_BUFFER_NEXT; + buffers[idx].next = idx + 1; + + buffers[idx + 1].address = (uint64_t)device; + buffers[idx + 1].length = devicelen; + buffers[idx + 1].flags = VIO_QUEUE_BUFFER_DEVICE; + + size_t driveridx = VIO_QUEUE_DRV_IDX(blkdev->queue, blkdev->queuesize)++; + VIO_QUEUE_DRV_RING(blkdev->queue, blkdev->queuesize)[driveridx % blkdev->queuesize] = idx; + + blkdev->queuewaiting[idx / 2] = &event; + + *blkdev->queuenotify = 0; + spinlock_release(&blkdev->queuelock); + + event_wait(&event, false); + + interrupt_set(intstatus); +} + +static int vioblk_rw(vioblkdev_t *blkdev, void *buffer, uintmax_t lba, size_t count, bool write) { + void *physpage = pmm_allocpage(PMM_SECTION_DEFAULT); + if (physpage == NULL) + return ENOMEM; + + int err = 0; + size_t drvlen = sizeof(requestheader_t) + (write ? 512 : 0); + size_t devlen = write ? 1 : 513; + requestheader_t header = { + .type = write ? HEADER_TYPE_WRITE : HEADER_TYPE_READ + }; + + for (int off = 0; off < count; ++off) { + header.sector = lba + off; + memcpy(MAKE_HHDM(physpage), &header, sizeof(requestheader_t)); + + if (write) + memcpy(MAKE_HHDM((uintptr_t)physpage + sizeof(requestheader_t)), (void *)((uintptr_t)buffer + off * 512), 512); + + vioblk_enqueue(blkdev, physpage, drvlen, (void *)((uintptr_t)physpage + drvlen), devlen); + + if (*(uint8_t *)((uintptr_t)MAKE_HHDM(physpage) + drvlen + devlen - 1)) { + err = EIO; + break; + } + + if (write == false) + memcpy((void *)((uintptr_t)buffer + off * 512), MAKE_HHDM((uintptr_t)physpage + drvlen), 512); + } + + pmm_release(physpage); + return err; +} + +static int vioblk_write(void *private, void *buffer, uintmax_t lba, size_t count) { + return vioblk_rw(private, buffer, lba, count, true); +} + +static int vioblk_read(void *private, void *buffer, uintmax_t lba, size_t count) { + return vioblk_rw(private, buffer, lba, count, false); +} + +int vioblk_newdevice(viodevice_t *viodevice) { + if (viodevice->e->msix.exists == false) { + printf("vioblk: device doesn't support msi-x\n"); + return 1; + } + + size_t intcount = pci_initmsix(viodevice->e); + + // TODO support several queues + // no extra features will be used yet + VIO_CONFIG_STATUS_SET(viodevice, VIO_CONFIG_STATUS_FEATURESOK); + if ((viodevice->config->status & VIO_CONFIG_STATUS_FEATURESOK) == 0) + return 1; + + // initialize device object + vioblkdev_t *blkdev = alloc(sizeof(vioblkdev_t)); + __assert(blkdev); + + static int id = 0; + volatile blkdevconfig_t *blkconfig = viodevice->devconfig; + + blkdev->viodevice = viodevice; + blkdev->capacity = blkconfig->capacity; + blkdev->id = id++; + + printf("vioblk%d: capacity of %lu blocks\n", blkdev->id, blkdev->capacity); + + isr_t *isr = interrupt_allocate(vioblk_irq, ARCH_EOI, IPL_DISK); + __assert(isr); + pci_msixadd(viodevice->e, 0, INTERRUPT_IDTOVECTOR(isr->id), 0, 0); + pci_msixsetmask(viodevice->e, 0); + __assert(hashtable_set(&inttable, blkdev, &isr->id, sizeof(isr->id), true) == 0); + + // initialize queue + blkdev->queuesize = min(QUEUE_MAX_SIZE, virtio_queuesize(viodevice, 0)); + blkdev->queue = MAKE_HHDM(virtio_createqueue(viodevice, 0, blkdev->queuesize, 0)); + blkdev->queuenotify = virtio_queuenotifyaddress(viodevice, 0); + SEMAPHORE_INIT(&blkdev->queuesem, blkdev->queuesize / 2); + SPINLOCK_INIT(blkdev->queuelock); + + virtio_enablequeue(viodevice, 0); + virtio_enabledevice(viodevice); + + blockdesc_t blkdesc = { + .private = blkdev, + .blockcapacity = blkdev->capacity, + .blocksize = 512, + .write = vioblk_write, + .read = vioblk_read + }; + + char name[20]; + snprintf(name, 20, "vioblk%d", blkdev->id); + + block_register(&blkdesc, name); + + return 0; +} + +void vioblk_init() { + __assert(hashtable_init(&inttable, 10) == 0); + headercache = slab_newcache(sizeof(requestheader_t), 0, NULL, NULL); + __assert(headercache); +}