1 files changed, 29 insertions, 2 deletions
diff --git a/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c b/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c
index c64b38fcfc..e779a31138 100644
--- a/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c
+++ b/MdeModulePkg/Bus/Pci/XhciPei/UsbHcMem.c
@@ -3,6 +3,7 @@ PEIM to produce gPeiUsb2HostControllerPpiGuid based on gPeiUsbControllerPpiGuid
 which is used to enable recovery function from USB Drivers.
 
 Copyright (c) 2014 - 2016, Intel Corporation. All rights reserved.<BR>
+Copyright (C) 2022 Advanced Micro Devices, Inc. All rights reserved.<BR>
 
 SPDX-License-Identifier: BSD-2-Clause-Patent
 
@@ -366,6 +367,32 @@ UsbHcInitMemPool (
 }
 
 /**
+  Unlink the memory block from the pool's list.
+
+  @param  Head           The block list head of the memory's pool.
+  @param  BlockToUnlink  The memory block to unlink.
+
+**/
+VOID
+UsbHcUnlinkMemBlock (
+  IN USBHC_MEM_BLOCK  *Head,
+  IN USBHC_MEM_BLOCK  *BlockToUnlink
+  )
+{
+  USBHC_MEM_BLOCK  *Block;
+
+  ASSERT ((Head != NULL) && (BlockToUnlink != NULL));
+
+  for (Block = Head; Block != NULL; Block = Block->Next) {
+    if (Block->Next == BlockToUnlink) {
+      Block->Next         = BlockToUnlink->Next;
+      BlockToUnlink->Next = NULL;
+      break;
+    }
+  }
+}
+
+/**
   Release the memory management pool.
 
   @param  Pool          The USB memory pool to free.
@@ -386,7 +413,7 @@ UsbHcFreeMemPool (
   // first block.
   //
   for (Block = Pool->Head->Next; Block != NULL; Block = Pool->Head->Next) {
-    // UsbHcUnlinkMemBlock (Pool->Head, Block);
+    UsbHcUnlinkMemBlock (Pool->Head, Block);
     UsbHcFreeMemBlock (Pool, Block);
   }
 
@@ -532,7 +559,7 @@ UsbHcFreeMem (
   // Release the current memory block if it is empty and not the head
   //
   if ((Block != Head) && UsbHcIsMemBlockEmpty (Block)) {
-    // UsbHcUnlinkMemBlock (Head, Block);
+    UsbHcUnlinkMemBlock (Head, Block);
     UsbHcFreeMemBlock (Pool, Block);
   }
 }