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 - 2014, 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)-1;
 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
 224   if(FIFO_IsFull(Self)) {                                                 // FIFO is full so can't add to it
 225     Count = 0;                                                              // Zero characters added
 226   }
 227   else {                                                                  // Otherwise, FIFO is not full...
 228     Count = MIN(Count, Self->FreeSpace(Self, AsElements));                  // Smaller of requested or available space
 229     SizeOfElement = Self->ElementSize;                                      // Size of Elements, in bytes
 230     Windex = Self->WriteIndex;                                              // Index of first writable slot in FIFO
 231
 232     ElemPtr = (uintptr_t)pElement;                                          // Addr. of element to add, as an integer
 233     QPtr    = (uintptr_t)Self->Queue + (SizeOfElement * Windex);            // Addr. in FIFO to write, as an integer
 234
 235     for(i = 0; i < Count; ++i) {                                            // For Count elements...
 236       (void)CopyMem((void *)QPtr, (const void *)ElemPtr, SizeOfElement);      // Copy an element into the FIFO
 237       Windex = (UINT32)ModuloIncrement(Windex, Self->NumElements);            // Increment the Write index, wrap if necessary
 238       if(Windex == 0) {                                                       // If the index wrapped
 239         QPtr = (uintptr_t)Self->Queue;                                          // Go to the beginning
 240       }
 241       else {
 242         QPtr += SizeOfElement;                                                // Otherwise, point to next in FIFO
 243       }
 244       ElemPtr += SizeOfElement;                                               // And also point to next Element to add
 245     }
 246     Self->WriteIndex = Windex;                                              // Finally, save the new Write Index
 247   }
 248   return Count;                                                           // Number of elements added to FIFO
 249 }
 250
 251 /** Read or copy elements from the FIFO.
 252
 253     This function allows one to read one or more elements, as specified by Count,
 254     from the FIFO.  Each element is of the size specified when the FIFO object
 255     was instantiated (FIFO.ElementSize).
 256
 257     pElement points to the destination of the first byte of the first element
 258     to be read. If multiple elements are to be read, the elements are expected
 259     to be organized as a packed array.
 260
 261     @param[in]    Self        Pointer to the FIFO instance.
 262     @param[out]   pElement    Pointer to where to store the element(s) read from the FIFO.
 263     @param[in]    Count       Number of elements to dequeue.
 264     @param[in]    Consume     If TRUE, consume read elements.  Otherwise, preserve.
 265
 266     @retval   0       The FIFO is empty.
 267     @retval   >=0     The number of elements read from the FIFO.
 268 **/
 269 static
 270 size_t
 271 EFIAPI
 272 FIFO_Dequeue (
 273   cFIFO    *Self,
 274   void     *pElement,
 275   size_t    Count,
 276   BOOLEAN   Consume
 277   )
 278 {
 279   UINTN         QPtr;
 280   UINT32        RDex;
 281   UINT32        SizeOfElement;
 282   UINT32        i;
 283
 284   assert(Self != NULL);
 285   assert(pElement != NULL);
 286
 287   if(FIFO_IsEmpty(Self)) {
 288     Count = 0;
 289   }
 290   else {
 291     RDex          = Self->ReadIndex;                                  // Get this FIFO's Read Index
 292     SizeOfElement = Self->ElementSize;                                // Get size of this FIFO's elements
 293     Count         = MIN(Count, Self->Count(Self, AsElements));        // Lesser of requested or actual
 294
 295     QPtr = (UINTN)Self->Queue + (RDex * SizeOfElement);           // Point to Read location in FIFO
 296     for(i = 0; i < Count; ++i) {                                      // Iterate Count times...
 297       (void)CopyMem(pElement, (const void *)QPtr, SizeOfElement);   // Copy element from FIFO to caller's buffer
 298       RDex = (UINT32)ModuloIncrement(RDex, Self->NumElements);          // Increment Read Index
 299       if(RDex == 0) {                                                   // If the index wrapped
 300         QPtr = (UINTN)Self->Queue;                                        // Point back to beginning of data
 301       }
 302       else {                                                            // Otherwise
 303         QPtr += SizeOfElement;                                        // Point to the next element in FIFO
 304       }
 305       pElement = (char*)pElement + SizeOfElement;                   // Point to next element in caller's buffer
 306     }                                                                 // Iterate: for loop
 307     if(Consume) {                                                     // If caller requests data consumption
 308       Self->ReadIndex = RDex;                                           // Set FIFO's Read Index to new Index
 309     }
 310   }
 311   return Count;                                                     // Return number of elements actually read
 312 }
 313
 314 /** Read elements from the FIFO.
 315
 316     Read the specified number of elements from the FIFO, removing each element read.
 317     The number of elements actually read from the FIFO is returned.  This number can differ
 318     from the Count requested if more elements are requested than are in the FIFO.
 319
 320     @param[in]    Self        Pointer to the FIFO instance.
 321     @param[out]   pElement    Pointer to where to store the element read from the FIFO.
 322     @param[in]    Count       Number of elements to dequeue.
 323
 324     @retval   0       The FIFO is empty.
 325     @retval   >=0     The number of elements read from the FIFO.
 326 **/
 327 static
 328 size_t
 329 EFIAPI
 330 FIFO_Read (
 331   cFIFO    *Self,
 332   void     *pElement,
 333   size_t    Count
 334   )
 335 {
 336   return FIFO_Dequeue(Self, pElement, Count, TRUE);
 337 }
 338
 339 /** Make a copy of the FIFO's data.
 340     The contents of the FIFO is copied out and linearized without affecting the
 341     FIFO contents.  This function is idempotent.
 342
 343     @param[in]    Self        Pointer to the FIFO instance.
 344     @param[out]   pElement    Pointer to where to store the elements copied from the FIFO.
 345     @param[in]    Count       Number of elements to copy.
 346
 347     @retval   0       The FIFO is empty.
 348     @retval   >=0     The number of elements copied from the FIFO.
 349 **/
 350 static
 351 size_t
 352 EFIAPI
 353 FIFO_Copy (
 354   cFIFO    *Self,
 355   void     *pElement,
 356   size_t    Count
 357   )
 358 {
 359   return FIFO_Dequeue(Self, pElement, Count, FALSE);
 360 }
 361
 362 /** Get the FIFO's current Read Index.
 363
 364     @param[in]    Self      Pointer to the FIFO instance.
 365 **/
 366 static
 367 UINT32
 368 EFIAPI
 369 FIFO_GetRDex (
 370   cFIFO *Self
 371 )
 372 {
 373   assert(Self != NULL);
 374
 375   return Self->ReadIndex;
 376 }
 377
 378 /** Get the FIFO's current Write Index.
 379
 380     @param[in]    Self      Pointer to the FIFO instance.
 381
 382     @return   The current value of the FIFO's WriteIndex member is returned.
 383 **/
 384 static
 385 UINT32
 386 EFIAPI
 387 FIFO_GetWDex (
 388   cFIFO *Self
 389 )
 390 {
 391   assert(Self != NULL);
 392
 393   return Self->WriteIndex;
 394 }
 395
 396 /** Cleanly delete a FIFO instance.
 397
 398     @param[in]    Self              Pointer to the FIFO instance.
 399 **/
 400 static
 401 void
 402 EFIAPI
 403 FIFO_Delete (
 404   cFIFO *Self
 405   )
 406 {
 407   assert(Self != NULL);
 408
 409   if(Self->Queue != NULL) {
 410     FreePool(Self->Queue);
 411     Self->Queue = NULL;     // Zombie catcher
 412   }
 413   FreePool(Self);
 414 }
 415
 416 /** Empty the FIFO, discarding up to NumToFlush elements.
 417
 418     @param[in]    Self              Pointer to the FIFO instance.
 419     @param[in]    NumToFlush        Number of elements to flush from the FIFO.
 420                                     If larger than the number of elements in the
 421                                     FIFO, the FIFO is emptied.
 422
 423     @return     Returns the number of elements remaining in the FIFO after the flush.
 424 **/
 425 static
 426 size_t
 427 EFIAPI
 428 FIFO_Flush (
 429   cFIFO  *Self,
 430   size_t  NumToFlush
 431   )
 432 {
 433   size_t  NumInQ;
 434   size_t  Remainder;
 435
 436   assert(Self != NULL);
 437
 438   NumInQ = FIFO_NumInQueue(Self, AsElements);
 439   if(NumToFlush >= NumInQ) {
 440     Self->ReadIndex   = 0;
 441     Self->WriteIndex  = 0;
 442     Remainder = 0;
 443   }
 444   else {
 445     Remainder = FIFO_Reduce(Self, NumToFlush);
 446   }
 447   return Remainder;
 448 }
 449
 450 /** Remove the most recently added element from the FIFO.
 451
 452     @param[in]    Self              Pointer to the FIFO instance.
 453
 454     @return     Returns the number of elements remaining in the FIFO.
 455 **/
 456 static
 457 size_t
 458 EFIAPI
 459 FIFO_Truncate (
 460   cFIFO  *Self
 461   )
 462 {
 463   size_t  Remainder;
 464
 465   assert(Self != NULL);
 466
 467   Remainder = FIFO_NumInQueue(Self, AsElements);
 468   if(Remainder > 0) {
 469     Self->WriteIndex = (UINT32)ModuloDecrement(Self->WriteIndex, Self->NumElements);
 470     --Remainder;
 471   }
 472   return Remainder;
 473 }
 474
 475 /** Construct a new instance of a FIFO Queue.
 476
 477     @param[in]    NumElements   Number of elements to be contained in the new FIFO.
 478     @param[in]    ElementSize   Size, in bytes, of an element.
 479
 480     @retval   NULL      Unable to create the instance.
 481     @retval   NonNULL   Pointer to the new FIFO instance.
 482 **/
 483 cFIFO *
 484 EFIAPI
 485 New_cFIFO(
 486   UINT32    NumElements,
 487   size_t    ElementSize
 488   )
 489 {
 490   cFIFO        *FIFO;
 491   UINT8        *Queue;
 492
 493   FIFO = NULL;
 494   if((NumElements > 2) && (ElementSize > 0)) {
 495     FIFO = (cFIFO *)AllocatePool(sizeof(cFIFO));
 496     if(FIFO != NULL) {
 497       Queue = (UINT8 *)AllocateZeroPool(NumElements * ElementSize);
 498       if(Queue != NULL) {
 499         FIFO->Write       = FIFO_Enqueue;
 500         FIFO->Read        = FIFO_Read;
 501         FIFO->Copy        = FIFO_Copy;
 502         FIFO->IsEmpty     = FIFO_IsEmpty;
 503         FIFO->IsFull      = FIFO_IsFull;
 504         FIFO->Count       = FIFO_NumInQueue;
 505         FIFO->FreeSpace   = FIFO_FreeSpace;
 506         FIFO->Flush       = FIFO_Flush;
 507         FIFO->Truncate    = FIFO_Truncate;
 508         FIFO->Delete      = FIFO_Delete;
 509         FIFO->GetRDex     = FIFO_GetRDex;
 510         FIFO->GetWDex     = FIFO_GetWDex;
 511
 512         FIFO->Queue       = Queue;
 513         FIFO->ElementSize = (UINT32)ElementSize;
 514         FIFO->NumElements = (UINT32)NumElements;
 515         FIFO->ReadIndex   = 0;
 516         FIFO->WriteIndex  = 0;
 517       }
 518       else {
 519         FreePool(FIFO);
 520         FIFO = NULL;
 521       }
 522     }
 523   }
 524   return FIFO;
 525 }