2 files changed, 10 insertions, 6 deletions
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
index e61ace9bad..0a74d448c8 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/CpuFeaturesInitialize.c
@@ -269,8 +269,10 @@ CpuInitDataInitialize (
     DEBUG ((DEBUG_INFO, "Package: %d, Valid Core : %d\n", Index, ValidCoreCountPerPackage[Index]));
   }
 
-  CpuFeaturesData->CpuFlags.SemaphoreCount = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount);
-  ASSERT (CpuFeaturesData->CpuFlags.SemaphoreCount != NULL);
+  CpuFeaturesData->CpuFlags.CoreSemaphoreCount = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount);
+  ASSERT (CpuFeaturesData->CpuFlags.CoreSemaphoreCount != NULL);
+  CpuFeaturesData->CpuFlags.PackageSemaphoreCount = AllocateZeroPool (sizeof (UINT32) * CpuStatus->PackageCount * CpuStatus->MaxCoreCount * CpuStatus->MaxThreadCount);
+  ASSERT (CpuFeaturesData->CpuFlags.PackageSemaphoreCount != NULL);
 
   //
   // Get support and configuration PCDs
@@ -963,9 +965,9 @@ ProgramProcessorRegister (
       //  V(0...n)       V(0...n)      ...           V(0...n)
       //  n * P(0)       n * P(1)      ...           n * P(n)
       //
-      SemaphorePtr = CpuFlags->SemaphoreCount;
       switch (RegisterTableEntry->Value) {
       case CoreDepType:
+        SemaphorePtr = CpuFlags->CoreSemaphoreCount;
         //
         // Get Offset info for the first thread in the core which current thread belongs to.
         //
@@ -986,6 +988,7 @@ ProgramProcessorRegister (
         break;
 
       case PackageDepType:
+        SemaphorePtr = CpuFlags->PackageSemaphoreCount;
         ValidCoreCountPerPackage = (UINT32 *)(UINTN)CpuStatus->ValidCoreCountPerPackage;
         //
         // Get Offset info for the first thread in the package which current thread belongs to.
diff --git a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
index 0a67a0581a..cf3da84837 100644
--- a/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
+++ b/UefiCpuPkg/Library/RegisterCpuFeaturesLib/RegisterCpuFeatures.h
@@ -60,9 +60,10 @@ typedef struct {
 // Flags used when program the register.
 //
 typedef struct {
-  volatile UINTN           ConsoleLogLock;       // Spinlock used to control console.
-  volatile UINTN           MemoryMappedLock;     // Spinlock used to program mmio
-  volatile UINT32          *SemaphoreCount;      // Semaphore used to program semaphore.
+  volatile UINTN           ConsoleLogLock;          // Spinlock used to control console.
+  volatile UINTN           MemoryMappedLock;        // Spinlock used to program mmio
+  volatile UINT32          *CoreSemaphoreCount;     // Semaphore containers used to program Core semaphore.
+  volatile UINT32          *PackageSemaphoreCount;  // Semaphore containers used to program Package semaphore.
 } PROGRAM_CPU_REGISTER_FLAGS;
 
 typedef struct {