When status code handler unregister itself, status code dispatch in status code router driver will have problem to get next handler. Fixed the issue in status code router.

Signed-off-by: li-elvin
Reviewed-by: lgao4


git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@12068 6f19259b-4bc3-4df7-8a09-765794883524

MdeModulePkg/Universal/ReportStatusCodeRouter/Pei/ReportStatusCodeRouterPei.c

@@ -1,7 +1,7 @@
 /** @file
   Report Status Code Router PEIM which produces Report Stataus Code Handler PPI and Status Code PPI.
 
-  Copyright (c) 2009 - 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2009 - 2011, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
   which accompanies this distribution.  The full text of the license may be found at
@@ -95,19 +95,26 @@ Register (
   EFI_PEI_RSC_HANDLER_CALLBACK  *CallbackEntry;
   UINTN                         *NumberOfEntries;
   UINTN                         Index;
+  UINTN                         FreeEntryIndex;
   UINTN                         *FreePacket;
 
   if (Callback == NULL) {
     return EFI_INVALID_PARAMETER;
   }
 
-  Hob.Raw  = GetFirstGuidHob (&gStatusCodeCallbackGuid);
-  FreePacket = NULL;
+  Hob.Raw        = GetFirstGuidHob (&gStatusCodeCallbackGuid);
+  FreePacket     = NULL;
+  FreeEntryIndex = 0;
   while (Hob.Raw != NULL) {
     NumberOfEntries = GET_GUID_HOB_DATA (Hob);
     CallbackEntry   = (EFI_PEI_RSC_HANDLER_CALLBACK *) (NumberOfEntries + 1);
-    if (*NumberOfEntries < 64) {
+    if (FreePacket == NULL && *NumberOfEntries < 64) {
+      //
+      // If current total number of handlers does not exceed 64, put new handler
+      // at the last of packet
+      //
       FreePacket = NumberOfEntries;
+      FreeEntryIndex = *NumberOfEntries;
     }
     for (Index = 0; Index < *NumberOfEntries; Index++) {
       if (CallbackEntry[Index] == Callback) {
@@ -116,6 +123,14 @@ Register (
         //
         return EFI_ALREADY_STARTED;
       }
+      if (FreePacket == NULL && CallbackEntry[Index] == NULL) {
+        //
+        // If the total number of handlers in current packet is max value 64,
+        // search an entry with NULL pointer and fill new handler into this entry.
+        //  
+        FreePacket = NumberOfEntries;
+        FreeEntryIndex = Index;
+      }
     }
     Hob.Raw = GET_NEXT_HOB (Hob);
     Hob.Raw = GetNextGuidHob (&gStatusCodeCallbackGuid, Hob.Raw);
@@ -126,8 +141,15 @@ Register (
   }
 
   CallbackEntry = (EFI_PEI_RSC_HANDLER_CALLBACK *) (FreePacket + 1);
-  CallbackEntry[*FreePacket] = Callback;
-  *FreePacket += 1;
+  CallbackEntry[FreeEntryIndex] = Callback;
+  
+  if (*FreePacket == FreeEntryIndex) {
+    //
+    // If new registered callback is added as a new entry in the packet,
+    // increase the total number of handlers in the packet.
+    //
+    *FreePacket += 1;
+  }
 
   return EFI_SUCCESS;
 }
@@ -166,8 +188,10 @@ Unregister (
     CallbackEntry   = (EFI_PEI_RSC_HANDLER_CALLBACK *) (NumberOfEntries + 1);
     for (Index = 0; Index < *NumberOfEntries; Index++) {
       if (CallbackEntry[Index] == Callback) {
-        CallbackEntry[Index] = CallbackEntry[*NumberOfEntries - 1];
-        *NumberOfEntries -= 1;
+        //
+        // Set removed entry as NULL.
+        //
+        CallbackEntry[Index] = NULL;
         return EFI_SUCCESS;
       }
     }

MdeModulePkg/Universal/ReportStatusCodeRouter/RuntimeDxe/ReportStatusCodeRouterRuntimeDxe.c

@@ -252,9 +252,12 @@ ReportDispatcher (
     return EFI_DEVICE_ERROR;
   }
 
-  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link); Link = GetNextNode (&mCallbackListHead, Link)) {
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link);) {
     CallbackEntry = CR (Link, RSC_HANDLER_CALLBACK_ENTRY, Node, RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
-
+    //
+    // The handler may remove itself, so get the next handler in advance.
+    //
+    Link = GetNextNode (&mCallbackListHead, Link);
     if ((CallbackEntry->Tpl == TPL_HIGH_LEVEL) || EfiAtRuntime ()) {
       CallbackEntry->RscHandlerCallback (
                        Type,

MdeModulePkg/Universal/ReportStatusCodeRouter/Smm/ReportStatusCodeRouterSmm.c

@@ -2,7 +2,7 @@
   Report Status Code Router Driver which produces SMM Report Stataus Code Handler Protocol
   and SMM Status Code Protocol.
 
-  Copyright (c) 2009 - 2010, Intel Corporation. All rights reserved.<BR>
+  Copyright (c) 2009 - 2011, Intel Corporation. All rights reserved.<BR>
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
   which accompanies this distribution.  The full text of the license may be found at
@@ -165,9 +165,12 @@ ReportDispatcher (
     return EFI_DEVICE_ERROR;
   }
 
-  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link); Link = GetNextNode (&mCallbackListHead, Link)) {
+  for (Link = GetFirstNode (&mCallbackListHead); !IsNull (&mCallbackListHead, Link);) {
     CallbackEntry = CR (Link, SMM_RSC_HANDLER_CALLBACK_ENTRY, Node, SMM_RSC_HANDLER_CALLBACK_ENTRY_SIGNATURE);
-
+    //
+    // The handler may remove itself, so get the next handler in advance.
+    //
+    Link = GetNextNode (&mCallbackListHead, Link);
     CallbackEntry->RscHandlerCallback (
                      Type,
                      Value,