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
 224   if(FIFO_IsFull(Self)) {
 225     Count = 0;
 226   }
 227   else {
 228     Count = MIN(Count, Self->FreeSpace(Self, AsElements));
 229     SizeOfElement = Self->ElementSize;
 230     Windex = Self->WriteIndex;
 231
 232     ElemPtr = (uintptr_t)pElement;
 233
 234     QPtr   = (uintptr_t)Self->Queue + (SizeOfElement * Windex);
 235     for(i = 0; i < Count; ++i) {
 236       (void)CopyMem((void *)QPtr, (const void *)ElemPtr, SizeOfElement);
 237       Windex = (UINT32)ModuloIncrement(Windex, Self->NumElements);
 238       if(Windex == 0) {   // If the index wrapped
 239         QPtr = (uintptr_t)Self->Queue;
 240       }
 241       else {
 242         QPtr += SizeOfElement;
 243       }
 244       ElemPtr += SizeOfElement;
 245     }
 246     (void)ZeroMem((void*)QPtr, SizeOfElement);
 247     Self->WriteIndex = Windex;
 248   }
 249   return Count;
 250 }
 251
 252 /** Read or copy elements from the FIFO.
 253
 254     This function allows one to read one or more elements, as specified by Count,
 255     from the FIFO.  Each element is of the size specified when the FIFO object
 256     was instantiated (FIFO.ElementSize).
 257
 258     pElement points to the destination of the first byte of the first element
 259     to be read. If multiple elements are to be read, the elements are expected
 260     to be organized as a packed array.
 261
 262     @param[in]    Self        Pointer to the FIFO instance.
 263     @param[out]   pElement    Pointer to where to store the element(s) read from the FIFO.
 264     @param[in]    Count       Number of elements to dequeue.
 265     @param[in]    Consume     If TRUE, consume read elements.  Otherwise, preserve.
 266
 267     @retval   0       The FIFO is empty.
 268     @retval   >=0     The number of elements read from the FIFO.
 269 **/
 270 static
 271 size_t
 272 EFIAPI
 273 FIFO_Dequeue (
 274   cFIFO    *Self,
 275   void     *pElement,
 276   size_t    Count,
 277   BOOLEAN   Consume
 278   )
 279 {
 280   UINTN         ElemPtr;
 281   UINTN         QPtr;
 282   UINT32        RDex;
 283   UINT32        SizeOfElement;
 284   UINT32        i;
 285
 286   assert(Self != NULL);
 287   assert(pElement != NULL);
 288   assert(Count != 0);
 289
 290   if(FIFO_IsEmpty(Self)) {
 291     Count = 0;
 292   }
 293   else {
 294     RDex          = Self->ReadIndex;
 295     SizeOfElement = Self->ElementSize;
 296     ElemPtr       = (UINTN)pElement;
 297     Count         = MIN(Count, Self->Count(Self, AsElements));
 298
 299     QPtr = (UINTN)Self->Queue + (RDex * Self->ElementSize);
 300     for(i = 0; i < Count; ++i) {
 301       (void)CopyMem((void *)ElemPtr, (const void *)QPtr, Self->ElementSize);
 302       RDex = (UINT32)ModuloIncrement(RDex, Self->NumElements);
 303       if(RDex == 0) {   // If the index wrapped
 304         QPtr = (UINTN)Self->Queue;
 305       }
 306       else {
 307         QPtr += Self->ElementSize;
 308       }
 309       ElemPtr += Self->ElementSize;
 310     }
 311     if(Consume) {
 312       Self->ReadIndex = RDex;
 313     }
 314   }
 315   return Count;
 316 }
 317
 318 /** Read elements from 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.
 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_FreeSpace(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 = Self->Count(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 }