Exemplo n.º 1
0
/**
 * Transitions the executing core to the desired P-state.
 *
 * This function implements the AMD_CPU_EARLY_PARAMS.MemInitPState parameter, and is
 * run by all system cores.
 *
 * @param[in]  StdHeader         Config handle for library and services
 * @param[in]  CpuEarlyParamsPtr Required input parameters for early CPU initialization
 *
 */
VOID
STATIC
GoToMemInitPstateCore (
  IN       AMD_CONFIG_PARAMS    *StdHeader,
  IN       AMD_CPU_EARLY_PARAMS *CpuEarlyParamsPtr
  )
{
  CPU_SPECIFIC_SERVICES *FamilySpecificServices;

  GetCpuServicesOfCurrentCore ((CONST CPU_SPECIFIC_SERVICES **)&FamilySpecificServices, StdHeader);
  FamilySpecificServices->TransitionPstate (FamilySpecificServices, CpuEarlyParamsPtr->MemInitPState, (BOOLEAN) FALSE, StdHeader);
}
Exemplo n.º 2
0
/**
 *  IDS Backend Function for Target Pstate
 *
 *
 *  @param[in,out]   DataPtr      The Pointer of AMD_CPU_EARLY_PARAMS.
 *  @param[in,out]   StdHeader    The Pointer of AMD_CONFIG_PARAMS.
 *  @param[in]     IdsNvPtr     The Pointer of NV Table.
 *
 *  @retval IDS_SUCCESS         Backend function is called successfully.
 *  @retval IDS_UNSUPPORTED     No Backend function is found.
 *
 **/
IDS_STATUS
IdsSubTargetPstate (
  IN OUT   VOID *DataPtr,
  IN OUT   AMD_CONFIG_PARAMS *StdHeader,
  IN       IDS_NV_ITEM *IdsNvPtr
  )
{
  IDS_STATUS tarpst;
  CPU_SPECIFIC_SERVICES   *FamilySpecificServices;

  IDS_NV_READ_SKIP (tarpst, AGESA_IDS_NV_TARGET_PSTATE, IdsNvPtr) {
    GetCpuServicesOfCurrentCore (&FamilySpecificServices, StdHeader);
    FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) tarpst, (BOOLEAN) FALSE, StdHeader);
  }
Exemplo n.º 3
0
/**
 * Cold reset support routine for F10PmNbCofVidInit.
 *
 * This function implements steps 3, 4, & 5 on each core.
 *
 * @param[in]  NewNbVid           NewNbVid determined by core 0 in step 2.
 * @param[in]  StdHeader          Config handle for library and services.
 *
 */
VOID
STATIC
PmNbCofVidInitP0P1Core (
  IN       VOID *NewNbVid,
  IN       AMD_CONFIG_PARAMS *StdHeader
  )
{
  UINT32 MsrAddress;
  UINT64 MsrRegister;
  CPU_SPECIFIC_SERVICES *FamilySpecificServices = NULL;

  GetCpuServicesOfCurrentCore (&FamilySpecificServices, StdHeader);
  LibAmdMsrRead (MSR_COFVID_STS, &MsrRegister, StdHeader);
  MsrAddress = (UINT32) ((((COFVID_STS_MSR *) &MsrRegister)->StartupPstate) + PS_REG_BASE);
  LibAmdMsrRead (MsrAddress, &MsrRegister, StdHeader);
  LibAmdMsrWrite ((UINT32) (PS_REG_BASE + 1), &MsrRegister, StdHeader);
  ((PSTATE_MSR *) &MsrRegister)->NbVid = *(UINT8 *) NewNbVid;
  LibAmdMsrWrite (PS_REG_BASE, &MsrRegister, StdHeader);
  FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) 1, (BOOLEAN) FALSE, StdHeader);
}
Exemplo n.º 4
0
/**
 *---------------------------------------------------------------------------------------
 *
 *  PutCoreInPState0
 *
 *  Description:
 *    This function will take the CPU core into P0
 *
 *  Parameters:
 *    @param[in]         *PStateBuffer
 *    @param[in]         *StdHeader
 *
 *    @retval         VOID
 *
 *---------------------------------------------------------------------------------------
 **/
VOID
STATIC
PutCoreInPState0 (
  IN       VOID   *PStateBuffer,
  IN       AMD_CONFIG_PARAMS *StdHeader
  )
{
  CPU_SPECIFIC_SERVICES   *FamilySpecificServices;
  PSTATE_LEVELING         *PStateBufferPtr;

  PStateBufferPtr = (PSTATE_LEVELING *) PStateBuffer;

  if ((PStateBufferPtr[0].SetPState0 == PSTATE_FLAG_1 ) ||
     (PStateBufferPtr[0].SetPState0 == PSTATE_FLAG_2)) {
    return;
  }

  GetCpuServicesOfCurrentCore (&FamilySpecificServices, StdHeader);

  FamilySpecificServices->TransitionPstate  (FamilySpecificServices, (UINT8) 0, (BOOLEAN) FALSE, StdHeader);
}
Exemplo n.º 5
0
/**
 * Warm reset support routine for F10PmNbCofVidInit.
 *
 * This function implements steps 8 & 9 on each core.
 *
 * @param[in]  FunctionData       Contains NewNbVid determined by core 0 in step
 *                                2, and NbVidUpdateAll.
 * @param[in]  StdHeader          Config handle for library and services.
 *
 */
VOID
STATIC
PmNbCofVidInitWarmCore (
  IN       VOID *FunctionData,
  IN       AMD_CONFIG_PARAMS *StdHeader
  )
{
  UINT32 MsrAddress;
  UINT64 MsrRegister;
  CPU_SPECIFIC_SERVICES *FamilySpecificServices;

  GetCpuServicesOfCurrentCore (&FamilySpecificServices, StdHeader);
  for (MsrAddress = PS_REG_BASE; MsrAddress <= PS_MAX_REG; MsrAddress++) {
    LibAmdMsrRead (MsrAddress, &MsrRegister, StdHeader);
    if (((PSTATE_MSR *) &MsrRegister)->PsEnable == 1) {
      if ((((PSTATE_MSR *) &MsrRegister)->NbDid == 0) || ((NB_COF_VID_INIT_WARM *) FunctionData)->NbVidUpdateAll) {
        ((PSTATE_MSR *) &MsrRegister)->NbVid = ((NB_COF_VID_INIT_WARM *) FunctionData)->NewNbVid;
        LibAmdMsrWrite (MsrAddress, &MsrRegister, StdHeader);
      }
    }
  }
  LibAmdMsrRead (MSR_COFVID_STS, &MsrRegister, StdHeader);
  FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) (((COFVID_STS_MSR *) &MsrRegister)->StartupPstate), (BOOLEAN) FALSE, StdHeader);
}
Exemplo n.º 6
0
/**
 * Support routine for F10PmAfterReset to perform MSR initialization on all
 * cores of a family 10h socket.
 *
 * This function implements steps 2 - 24 on each core.
 *
 * @param[in]  StdHeader          Config handle for library and services.
 *
 */
VOID
STATIC
F10PmAfterResetCore (
  IN       AMD_CONFIG_PARAMS *StdHeader
  )
{
  UINT32 Socket;
  UINT32 Module;
  UINT32 Ignored;
  UINT32 PsMaxVal;
  UINT32 LocalPciRegister;
  UINT64 LocalMsrRegister;
  UINT64 SavedMsr;
  UINT64 CurrentLimitMsr;
  PCI_ADDR PciAddress;
  GO_TO_STEP GoToStep;
  AGESA_STATUS IgnoredSts;
  CPU_LOGICAL_ID LogicalId;
  CPU_SPECIFIC_SERVICES *FamilySpecificServices;

  // Step 2 If MSR C001_0071[CurNbDid] = 0, set MSR C001_001F[GfxNbPstateDis]
  GetLogicalIdOfCurrentCore (&LogicalId, StdHeader);
  GetCpuServicesFromLogicalId (&LogicalId, &FamilySpecificServices, StdHeader);
  if ((LogicalId.Revision & (AMD_F10_C3 | AMD_F10_DA_C2)) != 0) {
    LibAmdMsrRead (MSR_COFVID_STS, &LocalMsrRegister, StdHeader);
    if (((COFVID_STS_MSR *) &LocalMsrRegister)->CurNbDid == 0) {
      LibAmdMsrRead (NB_CFG, &LocalMsrRegister, StdHeader);
      LocalMsrRegister |= BIT62;
      LibAmdMsrWrite (NB_CFG, &LocalMsrRegister, StdHeader);
    }
  }

  GoToStep = EXIT_SEQUENCE;

  LibAmdMsrRead (MSR_PSTATE_CURRENT_LIMIT, &CurrentLimitMsr, StdHeader);
  PsMaxVal = (UINT32) (((PSTATE_CURLIM_MSR *) &CurrentLimitMsr)->PstateMaxVal);

  // Step 3 If MSRC001_0071[CurPstate] != F3xDC[PstateMaxVal], go to step 20
  LibAmdMsrRead (MSR_COFVID_STS, &LocalMsrRegister, StdHeader);
  if (((COFVID_STS_MSR *) &LocalMsrRegister)->CurPstate !=
      ((PSTATE_CURLIM_MSR *) &CurrentLimitMsr)->PstateMaxVal) {
    GoToStep = STEP20;
  } else {
    // Step 4 If F3xDC[PstateMaxVal] = 0 || F3xDC[PstateMaxVal] != 4, go to step 7
    if ((PsMaxVal == 0) || (PsMaxVal != 4)) {
      GoToStep = STEP7;
    } else {
      // Step 5 If MSRC001_0061[CurPstateLimit] <= F3xDC[PstateMaxVal]-1, go to step 17
      if (((PSTATE_CURLIM_MSR *) &CurrentLimitMsr)->CurPstateLimit <=
          (((PSTATE_CURLIM_MSR *) &CurrentLimitMsr)->PstateMaxVal - 1)) {
        GoToStep = STEP17;
      }
    }
  }
  switch (GoToStep) {
  default:
  case EXIT_SEQUENCE:
    // Step 6 Exit the sequence
    break;
  case STEP7:
    // Workaround for S3 ----Save the value of [The PState[4:0] Registers] MSRC001_00[68:64]
    //                      pointed to by F3xDC[PstateMaxVal] + 1
    LibAmdMsrRead ((MSR_PSTATE_0 + (PsMaxVal + 1)), &SavedMsr, StdHeader);

    // Step 7 Copy the P-state register pointed to by F3xDC[PstateMaxVal] to the P-state
    //        register pointed to by F3xDC[PstateMaxVal]+1
    LibAmdMsrRead ((MSR_PSTATE_0 + PsMaxVal), &LocalMsrRegister, StdHeader);
    LibAmdMsrWrite ((MSR_PSTATE_0 + (PsMaxVal + 1)), &LocalMsrRegister, StdHeader);

    // Step 8 Write F3xDC[PstateMaxVal]+1 to F3xDC[PstateMaxVal]
    IdentifyCore (StdHeader, &Socket, &Module, &Ignored, &IgnoredSts);
    GetPciAddress (StdHeader, Socket, Module, &PciAddress, &IgnoredSts);
    PciAddress.Address.Function = FUNC_3;
    PciAddress.Address.Register = CPTC2_REG;
    LibAmdPciRead (AccessWidth32, PciAddress, &LocalPciRegister, StdHeader);
    ((CLK_PWR_TIMING_CTRL2_REGISTER *) &LocalPciRegister)->PstateMaxVal = PsMaxVal + 1;
    LibAmdPciWrite (AccessWidth32, PciAddress, &LocalPciRegister, StdHeader);

    // Step 9 Write (the new) F3xDC[PstateMaxVal] to MSRC001_0062[PstateCmd]
    FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) (PsMaxVal + 1), (BOOLEAN) FALSE, StdHeader);

    // Step 10 Wait for MSRC001_0071[CurCpuFid/CurCpuDid] = CpuFid/CpuDid from the P-state
    //         register pointed to by (the new) F3xDC[PstateMaxVal]
    WaitForCpuFidAndDidToMatch ((UINT32) (PsMaxVal + 1), StdHeader);

    // Step 11 Copy (the new) F3xDC[PstateMaxVal]-1 to MSRC001_0062[PstateCmd]
    FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) PsMaxVal, (BOOLEAN) FALSE, StdHeader);

    // Step 12 Wait for MSRC001_0071[CurCpuFid/CurCpuDid] = CpuFid/CpuDid from the P-state
    //         register pointed to by (the new) F3xDC[PstateMaxVal]-1
    WaitForCpuFidAndDidToMatch (PsMaxVal, StdHeader);

    // Step 13 If MSRC001_0071[CurNbDid] = 1, set MSRC001_001F[GfxNbPstateDis]
    if ((LogicalId.Revision & (AMD_F10_C3 | AMD_F10_DA_C2)) != 0) {
      LibAmdMsrRead (MSR_COFVID_STS, &LocalMsrRegister, StdHeader);
      if (((COFVID_STS_MSR *) &LocalMsrRegister)->CurNbDid == 1) {
        LibAmdMsrRead (NB_CFG, &LocalMsrRegister, StdHeader);
        LocalMsrRegister |= BIT62;
        LibAmdMsrWrite (NB_CFG, &LocalMsrRegister, StdHeader);
      }
    }

    // Step 14 If required, transition the NB COF and VID to the NbDid and NbVid from the
    //         P-state register pointed to by MSRC001_0061[CurPstateLimit] using the NB COF
    //         and VID transition sequence after a warm reset

    // Step 15 Write 0 to PstateEn of the P-state register pointed to by (the new) F3xDC[PstateMaxVal]
    // Workaround for S3----Restore the value of [The PState[4:0] Registers] MSRC001_00[68:64]
    //                   pointed to by F3xDC[PstateMaxVal] + 1
    ((PSTATE_MSR *) &SavedMsr)->PsEnable = 0;
    LibAmdMsrWrite ((MSR_PSTATE_0 + (PsMaxVal + 1)), &SavedMsr, StdHeader);

    // Step 16 Write (the new) F3xDC[PstateMaxVal]-1 to F3xDC[PstateMaxVal]
    LibAmdPciRead (AccessWidth32, PciAddress, &LocalPciRegister, StdHeader);
    ((CLK_PWR_TIMING_CTRL2_REGISTER *) &LocalPciRegister)->PstateMaxVal = PsMaxVal;
    LibAmdPciWrite (AccessWidth32, PciAddress, &LocalPciRegister, StdHeader);
    break;
  case STEP17:
    // Step 17 Copy F3xDC[PstateMaxVal]-1 to MSRC001_0062[PstateCmd]
    FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) (PsMaxVal - 1), (BOOLEAN) FALSE, StdHeader);

    // Step 18 Wait for MSRC001_0071[CurCpuFid/CurCpuDid] = CpuFid/CpuDid from the P-state
    //         register pointed to by F3xDC[PstateMaxVal]-1
    WaitForCpuFidAndDidToMatch ((UINT32) (PsMaxVal - 1), StdHeader);

    // Step 19 If MSR C001_0071[CurNbDid] = 0, set MSR C001_001F[GfxNbPstateDis]
    if ((LogicalId.Revision & (AMD_F10_C3 | AMD_F10_DA_C2)) != 0) {
      LibAmdMsrRead (MSR_COFVID_STS, &LocalMsrRegister, StdHeader);
      if (((COFVID_STS_MSR *) &LocalMsrRegister)->CurNbDid == 0) {
        LibAmdMsrRead (NB_CFG, &LocalMsrRegister, StdHeader);
        LocalMsrRegister |= BIT62;
        LibAmdMsrWrite (NB_CFG, &LocalMsrRegister, StdHeader);
      }
    }

    // Fall through from step 19 to step 20
  case STEP20:
    // Step 20 Copy F3xDC[PstateMaxVal] to MSRC001_0062[PstateCmd]
    FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) PsMaxVal, (BOOLEAN) FALSE, StdHeader);

    // Step 21 Wait for MSRC001_0071[CurCpuFid/CurCpuDid] = CpuFid/CpuDid from the P-state
    //         register pointed to by F3xDC[PstateMaxVal]
    WaitForCpuFidAndDidToMatch (PsMaxVal, StdHeader);

    // Step 22 If MSR C001_0071[CurNbDid] = 1, set MSR C001_001F[GfxNbPstateDis] and exit
    //         the sequence
    if ((LogicalId.Revision & (AMD_F10_C3 | AMD_F10_DA_C2)) != 0) {
      LibAmdMsrRead (MSR_COFVID_STS, &LocalMsrRegister, StdHeader);
      if (((COFVID_STS_MSR *) &LocalMsrRegister)->CurNbDid == 1) {
        LibAmdMsrRead (NB_CFG, &LocalMsrRegister, StdHeader);
        LocalMsrRegister |= BIT62;
        LibAmdMsrWrite (NB_CFG, &LocalMsrRegister, StdHeader);
        break;
      }
    }

    // Step 23 Issue an LDTSTOP and exit the sequence

    // Step 24 If required, transition the NB COF and VID to the NbDid and NbVid from the
    //         P-state register pointed to by F3xDC[PstateMaxVal] using the NB COF and VID
    //         transition sequence after a warm reset
    break;
  }
}
Exemplo n.º 7
0
/**
 * Core-level error handler called if any p-states were determined to be out
 * of range for the mother board.
 *
 * This function implements steps 2a-c and 3a-c on each core.
 *
 * @param[in]  ErrorData            Details about the error condition.
 * @param[in]  StdHeader            Config handle for library and services.
 *
 */
VOID
STATIC
F10PmPwrCheckCore (
  IN       VOID *ErrorData,
  IN       AMD_CONFIG_PARAMS *StdHeader
  )
{
  UINT8  i;
  UINT8  PsMaxVal;
  UINT8  DisPsNum;
  UINT8  CurrentPs;
  UINT8  EnBsNum;
  UINT64 LocalMsrRegister;
  CPU_SPECIFIC_SERVICES *FamilySpecificServices;

  GetCpuServicesOfCurrentCore (&FamilySpecificServices, StdHeader);
  PsMaxVal = (((PWRCHK_ERROR_DATA *) ErrorData)->HwPstateNumber - 1);
  DisPsNum = (((PWRCHK_ERROR_DATA *) ErrorData)->HwPstateNumber -
             ((PWRCHK_ERROR_DATA *) ErrorData)->AllowablePstateNumber);
  EnBsNum = ((PWRCHK_ERROR_DATA *) ErrorData)->NumberofBoostStates;

  LibAmdMsrRead (MSR_PSTATE_STS, &LocalMsrRegister, StdHeader);
  CurrentPs = (UINT8) (((PSTATE_STS_MSR *) &LocalMsrRegister)->CurPstate);

  if (((PWRCHK_ERROR_DATA *) ErrorData)->AllowablePstateNumber == 0) {

    // Step 1
    // Transition to Pstate Max if not there already

    if ((CurrentPs + EnBsNum) != PsMaxVal) {
      FamilySpecificServices->TransitionPstate (FamilySpecificServices, (PsMaxVal - EnBsNum), (BOOLEAN) TRUE, StdHeader);
    }


    // Step 2
    // If Pstate Max is not 000b, copy Pstate max contents to P0 and switch
    // to P0. This step uses software P-state numbering

    if (PsMaxVal != 0) {
      F10PmPwrChkCopyPstate (EnBsNum, PsMaxVal, StdHeader);
      FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) 0, (BOOLEAN) TRUE, StdHeader);
    }
  } else {

    // move remaining P-state register(s) up
    // Step 1
    // Transition to a valid Pstate if current Pstate has been disabled

    if ((CurrentPs + EnBsNum) < DisPsNum) {
      FamilySpecificServices->TransitionPstate (FamilySpecificServices, (DisPsNum - EnBsNum), (BOOLEAN) TRUE, StdHeader);
      CurrentPs = DisPsNum - EnBsNum;
    }

    // Step 2
    // Move enabled Pstates up and disable the remainder. This step uses software P-state numbering.
    if (DisPsNum > EnBsNum) {
      for (i = 0; (i + DisPsNum) <= PsMaxVal; ++i) {
        F10PmPwrChkCopyPstate ((i + EnBsNum), (i + DisPsNum), StdHeader);
      }
    }
    // Step 3
    // Transition to current COF/VID at shifted location

    CurrentPs = ((CurrentPs + EnBsNum) - DisPsNum);
    FamilySpecificServices->TransitionPstate (FamilySpecificServices, CurrentPs, (BOOLEAN) TRUE, StdHeader);
  }
  i = ((PWRCHK_ERROR_DATA *) ErrorData)->AllowablePstateNumber;
  if (i == 0) {
    ++i;
  }
  while (i <= PsMaxVal) {
    FamilySpecificServices->DisablePstate (FamilySpecificServices, i, StdHeader);
    ++i;
  }
}
Exemplo n.º 8
0
/**
 * Core-level error handler called if any p-states were determined to be out
 * of range for the mother board.
 *
 * This function implements steps 2a-c and 3a-c on each core.
 *
 * @param[in]  ErrorData            Details about the error condition.
 * @param[in]  StdHeader            Config handle for library and services.
 *
 */
VOID
STATIC
F15PmPwrCheckCore (
  IN       VOID *ErrorData,
  IN       AMD_CONFIG_PARAMS *StdHeader
  )
{
  UINT8  i;
  UINT8  HwPsMaxVal;
  UINT8  SwPsMaxVal;
  UINT8  HwDisPsNum;
  UINT8  CurrentSwPs;
  UINT8  PsDisableCount;
  UINT64 LocalMsrRegister;
  CPU_SPECIFIC_SERVICES *FamilySpecificServices;

  if (IsCoreComputeUnitPrimary (FirstCoreIsComputeUnitPrimary, StdHeader)) {
    // P-state MSRs are shared, so only 1 core per compute unit needs to perform this
    GetCpuServicesOfCurrentCore (&FamilySpecificServices, StdHeader);
    HwPsMaxVal = (((PWRCHK_ERROR_DATA *) ErrorData)->HwPstateNumber - 1);
    HwDisPsNum = (((PWRCHK_ERROR_DATA *) ErrorData)->HwPstateNumber -
                  ((PWRCHK_ERROR_DATA *) ErrorData)->AllowablePstateNumber);

    LibAmdMsrRead (MSR_PSTATE_STS, &LocalMsrRegister, StdHeader);
    CurrentSwPs = (UINT8) (((PSTATE_STS_MSR *) &LocalMsrRegister)->CurPstate);
    LibAmdMsrRead (MSR_PSTATE_CURRENT_LIMIT, &LocalMsrRegister, StdHeader);
    SwPsMaxVal = (UINT8) (((PSTATE_CURLIM_MSR *) &LocalMsrRegister)->PstateMaxVal);
    PsDisableCount = 0;

    if (((PWRCHK_ERROR_DATA *) ErrorData)->AllowablePstateNumber == 0) {
      // All P-States are over the limit.

      // Step 1
      // Transition to Pstate Max if not there already
      if (CurrentSwPs != SwPsMaxVal) {
        FamilySpecificServices->TransitionPstate (FamilySpecificServices, SwPsMaxVal, (BOOLEAN) TRUE, StdHeader);
      }

      // Step 2
      // If Pstate Max is not P0, copy Pstate max contents to P0 and switch
      // to P0.
      if (SwPsMaxVal != 0) {
        F15PmPwrChkCopyPstate (((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfBoostStates, HwPsMaxVal, StdHeader);
        FamilySpecificServices->TransitionPstate (FamilySpecificServices, (UINT8) 0, (BOOLEAN) TRUE, StdHeader);
      }

      // Disable all SW P-states except P0
      PsDisableCount = ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled - 1;
    } else {
      // At least one P-State is under the limit & at least one P-State is
      // over the limit.
      if (((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfBoostStates > HwDisPsNum) {
        // A subset of boosted P-states are disabled.  Copy the contents of the
        // highest performance boosted P-state still enabled to the boosted
        // P-states that have been disabled.
        for (i = 0; i < HwDisPsNum; i++) {
          F15PmPwrChkCopyPstate (i, HwDisPsNum, StdHeader);
        }
      } else if (((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled != 0) {
        // Move remaining P-state register(s) up
        // Step 1
        // Transition to a valid Pstate if current Pstate has been disabled
        if (CurrentSwPs < ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled) {
          FamilySpecificServices->TransitionPstate (FamilySpecificServices, ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled, (BOOLEAN) TRUE, StdHeader);
          CurrentSwPs = ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled;
        }

        // Step 2
        // Move enabled Pstates up and disable the remainder
        for (i = ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfBoostStates; (i + ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled) <= HwPsMaxVal; i++) {
          F15PmPwrChkCopyPstate (i, (i + ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled), StdHeader);
        }

        // Step 3
        // Transition to current COF/VID at shifted location
        CurrentSwPs = (CurrentSwPs - ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled);
        FamilySpecificServices->TransitionPstate (FamilySpecificServices, CurrentSwPs, (BOOLEAN) TRUE, StdHeader);

        // Disable the appropriate number of P-states
        PsDisableCount = ((PWRCHK_ERROR_DATA *) ErrorData)->NumberOfSwPstatesDisabled;
      }
    }
    // Disable the appropriate P-states if any, starting from HW Pmin
    for (i = 0; i < PsDisableCount; i++) {
      FamilySpecificServices->DisablePstate (FamilySpecificServices, (HwPsMaxVal - i), StdHeader);
    }
  }
}