/** @file
  Source file for FSP Init PEI module

 @copyright
  INTEL CONFIDENTIAL
  Copyright 2012 - 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 "FspInit.h"

#define EFI_PHYSICAL_ADDRESS  UINT64
#define MRC_DATA_REQUIRED_FROM_OUTSIDE
#define EFI_MEMORY_INIT
#include <MmrcData.h>

#include <Ppi/SecPlatformInformation.h>
#include <Library/BaseMemoryLib.h>
#include <Library/MemoryAllocationLib.h>

#define PLATFORM_CONFIG_STORE_FILE_NAME     "critical/mrc_trg_data"

UINT32  mHeci2Bar;
UINT32  mHeci3Bar;

EFI_SEC_PLATFORM_INFORMATION2_PPI mFspSecPlatformInformation2 = {
  FspSecPlatformInformation2
};

EFI_PEI_PPI_DESCRIPTOR mFspPeiSecPlatformInformation2 = {
  (EFI_PEI_PPI_DESCRIPTOR_PPI | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST),
  &gEfiSecPlatformInformation2PpiGuid,
  &mFspSecPlatformInformation2
};

EFI_STATUS
EFIAPI
TransfertoIaUntrustedMode (
  IN EFI_PEI_SERVICES  **PeiServices
  );

static EFI_PEI_NOTIFY_DESCRIPTOR  mNotifyList[] = {
  {
    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
    &gEfiEndOfPeiSignalPpiGuid,
    FspInitEndOfPeiCallback
  },
  {
    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
    &gFspEventEndOfFirmwareGuid,
    FspEndOfFirmwareCallback
  },
  {
    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
    &gEfiEventReadyToBootGuid,
    FspReadyToBootCallback
  },
  {
    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK | EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST,
    &gEfiPciEnumerationCompleteProtocolGuid,
    FspInitAfterPciEnumerationCallback
  }
};

// .Read the DEVIDLEC register value
// .If CIP=1, wait until cleared (if host is not using interrupts - poll on the bit value until cleared)
#define  WaitForDevIdleRegCipToClear(Addr, Data)   \
  do {                                             \
    Data = Mmio32(Addr, R_HECI_DEVIDLEC);          \
    if ((Data & B_HECI_DEVIDLEC_CIP) == 0) break;  \
  }while(TRUE)

/*++

Routine Description:

  FSP initialization after PCI enumeration

Arguments:

  Event             A pointer to the Event that triggered the callback.
  Context           A pointer to private data registered with the callback function.

Returns:

  EFI_SUCCESS       The function completed successfully

  --*/
EFI_STATUS
EFIAPI
FspInitEndOfPeiCallback (
  IN  EFI_PEI_SERVICES            **PeiServices,
  IN  EFI_PEI_NOTIFY_DESCRIPTOR   *NotifyDescriptor,
  IN  VOID                        *Ppi
  )
{
  FSPS_UPD                        *FspsUpd;
  EFI_BOOT_MODE                    BootMode;


  BootMode = 0;
  PeiServicesGetBootMode(&BootMode);

  DEBUG ((EFI_D_INFO, "FspInitEndOfPeiCallback++\n"));

  FspsUpd = (FSPS_UPD *)GetFspSiliconInitUpdDataPointer();
  if (FspsUpd->FspsConfig.P2sbUnhide == 0) {
    DEBUG ((EFI_D_INFO, "Hide P2SB device.\n"));
    PchHideP2sb (MmPciBase (DEFAULT_PCI_BUS_NUMBER_SC, PCI_DEVICE_NUMBER_P2SB, PCI_FUNCTION_NUMBER_P2SB));
  }

  DEBUG ((EFI_D_INFO, "FspInitEndOfPeiCallback--\n"));

  return EFI_SUCCESS;
}

/*++

Routine Description:

  FSP initialization triggered by ExitBootService event for Fsp

Arguments:

  Event             A pointer to the Event that triggered the callback.
  Context           A pointer to private data registered with the callback function.

Returns:

  EFI_SUCCESS       The function completed successfully

  --*/
EFI_STATUS
EFIAPI
FspEndOfFirmwareCallback (
  IN  EFI_PEI_SERVICES            **PeiServices,
  IN  EFI_PEI_NOTIFY_DESCRIPTOR   *NotifyDescriptor,
  IN  VOID                        *Ppi
  )
{
  EFI_STATUS Status;
  UINT32     Heci1Bar;
  UINT32     Data32;
  UINT64     Data;
  UINTN      P2sbBase;
  BOOLEAN    P2sbOrgStatus;
  EFI_HOB_GUID_TYPE  *FdoEnabledGuidHob = NULL;

  DEBUG ((EFI_D_INFO, "FspEndOfFirmwareCallback++\n"));

  // Check if FDO boot path is enabled
  // If in FDO path skip hiding HECI devices
  FdoEnabledGuidHob = GetFirstGuidHob (&gFdoModeEnabledHobGuid);
  if ((FdoEnabledGuidHob == NULL)) {
    DEBUG ((EFI_D_INFO, "Sending CSE End of Services HECI message...\n"));
    HeciEndOfServices ();

    DEBUG ((EFI_D_INFO, "Putting all HECI devices into D0i3...\n"));
    //
    // Put HECI1 into D0i3
    //
    Heci1Bar = HeciPciRead32 (R_HECIMBAR0) & 0xFFFFFFF0;
    if ((Mmio32 (Heci1Bar, R_HECI_DEVIDLEC) & B_HECI_DEVIDLEC_DEVIDLE) == 0) {
      WaitForDevIdleRegCipToClear (Heci1Bar, Data32);
      Mmio32Or (Heci1Bar, R_HECI_DEVIDLEC, B_HECI_DEVIDLEC_DEVIDLE);
    }

    //
    // Put HECI2 into D0i3
    //
    if ((Mmio32 (mHeci2Bar, R_HECI_DEVIDLEC) & B_HECI_DEVIDLEC_DEVIDLE) == 0) {
      WaitForDevIdleRegCipToClear (mHeci2Bar, Data32);
      Mmio32Or (mHeci2Bar, R_HECI_DEVIDLEC, B_HECI_DEVIDLEC_DEVIDLE);
    }

    //
    // Set HECI3 into D0i3
    //
    if ((Mmio32 (mHeci3Bar, R_HECI_DEVIDLEC) & B_HECI_DEVIDLEC_DEVIDLE) == 0) {
      WaitForDevIdleRegCipToClear (mHeci3Bar, Data32);
      Mmio32Or (mHeci3Bar, R_HECI_DEVIDLEC, B_HECI_DEVIDLEC_DEVIDLE);
    }
  } else {
    DEBUG ((EFI_D_WARN, "HECI devices were not placed into D0i3 since FDO mode is enabled.\n"));
  }

  P2sbOrgStatus = FALSE;
  P2sbBase      = MmPciBase (
                    DEFAULT_PCI_BUS_NUMBER_SC,
                    PCI_DEVICE_NUMBER_P2SB,
                    PCI_FUNCTION_NUMBER_P2SB
                    );
  PchRevealP2sb (P2sbBase, &P2sbOrgStatus);

  if (!P2sbOrgStatus) {
    PchHideP2sb (P2sbBase);
  }

  //
  // Enable IA Untrusted Mode If Not Already Enabled (This May Happen on S3 Resume)
  //
  Data = AsmReadMsr64 (MSR_UCODE_CR_POWER_MISC);
  if ((Data & BIT6) != BIT6) {
    Status = TransfertoIaUntrustedMode (PeiServices);
    ASSERT_EFI_ERROR (Status);
  }

  DEBUG ((EFI_D_INFO, "FspEndOfFirmwareCallback--\n"));

  return EFI_SUCCESS;
}

/*++

Routine Description:

  FSP initialization after PCI enumeration

Arguments:

  Event             A pointer to the Event that triggered the callback.
  Context           A pointer to private data registered with the callback function.

Returns:

  EFI_SUCCESS       The function completed successfully

  --*/
EFI_STATUS
EFIAPI
FspReadyToBootCallback (
  IN  EFI_PEI_SERVICES            **PeiServices,
  IN  EFI_PEI_NOTIFY_DESCRIPTOR   *NotifyDescriptor,
  IN  VOID                        *Ppi
  )
{
  FSPS_UPD      *FspsUpd;
  EFI_BOOT_MODE BootMode;

  DEBUG((EFI_D_INFO, "FspReadyToBootCallback++\n"));

  BootMode = 0;
  PeiServicesGetBootMode(&BootMode);
  if ((BootMode != BOOT_ON_S3_RESUME)) {
    FspsUpd = (FSPS_UPD *)GetFspSiliconInitUpdDataPointer();
    if (FspsUpd->FspsConfig.EndOfPostEnabled) {
      DEBUG((EFI_D_INFO, "Start Send EOP Heci Message\n"));
      HeciEndOfPost();
      DEBUG((EFI_D_INFO, "End Of Send EOP Heci Message\n"));
    }
  }

 DEBUG((EFI_D_INFO, "FspReadyToBootCallback--\n"));
 return EFI_SUCCESS;
}

/*++

Routine Description:

  FSP initialization after PCI enumeration

Arguments:

  Event             A pointer to the Event that triggered the callback.
  Context           A pointer to private data registered with the callback function.

Returns:

  EFI_SUCCESS       The function completed successfully

  --*/
EFI_STATUS
EFIAPI
FspInitAfterPciEnumerationCallback (
  IN  EFI_PEI_SERVICES            **PeiServices,
  IN  EFI_PEI_NOTIFY_DESCRIPTOR   *NotifyDescriptor,
  IN  VOID                        *Ppi
  )
{
  EFI_STATUS               Status;
  EFI_HOB_GUID_TYPE        *FdoEnabledGuidHob;
  UINTN                    Heci2BarAddress;
  UINTN                    Heci3BarAddress;
  UINT64                   Data;

  FdoEnabledGuidHob = NULL;
  Heci2BarAddress   = 0xFFFFFFFF;
  Heci3BarAddress   = 0xFFFFFFFF;

  DEBUG ((EFI_D_INFO, "FspInitAfterPciEnumerationCallback++\n"));

  FdoEnabledGuidHob = GetFirstGuidHob (&gFdoModeEnabledHobGuid);

  //
  // Hide the HECI2 and HECI3 devices if not already hidden
  //
  Heci2BarAddress = Heci2PciRead32 (R_HECIMBAR0) & 0xFFFFFFF0;
  Heci3BarAddress = Heci3PciRead32 (R_HECIMBAR0) & 0xFFFFFFF0;

  if (FdoEnabledGuidHob == NULL) {
    DEBUG ((DEBUG_INFO, "Hiding the HECI2 and HECI3 devices.\n"));
    if (Heci2BarAddress != 0xFFFFFFF0) {
      // Update the HECI2 BAR in case of reassignment since the entry point
      mHeci2Bar = Heci2BarAddress;

      Heci2PciOr32 (
        PCI_COMMAND_OFFSET,
        EFI_PCI_COMMAND_MEMORY_SPACE | EFI_PCI_COMMAND_BUS_MASTER | EFI_PCI_COMMAND_SERR
        );

      SideBandAndThenOr32 (
        SB_PORTID_PSF3,
        R_SC_PCH_PCR_PSF3_T0_SHDW_CSE_D15F1_FUN_DIS,
        0xFFFFFFFF,
        (UINT32) BIT0
        );
    }

    if (Heci3BarAddress != 0xFFFFFFF0) {
      // Update the HECI3 BAR in case of reassignment since the entry point
      mHeci3Bar = Heci3BarAddress;
      SideBandAndThenOr32 (
        SB_PORTID_PSF3,
        R_SC_PCH_PCR_PSF3_T0_SHDW_CSE_D15F2_FUN_DIS,
        0xFFFFFFFF,
        (UINT32) BIT0
        );
    }

  } else {
    DEBUG ((DEBUG_WARN, "HECI2 and HECI3 could not be hidden since FDO is enabled.\n"));
  }

  //
  // Enable IA Untrusted Mode If Not Already Enabled
  //
  Data = AsmReadMsr64 (MSR_UCODE_CR_POWER_MISC);
  if ((Data & BIT6) != BIT6) {
    Status = TransfertoIaUntrustedMode(PeiServices);
    ASSERT_EFI_ERROR (Status);
  }

  DEBUG ((EFI_D_INFO, "FspInitAfterPciEnumerationCallback--\n"));
  return EFI_SUCCESS;
}


VOID
InitPlatformDirectIrqs (
  VOID
  )
{

  UINT16                                AcpiBaseAddr = 0;
  UINT32                                PmcBaseAddr = 0;

  DEBUG((EFI_D_INFO, "InitPlatformDirectIrqs() - Start\n"));

  AcpiBaseAddr = (UINT16)PcdGet16(PcdScAcpiIoPortBaseAddress);
  PmcBaseAddr  = (UINT32)PcdGet32(PcdPmcIpc1BaseAddress0);

  IoAnd32 (
    AcpiBaseAddr + R_SC_DIRECT_IRQ_IO_EN,
   (UINT32)(~(B_SC_DIRECT_IRQ_IO_EN_XHCI_EN | B_SC_DIRECT_IRQ_IO_EN_XDCI_EN))
    );

  //
  // Write the SCI IRQ to ACPI Bridge in PMC
  //
  MmioAndThenOr32 (
    PmcBaseAddr + R_SC_PMC_MEM_IRQ_SEL_2,
    0x0000FFFF,
    (V_SC_PMC_MEM_IRQ_SEL_2_SCI_IRQ_9 << N_SC_PMC_MEM_IRQ_SEL_2_SCIS) +
    (41 << N_SC_PMC_MEM_IRQ_SEL_2_DIR_IRQ_SEL_PMIC)
    );

   DEBUG((EFI_D_INFO, "InitPlatformDirectIrqs() - End\n"));
}

EFI_STATUS
SaveMrcData (FSPM_UPD  *FspmUpd)
{
  EFI_STATUS                    Status;
  EFI_HOB_GUID_TYPE            *HobList;
  EFI_HOB_GUID_TYPE            *HobList1;
  MRC_PARAMS_SAVE_RESTORE      *MrcDataBuf;
  UINTN                         MrcDataSize;
  UINT32                       *MrcNvDataCrc;

  BOOT_VARIABLE_NV_DATA        *VarDataBuf;
  UINTN                         VarDataSize;

  UINTN                         fileSize;

  Status = EFI_NOT_FOUND;
  if (FspmUpd->FspmConfig.MrcFastBoot && FspmUpd->FspmConfig.MrcDataSaving) {
    DEBUG ((EFI_D_INFO, "Try to find MRC training data HOB.\n"));
    if (((HobList = GetFirstGuidHob (&gFspNonVolatileStorageHobGuid)) != NULL) &&
        ((HobList1 = GetFirstGuidHob (&gFspVariableNvDataHobGuid)) != NULL)) {
      //
      // Get MRC training data
      //
      MrcDataBuf   = GET_GUID_HOB_DATA (HobList);
      MrcDataSize  = sizeof(MRC_PARAMS_SAVE_RESTORE);

      HobList = GetFirstGuidHob (&gDramPolicyPpiGuid);
      ASSERT (HobList != NULL);
      MrcNvDataCrc = (UINT32 *)GET_GUID_HOB_DATA (HobList);

      //
      // Get variable boot data
      //
      VarDataBuf   = GET_GUID_HOB_DATA (HobList1);
      VarDataSize  = sizeof(BOOT_VARIABLE_NV_DATA);

      fileSize = 0;
      HeciGetNVMFileSize(PLATFORM_CONFIG_STORE_FILE_NAME, &fileSize);
      if (fileSize != (MrcDataSize + VarDataSize)) {
        DEBUG ((EFI_D_INFO, "No MRC training data found, perform data save via HECI.\n"));
        Status = HeciWriteNVMFile(PLATFORM_CONFIG_STORE_FILE_NAME, 0, (UINT8*)MrcDataBuf, MrcDataSize, FALSE);
        DEBUG ((EFI_D_INFO, "Saved MRC training data with status (0x%08X)\n", Status));

        Status = HeciWriteNVMFile(PLATFORM_CONFIG_STORE_FILE_NAME, MrcDataSize, (UINT8*)VarDataBuf, VarDataSize, FALSE);
        DEBUG ((EFI_D_INFO, "Saved variable data with status (0x%08X)\n", Status));
      } else {
        if ((MrcNvDataCrc[0] == MrcDataBuf->MrcParamsSaveRestoreCrc) && (MrcNvDataCrc[1] == MrcDataBuf->SaMemCfgCrc)) {
          DEBUG ((EFI_D_INFO, "MRC training data is identical, skip saving\n"));
          Status = EFI_SUCCESS;
        } else {
          Status = HeciWriteNVMFile(PLATFORM_CONFIG_STORE_FILE_NAME, 0, (UINT8*)MrcDataBuf, MrcDataSize, FALSE);
          DEBUG ((EFI_D_INFO, "Saved MRC training data with status (0x%08X)\n", Status));
        }

        if (MrcNvDataCrc[2] == VarDataBuf->BootVariableNvDataCrc) {
          DEBUG ((EFI_D_INFO, "MRC variable boot data is identical, skip saving\n"));
          Status = EFI_SUCCESS;
        } else {
          Status = HeciWriteNVMFile(PLATFORM_CONFIG_STORE_FILE_NAME, MrcDataSize, (UINT8*)VarDataBuf, VarDataSize, FALSE);
          DEBUG ((EFI_D_INFO, "Saved variable data with status (0x%08X)\n", Status));
        }
        Status = EFI_SUCCESS;
      }
    }
  }
  return Status;
}

/**
  This function determines if Flash Descriptor Override (FDO) mode is enabled and produces the FDO HOB in FSP so
  future calls to FSP can determine whether FDO mode is enabled.

  @retval  EFI_SUCCESS            The FDO HOB was produced successfully.
  @retval  EFI_OUT_OF_RESOURCES   The FDO HOB could not be created due to insufficient memory resources.
**/
EFI_STATUS
EFIAPI
InitializeFlashDescriptorOverrideModeFsp (
  VOID
  )
{
  UINTN               SpiBase;
  UINTN               SpiBaseAddress0;
  UINT16              Hsfs;
  HECI_FWS_REGISTER   CseFirmwareStatus;
  UINTN               HeciStatusRegisterAddress;
  EFI_STATUS          Status              = EFI_SUCCESS;
  EFI_HOB_GUID_TYPE   *FdoEnabledGuidHob  = NULL;

  DEBUG ((DEBUG_INFO, "Checking if the SPI flash descriptor security override pin-strap is set.\n"));

  SpiBase         = MmPciBase (DEFAULT_PCI_BUS_NUMBER_SC, PCI_DEVICE_NUMBER_SPI, PCI_FUNCTION_NUMBER_SPI);
  SpiBaseAddress0 = MmioRead32 (SpiBase + R_SC_SPI_CFG_BASE) & B_SC_SPI_CFG_BASE_BAR;
  Hsfs            = MmioRead16 (SpiBaseAddress0 + R_SC_SPI_MEM_HSFS);

  HeciStatusRegisterAddress = MmPciAddress (0, SEC_BUS, SEC_DEVICE_NUMBER, HECI_FUNCTION_NUMBER, R_SEC_FW_STS0);
  CseFirmwareStatus.ul      = MmioRead32 (HeciStatusRegisterAddress);

  if (((Hsfs & B_SC_SPI_MEM_HSFS_FDOPSS) == 0) || (CseFirmwareStatus.r.SeCOperationMode >= SEC_OPERATION_MODE_SECOVR_JMPR)) {
    DEBUG ((DEBUG_INFO, "SPI FDO is set. This is an FDO boot.\n"));

    //
    // Produce the FDO Enabled HOB
    //
    FdoEnabledGuidHob = BuildGuidHob (&gFdoModeEnabledHobGuid, 0);
    if (FdoEnabledGuidHob == NULL) {
      Status = EFI_OUT_OF_RESOURCES;
    }
  }

  return EFI_SUCCESS;
}

/**
  FSP Init PEI module entry point

  @param[in]  FileHandle           Not used.
  @param[in]  PeiServices          General purpose services available to every PEIM.

  @retval     EFI_SUCCESS          The function completes successfully
  @retval     EFI_OUT_OF_RESOURCES Insufficient resources to create database
**/
EFI_STATUS
FspInitEntryPoint (
  IN       EFI_PEI_FILE_HANDLE    FileHandle,
  IN CONST EFI_PEI_SERVICES       **PeiServices
  )
{
  EFI_STATUS          Status;
  FSPS_UPD            *FspsUpd;
  SI_POLICY_PPI       *SiPolicyPpi;
  SC_POLICY_PPI       *ScPolicyPpi;
  SI_CPU_POLICY_PPI   *SiCpuPolicyPpi;
  SI_SA_POLICY_PPI    *SiSaPolicyPpi;
  FSP_INFO_HEADER     *FspInfoHeader;
  FSPM_UPD            *FspmUpd;

  Status = EFI_SUCCESS;
  FspsUpd = NULL;

  DEBUG ((DEBUG_INFO, "FspInitEntryPoint() - start\n"));
  FspInfoHeader = GetFspInfoHeaderFromApiContext ();
  SetFspInfoHeader (FspInfoHeader);

  FspsUpd = (FSPS_UPD *)GetFspApiParameter();
  if (FspsUpd == NULL) {
    //
    // Use the UpdRegion as default
    //
    FspsUpd = (FSPS_UPD *) (FspInfoHeader->ImageBase + FspInfoHeader->CfgRegionOffset);
  } else {
    FspsUpd = (FSPS_UPD *)AllocatePool((UINTN)FspInfoHeader->CfgRegionSize);
    ASSERT(FspsUpd != NULL);
    if (FspsUpd == NULL) {
      return  EFI_OUT_OF_RESOURCES;
    }
    CopyMem (FspsUpd, (VOID *)GetFspApiParameter(), FspInfoHeader->CfgRegionSize);
  }  
  SetFspSiliconInitUpdDataPointer (FspsUpd);

  Status = InitializeFlashDescriptorOverrideModeFsp ();
  ASSERT_EFI_ERROR (Status);

  FspmUpd = (FSPM_UPD *)GetFspMemoryInitUpdDataPointer ();
  DEBUG ((DEBUG_INFO, "Saving MRC data using CSE through HECI interface\n"));
  SaveMrcData (FspmUpd);

  DEBUG_CODE_BEGIN ();
    UINT32  Index;
    DEBUG ((DEBUG_INFO, "Dumping FSPS_UPD - Size: 0x%8X", sizeof(FSPS_UPD)));
    for (Index = 0; Index < sizeof (FSPS_UPD); ++Index) {
      if (Index % 0x10 == 0) {
        DEBUG ((DEBUG_INFO, "\n0x%8X:", Index));
      }
      DEBUG ((DEBUG_INFO, " 0x%02X", *(((UINT8 *)FspsUpd) + Index)));
    }
    DEBUG ((DEBUG_INFO, "\n"));
  DEBUG_CODE_END ();

  DEBUG((DEBUG_INFO, "Updating UPDs consumed in FspSiliconInitApi...\n"));

  //
  // Call ScCreateConfigBlocks to initialize platform policy structure
  // and get all Intel default policy settings.
  //
  ScCreateConfigBlocks (&ScPolicyPpi);
  //
  // Update policies which are related to UPDs
  //
  FspUpdatePeiScPolicy (ScPolicyPpi, FspsUpd);
  //
  // Install SC Policy PPI
  //
  Status = ScInstallPolicyPpi (ScPolicyPpi);
  ASSERT_EFI_ERROR (Status);

  //
  // Call CreateCpuConfigBlocks to initialize platform policy structure
  // and get all Intel default policy settings.
  //
  Status = CreateCpuConfigBlocks (&SiCpuPolicyPpi);
  ASSERT_EFI_ERROR (Status);

  if (FspsUpd != NULL) {
      FspUpdatePeiCpuPolicy(SiCpuPolicyPpi, FspsUpd);

      //
      // If FSP UPD SkipMpInit is enabled, PcdCpuMaxLogicalProcessorNumber is to 1.
      // When set to 1, driver CpuMpPei doesn't start APs.
      //
      if (FspsUpd->FspsConfig.SkipMpInit) {
          PcdSet32(PcdCpuMaxLogicalProcessorNumber, 1);
      }
  }
  // Install an instance of SecPlatformInformation2 with BIST data
  //
  if ((FspsUpd != NULL) && (FspsUpd->FspsConfig.CpuBistData)) {
    Status = PeiServicesInstallPpi (&mFspPeiSecPlatformInformation2);
    ASSERT_EFI_ERROR (Status);
  }

  //
  //
  // Install SiCpuPolicyPpi.
  // While installed, RC assumes the Policy is ready and finalized. So please
  // update and override any setting before calling this function.
  //
  Status = CpuInstallPolicyPpi (SiCpuPolicyPpi);
  ASSERT_EFI_ERROR (Status);

  SiSaPolicyPpi = NULL;
  Status = PeiServicesLocatePpi (
             &gSiSaPolicyPpiGuid,
             0,
             NULL,
             (VOID **)&SiSaPolicyPpi
             );
  if (!EFI_ERROR(Status) && (FspsUpd != NULL)) {
    FspUpdatePeiSaPolicy (SiSaPolicyPpi, ScPolicyPpi, FspsUpd);
  } else {
    DEBUG ((DEBUG_ERROR, "Failed to locate gSiSaPolicyPpiGuid\n"));
    ASSERT_EFI_ERROR (Status);
  }

  SiPolicyPpi = NULL;
  Status = PeiServicesLocatePpi (
            &gSiPolicyPpiGuid,
            0,
            NULL,
            (VOID **) &SiPolicyPpi
            );
  if (!EFI_ERROR(Status) && (FspsUpd != NULL)) {
    FspUpdatePeiSiPolicy (SiPolicyPpi, FspsUpd);
  } else {
    DEBUG ((DEBUG_ERROR, "Failed to locate gSiPolicyPpiGuid\n"));
    ASSERT_EFI_ERROR (Status);
  }

  //
  // Do basic SC init
  //
  Status = PlatformScInit (NULL, PeiServices, 0);
  ASSERT_EFI_ERROR (Status);

  Status = PeiServicesNotifyPpi (&mNotifyList[0]);
  ASSERT_EFI_ERROR (Status);

  InitPlatformDirectIrqs ();
  //
  // Installs the Report Status Code PPI
  //
  if (PcdGetBool(PcdMonoStatusCode)) {
    InstallMonoStatusCode (FileHandle, PeiServices);
  }

  mHeci2Bar = Heci2PciRead32 (R_HECIMBAR0) & 0xFFFFFFF0;
  mHeci3Bar = Heci3PciRead32 (R_HECIMBAR0) & 0xFFFFFFF0;

  DEBUG ((DEBUG_INFO, "FspInitEntryPoint() - end\n"));
  return Status;
}


/**
  Set IA_UNTRUSTED 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);
}

/**
  Initiates Untrusted mode by setting IA_UNTRUSTED bit in MSR 0x120

  @param[in] PeiServices      Pointer to PEI Services Table
  @retval EFI_SUCCESS   Successfully transfered to IA_UNTRUSTED mode
**/
EFI_STATUS
EFIAPI
TransfertoIaUntrustedMode (
  IN EFI_PEI_SERVICES  **PeiServices
  )
{
  EFI_STATUS                Status;
  EFI_PEI_MP_SERVICES_PPI  *MpServicesPpi;
  FSPS_UPD                 *FspsUpd;

  Status = EFI_SUCCESS;
  MpServicesPpi = NULL;
  FspsUpd       = NULL;


  FspsUpd = (FSPS_UPD *)GetFspSiliconInitUpdDataPointer();
  ASSERT(FspsUpd != NULL);


  if (FspsUpd->FspsConfig.SkipMpInit == 0) {
    SetIaUntrustedMode();
    //
    // Locate CpuMpCpu MpService Ppi
    //
    Status = PeiServicesLocatePpi (
               &gEfiPeiMpServicesPpiGuid,
               0,
               NULL,
               (VOID **) &MpServicesPpi
               );
    ASSERT_EFI_ERROR (Status);

    Status = MpServicesPpi->StartupAllAPs (
                             (CONST EFI_PEI_SERVICES **) PeiServices,
                              MpServicesPpi,
                              (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)));
  return Status;
}

/**
  Implementation of the FspPlatformInformation2 service in EFI_SEC_PLATFORM_INFORMATION2_PPI.

  @param  PeiServices                The pointer to the PEI Services Table.
  @param  StructureSize              The pointer to the variable describing size of the input buffer.
  @param  PlatformInformationRecord2 The pointer to the EFI_SEC_PLATFORM_INFORMATION_RECORD2.

  @retval EFI_SUCCESS                The data was successfully returned.
  @retval EFI_BUFFER_TOO_SMALL       The buffer was too small. The current buffer size needed to
                                     hold the record is returned in StructureSize.
**/
EFI_STATUS
EFIAPI
FspSecPlatformInformation2 (
  IN CONST EFI_PEI_SERVICES                   **PeiServices,
  IN OUT UINT64                               *StructureSize,
  OUT EFI_SEC_PLATFORM_INFORMATION_RECORD2    *PlatformInformationRecord2
 )
{
  FSP_S_CONFIG                         *FspsConfigUpd;
  EFI_SEC_PLATFORM_INFORMATION_RECORD2 *PlatformInformationRecord2temp;
  UINTN                                InformationDataSize;

  FspsConfigUpd = &((FSPS_UPD *) GetFspSiliconInitUpdDataPointer())->FspsConfig;

  PlatformInformationRecord2temp = (EFI_SEC_PLATFORM_INFORMATION_RECORD2 *) FspsConfigUpd->CpuBistData;
  InformationDataSize = sizeof (EFI_SEC_PLATFORM_INFORMATION_RECORD2) + sizeof (EFI_SEC_PLATFORM_INFORMATION_CPU) * (PlatformInformationRecord2temp->NumberOfCpus - 1);
  if (*StructureSize < InformationDataSize) {
    *StructureSize = InformationDataSize;
    return EFI_BUFFER_TOO_SMALL;
  }
  
  CopyMem ((EFI_SEC_PLATFORM_INFORMATION_RECORD2 *) PlatformInformationRecord2,
    (VOID *) FspsConfigUpd->CpuBistData,
    InformationDataSize
    );

  DEBUG_CODE_BEGIN ();
    UINTN     Index;
    DEBUG ((DEBUG_INFO, "BIST From PPI NumberOfCpus - %x\n\n", PlatformInformationRecord2->NumberOfCpus));
    for (Index = 0; Index < PlatformInformationRecord2->NumberOfCpus; Index++) {  
      DEBUG ((DEBUG_INFO, "BIST From PPI IA32HealthFlags Instance(%x)- %x\n", Index, PlatformInformationRecord2->CpuInstance[Index].InfoRecord.x64HealthFlags.Uint32));
      DEBUG ((DEBUG_INFO, "BIST From PPI CpuLocation Instance(%x) - %x\n", Index, PlatformInformationRecord2->CpuInstance[Index].CpuLocation));
    }
  DEBUG_CODE_END ();
  return EFI_SUCCESS;
}
