From dc99315b8732b6e3032d01319d3f534d440b43d0 Mon Sep 17 00:00:00 2001
From: Jeff Fan <jeff.fan@intel.com>
Date: Tue, 22 Mar 2016 10:42:12 +0800
Subject: [PATCH] UefiCpuPkg/PiSmmCpuDxeSmm: Using MSRs semaphores in aligned
 buffer

Update MSRs semaphores to the ones in allocated aligned semaphores
buffer. If MSRs semaphores is not enough, allocate one page more.

Cc: Michael Kinney <michael.d.kinney@intel.com>
Cc: Feng Tian <feng.tian@intel.com>
Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeff Fan <jeff.fan@intel.com>
Reviewed-by: Feng Tian <feng.tian@intel.com>
Reviewed-by: Michael Kinney <michael.d.kinney@intel.com>
Regression-tested-by: Laszlo Ersek <lersek@redhat.com>
---
 UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c          | 32 ++++++++++++++++++----
 UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.h |  4 ++-
 2 files changed, 30 insertions(+), 6 deletions(-)

diff --git a/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c b/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
index 22c133202c..5e63f8aab2 100644
--- a/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
+++ b/UefiCpuPkg/PiSmmCpuDxeSmm/CpuS3.c
@@ -57,7 +57,7 @@ VOID                         *mGdtForAp = NULL;
 VOID                         *mIdtForAp = NULL;
 VOID                         *mMachineCheckHandlerForAp = NULL;
 MP_MSR_LOCK                  *mMsrSpinLocks = NULL;
-UINTN                        mMsrSpinLockCount = MSR_SPIN_LOCK_INIT_NUM;
+UINTN                        mMsrSpinLockCount;
 UINTN                        mMsrCount = 0;
 
 /**
@@ -76,7 +76,7 @@ GetMsrSpinLockByIndex (
   UINTN     Index;
   for (Index = 0; Index < mMsrCount; Index++) {
     if (MsrIndex == mMsrSpinLocks[Index].MsrIndex) {
-      return &mMsrSpinLocks[Index].SpinLock;
+      return mMsrSpinLocks[Index].SpinLock;
     }
   }
   return NULL;
@@ -93,30 +93,52 @@ InitMsrSpinLockByIndex (
   IN UINT32      MsrIndex
   )
 {
+  UINTN    MsrSpinLockCount;
   UINTN    NewMsrSpinLockCount;
+  UINTN    Index;
+  UINTN    AddedSize;
 
   if (mMsrSpinLocks == NULL) {
-    mMsrSpinLocks = (MP_MSR_LOCK *) AllocatePool (sizeof (MP_MSR_LOCK) * mMsrSpinLockCount);
+    MsrSpinLockCount = mSmmCpuSemaphores.SemaphoreMsr.AvailableCounter;
+    mMsrSpinLocks = (MP_MSR_LOCK *) AllocatePool (sizeof (MP_MSR_LOCK) * MsrSpinLockCount);
     ASSERT (mMsrSpinLocks != NULL);
+    for (Index = 0; Index < MsrSpinLockCount; Index++) {
+      mMsrSpinLocks[Index].SpinLock =
+       (SPIN_LOCK *)((UINTN)mSmmCpuSemaphores.SemaphoreMsr.Msr + Index * mSemaphoreSize);
+      mMsrSpinLocks[Index].MsrIndex = (UINT32)-1;
+    }
+    mMsrSpinLockCount = MsrSpinLockCount;
+    mSmmCpuSemaphores.SemaphoreMsr.AvailableCounter = 0;
   }
   if (GetMsrSpinLockByIndex (MsrIndex) == NULL) {
     //
     // Initialize spin lock for MSR programming
     //
     mMsrSpinLocks[mMsrCount].MsrIndex = MsrIndex;
-    InitializeSpinLock (&mMsrSpinLocks[mMsrCount].SpinLock);
+    InitializeSpinLock (mMsrSpinLocks[mMsrCount].SpinLock);
     mMsrCount ++;
     if (mMsrCount == mMsrSpinLockCount) {
       //
       // If MSR spin lock buffer is full, enlarge it
       //
-      NewMsrSpinLockCount = mMsrSpinLockCount + MSR_SPIN_LOCK_INIT_NUM;
+      AddedSize = SIZE_4KB;
+      mSmmCpuSemaphores.SemaphoreMsr.Msr =
+                        AllocatePages (EFI_SIZE_TO_PAGES(AddedSize));
+      ASSERT (mSmmCpuSemaphores.SemaphoreMsr.Msr != NULL);
+      NewMsrSpinLockCount = mMsrSpinLockCount + AddedSize / mSemaphoreSize;
       mMsrSpinLocks = ReallocatePool (
                         sizeof (MP_MSR_LOCK) * mMsrSpinLockCount,
                         sizeof (MP_MSR_LOCK) * NewMsrSpinLockCount,
                         mMsrSpinLocks
                         );
+      ASSERT (mMsrSpinLocks != NULL);
       mMsrSpinLockCount = NewMsrSpinLockCount;
+      for (Index = mMsrCount; Index < mMsrSpinLockCount; Index++) {
+        mMsrSpinLocks[Index].SpinLock =
+                 (SPIN_LOCK *)((UINTN)mSmmCpuSemaphores.SemaphoreMsr.Msr +
+                 (Index - mMsrCount)  * mSemaphoreSize);
+        mMsrSpinLocks[Index].MsrIndex = (UINT32)-1;
+      }
     }
   }
 }
diff --git a/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.h b/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.h
index ad2109fde8..b6c57e3f16 100644
--- a/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.h
+++ b/UefiCpuPkg/PiSmmCpuDxeSmm/PiSmmCpuDxeSmm.h
@@ -326,7 +326,7 @@ typedef struct {
 #define MSR_SPIN_LOCK_INIT_NUM 15
 
 typedef struct {
-  SPIN_LOCK    SpinLock;
+  SPIN_LOCK    *SpinLock;
   UINT32       MsrIndex;
 } MP_MSR_LOCK;
 
@@ -409,6 +409,8 @@ extern UINTN                               mSmmStackArrayEnd;
 extern UINTN                               mSmmStackSize;
 extern EFI_SMM_CPU_SERVICE_PROTOCOL        mSmmCpuService;
 extern IA32_DESCRIPTOR                     gcSmiInitGdtr;
+extern SMM_CPU_SEMAPHORES                  mSmmCpuSemaphores;
+extern UINTN                               mSemaphoreSize;
 extern SPIN_LOCK                           *mPFLock;
 extern SPIN_LOCK                           *mConfigSmmCodeAccessCheckLock;
 
-- 
2.39.2