1 files changed, 15 insertions, 4 deletions
diff --git a/ArmPlatformPkg/PrePi/MainMPCore.c b/ArmPlatformPkg/PrePi/MainMPCore.c
index e9a3ef56a0..2c18f9f3c3 100644
--- a/ArmPlatformPkg/PrePi/MainMPCore.c
+++ b/ArmPlatformPkg/PrePi/MainMPCore.c
@@ -59,6 +59,13 @@ PrimaryMain (
     ASSERT_EFI_ERROR (Status);
   DEBUG_CODE_END();
 
+  // Check PcdGicPrimaryCoreId has been set in case the Primary Core is not the core 0 of Cluster 0
+  DEBUG_CODE_BEGIN();
+  if ((PcdGet32(PcdArmPrimaryCore) != 0) && (PcdGet32 (PcdGicPrimaryCoreId) == 0)) {
+    DEBUG((EFI_D_WARN,"Warning: the PCD PcdGicPrimaryCoreId does not seem to be set up for the configuration.\n"));
+  }
+  DEBUG_CODE_END();
+
   // Enable the GIC Distributor
   ArmGicEnableDistributor(PcdGet32(PcdGicDistributorBase));
 
@@ -88,6 +95,7 @@ SecondaryMain (
   UINT32                  CoreId;
   VOID                    (*SecondaryStart)(VOID);
   UINTN                   SecondaryEntryAddr;
+  UINTN                   AcknowledgedCoreId;
 
   ClusterId = GET_CLUSTER_ID(MpId);
   CoreId    = GET_CORE_ID(MpId);
@@ -113,12 +121,15 @@ SecondaryMain (
   // Clear Secondary cores MailBox
   MmioWrite32 (ArmCoreInfoTable[Index].MailboxClearAddress, ArmCoreInfoTable[Index].MailboxClearValue);
 
-  SecondaryEntryAddr = 0;
-  while (SecondaryEntryAddr = MmioRead32 (ArmCoreInfoTable[Index].MailboxGetAddress), SecondaryEntryAddr == 0) {
+  do {
     ArmCallWFI ();
+
+    // Read the Mailbox
+    SecondaryEntryAddr = MmioRead32 (ArmCoreInfoTable[Index].MailboxGetAddress);
+
     // Acknowledge the interrupt and send End of Interrupt signal.
-    ArmGicAcknowledgeSgiFrom (PcdGet32(PcdGicInterruptInterfaceBase), PRIMARY_CORE_ID);
-  }
+    ArmGicAcknowledgeInterrupt (PcdGet32(PcdGicDistributorBase), PcdGet32(PcdGicInterruptInterfaceBase), &AcknowledgedCoreId, NULL);
+  } while ((SecondaryEntryAddr == 0) && (AcknowledgedCoreId != PcdGet32 (PcdGicPrimaryCoreId)));
 
   // Jump to secondary core entry point.
   SecondaryStart = (VOID (*)())SecondaryEntryAddr;