diff --git a/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleLib.c b/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleLib.c
index 2bb6ac887f..71e05bd557 100644
--- a/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleLib.c
+++ b/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleLib.c
@@ -47,8 +47,6 @@
 #include <Protocol/FirmwareManagement.h>
 #include <Protocol/DevicePath.h>
 
-BOOLEAN            mAreAllImagesProcessed;
-
 EFI_SYSTEM_RESOURCE_TABLE *mEsrtTable              = NULL;
 BOOLEAN                   mIsVirtualAddrConverted  = FALSE;
 BOOLEAN                   mDxeCapsuleLibEndOfDxe   = FALSE;
@@ -61,23 +59,6 @@ InitCapsuleVariable (
   VOID
   );
 
-/**
-  Check if this FMP capsule is processed.
-
-  @param[in] CapsuleHeader  The capsule image header
-  @param[in] PayloadIndex   FMP payload index
-  @param[in] ImageHeader    FMP image header
-
-  @retval TRUE  This FMP capsule is processed.
-  @retval FALSE This FMP capsule is not processed.
-**/
-BOOLEAN
-IsFmpCapsuleProcessed (
-  IN EFI_CAPSULE_HEADER                            *CapsuleHeader,
-  IN UINTN                                         PayloadIndex,
-  IN EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER  *ImageHeader
-  );
-
 /**
   Record capsule status variable.
 
@@ -768,6 +749,520 @@ DumpFmpCapsule (
   }
 }
 
+/**
+  Dump all FMP information.
+**/
+VOID
+DumpAllFmpInfo (
+  VOID
+  )
+{
+  EFI_STATUS                                    Status;
+  EFI_HANDLE                                    *HandleBuffer;
+  UINTN                                         NumberOfHandles;
+  EFI_FIRMWARE_MANAGEMENT_PROTOCOL              *Fmp;
+  UINTN                                         Index;
+  UINTN                                         ImageInfoSize;
+  EFI_FIRMWARE_IMAGE_DESCRIPTOR                 *FmpImageInfoBuf;
+  UINT32                                        FmpImageInfoDescriptorVer;
+  UINT8                                         FmpImageInfoCount;
+  UINTN                                         DescriptorSize;
+  UINT32                                        PackageVersion;
+  CHAR16                                        *PackageVersionName;
+
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareManagementProtocolGuid,
+                  NULL,
+                  &NumberOfHandles,
+                  &HandleBuffer
+                  );
+  if (EFI_ERROR(Status)) {
+    return ;
+  }
+
+  for (Index = 0; Index < NumberOfHandles; Index++) {
+    Status = gBS->HandleProtocol(
+                    HandleBuffer[Index],
+                    &gEfiFirmwareManagementProtocolGuid,
+                    (VOID **)&Fmp
+                    );
+    if (EFI_ERROR(Status)) {
+      continue;
+    }
+
+    ImageInfoSize = 0;
+    Status = Fmp->GetImageInfo (
+                    Fmp,
+                    &ImageInfoSize,
+                    NULL,
+                    NULL,
+                    NULL,
+                    NULL,
+                    NULL,
+                    NULL
+                    );
+    if (Status != EFI_BUFFER_TOO_SMALL) {
+      continue;
+    }
+
+    FmpImageInfoBuf = AllocateZeroPool (ImageInfoSize);
+    if (FmpImageInfoBuf == NULL) {
+      continue;
+    }
+
+    PackageVersionName = NULL;
+    Status = Fmp->GetImageInfo (
+                    Fmp,
+                    &ImageInfoSize,               // ImageInfoSize
+                    FmpImageInfoBuf,              // ImageInfo
+                    &FmpImageInfoDescriptorVer,   // DescriptorVersion
+                    &FmpImageInfoCount,           // DescriptorCount
+                    &DescriptorSize,              // DescriptorSize
+                    &PackageVersion,              // PackageVersion
+                    &PackageVersionName           // PackageVersionName
+                    );
+    if (EFI_ERROR(Status)) {
+      FreePool(FmpImageInfoBuf);
+      continue;
+    }
+
+    DEBUG((DEBUG_INFO, "FMP (%d) ImageInfo:\n", Index));
+    DumpFmpImageInfo(
+      ImageInfoSize,               // ImageInfoSize
+      FmpImageInfoBuf,             // ImageInfo
+      FmpImageInfoDescriptorVer,   // DescriptorVersion
+      FmpImageInfoCount,           // DescriptorCount
+      DescriptorSize,              // DescriptorSize
+      PackageVersion,              // PackageVersion
+      PackageVersionName           // PackageVersionName
+      );
+
+    if (PackageVersionName != NULL) {
+      FreePool(PackageVersionName);
+    }
+
+    FreePool(FmpImageInfoBuf);
+  }
+
+  return ;
+}
+
+/**
+  Get FMP handle by ImageTypeId and HardwareInstance.
+
+  @param[in]     UpdateImageTypeId       Used to identify device firmware targeted by this update.
+  @param[in]     UpdateHardwareInstance  The HardwareInstance to target with this update.
+  @param[in,out] NoHandles               The number of handles returned in Buffer.
+  @param[out]    Buffer[out]             A pointer to the buffer to return the requested array of handles.
+
+  @retval EFI_SUCCESS            The array of handles was returned in Buffer, and the number of
+                                 handles in Buffer was returned in NoHandles.
+  @retval EFI_NOT_FOUND          No handles match the search.
+  @retval EFI_OUT_OF_RESOURCES   There is not enough pool memory to store the matching results.
+**/
+EFI_STATUS
+GetFmpHandleBufferByType (
+  IN     EFI_GUID                     *UpdateImageTypeId,
+  IN     UINT64                       UpdateHardwareInstance,
+  IN OUT UINTN                        *NoHandles,
+  OUT    EFI_HANDLE                   **Buffer
+  )
+{
+  EFI_STATUS                                    Status;
+  EFI_HANDLE                                    *HandleBuffer;
+  UINTN                                         NumberOfHandles;
+  EFI_HANDLE                                    *MatchedHandleBuffer;
+  UINTN                                         MatchedNumberOfHandles;
+  EFI_FIRMWARE_MANAGEMENT_PROTOCOL              *Fmp;
+  UINTN                                         Index;
+  UINTN                                         ImageInfoSize;
+  EFI_FIRMWARE_IMAGE_DESCRIPTOR                 *FmpImageInfoBuf;
+  UINT32                                        FmpImageInfoDescriptorVer;
+  UINT8                                         FmpImageInfoCount;
+  UINTN                                         DescriptorSize;
+  UINT32                                        PackageVersion;
+  CHAR16                                        *PackageVersionName;
+  UINTN                                         Index2;
+  EFI_FIRMWARE_IMAGE_DESCRIPTOR                 *TempFmpImageInfo;
+
+  *NoHandles = 0;
+  *Buffer = NULL;
+
+  Status = gBS->LocateHandleBuffer (
+                  ByProtocol,
+                  &gEfiFirmwareManagementProtocolGuid,
+                  NULL,
+                  &NumberOfHandles,
+                  &HandleBuffer
+                  );
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  MatchedNumberOfHandles = 0;
+  MatchedHandleBuffer = AllocateZeroPool (sizeof(EFI_HANDLE) * NumberOfHandles);
+  if (MatchedHandleBuffer == NULL) {
+    FreePool (HandleBuffer);
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  for (Index = 0; Index < NumberOfHandles; Index++) {
+    Status = gBS->HandleProtocol(
+                    HandleBuffer[Index],
+                    &gEfiFirmwareManagementProtocolGuid,
+                    (VOID **)&Fmp
+                    );
+    if (EFI_ERROR(Status)) {
+      continue;
+    }
+
+    ImageInfoSize = 0;
+    Status = Fmp->GetImageInfo (
+                    Fmp,
+                    &ImageInfoSize,
+                    NULL,
+                    NULL,
+                    NULL,
+                    NULL,
+                    NULL,
+                    NULL
+                    );
+    if (Status != EFI_BUFFER_TOO_SMALL) {
+      continue;
+    }
+
+    FmpImageInfoBuf = AllocateZeroPool (ImageInfoSize);
+    if (FmpImageInfoBuf == NULL) {
+      continue;
+    }
+
+    PackageVersionName = NULL;
+    Status = Fmp->GetImageInfo (
+                    Fmp,
+                    &ImageInfoSize,               // ImageInfoSize
+                    FmpImageInfoBuf,              // ImageInfo
+                    &FmpImageInfoDescriptorVer,   // DescriptorVersion
+                    &FmpImageInfoCount,           // DescriptorCount
+                    &DescriptorSize,              // DescriptorSize
+                    &PackageVersion,              // PackageVersion
+                    &PackageVersionName           // PackageVersionName
+                    );
+    if (EFI_ERROR(Status)) {
+      FreePool(FmpImageInfoBuf);
+      continue;
+    }
+
+    if (PackageVersionName != NULL) {
+      FreePool(PackageVersionName);
+    }
+
+    TempFmpImageInfo = FmpImageInfoBuf;
+    for (Index2 = 0; Index2 < FmpImageInfoCount; Index2++) {
+      //
+      // Check if this FMP instance matches
+      //
+      if (CompareGuid(UpdateImageTypeId, &TempFmpImageInfo->ImageTypeId)) {
+        if ((UpdateHardwareInstance == 0) ||
+            ((FmpImageInfoDescriptorVer >= EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION) &&
+             (UpdateHardwareInstance == TempFmpImageInfo->HardwareInstance))) {
+          MatchedHandleBuffer[MatchedNumberOfHandles] = HandleBuffer[Index];
+          MatchedNumberOfHandles++;
+          break;
+        }
+      }
+      TempFmpImageInfo = (EFI_FIRMWARE_IMAGE_DESCRIPTOR *)((UINT8 *)TempFmpImageInfo + DescriptorSize);
+    }
+    FreePool(FmpImageInfoBuf);
+  }
+
+  if (MatchedNumberOfHandles == 0) {
+    return EFI_NOT_FOUND;
+  }
+
+  *NoHandles = MatchedNumberOfHandles;
+  *Buffer = MatchedHandleBuffer;
+
+  return EFI_SUCCESS;
+}
+
+/**
+  Return FmpImageInfoDescriptorVer by an FMP handle.
+
+  @param[in]  Handle   A FMP handle.
+
+  @return FmpImageInfoDescriptorVer associated with the FMP.
+**/
+UINT32
+GetFmpImageInfoDescriptorVer (
+  IN EFI_HANDLE                                   Handle
+  )
+{
+  EFI_STATUS                                    Status;
+  EFI_FIRMWARE_MANAGEMENT_PROTOCOL              *Fmp;
+  UINTN                                         ImageInfoSize;
+  EFI_FIRMWARE_IMAGE_DESCRIPTOR                 *FmpImageInfoBuf;
+  UINT32                                        FmpImageInfoDescriptorVer;
+  UINT8                                         FmpImageInfoCount;
+  UINTN                                         DescriptorSize;
+  UINT32                                        PackageVersion;
+  CHAR16                                        *PackageVersionName;
+
+  Status = gBS->HandleProtocol(
+                  Handle,
+                  &gEfiFirmwareManagementProtocolGuid,
+                  (VOID **)&Fmp
+                  );
+  if (EFI_ERROR(Status)) {
+    return 0;
+  }
+
+  ImageInfoSize = 0;
+  Status = Fmp->GetImageInfo (
+                  Fmp,
+                  &ImageInfoSize,
+                  NULL,
+                  NULL,
+                  NULL,
+                  NULL,
+                  NULL,
+                  NULL
+                  );
+  if (Status != EFI_BUFFER_TOO_SMALL) {
+    return 0;
+  }
+
+  FmpImageInfoBuf = AllocateZeroPool (ImageInfoSize);
+  if (FmpImageInfoBuf == NULL) {
+    return 0;
+  }
+
+  PackageVersionName = NULL;
+  Status = Fmp->GetImageInfo (
+                  Fmp,
+                  &ImageInfoSize,               // ImageInfoSize
+                  FmpImageInfoBuf,              // ImageInfo
+                  &FmpImageInfoDescriptorVer,   // DescriptorVersion
+                  &FmpImageInfoCount,           // DescriptorCount
+                  &DescriptorSize,              // DescriptorSize
+                  &PackageVersion,              // PackageVersion
+                  &PackageVersionName           // PackageVersionName
+                  );
+  if (EFI_ERROR(Status)) {
+    FreePool(FmpImageInfoBuf);
+    return 0;
+  }
+  return FmpImageInfoDescriptorVer;
+}
+
+/**
+  Set FMP image data.
+
+  @param[in]  Handle        A FMP handle.
+  @param[in]  ImageHeader   The payload image header.
+  @param[in]  PayloadIndex  The index of the payload.
+
+  @return The status of FMP->SetImage.
+**/
+EFI_STATUS
+SetFmpImageData (
+  IN EFI_HANDLE                                   Handle,
+  IN EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER *ImageHeader,
+  IN UINTN                                        PayloadIndex
+  )
+{
+  EFI_STATUS                                    Status;
+  EFI_FIRMWARE_MANAGEMENT_PROTOCOL              *Fmp;
+  UINT8                                         *Image;
+  VOID                                          *VendorCode;
+  CHAR16                                        *AbortReason;
+
+  Status = gBS->HandleProtocol(
+                  Handle,
+                  &gEfiFirmwareManagementProtocolGuid,
+                  (VOID **)&Fmp
+                  );
+  if (EFI_ERROR(Status)) {
+    return Status;
+  }
+
+  if (ImageHeader->Version >= EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER_INIT_VERSION) {
+    Image = (UINT8 *)(ImageHeader + 1);
+  } else {
+    //
+    // If the EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER is version 1,
+    // Header should exclude UpdateHardwareInstance field
+    //
+    Image = (UINT8 *)ImageHeader + OFFSET_OF(EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER, UpdateHardwareInstance);
+  }
+
+  if (ImageHeader->UpdateVendorCodeSize == 0) {
+    VendorCode = NULL;
+  } else {
+    VendorCode = Image + ImageHeader->UpdateImageSize;
+  }
+  AbortReason = NULL;
+  DEBUG((DEBUG_INFO, "Fmp->SetImage ...\n"));
+  DEBUG((DEBUG_INFO, "ImageTypeId - %g, ", &ImageHeader->UpdateImageTypeId));
+  DEBUG((DEBUG_INFO, "PayloadIndex - 0x%x, ", PayloadIndex));
+  DEBUG((DEBUG_INFO, "ImageIndex - 0x%x ", ImageHeader->UpdateImageIndex));
+  if (ImageHeader->Version >= EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER_INIT_VERSION) {
+    DEBUG((DEBUG_INFO, "(UpdateHardwareInstance - 0x%x)", ImageHeader->UpdateHardwareInstance));
+  }
+  DEBUG((DEBUG_INFO, "\n"));
+  Status = Fmp->SetImage(
+                  Fmp,
+                  ImageHeader->UpdateImageIndex,          // ImageIndex
+                  Image,                                  // Image
+                  ImageHeader->UpdateImageSize,           // ImageSize
+                  VendorCode,                             // VendorCode
+                  Update_Image_Progress,                  // Progress
+                  &AbortReason                            // AbortReason
+                  );
+  DEBUG((DEBUG_INFO, "Fmp->SetImage - %r\n", Status));
+  if (AbortReason != NULL) {
+    DEBUG ((DEBUG_ERROR, "%s\n", AbortReason));
+    FreePool(AbortReason);
+  }
+
+  return Status;
+}
+
+/**
+  Start a UEFI image in the FMP payload.
+
+  @param[in]  ImageBuffer   A pointer to the memory location containing a copy of the image to be loaded..
+  @param[in]  ImageSize     The size in bytes of ImageBuffer.
+
+  @return The status of gBS->LoadImage and gBS->StartImage.
+**/
+EFI_STATUS
+StartFmpImage (
+  IN VOID   *ImageBuffer,
+  IN UINTN  ImageSize
+  )
+{
+  MEMMAP_DEVICE_PATH                            MemMapNode;
+  EFI_STATUS                                    Status;
+  EFI_HANDLE                                    ImageHandle;
+  EFI_DEVICE_PATH_PROTOCOL                      *DriverDevicePath;
+  UINTN                                         ExitDataSize;
+
+  SetDevicePathNodeLength (&MemMapNode.Header, sizeof (MemMapNode));
+  MemMapNode.Header.Type     = HARDWARE_DEVICE_PATH;
+  MemMapNode.Header.SubType  = HW_MEMMAP_DP;
+  MemMapNode.MemoryType      = EfiBootServicesCode;
+  MemMapNode.StartingAddress = (EFI_PHYSICAL_ADDRESS)(UINTN)ImageBuffer;
+  MemMapNode.EndingAddress   = (EFI_PHYSICAL_ADDRESS)(UINTN)((UINT8 *)ImageBuffer + ImageSize - 1);
+
+  DriverDevicePath = AppendDevicePathNode (NULL, &MemMapNode.Header);
+  if (DriverDevicePath == NULL) {
+    return EFI_OUT_OF_RESOURCES;
+  }
+
+  DEBUG((DEBUG_INFO, "FmpCapsule: LoadImage ...\n"));
+  Status = gBS->LoadImage(
+                  FALSE,
+                  gImageHandle,
+                  DriverDevicePath,
+                  ImageBuffer,
+                  ImageSize,
+                  &ImageHandle
+                  );
+  DEBUG((DEBUG_INFO, "FmpCapsule: LoadImage - %r\n", Status));
+  if (EFI_ERROR(Status)) {
+    FreePool(DriverDevicePath);
+    return Status;
+  }
+
+  DEBUG((DEBUG_INFO, "FmpCapsule: StartImage ...\n"));
+  Status = gBS->StartImage(
+                  ImageHandle,
+                  &ExitDataSize,
+                  NULL
+                  );
+  DEBUG((DEBUG_INFO, "FmpCapsule: StartImage - %r\n", Status));
+  if (EFI_ERROR(Status)) {
+    DEBUG ((DEBUG_ERROR, "Driver Return Status = %r\n", Status));
+  }
+
+  FreePool(DriverDevicePath);
+  return Status;
+}
+
+/**
+  Record FMP capsule status.
+
+  @param[in]  Handle        A FMP handle.
+  @param[in] CapsuleHeader  The capsule image header
+  @param[in] CapsuleStatus  The capsule process stauts
+  @param[in] PayloadIndex   FMP payload index
+  @param[in] ImageHeader    FMP image header
+**/
+VOID
+RecordFmpCapsuleStatus (
+  IN EFI_HANDLE                                    Handle,  OPTIONAL
+  IN EFI_CAPSULE_HEADER                            *CapsuleHeader,
+  IN EFI_STATUS                                    CapsuleStatus,
+  IN UINTN                                         PayloadIndex,
+  IN EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER  *ImageHeader
+  )
+{
+  EFI_STATUS                                    Status;
+  EFI_DEVICE_PATH_PROTOCOL                      *FmpDevicePath;
+  UINT32                                        FmpImageInfoDescriptorVer;
+  EFI_STATUS                                    StatusEsrt;
+  ESRT_MANAGEMENT_PROTOCOL                      *EsrtProtocol;
+  EFI_SYSTEM_RESOURCE_ENTRY                     EsrtEntry;
+
+  FmpDevicePath = NULL;
+  if (Handle != NULL) {
+    gBS->HandleProtocol(
+           Handle,
+           &gEfiDevicePathProtocolGuid,
+           (VOID **)&FmpDevicePath
+           );
+  }
+
+  RecordFmpCapsuleStatusVariable (
+    CapsuleHeader,
+    CapsuleStatus,
+    PayloadIndex,
+    ImageHeader,
+    FmpDevicePath
+    );
+
+  //
+  // Update corresponding ESRT entry LastAttemp Status
+  //
+  Status = gBS->LocateProtocol(&gEsrtManagementProtocolGuid, NULL, (VOID **)&EsrtProtocol);
+  if (EFI_ERROR (Status)) {
+    return ;
+  }
+
+  if (Handle == NULL) {
+    return ;
+  }
+
+  //
+  // Update EsrtEntry For V1, V2 FMP instance.
+  // V3 FMP ESRT cache will be synced up through EsrtSyncFmp interface
+  //
+  FmpImageInfoDescriptorVer = GetFmpImageInfoDescriptorVer (Handle);
+  if (FmpImageInfoDescriptorVer < EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION) {
+    StatusEsrt = EsrtProtocol->GetEsrtEntry(&ImageHeader->UpdateImageTypeId, &EsrtEntry);
+    if (!EFI_ERROR(StatusEsrt)){
+      if (!EFI_ERROR(CapsuleStatus)) {
+        EsrtEntry.LastAttemptStatus = LAST_ATTEMPT_STATUS_SUCCESS;
+      } else {
+        EsrtEntry.LastAttemptStatus = LAST_ATTEMPT_STATUS_ERROR_UNSUCCESSFUL;
+      }
+      EsrtEntry.LastAttemptVersion = 0;
+      EsrtProtocol->UpdateEsrtEntry(&EsrtEntry);
+    }
+  }
+}
+
 /**
   Process Firmware management protocol data capsule.
 
@@ -779,65 +1274,38 @@ DumpFmpCapsule (
   This function need support nested FMP capsule.
 
   @param[in]   CapsuleHeader         Points to a capsule header.
-  @param[out]  AreAllImagesProcessed If all the FMP images in the capsule are processed.
 
   @retval EFI_SUCESS            Process Capsule Image successfully.
   @retval EFI_UNSUPPORTED       Capsule image is not supported by the firmware.
   @retval EFI_VOLUME_CORRUPTED  FV volume in the capsule is corrupted.
   @retval EFI_OUT_OF_RESOURCES  Not enough memory.
+  @retval EFI_NOT_READY         No FMP protocol to handle this FMP capsule.
 **/
 EFI_STATUS
 ProcessFmpCapsuleImage (
-  IN EFI_CAPSULE_HEADER  *CapsuleHeader,
-  OUT BOOLEAN            *AreAllImagesProcessed
+  IN EFI_CAPSULE_HEADER  *CapsuleHeader
   )
 {
   EFI_STATUS                                    Status;
-  EFI_STATUS                                    StatusEsrt;
-  EFI_STATUS                                    StatusRet;
   EFI_FIRMWARE_MANAGEMENT_CAPSULE_HEADER        *FmpCapsuleHeader;
   EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER  *ImageHeader;
-  UINT8                                         *Image;
-  EFI_HANDLE                                    ImageHandle;
   UINT64                                        *ItemOffsetList;
   UINT32                                        ItemNum;
   UINTN                                         Index;
-  UINTN                                         ExitDataSize;
   EFI_HANDLE                                    *HandleBuffer;
-  EFI_FIRMWARE_MANAGEMENT_PROTOCOL              *Fmp;
   UINTN                                         NumberOfHandles;
-  UINTN                                         DescriptorSize;
-  UINT8                                         FmpImageInfoCount;
-  UINT32                                        FmpImageInfoDescriptorVer;
-  UINTN                                         ImageInfoSize;
-  UINT32                                        PackageVersion;
-  CHAR16                                        *PackageVersionName;
-  CHAR16                                        *AbortReason;
-  EFI_FIRMWARE_IMAGE_DESCRIPTOR                 *FmpImageInfoBuf;
-  EFI_FIRMWARE_IMAGE_DESCRIPTOR                 *TempFmpImageInfo;
   UINTN                                         DriverLen;
-  UINTN                                         Index1;
+  UINT64                                        UpdateHardwareInstance;
   UINTN                                         Index2;
-  MEMMAP_DEVICE_PATH                            MemMapNode;
-  EFI_DEVICE_PATH_PROTOCOL                      *DriverDevicePath;
-  EFI_DEVICE_PATH_PROTOCOL                      *FmpDevicePath;
-  ESRT_MANAGEMENT_PROTOCOL                      *EsrtProtocol;
-  EFI_SYSTEM_RESOURCE_ENTRY                     EsrtEntry;
-  VOID                                          *VendorCode;
+  BOOLEAN                                       NotReady;
+  BOOLEAN                                       Abort;
 
   if (!IsFmpCapsuleGuid(&CapsuleHeader->CapsuleGuid)) {
-    return ProcessFmpCapsuleImage ((EFI_CAPSULE_HEADER *)((UINTN)CapsuleHeader + CapsuleHeader->HeaderSize), AreAllImagesProcessed);
+    return ProcessFmpCapsuleImage ((EFI_CAPSULE_HEADER *)((UINTN)CapsuleHeader + CapsuleHeader->HeaderSize));
   }
 
-  ASSERT(AreAllImagesProcessed != NULL);
-
-  Status           = EFI_SUCCESS;
-  StatusRet        = EFI_NOT_FOUND;
-  HandleBuffer     = NULL;
-  ExitDataSize     = 0;
-  DriverDevicePath = NULL;
-  EsrtProtocol     = NULL;
-  *AreAllImagesProcessed = FALSE;
+  NotReady = FALSE;
+  Abort = FALSE;
 
   DumpFmpCapsule(CapsuleHeader);
 
@@ -854,35 +1322,15 @@ ProcessFmpCapsuleImage (
   // capsule in which driver count and payload count are both zero is not processed.
   //
   if (ItemNum == 0) {
-    *AreAllImagesProcessed = TRUE;
     return EFI_SUCCESS;
   }
 
-  //
-  // Update corresponding ESRT entry LastAttemp Status
-  //
-  Status = gBS->LocateProtocol(&gEsrtManagementProtocolGuid, NULL, (VOID **)&EsrtProtocol);
-  if (EFI_ERROR (Status)) {
-    EsrtProtocol = NULL;
-  }
-
   //
   // 1. Try to load & start all the drivers within capsule
   //
-  SetDevicePathNodeLength (&MemMapNode.Header, sizeof (MemMapNode));
-  MemMapNode.Header.Type     = HARDWARE_DEVICE_PATH;
-  MemMapNode.Header.SubType  = HW_MEMMAP_DP;
-  MemMapNode.MemoryType      = EfiBootServicesCode;
-  MemMapNode.StartingAddress = (EFI_PHYSICAL_ADDRESS)(UINTN)CapsuleHeader;
-  MemMapNode.EndingAddress   = (EFI_PHYSICAL_ADDRESS)(UINTN)((UINT8 *)CapsuleHeader + CapsuleHeader->CapsuleImageSize - 1);
-
-  DriverDevicePath = AppendDevicePathNode (NULL, &MemMapNode.Header);
-  if (DriverDevicePath == NULL) {
-    return EFI_OUT_OF_RESOURCES;
-  }
-
   for (Index = 0; Index < FmpCapsuleHeader->EmbeddedDriverCount; Index++) {
-    if (FmpCapsuleHeader->PayloadItemCount == 0 && Index == (UINTN)FmpCapsuleHeader->EmbeddedDriverCount - 1) {
+    if ((FmpCapsuleHeader->PayloadItemCount == 0) &&
+        (Index == (UINTN)FmpCapsuleHeader->EmbeddedDriverCount - 1)) {
       //
       // When driver is last element in the ItemOffsetList array, the driver size is calculated by reference CapsuleImageSize in EFI_CAPSULE_HEADER
       //
@@ -891,32 +1339,13 @@ ProcessFmpCapsuleImage (
       DriverLen = (UINTN)ItemOffsetList[Index + 1] - (UINTN)ItemOffsetList[Index];
     }
 
-    DEBUG((DEBUG_INFO, "FmpCapsule: LoadImage ...\n"));
-    Status = gBS->LoadImage(
-                    FALSE,
-                    gImageHandle,
-                    DriverDevicePath,
-                    (UINT8 *)FmpCapsuleHeader + ItemOffsetList[Index],
-                    DriverLen,
-                    &ImageHandle
-                    );
-    DEBUG((DEBUG_INFO, "FmpCapsule: LoadImage - %r\n", Status));
-    if (EFI_ERROR(Status)) {
-      StatusRet = Status;
-      goto EXIT;
-    }
-
-    DEBUG((DEBUG_INFO, "FmpCapsule: StartImage ...\n"));
-    Status = gBS->StartImage(
-                    ImageHandle,
-                    &ExitDataSize,
-                    NULL
-                    );
-    DEBUG((DEBUG_INFO, "FmpCapsule: StartImage - %r\n", Status));
+    Status = StartFmpImage (
+               (UINT8 *)FmpCapsuleHeader + ItemOffsetList[Index],
+               DriverLen
+               );
     if (EFI_ERROR(Status)) {
       DEBUG ((DEBUG_ERROR, "Driver Return Status = %r\n", Status));
-      StatusRet = Status;
-      goto EXIT;
+      return Status;
     }
   }
 
@@ -925,207 +1354,80 @@ ProcessFmpCapsuleImage (
   //
   DEBUG((DEBUG_INFO, "FmpCapsule: route payload to right FMP instance ...\n"));
 
-  Status = gBS->LocateHandleBuffer (
-                  ByProtocol,
-                  &gEfiFirmwareManagementProtocolGuid,
-                  NULL,
-                  &NumberOfHandles,
-                  &HandleBuffer
-                  );
-
-  if (!EFI_ERROR(Status)) {
-    for(Index1 = 0; Index1 < NumberOfHandles; Index1++) {
-      Status = gBS->HandleProtocol(
-                      HandleBuffer[Index1],
-                      &gEfiFirmwareManagementProtocolGuid,
-                      (VOID **)&Fmp
-                      );
-      if (EFI_ERROR(Status)) {
-        continue;
-      }
-
-      FmpDevicePath = NULL;
-      gBS->HandleProtocol(
-             HandleBuffer[Index1],
-             &gEfiDevicePathProtocolGuid,
-             (VOID **)&FmpDevicePath
-             );
-
-      ImageInfoSize = 0;
-      Status = Fmp->GetImageInfo (
-                      Fmp,
-                      &ImageInfoSize,
-                      NULL,
-                      NULL,
-                      NULL,
-                      NULL,
-                      NULL,
-                      NULL
-                      );
-      if (Status != EFI_BUFFER_TOO_SMALL) {
-        continue;
-      }
-
-      FmpImageInfoBuf = NULL;
-      FmpImageInfoBuf = AllocateZeroPool (ImageInfoSize);
-      if (FmpImageInfoBuf == NULL) {
-        StatusRet = EFI_OUT_OF_RESOURCES;
-        goto EXIT;
-      }
-
-      PackageVersionName = NULL;
-      Status = Fmp->GetImageInfo (
-                      Fmp,
-                      &ImageInfoSize,               // ImageInfoSize
-                      FmpImageInfoBuf,              // ImageInfo
-                      &FmpImageInfoDescriptorVer,   // DescriptorVersion
-                      &FmpImageInfoCount,           // DescriptorCount
-                      &DescriptorSize,              // DescriptorSize
-                      &PackageVersion,              // PackageVersion
-                      &PackageVersionName           // PackageVersionName
-                      );
-
-      //
-      // If FMP GetInformation interface failed, skip this resource
-      //
-      if (EFI_ERROR(Status)) {
-        FreePool(FmpImageInfoBuf);
-        continue;
-      }
-
-      DEBUG((DEBUG_INFO, "FMP (%d) ImageInfo:\n", Index));
-      DumpFmpImageInfo(
-        ImageInfoSize,               // ImageInfoSize
-        FmpImageInfoBuf,             // ImageInfo
-        FmpImageInfoDescriptorVer,   // DescriptorVersion
-        FmpImageInfoCount,           // DescriptorCount
-        DescriptorSize,              // DescriptorSize
-        PackageVersion,              // PackageVersion
-        PackageVersionName           // PackageVersionName
-        );
-
-      if (PackageVersionName != NULL) {
-        FreePool(PackageVersionName);
-      }
-
-      TempFmpImageInfo = FmpImageInfoBuf;
-      for (Index2 = 0; Index2 < FmpImageInfoCount; Index2++) {
-        //
-        // Check all the payload entry in capsule payload list
-        //
-        for (Index = FmpCapsuleHeader->EmbeddedDriverCount; Index < ItemNum; Index++) {
-          ImageHeader  = (EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER *)((UINT8 *)FmpCapsuleHeader + ItemOffsetList[Index]);
-
-          if (IsFmpCapsuleProcessed(CapsuleHeader, Index - FmpCapsuleHeader->EmbeddedDriverCount, ImageHeader)) {
-            DEBUG((DEBUG_INFO, "FMP Capsule already processed (%g):", CapsuleHeader));
-            DEBUG((DEBUG_INFO, "ImageTypeId - %g, ", &ImageHeader->UpdateImageTypeId));
-            DEBUG((DEBUG_INFO, "PayloadIndex - 0x%x, ImageIndex - 0x%x\n", Index - FmpCapsuleHeader->EmbeddedDriverCount, ImageHeader->UpdateImageIndex));
-            continue;
-          }
-
-          if (CompareGuid(&ImageHeader->UpdateImageTypeId, &TempFmpImageInfo->ImageTypeId)) {
-            AbortReason = NULL;
-            if (ImageHeader->Version >= EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER_INIT_VERSION) {
-              if(ImageHeader->UpdateHardwareInstance != 0){
-                //
-                // FMP Version is >=2 & UpdateHardwareInstance Skip 2 case
-                //  1. FMP Image info Version < 3
-                //  2. HardwareInstance doesn't match
-                //
-                if (FmpImageInfoDescriptorVer < EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION ||
-                   ImageHeader->UpdateHardwareInstance != TempFmpImageInfo->HardwareInstance) {
-                  continue;
-                }
-              }
-              Image = (UINT8 *)(ImageHeader + 1);
-            } else {
-              //
-              // If the EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER is version 1, only match ImageTypeId.
-              // Header should exclude UpdateHardwareInstance field
-              //
-              Image = (UINT8 *)ImageHeader + OFFSET_OF(EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER, UpdateHardwareInstance);
-            }
-
-            if (ImageHeader->UpdateVendorCodeSize == 0) {
-              VendorCode = NULL;
-            } else {
-              VendorCode = Image + ImageHeader->UpdateImageSize;
-            }
-            DEBUG((DEBUG_INFO, "Fmp->SetImage ...\n"));
-            Status = Fmp->SetImage(
-                            Fmp,
-                            ImageHeader->UpdateImageIndex,          // ImageIndex
-                            Image,                                  // Image
-                            ImageHeader->UpdateImageSize,           // ImageSize
-                            VendorCode,                                   // VendorCode
-                            Update_Image_Progress,                  // Progress
-                            &AbortReason                            // AbortReason
-                            );
-            DEBUG((DEBUG_INFO, "Fmp->SetImage - %r\n", Status));
-            if (AbortReason != NULL) {
-              DEBUG ((DEBUG_ERROR, "%s\n", AbortReason));
-              FreePool(AbortReason);
-            }
-            RecordFmpCapsuleStatusVariable(
-              CapsuleHeader,                                 // CapsuleGuid
-              Status,                                        // CapsuleStatus
-              Index - FmpCapsuleHeader->EmbeddedDriverCount, // PayloadIndex
-              ImageHeader,                                   // ImageHeader
-              FmpDevicePath                                  // FmpDevicePath
-              );
-            if (StatusRet != EFI_SUCCESS) {
-              StatusRet = Status;
-            }
-            //
-            // Update EsrtEntry For V1, V2 FMP instance. V3 FMP ESRT cache will be synced up through EsrtSyncFmp interface
-            //
-            if (FmpImageInfoDescriptorVer < EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION && EsrtProtocol != NULL) {
-               StatusEsrt = EsrtProtocol->GetEsrtEntry(&TempFmpImageInfo->ImageTypeId, &EsrtEntry);
-               if (!EFI_ERROR(StatusEsrt)){
-                 if (!EFI_ERROR(Status)) {
-                   EsrtEntry.LastAttemptStatus = LAST_ATTEMPT_STATUS_SUCCESS;
-                 } else {
-                   EsrtEntry.LastAttemptStatus = LAST_ATTEMPT_STATUS_ERROR_UNSUCCESSFUL;
-                 }
-                 EsrtEntry.LastAttemptVersion = 0;
-                 EsrtProtocol->UpdateEsrtEntry(&EsrtEntry);
-               }
-             }
-          }
-        }
-        //
-        // Use DescriptorSize to move ImageInfo Pointer to stay compatible with different ImageInfo version
-        //
-        TempFmpImageInfo = (EFI_FIRMWARE_IMAGE_DESCRIPTOR *)((UINT8 *)TempFmpImageInfo + DescriptorSize);
-      }
-      FreePool(FmpImageInfoBuf);
-    }
-  }
+  DumpAllFmpInfo ();
 
   //
-  // final check for AreAllImagesProcessed
+  // Check all the payload entry in capsule payload list
   //
-  *AreAllImagesProcessed = TRUE;
   for (Index = FmpCapsuleHeader->EmbeddedDriverCount; Index < ItemNum; Index++) {
-    ImageHeader = (EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER *)((UINT8 *)FmpCapsuleHeader + ItemOffsetList[Index]);
+    ImageHeader  = (EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER *)((UINT8 *)FmpCapsuleHeader + ItemOffsetList[Index]);
 
-    if (!IsFmpCapsuleProcessed(CapsuleHeader, Index - FmpCapsuleHeader->EmbeddedDriverCount, ImageHeader)) {
-      *AreAllImagesProcessed = FALSE;
-      break;
+    UpdateHardwareInstance = 0;
+    if (ImageHeader->Version >= EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER_INIT_VERSION) {
+      UpdateHardwareInstance = ImageHeader->UpdateHardwareInstance;
+    }
+
+    Status = GetFmpHandleBufferByType (
+               &ImageHeader->UpdateImageTypeId,
+               UpdateHardwareInstance,
+               &NumberOfHandles,
+               &HandleBuffer
+               );
+    if (EFI_ERROR(Status)) {
+      NotReady = TRUE;
+      RecordFmpCapsuleStatus (
+        NULL,
+        CapsuleHeader,
+        EFI_NOT_READY,
+        Index - FmpCapsuleHeader->EmbeddedDriverCount,
+        ImageHeader
+        );
+      continue;
+    }
+
+    for (Index2 = 0; Index2 < NumberOfHandles; Index2++) {
+      if (Abort) {
+        RecordFmpCapsuleStatus (
+          HandleBuffer[Index2],
+          CapsuleHeader,
+          EFI_ABORTED,
+          Index - FmpCapsuleHeader->EmbeddedDriverCount,
+          ImageHeader
+          );
+        continue;
+      }
+
+      Status = SetFmpImageData (
+                 HandleBuffer[Index2],
+                 ImageHeader,
+                 Index - FmpCapsuleHeader->EmbeddedDriverCount
+                 );
+      if (Status != EFI_SUCCESS) {
+        Abort = TRUE;
+      }
+
+      RecordFmpCapsuleStatus (
+        HandleBuffer[Index2],
+        CapsuleHeader,
+        Status,
+        Index - FmpCapsuleHeader->EmbeddedDriverCount,
+        ImageHeader
+        );
+    }
+    if (HandleBuffer != NULL) {
+      FreePool(HandleBuffer);
     }
   }
 
-EXIT:
-
-  if (HandleBuffer != NULL) {
-    FreePool(HandleBuffer);
+  if (NotReady) {
+    return EFI_NOT_READY;
   }
 
-  if (DriverDevicePath != NULL) {
-    FreePool(DriverDevicePath);
-  }
-
-  return StatusRet;
+  //
+  // always return SUCCESS to indicate this capsule is processed.
+  // The status of SetImage is recorded in capsule result variable.
+  //
+  return EFI_SUCCESS;
 }
 
 /**
@@ -1278,9 +1580,9 @@ ProcessCapsuleImage (
   )
 {
   EFI_STATUS                   Status;
-  BOOLEAN                      AreAllImagesProcessed;
 
   if (SupportCapsuleImage (CapsuleHeader) != EFI_SUCCESS) {
+    RecordCapsuleStatusVariable(CapsuleHeader, EFI_UNSUPPORTED);
     return EFI_UNSUPPORTED;
   }
 
@@ -1303,6 +1605,7 @@ ProcessCapsuleImage (
     Status = ValidateFmpCapsule(CapsuleHeader, NULL);
     DEBUG((DEBUG_INFO, "ValidateFmpCapsule - %r\n", Status));
     if (EFI_ERROR(Status)) {
+      RecordCapsuleStatusVariable(CapsuleHeader, Status);
       return Status;
     }
 
@@ -1310,13 +1613,9 @@ ProcessCapsuleImage (
     // Press EFI FMP Capsule
     //
     DEBUG((DEBUG_INFO, "ProcessFmpCapsuleImage ...\n"));
-    Status = ProcessFmpCapsuleImage(CapsuleHeader, &AreAllImagesProcessed);
+    Status = ProcessFmpCapsuleImage(CapsuleHeader);
     DEBUG((DEBUG_INFO, "ProcessFmpCapsuleImage - %r\n", Status));
 
-    if (!AreAllImagesProcessed) {
-      mAreAllImagesProcessed = FALSE;
-    }
-
     return Status;
   }
 
diff --git a/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleProcessLib.c b/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleProcessLib.c
index 62257a43cc..ba3ff90b9f 100644
--- a/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleProcessLib.c
+++ b/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleProcessLib.c
@@ -95,60 +95,23 @@ IsValidCapsuleHeader (
   );
 
 extern BOOLEAN                   mDxeCapsuleLibEndOfDxe;
-extern BOOLEAN                   mAreAllImagesProcessed;
 BOOLEAN                          mNeedReset;
 
+VOID                        **mCapsulePtr;
+EFI_STATUS                  *mCapsuleStatusArray;
+UINT32                      mCapsuleTotalNumber;
+
 /**
-
-  This routine is called to process capsules.
-
-  Caution: This function may receive untrusted input.
-
-  Each individual capsule result is recorded in capsule record variable.
-
-  @param[in]  NeedBlockDriver         TRUE: Need skip the FMP capsules with non zero EmbeddedDriverCount.
-                                      FALSE: No need to skip any FMP capsules.
-
-  @retval EFI_SUCCESS             There is no error when processing capsules.
-  @retval EFI_OUT_OF_RESOURCES    No enough resource to process capsules.
-
+  This function initializes the mCapsulePtr, mCapsuleStatusArray and mCapsuleTotalNumber.
 **/
-EFI_STATUS
-ProcessTheseCapsules (
-  IN BOOLEAN  NeedBlockDriver
+VOID
+InitCapsulePtr (
+  VOID
   )
 {
-  EFI_STATUS                  Status;
   EFI_PEI_HOB_POINTERS        HobPointer;
-  EFI_CAPSULE_HEADER          *CapsuleHeader;
-  UINT32                      Size;
-  UINT32                      CapsuleNumber;
-  UINT32                      CapsuleTotalNumber;
-  EFI_CAPSULE_TABLE           *CapsuleTable;
-  UINT32                      Index;
-  UINT32                      CacheIndex;
-  UINT32                      CacheNumber;
-  VOID                        **CapsulePtr;
-  VOID                        **CapsulePtrCache;
-  EFI_GUID                    *CapsuleGuidCache;
-  EFI_STATUS                  *CapsuleStatusArray;
-  BOOLEAN                     DisplayCapsuleExist;
-  ESRT_MANAGEMENT_PROTOCOL    *EsrtManagement;
-  UINT16                      EmbeddedDriverCount;
+  UINTN                       Index;
 
-  REPORT_STATUS_CODE(EFI_PROGRESS_CODE, (EFI_SOFTWARE | PcdGet32(PcdStatusCodeSubClassCapsule) | PcdGet32(PcdCapsuleStatusCodeProcessCapsulesBegin)));
-
-  CapsuleNumber       = 0;
-  CapsuleTotalNumber  = 0;
-  CacheIndex          = 0;
-  CacheNumber         = 0;
-  CapsulePtr          = NULL;
-  CapsulePtrCache     = NULL;
-  CapsuleGuidCache    = NULL;
-  DisplayCapsuleExist = FALSE;
-  EsrtManagement      = NULL;
-
-  Status = EFI_SUCCESS;
   //
   // Find all capsule images from hob
   //
@@ -157,61 +120,108 @@ ProcessTheseCapsules (
     if (!IsValidCapsuleHeader((VOID *)(UINTN)HobPointer.Capsule->BaseAddress, HobPointer.Capsule->Length)) {
       HobPointer.Header->HobType = EFI_HOB_TYPE_UNUSED; // Mark this hob as invalid
     } else {
-      CapsuleTotalNumber++;
+      mCapsuleTotalNumber++;
     }
     HobPointer.Raw = GET_NEXT_HOB (HobPointer);
   }
 
-  if (CapsuleTotalNumber == 0) {
-    //
-    // We didn't find a hob, so had no errors.
-    //
-    DEBUG ((DEBUG_ERROR, "We can not find capsule data in capsule update boot mode.\n"));
-    Status = EFI_SUCCESS;
-    goto Done;
+  DEBUG ((DEBUG_INFO, "mCapsuleTotalNumber - 0x%x\n", mCapsuleTotalNumber));
+
+  if (mCapsuleTotalNumber == 0) {
+    return ;
   }
 
   //
   // Init temp Capsule Data table.
   //
-  CapsulePtr       = (VOID **) AllocateZeroPool (sizeof (VOID *) * CapsuleTotalNumber);
-  ASSERT (CapsulePtr != NULL);
-  if (CapsulePtr == NULL) {
-    Status = EFI_OUT_OF_RESOURCES;
-    goto Done;
+  mCapsulePtr       = (VOID **) AllocateZeroPool (sizeof (VOID *) * mCapsuleTotalNumber);
+  if (mCapsulePtr == NULL) {
+    DEBUG ((DEBUG_ERROR, "Allocate mCapsulePtr fail!\n"));
+    mCapsuleTotalNumber = 0;
+    return ;
   }
-  CapsulePtrCache  = (VOID **) AllocateZeroPool (sizeof (VOID *) * CapsuleTotalNumber);
-  ASSERT (CapsulePtrCache != NULL);
-  if (CapsulePtrCache == NULL) {
-    Status = EFI_OUT_OF_RESOURCES;
-    goto Done;
-  }
-  CapsuleGuidCache = (EFI_GUID *) AllocateZeroPool (sizeof (EFI_GUID) * CapsuleTotalNumber);
-  ASSERT (CapsuleGuidCache != NULL);
-  if (CapsuleGuidCache == NULL) {
-    Status = EFI_OUT_OF_RESOURCES;
-    goto Done;
-  }
-  CapsuleStatusArray = (EFI_STATUS *) AllocateZeroPool (sizeof (EFI_STATUS) * CapsuleTotalNumber);
-  ASSERT (CapsuleStatusArray != NULL);
-  if (CapsuleStatusArray == NULL) {
-    Status = EFI_OUT_OF_RESOURCES;
-    goto Done;
+  mCapsuleStatusArray = (EFI_STATUS *) AllocateZeroPool (sizeof (EFI_STATUS) * mCapsuleTotalNumber);
+  if (mCapsuleStatusArray == NULL) {
+    DEBUG ((DEBUG_ERROR, "Allocate mCapsuleStatusArray fail!\n"));
+    FreePool (mCapsulePtr);
+    mCapsulePtr = NULL;
+    mCapsuleTotalNumber = 0;
+    return ;
   }
+  SetMemN (mCapsuleStatusArray, sizeof (EFI_STATUS) * mCapsuleTotalNumber, EFI_NOT_READY);
 
   //
   // Find all capsule images from hob
   //
   HobPointer.Raw = GetHobList ();
+  Index = 0;
   while ((HobPointer.Raw = GetNextHob (EFI_HOB_TYPE_UEFI_CAPSULE, HobPointer.Raw)) != NULL) {
-    CapsulePtr [CapsuleNumber++] = (VOID *) (UINTN) HobPointer.Capsule->BaseAddress;
+    mCapsulePtr [Index++] = (VOID *) (UINTN) HobPointer.Capsule->BaseAddress;
     HobPointer.Raw = GET_NEXT_HOB (HobPointer);
   }
+}
 
-  //
-  // Check the capsule flags,if contains CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE, install
-  // capsuleTable to configure table with EFI_CAPSULE_GUID
-  //
+/**
+  This function returns if all capsule images are processed.
+
+  @retval TRUE   All capsule images are processed.
+  @retval FALSE  Not all capsule images are processed.
+**/
+BOOLEAN
+AreAllImagesProcessed (
+  VOID
+  )
+{
+  UINTN  Index;
+
+  for (Index = 0; Index < mCapsuleTotalNumber; Index++) {
+    if (mCapsuleStatusArray[Index] == EFI_NOT_READY) {
+      return FALSE;
+    }
+  }
+
+  return TRUE;
+}
+
+/**
+  This function populates capsule in the configuration table.
+**/
+VOID
+PopulateCapsuleInConfigurationTable (
+  VOID
+  )
+{
+  VOID                        **CapsulePtrCache;
+  EFI_GUID                    *CapsuleGuidCache;
+  EFI_CAPSULE_HEADER          *CapsuleHeader;
+  EFI_CAPSULE_TABLE           *CapsuleTable;
+  UINT32                      CacheIndex;
+  UINT32                      CacheNumber;
+  UINT32                      CapsuleNumber;
+  UINTN                       Index;
+  UINTN                       Size;
+  EFI_STATUS                  Status;
+
+  if (mCapsuleTotalNumber == 0) {
+    return ;
+  }
+
+  CapsulePtrCache     = NULL;
+  CapsuleGuidCache    = NULL;
+  CacheIndex          = 0;
+  CacheNumber         = 0;
+
+  CapsulePtrCache  = (VOID **) AllocateZeroPool (sizeof (VOID *) * mCapsuleTotalNumber);
+  if (CapsulePtrCache == NULL) {
+    DEBUG ((DEBUG_ERROR, "Allocate CapsulePtrCache fail!\n"));
+    return ;
+  }
+  CapsuleGuidCache = (EFI_GUID *) AllocateZeroPool (sizeof (EFI_GUID) * mCapsuleTotalNumber);
+  if (CapsuleGuidCache == NULL) {
+    DEBUG ((DEBUG_ERROR, "Allocate CapsuleGuidCache fail!\n"));
+    FreePool (CapsulePtrCache);
+    return ;
+  }
 
   //
   // Capsules who have CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE always are used for operating
@@ -223,9 +233,8 @@ ProcessTheseCapsules (
   // Firstly, Loop for all coalesced capsules, record unique CapsuleGuids and cache them in an
   // array for later sorting capsules by CapsuleGuid.
   //
-  for (Index = 0; Index < CapsuleTotalNumber; Index++) {
-    CapsuleStatusArray [Index] = EFI_UNSUPPORTED;
-    CapsuleHeader = (EFI_CAPSULE_HEADER*) CapsulePtr [Index];
+  for (Index = 0; Index < mCapsuleTotalNumber; Index++) {
+    CapsuleHeader = (EFI_CAPSULE_HEADER*) mCapsulePtr [Index];
     if ((CapsuleHeader->Flags & CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) != 0) {
       //
       // For each capsule, we compare it with known CapsuleGuid in the CacheArray.
@@ -254,38 +263,90 @@ ProcessTheseCapsules (
   // will look in EFI System Table and search for the capsule's Guid and associated
   // pointer to retrieve the data.
   //
-  CacheIndex = 0;
-  while (CacheIndex < CacheNumber) {
+  for (CacheIndex = 0; CacheIndex < CacheNumber; CacheIndex++) {
     CapsuleNumber = 0;
-    for (Index = 0; Index < CapsuleTotalNumber; Index++) {
-      CapsuleHeader = (EFI_CAPSULE_HEADER*) CapsulePtr [Index];
+    for (Index = 0; Index < mCapsuleTotalNumber; Index++) {
+      CapsuleHeader = (EFI_CAPSULE_HEADER*) mCapsulePtr [Index];
       if ((CapsuleHeader->Flags & CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE) != 0) {
         if (CompareGuid (&CapsuleGuidCache[CacheIndex], &CapsuleHeader->CapsuleGuid)) {
           //
           // Cache Caspuleheader to the array, this array is uniqued with certain CapsuleGuid.
           //
           CapsulePtrCache[CapsuleNumber++] = (VOID*)CapsuleHeader;
-          //
-          // When a Capsule is listed in CapsulePtrCache, it will be reported in ConfigurationTable
-          // So, report the CapsuleStatus as "processed successfully".
-          //
-          CapsuleStatusArray [Index] = EFI_SUCCESS;
         }
       }
     }
     if (CapsuleNumber != 0) {
       Size = sizeof(EFI_CAPSULE_TABLE) + (CapsuleNumber - 1) * sizeof(VOID*);
       CapsuleTable = AllocateRuntimePool (Size);
-      ASSERT (CapsuleTable != NULL);
       if (CapsuleTable == NULL) {
-        return EFI_OUT_OF_RESOURCES;
+        DEBUG ((DEBUG_ERROR, "Allocate CapsuleTable (%g) fail!\n", &CapsuleGuidCache[CacheIndex]));
+        continue;
       }
       CapsuleTable->CapsuleArrayNumber =  CapsuleNumber;
       CopyMem(&CapsuleTable->CapsulePtr[0], CapsulePtrCache, CapsuleNumber * sizeof(VOID*));
       Status = gBS->InstallConfigurationTable (&CapsuleGuidCache[CacheIndex], (VOID*)CapsuleTable);
-      ASSERT_EFI_ERROR (Status);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_ERROR, "InstallConfigurationTable (%g) fail!\n", &CapsuleGuidCache[CacheIndex]));
+      }
     }
-    CacheIndex++;
+  }
+
+  FreePool(CapsuleGuidCache);
+  FreePool(CapsulePtrCache);
+}
+
+/**
+
+  This routine is called to process capsules.
+
+  Caution: This function may receive untrusted input.
+
+  Each individual capsule result is recorded in capsule record variable.
+
+  @param[in]  FirstRound         TRUE:  First round. Need skip the FMP capsules with non zero EmbeddedDriverCount.
+                                 FALSE: Process rest FMP capsules.
+
+  @retval EFI_SUCCESS             There is no error when processing capsules.
+  @retval EFI_OUT_OF_RESOURCES    No enough resource to process capsules.
+
+**/
+EFI_STATUS
+ProcessTheseCapsules (
+  IN BOOLEAN  FirstRound
+  )
+{
+  EFI_STATUS                  Status;
+  EFI_CAPSULE_HEADER          *CapsuleHeader;
+  UINT32                      Index;
+  BOOLEAN                     DisplayCapsuleExist;
+  ESRT_MANAGEMENT_PROTOCOL    *EsrtManagement;
+  UINT16                      EmbeddedDriverCount;
+
+  REPORT_STATUS_CODE(EFI_PROGRESS_CODE, (EFI_SOFTWARE | PcdGet32(PcdStatusCodeSubClassCapsule) | PcdGet32(PcdCapsuleStatusCodeProcessCapsulesBegin)));
+
+  if (FirstRound) {
+    InitCapsulePtr ();
+  }
+
+  if (mCapsuleTotalNumber == 0) {
+    //
+    // We didn't find a hob, so had no errors.
+    //
+    DEBUG ((DEBUG_ERROR, "We can not find capsule data in capsule update boot mode.\n"));
+    return EFI_SUCCESS;
+  }
+
+  if (AreAllImagesProcessed ()) {
+    return EFI_SUCCESS;
+  }
+
+  //
+  // Check the capsule flags,if contains CAPSULE_FLAGS_POPULATE_SYSTEM_TABLE, install
+  // capsuleTable to configure table with EFI_CAPSULE_GUID
+  //
+  if (FirstRound) {
+    PopulateCapsuleInConfigurationTable ();
   }
 
   REPORT_STATUS_CODE(EFI_PROGRESS_CODE, (EFI_SOFTWARE | PcdGet32(PcdStatusCodeSubClassCapsule) | PcdGet32(PcdCapsuleStatusCodeUpdatingFirmware)));
@@ -293,15 +354,16 @@ ProcessTheseCapsules (
   //
   // If Windows UX capsule exist, process it first
   //
-  for (Index = 0; Index < CapsuleTotalNumber; Index++) {
-    CapsuleHeader = (EFI_CAPSULE_HEADER*) CapsulePtr [Index];
-    if (CompareGuid(&CapsuleHeader->CapsuleGuid ,&gWindowsUxCapsuleGuid)) {
+  DisplayCapsuleExist = FALSE;
+  for (Index = 0; Index < mCapsuleTotalNumber; Index++) {
+    CapsuleHeader = (EFI_CAPSULE_HEADER*) mCapsulePtr [Index];
+    if (CompareGuid (&CapsuleHeader->CapsuleGuid, &gWindowsUxCapsuleGuid)) {
       DEBUG ((DEBUG_INFO, "ProcessCapsuleImage (Ux) - 0x%x\n", CapsuleHeader));
       DisplayCapsuleExist = TRUE;
       DEBUG ((DEBUG_INFO, "Display logo capsule is found.\n"));
       Status = ProcessCapsuleImage (CapsuleHeader);
+      mCapsuleStatusArray [Index] = EFI_SUCCESS;
       DEBUG((DEBUG_INFO, "ProcessCapsuleImage (Ux) - %r\n", Status));
-      CapsuleStatusArray [Index] = Status;
       break;
     }
   }
@@ -316,38 +378,48 @@ ProcessTheseCapsules (
   //
   // All capsules left are recognized by platform.
   //
-  for (Index = 0; Index < CapsuleTotalNumber; Index++) {
-    CapsuleHeader = (EFI_CAPSULE_HEADER*) CapsulePtr [Index];
-    if (!CompareGuid(&CapsuleHeader->CapsuleGuid ,&gWindowsUxCapsuleGuid)) {
+  for (Index = 0; Index < mCapsuleTotalNumber; Index++) {
+    if (mCapsuleStatusArray [Index] != EFI_NOT_READY) {
+      // already processed
+      continue;
+    }
+    CapsuleHeader = (EFI_CAPSULE_HEADER*) mCapsulePtr [Index];
+    if (!CompareGuid (&CapsuleHeader->CapsuleGuid, &gWindowsUxCapsuleGuid)) {
       //
       // Call capsule library to process capsule image.
       //
       EmbeddedDriverCount = 0;
       if (IsFmpCapsule(CapsuleHeader)) {
-        Status = ValidateFmpCapsule(CapsuleHeader, &EmbeddedDriverCount);
+        Status = ValidateFmpCapsule (CapsuleHeader, &EmbeddedDriverCount);
         if (EFI_ERROR(Status)) {
           DEBUG((DEBUG_ERROR, "ValidateFmpCapsule failed. Ignore!\n"));
+          mCapsuleStatusArray [Index] = EFI_ABORTED;
           continue;
         }
+      } else {
+        mCapsuleStatusArray [Index] = EFI_ABORTED;
+        continue;
       }
 
-      if ((!NeedBlockDriver) || (EmbeddedDriverCount == 0)) {
+      if ((!FirstRound) || (EmbeddedDriverCount == 0)) {
         DEBUG((DEBUG_INFO, "ProcessCapsuleImage - 0x%x\n", CapsuleHeader));
         Status = ProcessCapsuleImage (CapsuleHeader);
-        CapsuleStatusArray [Index] = Status;
+        mCapsuleStatusArray [Index] = Status;
         DEBUG((DEBUG_INFO, "ProcessCapsuleImage - %r\n", Status));
 
-        if (EFI_ERROR(Status)) {
-          REPORT_STATUS_CODE(EFI_ERROR_CODE, (EFI_SOFTWARE | PcdGet32(PcdStatusCodeSubClassCapsule) | PcdGet32(PcdCapsuleStatusCodeUpdateFirmwareFailed)));
-          DEBUG ((DEBUG_ERROR, "Capsule process failed. reset the system!\n"));
-          Print (L"Firmware update failed...\r\n");
-        } else {
-          REPORT_STATUS_CODE(EFI_PROGRESS_CODE, (EFI_SOFTWARE | PcdGet32(PcdStatusCodeSubClassCapsule) | PcdGet32(PcdCapsuleStatusCodeUpdateFirmwareSuccess)));
-        }
+        if (Status != EFI_NOT_READY) {
+          if (EFI_ERROR(Status)) {
+            REPORT_STATUS_CODE(EFI_ERROR_CODE, (EFI_SOFTWARE | PcdGet32(PcdStatusCodeSubClassCapsule) | PcdGet32(PcdCapsuleStatusCodeUpdateFirmwareFailed)));
+            DEBUG ((DEBUG_ERROR, "Capsule process failed!\n"));
+            Print (L"Firmware update failed...\r\n");
+          } else {
+            REPORT_STATUS_CODE(EFI_PROGRESS_CODE, (EFI_SOFTWARE | PcdGet32(PcdStatusCodeSubClassCapsule) | PcdGet32(PcdCapsuleStatusCodeUpdateFirmwareSuccess)));
+          }
 
-        if ((CapsuleHeader->Flags & PcdGet16(PcdSystemRebootAfterCapsuleProcessFlag)) != 0 ||
-            IsFmpCapsule(CapsuleHeader)) {
-          mNeedReset = TRUE;
+          if ((CapsuleHeader->Flags & PcdGet16(PcdSystemRebootAfterCapsuleProcessFlag)) != 0 ||
+              IsFmpCapsule(CapsuleHeader)) {
+            mNeedReset = TRUE;
+          }
         }
       }
     }
@@ -362,20 +434,6 @@ ProcessTheseCapsules (
   }
   Status = EFI_SUCCESS;
 
-Done:
-  //
-  // Free the allocated temp memory space.
-  //
-  if (CapsuleGuidCache != NULL) {
-    FreePool(CapsuleGuidCache);
-  }
-  if (CapsulePtrCache != NULL) {
-    FreePool(CapsulePtrCache);
-  }
-  if (CapsulePtr != NULL) {
-    FreePool(CapsulePtr);
-  }
-
   REPORT_STATUS_CODE(EFI_PROGRESS_CODE, (EFI_SOFTWARE | PcdGet32(PcdStatusCodeSubClassCapsule) | PcdGet32(PcdCapsuleStatusCodeProcessCapsulesEnd)));
 
   return Status;
@@ -446,20 +504,13 @@ ProcessCapsules (
   EFI_STATUS                    Status;
 
   if (!mDxeCapsuleLibEndOfDxe) {
-    //
-    // Initialize mAreAllImagesProcessed to be TRUE.
-    //
-    // It will be updated to FALSE in ProcessTheseCapsules()->ProcessCapsuleImage(),
-    // if there is any FMP image in any FMP capsule not processed.
-    //
-    mAreAllImagesProcessed = TRUE;
-
     Status = ProcessTheseCapsules(TRUE);
+
     //
     // Reboot System if and only if all capsule processed.
     // If not, defer reset to 2nd process.
     //
-    if (mNeedReset && mAreAllImagesProcessed) {
+    if (mNeedReset && AreAllImagesProcessed()) {
       DoResetSystem();
     }
   } else {
diff --git a/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleReportLib.c b/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleReportLib.c
index ce79a5a619..3a9bb6e098 100644
--- a/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleReportLib.c
+++ b/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleReportLib.c
@@ -35,17 +35,6 @@
 
 #include <IndustryStandard/WindowsUxCapsule.h>
 
-typedef struct {
-  EFI_CAPSULE_RESULT_VARIABLE_HEADER  CapsuleResultHeader;
-  EFI_CAPSULE_RESULT_VARIABLE_FMP     CapsuleResultFmp;
-} CAPSULE_RESULT_VARIABLE_CACHE;
-
-#define CAPSULE_RESULT_VARIABLE_CACHE_COUNT   0x10
-
-CAPSULE_RESULT_VARIABLE_CACHE *mCapsuleResultVariableCache;
-UINTN                         mCapsuleResultVariableCacheMaxCount;
-UINTN                         mCapsuleResultVariableCacheCount;
-
 /**
   Get current capsule last variable index.
 
@@ -77,92 +66,6 @@ GetCurrentCapsuleLastIndex (
   return CurrentIndex;
 }
 
-/**
-  Check if this FMP capsule is processed.
-
-  @param[in] CapsuleHeader  The capsule image header
-  @param[in] PayloadIndex   FMP payload index
-  @param[in] ImageHeader    FMP image header
-
-  @retval TRUE  This FMP capsule is processed.
-  @retval FALSE This FMP capsule is not processed.
-**/
-BOOLEAN
-IsFmpCapsuleProcessed (
-  IN EFI_CAPSULE_HEADER                            *CapsuleHeader,
-  IN UINTN                                         PayloadIndex,
-  IN EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER  *ImageHeader
-  )
-{
-  UINTN                               Index;
-  EFI_CAPSULE_RESULT_VARIABLE_HEADER  *CapsuleResult;
-  EFI_CAPSULE_RESULT_VARIABLE_FMP     *CapsuleResultFmp;
-
-  for (Index = 0; Index < mCapsuleResultVariableCacheCount; Index++) {
-    //
-    // Check
-    //
-    CapsuleResult = &mCapsuleResultVariableCache[Index].CapsuleResultHeader;
-    if (CapsuleResult->VariableTotalSize >= sizeof(EFI_CAPSULE_RESULT_VARIABLE_HEADER)) {
-      if (CompareGuid(&CapsuleResult->CapsuleGuid, &gEfiFmpCapsuleGuid)) {
-        if (CapsuleResult->VariableTotalSize >= sizeof(EFI_CAPSULE_RESULT_VARIABLE_HEADER) + sizeof(EFI_CAPSULE_RESULT_VARIABLE_FMP) + sizeof(CHAR16) * 2) {
-          CapsuleResultFmp = (EFI_CAPSULE_RESULT_VARIABLE_FMP *)(CapsuleResult + 1);
-          if (CompareGuid(&CapsuleResultFmp->UpdateImageTypeId, &ImageHeader->UpdateImageTypeId) &&
-              (CapsuleResultFmp->UpdateImageIndex == ImageHeader->UpdateImageIndex) &&
-              (CapsuleResultFmp->PayloadIndex == PayloadIndex) ) {
-            return TRUE;
-          }
-        }
-      }
-    }
-  }
-
-  return FALSE;
-}
-
-/**
-  Write a new capsule status variable cache.
-
-  @param[in] CapsuleResult      The capsule status variable
-  @param[in] CapsuleResultSize  The size of the capsule stauts variable in bytes
-
-  @retval EFI_SUCCESS          The capsule status variable is cached.
-  @retval EFI_OUT_OF_RESOURCES No resource to cache the capsule status variable.
-**/
-EFI_STATUS
-WriteNewCapsuleResultVariableCache (
-  IN VOID    *CapsuleResult,
-  IN UINTN   CapsuleResultSize
-  )
-{
-  if (CapsuleResultSize > sizeof(CAPSULE_RESULT_VARIABLE_CACHE)) {
-    CapsuleResultSize = sizeof(CAPSULE_RESULT_VARIABLE_CACHE);
-  }
-
-  if (mCapsuleResultVariableCacheCount == mCapsuleResultVariableCacheMaxCount) {
-    mCapsuleResultVariableCache = ReallocatePool(
-                                    mCapsuleResultVariableCacheMaxCount * sizeof(CAPSULE_RESULT_VARIABLE_CACHE),
-                                    (mCapsuleResultVariableCacheMaxCount + CAPSULE_RESULT_VARIABLE_CACHE_COUNT) * sizeof(CAPSULE_RESULT_VARIABLE_CACHE),
-                                    mCapsuleResultVariableCache
-                                    );
-    if (mCapsuleResultVariableCache == NULL) {
-      return EFI_OUT_OF_RESOURCES;
-    }
-    mCapsuleResultVariableCacheMaxCount += CAPSULE_RESULT_VARIABLE_CACHE_COUNT;
-  }
-
-  ASSERT(mCapsuleResultVariableCacheCount < mCapsuleResultVariableCacheMaxCount);
-  ASSERT(mCapsuleResultVariableCache != NULL);
-  CopyMem(
-    &mCapsuleResultVariableCache[mCapsuleResultVariableCacheCount],
-    CapsuleResult,
-    CapsuleResultSize
-    );
-  mCapsuleResultVariableCacheCount++;
-
-  return EFI_SUCCESS;
-}
-
 /**
   Get a new capsule status variable index.
 
@@ -262,11 +165,7 @@ RecordCapsuleStatusVariable (
   gRT->GetTime(&CapsuleResultVariable.CapsuleProcessed, NULL);
   CapsuleResultVariable.CapsuleStatus = CapsuleStatus;
 
-  //
-  // Save Local Cache
-  //
-  Status = WriteNewCapsuleResultVariableCache(&CapsuleResultVariable, sizeof(CapsuleResultVariable));
-
+  Status = EFI_SUCCESS;
   if ((CapsuleHeader->Flags & CAPSULE_FLAGS_PERSIST_ACROSS_RESET) != 0) {
     Status = WriteNewCapsuleResultVariable(&CapsuleResultVariable, sizeof(CapsuleResultVariable));
   }
@@ -338,11 +237,7 @@ RecordFmpCapsuleStatusVariable (
     DevicePathStr = NULL;
   }
 
-  //
-  // Save Local Cache
-  //
-  Status = WriteNewCapsuleResultVariableCache(CapsuleResultVariable, CapsuleResultVariableSize);
-
+  Status = EFI_SUCCESS;
   if ((CapsuleHeader->Flags & CAPSULE_FLAGS_PERSIST_ACROSS_RESET) != 0) {
     Status = WriteNewCapsuleResultVariable(CapsuleResultVariable, CapsuleResultVariableSize);
   }
diff --git a/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleReportLibNull.c b/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleReportLibNull.c
index 6ab198d0f1..a6860ef45d 100644
--- a/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleReportLibNull.c
+++ b/MdeModulePkg/Library/DxeCapsuleLibFmp/DxeCapsuleReportLibNull.c
@@ -19,26 +19,6 @@
 #include <Guid/FmpCapsule.h>
 #include <Library/CapsuleLib.h>
 
-/**
-  Check if this FMP capsule is processed.
-
-  @param[in] CapsuleHeader  The capsule image header
-  @param[in] PayloadIndex   FMP payload index
-  @param[in] ImageHeader    FMP image header
-
-  @retval TRUE  This FMP capsule is processed.
-  @retval FALSE This FMP capsule is not processed.
-**/
-BOOLEAN
-IsFmpCapsuleProcessed (
-  IN EFI_CAPSULE_HEADER                            *CapsuleHeader,
-  IN UINTN                                         PayloadIndex,
-  IN EFI_FIRMWARE_MANAGEMENT_CAPSULE_IMAGE_HEADER  *ImageHeader
-  )
-{
-  return FALSE;
-}
-
 /**
   Record capsule status variable and to local cache.