diff --git a/StandaloneMmPkg/Core/Mmi.c b/StandaloneMmPkg/Core/Mmi.c
index 0de6fd17fc..e035245c87 100644
--- a/StandaloneMmPkg/Core/Mmi.c
+++ b/StandaloneMmPkg/Core/Mmi.c
@@ -34,11 +34,51 @@ typedef struct {
   LIST_ENTRY                    Link;        // Link on MMI_ENTRY.MmiHandlers
   EFI_MM_HANDLER_ENTRY_POINT    Handler;     // The mm handler's entry point
   MMI_ENTRY                     *MmiEntry;
+  BOOLEAN                       ToRemove;    // To remove this MMI_HANDLER later
 } MMI_HANDLER;
 
+//
+// mMmiManageCallingDepth is used to track the depth of recursive calls of MmiManage.
+//
+UINTN  mMmiManageCallingDepth = 0;
+
 LIST_ENTRY  mRootMmiHandlerList = INITIALIZE_LIST_HEAD_VARIABLE (mRootMmiHandlerList);
 LIST_ENTRY  mMmiEntryList       = INITIALIZE_LIST_HEAD_VARIABLE (mMmiEntryList);
 
+/**
+  Remove MmiHandler and free the memory it used.
+  If MmiEntry is empty, remove MmiEntry and free the memory it used.
+
+  @param  MmiHandler  Points to MMI handler.
+  @param  MmiEntry    Points to MMI Entry or NULL for root MMI handlers.
+
+  @retval TRUE        MmiEntry is removed.
+  @retval FALSE       MmiEntry is not removed.
+**/
+BOOLEAN
+RemoveMmiHandler (
+  IN MMI_HANDLER  *MmiHandler,
+  IN MMI_ENTRY    *MmiEntry
+  )
+{
+  ASSERT (MmiHandler->ToRemove);
+  RemoveEntryList (&MmiHandler->Link);
+  FreePool (MmiHandler);
+
+  //
+  // Remove the MMI_ENTRY if all handlers have been removed.
+  //
+  if (MmiEntry != NULL) {
+    if (IsListEmpty (&MmiEntry->MmiHandlers)) {
+      RemoveEntryList (&MmiEntry->AllEntries);
+      FreePool (MmiEntry);
+      return TRUE;
+    }
+  }
+
+  return FALSE;
+}
+
 /**
   Finds the MMI entry for the requested handler type.
 
@@ -126,13 +166,16 @@ MmiManage (
 {
   LIST_ENTRY   *Link;
   LIST_ENTRY   *Head;
+  LIST_ENTRY   *EntryLink;
   MMI_ENTRY    *MmiEntry;
   MMI_HANDLER  *MmiHandler;
-  BOOLEAN      SuccessReturn;
+  EFI_STATUS   ReturnStatus;
+  BOOLEAN      WillReturn;
   EFI_STATUS   Status;
 
-  Status        = EFI_NOT_FOUND;
-  SuccessReturn = FALSE;
+  mMmiManageCallingDepth++;
+  Status       = EFI_NOT_FOUND;
+  ReturnStatus = Status;
   if (HandlerType == NULL) {
     //
     // Root MMI handler
@@ -171,7 +214,16 @@ MmiManage (
         // no additional handlers will be processed and EFI_INTERRUPT_PENDING will be returned.
         //
         if (HandlerType != NULL) {
-          return EFI_INTERRUPT_PENDING;
+          ReturnStatus = EFI_INTERRUPT_PENDING;
+          WillReturn   = TRUE;
+        } else {
+          //
+          // If any other handler's result sets ReturnStatus as EFI_SUCCESS, the return status
+          // will be EFI_SUCCESS.
+          //
+          if (ReturnStatus != EFI_SUCCESS) {
+            ReturnStatus = Status;
+          }
         }
 
         break;
@@ -183,10 +235,10 @@ MmiManage (
         // additional handlers will be processed.
         //
         if (HandlerType != NULL) {
-          return EFI_SUCCESS;
+          WillReturn = TRUE;
         }
 
-        SuccessReturn = TRUE;
+        ReturnStatus = EFI_SUCCESS;
         break;
 
       case EFI_WARN_INTERRUPT_SOURCE_QUIESCED:
@@ -194,7 +246,7 @@ MmiManage (
         // If at least one of the handlers returns EFI_WARN_INTERRUPT_SOURCE_QUIESCED
         // then the function will return EFI_SUCCESS.
         //
-        SuccessReturn = TRUE;
+        ReturnStatus = EFI_SUCCESS;
         break;
 
       case EFI_WARN_INTERRUPT_SOURCE_PENDING:
@@ -202,6 +254,10 @@ MmiManage (
         // If all the handlers returned EFI_WARN_INTERRUPT_SOURCE_PENDING
         // then EFI_WARN_INTERRUPT_SOURCE_PENDING will be returned.
         //
+        if (ReturnStatus != EFI_SUCCESS) {
+          ReturnStatus = Status;
+        }
+
         break;
 
       default:
@@ -211,13 +267,76 @@ MmiManage (
         ASSERT_EFI_ERROR (Status);
         break;
     }
+
+    if (WillReturn) {
+      break;
+    }
   }
 
-  if (SuccessReturn) {
-    Status = EFI_SUCCESS;
+  ASSERT (mMmiManageCallingDepth > 0);
+  mMmiManageCallingDepth--;
+
+  //
+  // MmiHandlerUnRegister() calls from MMI handlers are deferred till this point.
+  // Before returned from MmiManage, delete the MmiHandler which is
+  // marked as ToRemove.
+  // Note that MmiManage can be called recursively.
+  //
+  if (mMmiManageCallingDepth == 0) {
+    //
+    // Go through all MmiHandler in root Mmi handlers
+    //
+    for ( Link = GetFirstNode (&mRootMmiHandlerList)
+          ; !IsNull (&mRootMmiHandlerList, Link);
+          )
+    {
+      //
+      // MmiHandler might be removed in below, so cache the next link in Link
+      //
+      MmiHandler = CR (Link, MMI_HANDLER, Link, MMI_HANDLER_SIGNATURE);
+      Link       = GetNextNode (&mRootMmiHandlerList, Link);
+      if (MmiHandler->ToRemove) {
+        //
+        // Remove MmiHandler if the ToRemove is set.
+        //
+        RemoveMmiHandler (MmiHandler, NULL);
+      }
+    }
+
+    //
+    // Go through all MmiHandler in non-root MMI handlers
+    //
+    for ( EntryLink = GetFirstNode (&mMmiEntryList)
+          ; !IsNull (&mMmiEntryList, EntryLink);
+          )
+    {
+      //
+      // MmiEntry might be removed in below, so cache the next link in EntryLink
+      //
+      MmiEntry  = CR (EntryLink, MMI_ENTRY, AllEntries, MMI_ENTRY_SIGNATURE);
+      EntryLink = GetNextNode (&mMmiEntryList, EntryLink);
+      for ( Link = GetFirstNode (&MmiEntry->MmiHandlers)
+            ; !IsNull (&MmiEntry->MmiHandlers, Link);
+            )
+      {
+        //
+        // MmiHandler might be removed in below, so cache the next link in Link
+        //
+        MmiHandler = CR (Link, MMI_HANDLER, Link, MMI_HANDLER_SIGNATURE);
+        Link       = GetNextNode (&MmiEntry->MmiHandlers, Link);
+        if (MmiHandler->ToRemove) {
+          //
+          // Remove MmiHandler if the ToRemove is set.
+          //
+          if (RemoveMmiHandler (MmiHandler, MmiEntry)) {
+            break;
+          }
+        }
+      }
+    }
   }
 
-  return Status;
+  return ReturnStatus;
 }
 
 /**
@@ -254,6 +373,7 @@ MmiHandlerRegister (
 
   MmiHandler->Signature = MMI_HANDLER_SIGNATURE;
   MmiHandler->Handler   = Handler;
+  MmiHandler->ToRemove  = FALSE;
 
   if (HandlerType == NULL) {
     //
@@ -309,26 +429,17 @@ MmiHandlerUnRegister (
     return EFI_INVALID_PARAMETER;
   }
 
-  MmiEntry = MmiHandler->MmiEntry;
-
-  RemoveEntryList (&MmiHandler->Link);
-  FreePool (MmiHandler);
-
-  if (MmiEntry == NULL) {
+  MmiHandler->ToRemove = TRUE;
+  if (mMmiManageCallingDepth > 0) {
     //
-    // This is root MMI handler
+    // This function is called from MmiManage()
+    // Do not delete or remove MmiHandler or MmiEntry now.
     //
     return EFI_SUCCESS;
   }
 
-  if (IsListEmpty (&MmiEntry->MmiHandlers)) {
-    //
-    // No handler registered for this interrupt now, remove the MMI_ENTRY
-    //
-    RemoveEntryList (&MmiEntry->AllEntries);
-
-    FreePool (MmiEntry);
-  }
+  MmiEntry = MmiHandler->MmiEntry;
+  RemoveMmiHandler (MmiHandler, MmiEntry);
 
   return EFI_SUCCESS;
 }