/** @file
  CPU feature control module

 @copyright
  INTEL CONFIDENTIAL
  Copyright 1999 - 2018 Intel Corporation.

  The source code contained or described herein and all documents related to the
  source code ("Material") are owned by Intel Corporation or its suppliers or
  licensors. Title to the Material remains with Intel Corporation or its suppliers
  and licensors. The Material may contain trade secrets and proprietary and
  confidential information of Intel Corporation and its suppliers and licensors,
  and is protected by worldwide copyright and trade secret laws and treaty
  provisions. No part of the Material may be used, copied, reproduced, modified,
  published, uploaded, posted, transmitted, distributed, or disclosed in any way
  without Intel's prior express written permission.

  No license under any patent, copyright, trade secret or other intellectual
  property right is granted to or conferred upon you by disclosure or delivery
  of the Materials, either expressly, by implication, inducement, estoppel or
  otherwise. Any license under such intellectual property rights must be
  express and approved by Intel in writing.

  Unless otherwise agreed by Intel in writing, you may not remove or alter
  this notice or any other notice embedded in Materials by Intel or
  Intel's suppliers or licensors in any way.

  This file contains an 'Intel Peripheral Driver' and is uniquely identified as
  "Intel Reference Module" and is licensed for Intel CPUs and chipsets under
  the terms of your license agreement with Intel or your vendor. This file may
  be modified by the user, subject to additional terms of the license agreement.

@par Specification
**/
#include <Library/ReportStatusCodeLib.h>

#include "Features.h"
#include "MachineCheck.h"
#include <ScAccess.h>
#include <Library/BaseLib.h>
#include <Library/PciLib.h>
#include <Library/IoLib.h>
#include <Library/CpuPlatformLib.h>
#include <Library/TimerLib.h>
#include <Private/Library/CpuCommonLib.h>
#include <Private/Library/CpuPowerOnConfigLib.h>
#include <Library/ConfigBlockLib.h>
#include <Private/CpuInitDataHob.h>
#include <Library/CpuPolicyLib.h>
#include <Library/PreSiliconLib.h>
#include <Library/PostCodeLib.h>
#include <Private/Library/SoftwareGuardLib.h>

//
// Global variables
//
GLOBAL_REMOVE_IF_UNREFERENCED EFI_PEI_MP_SERVICES_PPI      *gMpServicesPpi        = NULL;
GLOBAL_REMOVE_IF_UNREFERENCED MP_SYSTEM_DATA               *mMpSystemData         = NULL;
GLOBAL_REMOVE_IF_UNREFERENCED SI_CPU_POLICY_PPI            *mSiCpuPolicyPpi       = NULL;
GLOBAL_REMOVE_IF_UNREFERENCED CPU_CONFIG                   *mCpuConfig            = NULL;
GLOBAL_REMOVE_IF_UNREFERENCED POWER_MGMT_CONFIG            *mPowerMgmtConfig      = NULL;

#ifdef SGX_SUPPORT
SOFTWARE_GUARD_CONFIG       *mSoftwareGuardConfig  = NULL;
UINTN                       mMicroCodeBufferPointer = 0;
#endif

/**
  This function handles Cpu Initialization routine at the end of PEI

  @param[in] PeiServices   - Pointer to PEI Services Table.
  @param[in] NotifyDesc    - Pointer to the descriptor for the Notification event that
                             caused this function to execute.
  @param[in] Ppi           - Pointer to the PPI data associated with this function.

  @retval EFI_STATUS       - Always return EFI_SUCCESS
**/
STATIC
EFI_STATUS
CpuInitAtEndOfPei (
  IN EFI_PEI_SERVICES    **PeiServices,
  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
  IN VOID                      *Ppi
  );

STATIC EFI_PEI_NOTIFY_DESCRIPTOR mCpuInitAtEndOfPeiNotifyDesc[] = {
  { 
    (EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
    &gEfiEndOfPeiSignalPpiGuid,
    CpuInitAtEndOfPei
  }
};

/**
  Perform BIOS uCode PM_TMR Emulation Configuration by configuring MSR 121h and setting
  the ACPI Timer Disable Bit

  The uCode emulation of the ACPI Timer allows disabling of the ACPI Timer to have no
  impact on the system, with the exception that TMR_STS will not be set.  All aligned
  32-bit reads   to the ACPI Timer port are valid and will behave as if the ACPI timer
  remains enabled.
**/
VOID
BiosUcodePmTmrEmulationMsrCfg (
  VOID
  )
{
  UINT64 Data64;
  UINT16 ABase;
  
 ABase = (UINT16)PcdGet16(PcdScAcpiIoPortBaseAddress);

  Data64 = ((LShiftU64 (V_BIOS_UCODE_PM_TMR_EMULATION_CFG_CORRECTION_FACTOR,
                        N_BIOS_UCODE_PM_TMR_EMULATION_CFG_CORRECTION_FACTOR_OFFSET)) |
           ((B_BIOS_UCODE_PM_TMR_EMULATION_CFG_VALID |
		   (ABase + R_SC_ACPI_IO_PM1_TMR))));
  if (IsBsp ()) {
    DEBUG ((DEBUG_INFO, "ACPI Timer Emulation Config, writing 0x%lx to MSR 121h.\n", Data64));
  }
  AsmWriteMsr64 (MSR_BIOS_UCODE_PM_TMR_EMULATION_CFG, Data64);
}

/**
  Provide access to the CPU misc enables MSR

  @param[in] Enable  - Enable or Disable Misc Features
  @param[in] BitMask - The register bit offset of MSR MSR_IA32_MISC_ENABLE
**/
VOID
CpuMiscEnable (
  BOOLEAN Enable,
  UINT64  BitMask
  )
{
  UINT64 MsrValue;

  MsrValue = AsmReadMsr64 (MSR_IA32_MISC_ENABLE);
  if (Enable) {
    MsrValue |= BitMask;
  } else {
    MsrValue &= ~BitMask;
  }

  AsmWriteMsr64 (MSR_IA32_MISC_ENABLE, MsrValue);
}

///
/// DCA contains processor code and chipset code
/// CPU driver has the following assumption on the initialization flow
/// 1. Chipset pre-initialization should detect DCA support per chipset capability after SiCpuPolicy
/// 2. If not support, it should update SiCpuPolicy DCA to disable state
/// 3. If support, it should enable the DCA related registers
/// 4. CPU initialization for DCA (CPU may change SiCpuPolicy DCA states per CPU capability)
/// 5. Normal chipset driver (IOH) should look at SiCpuPolicy DCA policy again in PCI enumeration
/// 6. Chipset enable or disable DCA according to SiCpuPolicy DCA state
///
/**
  Detect DCA supported or not

  @retval DCA_SUPPORT if supported or 0 if not supported
**/
UINTN
IsDcaSupported (
  VOID
  )
{
  CPUID_VERSION_INFO_ECX Ecx;
  UINTN                  Support;

  Support = 0;
  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, &Ecx.Uint32, NULL);
  if (Ecx.Bits.DCA == 1) {
    Support = DCA_SUPPORT;
  }
  return Support;
}

/**
  Detect HT supported or not

  @retval HT_SUPPORT if supported or 0 if not supported
**/
UINTN
IsHTSupported (
  VOID
  )
{
  EFI_CPUID_REGISTER CpuidRegisters;
  UINTN              Support;

  Support = 0;

  AsmCpuidEx (
    CPUID_EXTENDED_TOPOLOGY,
    0,
    &CpuidRegisters.RegEax,
    &CpuidRegisters.RegEbx,
    &CpuidRegisters.RegEcx,
    &CpuidRegisters.RegEdx
    );
  if ((CpuidRegisters.RegEbx & 0x00FF) > 1) {
    Support = HT_SUPPORT;
  }
  return Support;

}

/**
  Detect if AES supported or not

  @retval AES_SUPPORT if supported or 0 if not supported
**/
UINTN
IsAesSupported (
  VOID
  )
{
  CPUID_VERSION_INFO_ECX Ecx;
  UINTN                  Support;

  Support = 0;
  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, &Ecx.Uint32, NULL);
  if (Ecx.Bits.AESNI == 1) {
    Support = AES_SUPPORT;
  }
  return Support;
}

/**
  Detect if XD supported or not

  @retval XD_SUPPORT if supported or 0 if not supported
**/
UINTN
IsXdSupported (
  VOID
  )
{
  UINT32                     RegEax;
  CPUID_EXTENDED_CPU_SIG_EDX Edx;
  UINTN                      Support;

  Support = 0;
  AsmCpuid (CPUID_EXTENDED_FUNCTION, &RegEax, NULL, NULL, NULL);
  if (RegEax >= CPUID_EXTENDED_CPU_SIG) {
    AsmCpuid (CPUID_EXTENDED_CPU_SIG, NULL, NULL, NULL, &Edx.Uint32);
    if (Edx.Bits.NX == 1) {
      ///
      /// Execute Disable Bit feature is supported on this processor.
      ///
      Support = XD_SUPPORT;
    }
  }
  return Support;
}

/**
  Program XD if supported or disable it if not supported

  @param[in] Support  - bitmap that indicate XD supported or not
**/
VOID
ProgramXd (
  IN UINTN Support
  )
{
  BOOLEAN XdSupport;

  XdSupport = (BOOLEAN) ((Support & XD_SUPPORT) == XD_SUPPORT);
  ///
  /// MSR MISC_ENABLE[34] has negative logic: 0 - XD Enabled, 1 - XD Disabled
  ///
  CpuMiscEnable (!XdSupport, B_MSR_IA32_MISC_ENABLE_XD);
}

/**
  Check on the processor if VMX/TXT is supported.

  @retval VMX_SUPPORT and/or TXT_SUPPORT if supported or 0 if neither supported
**/
UINTN
IsVmxSupported (
  VOID
  )
{
  CPUID_VERSION_INFO_ECX Ecx;
  UINTN                  Support;

  Support = 0;

  ///
  /// Get CPUID to check if the processor supports Vanderpool Technology.
  ///
  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, &Ecx.Uint32, NULL);
  if (Ecx.Bits.VMX == 1) {
    ///
    /// VT is supported.
    ///
    Support |= VMX_SUPPORT;
  }
  if (Ecx.Bits.SMX == 1) {
    ///
    /// TXT is supported.
    ///
    Support |= TXT_SUPPORT;
  }
  return Support;
}

/**
  Enable VMX/TXT on the processor.

  @param[in] Support  - To enable or disable VMX/TXT feature.
**/
VOID
EnableDisableVmx (
  IN UINTN Support
  )
{
  MSR_IA32_FEATURE_CONTROL_REGISTER Msr;

  Msr.Uint64 = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);
  Msr.Uint64 &= ~((UINT64) OPTION_FEATURE_RESERVED_MASK); 

  if (mMpSystemData->CommonFeatures & VMX_SUPPORT) {
    if (Support & VMX_SUPPORT) {
      Msr.Bits.EnableVmxOutsideSmx = 1;
    } else {
      Msr.Bits.EnableVmxOutsideSmx = 0;
    }
  }

  ///
  /// Check the Feature Lock Bit.
  /// If it is already set, which indicates we are executing POST
  /// due to a warm RESET (i.e., PWRGOOD was not de-asserted).
  ///
  if (Msr.Bits.Lock == 0) {
    AsmWriteMsr64 (MSR_IA32_FEATURE_CONTROL, Msr.Uint64);
  }
}

/**
  Enable / Disable AES on the processor.

  @param[in] Support  - To enable or disable AES feature.
**/
VOID
EnableDisableAes (
  IN UINTN Support
  )
{
  UINT64 MsrValue;

  if (!(mMpSystemData->CommonFeatures & AES_SUPPORT) || (IsSecondaryThread ())) {
    return;
  }

  ///
  /// The processor was manufactured with AES-NI feature
  ///
  MsrValue = AsmReadMsr64 (MSR_IA32_FEATURE_CONFIG);

  ///
  /// Check the Feature Lock Bit.
  /// If it is already set, which indicates we are executing POST
  /// due to a warm RESET (i.e., PWRGOOD was not de-asserted).
  ///
  if ((MsrValue & B_IA32_FEATURE_CONFIG_LOCK) == 0) {
    if (Support & AES_SUPPORT) {
      ///
      /// Enabled AES, writes of 00b, 01b pr 10b to the MSR will result in AES enable.
      /// Should lock this MSR always, so write 01b to the MSR.
      ///
      MsrValue &= (UINT64) ~B_IA32_FEATURE_CONFIG_AES_DIS;
      MsrValue |= B_IA32_FEATURE_CONFIG_LOCK;
    } else {
      ///
      /// To disable AES, system BIOS must write 11b to this MSR.
      ///
      MsrValue |= (UINT64) (B_IA32_FEATURE_CONFIG_AES_DIS + B_IA32_FEATURE_CONFIG_LOCK);
    }
    AsmWriteMsr64 (MSR_IA32_FEATURE_CONFIG, MsrValue);
  }
  return;
}

/**
  Check on the processor if Debug Interface is supported

  @retval Value of DEBUG_SUPPORT and DEBUG_LOCK_SUPPORT
**/
UINTN
IsDebugInterfaceSupported (
  VOID
  )
{
  UINTN              Support;
  EFI_CPUID_REGISTER CpuIdRegister;

  Support = 0;

  ///
  /// Debug interface is supported if CPUID (EAX=1): ECX[11] = 1,
  ///
  AsmCpuid (
    CPUID_VERSION_INFO,
    &CpuIdRegister.RegEax,
    &CpuIdRegister.RegEbx,
    &CpuIdRegister.RegEcx,
    &CpuIdRegister.RegEdx
    );

  if (CpuIdRegister.RegEcx & BIT11) {
    Support |= DEBUG_SUPPORT;
    Support |= DEBUG_LOCK_SUPPORT;
  }

  return Support;
}

/**
  Enable/Disable Debug Interfaces in the processor.

  @param[in] Support  - To enable or disable Debug Interface feature.
**/
VOID
EnableDisableDebugInterface (
  IN UINTN Support
  )
{
  UINT64  Ia32DebugInterface;

  ///
  /// IA32_DEBUG_INTERFACE_MSR scope is "Package", program on BSP only
  ///
  if (!(mMpSystemData->CommonFeatures & DEBUG_SUPPORT) || (IsBsp () == FALSE)) {
    return;
  }

  ///
  /// Check if the processor supports debug interface
  ///
  if (IsDebugInterfaceSupported ()) {
    Ia32DebugInterface = AsmReadMsr64 (MSR_IA32_DEBUG_INTERFACE);
    if (!(Ia32DebugInterface & B_DEBUG_INTERFACE_LOCK)) {
      if (Support & DEBUG_SUPPORT) {
        ///
        /// Enable Debug Interface (MSR 0xC80.Bit0 = 1)
        ///
        Ia32DebugInterface |= B_DEBUG_INTERFACE_ENABLE;
      } else {
        ///
        /// Disable Debug Interface (MSR 0xC80.Bit0 = 0)
        ///
        Ia32DebugInterface &= (UINT64) ~B_DEBUG_INTERFACE_ENABLE;
      }
      if (Support & DEBUG_LOCK_SUPPORT) {
        Ia32DebugInterface |= B_DEBUG_INTERFACE_LOCK;
      }
      AsmWriteMsr64 (MSR_IA32_DEBUG_INTERFACE, Ia32DebugInterface);
    }
  }
  return;
}

/**
  Lock VMX/TXT feature bits on the processor.
  Set "CFG Lock" (MSR 0E2h Bit[15]
**/

VOID
LockFeatureBit (
  VOID
  )
{
  MSR_IA32_FEATURE_CONTROL_REGISTER Msr;

  ///
  /// MSR 3Ah for VMX/TXT Lock
  ///
  Msr.Uint64 = AsmReadMsr64 (MSR_IA32_FEATURE_CONTROL);
  Msr.Uint64 &= ~((UINT64) OPTION_FEATURE_RESERVED_MASK); 

  if (Msr.Bits.Lock == 0) {
    ///
    /// Set Feature Lock bits.
    ///
    Msr.Bits.Lock = 1;
    AsmWriteMsr64 (MSR_IA32_FEATURE_CONTROL, Msr.Uint64);
  }

  return;
}

/**
  Detect if X2APIC supported or not

  @retval X2APIC_SUPPORT if supported or 0 if not supported
**/
UINTN
IsX2apicSupported (
  VOID
  )
{
  CPUID_VERSION_INFO_ECX Ecx;
  UINTN                  Support;

  Support = 0;

  AsmCpuid (CPUID_VERSION_INFO, NULL, NULL, &Ecx.Uint32, NULL);
  if (Ecx.Bits.x2APIC == 1) {
    ///
    /// X2APIC Mode feature is supported on this processor.
    ///
    Support = X2APIC_SUPPORT;
  }
  return Support;
}

/**
  Detect if Processor Trace Feature is supported or not

  @retval PROC_TRACE_SUPPORT if supported or 0 if not supported
**/
UINTN
IsProcessorTraceSupported (
  VOID
  )
{
  CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS_EBX Ebx;
  UINTN                                       Support;

  Support = 0;
  AsmCpuidEx (CPUID_STRUCTURED_EXTENDED_FEATURE_FLAGS, 0, NULL, &Ebx.Uint32, NULL, NULL);
  if (Ebx.Bits.IntelProcessorTrace == 1) {
    Support = PROC_TRACE_SUPPORT;
  }
  return Support;
};

/**
  Aligns a base address to the specified boundary alignment.

  @param  BaseAddress            64 bit value to align
  @param  Alignment              Alignment value must be a power of 2.

  @return EFI_PHYSICAL_ADDRESS   Aligned base address value.
**/
UINT64
CalculateAlignedBaseAddress (
  UINT64 BaseAddress,
  UINT32 Alignment
  )
{
  UINT64  AlignmentMask;

  if (Alignment == 0) {
    return BaseAddress;
  }
  AlignmentMask = Alignment - 1;
  ASSERT ((Alignment & AlignmentMask) == 0);
  BaseAddress += AlignmentMask;
  return BaseAddress & (~AlignmentMask);
}

VOID
CalculateAlignedBaseAndSize (
  EFI_PHYSICAL_ADDRESS *AlignedBaseAddress,
  UINT32               *SizePerThread
  )
{
  UINT32  Size;
  UINT8   Count;

  Size = mCpuConfig->ProcessorTraceMemLength / mMpSystemData->TotalCpusForThisSystem;

  //
  // Align to 2^N
  //
  Count = 0;
  while (Size > 0) {
    Size = Size >> 1;
    Count++;
  }
  Size = 1 << Count;

  ///
  /// Calculate AlignedBaseAddress, using Size per thread for alignment.
  ///
  *AlignedBaseAddress = (EFI_PHYSICAL_ADDRESS) CalculateAlignedBaseAddress ((UINT64) mCpuConfig->ProcessorTraceMemBase, Size);

  ///
  /// Then check if AlignedBaseAddress plus Size * threads overflows the total region.
  /// If so, go to next lower size, and recalculate AlignedBaseAddress
  ///
  if ((*AlignedBaseAddress + Size * mMpSystemData->TotalCpusForThisSystem) > (mCpuConfig->ProcessorTraceMemBase + mCpuConfig->ProcessorTraceMemLength)) {
    Size = Size >> 1;
    *AlignedBaseAddress = (EFI_PHYSICAL_ADDRESS) CalculateAlignedBaseAddress ((UINT64) mCpuConfig->ProcessorTraceMemBase, Size);
  }

  ///
  /// Return the SizePerThread, between 4KB to 128MB, or 0 if not enough allocated space.
  ///
  if (Size > MAX_PROCESSOR_TRACE_SIZE) {
    Size = MAX_PROCESSOR_TRACE_SIZE;
  } else if (Size < MIN_PROCESSOR_TRACE_SIZE) {
    Size = 0;
    if (IsBsp ()) {
      DEBUG ((DEBUG_WARN, "ProcessorTrace: Pre-allocated memory region is not large enough!\n"));
    }
  }
  *SizePerThread = Size;
}

/**
  Initialize Processor Trace Feature
**/
VOID
InitializeProcessorTrace (
  VOID
  )
{
  EFI_STATUS                                Status;
  BOOLEAN                                   IsBspInt;
  UINTN                                     ProcessorNumber;
  UINT64                                    MsrValue;
  CPUID_INTEL_PROCESSOR_TRACE_MAIN_LEAF_ECX Ecx;
  BOOLEAN                                   IsTopaSupported;
  BOOLEAN                                   IsSingleRangeSupported;
  EFI_PHYSICAL_ADDRESS                      AlignedBaseAddress;
  UINT32                                    SizePerThread;
  UINT8                                     EncodedSize;
  UINT32                                    TempSize;
  STATIC EFI_PHYSICAL_ADDRESS               *ThreadMemRegionTable;
  EFI_PHYSICAL_ADDRESS                      ThreadBaseAddress = 0;
  UINTN                                     Index;
  UINTN                                     TopaPages;
  UINTN                                     TopaAlignment;
  UINTN                                     TopaAlignedAddress;
  UINTN                                     TopaTableBaseAddr;
  STATIC UINTN                              *TopaMemArray;
  PROC_TRACE_TOPA_TABLE                     *TopaTable;

  IsBspInt     = IsBsp ();

  if (IsBspInt) {
    DEBUG ((DEBUG_INFO, "Initialize Processor Trace\n"));
  }

  ///
  /// Check that Processor Trace is supported, and ProcessorTraceMemBase and Length are provided
  ///
  if ((!IsProcessorTraceSupported ()) || (mCpuConfig->ProcessorTraceMemBase == 0) || (mCpuConfig->ProcessorTraceMemLength == 0)) {
    return;
  }

  ///
  /// Find ProcessorNumber for this thread
  ///
  Status = gMpServicesPpi->WhoAmI (GetPeiServicesTablePointer (), gMpServicesPpi, &ProcessorNumber);
  if (EFI_ERROR (Status)) {
    ASSERT_EFI_ERROR (Status);
    return;
  }

  ///
  /// Detect which processor trace output schemes are supported.
  ///
  AsmCpuidEx (CPUID_INTEL_PROCESSOR_TRACE, CPUID_INTEL_PROCESSOR_TRACE_MAIN_LEAF, NULL, NULL, &Ecx.Uint32, NULL);
  IsTopaSupported = (Ecx.Bits.RTIT == 1) ? TRUE : FALSE;
  IsSingleRangeSupported = (Ecx.Bits.SingleRangeOutput == 1) ? TRUE : FALSE;

  if (!(IsTopaSupported || IsSingleRangeSupported)) {
    return;
  }

  CalculateAlignedBaseAndSize (&AlignedBaseAddress, &SizePerThread);
  if (IsBspInt) {
    DEBUG ((DEBUG_INFO, "ProcessorTrace: MemSize, in bytes, per thread: 0x%X \n", SizePerThread));
  }
  if (SizePerThread == 0) {
    return;
  }

  //
  // Clear MSR_IA32_RTIT_CTL[0] and IA32_RTIT_STS only if MSR_IA32_RTIT_CTL[0]==1b
  //
  MsrValue = AsmReadMsr64 (MSR_IA32_RTIT_CTL);
  if (MsrValue & B_RTIT_CTL_TRACE_ENABLE) {
    ///
    /// Clear bit 0 in MSR IA32_RTIT_CTL (570)
    ///
    MsrValue &= (UINT64) ~B_RTIT_CTL_TRACE_ENABLE;
    AsmWriteMsr64 (MSR_IA32_RTIT_CTL, MsrValue);

    ///
    /// Clear MSR IA32_RTIT_STS (571h) to all zeros
    ///
    MsrValue = AsmReadMsr64 (MSR_IA32_RTIT_STATUS);
    MsrValue &= 0x0;
    AsmWriteMsr64 (MSR_IA32_RTIT_STATUS, MsrValue);
  }

  if (IsBspInt) {
    /**
      Partition and align the pre-allocated memory region, based on the SizePerThread,
      for all the enabled threads for storing Processor Trace debug data. Then Configure the trace
      address base in MSR, IA32_RTIT_OUTPUT_BASE (560h) bits 47:12. Note that all regions must be
      aligned based on their size, not just 4K. Thus a 2M region must have bits 20:12 clear.
    **/
    ThreadMemRegionTable = (EFI_PHYSICAL_ADDRESS *) AllocatePool (mMpSystemData->TotalCpusForThisSystem * sizeof (EFI_PHYSICAL_ADDRESS));
    if (ThreadMemRegionTable == NULL) {
      DEBUG ((DEBUG_ERROR, "Allocate ProcessorTrace ThreadMemRegionTable Failed\n"));
      return;
    }

    for (Index = 0; Index < mMpSystemData->TotalCpusForThisSystem; Index++) {
      ThreadMemRegionTable[Index] = AlignedBaseAddress + (Index * SizePerThread);
      DEBUG((DEBUG_INFO, "ProcessorTrace: PT ThreadBaseAddress(aligned) for thread %d: 0x%llX \n", Index, (UINT64)ThreadMemRegionTable[Index]));
    }
  }

  //
  // Each thread gets a different memory region based on ProcessorNumber.
  // If BSP failed to initialize, don't initialize APs.
  //
  if (ThreadMemRegionTable == NULL) {
    return;
  }
  ThreadBaseAddress = ThreadMemRegionTable[ProcessorNumber];

  ///
  /// Check Processor Trace output scheme: Single Range output or ToPA table
  ///
  //
  //  Single Range output scheme
  //
  if (IsSingleRangeSupported && (mCpuConfig->ProcessorTraceOutputScheme == 0)) {
    if (IsBspInt) {
      DEBUG((DEBUG_INFO, "ProcessorTrace: Enabling Single Range Output scheme \n"));
    }

    //
    // Clear MSR IA32_RTIT_CTL (0x570) ToPA (Bit 8)
    //
    MsrValue = AsmReadMsr64 (MSR_IA32_RTIT_CTL);
    MsrValue &= (UINT64) ~BIT8;
    AsmWriteMsr64 (MSR_IA32_RTIT_CTL, MsrValue);

    //
    // Program MSR IA32_RTIT_OUTPUT_BASE (0x560) bits[47:12] with the allocated Memory Region
    //
    MsrValue = (UINT64) ThreadBaseAddress;
    AsmWriteMsr64 (MSR_IA32_RTIT_OUTPUT_BASE, MsrValue);

    //
    // Program the Mask bits for the Memory Region to MSR IA32_RTIT_OUTPUT_MASK_PTRS (561h)
    //
    MsrValue = (UINT64) SizePerThread - 1;
    AsmWriteMsr64 (MSR_IA32_RTIT_OUTPUT_MASK_PTRS, MsrValue);
  }

  //
  //  ToPA(Table of physical address) scheme
  //
  if (IsTopaSupported && (mCpuConfig->ProcessorTraceOutputScheme == 1)) {
    /**
      Create ToPA structure aligned at 4KB for each logical thread
      with at least 2 entries by 8 bytes size each. The first entry
      should have the trace output base address in bits 47:12, 6:9
      for Size, bits 4,2 and 0 must be cleared. The second entry
      should have the base address of the table location in bits
      47:12, bits 4 and 2 must be cleared and bit 0 must be set.
    **/
    if (IsBspInt) {
      DEBUG((DEBUG_INFO, "ProcessorTrace: Enabling ToPA scheme \n"));
      /**
         Let BSP allocate ToPA table mem for all threads
      **/
      TopaMemArray = (UINTN *) AllocatePool (mMpSystemData->TotalCpusForThisSystem * sizeof (UINTN *));
      if (TopaMemArray == NULL) {
        ///
        /// Set ThreadMemRegionTable to NULL to prevent APs from initializing after BSP failed to allocate memory.
        ///
        ThreadMemRegionTable = NULL;
        DEBUG ((DEBUG_ERROR, "ProcessorTrace: Allocate mem for ToPA Failed\n"));
        return;
      }

      TopaPages = EFI_SIZE_TO_PAGES (sizeof (PROC_TRACE_TOPA_TABLE));
      TopaAlignment = EFI_PAGES_TO_SIZE (TopaPages);
      TopaAlignedAddress = (UINTN) AllocateAlignedReservedPages ((TopaPages * mMpSystemData->TotalCpusForThisSystem), TopaAlignment);
      if (TopaAlignedAddress == 0) {
        ///
        /// Set ThreadMemRegionTable to NULL to prevent APs from initializing after BSP failed to allocate memory.
        ///
        ThreadMemRegionTable = NULL;
        DEBUG ((DEBUG_ERROR, "ProcessorTrace:  Out of mem, trying to allocate ToPA mem"));
        return;
      }

      for (Index=0; Index < mMpSystemData->TotalCpusForThisSystem; Index++) {
        TopaMemArray[Index] = TopaAlignedAddress + (Index * TopaAlignment);
        DEBUG((DEBUG_INFO, "ProcessorTrace: Topa table address(aligned) for thread %d is 0x%llX \n", Index, (UINT64)TopaMemArray[Index]));
      }
    }

    //
    // Each thread gets a different memory region based on ProcessorNumber
    //
    if (TopaMemArray == NULL) {
      return;
    }
    TopaTableBaseAddr = TopaMemArray[ProcessorNumber];
    TopaTable = (PROC_TRACE_TOPA_TABLE *) TopaTableBaseAddr;

    ///
    /// Encode SizePerThread as value N such that SizePerThread = 2^(N+12)
    ///
    EncodedSize = 0;
    TempSize = SizePerThread;
    while ((TempSize >> 1) > 0) {
      TempSize = TempSize >> 1;
      EncodedSize++;
    }
    ASSERT (EncodedSize >= 12); ///< 4 KB
    ASSERT (EncodedSize <= 27); /// 128 MB
    EncodedSize = EncodedSize - 12;

    TopaTable->TopaEntry[0] = (UINT64) (ThreadBaseAddress | (EncodedSize << 6)) & ~BIT0;
    TopaTable->TopaEntry[1] = (UINT64) TopaTableBaseAddr | BIT0;

    //
    // Program the MSR IA32_RTIT_OUTPUT_BASE (0x560) bits[47:12] with ToPA base
    //
    MsrValue = (UINT64) TopaTableBaseAddr;
    AsmWriteMsr64 (MSR_IA32_RTIT_OUTPUT_BASE, MsrValue);
    //
    // Set the MSR IA32_RTIT_OUTPUT_MASK (0x561) bits[63:7] to 0
    //
    AsmWriteMsr64 (MSR_IA32_RTIT_OUTPUT_MASK_PTRS, 0x7f);
    //
    // Enable ToPA output scheme by enabling MSR IA32_RTIT_CTL (0x570) ToPA (Bit 8)
    //
    MsrValue = AsmReadMsr64 (MSR_IA32_RTIT_CTL);
    MsrValue |= BIT8;
    AsmWriteMsr64 (MSR_IA32_RTIT_CTL, MsrValue);
  }

  ///
  /// Enable the Processor Trace feature from MSR IA32_RTIT_CTL (570h)
  ///
  MsrValue = AsmReadMsr64 (MSR_IA32_RTIT_CTL);
  MsrValue |= (UINT64) BIT0 + BIT2 + BIT3 + BIT13;
  if (!(mCpuConfig->ProcessorTraceEnable)) {
    MsrValue &= (UINT64) ~BIT0;
  }
  AsmWriteMsr64 (MSR_IA32_RTIT_CTL, MsrValue);

}

/**
  Create feature control structure which will be used to program each feature on each core.
**/
VOID
InitializeFeaturePerSetup (
  VOID
  )
{

  PostCode (0xC0E);

  mMpSystemData->CommonFeatures = (UINTN) -1;
  mMpSystemData->SetupFeatures  = (UINTN) -1;

  if (!mCpuConfig->VmxEnable) {
    mMpSystemData->SetupFeatures &= ~VMX_SUPPORT;
  }
  if (!mCpuConfig->AesEnable) {
    mMpSystemData->SetupFeatures &= ~AES_SUPPORT;
  }
  if (!mCpuConfig->DebugInterfaceEnable) {
    mMpSystemData->SetupFeatures &= ~DEBUG_SUPPORT;
  }
  if (!mCpuConfig->DebugInterfaceLockEnable) {
    mMpSystemData->SetupFeatures &= ~DEBUG_LOCK_SUPPORT;
  }
  if (!mCpuConfig->ProcessorTraceEnable) {
    mMpSystemData->SetupFeatures &= ~PROC_TRACE_SUPPORT;
  }
  PostCode (0xC12);
}

/**
  Detect each processor feature and log all supported features
**/
VOID
EFIAPI
CollectProcessorFeature (
  VOID
  )
{
  UINTN Support;

  Support = 0;
  Support |= IsXdSupported ();
  Support |= IsVmxSupported ();
  Support |= IsDcaSupported ();
  Support |= IsAesSupported ();
  Support |= IsX2apicSupported ();
  Support |= IsHTSupported ();
  Support |= IsDebugInterfaceSupported ();
  Support |= IsProcessorTraceSupported ();

  mMpSystemData->CommonFeatures &= Support;
  return;
}

/**
  Program all processor features basing on desired settings
**/
VOID
EFIAPI
ProgramProcessorFeature (
  )
{
  UINTN   Supported;
  BOOLEAN IsBspInt;
  #ifdef SGX_SUPPORT
  #endif
  IsBspInt = IsBsp();
  Supported = mMpSystemData->CommonFeatures & mMpSystemData->SetupFeatures;

  ///
  /// Configure features such as Xd, Vmx, Smx, XAPIC, AES, DebugInterface, Processor Trace feature
  ///
  ProgramXd (Supported);
  EnableDisableVmx (Supported);
  EnableDisableAes (Supported);
  EnableDisableDebugInterface (Supported);
  
  InitializeProcessorTrace ();

  ///
  /// Program XApic register
  ///
  if (IsBspInt) {
    DEBUG ((DEBUG_INFO, "Program xAPIC\n"));
  }
  ProgramXApic (IsBspInt);

  ///
  /// Initialize MonitorMWait register
  ///
  if (IsBspInt) {
    DEBUG ((DEBUG_INFO, "Initialize Monitor Wait register\n"));
  }
  CpuMiscEnable ((BOOLEAN)mCpuConfig->MonitorMwaitEnable, B_MSR_IA32_MISC_ENABLE_MONITOR);

  ///
  /// Initialize Machine Check registers
  ///
  if (IsBspInt) {
    DEBUG ((DEBUG_INFO, "Initialize Machine check registers\n"));
  }
  InitializeMachineCheckRegisters (NULL, (BOOLEAN) mCpuConfig->MachineCheckEnable);

  ///
  /// Configure BIOS uCode PM_TMR Emulation MSR 121h on all logical processors (BSP & APs).
  ///
  BiosUcodePmTmrEmulationMsrCfg ();
  return;
}

/**
  Program CPUID Limit before booting to OS
**/
VOID
EFIAPI
ProgramCpuidLimit (
  VOID
  )
{
  UINT64 MsrValue;

  ///
  /// Finally write MISC MSR to avoid access for multiple times
  ///
  MsrValue = AsmReadMsr64 (MSR_IA32_MISC_ENABLE);
  AsmWriteMsr64 (MSR_IA32_MISC_ENABLE, MsrValue);

  return;
}

/**
  Initialize prefetcher settings

  @param[in] MlcStreamerprefecterEnabled - Enable/Disable MLC streamer prefetcher
  @param[in] MlcSpatialPrefetcherEnabled - Enable/Disable MLC spatial prefetcher
**/
VOID
InitializeProcessorsPrefetcher (
  IN UINTN MlcStreamerprefecterEnabled,
  IN UINTN MlcSpatialPrefetcherEnabled
  )
{
  UINT64 MsrValue;
  MsrValue = AsmReadMsr64 (MISC_FEATURE_CONTROL);

  if (MlcStreamerprefecterEnabled == CPU_FEATURE_DISABLE) {
    MsrValue |= B_MISC_FEATURE_CONTROL_MLC_STRP;
  }

  if (MlcSpatialPrefetcherEnabled == CPU_FEATURE_DISABLE) {
    MsrValue |= B_MISC_FEATURE_CONTROL_MLC_SPAP;
  }

  if ((MsrValue & (B_MISC_FEATURE_CONTROL_MLC_STRP | B_MISC_FEATURE_CONTROL_MLC_SPAP)) != 0) {
    AsmWriteMsr64 (MISC_FEATURE_CONTROL, MsrValue);
  }

  return;
}


#ifdef SGX_SUPPORT

/**
  This will load the microcode to the processors.

  @param[in] MicrocodeEntryPoint - The microcode update pointer
  @param[in, out] Revision       - The current (before load this microcode update) microcode revision
                                   as output parameter, the microcode revision after microcode update is loaded

  @retval EFI_SUCCESS            - Microcode loaded
  @retval EFI_LOAD_ERROR         - Microcode not loaded
**/
EFI_STATUS
LoadMicrocode (
  IN CPU_MICROCODE_HEADER *MicrocodeEntryPoint,
  IN OUT UINT32           *Revision
  )
{
  EFI_STATUS Status;
  UINT32     NewRevision;

  Status = EFI_SUCCESS;

  DEBUG_CODE_BEGIN ();
  if (IsBsp ()) {
    DEBUG ((DEBUG_INFO, "LoadMicrocode: Before load, revision = 0x%x\n", *Revision));
  }
  DEBUG_CODE_END ();

  ///
  /// Load the Processor Microcode
  ///
  AsmWriteMsr64 (
    MSR_IA32_BIOS_UPDT_TRIG,
    (UINT64) ((UINTN) MicrocodeEntryPoint + sizeof (CPU_MICROCODE_HEADER))
    );

  NewRevision = GetCpuUcodeRevision ();

  DEBUG_CODE_BEGIN ();
  if (IsBsp ()) {
    DEBUG ((DEBUG_INFO, "LoadMicrocode: After load, revision = 0x%x\n", NewRevision));
  }
  DEBUG_CODE_END ();

  ///
  /// Verify that the microcode has been loaded
  ///
  if (NewRevision == *Revision) {
    return EFI_LOAD_ERROR;
  }

  *Revision = MicrocodeEntryPoint->UpdateRevision;
  return Status;
}

/**
  This will check if the microcode address is valid for this processor, and if so, it will
  load it to the processor.

  @param[in]  MicrocodeAddress - The address of the microcode update binary (in memory).
  @param[out] FailedRevision   - The microcode revision that fails to be loaded.

  @retval EFI_SUCCESS           - A new microcode update is loaded.
  @retval Other                 - Due to some reason, no new microcode update is loaded.
**/
EFI_STATUS
InitializeMicrocode (
  IN  CPU_MICROCODE_HEADER *MicrocodeAddress,
  OUT UINT32               *FailedRevision
  )
{
  EFI_STATUS         Status;
  EFI_CPUID_REGISTER Cpuid;
  UINT32             UcodeRevision;

  Status = EFI_NOT_FOUND;

  AsmCpuid (
    CPUID_VERSION_INFO,
    &Cpuid.RegEax,
    &Cpuid.RegEbx,
    &Cpuid.RegEcx,
    &Cpuid.RegEdx
    );
  UcodeRevision = GetCpuUcodeRevision ();

  if (CheckMicrocode (Cpuid.RegEax, MicrocodeAddress, &UcodeRevision)) {
    Status = LoadMicrocode (MicrocodeAddress, &UcodeRevision);
    *FailedRevision = UcodeRevision;
  }

  return Status;
}

/**
  Load Microcode on APs if not loaded already.

  @param[in] Buffer - Pointer to UINT8 status to report if microcode update is loaded, not found, or loaded here and not by FIT.
                      Before calling status (pointed to buffer) must be initialized to zero.
**/

VOID
LoadMicrocodePatchAps (
    IN  VOID  *Buffer
)
{
  UINT32      FailedRevision;
  EFI_STATUS  Status;

  ///
  /// Only execute primary thread of core.
  ///
  if (IsSecondaryThread()) {
    return;
  }

  ///
  /// Check if first microcode patch loaded, if loaded, don't load again.
  ///
  if (GetCpuUcodeRevision() > 0) {
    return;
  }

  Status = InitializeMicrocode (
      (CPU_MICROCODE_HEADER *) (UINTN) mCpuConfig->MicrocodePatchAddress,
      &FailedRevision
      );

  ///
  /// Microcode loaded by FIT Buffer = 0. (Pre-initialized)
  /// Microcode not loaded here Buffer = 1.
  /// Microcode loaded here Buffer = 2.
  ///
  if (EFI_ERROR(Status)) {
    *(UINT8*)Buffer = 1;
  } else {
    *(UINT8*)Buffer = 2;
  }
}




/**
  Re-load microcode patch.

  @param[in] Buffer      - A pointer to buffer which need to append the element
**/
VOID
EFIAPI
ReloadMicrocodePatch (
  VOID
  )
{
  EFI_STATUS                        Status;
  UINT32                            FailedRevision;
  BOOLEAN                           IsBspVal;

  IsBspVal = IsBsp();

  if (IsBspVal) {
    DEBUG ((DEBUG_INFO, "ReloadMicrocodePatch: second patch load started\n"));
  }


  ///
  /// Init XMM
  ///
  XmmInit ();

  if (IsBspVal) {
    ProgramXApic (TRUE);
  } else {
    ProgramXApic (FALSE);
  }
  
  if (IsBsp()) {
       DEBUG ((DEBUG_INFO, "SGX_DEBUG:Microcode pointer found at 0x%08X\n",mCpuConfig->MicrocodePatchAddress));
     }
  
  Status = InitializeMicrocode (
             (CPU_MICROCODE_HEADER *) (UINTN) mCpuConfig->MicrocodePatchAddress,
             &FailedRevision
             );

  if (!IsBspVal) {
    ProcessorsPrefetcherInitialization (
      (UINTN) mCpuConfig->MlcStreamerPrefetcher,
      (UINTN) mCpuConfig->MlcSpatialPrefetcher
      );
  }
  
  if (Status != EFI_SUCCESS) {
    if (IsBspVal) {
      DEBUG ((DEBUG_INFO, "ReloadMicrocodePatch: Second patch load failed, SGX is not activated\n"));
    }
  }

   SgxInitializationPostPatchLoad (mCpuConfig,mSoftwareGuardConfig);

  return;
}

/**
  Initialize SGX PRMRR and Reload microcode

  @retval EFI_SUCCESS     The driver installes/initialized correctly.
**/
EFI_STATUS
InitializeSgxPrmrr (
   IN  CONST EFI_PEI_SERVICES  **PeiServices,
   IN  SI_CPU_POLICY_PPI       *SiCpuPolicyPpi
   )
{
  if (IsPrmrrSupported()) {

    // 
    // Initialize SGX, PRM core MSRs.
    //
    DEBUG ((EFI_D_INFO, "---SGX--- Calling InitializeCorePrmrr:\n"));
    InitializeCorePrmrr ();
    DEBUG ((EFI_D_INFO, "---SGX--- Left InitializeCorePrmrr:\n"));

    ///
    /// Check if PRMRR was set correctly by BSP
    ///
    if (IsPrmrrAlreadySet()) {
      gMpServicesPpi->StartupAllAPs (
                      PeiServices,
                      gMpServicesPpi,
                      (EFI_AP_PROCEDURE) InitializeCorePrmrr,
                      FALSE,
                      0,
                      NULL
                      );

      ///
      /// Flush cache into memory
      ///
      AsmWbinvd (); 
      SgxInitializationByLogicalProcessorPrePatchLoad (SiCpuPolicyPpi);
      gMpServicesPpi->StartupAllAPs ( PeiServices,
                                      gMpServicesPpi,
                                      (EFI_AP_PROCEDURE) SgxInitializationByLogicalProcessorPrePatchLoad,
                                      FALSE,
                                      0,
                                      (VOID *)SiCpuPolicyPpi
                                    );

    }
  }
  
  return EFI_SUCCESS;
}

#endif


/**
  Initialize CPU Data Hob

  @retval EFI_SUCCESS     The driver installed/initialized correctly.
  @retval EFI_OUT_OF_RESOURCES  Allocation of the hob failed.
**/
EFI_STATUS
InitializeCpuDataHob (
  VOID
  )
{
  CPU_INIT_DATA_HOB      *CpuInitDataHob;
  VOID                   *Hob;
  CPU_CONFIG_DATA        *CpuConfigData;

#ifdef SGX_SUPPORT
SOFTWARE_GUARD_CONFIG  *SoftwareGuardConfig;
#endif

  DEBUG((DEBUG_INFO, "InitializeCpuDataHob Start \n"));
  PostCode (0xC3B);

  ///
  /// Initial cpu data into one hob, it will be used by MP CPU DXE.
  ///
  CpuInitDataHob = AllocateRuntimeZeroPool (sizeof (CPU_INIT_DATA_HOB));
  if (CpuInitDataHob==NULL) {
    return EFI_OUT_OF_RESOURCES;
  }

  CpuConfigData = (CPU_CONFIG_DATA *) &mMpSystemData->CpuConfigData;

  CpuConfigData->SmmbaseSwSmiNumber      = (UINT8) mCpuConfig->SmmbaseSwSmiNumber;
  CpuConfigData->ApHandoffManner         = mCpuConfig->ApHandoffManner;
  CpuConfigData->ApIdleManner            = mCpuConfig->ApIdleManner;
  CpuConfigData->EnableDts               = mCpuConfig->EnableDts;
  CpuConfigData->PackageDts              = mCpuConfig->PackageDts;
  CpuConfigData->OsBoot                  = mCpuConfig->OsBoot;
  CpuConfigData->SmbiosSocketInfo        = mCpuConfig->SmbiosSocketInfo;
  CpuInitDataHob->CpuConfigData          = (EFI_PHYSICAL_ADDRESS)(UINTN) &mMpSystemData->CpuConfigData;
  CpuInitDataHob->FvidTable              = (EFI_PHYSICAL_ADDRESS)(UINTN)&mMpSystemData->FvidTable;
  
#ifdef SGX_SUPPORT
  SoftwareGuardConfig = AllocateRuntimeZeroPool (sizeof (SOFTWARE_GUARD_CONFIG));
  if (SoftwareGuardConfig == NULL) {
    return EFI_OUT_OF_RESOURCES;
  }
  SoftwareGuardConfig->SgxEpoch0          = (UINT64)mSoftwareGuardConfig->SgxEpoch0;
  SoftwareGuardConfig->SgxEpoch1          = (UINT64)mSoftwareGuardConfig->SgxEpoch1;
  CpuInitDataHob->SoftwareGuardConfig     = (EFI_PHYSICAL_ADDRESS)(UINTN)&SoftwareGuardConfig;
#endif


  Hob = BuildGuidDataHob (
                        &gCpuInitDataHobGuid,
                        (VOID *)CpuInitDataHob,
                        (UINTN) sizeof (CPU_INIT_DATA_HOB)
                        );
  ASSERT (Hob != NULL);

  DEBUG((DEBUG_INFO, "InitializeCpuDataHob End\n"));  
  PostCode (0xC3E);
  return EFI_SUCCESS;
}
/**
  Get CPU platform features settings to fill MP system data.

  @param[in] TotalCpusForThisSystem - Total number of logical processors in this system.

  @retval EFI_SUCCESS              - Function successfully executed.
  @retval Other                    - Error occurred while allocating memory.
**/
EFI_STATUS
EFIAPI
FillMpSystemData (
  IN UINTN                  TotalCpusForThisSystem
  )
{
  ///
  /// Fill up MpSystemData
  ///
  mMpSystemData->AcpiCpuData.CpuPrivateData       = (EFI_PHYSICAL_ADDRESS) (UINTN) (&mMpSystemData->S3BspMtrrTablePointer);
  mMpSystemData->AcpiCpuData.NumberOfCpus         = TotalCpusForThisSystem;
  mMpSystemData->AcpiCpuData.APState              = TRUE;
  mMpSystemData->HdcSupported                     = (AsmReadMsr64 (MSR_MISC_PWR_MGMT) & B_MISC_PWR_MGMT_ENABLE_SDC) != 0;
  mMpSystemData->S3BspMtrrTablePointer            = (UINT32) (UINTN) mMpSystemData->S3BspMtrrTable;
  mMpSystemData->TotalCpusForThisSystem           = TotalCpusForThisSystem;
  mMpSystemData->BSP                              = 0;
  
  return EFI_SUCCESS;
}

/**
  Set IA_UNTRSUTED BIT in MSR_UCODE_CR_POWER_MISC.

**/
VOID
EFIAPI
SetIaUntrustedMode (
  IN  VOID
  )
{
  UINT64	  Data;

  Data = AsmReadMsr64 (MSR_UCODE_CR_POWER_MISC);
  Data |= BIT6;
  AsmWriteMsr64 (MSR_UCODE_CR_POWER_MISC, Data);
}

/**
  This function handles Cpu Initialization routine at the end of PEI

  @param[in] PeiServices   - Pointer to PEI Services Table.
  @param[in] NotifyDesc    - Pointer to the descriptor for the Notification event that
                             caused this function to execute.
  @param[in] Ppi           - Pointer to the PPI data associated with this function.

  @retval EFI_STATUS       - Always return EFI_SUCCESS
**/
STATIC
EFI_STATUS
CpuInitAtEndOfPei (
  IN EFI_PEI_SERVICES    **PeiServices,
  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
  IN VOID                      *Ppi
  )
{

  EFI_BOOT_MODE             BootMode;
  EFI_STATUS                Status;

  Status = PeiServicesGetBootMode (&BootMode);
  ASSERT_EFI_ERROR (Status);

#ifdef SGX_SUPPORT
  ///
  /// Re-load microcode patch only if PRMRR is supported and then is programmed
  ///
  if (IsPrmrrSupported()) {
    if (IsPrmrrAlreadySet()) {
      DEBUG((DEBUG_INFO, "ReloadMicrocodePatch Start\n"));
      ///
      /// Reload Microcode Patch
      ///
      ReloadMicrocodePatch ();
      gMpServicesPpi->StartupAllAPs (
                        (CONST EFI_PEI_SERVICES **)PeiServices,
                        gMpServicesPpi,
                        (EFI_AP_PROCEDURE) ReloadMicrocodePatch,
                        FALSE,
                        0,
                        NULL
                        );
      DEBUG((DEBUG_INFO, "ReloadMicrocodePatch Done\n"));
    }
  }
#endif

#ifndef FSP_FLAG
  if (BootMode == BOOT_ON_S3_RESUME) {
    SetIaUntrustedMode();
    gMpServicesPpi->StartupAllAPs (
                     (CONST EFI_PEI_SERVICES **) PeiServices,
                     gMpServicesPpi,
                     (EFI_AP_PROCEDURE) SetIaUntrustedMode,
                     FALSE,
                     0,
                     NULL
                     );
    DEBUG ((EFI_D_INFO, "IA_UNTRUSTED BIT is set.Msr(0x%x)value is %x \n", MSR_UCODE_CR_POWER_MISC, AsmReadMsr64 (MSR_UCODE_CR_POWER_MISC)));
  }
#endif

  return EFI_SUCCESS;
}

/**
  Initialize processor features, performance and power management features, BIOS GUARD, and Overclocking etc features before RESET_CPL at post-memory phase.

  @param[in] PeiServices      Pointer to PEI Services Table
  @param[in] SiPolicyPpi      The SI Policy PPI instance

  @retval EFI_SUCCESS     The driver installed/initialized correctly.
**/
EFI_STATUS
CpuInit (
  IN  CONST EFI_PEI_SERVICES  **PeiServices,
  IN  SI_CPU_POLICY_PPI       *SiCpuPolicyPpi
  )
{
  EFI_STATUS                  Status;
  UINTN                       NumberOfProcessors;
  UINTN                       NumberOfEnabledProcessors;    
  #ifdef SGX_SUPPORT
  UINT8                       MicrocodeLoadStatus;
  #endif
  
  DEBUG((DEBUG_INFO, "CpuInit Start \n"));
  PostCode (0xC30);

  ///
  /// Locate CpuMpCpu MpService Ppi
  ///
  Status = PeiServicesLocatePpi (
             &gEfiPeiMpServicesPpiGuid,
             0,
             NULL,
             (VOID **) &gMpServicesPpi);
  ASSERT_EFI_ERROR (Status);

  ///
  /// Allocate Cpu MP system data structure memory.
  ///
  mMpSystemData = (MP_SYSTEM_DATA *) AllocatePages (EFI_SIZE_TO_PAGES ((sizeof (MP_SYSTEM_DATA))));
  if (mMpSystemData == NULL) {
    return EFI_OUT_OF_RESOURCES;
  }
  SetMem ((VOID *) mMpSystemData, sizeof (MP_SYSTEM_DATA), 0x00);

  Status = GetConfigBlock ((CONFIG_BLOCK_TABLE_HEADER *)SiCpuPolicyPpi, &gCpuConfigGuid , (VOID *)&mCpuConfig);
  ASSERT_EFI_ERROR (Status);

  Status = GetConfigBlock ((CONFIG_BLOCK_TABLE_HEADER *)SiCpuPolicyPpi, &gPowerMgmtConfigGuid , (VOID *)&mPowerMgmtConfig);
  ASSERT_EFI_ERROR (Status);

#ifdef SGX_SUPPORT
  Status = GetConfigBlock ((CONFIG_BLOCK_TABLE_HEADER *)SiCpuPolicyPpi, &gSoftwareGuardConfigGuid , (VOID *)&mSoftwareGuardConfig);
  DEBUG((DEBUG_ERROR,"Getconfig block for softwareguard = %r\n",Status));
  ASSERT_EFI_ERROR (Status);
#endif

  ///
  /// Fill Cpu MP system data
  ///
  gMpServicesPpi->GetNumberOfProcessors (PeiServices, gMpServicesPpi, (UINTN *)&NumberOfProcessors, (UINTN *)&NumberOfEnabledProcessors);
  Status = FillMpSystemData (NumberOfEnabledProcessors);

  DEBUG ((DEBUG_INFO, "Cpu Initialization in PostMem start\n"));
  DEBUG ((DEBUG_INFO, "DBG: MP_SYSTEM_DATA Pointer - %x\n", (VOID *) mMpSystemData));

  if (mCpuConfig->SkipMpInit == 0) {
    ///
    /// Initialize feature control structure
    ///
    DEBUG((DEBUG_INFO, "Initialize feature control Start \n"));
    InitializeFeaturePerSetup ();

  #ifdef SGX_SUPPORT
    ///
    /// Initialize MicrocodeLoadStatus to 0, which means microcode loaded by FIT.
    /// LoadMicrocodePatchAps will change this value, if microcode not loaded by FIT.
    ///
    MicrocodeLoadStatus = 0;
    ///
    /// Load Microcode Patch on APs if not already loaded.
    ///
    gMpServicesPpi->StartupAllAPs (
                          PeiServices,
                          gMpServicesPpi,
                          LoadMicrocodePatchAps,
                          FALSE,
                          0,
                          &MicrocodeLoadStatus
                                  );
    if (MicrocodeLoadStatus == 1) {
      DEBUG((DEBUG_ERROR, "Microcode update for installed CPU not found.\n"));
    } else if (MicrocodeLoadStatus == 2) {
      DEBUG((DEBUG_ERROR, "Microcode update loaded here. It was not loaded by FIT.\n"));
    }
  #endif
    ///
    /// Detect and log all processor supported features
    ///
    DEBUG((DEBUG_INFO, "CollectProcessorFeature Start \n"));
    PostCode (0xC37);
    CollectProcessorFeature ();

    DEBUG((DEBUG_INFO, "ProgramProcessorFeature Start \n"));
    PostCode (0xC38);
    ProgramProcessorFeature ();
    gMpServicesPpi->StartupAllAPs (
                      PeiServices,
                      gMpServicesPpi,
                      (EFI_AP_PROCEDURE) ProgramProcessorFeature,
                      FALSE,
                      0,
                      NULL
                      );

    #ifdef SGX_SUPPORT
      ///
      /// Initialize SGX PRMRR & Reload Microcode.
      ///
      InitializeSgxPrmrr (PeiServices,SiCpuPolicyPpi);
      DEBUG ((DEBUG_INFO, "Initialize SGX PRMRR & Reload Microcode Done\n"));
    #endif

    DEBUG((DEBUG_INFO, "Lock Feature bit Start \n"));
    PostCode (0xC3A);
    LockFeatureBit();
    gMpServicesPpi->StartupAllAPs (
                      PeiServices,
                      gMpServicesPpi,
                      (EFI_AP_PROCEDURE) LockFeatureBit,
                      FALSE,
                      0,
                      NULL
                      );

    ///
    /// Initialize CPU Data Hob
    ///
    InitializeCpuDataHob ();
  
    DEBUG ((DEBUG_INFO, "CPU Init: Register CpuInitEndOfPei\n"));
    Status = PeiServicesNotifyPpi (&mCpuInitAtEndOfPeiNotifyDesc[0]);
    ASSERT_EFI_ERROR (Status);
  }

  DEBUG((DEBUG_INFO, "CpuInit Done \n"));
  PostCode (0xC3F);
  return EFI_SUCCESS;
}
