KernelLand/Modules/Libraries/VirtIO/virtio.c

   1 /*
   2  * Acess2 VirtIO Common Code
   3  * - By John Hodge (thePowersGang)
   4  *
   5  * virtio.c
   6  * - Core
   7  */
   8 #define DEBUG   0
   9 #define VERSION 0x100
  10 #include <acess.h>
  11 #include <modules.h>
  12 #include <semaphore.h>
  13 #include "include/virtio.h"
  14 #include "include/virtio_hw.h"
  15
  16 // === TYPES ===
  17 typedef struct sVirtIO_Queue    tVirtIO_Queue;
  18
  19 // === STRUCTURES ===
  20 struct sVirtIO_Buf
  21 {
  22         Uint16  Idx;
  23         Uint16  Queue;
  24         tVirtIO_Dev     *Dev;
  25         void    *Handle;
  26         const void      *BufPtr;
  27 };
  28
  29 struct sVirtIO_Queue
  30 {
  31          int    Size;
  32         tVirtIO_QueueCallback   Callback;
  33          int    NoAutoRel;
  34
  35         volatile struct sVirtIO_RingDesc        *Entries;
  36         tShortSpinlock  lAvailQueue;
  37         volatile struct sVirtIO_AvailRing       *Avail;
  38         Uint16  NextUsedPop;
  39         Uint16  LastSeenUsed;
  40         volatile struct sVirtIO_UsedRing        *Used;
  41
  42         tSemaphore      FreeDescsSem;
  43         tShortSpinlock  lFreeList;
  44         Uint16  FirstUnused;
  45
  46         tVirtIO_Buf     Buffers[];
  47 };
  48
  49 struct sVirtIO_Dev
  50 {
  51         Uint    IRQ;
  52         Uint16  IOBase;
  53         Uint16  DevCfgBase;
  54
  55         void    *DataPtr;
  56
  57          int    nQueues;
  58         struct sVirtIO_Queue    *Queues[];
  59 };
  60
  61 // === PROTOTYPES ===
  62  int    VirtIO_Install(char **Arguments);
  63  int    VirtIO_Cleanup(void);
  64 void    VirtIO_IRQHandler(int IRQ, void *Ptr);
  65
  66 // === GLOBALS ===
  67 MODULE_DEFINE(0, VERSION, VirtIOCommon, VirtIO_Install, VirtIO_Cleanup, NULL);
  68
  69 // === CODE ===
  70 int VirtIO_Install(char **Arguments)
  71 {
  72         return 0;
  73 }
  74
  75 int VirtIO_Cleanup(void)
  76 {
  77         Log_Warning("VirtIO", "TODO: Cleanup");
  78         return 1;
  79 }
  80
  81 // --- API ---
  82 // - Device management
  83 tVirtIO_Dev *VirtIO_InitDev(Uint16 IOBase, Uint IRQ, Uint32 Features, int MaxQueues, size_t DataSize)
  84 {
  85         tVirtIO_Dev     *ret;
  86
  87         // Reset and init device
  88         outb(IOBase + VIRTIO_REG_DEVSTS, 0);
  89         outb(IOBase + VIRTIO_REG_DEVSTS, VIRTIO_DEVSTS_ACKNOWLEDGE);
  90         outb(IOBase + VIRTIO_REG_DEVSTS, VIRTIO_DEVSTS_DRIVER);
  91
  92         // Negotiate Features
  93         Uint32  support_feat = ind(IOBase + VIRTIO_REG_DEVFEAT);
  94         outd(IOBase + VIRTIO_REG_GUESTFEAT, Features & support_feat);
  95         LOG("Features: (Dev 0x%08x, Driver 0x%08x)", support_feat, Features);
  96
  97         // Create structure
  98         ret = malloc( offsetof(tVirtIO_Dev, Queues[MaxQueues]) + DataSize );
  99         ret->IRQ = IRQ;
 100         ret->IOBase = IOBase;
 101         ret->nQueues = MaxQueues;
 102         ret->DataPtr = &ret->Queues[MaxQueues];
 103
 104         // TODO: MSI-X makes this move
 105         ret->DevCfgBase = IOBase + VIRTIO_REG_DEVSPEC_0;
 106
 107         // Discover virtqueues
 108         for( int i = 0; i < MaxQueues; i ++ )
 109         {
 110                 outw(IOBase + VIRTIO_REG_QUEUESELECT, i);
 111                 size_t qsz = inw(IOBase + VIRTIO_REG_QUEUESIZE);
 112                 LOG("Queue #%i: QSZ = %i", i, qsz);
 113                 if( qsz == 0 ) {
 114                         ret->Queues[i] = NULL;
 115                         continue ;
 116                 }
 117                 // TODO: Assert that qsz is a power of 2
 118
 119                 tVirtIO_Queue   *queue = calloc( offsetof(tVirtIO_Queue, Buffers[qsz]), 1 );
 120                 queue->Size = qsz;
 121                 queue->FirstUnused = 0;
 122
 123                 Semaphore_Init(&queue->FreeDescsSem, qsz, qsz, "VirtIO", "FreeDescs");
 124
 125                 // Allocate virtqueue spaces
 126                 size_t  sz1 = qsz*16 + offsetof(struct sVirtIO_AvailRing, Ring[qsz])+2;
 127                 size_t  sz2 = offsetof(struct sVirtIO_UsedRing, Ring[qsz]) + 2;
 128                 sz1 = (sz1 + PAGE_SIZE-1) & ~(PAGE_SIZE-1);
 129                 LOG(" sz{1,2} = 0x%x,0x%x", sz1, sz2);
 130                 queue->Entries = MM_AllocDMA( (sz1+sz2+0xFFF)>>12, 32+12, NULL );
 131                 queue->Avail = (void*)(queue->Entries + qsz);
 132                 queue->Used = (void*)((char*)queue->Entries + sz1);
 133
 134                 // Clear and prepare unused list
 135                 memset((void*)queue->Entries, 0, sz1 + sz2);
 136                 for( int j = 0; j < qsz; j ++ )
 137                 {
 138                         queue->Entries[j].Flags = 1;
 139                         queue->Entries[j].Next = j+1;
 140
 141                         queue->Buffers[j].Idx = j;
 142                         queue->Buffers[j].Queue = i;
 143                         queue->Buffers[j].Dev = ret;
 144                 }
 145                 queue->Entries[qsz-1].Flags = 0;
 146
 147                 ret->Queues[i] = queue;
 148
 149                 Uint32  queueaddr = MM_GetPhysAddr(queue->Entries) / 4096;
 150                 LOG(" Phys %P", MM_GetPhysAddr(queue->Entries));
 151                 outd(IOBase + VIRTIO_REG_QUEUEADDR, queueaddr);
 152                 ASSERTC(queueaddr, ==, ind(IOBase + VIRTIO_REG_QUEUEADDR));
 153         }
 154
 155         // Register IRQ Handler
 156         IRQ_AddHandler(IRQ, VirtIO_IRQHandler, ret);
 157         LOG("isr = %x", inb(IOBase + VIRTIO_REG_ISRSTS));
 158
 159         // Start
 160         outb(IOBase + VIRTIO_REG_DEVSTS, VIRTIO_DEVSTS_DRIVER_OK);
 161
 162         return ret;
 163 }
 164
 165 Uint32 VirtIO_GetFeatures(tVirtIO_Dev *Dev)
 166 {
 167         return ind(Dev->IOBase + VIRTIO_REG_GUESTFEAT);
 168 }
 169 Uint32 VirtIO_GetDevConfig(tVirtIO_Dev *Dev, int Size, Uint8 Offset)
 170 {
 171         switch(Size)
 172         {
 173         case 8:
 174                 return inb(Dev->DevCfgBase + Offset);
 175         case 16:
 176                 return inw(Dev->DevCfgBase + Offset);
 177         case 32:
 178                 return ind(Dev->DevCfgBase + Offset);
 179         }
 180         return 0;
 181 }
 182 void *VirtIO_GetDataPtr(tVirtIO_Dev *Dev)
 183 {
 184         return Dev->DataPtr;
 185 }
 186 void VirtIO_RemoveDev(tVirtIO_Dev *Dev)
 187 {
 188         UNIMPLEMENTED();
 189 }
 190
 191 /**
 192  * \brief Sets the Queue Callback
 193  *
 194  * The queue callback is called when
 195  * a) a read-only queue entry is retired (device writes it to the Available ring)
 196  * b) a write-only queue is handed to the guest (devices writes it to the used ring)
 197  */
 198 int VirtIO_SetQueueCallback(tVirtIO_Dev *Dev, int QueueID, tVirtIO_QueueCallback Callback, int NoAutoRel)
 199 {
 200         ASSERTCR(QueueID, <, Dev->nQueues, -1);
 201
 202         Dev->Queues[QueueID]->Callback = Callback;
 203         Dev->Queues[QueueID]->NoAutoRel = NoAutoRel;
 204
 205         if( !Callback && NoAutoRel ) {
 206                 Log_Warning("VirtIO", "%p:%i has had callback==NULL with auto release disabled",
 207                         Dev, QueueID);
 208         }
 209
 210         return 0;
 211 }
 212
 213 int VirtIO_int_AllocQueueEntry(tVirtIO_Queue *Queue)
 214 {
 215         if( Semaphore_Wait(&Queue->FreeDescsSem, 1) != 1 ) {
 216                 return -1;
 217         }
 218
 219         SHORTLOCK(&Queue->lFreeList);
 220         int idx = Queue->FirstUnused;
 221         ASSERT( Queue->Entries[idx].Flags & VRING_DESC_F_NEXT );
 222         Queue->FirstUnused = Queue->Entries[idx].Next;
 223         SHORTREL(&Queue->lFreeList);
 224
 225         return idx;
 226 }
 227
 228 tVirtIO_Buf *VirtIO_int_AllocBuf(tVirtIO_Queue *Queue, const void *Ptr, size_t Size, Uint Flags, Uint16 Next)
 229 {
 230         int idx = VirtIO_int_AllocQueueEntry(Queue);
 231         tVirtIO_Buf     *buf = &Queue->Buffers[idx];
 232         ASSERTC(idx, ==, buf->Idx);
 233
 234         LOG("%p:%i[%i] = {%P+0x%x}",
 235                 buf->Dev, buf->Queue, buf->Idx,
 236                 MM_GetPhysAddr(Ptr), Size);
 237
 238         Queue->Entries[idx].Addr = MM_GetPhysAddr(Ptr);
 239         Queue->Entries[idx].Len = Size;
 240         Queue->Entries[idx].Flags = Flags;
 241         Queue->Entries[idx].Next = Next;
 242
 243         buf->Handle = NULL;
 244         buf->BufPtr = Ptr;
 245
 246         return buf;
 247 }
 248
 249 tVirtIO_Buf *VirtIO_int_AllocBufV(tVirtIO_Queue *Queue, const char *Ptr, size_t Size, Uint Flags, Uint16 Next)
 250 {
 251         if( ((tVAddr)Ptr & (PAGE_SIZE-1)) + Size > PAGE_SIZE*2 )
 252         {
 253                 Log_Error("VirtIO", ">2 page buffers are not supported");
 254                 return NULL;
 255         }
 256
 257         tVirtIO_Buf     *ret;
 258
 259         tPAddr  phys = MM_GetPhysAddr(Ptr);
 260         if( phys + Size-1 != MM_GetPhysAddr( Ptr + Size-1 ) )
 261         {
 262                 size_t  fp_size = PAGE_SIZE-(phys%PAGE_SIZE);
 263                 tVirtIO_Buf *last = VirtIO_int_AllocBuf(Queue, Ptr+fp_size, Size-fp_size, Flags, Next);
 264                 ret = VirtIO_int_AllocBuf(Queue, Ptr, fp_size, Flags|VRING_DESC_F_NEXT, last->Idx);
 265         }
 266         else
 267         {
 268                 ret = VirtIO_int_AllocBuf(Queue, Ptr, Size, Flags, Next);
 269         }
 270         return ret;
 271 }
 272
 273 /*
 274  * Append a ring descriptor to the available ring
 275  */
 276 void VirtIO_int_AddAvailBuf(tVirtIO_Queue *Queue, tVirtIO_Buf *Buf)
 277 {
 278         __sync_synchronize();
 279         SHORTLOCK(&Queue->lAvailQueue);
 280         Queue->Avail->Ring[ Queue->Avail->Idx & (Queue->Size-1) ] = Buf->Idx;
 281         Queue->Avail->Idx ++;
 282         SHORTREL(&Queue->lAvailQueue);
 283
 284         // Notify
 285         __sync_synchronize();
 286         // TODO: Delay notifications
 287         tVirtIO_Dev     *dev = Buf->Dev;
 288         outw(dev->IOBase + VIRTIO_REG_QUEUENOTIFY, Buf->Queue);
 289         LOG("Notifying %p:%i", Buf->Dev, Buf->Queue);
 290 }
 291
 292 // Send a set of RO buffers
 293 tVirtIO_Buf *VirtIO_SendBuffers(tVirtIO_Dev *Dev, int QueueID, int nBufs, size_t Sizes[], const void *Ptrs[], void *Handle)
 294 {
 295         tVirtIO_Queue   *queue = Dev->Queues[QueueID];
 296         tVirtIO_Buf     *prev = NULL;
 297
 298         // Allocate buffers for each non-contiguious region
 299         // - these come from the queue's unallocated pool
 300         size_t  totalsize = 0;
 301         for( int i = nBufs; i --; )
 302         {
 303                 if( prev )
 304                         prev = VirtIO_int_AllocBufV(queue, Ptrs[i], Sizes[i], VRING_DESC_F_NEXT, prev->Idx);
 305                 else
 306                         prev = VirtIO_int_AllocBufV(queue, Ptrs[i], Sizes[i], 0, 0);
 307                 totalsize += Sizes[i];
 308         }
 309         LOG("Total size 0x%x", totalsize);
 310
 311         // Final buffer has the handle set to the passed handle
 312         // - all others get NULL
 313         prev->Handle = Handle;
 314
 315         // Add first to avaliable ring
 316         VirtIO_int_AddAvailBuf(queue, prev);
 317
 318         return prev;
 319 }
 320
 321 // Supply a single WO buffer for the device
 322 tVirtIO_Buf *VirtIO_ReceiveBuffer(tVirtIO_Dev *Dev, int QueueID, size_t Size, void *Ptr, void *Handle)
 323 {
 324         LOG("%p:%i - Add %p+0x%x for RX", Dev, QueueID, Ptr, Size);
 325         tVirtIO_Queue   *queue = Dev->Queues[QueueID];
 326         tVirtIO_Buf     *ret = VirtIO_int_AllocBufV(queue, Ptr, Size, VRING_DESC_F_WRITE, 0);
 327         ret->Handle = Handle;
 328
 329         VirtIO_int_AddAvailBuf(queue, ret);
 330         return ret;
 331 }
 332
 333 tVirtIO_Buf *VirtIO_PopBuffer(tVirtIO_Dev *Dev, int QueueID, size_t *Size, const void **Ptr)
 334 {
 335         ASSERTCR(QueueID, <, Dev->nQueues, NULL);
 336         tVirtIO_Queue   *queue = Dev->Queues[QueueID];
 337
 338         // TODO: Lock
 339         if( queue->NextUsedPop == queue->Used->Idx )
 340                 return NULL;
 341          int    qidx = queue->NextUsedPop;
 342         queue->NextUsedPop ++;
 343
 344          int    idx = queue->Used->Ring[qidx].ID;
 345         if( Size )
 346                 *Size = queue->Used->Ring[qidx].Len;
 347         if( Ptr ) {
 348                 *Ptr = queue->Buffers[idx].BufPtr;
 349                 ASSERTC(MM_GetPhysAddr(*Ptr), ==, queue->Entries[idx].Addr);
 350         }
 351         return &queue->Buffers[idx];
 352 }
 353
 354 const void *VirtIO_GetBufferPtr(tVirtIO_Buf *Buf, size_t *Size)
 355 {
 356         tVirtIO_Queue   *queue = Buf->Dev->Queues[Buf->Queue];
 357         if(Size)
 358                 *Size = queue->Entries[Buf->Idx].Len;
 359         return Buf->BufPtr;
 360 }
 361 tVirtIO_Dev *VirtIO_GetBufferDev(tVirtIO_Buf *Buf)
 362 {
 363         return Buf->Dev;
 364 }
 365
 366 void VirtIO_int_ReleaseQDesc(tVirtIO_Queue *Queue, Uint16 Index)
 367 {
 368         LOG("Release QDesc %p:%i into free pool",
 369                 Queue, Index);
 370         SHORTLOCK(&Queue->lFreeList);
 371         Queue->Entries[Index].Next = Queue->FirstUnused;
 372         Queue->Entries[Index].Flags = VRING_DESC_F_NEXT;
 373         Queue->FirstUnused = Index;
 374         SHORTREL(&Queue->lFreeList);
 375         Semaphore_Signal(&Queue->FreeDescsSem, 1);
 376 }
 377
 378 /**
 379  * \brief Releases all qdescs in the buffer to the free list
 380  */
 381 void VirtIO_ReleaseBuffer(tVirtIO_Buf *Buffer)
 382 {
 383          int    idx = Buffer->Idx;
 384         tVirtIO_Queue   *queue = Buffer->Dev->Queues[Buffer->Queue];
 385
 386         LOG("Releasing chain at %p:%i/%i",
 387                 Buffer->Dev, Buffer->Queue, Buffer->Idx);
 388
 389          int    has_next;
 390         do {
 391                 has_next = !!(queue->Entries[idx].Flags & VRING_DESC_F_NEXT);
 392                 int next_idx = queue->Entries[idx].Next;
 393                 ASSERTC(!has_next || next_idx,!=,idx);
 394
 395                 VirtIO_int_ReleaseQDesc(queue, idx);
 396
 397                 idx = next_idx;
 398         } while(has_next);
 399 }
 400
 401 void VirtIO_int_ProcessUsedList(tVirtIO_Dev *Dev, tVirtIO_Queue *Queue, int UsedIdx)
 402 {
 403         Uint16  qent = Queue->Used->Ring[UsedIdx].ID;
 404         size_t  len  = Queue->Used->Ring[UsedIdx].Len;
 405         LOG("QEnt %i (0x%x bytes) callback w/ Handle=%p",
 406                 qent, len, Queue->Buffers[qent].Handle);
 407         if( Queue->Callback )
 408                 Queue->Callback(Dev, qent, len, Queue->Buffers[qent].Handle);
 409
 410         if( !Queue->NoAutoRel )
 411         {
 412                 LOG("Releasing");
 413                 // Return the buffer to the avaliable pool
 414                 VirtIO_ReleaseBuffer(&Queue->Buffers[qent]);
 415                 if(Queue->NextUsedPop == UsedIdx)
 416                         Queue->NextUsedPop ++;
 417         }
 418 }
 419
 420 void VirtIO_IRQHandler(int IRQ, void *Ptr)
 421 {
 422         tVirtIO_Dev     *Dev = Ptr;
 423         Uint8   isr = inb(Dev->IOBase + VIRTIO_REG_ISRSTS);
 424         LOG("IRQ for %p - ISR = 0x%x", Dev, isr);
 425
 426         // ISR == 0: Interrupt was not from this card
 427         if( isr == 0 )
 428                 return ;
 429
 430         // Check each queue
 431         for( int i = 0; i < Dev->nQueues; i ++ )
 432         {
 433                 tVirtIO_Queue   *queue = Dev->Queues[i];
 434                 // Check 'used' ring
 435                 LOG("Queue %i Used: %i ?!= %i (Avail: %i)",
 436                         i, queue->LastSeenUsed, queue->Used->Idx, queue->Avail->Idx);
 437                 while( queue->LastSeenUsed != queue->Used->Idx )
 438                 {
 439                         int idx = queue->LastSeenUsed;
 440                         queue->LastSeenUsed ++;
 441                         VirtIO_int_ProcessUsedList(Dev, queue, idx);
 442                 }
 443                 LOG("- Done");
 444         }
 445 }
 446