]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Core/Pei/Dispatcher/Dispatcher.c
Merge Temporary Ram support patch.
[mirror_edk2.git] / MdeModulePkg / Core / Pei / Dispatcher / Dispatcher.c
index 73a6245b7e9001d8cc88e4092147afeccc35b5f9..cf9ea5d173fe1a78ddb813cc405f3a2d3f014fcd 100644 (file)
@@ -28,13 +28,6 @@ typedef struct {
   EFI_HANDLE            Handle;\r
 } PEIM_FILE_HANDLE_EXTENDED_DATA;\r
 \r
-STATIC\r
-VOID\r
-InvokePeiCore (\r
-  VOID          *Context1,\r
-  VOID          *Context2\r
-  );\r
-\r
 VOID\r
 DiscoverPeimsAndOrderWithApriori (\r
   IN  PEI_CORE_INSTANCE    *Private,\r
@@ -55,7 +48,7 @@ Returns:
 \r
   NONE\r
 \r
---*/  \r
+--*/\r
 {\r
   EFI_STATUS                          Status;\r
   EFI_PEI_FV_HANDLE                   FileHandle;\r
@@ -90,20 +83,20 @@ Returns:
   //\r
   for (PeimCount = 0; PeimCount < FixedPcdGet32 (PcdPeiCoreMaxPeimPerFv); PeimCount++) {\r
     Status = PeiFindFileEx (\r
-                VolumeHandle, \r
-                NULL, \r
-                PEI_CORE_INTERNAL_FFS_FILE_DISPATCH_TYPE, \r
+                VolumeHandle,\r
+                NULL,\r
+                PEI_CORE_INTERNAL_FFS_FILE_DISPATCH_TYPE,\r
                 &FileHandle,\r
                 &AprioriFileHandle\r
                 );\r
     if (Status != EFI_SUCCESS) {\r
       break;\r
     }\r
-    \r
+\r
     Private->CurrentFvFileHandles[PeimCount] = FileHandle;\r
   }\r
 \r
-  Private->AprioriCount = 0; \r
+  Private->AprioriCount = 0;\r
   if (AprioriFileHandle != NULL) {\r
     //\r
     // Read the Apriori file\r
@@ -114,21 +107,21 @@ Returns:
       // Calculate the number of PEIMs in the A Priori list\r
       //\r
       Private->AprioriCount = *(UINT32 *)(((EFI_FFS_FILE_HEADER *)AprioriFileHandle)->Size) & 0x00FFFFFF;\r
-      Private->AprioriCount -= sizeof (EFI_FFS_FILE_HEADER) - sizeof (EFI_COMMON_SECTION_HEADER);   \r
+      Private->AprioriCount -= sizeof (EFI_FFS_FILE_HEADER) - sizeof (EFI_COMMON_SECTION_HEADER);\r
       Private->AprioriCount /= sizeof (EFI_GUID);\r
-        \r
+\r
       SetMem (FileGuid, sizeof (FileGuid), 0);\r
       for (Index = 0; Index < PeimCount; Index++) {\r
         //\r
         // Make an array of file name guids that matches the FileHandle array so we can convert\r
         // quickly from file name to file handle\r
         //\r
-        CopyMem (&FileGuid[Index], &((EFI_FFS_FILE_HEADER *)Private->CurrentFvFileHandles[Index])->Name,sizeof(EFI_GUID));    \r
+        CopyMem (&FileGuid[Index], &((EFI_FFS_FILE_HEADER *)Private->CurrentFvFileHandles[Index])->Name,sizeof(EFI_GUID));\r
       }\r
 \r
       //\r
       // Walk through FileGuid array to find out who is invalid PEIM guid in Apriori file.\r
-      // Add avalible PEIMs in Apriori file into TempFileHandles array at first. \r
+      // Add avalible PEIMs in Apriori file into TempFileHandles array at first.\r
       //\r
       Index2 = 0;\r
       for (Index = 0; Index2 < Private->AprioriCount; Index++) {\r
@@ -139,7 +132,7 @@ Returns:
           }\r
         }\r
         if (Guid == NULL) {\r
-          break;  \r
+          break;\r
         }\r
         PeimIndex = ((UINTN)Guid - (UINTN)&FileGuid[0])/sizeof (EFI_GUID);\r
         TempFileHandles[Index] = Private->CurrentFvFileHandles[PeimIndex];\r
@@ -154,7 +147,7 @@ Returns:
       // Update valid Aprioricount\r
       //\r
       Private->AprioriCount = Index;\r
-      \r
+\r
       //\r
       // Add in any PEIMs not in the Apriori file\r
       //\r
@@ -171,11 +164,11 @@ Returns:
       //Index the end of array contains re-range Pei moudle.\r
       //\r
       TempFileHandles[Index] = NULL;\r
-      \r
+\r
       //\r
       // Private->CurrentFvFileHandles is currently in PEIM in the FV order.\r
-      // We need to update it to start with files in the A Priori list and \r
-      // then the remaining files in PEIM order. \r
+      // We need to update it to start with files in the A Priori list and\r
+      // then the remaining files in PEIM order.\r
       //\r
       CopyMem (Private->CurrentFvFileHandles, TempFileHandles, sizeof (Private->CurrentFvFileHandles));\r
     }\r
@@ -186,7 +179,46 @@ Returns:
   //\r
   Private->Fv[Private->CurrentPeimFvCount].ScanFv = TRUE;\r
   CopyMem (Private->Fv[Private->CurrentPeimFvCount].FvFileHandles, Private->CurrentFvFileHandles, sizeof (Private->CurrentFvFileHandles));\r
-  \r
+\r
+}\r
+\r
+VOID*\r
+ShadowPeiCore(\r
+  EFI_PEI_SERVICES     **PeiServices,\r
+  PEI_CORE_INSTANCE    *PrivateInMem\r
+  )\r
+{\r
+  EFI_PEI_FILE_HANDLE  PeiCoreFileHandle;\r
+  EFI_PHYSICAL_ADDRESS EntryPoint;\r
+  EFI_STATUS           Status;\r
+  UINT32               AuthenticationState;\r
+\r
+  PeiCoreFileHandle = NULL;\r
+\r
+  //\r
+  // Find the PEI Core in the BFV\r
+  //\r
+  Status = PeiFindFileEx (\r
+             (EFI_PEI_FV_HANDLE)PrivateInMem->Fv[0].FvHeader,\r
+             NULL,\r
+             EFI_FV_FILETYPE_PEI_CORE,\r
+             &PeiCoreFileHandle,\r
+             NULL\r
+             );\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
+  //\r
+  // Shadow PEI Core into memory so it will run faster\r
+  //\r
+  Status = PeiLoadImage (\r
+              PeiServices,\r
+              *((EFI_PEI_FILE_HANDLE*)&PeiCoreFileHandle),\r
+              &EntryPoint,\r
+              &AuthenticationState\r
+              );\r
+  ASSERT_EFI_ERROR (Status);\r
+\r
+  return (VOID*) ((UINTN) EntryPoint + (UINTN) PeiCore - (UINTN) _ModuleEntryPoint);\r
 }\r
 \r
 VOID\r
@@ -220,9 +252,8 @@ Returns:
   UINT32                              Index1;\r
   UINT32                              Index2;\r
   EFI_PEI_SERVICES                    **PeiServices;\r
-  VOID                                *PrivateInMem;\r
-  EFI_PEI_FV_HANDLE                   VolumeHandle; \r
-  EFI_PEI_FILE_HANDLE                 PeiCoreFileHandle;\r
+  EFI_PEI_FV_HANDLE                   VolumeHandle;\r
+  //EFI_PHYSICAL_ADDRESS                PeiCoreFileHandle;\r
   EFI_PEI_FILE_HANDLE                 PeimFileHandle;\r
   UINTN                               FvCount;\r
   UINTN                               PeimCount;\r
@@ -234,10 +265,19 @@ Returns:
   UINTN                               SaveCurrentPeimCount;\r
   UINTN                               SaveCurrentFvCount;\r
   EFI_PEI_FILE_HANDLE                 SaveCurrentFileHandle;\r
-  VOID                                *TopOfStack;\r
-  PEI_CORE_PARAMETERS                 PeiCoreParameters;\r
   PEIM_FILE_HANDLE_EXTENDED_DATA      ExtendedData;\r
+  EFI_PHYSICAL_ADDRESS                NewPermenentMemoryBase;\r
+  TEMPORARY_RAM_SUPPORT_PPI           *TemporaryRamSupportPpi;\r
+  EFI_HOB_HANDOFF_INFO_TABLE          *OldHandOffTable;\r
+  EFI_HOB_HANDOFF_INFO_TABLE          *NewHandOffTable;\r
+  INTN                                Offset;\r
+  PEI_CORE_INSTANCE                   *PrivateInMem;\r
+  UINT64                              NewPeiStackSize;\r
+  UINT64                              OldPeiStackSize;\r
+  UINT64                              StackGap;\r
   EFI_FV_FILE_INFO                    FvFileInfo;\r
+  UINTN                               OldCheckingTop;\r
+  UINTN                               OldCheckingBottom;\r
 \r
 \r
   PeiServices = &Private->PS;\r
@@ -257,11 +297,11 @@ Returns:
     for (Index1 = 0; Index1 <= SaveCurrentFvCount; Index1++) {\r
       for (Index2 = 0; (Index2 < FixedPcdGet32 (PcdPeiCoreMaxPeimPerFv)) && (Private->Fv[Index1].FvFileHandles[Index2] != NULL); Index2++) {\r
         if (Private->Fv[Index1].PeimState[Index2] == PEIM_STATE_REGISITER_FOR_SHADOW) {\r
-          PeimFileHandle = Private->Fv[Index1].FvFileHandles[Index2];  \r
+          PeimFileHandle = Private->Fv[Index1].FvFileHandles[Index2];\r
           Status = PeiLoadImage (\r
-                    &Private->PS, \r
-                    PeimFileHandle,  \r
-                    &EntryPoint, \r
+                    &Private->PS,\r
+                    PeimFileHandle,\r
+                    &EntryPoint,\r
                     &AuthenticationState\r
                     );\r
           if (Status == EFI_SUCCESS) {\r
@@ -270,18 +310,18 @@ Returns:
             //\r
             Private->Fv[Index1].PeimState[Index2]++;\r
             Private->CurrentFileHandle   = PeimFileHandle;\r
-            Private->CurrentPeimFvCount  = Index1;        \r
-            Private->CurrentPeimCount    = Index2;        \r
+            Private->CurrentPeimFvCount  = Index1;\r
+            Private->CurrentPeimCount    = Index2;\r
             //\r
             // Call the PEIM entry point\r
             //\r
             PeimEntryPoint = (EFI_PEIM_ENTRY_POINT2)(UINTN)EntryPoint;\r
-            \r
+\r
             PERF_START (0, "PEIM", NULL, 0);\r
             PeimEntryPoint(PeimFileHandle, (const EFI_PEI_SERVICES **) &Private->PS);\r
             PERF_END (0, "PEIM", NULL, 0);\r
-          } \r
-          \r
+          }\r
+\r
           //\r
           // Process the Notify list and dispatch any notifies for\r
           // newly installed PPIs.\r
@@ -290,9 +330,9 @@ Returns:
         }\r
       }\r
     }\r
-    Private->CurrentFileHandle  = SaveCurrentFileHandle;   \r
-    Private->CurrentPeimFvCount = SaveCurrentFvCount;    \r
-    Private->CurrentPeimCount   = SaveCurrentPeimCount;    \r
+    Private->CurrentFileHandle  = SaveCurrentFileHandle;\r
+    Private->CurrentPeimFvCount = SaveCurrentFvCount;\r
+    Private->CurrentPeimCount   = SaveCurrentPeimCount;\r
   }\r
 \r
   //\r
@@ -314,7 +354,7 @@ Returns:
       if (Private->CurrentPeimCount == 0) {\r
         //\r
         // When going through each FV, at first, search Apriori file to\r
-        // reorder all PEIMs to ensure the PEIMs in Apriori file to get \r
+        // reorder all PEIMs to ensure the PEIMs in Apriori file to get\r
         // dispatch at first.\r
         //\r
         DiscoverPeimsAndOrderWithApriori (Private, VolumeHandle);\r
@@ -323,8 +363,8 @@ Returns:
       //\r
       // Start to dispatch all modules within the current Fv.\r
       //\r
-      for (PeimCount = Private->CurrentPeimCount; \r
-           (PeimCount < FixedPcdGet32 (PcdPeiCoreMaxPeimPerFv)) && (Private->CurrentFvFileHandles[PeimCount] != NULL); \r
+      for (PeimCount = Private->CurrentPeimCount;\r
+           (PeimCount < FixedPcdGet32 (PcdPeiCoreMaxPeimPerFv)) && (Private->CurrentFvFileHandles[PeimCount] != NULL);\r
            PeimCount++) {\r
         Private->CurrentPeimCount  = PeimCount;\r
         PeimFileHandle = Private->CurrentFileHandle = Private->CurrentFvFileHandles[PeimCount];\r
@@ -345,16 +385,17 @@ Returns:
               // For PEIM driver, Load its entry point\r
               //\r
               Status = PeiLoadImage (\r
-                         PeiServices, \r
-                         PeimFileHandle,  \r
-                         &EntryPoint, \r
+                         PeiServices,\r
+                         PeimFileHandle,\r
+                         &EntryPoint,\r
                          &AuthenticationState\r
                          );\r
             }\r
 \r
             if ((Status == EFI_SUCCESS)) {\r
               //\r
-              // The PEIM has its dependencies satisfied, and is processed.\r
+              // The PEIM has its dependencies satisfied, and its entry point\r
+              // has been found, so invoke it.\r
               //\r
               PERF_START (0, "PEIM", NULL, 0);\r
 \r
@@ -373,7 +414,7 @@ Returns:
                 // PEIM_STATE_NOT_DISPATCHED move to PEIM_STATE_DISPATCHED\r
                 //\r
                 Private->Fv[FvCount].PeimState[PeimCount]++;\r
-                \r
+\r
                 if (FvFileInfo.FileType != EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE) {\r
                   //\r
                   // Call the PEIM entry point for PEIM driver\r
@@ -393,102 +434,203 @@ Returns:
                 );\r
               PERF_END (0, "PEIM", NULL, 0);\r
 \r
-            } else {\r
+            }\r
+\r
+            if (Private->SwitchStackSignal) {\r
+\r
               //\r
-              // If PeiLoadImage fails, the section extraction PPI or Decompress PPI may not be ready,\r
-              // we flag that more Peims need to be dispatched.\r
+              // Reserve the size of new stack at bottom of physical memory\r
               //\r
-              PeimNeedingDispatch = TRUE;\r
-            }\r
+              OldPeiStackSize = Private->StackSize;\r
+              NewPeiStackSize = (RShiftU64 (Private->PhysicalMemoryLength, 1) + EFI_PAGE_MASK) & ~EFI_PAGE_MASK;\r
+              if (FixedPcdGet32(PcdPeiCoreMaxPeiStackSize) > (UINT32) NewPeiStackSize) {\r
+                Private->StackSize = NewPeiStackSize;\r
+              } else {\r
+                Private->StackSize = FixedPcdGet32(PcdPeiCoreMaxPeiStackSize);\r
+              }\r
 \r
-            //\r
-            // Process the Notify list and dispatch any notifies for\r
-            // newly installed PPIs.\r
-            //\r
-            ProcessNotifyList (Private);\r
+              //\r
+              // In theory, the size of new stack in permenent memory should large than\r
+              // size of old stack in temporary memory.\r
+              // But if new stack is smaller than the size of old stack, we also reserve\r
+              // the size of old stack at bottom of permenent memory.\r
+              //\r
+              StackGap = 0;\r
+              if (Private->StackSize > OldPeiStackSize) {\r
+                StackGap = Private->StackSize - OldPeiStackSize;\r
+              }\r
 \r
-            //\r
-            // If permanent memory was discovered and installed by this\r
-            // PEIM, shadow PEI Core and switch the stacks to the new memory.  \r
-            //\r
-            if (Private->SwitchStackSignal) {\r
+              //\r
+              // Update HandOffHob for new installed permenent memory\r
+              //\r
+              OldHandOffTable   = Private->HobList.HandoffInformationTable;\r
+              OldCheckingBottom = (UINTN)OldHandOffTable;\r
+              OldCheckingTop    = (UINTN)(OldCheckingBottom + SecCoreData->TemporaryRamSize);\r
 \r
               //\r
-              // Make sure we don't retry the same PEIM that added memory\r
+              // The whole temporary memory will be migrated to physical memory.\r
+              // CAUTION: The new base is computed accounding to gap of new stack.\r
               //\r
-              Private->CurrentPeimCount++;\r
+              NewPermenentMemoryBase = Private->PhysicalMemoryBegin + StackGap;\r
+              Offset                 = (UINTN) NewPermenentMemoryBase - (UINTN) SecCoreData->TemporaryRamBase;\r
+              NewHandOffTable        = (EFI_HOB_HANDOFF_INFO_TABLE *)((UINTN)OldHandOffTable + Offset);\r
+              PrivateInMem           = (PEI_CORE_INSTANCE *)((UINTN) (VOID*) Private + Offset);\r
 \r
               //\r
-              // Migrate IDT from CAR into real memory, so after stack switches to\r
-              // the new memory, the caller can get memory version PeiServiceTable. \r
+              // TemporaryRamSupportPpi is produced by platform's SEC\r
               //\r
-              MigrateIdtTable (PeiServices);\r
+              Status = PeiLocatePpi (\r
+                         (CONST EFI_PEI_SERVICES **) PeiServices,\r
+                         &gEfiTemporaryRamSupportPpiGuid,\r
+                         0,\r
+                         NULL,\r
+                         (VOID**)&TemporaryRamSupportPpi\r
+                         );\r
+\r
 \r
+              if (!EFI_ERROR (Status)) {\r
+                TemporaryRamSupportPpi->TemporaryRamMigration (\r
+                                          (CONST EFI_PEI_SERVICES **) PeiServices,\r
+                                          (EFI_PHYSICAL_ADDRESS)(UINTN) SecCoreData->TemporaryRamBase,\r
+                                          (EFI_PHYSICAL_ADDRESS)(UINTN) NewPermenentMemoryBase,\r
+                                          SecCoreData->TemporaryRamSize\r
+                                          );\r
+\r
+              } else {\r
+                CopyMem (\r
+                  (VOID*)(UINTN) NewPermenentMemoryBase,\r
+                  SecCoreData->TemporaryRamBase,\r
+                  SecCoreData->TemporaryRamSize\r
+                  );\r
+              }\r
+\r
+\r
+              //\r
               //\r
-              // Since we are at dispatch level, only the Core's private data \r
-              // is preserved, nobody else should have any data on the stack. \r
-              // So we need to copy PEI core instance data to memory.\r
+              // Fixup the PeiCore's private data\r
               //\r
-              PrivateInMem = AllocateCopyPool (sizeof (PEI_CORE_INSTANCE), Private);\r
-              ASSERT (PrivateInMem != NULL);\r
+              PrivateInMem->PS          = &PrivateInMem->ServiceTableShadow;\r
+              PrivateInMem->CpuIo       = &PrivateInMem->ServiceTableShadow.CpuIo;\r
+              PrivateInMem->HobList.Raw = (VOID*) ((UINTN) PrivateInMem->HobList.Raw + Offset);\r
+              PrivateInMem->StackBase   = (EFI_PHYSICAL_ADDRESS)(((UINTN)PrivateInMem->PhysicalMemoryBegin + EFI_PAGE_MASK) & ~EFI_PAGE_MASK);\r
+\r
+              PeiServices = &PrivateInMem->PS;\r
+\r
+              //\r
+              // Fixup for PeiService's address\r
+              //\r
+              SetPeiServicesTablePointer(PeiServices);\r
+\r
+              //\r
+              // Update HandOffHob for new installed permenent memory\r
+              //\r
+              NewHandOffTable->EfiEndOfHobList =\r
+                (EFI_PHYSICAL_ADDRESS)(VOID*)((UINTN) NewHandOffTable->EfiEndOfHobList + Offset);\r
+              NewHandOffTable->EfiMemoryTop        = PrivateInMem->PhysicalMemoryBegin +\r
+                                                     PrivateInMem->PhysicalMemoryLength;\r
+              NewHandOffTable->EfiMemoryBottom     = PrivateInMem->PhysicalMemoryBegin;\r
+              NewHandOffTable->EfiFreeMemoryTop    = PrivateInMem->FreePhysicalMemoryTop;\r
+              NewHandOffTable->EfiFreeMemoryBottom = NewHandOffTable->EfiEndOfHobList +\r
+                                                     sizeof (EFI_HOB_GENERIC_HEADER);\r
+\r
+              //\r
+              // We need convert the PPI desciptor's pointer\r
+              //\r
+              ConvertPpiPointers ((CONST EFI_PEI_SERVICES **)PeiServices, \r
+                                  OldCheckingBottom, \r
+                                  OldCheckingTop, \r
+                                  NewHandOffTable);\r
+\r
+              DEBUG ((EFI_D_INFO, "Stack Hob: BaseAddress=0x%X Length=0x%X\n",\r
+                                  (UINTN)PrivateInMem->StackBase,\r
+                                  PrivateInMem->StackSize));\r
+              BuildStackHob (PrivateInMem->StackBase, PrivateInMem->StackSize);\r
+\r
+              //\r
+              // After the whole temporary memory is migrated, then we can allocate page in\r
+              // permenent memory.\r
+              //\r
+              PrivateInMem->PeiMemoryInstalled     = TRUE;\r
+\r
+              //\r
+              // Make sure we don't retry the same PEIM that added memory\r
+              //\r
+              PrivateInMem->CurrentPeimCount++;\r
 \r
               //\r
               // Shadow PEI Core. When permanent memory is avaiable, shadow\r
               // PEI Core and PEIMs to get high performance.\r
               //\r
-              PeiCoreFileHandle = NULL;\r
+              //PeiCoreFileHandle = 0;\r
               //\r
               // Find the PEI Core in the BFV\r
               //\r
-              Status = PeiFindFileEx (\r
-                        (EFI_PEI_FV_HANDLE)Private->Fv[0].FvHeader, \r
-                        NULL, \r
-                        EFI_FV_FILETYPE_PEI_CORE, \r
-                        &PeiCoreFileHandle,\r
-                        NULL\r
-                        );\r
-              ASSERT_EFI_ERROR (Status);\r
-  \r
+              //Status = PeiFindFileEx (\r
+              //          (EFI_PEI_FV_HANDLE)PrivateInMem->Fv[0].FvHeader,\r
+              //          NULL,\r
+               //         EFI_FV_FILETYPE_PEI_CORE,\r
+              //          (EFI_PEI_FILE_HANDLE*)&PeiCoreFileHandle,\r
+              //          NULL\r
+              //          );\r
+              //ASSERT_EFI_ERROR (Status);\r
+\r
               //\r
               // Shadow PEI Core into memory so it will run faster\r
               //\r
-              Status = PeiLoadImage (PeiServices, PeiCoreFileHandle, &EntryPoint, &AuthenticationState);\r
-              ASSERT_EFI_ERROR (Status);\r
-  \r
+              //Status = PeiLoadImage (\r
+              //           PeiServices,\r
+              //           *((EFI_PEI_FILE_HANDLE*)&PeiCoreFileHandle),\r
+              //           &EntryPoint,\r
+               //          &AuthenticationState\r
+               //          );\r
+              //ASSERT_EFI_ERROR (Status);\r
+\r
+              //PrivateInMem->ShadowedPeiCore = (VOID*) ((UINTN) EntryPoint +\r
+              //                                         (UINTN) PeiCore    -\r
+              //                                         (UINTN) _ModuleEntryPoint);\r
+              PrivateInMem->ShadowedPeiCore = ShadowPeiCore (\r
+                                                PeiServices,\r
+                                                PrivateInMem\r
+                                                );\r
               //\r
-              // Switch to memory based stack and reenter PEI Core that has been\r
-              //  shadowed to memory.\r
+              // Process the Notify list and dispatch any notifies for\r
+              // newly installed PPIs.\r
               //\r
+              ProcessNotifyList (PrivateInMem);\r
+\r
               //\r
-              // Adjust the top of stack to be aligned at CPU_STACK_ALIGNMENT\r
+              // Entry PEI Phase 2\r
               //\r
-              TopOfStack = (VOID *)((UINTN)Private->StackBase + (UINTN)Private->StackSize - CPU_STACK_ALIGNMENT);\r
-              TopOfStack = ALIGN_POINTER (TopOfStack, CPU_STACK_ALIGNMENT);\r
-              \r
-              PeiCoreParameters.SecCoreData = SecCoreData;\r
-              PeiCoreParameters.PpiList     = NULL;\r
-              PeiCoreParameters.Data        = PrivateInMem;\r
-              ASSERT (PeiCoreParameters.Data != 0);\r
+              PeiCore (SecCoreData, NULL, PrivateInMem);\r
 \r
-              PeiSwitchStacks (\r
-                InvokePeiCore,\r
-                (VOID*) ((UINTN) EntryPoint + ((UINTN) PeiCore - (UINTN) _ModuleEntryPoint)),\r
-                (VOID*) &PeiCoreParameters,  \r
-                TopOfStack,\r
-                (VOID*)(UINTN)Private->StackBase\r
-                );\r
+              //((PEI_CORE_ENTRY_POINT) (UINTN) PrivateInMem->ShadowedPeiCore) (\r
+              //                                        SecCoreData,\r
+              //                                        NULL,\r
+              //                                        PrivateInMem\r
+              //                                        );\r
+\r
+              //\r
+              // Code should not come here\r
+              //\r
+              ASSERT_EFI_ERROR(FALSE);\r
             }\r
 \r
+            //\r
+            // Process the Notify list and dispatch any notifies for\r
+            // newly installed PPIs.\r
+            //\r
+            ProcessNotifyList (Private);\r
+\r
             if ((Private->PeiMemoryInstalled) && (Private->Fv[FvCount].PeimState[PeimCount] == PEIM_STATE_REGISITER_FOR_SHADOW) &&   \\r
                 (Private->HobList.HandoffInformationTable->BootMode != BOOT_ON_S3_RESUME)) {\r
               //\r
-              // If memory is availble we shadow images by default for performance reasons. \r
-              // We call the entry point a 2nd time so the module knows it's shadowed. \r
+              // If memory is availble we shadow images by default for performance reasons.\r
+              // We call the entry point a 2nd time so the module knows it's shadowed.\r
               //\r
               //PERF_START (PeiServices, L"PEIM", PeimFileHandle, 0);\r
               PeimEntryPoint (PeimFileHandle, (const EFI_PEI_SERVICES **) PeiServices);\r
               //PERF_END (PeiServices, L"PEIM", PeimFileHandle, 0);\r
-              \r
+\r
               //\r
               // PEIM_STATE_REGISITER_FOR_SHADOW move to PEIM_STATE_DONE\r
               //\r
@@ -518,16 +660,16 @@ Returns:
     }\r
 \r
     //\r
-    // Before making another pass, we should set Private->CurrentPeimFvCount =0 to go \r
+    // Before making another pass, we should set Private->CurrentPeimFvCount =0 to go\r
     // through all the FV.\r
     //\r
     Private->CurrentPeimFvCount = 0;\r
 \r
     //\r
-    // PeimNeedingDispatch being TRUE means we found a PEIM that did not get \r
+    // PeimNeedingDispatch being TRUE means we found a PEIM that did not get\r
     //  dispatched. So we need to make another pass\r
     //\r
-    // PeimDispatchOnThisPass being TRUE means we dispatched a PEIM on this \r
+    // PeimDispatchOnThisPass being TRUE means we dispatched a PEIM on this\r
     //  pass. If we did not dispatch a PEIM there is no point in trying again\r
     //  as it will fail the next time too (nothing has changed).\r
     //\r
@@ -602,13 +744,13 @@ Returns:
     //\r
     return TRUE;\r
   }\r
-  \r
+\r
   //\r
-  // Depex section not in the encapsulated section. \r
+  // Depex section not in the encapsulated section.\r
   //\r
   Status = PeiServicesFfsFindSectionData (\r
               EFI_SECTION_PEI_DEPEX,\r
-              FileHandle, \r
+              FileHandle,\r
               (VOID **)&DepexData\r
               );\r
 \r
@@ -630,12 +772,12 @@ Returns:
   discovery permanent memory.\r
 \r
        @param FileHandle       File handle of a PEIM.\r
-  \r
+\r
   @retval EFI_NOT_FOUND                                The file handle doesn't point to PEIM itself.\r
   @retval EFI_ALREADY_STARTED          Indicate that the PEIM has been registered itself.\r
   @retval EFI_SUCCESS                                          Successfully to register itself.\r
 \r
-**/ \r
+**/\r
 EFI_STATUS\r
 EFIAPI\r
 PeiRegisterForShadow (\r
@@ -658,52 +800,13 @@ PeiRegisterForShadow (
     //\r
     return EFI_ALREADY_STARTED;\r
   }\r
-  \r
+\r
   Private->Fv[Private->CurrentPeimFvCount].PeimState[Private->CurrentPeimCount] = PEIM_STATE_REGISITER_FOR_SHADOW;\r
 \r
   return EFI_SUCCESS;\r
 }\r
 \r
 \r
-/**\r
-  This routine invoke the PeiCore's entry in new stack environment.\r
-\r
-       @param Context1         The first context parameter is entry of PeiCore\r
-  @param Context2      The second context parameter is parameter structure point for PeiCore\r
-\r
-**/ \r
-STATIC\r
-VOID\r
-InvokePeiCore (\r
-  VOID          *Context1,\r
-  VOID          *Context2\r
-  )\r
-{\r
-  PEI_CORE_ENTRY_POINT  PeiCoreEntryPoint;\r
-  PEI_CORE_PARAMETERS       *PeiCoreParameters;\r
-\r
-  //\r
-  // Running on new stack in SEC Core\r
-  //\r
-\r
-  PeiCoreEntryPoint = (PEI_CORE_ENTRY_POINT) (UINTN) Context1;\r
-  PeiCoreParameters = (PEI_CORE_PARAMETERS *)Context2;\r
-\r
-  //\r
-  // Call PEI Core using new stack\r
-  //\r
-  PeiCoreEntryPoint (\r
-    PeiCoreParameters->SecCoreData,\r
-    PeiCoreParameters->PpiList,\r
-    PeiCoreParameters->Data\r
-    );\r
-\r
-  //\r
-  // Never returns\r
-  //\r
-  ASSERT (FALSE);\r
-  CpuDeadLoop ();\r
-}\r
 \r
 /**\r
   Get Fv image from the FV type file, then install FV INFO ppi, Build FV hob.\r
@@ -712,7 +815,7 @@ InvokePeiCore (
        @param FileHandle               File handle of a Fv type file.\r
   @param AuthenticationState  Pointer to attestation authentication state of image.\r
 \r
-  \r
+\r
   @retval EFI_NOT_FOUND                                FV image can't be found.\r
   @retval EFI_SUCCESS                                          Successfully to process it.\r
 \r
@@ -730,12 +833,12 @@ ProcessFvFile (
   UINT32                FvAlignment;\r
   VOID                  *FvBuffer;\r
   EFI_PEI_HOB_POINTERS  HobFv2;\r
-  \r
+\r
   FvBuffer             = NULL;\r
   *AuthenticationState = 0;\r
 \r
   //\r
-  // Check if this EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE file has already \r
+  // Check if this EFI_FV_FILETYPE_FIRMWARE_VOLUME_IMAGE file has already\r
   // been extracted.\r
   //\r
   HobFv2.Raw = GetHobList ();\r
@@ -748,7 +851,7 @@ ProcessFvFile (
     }\r
     HobFv2.Raw = GET_NEXT_HOB (HobFv2);\r
   }\r
-  \r
+\r
   //\r
   // Find FvImage in FvFile\r
   //\r
@@ -758,7 +861,7 @@ ProcessFvFile (
              FvFileHandle,\r
              (VOID **)&FvImageHandle\r
              );\r
-  \r
+\r
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
@@ -774,7 +877,7 @@ ProcessFvFile (
   if (FvAlignment < 8) {\r
     FvAlignment = 8;\r
   }\r
-  // \r
+  //\r
   // Check FvImage\r
   //\r
   if ((UINTN) FvImageInfo.FvStart % FvAlignment != 0) {\r
@@ -788,7 +891,7 @@ ProcessFvFile (
     //\r
     PeiFfsGetVolumeInfo ((EFI_PEI_FV_HANDLE) FvBuffer, &FvImageInfo);\r
   }\r
-  \r
+\r
   //\r
   // Install FvPpi and Build FvHob\r
   //\r
@@ -817,6 +920,6 @@ ProcessFvFile (
     &FvImageInfo.FvName,\r
     &(((EFI_FFS_FILE_HEADER *)FvFileHandle)->Name)\r
     );\r
-    \r
+\r
   return EFI_SUCCESS;\r
 }\r