/** @file
  PEIM to initialize IGFX PM

 @copyright
  INTEL CONFIDENTIAL
  Copyright 1999 - 2017 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 Reference:
**/
#include <Library/PeiServicesLib.h>
#include <Library/IoLib.h>
#include <Library/BaseMemoryLib.h>
#include <Library/DebugLib.h>
#include <Library/MmPciLib.h>
#include <IndustryStandard/Pci22.h>
#include <Library/PeiGraphicsInitLib.h>
#include <Library/PciLib.h>
#include <Library/TimerLib.h>
#include <SaAccess.h>
#include <ScAccess.h>
#include <CpuRegs.h>
#include <Ppi/SiPolicyPpi.h>
#include <Pi/PiBootMode.h>

///
/// Driver Consumed PPI Prototypes
///
#include <Ppi/SaPolicy.h>

GLOBAL_REMOVE_IF_UNREFERENCED BOOT_SCRIPT_REGISTER_SETTING  gSaGtRC6Registers[] = {
  //
  // Render/Video/Blitter Idle Max Count
  //
  { 0x0, R_GTT_PWRCTX_MAXCNT,           0x0, V_GTT_IDLE_WAIT_TIME},
  { 0x0, R_GTT_VCS_PWRCTX_MAXCNT,       0x0, V_GTT_IDLE_WAIT_TIME},
  { 0x0, R_GTT_BCS_PWRCTX_MAXCNT,       0x0, V_GTT_IDLE_WAIT_TIME},
  { 0x0, R_GTT_VECS_PWRCTX_MAXCNT,      0x0, V_GTT_IDLE_WAIT_TIME},
  { 0x0, R_GTT_VCS_UNIT1_PWRCTX_MAXCNT, 0x0, V_GTT_IDLE_WAIT_TIME},
  { 0x0, R_GTT_VCS_UNIT1_PWRCTX_MAXCNT, 0x0, V_GTT_IDLE_WAIT_TIME},
  { 0x0, R_GTT_GUC_PM_MAXCNT,           0x0, V_GTT_IDLE_WAIT_TIME},
  //
  // Enable Idle Messages
  //
  {0x0, R_GTT_RC_PSMI_CTRL,       0x0, V_GTT_IDLE_MASK},
  {0x0, R_GTT_VCS_RC_PSMI_CTL,    0x0, V_GTT_IDLE_MASK},
  {0x0, R_GTT_BCS_RC_PSMI_CTL,    0x0, V_GTT_IDLE_MASK},
  {0x0, R_GTT_VECS_PSMI_CTL,      0x0, V_GTT_IDLE_MASK},
  {0x0, R_GTT_VCS_UNIT1_PSMI_CTL, 0x0, V_GTT_IDLE_MASK},
};

/**
  InitializeCdClock: Initialize Cd Clock for display engine.

  @param[in] GtConfig             Instance of GRAPHICS_CONFIG

  @retval EFI_SUCCESS             The function completed successfully
  @retval EFI_ABORTED             S3 boot - display already initialized
  @retval EFI_UNSUPPORTED         iGfx disabled, iDisplay Audio not present
  @retval EFI_NOT_FOUND           SaPolicy or temporary GTT base address not found
**/
EFI_STATUS
InitializeCdClock (
  IN GRAPHICS_CONFIG *GtConfig
  )
{
  EFI_BOOT_MODE    BootMode;
  EFI_STATUS       Status;
  UINT32           Data32;
  UINTN            GttMmAdr;

  DEBUG ((DEBUG_INFO, "InitializeCdClock() Start\n"));

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

  if (BootMode == BOOT_ON_S3_RESUME) {

    GttMmAdr = GtConfig->GttMmAdr;

    MmioAnd32 (GttMmAdr + R_GTT_NDE_RSTWRN_OPT, (UINT32) ~(BIT4)); // Clear NDE_RSTWRN_OPT RST PCH Handshake En to 0b.
    PollGtReady (GttMmAdr, R_GTT_FUSE_STATUS, B_GTT_FUSE_PG0_DIST_STATUS, B_GTT_FUSE_PG0_DIST_STATUS);  // Poll Fuse PG0 distribution status

    MmioOr32 (GttMmAdr + R_GTT_PWR_WELL_CTL1, BIT29);           // Enable PG1
    PollGtReady (GttMmAdr, R_GTT_PWR_WELL_CTL1, BIT28, BIT28);  // Poll for PG1 status
    PollGtReady (GttMmAdr, R_GTT_FUSE_STATUS, B_GTT_FUSE_PG1_DIST_STATUS, B_GTT_FUSE_PG1_DIST_STATUS);  // Poll Fuse PG1 distribution status

    MmioOr32 (GttMmAdr + R_GTT_PWR_WELL_CTL1, BIT31);           // Enable PG2
    PollGtReady (GttMmAdr, R_GTT_PWR_WELL_CTL1, BIT30, BIT30);  // Poll for PG2 status
    PollGtReady (GttMmAdr, R_GTT_FUSE_STATUS, B_GTT_FUSE_PG2_DIST_STATUS, B_GTT_FUSE_PG2_DIST_STATUS);  // Poll Fuse PG2 distribution status
    
    //
    //Inform Power controll of upcoming freq change
    //
    PollGtReady (GttMmAdr, R_GTT_GTDRIVER_MAILBOX_INTERFACE, BIT31, 0);
    MmioWrite32 (GttMmAdr + R_GTT_GTDRIVER_MAILBOX_DATA0, 0x80000000);
    MmioWrite32 (GttMmAdr + R_GTT_GTDRIVER_MAILBOX_INTERFACE, 0x80000017);
    PollGtReady (GttMmAdr, R_GTT_GTDRIVER_MAILBOX_INTERFACE, BIT31, 0);
    
    //
    //Enable DE_PLL
    //
    MmioWrite32 (GttMmAdr + R_GTT_DE_PLL_CTL,V_DE_PLL_1152MHZ);
    MmioOr32 (GttMmAdr + R_GTT_DE_PLL_ENABLE, B_GTT_DE_PLL_ENABLE);
    PollGtReady (GttMmAdr, R_GTT_DE_PLL_ENABLE, B_GTT_DE_PLL_LOCK, B_GTT_DE_PLL_LOCK);

    //
    // Program CD clock and 2x Divider.
    // CdClock = 0; 158.4 Mhz
    // CdClock = 1; 316.8 Mhz
    // CdClock = 2; 79.2  Mhz
    //    
    switch (GtConfig->CdClock) {
      case 0 :
        Data32 = (V_GTT_DE_CD2X_DIVIDER_158MHZ << 22) | (V_GTT_PIPE_NONE << 20) | (V_GTT_CDCLK_CTL_158MHZ);
        break;
      case 1 :
        Data32 = (V_GTT_DE_CD2X_DIVIDER_316MHZ << 22) | (V_GTT_PIPE_NONE << 20) | (V_GTT_CDCLK_CTL_316MHZ);
        break;
      case 2 :
        Data32 = (V_GTT_DE_CD2X_DIVIDER_79MHZ << 22) | (V_GTT_PIPE_NONE << 20) | (V_GTT_CDCLK_CTL_79MHZ);
        break;
      default:
        Data32 = (V_GTT_DE_CD2X_DIVIDER_316MHZ << 22) | (V_GTT_PIPE_NONE << 20) | (V_GTT_CDCLK_CTL_316MHZ);
    }
    MmioWrite32 (GttMmAdr + R_GTT_CDCLK_CTL,Data32 );
    
    //    
    //Inform Power controller of the selected freq
    // 
    MmioWrite32 (GttMmAdr + R_GTT_GTDRIVER_MAILBOX_DATA0, 0x0000000D);
    MmioWrite32 (GttMmAdr + R_GTT_GTDRIVER_MAILBOX_INTERFACE, 0x80000017);

    //DBUF Power Well Control
    MmioOr32 (GttMmAdr + R_GTT_DBUF_CTL, B_GTT_DBUF_CTL_POWER_REQUEST);
    PollGtReady (GttMmAdr, R_GTT_DBUF_CTL, B_GTT_DBUF_CTL_POWER_STATE, B_GTT_DBUF_CTL_POWER_STATE);
  }

  DEBUG ((DEBUG_INFO, "InitializeCdClock() End\n"));
  return EFI_SUCCESS;
}

/**
  Program the max Cd Clock supported by the platform

  @param[in] SaPolicy            Instance of SA policy
  @param[in] GttMmAdr            Base Address of IGFX MMIO BAR

  @retval EFI_SUCCESS            GT Power Management initialization complete
  @retval EFI_INVALID_PARAMETER  The input parameter is invalid
**/
EFI_STATUS
CdClkInit (
  IN GRAPHICS_CONFIG *GtConfig,
  IN UINTN           GttMmAdr
  )
{
  UINT32 Data32Or;
  ///
  /// CDCLK_CTL - GttMmAdr + 0x46000
  /// CdClock = 0; 158.4 Mhz - [10:0] = 0x13B
  /// CdClock = 1; 316.8 Mhz - [10:0] = 0x278
  /// CdClock = 2; 79.2  Mhz - [10:0] = 0x09C
  ///
  switch (GtConfig->CdClock) {
    case 0 :
		Data32Or = V_GTT_CDCLK_CTL_158MHZ;
    break;
    case 1 :
		Data32Or = V_GTT_CDCLK_CTL_316MHZ;
    break;
    case 2 :
		Data32Or = V_GTT_CDCLK_CTL_79MHZ;
    break;
    default:
      return EFI_INVALID_PARAMETER;
  }
  DEBUG ((DEBUG_INFO, "CdClkInit Value[10:0] = 0x%x\n", Data32Or));
  MmioAndThenOr32 (GttMmAdr + R_GTT_CDCLK_CTL, 0xFFFFF800, Data32Or);
  return EFI_SUCCESS;
}


/**
  Initialize GT PowerManagement of SystemAgent.

  @param[in] GtConfig             Instance of GRAPHICS_CONFIG
  @param[in] GttMmAdr             Base Address of IGFX MMIO BAR
  @param[in] MchBarBase           Base Address of MCH_BAR

  @retval EFI_SUCCESS            GT Power Management initialization complete
  @retval EFI_INVALID_PARAMETER  The input parameter is invalid
**/
EFI_STATUS
PmInit (
  IN GRAPHICS_CONFIG *GtConfig,
  IN UINTN           GttMmAdr,
  IN UINT32          MchBarBase
  )
{
  UINT8                         i;
  UINT32                        Data32And;
  UINT32                        Data32Or;
  UINT32                        Data32;
  UINT32                        RegOffset;
  UINT32                        Data32Mask;
  UINT32                        Result;
  UINT32                        GMSSizeSelector;
  UINT32                        GMSSize;
  UINT32                        GMSBase;
  UINT32                        RC6CXTBASE;
  UINTN                         McD0BaseAddress;

  McD0BaseAddress   = MmPciBase (SA_MC_BUS, SA_MC_DEV, SA_MC_FUN);
  //
  // PmInit Initialization
  //
  DEBUG ((EFI_D_INFO, "Initializing GT PowerManagement\n"));

  if ((GttMmAdr == 0) || (MchBarBase == 0) || (GtConfig == NULL)) {
    DEBUG ((DEBUG_ERROR, "Invalid parameters for PmInit\n"));
    return EFI_INVALID_PARAMETER;
  }

  ///
  /// 1a. Set RC6 Context Location
  ///
  RegOffset                     = R_GTT_RC6_LOCATION;
  Data32                        = (B_GTT_RC6_CONTEXT_LOCK | B_GTT_RC6_SEND_DRAM);
  MmioWrite32 (GttMmAdr + RegOffset, Data32);

  ///
  /// 1b. Set Context Base
  /// Must set to a physical address within GT stolen Memory WOPCM, at least 24KB from the top.
  /// This range must be coordinated with other uses (like GuC and PAVP);
  /// It is expected that the upper 24KB of stolen memory is the proper location
  /// Also set bit 2:0 to 111b
  ///

  GMSSizeSelector = MmioRead32 (McD0BaseAddress + R_SA_GGC);
  GMSSizeSelector = (GMSSizeSelector & B_SA_GGC_GMS_MASK) >> N_SA_GGC_GMS_OFFSET;
  GMSSize = (UINT32) GMSSizeSelector * 32;
  DEBUG ((DEBUG_INFO, "GMSSize: %dMB\n",GMSSize));
  GMSBase = MmioRead32 (McD0BaseAddress + R_SA_BDSM) & B_SA_BDSM_BDSM_MASK;
  DEBUG ((DEBUG_INFO, "GMSBase read from R_SA_BDSM: 0x%x\n",GMSBase));

  //
  //RC6CXTBASE: the algorithm is BDSM+BDSM_SIZE-RC6CTXBASE_SIZE if WOPCM is not enabled.  If WOPCM is enabled, it should be WOPCMBASE+WOPCM_SIZE-RC6CTXBASE_SIZE.
  //In current design, WOPCM region : WOPCMbase (PAVPC bits 31:20) to DSMtop (BDSM + GMS) with PCME (PAVPC bit 0) = 1
  //so we can use RC6CXTBASE = DSMtop (BDSM + GMS) -RC6CTXBASE_SIZE in either case
  //

  RegOffset                     = R_GTT_RC6_CONTEXT_BASE;
  RC6CXTBASE                    = GMSBase + GMSSize * SIZE_1MB - RC6CTXBASE_SIZE;
  Data32                        = RC6CXTBASE | B_GTT_RC6_CONTEXT_BASE_DRAM | B_GTT_RC6_CONTEXT_BASE_LOCK;
  DEBUG ((DEBUG_INFO, "RC6 Context Base: 0x%x\n",RC6CXTBASE));
  MmioWrite32 (GttMmAdr + RegOffset, Data32);

  if ((GtConfig->PmSupport) && (BxtStepping() != BxtPB1)){   

    ///
    /// Following steps are following Gen9 GT PM Programming Guide
    ///
    ///
    /// Set chicken bits to enable GT B+ stepping workarounds
    ///
    RegOffset = R_GTT_CONFIG0_RPM_UNIT;
    Data32 = V_GTT_CONFIG0_SET_CHICKEN_BITS;
    MmioWrite32(GttMmAdr + RegOffset, Data32);

    ///
    /// 1aa. Enable all GTI-Uncore clock gating
    ///
    RegOffset                     = R_GTT_CONFIG_RCP_UNIT;
    Data32                        = V_GTT_CONFIG_RCP_ENABLE_CLKGATE_ALL;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    ///
    /// 2a. Enable Force Wake
    ///
    RegOffset                     = R_GTT_MUL_FORCE_WAKE;
    Data32                        = B_GTT_MUL_FORCE_WAKE_MFW_MASK | B_GTT_MUL_FORCE_WAKE_MFW0;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    ///
    /// 2b. Poll to verify Force Wake Acknowledge Bit
    ///
    RegOffset                     = R_GTT_GTSP1_0_2_0_GTTMMADR;
    Data32Mask                    = BIT0;
    Result                        = 1;
    DEBUG ((DEBUG_INFO, "Polling Force Wake Acknowledge Bit 0x130044 bit0 to be 1...\n"));
    PollGtReady (GttMmAdr, RegOffset, Data32Mask, Result);

    ///
    /// 3a. Enabling Push Bus Metric Control
    ///
    RegOffset                     = R_GTT_PUSHBUS_ENABLE;
    Data32                        = V_PUSHBUS_ENABLE;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    ///
    /// 3b. Configuring Push Bus Shift
    ///
    RegOffset                     = R_GTT_PUSHBUS_SHIFT;
    Data32                        = B_GTT_PUSHBUS_SHIFT_C0RESIDENCY;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    ///
    /// 3c. Pushbus Metric Control
    ///
    RegOffset                     = R_GTT_PUSHBUS_CONTROL;
    Data32                        = B_GTT_PUSHBUS_CONTROL_LOCK | B_GTT_PUSHBUS_CONTROL_TIME_EN_C0RESIDENCY;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    ///
    /// 4. Program GfxPause Register
    ///
    RegOffset                     = R_GTT_GFX_PAUSE;
    Data32                        = (B_GTT_GFX_PAUSE_LOCK | 
                                     B_GTT_GFX_PAUSE_EU_EN |
                                     B_GTT_GFX_PAUSE_SAMPLER_EN |
                                     V_GTT_GFX_MIN_PMUNIT_WAIT_CLK) ;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    ///
    /// 5a. GPM Control
    ///
    RegOffset                     = R_GTT_MISC_CTRL0;
    Data32                        = (B_GTT_MISC_CTRL0_LOCK |
                                     B_GTT_MISC_CTRL0_RC_FIFOBLOCK_TYPE |
                                     (V_GTT_MISC_CTRL0_RC_SW_STATE_FLR_DEFAULT << 22) |
                                     B_GTT_MISC_CTRL0_CPD_OVERRIDE_IA_AS_GVL |
                                     B_GTT_MISC_CTRL0_RC6_ALL_IDLE_EN);
                                     
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    if ((GetBxtSeries() == BxtP && BxtStepping() >= BxtPB0) || (GetBxtSeries() != BxtP && BxtStepping() >= BxtC0) || (GetBxtSeries() == Glk)) {
      RegOffset                     = R_GTT_ECO_BUSRST;
      Data32Or                      = B_GTT_ECO_BUSRST_CPDENTER_MGSRBLOCK_SWAP_EN | R_GTT_ECO_BUSRST_LOCK | B_GTT_ECO_BUSRST_GACFG_FENCE_C6STATUS_WRITE ; // This must be added to the GFx programming document
      Data32And                     = (UINT32) ~(B_GTT_ECO_BUSRST_CPDENTER_MGSRBLOCK_SWAP_EN | B_GTT_ECO_BUSRST_CPDEXIT_MGSRUNBLOCK_SWAP_EN);
      MmioAndThenOr32 (GttMmAdr + RegOffset, Data32And, Data32Or);
    }

    ///
    /// 5b. Enable DOP clock gating.
    ///

        Data32 = V_GTT_MISCCPCTL_GLK;

    RegOffset                     = R_GTT_MISCCPCTL;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);
    DEBUG ((DEBUG_INFO, "Enabled DOP clock gating \n"));

    ///
    /// 5c-5g. Enable unit level clock gates
    ///
      RegOffset                     = R_GTT_UCGCTL1;
      Data32                        = 0x00000000;
      MmioWrite32 (GttMmAdr + RegOffset, Data32);

      RegOffset                     = R_GTT_UCGCTL2;
      Data32                        = B_GTT_UCGCTL2_CG3DDISVDS | B_GTT_UCGCTL2_CG3DDISTDL | B_GTT_UCGCTL2_CG3DDISRCPB;
      MmioWrite32 (GttMmAdr + RegOffset, Data32);

      RegOffset                     = R_GTT_UCGCTL3;
      Data32                        = 0x00000000;
      MmioWrite32 (GttMmAdr + RegOffset, Data32);

      RegOffset                     = R_GTT_UCGCTL4;
      Data32                        = B_GTT_UCGCTL4_CG3DDISL3BANK_CR2X | B_GTT_UCGCTL4_CG3DDISVDS;
      MmioWrite32 (GttMmAdr + RegOffset, Data32);
      //
      //0x9430 [28] = 1: HDCREQ Clock Gating Disable for all steppings
      //0x9430[14] = 1: SDE units Clock Gating Disable for A steppings
      //
      RegOffset                     = R_GTT_UCGCTL6;
      Data32 =  MmioRead32 (GttMmAdr + RegOffset);

      Data32 |= B_GTT_UCGCTL6_CG3DDISHDC;
      MmioWrite32 (GttMmAdr + RegOffset, Data32);



    ///
    /// 6-7 SW RC6 Settings
    ///
    for (i = 0; i < sizeof (gSaGtRC6Registers) / sizeof (BOOT_SCRIPT_REGISTER_SETTING); ++i) {
      RegOffset                     = gSaGtRC6Registers[i].Offset;
      Data32And                     = gSaGtRC6Registers[i].AndMask;
      Data32Or                      = gSaGtRC6Registers[i].OrMask;

      MmioAndThenOr32 (GttMmAdr + RegOffset, Data32And, Data32Or);
    }

    ///
    /// 8A. Set Normal frequency request:
    ///
    Data32 = (V_GTT_RP_FREQ_NORMAL_UNSLICE_RATIO << 23);
    RegOffset = R_GTT_RP_FREQ_NORMAL;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    ///
    /// 8A. RP Control
    ///
    Data32 = ((V_GTT_RP_CTRL_RP_RP_SW_MODE_SEL << 9) |
              B_GTT_RP_CTRL_RP_INCL_MEDIA |
              B_GTT_RP_CTRL_RP_HW_MODE_EN |
              (V_GTT_RP_CTRL_RP_HW_INCFREQ_MODE_SEL << 3)|
              V_GTT_RP_CTRL_RP_HW_DECFREQ_MODE_SEL);                    
               
    RegOffset = R_GTT_RP_CTRL;
    MmioWrite32 (GttMmAdr + RegOffset, Data32);

    ///
    /// 9. Enabling to enter RC6 state in idle mode.
    ///
    if (GtConfig->EnableRenderStandby) {
      RegOffset                     = R_GTT_RC_CTRL1;
      Data32                        = (V_GTT_RC_SW_STATE_REQ << 16);
      MmioWrite32 (GttMmAdr + RegOffset, Data32);
      DEBUG ((DEBUG_INFO, "Entered RC6 state in idle mode\n"));
    }

      ///
      /// 10a. Clear offset 0xA188 [31:0] to clear the force wake enable
      ///
      RegOffset                     = R_GTT_MUL_FORCE_WAKE;
      Data32                        = B_GTT_MUL_FORCE_WAKE_MFW_MASK;
      MmioWrite32 (GttMmAdr + RegOffset, Data32);

      ///
      /// 10b. Poll until clearing is cleared to verify the force wake acknowledge.
      ///
      RegOffset                     = R_GTT_GTSP1_0_2_0_GTTMMADR;
      Data32Mask                    = BIT0;
      Result                        = 0;
      DEBUG ((DEBUG_INFO, "Polling Force Wake acknowledge Bit 0x130044 bit0 to be 0...\n"));
      PollGtReady (GttMmAdr, RegOffset, Data32Mask, Result);
  }
  return EFI_SUCCESS;
}

/**
  Initialize PAVP feature of SystemAgent.

  @param[in] GRAPHICS_CONFIG  *GtConfig

  @retval EFI_SUCCESS     PAVP initialization complete
**/
EFI_STATUS
PavpInit (
  IN       GRAPHICS_CONFIG             *GtConfig
  )
{
  UINT32        PcmBase = 0;
  UINTN         McD0BaseAddress;
  UINT32        Pavpc;
  UINT32        GMSSizeSelector;
  UINT32        GMSSize;
  UINT32        GMSBase;
  SI_POLICY_PPI *SiPolicyPpi;
  EFI_STATUS    Status;

  Status = PeiServicesLocatePpi (
             &gSiPolicyPpiGuid,
             0,
             NULL,
             (VOID **)&SiPolicyPpi
             );
  ASSERT_EFI_ERROR (Status);

  ///
  /// PAVP Initialization
  ///
  DEBUG ((EFI_D_INFO, "Initializing PAVP - Start\n"));
  McD0BaseAddress = MmPciBase (SA_MC_BUS, SA_MC_DEV, SA_MC_FUN);
  Pavpc           = MmioRead32 (McD0BaseAddress + R_SA_PAVPC);
  Pavpc &= (UINT32) ~(B_SA_PAVPC_HVYMODSEL_MASK | B_SA_PAVPC_PCMBASE_MASK | B_SA_PAVPC_PAVPE_MASK | B_SA_PAVPC_PCME_MASK);

  //
  //WOPCM region : WOPCMbase (PAVPC bits 31:20) to DSMtop (BDSM + GMS) with PCME (PAVPC bit 0) = 1
  //
  GMSSizeSelector = MmioRead32 (McD0BaseAddress + R_SA_GGC);
  GMSSizeSelector = (GMSSizeSelector & B_SA_GGC_GMS_MASK) >> N_SA_GGC_GMS_OFFSET;
  GMSSize = (UINT32) GMSSizeSelector * 32;
  GMSBase = MmioRead32 (McD0BaseAddress + R_SA_BDSM) & B_SA_BDSM_BDSM_MASK;

  if (GtConfig->PavpEnable == 1) {
        PcmBase = GMSBase + (GMSSize - PAVP_PCM_SIZE_1_MB ) * SIZE_1MB;

    Pavpc |= PcmBase;
    Pavpc |= B_SA_PAVPC_PCME_MASK | B_SA_PAVPC_PAVPE_MASK;
      Pavpc &= (UINT32) ~(B_SA_PAVPC_OVTATTACK_MASK);

      Pavpc &= (UINT32) ~(B_SA_PAVPC_AUTO_TEARDOWN_GRACE_PERIOD_MASK);
 
    if ((GtConfig->PavpAsmf == 1) && (SiPolicyPpi->OsSelection == SiWindows)) {
      Pavpc |= B_SA_PAVPC_ASMFEN_MASK;
    } else {
      Pavpc &= (UINT32) ~(B_SA_PAVPC_ASMFEN_MASK);
    }

  }

  ///
  /// Lock PAVPC Register
  ///
    Pavpc |= B_SA_PAVPC_PAVPLCK_MASK;
  MmioWrite32 (McD0BaseAddress + R_SA_PAVPC, Pavpc);

  DEBUG ((EFI_D_INFO, "Initializing PAVP - End\n"));
  return EFI_SUCCESS;
}



/**
  Initialize GT Power management

  @param[in] GRAPHICS_CONFIG             GtConfig

  @retval EFI_SUCCESS           GT Power management initialization complete
**/
EFI_STATUS
GraphicsPmInit (
  IN       GRAPHICS_CONFIG             *GtConfig
  )
{
  UINT32                LoGTBaseAddress;
  UINT32                HiGTBaseAddress;
  UINTN                 McD2BaseAddress;
  UINTN                 McD0BaseAddress;
  UINT32                GttMmAdr;
  UINT32                MchBarBase;
  UINT32                Data32;
  UINT32                Data32And;

  DEBUG((DEBUG_INFO, " iGfx Power management start.\n"));

  GttMmAdr   = 0;
  MchBarBase = 0;
  MchBarBase = MmioRead64 (MmPciBase (SA_MC_BUS, SA_MC_DEV, SA_MC_FUN) + R_SA_MCHBAR_REG) &~BIT0;
  McD0BaseAddress = MmPciBase (SA_MC_BUS, SA_MC_DEV, SA_MC_FUN);

  ///
  /// If device 0:2:0 (Internal Graphics Device, or GT) is enabled, then Program GttMmAdr,
  ///
  McD2BaseAddress = MmPciBase (SA_IGD_BUS, SA_IGD_DEV, SA_IGD_FUN_0);
  if (MmioRead16 (McD2BaseAddress + R_SA_IGD_VID) != 0xFFFF) {
    ///
    /// Program GT PM Settings if GttMmAdr allocation is Successful
    ///
    GttMmAdr                          = GtConfig->GttMmAdr;
    LoGTBaseAddress                   = (UINT32) (GttMmAdr & 0xFFFFFFFF);
    HiGTBaseAddress                   = 0;
    MmioWrite32 (McD2BaseAddress + R_SA_IGD_GTTMMADR, LoGTBaseAddress);
    MmioWrite32 (McD2BaseAddress + R_SA_IGD_GTTMMADR + 4, HiGTBaseAddress);

    ///
    /// Enable Bus Master and Memory access on 0:2:0
    ///
    MmioOr16 (McD2BaseAddress + R_SA_IGD_CMD, (BIT2 | BIT1));

    if (BxtStepping () >= BxtB0) {
      MmioOr32 (GttMmAdr + R_GTT_GU_CNTL, B_GTT_GU_CNTL_IPCOVRDDIS);
    }
    
    if (GetBxtSeries() == Glk) {
      MmioOr32 (GttMmAdr + R_10100C_RSVD, BIT0);
    }


    ///
    /// Enable VGA paging only when IGD has VGA decode enabled.
    ///
    Data32 = MmioRead32 (McD0BaseAddress + R_SA_GGC);
    Data32 = (Data32 & 0x2) >> 1;
    if (Data32 == 0) {
      MmioWrite32 (MchBarBase + MCHBAR_RANGE_BUNIT, 0x3);
    }

    ///
    /// PmInit Initialization
    ///
    DEBUG ((DEBUG_INFO, "Initializing GT PowerManagement\n"));
    PmInit (GtConfig, GttMmAdr, MchBarBase);

    ///
    /// Copy MSR_PLATFORM_INFO.SAMPLE_PART(FUSE_PROD_PART) bit to CONFIG0 Address D00h, bit 30 and Lock bit
    ///
    Data32 = (AsmReadMsr64(MSR_PLATFORM_INFO) & B_PLATFORM_INFO_SAMPLE_PART);
    Data32 = (Data32 << 3) | BIT31;
    Data32And = (UINT32) ~(BIT31 | BIT30);
    MmioAndThenOr32 (GttMmAdr + R_GTT_CONFIG0_RPM_UNIT, Data32And, Data32);
    Data32 = MmioRead32 (GttMmAdr + R_GTT_CONFIG0_RPM_UNIT);
    DEBUG ((DEBUG_INFO, "Update CONFIG0 Address D00 : %x\n", Data32));

    ///
    ///Program the max Cd Clock supported by the platform
    ///
    if (PLATFORM_ID == VALUE_REAL_PLATFORM) {
      CdClkInit (GtConfig, GttMmAdr);
    }

    ///
    ///Initialize Display CD Clock.
    ///
    InitializeCdClock (GtConfig);

    ///
    /// Lock Programmable BDFs for the Audio functions and Touch Controller
    ///
    MmioWrite32 (GttMmAdr + R_GTT_GSA_AUDIO_BDF,
                 ((DEFAULT_PCI_BUS_NUMBER_SC << 24)|
                  (PCI_DEVICE_NUMBER_HDA << 19) |
                  (PCI_FUNCTION_NUMBER_HDA << 16) |
                  B_GTT_GSA_AUDIO_BDF_LOCK));

    MmioOr32 (GttMmAdr + R_GTT_GSA_TOUCH_BDF, B_GTT_GSA_TOUCH_BDF_LOCK);

    ///
    /// Disable Bus Master and Memory access on 0:2:0
    ///
    MmioAnd16 (McD2BaseAddress + R_SA_IGD_CMD, (UINT16) ~(BIT2 | BIT1));

    ///
    /// Clear GttMmAdr
    ///
    MmioWrite32 (McD2BaseAddress + R_SA_IGD_GTTMMADR, 0);
    MmioWrite32 (McD2BaseAddress + R_SA_IGD_GTTMMADR + 0x4, 0);

  }
  DEBUG((DEBUG_INFO, "iGfx Power management end.\n"));
  DEBUG ((DEBUG_INFO, "Lock GGC, BDSM and BGSM registers\n"));

  //
  // Lock the following registers - GGC, BDSM, BGSM
  //
  MmioOr32 (McD0BaseAddress + R_SA_GGC, BIT0);
  MmioOr32 (McD0BaseAddress + R_SA_BDSM, BIT0);
  MmioOr32 (McD0BaseAddress + R_SA_BGSM, BIT0);

  return EFI_SUCCESS;
}

/**
  "Poll Status" for GT Readiness

  @param[in] Base             Base address of MMIO
  @param[in] Offset           MMIO Offset
  @param[in] Mask             Mask
  @param[in] Result           Value to wait for

  @retval  None
**/
VOID
PollGtReady (
  IN       UINT64                       Base,
  IN       UINT32                       Offset,
  IN       UINT32                       Mask,
  IN       UINT32                       Result
  )
{
  UINT32  GtStatus;
  UINT16  StallCount;

  StallCount = 0;

  ///
  /// Register read
  ///
  GtStatus = MmioRead32 ((UINTN) Base + Offset);

  while (((GtStatus & Mask) != Result) && (StallCount < GT_WAIT_TIMEOUT)) {

    GtStatus = MmioRead32 ((UINTN) Base + Offset);
    ///
    /// 1mSec wait
    ///
    MicroSecondDelay (1000);
    StallCount = StallCount + 1;
  }
  ASSERT ((StallCount != GT_WAIT_TIMEOUT));
}
