UefiCpuPkg CpuCommFeaturesLib: Fix GP fault issue about ProcTrace

[mirror_edk2.git] / UefiCpuPkg / Library / CpuCommonFeaturesLib / ProcTrace.c

diff --git a/UefiCpuPkg/Library/CpuCommonFeaturesLib/ProcTrace.c b/UefiCpuPkg/Library/CpuCommonFeaturesLib/ProcTrace.c
index b98eb116b7b6da2f51070900e9b22e278a23070c..611459105d38eeddb818a78e6278b250cda2b858 100644 (file)
--- a/UefiCpuPkg/Library/CpuCommonFeaturesLib/ProcTrace.c
+++ b/UefiCpuPkg/Library/CpuCommonFeaturesLib/ProcTrace.c
@@ -29,9 +29,11 @@ typedef enum {
 } RTIT_OUTPUT_SCHEME;\r
 \r
 typedef struct  {\r
-  BOOLEAN  ProcTraceSupported;\r
-  BOOLEAN  TopaSupported;\r
-  BOOLEAN  SingleRangeSupported;\r
+  BOOLEAN                                   TopaSupported;\r
+  BOOLEAN                                   SingleRangeSupported;\r
+  MSR_IA32_RTIT_CTL_REGISTER                RtitCtrl;\r
+  MSR_IA32_RTIT_OUTPUT_BASE_REGISTER        RtitOutputBase;\r
+  MSR_IA32_RTIT_OUTPUT_MASK_PTRS_REGISTER   RtitOutputMaskPtrs;\r
 } PROC_TRACE_PROCESSOR_DATA;\r
 \r
 typedef struct  {\r
@@ -44,7 +46,6 @@ typedef struct  {
   UINTN                       AllocatedThreads;\r
 \r
   UINTN                       *TopaMemArray;\r
-  UINTN                       TopaMemArrayCount;\r
 \r
   PROC_TRACE_PROCESSOR_DATA   *ProcessorData;\r
 } PROC_TRACE_DATA;\r
@@ -124,8 +125,7 @@ ProcTraceSupport (
   // Check if Processor Trace is supported\r
   //\r
   AsmCpuidEx (CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS, 0, NULL, &Ebx.Uint32, NULL, NULL);\r
-  ProcTraceData->ProcessorData[ProcessorNumber].ProcTraceSupported = (BOOLEAN) (Ebx.Bits.IntelProcessorTrace == 1);\r
-  if (!ProcTraceData->ProcessorData[ProcessorNumber].ProcTraceSupported) {\r
+  if (Ebx.Bits.IntelProcessorTrace == 0) {\r
     return FALSE;\r
   }\r
 \r
@@ -134,6 +134,9 @@ ProcTraceSupport (
   ProcTraceData->ProcessorData[ProcessorNumber].SingleRangeSupported = (BOOLEAN) (Ecx.Bits.SingleRangeOutput == 1);\r
   if ((ProcTraceData->ProcessorData[ProcessorNumber].TopaSupported && (ProcTraceData->ProcTraceOutputScheme == RtitOutputSchemeToPA)) ||\r
       (ProcTraceData->ProcessorData[ProcessorNumber].SingleRangeSupported && (ProcTraceData->ProcTraceOutputScheme == RtitOutputSchemeSingleRange))) {\r
+    ProcTraceData->ProcessorData[ProcessorNumber].RtitCtrl.Uint64 = AsmReadMsr64 (MSR_IA32_RTIT_CTL);\r
+    ProcTraceData->ProcessorData[ProcessorNumber].RtitOutputBase.Uint64 = AsmReadMsr64 (MSR_IA32_RTIT_OUTPUT_BASE);\r
+    ProcTraceData->ProcessorData[ProcessorNumber].RtitOutputMaskPtrs.Uint64 = AsmReadMsr64 (MSR_IA32_RTIT_OUTPUT_MASK_PTRS);\r
     return TRUE;\r
   }\r
 \r
@@ -202,7 +205,7 @@ ProcTraceInitialize (
   //\r
   // Clear MSR_IA32_RTIT_CTL[0] and IA32_RTIT_STS only if MSR_IA32_RTIT_CTL[0]==1b\r
   //\r
-  CtrlReg.Uint64 = AsmReadMsr64 (MSR_IA32_RTIT_CTL);\r
+  CtrlReg.Uint64 = ProcTraceData->ProcessorData[ProcessorNumber].RtitCtrl.Uint64;\r
   if (CtrlReg.Bits.TraceEn != 0) {\r
     ///\r
     /// Clear bit 0 in MSR IA32_RTIT_CTL (570)\r
@@ -251,9 +254,9 @@ ProcTraceInitialize (
     //\r
     //   Let BSP allocate and create the necessary memory region (Aligned to the size of\r
     //   the memory region from setup option(ProcTraceMemSize) which is an integral multiple of 4kB)\r
-    //   for the all the enabled threads for storing Processor Trace debug data. Then Configure the trace\r
+    //   for all the enabled threads to store Processor Trace debug data. Then Configure the trace\r
     //   address base in MSR, IA32_RTIT_OUTPUT_BASE (560h) bits 47:12. Note that all regions must be\r
-    //   aligned based on their size, not just 4K. Thus a 2M region must have bits 20:12 clear.\r
+    //   aligned based on their size, not just 4K. Thus a 2M region must have bits 20:12 cleared.\r
     //\r
     ThreadMemRegionTable = (UINTN *) AllocatePool (ProcTraceData->NumberOfProcessors * sizeof (UINTN *));\r
     if (ThreadMemRegionTable == NULL) {\r
@@ -284,13 +287,12 @@ ProcTraceInitialize (
     }\r
 \r
     DEBUG ((DEBUG_INFO, "ProcTrace: Allocated PT mem for %d thread \n", ProcTraceData->AllocatedThreads));\r
-    MemRegionBaseAddr = ThreadMemRegionTable[0];\r
+  }\r
+\r
+  if (ProcessorNumber < ProcTraceData->AllocatedThreads) {\r
+    MemRegionBaseAddr = ProcTraceData->ThreadMemRegionTable[ProcessorNumber];\r
   } else {\r
-    if (ProcessorNumber < ProcTraceData->AllocatedThreads) {\r
-      MemRegionBaseAddr = ProcTraceData->ThreadMemRegionTable[ProcessorNumber];\r
-    } else {\r
-      return RETURN_SUCCESS;\r
-    }\r
+    return RETURN_SUCCESS;\r
   }\r
 \r
   ///\r
@@ -309,7 +311,6 @@ ProcTraceInitialize (
     //\r
     // Clear MSR IA32_RTIT_CTL (0x570) ToPA (Bit 8)\r
     //\r
-    CtrlReg.Uint64 = AsmReadMsr64 (MSR_IA32_RTIT_CTL);\r
     CtrlReg.Bits.ToPA = 0;\r
     CPU_REGISTER_TABLE_WRITE64 (\r
       ProcessorNumber,\r
@@ -321,6 +322,7 @@ ProcTraceInitialize (
     //\r
     // Program MSR IA32_RTIT_OUTPUT_BASE (0x560) bits[63:7] with the allocated Memory Region\r
     //\r
+    OutputBaseReg.Uint64 = ProcTraceData->ProcessorData[ProcessorNumber].RtitOutputBase.Uint64;\r
     OutputBaseReg.Bits.Base = (MemRegionBaseAddr >> 7) & 0x01FFFFFF;\r
     OutputBaseReg.Bits.BaseHi = RShiftU64 ((UINT64) MemRegionBaseAddr, 32) & 0xFFFFFFFF;\r
     CPU_REGISTER_TABLE_WRITE64 (\r
@@ -333,6 +335,7 @@ ProcTraceInitialize (
     //\r
     // Program the Mask bits for the Memory Region to MSR IA32_RTIT_OUTPUT_MASK_PTRS (561h)\r
     //\r
+    OutputMaskPtrsReg.Uint64 = ProcTraceData->ProcessorData[ProcessorNumber].RtitOutputMaskPtrs.Uint64;\r
     OutputMaskPtrsReg.Bits.MaskOrTableOffset = ((MemRegionSize - 1) >> 7) & 0x01FFFFFF;\r
     OutputMaskPtrsReg.Bits.OutputOffset = RShiftU64 (MemRegionSize - 1, 32) & 0xFFFFFFFF;\r
     CPU_REGISTER_TABLE_WRITE64 (\r
@@ -376,10 +379,10 @@ ProcTraceInitialize (
           if (Index < ProcTraceData->AllocatedThreads) {\r
             ProcTraceData->AllocatedThreads = Index;\r
           }\r
-          DEBUG ((DEBUG_ERROR, "ProcTrace:  Out of mem, allocating ToPA mem only for %d threads\n", ProcTraceData->AllocatedThreads));\r
+          DEBUG ((DEBUG_ERROR, "ProcTrace:  Out of mem, allocated ToPA mem only for %d threads\n", ProcTraceData->AllocatedThreads));\r
           if (Index == 0) {\r
             //\r
-            // Could not allocate for BSP\r
+            // Could not allocate for BSP even\r
             //\r
             FreePool ((VOID *) TopaMemArray);\r
             TopaMemArray = NULL;\r
@@ -393,29 +396,24 @@ ProcTraceInitialize (
       }\r
 \r
       DEBUG ((DEBUG_INFO, "ProcTrace: Allocated ToPA mem for %d thread \n", ProcTraceData->AllocatedThreads));\r
-      //\r
-      // BSP gets the first block\r
-      //\r
-      TopaTableBaseAddr = TopaMemArray[0];\r
+    }\r
+\r
+    if (ProcessorNumber < ProcTraceData->AllocatedThreads) {\r
+      TopaTableBaseAddr = ProcTraceData->TopaMemArray[ProcessorNumber];\r
     } else {\r
-      //\r
-      // Count for currently executing AP.\r
-      //\r
-      if (ProcessorNumber < ProcTraceData->AllocatedThreads) {\r
-        TopaTableBaseAddr = ProcTraceData->TopaMemArray[ProcessorNumber];\r
-      } else {\r
-        return RETURN_SUCCESS;\r
-      }\r
+      return RETURN_SUCCESS;\r
     }\r
 \r
     TopaTable = (PROC_TRACE_TOPA_TABLE *) TopaTableBaseAddr;\r
     TopaEntryPtr = &TopaTable->TopaEntry[0];\r
+    TopaEntryPtr->Uint64 = 0;\r
     TopaEntryPtr->Bits.Base = (MemRegionBaseAddr >> 12) & 0x000FFFFF;\r
     TopaEntryPtr->Bits.BaseHi = RShiftU64 ((UINT64) MemRegionBaseAddr, 32) & 0xFFFFFFFF;\r
     TopaEntryPtr->Bits.Size = ProcTraceData->ProcTraceMemSize;\r
     TopaEntryPtr->Bits.END = 0;\r
 \r
     TopaEntryPtr = &TopaTable->TopaEntry[1];\r
+    TopaEntryPtr->Uint64 = 0;\r
     TopaEntryPtr->Bits.Base = (TopaTableBaseAddr >> 12) & 0x000FFFFF;\r
     TopaEntryPtr->Bits.BaseHi = RShiftU64 ((UINT64) TopaTableBaseAddr, 32) & 0xFFFFFFFF;\r
     TopaEntryPtr->Bits.END = 1;\r
@@ -423,6 +421,7 @@ ProcTraceInitialize (
     //\r
     // Program the MSR IA32_RTIT_OUTPUT_BASE (0x560) bits[63:7] with ToPA base\r
     //\r
+    OutputBaseReg.Uint64 = ProcTraceData->ProcessorData[ProcessorNumber].RtitOutputBase.Uint64;\r
     OutputBaseReg.Bits.Base = (TopaTableBaseAddr >> 7) & 0x01FFFFFF;\r
     OutputBaseReg.Bits.BaseHi = RShiftU64 ((UINT64) TopaTableBaseAddr, 32) & 0xFFFFFFFF;\r
     CPU_REGISTER_TABLE_WRITE64 (\r
@@ -435,6 +434,7 @@ ProcTraceInitialize (
     //\r
     // Set the MSR IA32_RTIT_OUTPUT_MASK (0x561) bits[63:7] to 0\r
     //\r
+    OutputMaskPtrsReg.Uint64 = ProcTraceData->ProcessorData[ProcessorNumber].RtitOutputMaskPtrs.Uint64;\r
     OutputMaskPtrsReg.Bits.MaskOrTableOffset = 0;\r
     OutputMaskPtrsReg.Bits.OutputOffset = 0;\r
     CPU_REGISTER_TABLE_WRITE64 (\r
@@ -446,7 +446,6 @@ ProcTraceInitialize (
     //\r
     // Enable ToPA output scheme by enabling MSR IA32_RTIT_CTL (0x570) ToPA (Bit 8)\r
     //\r
-    CtrlReg.Uint64 = AsmReadMsr64 (MSR_IA32_RTIT_CTL);\r
     CtrlReg.Bits.ToPA = 1;\r
     CPU_REGISTER_TABLE_WRITE64 (\r
       ProcessorNumber,\r
@@ -459,7 +458,6 @@ ProcTraceInitialize (
   ///\r
   /// Enable the Processor Trace feature from MSR IA32_RTIT_CTL (570h)\r
   ///\r
-  CtrlReg.Uint64 = AsmReadMsr64 (MSR_IA32_RTIT_CTL);\r
   CtrlReg.Bits.OS = 1;\r
   CtrlReg.Bits.User = 1;\r
   CtrlReg.Bits.BranchEn = 1;\r