GitBucket
Newer
Ajax Dong
/** @file
PpmInitialize -- Processor Power Management (PPM) DXE driver for Purley platform.
This module initializes processor power management features. It consumes
the PPM Info HOB produced by earlier PEIMs, locates the MP service protocol,
and programs MSRs per socket for ConfigTDP, PBF, C-states, Turbo, and
energy-efficient features. It also registers SmmReadyToLock and ReadyToBoot
callbacks for late-stage configuration and boot script save/restore for S3.
Source: PurleySktPkg/Dxe/PowerManagement/PpmInitialize.c
Build: HR6N0XMLK, DEBUG_VS2015, X64
Binary: PpmInitialize.efi (MD5: 950c3fca6b051772d650310aa2eb495c)
Copyright (c) 2017, Intel Corporation. All rights reserved.
SPDX-License-Identifier: BSD-2-Clause-Patent
**/
#include "PpmInitialize.h"
//
// Global data (gImageHandle, gST, gBS, gRT) are declared in
// UefiBootServicesTableLib and UefiRuntimeServicesTableLib headers.
//
// Module-specific globals:
//
STATIC EFI_MP_SERVICES_PROTOCOL *mMpService = NULL;
STATIC EFI_SMM_COMMUNICATION_PROTOCOL *mSmmComm = NULL;
STATIC VOID *mPcd = NULL;
STATIC VOID *mPpmContext = NULL;
STATIC VOID *mPpmRegAddr = NULL;
STATIC VOID *mDebugProtocol = NULL;
STATIC VOID *mHobList = NULL;
STATIC VOID *mPpmInfoTable = NULL;
STATIC UINT8 mS3BootScriptInited = FALSE;
STATIC UINT8 mS3LockBoxInited = FALSE;
STATIC VOID *mEventDxeSmmReadyToLock = NULL;
STATIC VOID *mEventReadyToBoot = NULL;
STATIC UINT64 mS3BootScriptTable = 0;
STATIC VOID *mCpuConfigLibConfigContextBuffer = NULL;
STATIC EFI_EVENT mEfiSmmReadyToLockEvent;
STATIC EFI_EVENT mEfiSmmLockedEvent;
STATIC VOID *mSmmReadyToLockRegistration;
//=============================================================================
// Module Entry Point
//=============================================================================
/**
Module entry point.
@param[in] ImageHandle Handle for this image.
@param[in] SystemTable Pointer to EFI System Table.
@retval EFI_SUCCESS Initialization completed.
@retval Others An error occurred.
**/
EFI_STATUS
EFIAPI
PpmInitializeEntryPoint (
IN EFI_HANDLE ImageHandle,
IN EFI_SYSTEM_TABLE *SystemTable
)
{
RETURN_STATUS Status;
gImageHandle = ImageHandle;
ASSERT (gImageHandle != NULL);
gSystemTable = SystemTable;
ASSERT (SystemTable != NULL);
gBootServices = SystemTable->BootServices;
ASSERT (gBootServices != NULL);
gRuntimeServices = SystemTable->RuntimeServices;
ASSERT (gRuntimeServices != NULL);
(VOID)GetPcdProtocol ();
(VOID)GetHobList ();
mPpmRegAddr = (VOID *)(UINTN)GetPcdProtocol ()->GetPcd (PCD_TOKEN_CPU_PM_STRUCT);
InternalGetRegAddr ();
//
// Check PPM enable flag
//
if ((INT8)*((UINT8 *)GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_TDP)) >= 0) {
PciWrite16 ((UINTN)PciRead32 (PCD_TOKEN_PPM_INFO) & ~0xFFFFFFF, 0x500);
*((UINT8 *)GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_TDP)) |= 0x80;
}
Status = PpmInitialize ();
return Status;
}
//=============================================================================
// Main PPM Initialization
//=============================================================================
/**
Main PPM initialization routine.
@retval EFI_SUCCESS Initialization completed.
@retval Others An error occurred.
**/
EFI_STATUS
PpmInitialize (
VOID
)
{
RETURN_STATUS Status;
mMpService = (EFI_MP_SERVICES_PROTOCOL *)(UINTN)1;
//
// Initialize PPM Info structures (PpmInfo.c equivalent)
//
PpmInfoInit ();
//
// Call PPM MSR config on all APs, then BSP
//
(VOID)mMpService->StartupAllAPs (
mMpService,
(EFI_AP_PROCEDURE)PpmMsrConfig,
FALSE,
NULL,
0,
NULL,
NULL
);
(VOID)mMpService->StartupAllAPs (
mMpService,
(EFI_AP_PROCEDURE)PpmIdleInit,
FALSE,
NULL,
0,
NULL,
NULL
);
PpmMsrConfig ();
PpmIdleInit (NULL);
//
// Register SmmReadyToLock event
//
Status = gBootServices->CreateEvent (
EVT_NOTIFY_SIGNAL,
TPL_CALLBACK,
SmmReadyToLockCallback,
NULL,
&mEfiSmmReadyToLockEvent
);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
Status = gBootServices->RegisterProtocolNotify (
&gEfiEventDxeSmmReadyToLockGuid,
mEfiSmmReadyToLockEvent,
&mSmmReadyToLockRegistration
);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
mCpuConfigLibConfigContextBuffer = GetPcdProtocol ()->GetPcd (PCD_TOKEN_PM_MODE);
mPpmContext = GetPcdProtocol ()->GetPcd (PCD_TOKEN_CPU_PM_STRUCT);
return EFI_SUCCESS;
}
//=============================================================================
// PPM Info Initialization
//=============================================================================
/**
Initialize PPM Info per socket: CPUID, MSR_PLATFORM_INFO, PPM Info HOB.
Original equivalent: PpmInfo.c
**/
VOID
PpmInfoInit (
VOID
)
{
UINT8 SocketIndex;
UINT32 CpuidEax;
UINT32 CpuidEbx;
GUID *GuidHob;
VOID *Hob;
UINT32 CoreBitmap;
UINT32 PpmInfoFlags;
UINT32 PlatformId;
UINT32 CoreCount;
UINT8 LimitCores;
UINTN HobList;
UINT32 MsrPlatformInfo;
UINT32 MsrPlatformInfoHi;
UINT32 MsrPpmConfig;
VOID *PpmInfoData;
VOID *PpmInfoHob;
UINT32 *PpmInfoTableBuf;
UINT32 TableData[4];
INT8 *MpServiceProtocol;
EFI_MP_SERVICES_PROTOCOL *MpService;
UINTN NumberOfProcessors;
UINTN NumberOfEnabledProcessors;
UINTN BspIndex;
UINTN ProcessorIndex;
EFI_STATUS Status;
SocketIndex = 0;
TableData[0] = 0x7FF82421;
TableData[1] = 0x431FEE7D;
TableData[2] = 0xCA2A3C3A;
TableData[3] = 0xC0B42E52;
//
// Read CPUID
//
CpuidEx (CPUID_VERSION_INFO, 0, &CpuidEax, NULL, NULL, NULL);
CpuidEx (CPUID_SIGNATURE, 0, NULL, NULL, NULL, &PlatformId);
//
// Locate MP Service protocol
//
Status = gBootServices->LocateProtocol (
&gEfiMpServiceProtocolGuid,
NULL,
(VOID **)&mMpService
);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
Status = GetPpmInfoFromHob ();
ASSERT_EFI_ERROR (Status);
//
// Locate PPM Info HOB
//
GuidHob = (GUID *)TableData;
Hob = GetHobList ();
if (Hob != NULL) {
HobList = (UINTN)Hob;
while (*((UINT16 *)HobList) != 0xFFFF) {
if (*((UINT16 *)HobList) == 4) {
if (CompareGuid ((GUID *)(HobList + 8), GuidHob)) {
mPpmInfoTable = (VOID *)(*((UINT64 *)(HobList + 24)));
break;
}
}
HobList += *((UINT16 *)(HobList + 2));
}
}
ASSERT (mPpmInfoTable != NULL);
//
// Allocate per-socket buffer
//
Status = gBootServices->AllocatePool (
EfiBootServicesData,
92,
(VOID **)&PpmInfoTableBuf
);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
ZeroMemSafe (PpmInfoTableBuf, 92);
//
// Locate SMM Communication protocol
//
Status = gBootServices->LocateProtocol (
&gEfiSmmCommunicationProtocolGuid,
NULL,
(VOID **)&mSmmComm
);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
mMpService->GetNumberOfProcessors (
mMpService,
&NumberOfProcessors,
&NumberOfEnabledProcessors
);
mMpService->WhoAmI (mMpService, &BspIndex);
PpmInfoData = (UINT8 *)GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_INFO);
ASSERT (PpmInfoData != NULL);
PpmInfoTableBuf[2] = *(UINT32 *)((UINTN)mPpmInfoTable + 1750);
for (SocketIndex = 0; SocketIndex < 4; SocketIndex++) {
UINT8 CoreMask;
UINTN HobPtr;
UINT8 ThreadIndex;
if (!(PpmInfoTableBuf[2] & (1 << SocketIndex))) {
continue;
}
*((UINT8 *)PpmInfoTableBuf + 1) = SocketIndex;
HobPtr = (UINTN)mPpmInfoTable;
CoreCount = *(UINT32 *)(HobPtr + SocketIndex * 4 + 1750);
MsrPlatformInfo = (UINT32)AsmReadMsr64 (MSR_PLATFORM_INFO);
*(UINT32 *)((UINTN)mPpmInfoTable + 1773) = (UINT16)MsrPlatformInfo;
if ((MsrPlatformInfo >> 16) != (UINT16)MsrPlatformInfo) {
*(UINT32 *)((UINTN)mPpmInfoTable + 1773) |= 0x20000;
}
Status = mMpService->WhoAmI (mMpService, &BspIndex);
if ((*(UINT32 *)&CpuidEbx & 0x2000000) == 0) {
*(UINT32 *)((UINTN)mPpmInfoTable + 1773) |= 0x10000;
}
mMpService->WhoAmI (mMpService, &BspIndex);
Status = mMpService->WhoAmI (mMpService, &BspIndex);
*(UINTN *)(PpmInfoTableBuf + 81) = BspIndex;
PpmInfoData = GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_INFO);
ASSERT (PpmInfoData != 0);
mPpmRegAddr = (VOID *)(UINTN)PpmInfoData;
mPpmContext = (VOID *)(UINTN)PpmInfoData;
mPpmRegAddr = (VOID *)(UINTN)mMpService;
*((UINT8 *)PpmInfoTableBuf + 89) = (BOOLEAN)(GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_TDP) < 0);
*(UINT32 *)((UINTN)PpmInfoTableBuf + 68) = *(UINT32 *)&CpuidEbx;
if (!*(UINT8 *)((UINTN)mPpmContext + 1)) {
*(UINT8 *)((UINTN)mPpmContext + 1) = (UINT8)(AsmReadMsr64 (MSR_PPM_CONFIG) >> 16);
}
if (*(UINT32 *)((UINTN)PpmInfoTableBuf + 72) == 0x5065 ||
*(UINT32 *)((UINTN)PpmInfoTableBuf + 72) == 0x50655) {
*((UINT8 *)PpmInfoTableBuf + 91) = 3;
for (LimitCores = 0; LimitCores < 4; LimitCores++) {
if (*(UINT32 *)((UINTN)PpmInfoTableBuf + 2) & (1 << LimitCores)) {
UINT8 FeatureControl;
*((UINT8 *)PpmInfoTableBuf + 1) = LimitCores;
FeatureControl = (UINT8)((mMpService->WhoAmI (
mMpService,
&ProcessorIndex
) >> 25) & 3);
*((UINT8 *)PpmInfoTableBuf + 91) &= FeatureControl;
}
}
}
}
}
//=============================================================================
// PPM Configure TDP
//=============================================================================
/**
ConfigTDP level configuration per socket.
**/
CHAR8 *
PpmConfigTdp (
VOID
)
{
UINT8 SocketIndex;
UINT8 ConfigTdpLevel;
UINT32 PkgTdp;
UINT32 ProcessorPowerUnit;
UINT32 TdpRatio;
UINT16 MsrValue;
UINT16 MsrValue2;
for (SocketIndex = 0; SocketIndex < 4; SocketIndex++) {
if (!(*(UINT32 *)((UINTN)GetPpmInfoTable () + 2) & (1 << SocketIndex))) {
continue;
}
*((UINT8 *)GetPpmInfoTable () + 1) = SocketIndex;
ConfigTdpLevel = *(UINT8 *)((UINTN)mPpmContext + SocketIndex + 138);
if (SocketIndex == 0) {
UINT16 MaxRatio = 256;
}
PkgTdp = mMpService->WhoAmI (mMpService, &SocketIndex);
if (ConfigTdpLevel == 1) {
MsrValue = mMpService->WhoAmI (mMpService, &SocketIndex);
} else if (ConfigTdpLevel == 2) {
MsrValue2 = mMpService->WhoAmI (mMpService, &SocketIndex);
MsrValue = 0x7FFF;
goto CalculateTdp;
}
MsrValue2 = 0x7FFF;
CalculateTdp:
ProcessorPowerUnit = (PkgTdp & 0xF);
if (ProcessorPowerUnit != 0) {
TdpRatio = (PkgTdp & 0xF) - 1;
} else {
TdpRatio = 1;
}
if ((PkgTdp & 0xF0000) != 0) {
ProcessorPowerUnit = (BYTE)(PkgTdp >> 16) & 0xF;
if (ProcessorPowerUnit != 0) {
TdpRatio = (ProcessorPowerUnit - 1);
}
}
*((UINT8 *)GetPpmInfoTable () + SocketIndex + 52) = (UINT8)TdpRatio;
*((UINT8 *)GetPpmInfoTable () + SocketIndex + 56) = (UINT8)ProcessorPowerUnit;
if (*(UINT8 *)((UINTN)mPpmContext + SocketIndex + 138) <= 2) {
*(UINT16 *)((UINTN)GetPpmInfoTable () + 2 * SocketIndex + 60) =
(UINT16)((MsrValue2 & MsrValue) / TdpRatio);
} else {
*(UINT16 *)((UINTN)GetPpmInfoTable () + 2 * SocketIndex + 60) =
*(UINT16 *)((UINTN)mPpmContext + 2 * SocketIndex + 142);
}
DEBUG ((
EFI_D_INFO,
":PPM: ConfigTdpLevel: %d; pkg_tdp: %d; ProcessorPowerUnit: %d, ppm->Info->PackageTdp[%d]: %d\n",
ConfigTdpLevel,
MsrValue2 & MsrValue,
TdpRatio,
SocketIndex,
*(UINT16 *)((UINTN)GetPpmInfoTable () + 2 * SocketIndex + 60)
));
}
return NULL;
}
//=============================================================================
// PPM MSR Configuration (per socket, per thread)
//=============================================================================
/**
Read and apply PPM MSR configuration per socket.
Called on BSP and all APs.
**/
CHAR8 *
PpmMsrConfig (
VOID
)
{
UINT64 BspCpuIndex;
UINT64 MsrValue;
UINT64 MsrValue2;
UINT32 EaxReg;
UINT32 EbxReg;
UINT32 EcxReg;
UINT32 EdxReg;
UINT32 MsrPpmCurrentConfig;
//
// Get BSP index
//
BspCpuIndex = 0;
mMpService->WhoAmI (mMpService, &BspCpuIndex);
BspCpuIndex = 0;
mMpService->WhoAmI (mMpService, &BspCpuIndex);
//
// MSR_PPM_CURRENT_CONFIG (0x1FC)
//
MsrValue = AsmReadMsr64 (MSR_PPM_CURRENT_CONFIG);
MsrPpmCurrentConfig = *(UINT32 *)((UINTN)mPpmContext + 8) & 0xF7FFFFFB;
MsrPpmCurrentConfig |= (UINT32)(MsrValue & 0x941FFFF8);
if ((*(UINT32 *)(GetPpmInfoTable () + 68) & 0x1000000) != 0) {
MsrPpmCurrentConfig |= 0x400000;
}
AsmWriteMsr64 (MSR_PPM_CURRENT_CONFIG, MsrPpmCurrentConfig);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PPM_CURRENT_CONFIG, MsrPpmCurrentConfig, 508, 0, 64, MsrPpmCurrentConfig);
//
// MSR_PM_PBF_MISC_CTL (0x603)
//
MsrValue = AsmReadMsr64 (MSR_PM_PBF_MISC_CTL);
EbxReg = (UINT32)(MsrValue >> 32) & 0xFFEFFFFF;
if ((*(UINT32 *)((UINTN)mPpmContext + 48) & 0x40000000) == 0) {
MsrValue = (MsrValue & 0xFFFFFFFFFFEFFFFFULL) | ((UINT64)EbxReg << 32);
} else {
MsrValue = (MsrValue & 0xFFFFFFFFFFEFFFFFULL) | ((UINT64)(EbxReg | 0x100000) << 32);
}
AsmWriteMsr64 (MSR_PM_PBF_MISC_CTL, MsrValue);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PM_PBF_MISC_CTL, MsrValue, 1539, 0, 64, MsrValue);
//
// MSR_PM_PBF_CMD (0x601)
//
MsrValue = AsmReadMsr64 (MSR_PM_PBF_CMD);
if ((*(UINT8 *)((UINTN)mPpmContext + 4) & 0x20) != 0) {
UINT16 PkgCstLimit = *(UINT32 *)((UINTN)mPpmContext + 24) & 0x1FFF;
if ((*(UINT32 *)((UINTN)mPpmContext + 24) & 0x1FFF) != 0x1FFF) {
MsrValue = (MsrValue & 0xFFFFFFFFFFFE000ULL) | PkgCstLimit;
AsmWriteMsr64 (MSR_PM_PBF_CMD, MsrValue);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PM_PBF_CMD, MsrValue, 1537, 0, 64, MsrValue);
}
}
//
// PBF MSR programming for interrupt/ratio limits
//
PpmPbfMsrConfig ();
//
// MSR_PM_WAY_0 (0xA01) -- C-state way
//
MsrValue = AsmReadMsr64 (MSR_PM_WAY_0);
MsrValue = (MsrValue & 0xF80) | *(UINT32 *)((UINTN)mPpmContext + 32);
AsmWriteMsr64 (MSR_PM_WAY_0, MsrValue);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PM_WAY_0, MsrValue, 2561, 0, 64, MsrValue);
//
// PBF2 enable
//
if (*(UINT8 *)((UINTN)mPpmContext + 299)) {
MsrValue = AsmReadMsr64 (MSR_PM_PBF_MISC_CTL);
MsrValue |= ((UINT64)0xE7 << 32);
MsrValue = (MsrValue & 0xFFFFFFFFFFFCFFFFULL) | ((UINT64)1 << 48);
AsmWriteMsr64 (MSR_PM_PBF_MISC_CTL, MsrValue);
}
//
// Socket type specific: SKX (0x5065) and variants
//
if (*(UINT32 *)(GetPpmInfoTable () + 72) == 0x5065 ||
*(UINT32 *)(GetPpmInfoTable () + 72) == 0x50655) {
UINT8 PpmCoreCount = *(UINT8 *)((UINTN)mPpmContext + 297) & 0xF;
UINT8 ConfigTdpLevel = PpmCoreCount;
if (PpmCoreCount > *(UINT8 *)((UINTN)mPpmContext + 1)) {
ConfigTdpLevel = *(UINT8 *)((UINTN)mPpmContext + 1);
}
if ((AsmReadMsr64 (MSR_PLATFORM_INFO) & 0x40000000) != 0 && ConfigTdpLevel != 0) {
MsrValue = AsmReadMsr64 (MSR_PPM_CONFIG) & 0xFFFFFFFFFF0FFFFFFFULL;
MsrValue |= (UINT64)ConfigTdpLevel << 24;
AsmWriteMsr64 (MSR_PPM_CONFIG, MsrValue);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PPM_CONFIG, MsrValue, 418, 0, 64, MsrValue);
}
//
// MSR_PPM_HWP_LIMIT (0x1AA)
//
if ((*(UINT8 *)((UINTN)mPpmContext + 150) & 4) != 0) {
MsrValue = AsmReadMsr64 (MSR_PPM_HWP_LIMIT);
MsrValue |= 1;
AsmWriteMsr64 (MSR_PPM_HWP_LIMIT, MsrValue);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PPM_HWP_LIMIT, MsrValue, 426, 0, 64, MsrValue);
}
//
// Package power limit config
//
PpmPackagePowerLimitConfig ();
//
// Energy-efficient turbo for specific stepping
//
if (*(UINT32 *)(GetPpmInfoTable () + 72) == 0x50655 &&
(*(UINT8 *)(GetPpmInfoTable () + 91) & 1) != 0) {
PpmEnergyEfficientTurboConfig ();
}
}
//
// PBF core programming via MSR_PM_DISABLE (0x774)
//
if (*(UINT8 *)((UINTN)mPpmContext + 223)) {
if (*(UINT8 *)((UINTN)mPpmContext + 224)) {
UINT8 PbfCore;
UINT64 PbfCoreMap;
UINT64 MsrPmDisable;
CpuidEx (CPUID_EXTENDED_TOPOLOGY, 1, &EaxReg, NULL, NULL, &EdxReg);
PbfCore = (UINT8)(EdxReg >> (EaxReg & 0x1F));
MsrValue = AsmReadMsr64 (0x53);
PbfCoreMap = *(UINT64 *)((UINTN)mPpmContext + 8 * PbfCore + 225);
if ((PbfCoreMap >> ((UINT32)MsrValue >> 1)) & 1) {
MsrPmDisable = AsmReadMsr64 (MSR_PM_DISABLE);
MsrPmDisable = MsrPmDisable & 0xFFFFFFFFFFFFFF00ULL;
MsrPmDisable |= *(UINT8 *)((UINTN)mPpmContext + PbfCore + 257);
AsmWriteMsr64 (MSR_PM_DISABLE, MsrPmDisable);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PM_DISABLE, MsrPmDisable, 1908, 0, 64, MsrPmDisable);
if (BspCpuIndex == 0) {
DEBUG ((
EFI_D_INFO,
":PPM: MSR: 0x%x PbfHighPriCoreMap:PbfP1HighRatio:PbfP1LowRatio = 0x%lx : %d : %d\n",
(UINT32)MsrPmDisable,
PbfCoreMap,
*(UINT8 *)((UINTN)mPpmContext + PbfCore + 257),
*(UINT8 *)((UINTN)mPpmContext + PbfCore + 261)
));
}
}
}
}
return NULL;
}
//=============================================================================
// PPM Idle / C-State Initialization
//=============================================================================
/**
PPM idle/C-state initialization for a socket/thread.
@param[in] Context Pointer to PPM context structure.
**/
VOID
PpmIdleInit (
UINT64 *Context
)
{
UINT64 BspCpuIndex;
UINT64 MsrValue;
UINT64 MsrValueHi;
//
// Get BSP index
//
BspCpuIndex = 0;
mMpService->WhoAmI (mMpService, &BspCpuIndex);
//
// MSR_PKG_CST_CONFIG_CONTROL (0xE2)
//
MsrValue = AsmReadMsr64 (MSR_PKG_CST_CONFIG_CONTROL);
//
// Check C1E and C-state limits
//
if (*(UINT8 *)((UINTN)mPpmContext + 216) &&
(*(UINT8 *)((UINTN)Context + 91) & 2) != 0) {
MsrValue |= 0x10000;
AsmWriteMsr64 (MSR_PKG_CST_CONFIG_CONTROL, MsrValue);
}
//
// Check IO MWAIT redirection
//
if (*(INT8 *)((UINTN)mPpmContext + 4) < 0) {
MsrValue = AsmReadMsr64 (MSR_PKG_CST_CONFIG_CONTROL);
if ((MsrValue & 0x8000) == 0) {
MsrValue |= 0x8000;
AsmWriteMsr64 (MSR_PKG_CST_CONFIG_CONTROL, MsrValue);
}
}
}
//=============================================================================
// PPM PBF MSR Configuration
//=============================================================================
/**
PBF MSR programming: MSR_PPM_INTERRUPT (0x1AD) and MSR_PPM_INTERRUPT2 (0x1AE).
**/
VOID
PpmPbfMsrConfig (
VOID
)
{
UINT64 BspCpuIndex;
UINT64 MsrInterrupt;
UINT64 MsrInterrupt2;
UINT64 MsrPlatformInfo;
BspCpuIndex = 0;
mMpService->WhoAmI (mMpService, &BspCpuIndex);
MsrInterrupt = AsmReadMsr64 (MSR_PPM_INTERRUPT);
MsrInterrupt2 = AsmReadMsr64 (MSR_PPM_INTERRUPT2);
MsrPlatformInfo = AsmReadMsr64 (MSR_PLATFORM_INFO);
if ((MsrPlatformInfo & 0x10000000) != 0) {
//
// Check PPM_LIMIT_RATIOS
//
UINT64 MsrLimitRatios;
MsrLimitRatios = AsmReadMsr64 (MSR_PPM_LIMIT_RATIOS);
if ((MsrLimitRatios & 0x100000) != 0) {
//
// Clear bit 20 and program from context
//
MsrLimitRatios &= 0xFFEFFFFF;
AsmWriteMsr64 (MSR_PPM_LIMIT_RATIOS, MsrLimitRatios);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PPM_LIMIT_RATIOS, MsrLimitRatios, 404, 0, 64, MsrLimitRatios);
MsrLimitRatios = (MsrLimitRatios & 0xFFFFFF00) | *(UINT32 *)((UINTN)mPpmContext + 152);
AsmWriteMsr64 (MSR_PPM_LIMIT_RATIOS, MsrLimitRatios);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PPM_LIMIT_RATIOS, MsrLimitRatios, 404, 0, 64, MsrLimitRatios);
}
//
// Program MSR_PPM_INTERRUPT and MSR_PPM_INTERRUPT2 with mask values
//
MsrInterrupt = *(UINT64 *)((UINTN)mPpmContext + 265) | (MsrInterrupt & *(UINT64 *)((UINTN)mPpmContext + 273));
MsrInterrupt2 = *(UINT64 *)((UINTN)mPpmContext + 281) | (MsrInterrupt2 & *(UINT64 *)((UINTN)mPpmContext + 289));
AsmWriteMsr64 (MSR_PPM_INTERRUPT, MsrInterrupt);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PPM_INTERRUPT, MsrInterrupt, 429, 0, 64, MsrInterrupt);
AsmWriteMsr64 (MSR_PPM_INTERRUPT2, MsrInterrupt2);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PPM_INTERRUPT2, MsrInterrupt2, 430, 0, 64, MsrInterrupt2);
}
}
//=============================================================================
// PPM Energy-Efficient Turbo / HWP Configuration
//=============================================================================
/**
Configure energy-efficient turbo and HWP limits (MSR_PPM_HWP_LIMIT 0x1AA).
**/
VOID
PpmEnergyEfficientTurboConfig (
VOID
)
{
UINT64 BspCpuIndex;
UINT64 MsrValue;
BspCpuIndex = 0;
mMpService->WhoAmI (mMpService, &BspCpuIndex);
MsrValue = AsmReadMsr64 (MSR_PPM_HWP_LIMIT);
MsrValue &= 0xFFFFEE3F;
if (*(UINT8 *)((UINTN)mPpmContext + 213)) {
MsrValue |= 0x40;
} else if (*(UINT8 *)((UINTN)mPpmContext + 214)) {
MsrValue |= 0x100;
}
if (*(UINT8 *)((UINTN)mPpmContext + 215)) {
MsrValue |= 0x80;
}
if (*(UINT8 *)((UINTN)mPpmContext + 217)) {
MsrValue |= 0x1000;
}
AsmWriteMsr64 (MSR_PPM_HWP_LIMIT, MsrValue);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PPM_HWP_LIMIT, MsrValue, 426, 0, 64, MsrValue);
//
// Check for Package C-state limit type 3
//
if (*(UINT8 *)((UINTN)mPpmContext + 212) == 3) {
MsrValue = AsmReadMsr64 (MSR_PM_ENABLE) | 1;
AsmWriteMsr64 (MSR_PM_ENABLE, MsrValue);
S3BootScriptSaveMsrWrite (BspCpuIndex, MSR_PM_ENABLE, MsrValue, 1904, 0, 64, MsrValue);
}
//
// Package power limit MSR_MSR_PKG_POWER_LIMIT if needed
//
if (*(UINT8 *)((UINTN)mPpmContext + 215)) {
PpmPackagePowerLimitConfig ();
}
}
//=============================================================================
// PPM Package Power Limit Configuration
//=============================================================================
/**
Configure package power limit MSR (MSR_PKG_POWER_LIMIT).
**/
VOID
PpmPackagePowerLimitConfig (
VOID
)
{
//
// This area configures package power limit MSR registers.
// In the original binary, this is found in the SKX-specific
// path within PpmMsrConfig.
//
}
//=============================================================================
// PPM PMAX Offset Table Search
//=============================================================================
/**
Search the PMAX offset table for closest matching TDP value.
The table maps package TDP values to offset corrections.
If an exact match is found, the associated correction is returned.
If no exact match but the value falls within a range, linear
interpolation is performed.
@param[in] Context Pointer to PPM context.
@return Offset correction value.
**/
UINT32
PpmPmaxOffsetSearch (
UINT64 *Context
)
{
UINT32 TdpValue;
UINT32 IccMax;
UINT32 Result;
UINT32 TableIndex;
UINT64 ConfigContext;
UINT8 SocketIndex;
//
// Read CPU configuration context
//
ConfigContext = (UINTN)GetPcdProtocol ()->GetPcd (PCD_TOKEN_PM_MODE);
SocketIndex = *(UINT8 *)(*(UINTN *)(Context + 3) + 1);
//
// Read TDP and IccMax from PPM config registers
//
TdpValue = ((UINT32)GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_TDP) >> 3) & 7;
TdpValue = TdpValue - 2;
if (TdpValue <= 1) {
//
// Use table-based PMAX offset search
//
Result = (UINT32)GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_INFO);
} else {
//
// Use MSR-based TDP
//
UINT16 MsrTdp;
MsrTdp = GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_TDP);
TdpValue = (MsrTdp & 0x7FFF) / *(UINT8 *)((UINTN)Context + SocketIndex + 52);
Result = 0;
}
return Result;
}
//=============================================================================
// BIOS_RESET_CPL Handshake
//=============================================================================
/**
BIOS_RESET_CPL handshake with PCU.
Writes the CPL value to the PCU register and polls for acknowledgement.
@param[in] Context Pointer to PPM context.
@return Final CPL status byte.
**/
CHAR8
PpmBiosResetCpl (
UINT64 *Context
)
{
UINT8 SocketIndex;
UINT32 CplValue;
UINT32 RegValue;
UINT8 CplPhase;
UINT8 PollCount;
UINT8 Result;
SocketIndex = *(UINT8 *)(*(UINTN *)(Context + 3) + 1);
CplPhase = *(UINT8 *)(*(UINTN *)(Context + 3) + 90);
//
// Read current register value
//
RegValue = GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_INFO);
//
// Set CPL value based on phase
//
switch (CplPhase) {
case 1:
CplValue = RegValue | 2;
break;
case 2:
CplValue = RegValue | 4;
break;
case 3:
CplValue = RegValue | 8;
break;
case 4:
CplValue = RegValue | 0x10;
break;
default:
ASSERT (FALSE);
goto Done;
}
//
// If CplPhase is 0, set bit 0
//
if (CplPhase == 0) {
CplValue = RegValue | 1;
}
DEBUG ((
EFI_D_INFO,
" \n:PM - BIOS_RESET_CPL: write CPL%d on #S%d into BIOS_RESET_CPL_PCU_FUN1_REG\n\n",
CplPhase,
SocketIndex
));
//
// Write CPL value to register and record in boot script
//
GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_INFO);
//
// Poll for PCU acknowledgement
//
PollCount = 5;
if (*(UINT8 *)PciRead32 (252) != 4) {
do {
RegValue = GetPcdProtocol ()->GetPcd (PCD_TOKEN_PPM_INFO);
MicroSecondDelay (3579);
PollCount--;
} while (!(RegValue & (1 << (CplPhase + 8))) && PollCount);
if (!PollCount) {
DEBUG ((EFI_D_ERROR, " \n:PM - Pcode fails to ACK on Socket%d\n\n", SocketIndex));
}
}
Done:
return (CHAR8)CplValue;
}
//=============================================================================
// SmmReadyToLock Callback
//=============================================================================
/**
SmmReadyToLock notification callback.
Saves boot script and closes tables.
@param[in] Event Event handle.
@param[in] Context Event context.
**/
STATIC
VOID
EFIAPI
SmmReadyToLockCallback (
IN EFI_EVENT Event,
IN VOID *Context
)
{
RETURN_STATUS Status;
if (mEventDxeSmmReadyToLock) {
//
// Close boot script table
//
BootScriptCloseTable (NULL);
}
if (mEventReadyToBoot) {
//
// Free boot script buffer
//
(VOID)gBootServices->FreePool ((VOID *)(UINTN)mEventReadyToBoot);
GetPcdProtocol ()->SetPcd (PCD_TOKEN_CPU_PM_REG_ADDR, 0);
mS3LockBoxInited = FALSE;
}
if (mS3BootScriptInited) {
//
// Free boot script pool
//
VOID *ScriptBuffer;
ScriptBuffer = (VOID *)(UINTN)mS3BootScriptTable;
(VOID)gBootServices->FreePool (ScriptBuffer);
GetPcdProtocol ()->SetPcd (PCD_TOKEN_CPU_PM_STRUCT, 0);
mS3BootScriptInited = FALSE;
}
}
//=============================================================================
// ReadyToBoot Callback
//=============================================================================
/**
ReadyToBoot notification callback.
@param[in] Event Event handle.
@param[in] Context Event context.
**/
STATIC
VOID
EFIAPI
ReadyToBootCallback (
IN EFI_EVENT Event,
IN VOID *Context
)
{
}
//=============================================================================
// Internal Helpers
//=============================================================================
STATIC
UINT64
InternalGetBootScriptTable (
VOID
)
{
return mS3BootScriptTable;
}
STATIC
UINT64
InternalGetCpuConfigContext (
VOID
)
{
return (UINT64)(UINTN)mCpuConfigLibConfigContextBuffer;
}
STATIC
UINT64
InternalGetRegAddr (
VOID
)
{
return (UINT64)(UINTN)mPpmRegAddr;
}
//=============================================================================
// CPUID / TSC / Intrinsics
//=============================================================================
UINT64
CpuidEx (
IN UINT32 Leaf,
IN UINT32 Subleaf,
OUT UINT32 *Eax OPTIONAL,
OUT UINT32 *Ebx OPTIONAL,
OUT UINT32 *Ecx OPTIONAL,
OUT UINT32 *Edx OPTIONAL
)
{
INTN Result;
INT32 RegEax, RegEbx, RegEcx, RegEdx;
AsmCpuidEx (Leaf, Subleaf, &RegEax, &RegEbx, &RegEcx, &RegEdx);
if (Eax) *Eax = RegEax;
if (Ebx) *Ebx = RegEbx;
if (Ecx) *Ecx = RegEcx;
if (Edx) *Edx = RegEdx;
return (INTN)(UINTN)Leaf;
}
UINT64
Cpuid (
IN UINT32 Leaf,
OUT UINT32 *Eax OPTIONAL,
OUT UINT32 *Ebx OPTIONAL,
OUT UINT32 *Ecx OPTIONAL,
OUT UINT32 *Edx OPTIONAL
)
{
INT32 RegEax, RegEbx, RegEcx, RegEdx;
AsmCpuid (Leaf, &RegEax, &RegEbx, &RegEcx, &RegEdx);
if (Eax) *Eax = RegEax;
if (Ebx) *Ebx = RegEbx;
if (Ecx) *Ecx = RegEcx;
if (Edx) *Edx = RegEdx;
return (INTN)(UINTN)Leaf;
}
UINT64
ReadTsc (
VOID
)
{
return AsmReadTsc ();
}
VOID
CpuPause (
VOID
)
{
CpuPause ();
}
VOID
EnableInterrupts (
VOID
)
{
EnableInterrupts ();
}
VOID
DisableInterrupts (
VOID
)
{
DisableInterrupts ();
}
UINTN
ReadEflags (
VOID
)
{
return AsmReadEflags ();
}
VOID
ZeroMemSafe (
IN VOID *Buffer,
IN UINTN Length
)
{
ASSERT (Buffer != NULL);
ASSERT (Length <= (MAX_ADDRESS - (UINTN)Buffer + 1));
ZeroMem (Buffer, Length);
}
VOID *
CopyMemSafe (
OUT VOID *DestinationBuffer,
IN CONST VOID *SourceBuffer,
IN UINTN Length
)
{
if (Length == 0) {
return DestinationBuffer;
}
ASSERT ((Length - 1) <= (MAX_ADDRESS - (UINTN)DestinationBuffer));
ASSERT ((Length - 1) <= (MAX_ADDRESS - (UINTN)SourceBuffer));
if (DestinationBuffer == SourceBuffer) {
return DestinationBuffer;
}
return CopyMem (DestinationBuffer, SourceBuffer, Length);
}
UINT32
PciRead32 (
IN UINTN Address
)
{
ASSERT ((Address & ~0xFFFFFFF) == 0);
return MmioRead32 (Address + (UINTN)mPpmRegAddr);
}
VOID
PciWrite32 (
IN UINTN Address,
IN UINT32 Value
)
{
ASSERT ((Address & ~0xFFFFFFF) == 0);
MmioWrite32 (Address + (UINTN)mPpmRegAddr, Value);
}
UINT32
IoRead32 (
IN UINT16 Port
)
{
ASSERT ((Port & 3) == 0);
return IoRead32 (Port);
}
UINT16
IoRead16 (
IN UINT16 Port
)
{
ASSERT ((Port & 1) == 0);
return IoRead16 (Port);
}
VOID
IoWrite16 (
IN UINT16 Port,
IN UINT16 Value
)
{
ASSERT ((Port & 1) == 0);
IoWrite16 (Port, Value);
}
UINT8
IoRead8 (
IN UINT16 Port
)
{
return IoRead8 (Port);
}
VOID
IoWrite8 (
IN UINT16 Port,
IN UINT8 Value
)
{
IoWrite8 (Port, Value);
}
//=============================================================================
// Protocol / HOB / PCD Helpers
//=============================================================================
VOID *
GetHobList (
VOID
)
{
EFI_STATUS Status;
if (mHobList == NULL) {
Status = EfiGetSystemConfigurationTable (&gEfiHobListGuid, &mHobList);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
if (mHobList == NULL) {
ASSERT (mHobList != NULL);
}
}
return mHobList;
}
VOID *
GetPcdProtocol (
VOID
)
{
EFI_STATUS Status;
if (mPcd == NULL) {
Status = gBootServices->LocateProtocol (
&gEfiPcdProtocolGuid,
NULL,
&mPcd
);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
if (mPcd == NULL) {
ASSERT (mPcd != NULL);
}
}
return mPcd;
}
VOID *
GetDebugProtocol (
VOID
)
{
UINTN BufferSize;
EFI_STATUS Status;
if (mDebugProtocol == NULL) {
BufferSize = gBootServices->GetPoolSize (31);
(VOID)gBootServices->SetPoolSize (BufferSize);
if (BufferSize <= 16) {
Status = gBootServices->LocateProtocol (
&gEfiPcdProtocolGuid,
NULL,
&mDebugProtocol
);
if (EFI_ERROR (Status)) {
mDebugProtocol = NULL;
}
}
}
return mDebugProtocol;
}
VOID *
GetPpmContext (
VOID
)
{
return mPpmContext;
}
VOID *
GetPpmInfoTable (
VOID
)
{
return mPpmInfoTable;
}
VOID *
GetPpmInfoFromHob (
VOID
)
{
EFI_STATUS Status;
Status = EfiGetSystemConfigurationTable (
&gPpmInfoHobGuid,
&mPpmInfoTable
);
return (VOID *)(UINTN)Status;
}
VOID *
GetMpServiceProtocol (
VOID
)
{
return mMpService;
}
VOID *
GetSmmCommunicationProtocol (
VOID
)
{
return mSmmComm;
}
//=============================================================================
// S3 Boot Script Helpers
//=============================================================================
/**
Allocate boot script table entry.
@param[in] Type Boot script entry type.
**/
UINT64
BootScriptAllocate (
UINT8 Type
)
{
VOID *Buffer;
UINT64 Result;
Result = (UINTN)GetPcdProtocol ()->GetPcd (PCD_TOKEN_CPU_PM_STRUCT);
if (Result != (UINTN)-1) {
Buffer = (VOID *)(UINTN)Result;
} else {
//
// Allocate new buffer
//
EFI_STATUS Status;
Status = gBootServices->AllocatePool (
EfiBootServicesData,
1,
&Buffer
);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
Result = (UINTN)Buffer;
mS3BootScriptInited = TRUE;
GetPcdProtocol ()->SetPcd (PCD_TOKEN_CPU_PM_STRUCT, Result);
ZeroMemSafe (Buffer, 32);
//
// Register SmmReadyToLock event for boot script save
//
mEventDxeSmmReadyToLock = (VOID *)GetPcdProtocol ()->GetPcd (PCD_TOKEN_CPU_PM_REG_ADDR);
if (mEventDxeSmmReadyToLock == NULL) {
;
}
}
return Result;
}
VOID
BootScriptCloseTable (
VOID *TableEntry
)
{
//
// In the original binary, this writes the table length and
// finalizes the boot script entry list.
//
if (mS3BootScriptInited) {
//
// Close the S3 boot script table
//
}
}
/**
Record an MSR write as a boot script entry.
@param[in] BspCpuIndex BSP CPU index.
@param[in] MsrIndex MSR register index.
@param[in] Value Value written to MSR.
@param[in] Width Width parameter for boot script opcode.
@param[in] BitIndex Bit index (for bit-field writes).
@param[in] BitValue Bit value.
@param[in] Mask Data mask.
@return Written value.
**/
UINT64
S3BootScriptSaveMsrWrite (
UINT64 BspCpuIndex,
UINT64 MsrIndex,
UINT64 Value,
UINT32 Width,
UINT8 BitIndex,
UINT8 BitValue,
UINT64 Mask
)
{
UINT64 ConfigContext;
UINT64 BufferEntry;
UINT64 CurrentCount;
UINT64 Index;
ConfigContext = InternalGetCpuConfigContext ();
if (ConfigContext == 0) {
return 0;
}
//
// Get current entry count
//
Index = 3 * MsrIndex;
CurrentCount = *(UINT32 *)(*(UINTN *)(ConfigContext + 40) + 8 * Index + 8);
if (*(UINT32 *)(*(UINTN *)(ConfigContext + 40) + 8 * Index) == CurrentCount / 0x18) {
//
// Need to expand the buffer
//
UINT64 NewCount;
UINT64 OldBuffer;
NewCount = (UINT64)(UINTN)((UINT32)CurrentCount >> 12);
OldBuffer = 0;
EFI_STATUS Status;
Status = gBootServices->AllocatePool (
EfiBootServicesData,
1,
(VOID **)&NewCount + 1
);
if (EFI_ERROR (Status)) {
DEBUG ((EFI_D_ERROR, "\nASSERT_EFI_ERROR (Status = %r)\n", Status));
ASSERT_EFI_ERROR (Status);
}
OldBuffer = *(UINTN *)(*(UINTN *)(ConfigContext + 40) + 8 * Index + 16);
if (CurrentCount != 0) {
CopyMemSafe (
(VOID *)(UINTN)NewCount,
(VOID *)(UINTN)OldBuffer,
(UINTN)CurrentCount
);
gBootServices->FreePool ((VOID *)(UINTN)OldBuffer);
}
*(UINT32 *)(*(UINTN *)(ConfigContext + 40) + 8 * Index + 8) = (UINT32)((UINTN)NewCount + 4096);
*(UINTN *)(*(UINTN *)(ConfigContext + 40) + 8 * Index + 16) = (UINTN)NewCount;
}
//
// Write boot script entry
//
BufferEntry = *(UINTN *)(*(UINTN *)(ConfigContext + 40) + 8 * Index + 16);
Index = *(UINT32 *)(*(UINTN *)(ConfigContext + 40) + 8 * Index);
*(UINT32 *)(BufferEntry + 24 * Index) = 0;
*(UINT32 *)(BufferEntry + 24 * Index + 4) = Width;
*(UINT8 *)(BufferEntry + 24 * Index + 8) = BitIndex;
*(UINT8 *)(BufferEntry + 24 * Index + 9) = BitValue;
*(UINT64 *)(BufferEntry + 24 * Index + 16) = Mask;
(*(UINT32 *)(*(UINTN *)(ConfigContext + 40) + 8 * (3ULL * MsrIndex)))++;
return Mask;
}
//=============================================================================
// PPM HOB Search
//=============================================================================
/**
Walk HOB list to find a specific GUID HOB.
@param[in] Guid GUID to search for.
@return Pointer to HOB data if found, NULL otherwise.
**/
VOID *
FindPpmGuidHob (
IN EFI_GUID *Guid
)
{
VOID *Hob;
UINTN HobList;
Hob = GetHobList ();
if (Hob == NULL) {
return NULL;
}
HobList = (UINTN)Hob;
while (*((UINT16 *)HobList) != 0xFFFF) {
if (*((UINT16 *)HobList) == 4) {
if (CompareGuid ((GUID *)(HobList + 8), Guid)) {
return (VOID *)(*((UINT64 *)(HobList + 24)));
}
}
HobList += *((UINT16 *)(HobList + 2));
}
return NULL;
}
//=============================================================================
// Memory Copy Implementation (Used internally by boot script save)
//=============================================================================
/**
Internal memory copy with forward/backward overlap handling.
@param[out] Dst Destination buffer.
@param[in] Src Source buffer.
@param[in] Count Number of bytes to copy.
@return Dest Destination buffer.
**/
CHAR8 *
InternalCopyMem (
OUT CHAR8 *Dst,
IN CONST CHAR8 *Src,
IN UINT64 Count
)
{
CHAR8 *DstOrig;
DstOrig = Dst;
if (Src < Dst && &Src[Count - 1] >= Dst) {
//
// Overlapping, copy backward
//
Src = &Src[Count - 1];
Dst = &Dst[Count - 1];
} else {
//
// Copy forward in 8-byte chunks then remainder
//
UINT64 Count8;
Count8 = Count >> 3;
CopyMem (Dst, Src, 8 * Count8);
Src = &Src[8 * Count8];
Dst = &Dst[8 * Count8];
}
CopyMem (Dst, Src, Count & 7);
return DstOrig;
}
//=============================================================================
// Assert / Print Handler
//=============================================================================
/**
Debug print handler with CMOS-based platform ID detection.
@param[in] ErrorLevel Error level.
@param[in] Format Format string.
@param[in] ... Variable arguments.
**/
VOID
DebugPrintHandler (
IN UINTN ErrorLevel,
IN CONST CHAR8 *Format,
...
)
{
VA_LIST Marker;
UINTN DebugProtocol;
UINT8 CmosIndex;
UINT8 CmosValue;
UINT8 PlatformId;
DebugProtocol = (UINTN)GetDebugProtocol ();
if (DebugProtocol == 0) {
return;
}
//
// Read CMOS 0x4B to get platform ID
//
IoWrite8 (0x70, IoRead8 (0x70) & 0x80 | 0x4B);
CmosValue = IoRead8 (0x71);
if (CmosValue > 3) {
if (CmosValue == 0) {
CmosValue = (*(UINT8 *)(UINTN)0xFDAF0490 & 2) | 1;
}
} else {
if ((UINT8)(CmosValue - 1) <= 0xFD) {
CmosValue = 4;
}
}
PlatformId = CmosValue - 1;
//
// Determine error level mapping based on platform ID
//
switch (PlatformId) {
case 0:
ErrorLevel = 0x80000004; // EFI_D_INFO equivalent
break;
default:
ErrorLevel = 0x80000006; // EFI_D_ERROR equivalent
break;
}
//
// Call debug protocol if error level matches
//
if (ErrorLevel != 0) {
VA_START (Marker, Format);
((VOID (*)(UINTN, CONST CHAR8 *, VA_LIST))DebugProtocol)(ErrorLevel, Format, Marker);
VA_END (Marker);
}
}
//=============================================================================
// Assert Macro Implementation
//=============================================================================
/**
Assert handler: prints file/line/expression and optionally breaks.
@param[in] FileName Source file name.
@param[in] LineNumber Line number.
@param[in] Expression Assertion expression string.
**/
VOID
AssertHandler (
IN CONST CHAR8 *FileName,
IN UINTN LineNumber,
IN CONST CHAR8 *Expression
)
{
UINTN DebugProtocol;
DebugProtocol = (UINTN)GetDebugProtocol ();
if (DebugProtocol != 0) {
((VOID (*)(UINTN, CONST CHAR8 *, UINTN, CONST CHAR8 *))DebugProtocol)(
FileName,
LineNumber,
Expression
);
}
}