StdLib/LibC/Containers/Queues/Fifo.c

   1 /** @file
   2   Class for arbitrary sized FIFO queues.
   3
   4   The FIFO is empty if both the Read and Write indexes are equal.
   5   The FIFO is full if the next write would make the Read and Write indexes equal.
   6
   7   Member variable NumElements is the maximum number of elements that can be
   8   contained in the FIFO.
   9     If NumElements is ZERO, there is an error.
  10     NumElements should be in the range 1:N.
  11
  12   Members WriteIndex and ReadIndex are indexes into the array implementing the
  13   FIFO.  They should be in the range 0:(NumElements - 1).
  14
  15   One element of the FIFO is always reserved as the "terminator" element.  Thus,
  16   the capacity of a FIFO is actually NumElements-1.
  17
  18   Copyright (c) 2012, Intel Corporation. All rights reserved.<BR>
  19   This program and the accompanying materials are licensed and made available
  20   under the terms and conditions of the BSD License which accompanies this
  21   distribution.  The full text of the license may be found at
  22   http://opensource.org/licenses/bsd-license.php.
  23
  24   THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  25   WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
  26 **/
  27 #include  <Uefi.h>
  28 #include  <Library/BaseLib.h>
  29 #include  <Library/BaseMemoryLib.h>
  30 #include  <Library/MemoryAllocationLib.h>
  31
  32 #include  <LibConfig.h>
  33
  34 #include  <assert.h>
  35 #include  <errno.h>
  36 #include  <stdlib.h>
  37 #include  <stdint.h>
  38 #include  <wchar.h>
  39 #include  <Containers/Fifo.h>
  40
  41 /** Determine number of items available to read from the FIFO.
  42
  43     The number of items are either the number of bytes, or the number of elements
  44     depending upon the value of the As enumerator.
  45
  46     @param[in]    Self      Pointer to the FIFO instance.
  47     @param[in]    As        An enumeration variable whose value determines whether the
  48                             returned value is the number of bytes or the number of elements
  49                             currently contained by the FIFO.
  50
  51     @retval   0       The FIFO is empty.
  52     @retval   >=0     The number of items contained by the FIFO.
  53 **/
  54 static
  55 size_t
  56 EFIAPI
  57 FIFO_NumInQueue (
  58   cFIFO        *Self,
  59   FIFO_ElemBytes    As
  60 )
  61 {
  62   size_t    Count;
  63
  64   if(Self->ReadIndex <= Self->WriteIndex) {
  65     Count = Self->WriteIndex - Self->ReadIndex;
  66   }
  67   else {
  68     Count = Self->NumElements - (Self->ReadIndex - Self->WriteIndex);
  69   }
  70   if(As == AsBytes) {
  71     Count *= Self->ElementSize;
  72   }
  73   return Count;
  74 }
  75
  76 /** Determine amount of free space in the FIFO that can be written into.
  77
  78     The number of items are either the number of bytes, or the number of elements
  79     depending upon the value of the As enumerator.
  80
  81     @param[in]    Self      Pointer to the FIFO instance.
  82     @param[in]    As        An enumeration variable whose value determines whether the
  83                             returned value is the number of bytes or the number of elements
  84                             currently available in the FIFO.
  85
  86     @retval   0       The FIFO is full.
  87     @retval   >=0     The number of items which can be accepted by the FIFO.
  88 **/
  89 static
  90 size_t
  91 EFIAPI
  92 FIFO_FreeSpace (
  93   cFIFO            *Self,
  94   FIFO_ElemBytes    As
  95 )
  96 {
  97   size_t    Count;
  98   UINT32    RDex;
  99   UINT32    WDex;
 100
 101   RDex = Self->ReadIndex;
 102   WDex = Self->WriteIndex;
 103
 104   if(RDex <= WDex) {
 105     Count = Self->NumElements - ((WDex - RDex) - 1);
 106   }
 107   else {
 108     Count = (RDex - WDex);
 109   }
 110   if(As == AsBytes) {
 111     Count *= Self->ElementSize;
 112   }
 113   return Count;
 114 }
 115
 116 /** Reduce the FIFO contents by NumElem elements.
 117
 118     @param[in]    Self      Pointer to the FIFO instance.
 119     @param[in]    NumElem   Number of elements to delete from the FIFO.
 120
 121     @retval   0   FIFO is now empty.
 122     @retval   N>0 There are still N elements in the FIFO.
 123     @retval   -1  There are fewer than NumElem elements in the FIFO.
 124 **/
 125 static
 126 ssize_t
 127 FIFO_Reduce (
 128   cFIFO    *Self,
 129   size_t    NumElem
 130   )
 131 {
 132   size_t    QCount;
 133   ssize_t   RetVal;
 134
 135   assert(Self != NULL);
 136
 137   QCount = FIFO_NumInQueue(Self, AsElements);
 138   if(NumElem > QCount) {
 139     RetVal = -1;
 140     errno   = EINVAL;
 141   }
 142   else {
 143     RetVal = (ssize_t)ModuloAdd(Self->ReadIndex, (UINT32)NumElem, Self->NumElements);
 144     Self->ReadIndex = (UINT32)RetVal;
 145
 146     RetVal = (ssize_t)(QCount - NumElem);
 147   }
 148   return RetVal;
 149 }
 150
 151 /** Test whether the FIFO is empty.
 152
 153     @param[in]    Self      Pointer to the FIFO instance.
 154
 155     @retval   TRUE    The FIFO is empty.
 156     @retval   FALSE   There is data in the FIFO.
 157 **/
 158 static
 159 BOOLEAN
 160 EFIAPI
 161 FIFO_IsEmpty (
 162   cFIFO *Self
 163   )
 164 {
 165   assert(Self != NULL);
 166
 167   return (BOOLEAN)(Self->WriteIndex == Self->ReadIndex);
 168 }
 169
 170 /** Test whether the FIFO is full.
 171
 172     @param[in]    Self      Pointer to the FIFO instance.
 173
 174     @retval   TRUE    The FIFO is full.
 175     @retval   FALSE   There is free space in the FIFO.
 176 **/
 177 static
 178 BOOLEAN
 179 EFIAPI
 180 FIFO_IsFull (
 181   cFIFO *Self
 182   )
 183 {
 184   assert(Self != NULL);
 185
 186   return (BOOLEAN)(ModuloIncrement(Self->WriteIndex, Self->NumElements) == (INT32)Self->ReadIndex);
 187 }
 188
 189 /** Add one or more elements to the FIFO.
 190
 191     This function allows one to add one or more elements, as specified by Count,
 192     to the FIFO.  Each element is of the size specified when the FIFO object
 193     was instantiated (FIFO.ElementSize).
 194
 195     pElement points to the first byte of the first element to be added.
 196     If multiple elements are to be added, the elements are expected to be
 197     organized as a packed array.
 198
 199     @param[in]    Self        Pointer to the FIFO instance.
 200     @param[in]    pElement    Pointer to the element(s) to enqueue (add).
 201     @param[in]    Count       Number of elements to add.
 202
 203     @retval   0       The FIFO is full.
 204     @retval   >=0     The number of elements added to the FIFO.
 205 **/
 206 static
 207 size_t
 208 EFIAPI
 209 FIFO_Enqueue (
 210   cFIFO        *Self,
 211   const void   *pElement,
 212   size_t        Count
 213   )
 214 {
 215   uintptr_t     ElemPtr;
 216   uintptr_t     QPtr;
 217   size_t        i;
 218   UINT32        SizeOfElement;
 219   UINT32        Windex;
 220
 221   assert(Self != NULL);
 222   assert(pElement != NULL);
 223   assert(Count >= 0);
 224
 225   if(FIFO_IsFull(Self)) {
 226     Count = 0;
 227   }
 228   else {
 229     Count = MIN(Count, Self->FreeSpace(Self, AsElements));
 230     SizeOfElement = Self->ElementSize;
 231     Windex = Self->WriteIndex;
 232
 233     ElemPtr = (uintptr_t)pElement;
 234
 235     QPtr   = (uintptr_t)Self->Queue + (SizeOfElement * Windex);
 236     for(i = 0; i < Count; ++i) {
 237       (void)CopyMem((void *)QPtr, (const void *)ElemPtr, SizeOfElement);
 238       Windex = (UINT32)ModuloIncrement(Windex, Self->NumElements);
 239       if(Windex == 0) {   // If the index wrapped
 240         QPtr = (uintptr_t)Self->Queue;
 241       }
 242       else {
 243         QPtr += SizeOfElement;
 244       }
 245       ElemPtr += SizeOfElement;
 246     }
 247     (void)ZeroMem((void*)QPtr, SizeOfElement);
 248     Self->WriteIndex = Windex;
 249   }
 250   return Count;
 251 }
 252
 253 /** Read or copy elements from the FIFO.
 254
 255     This function allows one to read one or more elements, as specified by Count,
 256     from the FIFO.  Each element is of the size specified when the FIFO object
 257     was instantiated (FIFO.ElementSize).
 258
 259     pElement points to the destination of the first byte of the first element
 260     to be read. If multiple elements are to be read, the elements are expected
 261     to be organized as a packed array.
 262
 263     @param[in]    Self        Pointer to the FIFO instance.
 264     @param[out]   pElement    Pointer to where to store the element(s) read from the FIFO.
 265     @param[in]    Count       Number of elements to dequeue.
 266     @param[in]    Consume     If TRUE, consume read elements.  Otherwise, preserve.
 267
 268     @retval   0       The FIFO is empty.
 269     @retval   >=0     The number of elements read from the FIFO.
 270 **/
 271 static
 272 size_t
 273 EFIAPI
 274 FIFO_Dequeue (
 275   cFIFO    *Self,
 276   void     *pElement,
 277   size_t    Count,
 278   BOOLEAN   Consume
 279   )
 280 {
 281   UINTN         ElemPtr;
 282   UINTN         QPtr;
 283   UINT32        RDex;
 284   UINT32        SizeOfElement;
 285   UINT32        i;
 286
 287   assert(Self != NULL);
 288   assert(pElement != NULL);
 289   assert(Count != 0);
 290
 291   if(FIFO_IsEmpty(Self)) {
 292     Count = 0;
 293   }
 294   else {
 295     RDex          = Self->ReadIndex;
 296     SizeOfElement = Self->ElementSize;
 297     ElemPtr       = (UINTN)pElement;
 298     Count         = MIN(Count, Self->Count(Self, AsElements));
 299
 300     QPtr = (UINTN)Self->Queue + (RDex * Self->ElementSize);
 301     for(i = 0; i < Count; ++i) {
 302       (void)CopyMem((void *)ElemPtr, (const void *)QPtr, Self->ElementSize);
 303       RDex = (UINT32)ModuloIncrement(RDex, Self->NumElements);
 304       if(RDex == 0) {   // If the index wrapped
 305         QPtr = (UINTN)Self->Queue;
 306       }
 307       else {
 308         QPtr += Self->ElementSize;
 309       }
 310       ElemPtr += Self->ElementSize;
 311     }
 312     if(Consume) {
 313       Self->ReadIndex = RDex;
 314     }
 315   }
 316   return Count;
 317 }
 318
 319 /** Read elements from the FIFO.
 320
 321     @param[in]    Self        Pointer to the FIFO instance.
 322     @param[out]   pElement    Pointer to where to store the element read from the FIFO.
 323     @param[in]    Count       Number of elements to dequeue.
 324
 325     @retval   0       The FIFO is empty.
 326     @retval   >=0     The number of elements read from the FIFO.
 327 **/
 328 static
 329 size_t
 330 EFIAPI
 331 FIFO_Read (
 332   cFIFO    *Self,
 333   void     *pElement,
 334   size_t    Count
 335   )
 336 {
 337   return FIFO_Dequeue(Self, pElement, Count, TRUE);
 338 }
 339
 340 /** Make a copy of the FIFO's data.
 341     The contents of the FIFO is copied out and linearized without affecting the
 342     FIFO contents.
 343
 344     @param[in]    Self        Pointer to the FIFO instance.
 345     @param[out]   pElement    Pointer to where to store the elements copied from the FIFO.
 346     @param[in]    Count       Number of elements to copy.
 347
 348     @retval   0       The FIFO is empty.
 349     @retval   >=0     The number of elements copied from the FIFO.
 350 **/
 351 static
 352 size_t
 353 EFIAPI
 354 FIFO_Copy (
 355   cFIFO    *Self,
 356   void     *pElement,
 357   size_t    Count
 358   )
 359 {
 360   return FIFO_Dequeue(Self, pElement, Count, FALSE);
 361 }
 362
 363 /** Get the FIFO's current Read Index.
 364
 365     @param[in]    Self      Pointer to the FIFO instance.
 366 **/
 367 static
 368 UINT32
 369 EFIAPI
 370 FIFO_GetRDex (
 371   cFIFO *Self
 372 )
 373 {
 374   assert(Self != NULL);
 375
 376   return Self->ReadIndex;
 377 }
 378
 379 /** Get the FIFO's current Write Index.
 380
 381     @param[in]    Self      Pointer to the FIFO instance.
 382
 383     @return   The current value of the FIFO's WriteIndex member is returned.
 384 **/
 385 static
 386 UINT32
 387 EFIAPI
 388 FIFO_GetWDex (
 389   cFIFO *Self
 390 )
 391 {
 392   assert(Self != NULL);
 393
 394   return Self->WriteIndex;
 395 }
 396
 397 /** Cleanly delete a FIFO instance.
 398
 399     @param[in]    Self              Pointer to the FIFO instance.
 400 **/
 401 static
 402 void
 403 EFIAPI
 404 FIFO_Delete (
 405   cFIFO *Self
 406   )
 407 {
 408   assert(Self != NULL);
 409
 410   if(Self->Queue != NULL) {
 411     FreePool(Self->Queue);
 412     Self->Queue = NULL;     // Zombie catcher
 413   }
 414   FreePool(Self);
 415 }
 416
 417 /** Empty the FIFO, discarding up to NumToFlush elements.
 418
 419     @param[in]    Self              Pointer to the FIFO instance.
 420     @param[in]    NumToFlush        Number of elements to flush from the FIFO.
 421                                     If larger than the number of elements in the
 422                                     FIFO, the FIFO is emptied.
 423
 424     @return     Returns the number of elements remaining in the FIFO after the flush.
 425 **/
 426 static
 427 size_t
 428 EFIAPI
 429 FIFO_Flush (
 430   cFIFO  *Self,
 431   size_t  NumToFlush
 432   )
 433 {
 434   size_t  NumInQ;
 435   size_t  Remainder;
 436
 437   assert(Self != NULL);
 438
 439   NumInQ = FIFO_FreeSpace(Self, AsElements);
 440   if(NumToFlush >= NumInQ) {
 441     Self->ReadIndex   = 0;
 442     Self->WriteIndex  = 0;
 443     Remainder = 0;
 444   }
 445   else {
 446     Remainder = FIFO_Reduce(Self, NumToFlush);
 447   }
 448   return Remainder;
 449 }
 450
 451 /** Remove the most recently added element from the FIFO.
 452
 453     @param[in]    Self              Pointer to the FIFO instance.
 454
 455     @return     Returns the number of elements remaining in the FIFO.
 456 **/
 457 static
 458 size_t
 459 EFIAPI
 460 FIFO_Truncate (
 461   cFIFO  *Self
 462   )
 463 {
 464   size_t  Remainder;
 465
 466   assert(Self != NULL);
 467
 468   Remainder = Self->Count(Self, AsElements);
 469   if(Remainder > 0) {
 470     Self->WriteIndex = (UINT32)ModuloDecrement(Self->WriteIndex, Self->NumElements);
 471     --Remainder;
 472   }
 473   return Remainder;
 474 }
 475
 476 /** Construct a new instance of a FIFO Queue.
 477
 478     @param[in]    NumElements   Number of elements to be contained in the new FIFO.
 479     @param[in]    ElementSize   Size, in bytes, of an element.
 480
 481     @retval   NULL      Unable to create the instance.
 482     @retval   NonNULL   Pointer to the new FIFO instance.
 483 **/
 484 cFIFO *
 485 EFIAPI
 486 New_cFIFO(
 487   UINT32    NumElements,
 488   size_t    ElementSize
 489   )
 490 {
 491   cFIFO        *FIFO;
 492   UINT8        *Queue;
 493
 494   FIFO = NULL;
 495   if((NumElements > 2) && (ElementSize > 0)) {
 496     FIFO = (cFIFO *)AllocatePool(sizeof(cFIFO));
 497     if(FIFO != NULL) {
 498       Queue = (UINT8 *)AllocateZeroPool(NumElements * ElementSize);
 499       if(Queue != NULL) {
 500         FIFO->Write       = FIFO_Enqueue;
 501         FIFO->Read        = FIFO_Read;
 502         FIFO->Copy        = FIFO_Copy;
 503         FIFO->IsEmpty     = FIFO_IsEmpty;
 504         FIFO->IsFull      = FIFO_IsFull;
 505         FIFO->Count       = FIFO_NumInQueue;
 506         FIFO->FreeSpace   = FIFO_FreeSpace;
 507         FIFO->Flush       = FIFO_Flush;
 508         FIFO->Truncate    = FIFO_Truncate;
 509         FIFO->Delete      = FIFO_Delete;
 510         FIFO->GetRDex     = FIFO_GetRDex;
 511         FIFO->GetWDex     = FIFO_GetWDex;
 512
 513         FIFO->Queue       = Queue;
 514         FIFO->ElementSize = (UINT32)ElementSize;
 515         FIFO->NumElements = (UINT32)NumElements;
 516         FIFO->ReadIndex   = 0;
 517         FIFO->WriteIndex  = 0;
 518       }
 519       else {
 520         FreePool(FIFO);
 521         FIFO = NULL;
 522       }
 523     }
 524   }
 525   return FIFO;
 526 }