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 } 526