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   1
   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         Uint8 isr = inb(IOBase + VIRTIO_REG_ISRSTS);
 158         LOG("isr = %x", isr);
 159
 160         // Start
 161         outb(IOBase + VIRTIO_REG_DEVSTS, VIRTIO_DEVSTS_DRIVER_OK);
 162
 163         return ret;
 164 }
 165
 166 Uint32 VirtIO_GetFeatures(tVirtIO_Dev *Dev)
 167 {
 168         return ind(Dev->IOBase + VIRTIO_REG_GUESTFEAT);
 169 }
 170 Uint32 VirtIO_GetDevConfig(tVirtIO_Dev *Dev, int Size, Uint8 Offset)
 171 {
 172         switch(Size)
 173         {
 174         case 8:
 175                 return inb(Dev->DevCfgBase + Offset);
 176         case 16:
 177                 return inw(Dev->DevCfgBase + Offset);
 178         case 32:
 179                 return ind(Dev->DevCfgBase + Offset);
 180         }
 181         return 0;
 182 }
 183 void *VirtIO_GetDataPtr(tVirtIO_Dev *Dev)
 184 {
 185         return Dev->DataPtr;
 186 }
 187 void VirtIO_RemoveDev(tVirtIO_Dev *Dev)
 188 {
 189         UNIMPLEMENTED();
 190 }
 191
 192 /**
 193  * \brief Sets the Queue Callback
 194  *
 195  * The queue callback is called when
 196  * a) a read-only queue entry is retired (device writes it to the Available ring)
 197  * b) a write-only queue is handed to the guest (devices writes it to the used ring)
 198  */
 199 int VirtIO_SetQueueCallback(tVirtIO_Dev *Dev, int QueueID, tVirtIO_QueueCallback Callback, int NoAutoRel)
 200 {
 201         ASSERTCR(QueueID, <, Dev->nQueues, -1);
 202
 203         Dev->Queues[QueueID]->Callback = Callback;
 204         Dev->Queues[QueueID]->NoAutoRel = NoAutoRel;
 205
 206         if( !Callback && NoAutoRel ) {
 207                 Log_Warning("VirtIO", "%p:%i has had callback==NULL with auto release disabled",
 208                         Dev, QueueID);
 209         }
 210
 211         return 0;
 212 }
 213
 214 int VirtIO_int_AllocQueueEntry(tVirtIO_Queue *Queue)
 215 {
 216         if( Semaphore_Wait(&Queue->FreeDescsSem, 1) != 1 ) {
 217                 return -1;
 218         }
 219
 220         SHORTLOCK(&Queue->lFreeList);
 221         int idx = Queue->FirstUnused;
 222         ASSERT( Queue->Entries[idx].Flags & VRING_DESC_F_NEXT );
 223         Queue->FirstUnused = Queue->Entries[idx].Next;
 224         SHORTREL(&Queue->lFreeList);
 225
 226         return idx;
 227 }
 228
 229 tVirtIO_Buf *VirtIO_int_AllocBuf(tVirtIO_Queue *Queue, const void *Ptr, size_t Size, Uint Flags, Uint16 Next)
 230 {
 231         int idx = VirtIO_int_AllocQueueEntry(Queue);
 232         tVirtIO_Buf     *buf = &Queue->Buffers[idx];
 233         ASSERTC(idx, ==, buf->Idx);
 234
 235         LOG("%p:%i[%i] = {%P+0x%x}",
 236                 buf->Dev, buf->Queue, buf->Idx,
 237                 MM_GetPhysAddr(Ptr), Size);
 238
 239         Queue->Entries[idx].Addr = MM_GetPhysAddr(Ptr);
 240         Queue->Entries[idx].Len = Size;
 241         Queue->Entries[idx].Flags = Flags;
 242         Queue->Entries[idx].Next = Next;
 243
 244         buf->Handle = NULL;
 245         buf->BufPtr = Ptr;
 246
 247         return buf;
 248 }
 249
 250 tVirtIO_Buf *VirtIO_int_AllocBufV(tVirtIO_Queue *Queue, const char *Ptr, size_t Size, Uint Flags, Uint16 Next)
 251 {
 252         if( ((tVAddr)Ptr & (PAGE_SIZE-1)) + Size > PAGE_SIZE*2 )
 253         {
 254                 Log_Error("VirtIO", ">2 page buffers are not supported");
 255                 return NULL;
 256         }
 257
 258         tVirtIO_Buf     *ret;
 259
 260         tPAddr  phys = MM_GetPhysAddr(Ptr);
 261         if( phys + Size-1 != MM_GetPhysAddr( Ptr + Size-1 ) )
 262         {
 263                 size_t  fp_size = PAGE_SIZE-(phys%PAGE_SIZE);
 264                 tVirtIO_Buf *last = VirtIO_int_AllocBuf(Queue, Ptr+fp_size, Size-fp_size, Flags, Next);
 265                 ret = VirtIO_int_AllocBuf(Queue, Ptr, fp_size, Flags|VRING_DESC_F_NEXT, last->Idx);
 266         }
 267         else
 268         {
 269                 ret = VirtIO_int_AllocBuf(Queue, Ptr, Size, Flags, Next);
 270         }
 271         return ret;
 272 }
 273
 274 /*
 275  * Append a ring descriptor to the available ring
 276  */
 277 void VirtIO_int_AddAvailBuf(tVirtIO_Queue *Queue, tVirtIO_Buf *Buf)
 278 {
 279         __sync_synchronize();
 280         SHORTLOCK(&Queue->lAvailQueue);
 281         Queue->Avail->Ring[ Queue->Avail->Idx & (Queue->Size-1) ] = Buf->Idx;
 282         Queue->Avail->Idx ++;
 283         SHORTREL(&Queue->lAvailQueue);
 284
 285         // Notify
 286         __sync_synchronize();
 287         // TODO: Delay notifications
 288         tVirtIO_Dev     *dev = Buf->Dev;
 289         outw(dev->IOBase + VIRTIO_REG_QUEUENOTIFY, Buf->Queue);
 290         LOG("Notifying %p:%i", Buf->Dev, Buf->Queue);
 291 }
 292
 293 // Send a set of RO buffers
 294 tVirtIO_Buf *VirtIO_SendBuffers(tVirtIO_Dev *Dev, int QueueID, int nBufs, size_t Sizes[], const void *Ptrs[], void *Handle)
 295 {
 296         tVirtIO_Queue   *queue = Dev->Queues[QueueID];
 297         tVirtIO_Buf     *prev = NULL;
 298
 299         // Allocate buffers for each non-contiguious region
 300         // - these come from the queue's unallocated pool
 301         size_t  totalsize = 0;
 302         for( int i = nBufs; i --; )
 303         {
 304                 if( prev )
 305                         prev = VirtIO_int_AllocBufV(queue, Ptrs[i], Sizes[i], VRING_DESC_F_NEXT, prev->Idx);
 306                 else
 307                         prev = VirtIO_int_AllocBufV(queue, Ptrs[i], Sizes[i], 0, 0);
 308                 totalsize += Sizes[i];
 309         }
 310         LOG("Total size 0x%x", totalsize);
 311
 312         // Final buffer has the handle set to the passed handle
 313         // - all others get NULL
 314         prev->Handle = Handle;
 315
 316         // Add first to avaliable ring
 317         VirtIO_int_AddAvailBuf(queue, prev);
 318
 319         return prev;
 320 }
 321
 322 // Supply a single WO buffer for the device
 323 tVirtIO_Buf *VirtIO_ReceiveBuffer(tVirtIO_Dev *Dev, int QueueID, size_t Size, void *Ptr, void *Handle)
 324 {
 325         LOG("%p:%i - Add %p+0x%x for RX", Dev, QueueID, Ptr, Size);
 326         tVirtIO_Queue   *queue = Dev->Queues[QueueID];
 327         tVirtIO_Buf     *ret = VirtIO_int_AllocBufV(queue, Ptr, Size, VRING_DESC_F_WRITE, 0);
 328         ret->Handle = Handle;
 329
 330         VirtIO_int_AddAvailBuf(queue, ret);
 331         return ret;
 332 }
 333
 334 tVirtIO_Buf *VirtIO_PopBuffer(tVirtIO_Dev *Dev, int QueueID, size_t *Size, const void **Ptr)
 335 {
 336         ASSERTCR(QueueID, <, Dev->nQueues, NULL);
 337         tVirtIO_Queue   *queue = Dev->Queues[QueueID];
 338
 339         // TODO: Lock
 340         if( queue->NextUsedPop == queue->Used->Idx )
 341                 return NULL;
 342          int    qidx = queue->NextUsedPop;
 343         queue->NextUsedPop ++;
 344
 345          int    idx = queue->Used->Ring[qidx].ID;
 346         if( Size )
 347                 *Size = queue->Used->Ring[qidx].Len;
 348         if( Ptr ) {
 349                 *Ptr = queue->Buffers[idx].BufPtr;
 350                 ASSERTC(MM_GetPhysAddr(*Ptr), ==, queue->Entries[idx].Addr);
 351         }
 352         return &queue->Buffers[idx];
 353 }
 354
 355 const void *VirtIO_GetBufferPtr(tVirtIO_Buf *Buf, size_t *Size)
 356 {
 357         tVirtIO_Queue   *queue = Buf->Dev->Queues[Buf->Queue];
 358         if(Size)
 359                 *Size = queue->Entries[Buf->Idx].Len;
 360         return Buf->BufPtr;
 361 }
 362 tVirtIO_Dev *VirtIO_GetBufferDev(tVirtIO_Buf *Buf)
 363 {
 364         return Buf->Dev;
 365 }
 366
 367 void VirtIO_int_ReleaseQDesc(tVirtIO_Queue *Queue, Uint16 Index)
 368 {
 369         LOG("Release QDesc %p:%i into free pool",
 370                 Queue, Index);
 371         SHORTLOCK(&Queue->lFreeList);
 372         Queue->Entries[Index].Next = Queue->FirstUnused;
 373         Queue->Entries[Index].Flags = VRING_DESC_F_NEXT;
 374         Queue->FirstUnused = Index;
 375         SHORTREL(&Queue->lFreeList);
 376         Semaphore_Signal(&Queue->FreeDescsSem, 1);
 377 }
 378
 379 /**
 380  * \brief Releases all qdescs in the buffer to the free list
 381  */
 382 void VirtIO_ReleaseBuffer(tVirtIO_Buf *Buffer)
 383 {
 384          int    idx = Buffer->Idx;
 385         tVirtIO_Queue   *queue = Buffer->Dev->Queues[Buffer->Queue];
 386
 387         LOG("Releasing chain at %p:%i/%i",
 388                 Buffer->Dev, Buffer->Queue, Buffer->Idx);
 389
 390          int    has_next;
 391         do {
 392                 has_next = !!(queue->Entries[idx].Flags & VRING_DESC_F_NEXT);
 393                 int next_idx = queue->Entries[idx].Next;
 394                 ASSERTC(!has_next || next_idx,!=,idx);
 395
 396                 VirtIO_int_ReleaseQDesc(queue, idx);
 397
 398                 idx = next_idx;
 399         } while(has_next);
 400 }
 401
 402 void VirtIO_int_ProcessUsedList(tVirtIO_Dev *Dev, tVirtIO_Queue *Queue, int UsedIdx)
 403 {
 404         Uint16  qent = Queue->Used->Ring[UsedIdx].ID;
 405         size_t  len  = Queue->Used->Ring[UsedIdx].Len;
 406         LOG("QEnt %i (0x%x bytes) callback w/ Handle=%p",
 407                 qent, len, Queue->Buffers[qent].Handle);
 408         if( Queue->Callback )
 409                 Queue->Callback(Dev, qent, len, Queue->Buffers[qent].Handle);
 410
 411         if( !Queue->NoAutoRel )
 412         {
 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         }
 444 }
 445