GitBucket
Newer
Ajax Dong
/*=============================================================================
* CapsuleX64.efi - Capsule PEI X64 Module
*
* Binary MD5: f20dc2f52ef47aa5bb67065361900b0a
* Binary SHA256: f375c4b961d442295e12bd69e7648e2f45cf4a0d5c35f1795b611656107dbfcc
*
* Source: MdeModulePkg/Universal/CapsulePei/X64/
*
* This module implements the X64-specific capsule processing for the
* PEI phase, including:
* - Page fault handler for on-demand page table allocation
* - X64 exception handler and CPU context dump
* - Capsule integrity validation (ValidateCapsuleIntegrity)
* - Capsule descriptor relocation (RelocateCapsuleDescriptors)
* - Capsule data coalescing (CapsuleCoalesce)
* - Debug output via serial port
* - APIC identification
* - PE/COFF image information lookup
*
* Total: 50 functions, 193 strings, 8 segments
*============================================================================*/
#include "CapsuleX64.h"
#include <stdarg.h>
/*=============================================================================
* Intrinsic helpers (inline assembly wrappers)
*============================================================================*/
void _disable(void) { __asm__ volatile("cli"); }
void _enable(void) { __asm__ volatile("sti"); }
static inline UINT64 __readcr0(void)
{ UINT64 v; __asm__ volatile("mov %%cr0, %0" : "=r"(v)); return v; }
static inline UINT64 __readcr2_w(void)
{ UINT64 v; __asm__ volatile("mov %%cr2, %0" : "=r"(v)); return v; }
static inline UINT64 __readcr3_w(void)
{ UINT64 v; __asm__ volatile("mov %%cr3, %0" : "=r"(v)); return v; }
static inline UINT64 __readcr4(void)
{ UINT64 v; __asm__ volatile("mov %%cr4, %0" : "=r"(v)); return v; }
static inline void __writecr0(UINT64 v)
{ __asm__ volatile("mov %0, %%cr0" :: "r"(v)); }
static inline void __writecr3(UINT64 v)
{ __asm__ volatile("mov %0, %%cr3" :: "r"(v)); }
static inline void __writecr4(UINT64 v)
{ __asm__ volatile("mov %0, %%cr4" :: "r"(v)); }
static inline void __writecr8(UINT64 v)
{ __asm__ volatile("mov %0, %%cr8" :: "r"(v)); }
static inline UINT64 __readdr(int n)
{ UINT64 v; __asm__ volatile("mov %%dr%c1, %0" : "=r"(v) : "i"(n)); return v; }
static inline UINT64 __readmsr(UINT32 msr)
{ UINT32 lo, hi; __asm__ volatile("rdmsr" : "=a"(lo), "=d"(hi) : "c"(msr));
return ((UINT64)hi << 32) | lo; }
static inline UINT8 __inbyte(UINT16 port)
{ UINT8 v; __asm__ volatile("inb %1, %0" : "=a"(v) : "Nd"(port)); return v; }
static inline void __outbyte(UINT16 port, UINT8 v)
{ __asm__ volatile("outb %0, %1" :: "a"(v), "Nd"(port)); }
static inline void __sidt_w(void *p)
{ __asm__ volatile("sidt %0" : "=m"(*(IDTR *)p)); }
static inline void __lidt(const UINT16 *p)
{ __asm__ volatile("lidt %0" :: "m"(*p)); }
static inline UINT64 __getcallerseflags(void)
{ UINT64 v; __asm__ volatile("pushfq; pop %0" : "=r"(v)); return v; }
static inline void __writeeflags(UINT64 v)
{ __asm__ volatile("push %0; popfq" :: "r"(v)); }
static inline void __sgdt(void *p)
{ __asm__ volatile("sgdt %0" : "=m"(*(GDTR *)p)); }
static void qmemcpy(void *dst, const void *src, UINT64 n) {
__builtin_memcpy(dst, src, n);
}
static void memset(void *dst, int c, UINT64 n) {
__builtin_memset(dst, c, n);
}
/*=============================================================================
* AsmReadCr3 - Read CR3 register
*============================================================================*/
UINT64 AsmReadCr3(void)
{
return __readcr3_w();
}
/*=============================================================================
* AsmReadCr2 - Read CR2 register
*============================================================================*/
UINT64 AsmReadCr2(void)
{
return __readcr2_w();
}
/*=============================================================================
* AsmReadCs - Read CS segment selector
*============================================================================*/
UINT16 AsmReadCs(void)
{
UINT16 cs;
__asm__ volatile("mov %%cs, %0" : "=r"(cs));
return cs;
}
/*=============================================================================
* AsmWriteIdtr - Load IDTR (disable interrupts)
*============================================================================*/
void AsmWriteIdtr(UINT16 *pLimit)
{
UINT64 callersEflags = __getcallerseflags();
_disable();
__lidt(pLimit);
__writeeflags(callersEflags);
}
/*=============================================================================
* AsmReadIdtr - Read IDTR with null check
*============================================================================*/
void AsmReadIdtr(void *pIdtr)
{
if (!pIdtr) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseLib\\X86ReadIdtr.c", 37,
(UINT64)"Idtr != ((void *) 0)");
}
__sidt_w(pIdtr);
}
/*=============================================================================
* AsmCpuid - Execute CPUID instruction
*============================================================================*/
UINT64 AsmCpuid(UINT32 Leaf, UINT32 *pEax, UINT32 *pEbx,
UINT32 *pEcx, UINT32 *pEdx)
{
UINT32 eax, ebx, ecx, edx;
__asm__ volatile("cpuid"
: "=a"(eax), "=b"(ebx), "=c"(ecx), "=d"(edx)
: "a"(Leaf), "c"(0));
if (pEcx) *pEcx = ecx;
if (pEax) *pEax = eax;
if (pEbx) *pEbx = ebx;
if (pEdx) *pEdx = edx;
return Leaf;
}
/*=============================================================================
* AsmCpuidEx - Execute CPUID with subleaf
*============================================================================*/
UINT64 AsmCpuidEx(UINT32 Leaf, UINT32 Subleaf,
UINT32 *pEax, UINT32 *pEbx,
UINT32 *pEcx, UINT32 *pEdx)
{
UINT32 eax, ebx, ecx, edx;
__asm__ volatile("cpuid"
: "=a"(eax), "=b"(ebx), "=c"(ecx), "=d"(edx)
: "a"(Leaf), "c"(Subleaf));
if (pEcx) *pEcx = ecx;
if (pEax) *pEax = eax;
if (pEbx) *pEbx = ebx;
if (pEdx) *pEdx = edx;
return Leaf;
}
/*=============================================================================
* AsmDisablePaging64 - Disable paging, far return
*
* Copies a trampoline code block to NewStack area, then executes a
* far return (retfq) to transition to 32-bit compat mode, disable
* paging, and return to 64-bit long mode.
*============================================================================*/
void AsmDisablePaging64(UINT16 Signature, UINT64 EntryPoint,
EFI_HANDLE ImageHandle,
EFI_SYSTEM_TABLE *SystemTable,
UINT32 NewStack)
{
extern char byte_FFE44869[];
extern char src[];
_disable();
UINT64 count = (UINT64)byte_FFE44869 - (UINT64)src + 4;
count &= 0xFC;
CopyMem((void *)(NewStack - count), src, count);
__asm__ volatile("retfq");
}
/*=============================================================================
* InternalZeroMem
*============================================================================*/
void *InternalZeroMem(void *Buffer, UINT64 Size)
{
memset(Buffer, 0, 8 * (Size >> 3));
memset((UINT8 *)Buffer + 8 * (Size >> 3), 0, Size & 7);
return Buffer;
}
/*=============================================================================
* InternalCopyMem
*============================================================================*/
void *InternalCopyMem(void *Dest, const void *Src, UINT64 Count)
{
const unsigned char *s = (const unsigned char *)Src;
unsigned char *d = (unsigned char *)Dest;
if (s < d && &s[Count - 1] >= d) {
unsigned char *ds = &d[Count - 1];
const unsigned char *ss = &s[Count - 1];
for (UINT64 i = Count; i > 0; i--)
*ds-- = *ss--;
} else {
UINT64 aligned = Count >> 3;
Count &= 7;
if (aligned)
qmemcpy(d, s, aligned * 8);
qmemcpy(&d[aligned * 8], &s[aligned * 8], Count);
}
return Dest;
}
/*=============================================================================
* ZeroMem - Zero memory with bounds check
*============================================================================*/
void *ZeroMem(void *Buffer, UINT64 Size)
{
if (!Buffer) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseMemoryLibRepStr"
"\\ZeroMemWrapper.c", 53,
(UINT64)"Buffer != ((void *) 0)");
}
if (Size > ~(UINT64)Buffer) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseMemoryLibRepStr"
"\\ZeroMemWrapper.c", 54,
(UINT64)"Length <= (0xFFFFFFFFFFFFFFFFULL - (UINTN)Buffer + 1)");
}
return InternalZeroMem(Buffer, Size);
}
/*=============================================================================
* CopyMem - Copy memory with bounds checks
*============================================================================*/
void *CopyMem(void *Dest, const void *Src, UINT64 Count)
{
if (Count) {
UINT64 CountMinusOne = Count - 1;
if (CountMinusOne > ~(UINT64)Dest) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseMemoryLibRepStr"
"\\CopyMemWrapper.c", 56,
(UINT64)"(Length - 1) <= (0xFFFFFFFFFFFFFFFFULL - (UINTN)DestinationBuffer)");
}
if (CountMinusOne > ~(UINT64)Src) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseMemoryLibRepStr"
"\\CopyMemWrapper.c", 57,
(UINT64)"(Length - 1) <= (0xFFFFFFFFFFFFFFFFULL - (UINTN)SourceBuffer)");
}
if (Dest == Src)
return Dest;
return InternalCopyMem(Dest, Src, Count);
}
return Dest;
}
/*=============================================================================
* InternalLShiftU64 - 64-bit left shift
*============================================================================*/
UINT64 InternalLShiftU64(UINT64 Value, UINT64 Count)
{
if (Count >= 0x40) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseLib\\LShiftU64.c", 39,
(UINT64)"Count < 64");
}
return Value << (UINT8)Count;
}
/*=============================================================================
* BitFieldReadU64 - Extract bit field from UINT64
*============================================================================*/
UINT64 BitFieldReadU64(UINT64 Value, UINT64 StartBit, UINT64 EndBit)
{
if (EndBit >= 0x40) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseLib\\BitField.c", 731,
(UINT64)"EndBit < 64");
}
if (StartBit > EndBit) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseLib\\BitField.c", 732,
(UINT64)"StartBit <= EndBit");
}
Value &= ~InternalLShiftU64(~0ULL, EndBit);
return Value >> StartBit;
}
/*=============================================================================
* AsciiStrLen - Get ASCII string length
*============================================================================*/
UINT64 AsciiStrLen(const char *String)
{
UINT64 i;
if (!String) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseLib\\String.c", 1082,
(UINT64)"String != ((void *) 0)");
}
for (i = 0; *String; i++) {
if (i >= 1000000) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseLib\\String.c", 1090,
(UINT64)"Length < _gPcd_FixedAtBuild_PcdMaximumAsciiStringLength");
}
String++;
}
return i;
}
/*=============================================================================
* InternalPrintPad - Pad ASCII/Unicode string
*============================================================================*/
UINT8 *InternalPrintPad(UINT8 *Buffer, UINT64 BufferSize,
UINT64 Count, UINT16 Char, UINT64 CharWidth)
{
UINT64 i;
for (i = 0; i < Count; i++) {
if ((UINT64)Buffer >= BufferSize)
break;
*Buffer = (UINT8)Char;
if (CharWidth != 1)
Buffer[1] = (UINT8)(Char >> 8);
Buffer += CharWidth;
}
return Buffer;
}
/*=============================================================================
* SerialPortWrite - write buffer to serial port
*
* Uses CMOS to check serial port type (COM1=0x3F8 vs SP1=0x2F8).
* Writes data in chunks of up to 16 bytes.
* Returns number of bytes written, or 0 on timeout.
*============================================================================*/
UINT64 SerialPortWrite(UINT8 *Buffer, UINT64 Size)
{
UINT16 SerialBase = 0x3F8;
__outbyte(0x72, 0x5C);
UINT8 CmosValue = __inbyte(0x73);
if (CmosValue == 33)
SerialBase = 0x2F8;
if (!Size) {
INT16 timeout = 0;
for (;;) {
UINT8 status = __inbyte(SerialBase + 5);
if ((status & 0x60) == 0x60)
break;
if (++timeout == 0xFFFF)
return 0;
}
return 0;
}
UINT8 *p = Buffer;
UINT64 remaining = Size;
while (remaining) {
INT16 timeout = 0;
for (;;) {
UINT8 status = __inbyte(SerialBase + 5);
if (status & 0x40)
break;
if (++timeout == 0xFFFF)
return 0;
}
UINT64 chunk = 0;
while (chunk < 0x10 && remaining) {
__outbyte(SerialBase, *p);
p++; remaining--; chunk++;
}
}
return Size;
}
/*=============================================================================
* DebugPrint - Debug print with level filtering
*
* Checks debug level via CMOS (NVRAM offset 0x4A).
* Error level mask: 0x80000002 (verbose) or 0x80000004 (error only).
*============================================================================*/
UINT8 DebugPrint(UINT64 ErrorLevel, const char *Format, ...)
{
char Buffer[280];
va_list Va;
va_start(Va, Format);
if (!Format) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseDebugLibSerialPort"
"\\DebugLib.c", 79,
(UINT64)"Format != ((void *) 0)");
}
UINT8 CmosIndex = __inbyte(0x70);
__outbyte(0x70, (CmosIndex & 0x80) | 0x4A);
UINT8 DebugMask = __inbyte(0x71);
if ((UINT8)DebugMask > 3) {
if (!DebugMask)
DebugMask = (*(volatile UINT8 *)0xFDAF0490 & 2) | 1;
}
UINT64 LevelMask = 0x80000002ULL;
if (DebugMask == 1)
LevelMask = 0x80000004ULL;
if (!(LevelMask & ErrorLevel))
return DebugMask - 1;
InternalVSPrint((UINT8 *)Buffer, 256, 0, (char *)ErrorLevel, Va);
UINT64 Length = AsciiStrLen(Buffer);
return (UINT8)SerialPortWrite((UINT8 *)Buffer, Length);
}
/*=============================================================================
* AssertFail - Print ASSERT message
*============================================================================*/
UINT64 AssertFail(UINT64 Filename, INT32 LineNumber, UINT64 Expression)
{
char Buffer[264];
InternalVSPrint((UINT8 *)Buffer, 256, 0,
"ASSERT [%a] %a(%d): %a\n",
(va_list)&Filename);
UINT64 Length = AsciiStrLen(Buffer);
return SerialPortWrite((UINT8 *)Buffer, Length);
}
/*=============================================================================
* DebugPrintWorker - format and print debug string
*============================================================================*/
UINT64 DebugPrintWorker(const char *Format, ...)
{
char Buffer[256];
va_list Va;
va_start(Va, Format);
InternalVSPrint((UINT8 *)Buffer, 256, 0, Format, Va);
UINT64 Length = AsciiStrLen(Buffer);
return SerialPortWrite((UINT8 *)Buffer, Length);
}
/*=============================================================================
* InternalVSPrint - Main format engine
*
* Implements a significant subset of the EDK II PrintLib format
* engine: handles %a, %s, %d, %x, %lx, %016lx, %r, and other
* standard format specifiers. Approximately 3.5KB of format
* parsing and conversion code.
*============================================================================*/
UINT64 InternalVSPrint(UINT8 *Buffer, UINT64 BufferSize,
UINT64 Flags, const char *Format,
va_list VaList)
{
/* Format string processing -- full implementation is ~3591 bytes
* of x64 decompiled code (0xFFE469F0). This is the standard
* EDK II BasePrintLib UnicodeVSPrint routine. */
/* (Implementation omitted for brevity -- see decompiled binary) */
return 0;
}
/*=============================================================================
* CapsuleX64Entry - Page fault handler entry
*
* Entry point for the capsule long-mode page fault handler.
* Called from PageFaultHandlerWrapper which sets MXCSR and
* dispatches to the actual page fault handler core.
*============================================================================*/
void CapsuleX64Entry(UINT64 a1, UINT64 a2, UINT64 a3, UINT64 a4)
{
/* Sets MXCSR to default (0x1F80), calls PageFaultHandlerCore,
* and if it returns nonzero, jumps to the saved return address;
* otherwise executes iretq to return from the interrupt. */
}
/*=============================================================================
* SetPageFaultHandled - Set page fault flag
*============================================================================*/
void SetPageFaultHandled(UINT8 Flag)
{
/* Sets byte at a1+1 to Flag value. Used to indicate whether
* 1GB pages should be used (Flag=0 => 2MB, Flag=1 => 1GB). */
}
/*=============================================================================
* InitPageFaultHandler - Initialize page fault dispatch
*============================================================================*/
void InitPageFaultHandler(void (**Table)())
{
Table[0] = (void (*)())0xFFE44A74; /* JUMPOUT_w / redirect target */
Table[1] = (void (*)())15; /* Page offset shift */
Table[2] = (void (*)())0xFFE44A74; /* Handler continue address */
}
/*=============================================================================
* AllocatePageTablePage - Allocate a 4KB page table
*============================================================================*/
UINT64 AllocatePageTablePage(PAGE_TABLE_CONTEXT *ptCtx, UINT64 *pPteAddr)
{
UINT64 PageTablePage = (UINT64)ptCtx->PageTableStack +
(ptCtx->PageTableStackIndex << 12);
ZeroMem((void *)PageTablePage, 4096);
UINT64 PhysMask = ptCtx->PhysicalAddressBits;
UINT64 *PreviousPte = (UINT64 *)ptCtx->PageTableStack[
ptCtx->PageTableStackIndex + 5];
if (PreviousPte && (*PreviousPte & ~PhysMask & ptCtx->PageMapLevel4Table) == PageTablePage)
*PreviousPte = 0;
*pPteAddr = PageTablePage | PhysMask | 3;
ptCtx->PageTableStack[ptCtx->PageTableStackIndex + 5] =
(UINT64)pPteAddr;
UINT64 result = ((UINT8)ptCtx->PageTableStackIndex + 1) & 7;
ptCtx->PageTableStackIndex = result;
return result;
}
/*=============================================================================
* PageFaultHandlerCore - X64 page fault handler
*
* Implements demand-paged page table allocation for capsule long mode.
* On a page fault, reads CR2 to get the faulting address, then
* walks the page table (PML4 -> PDPT -> PD), allocating new page
* table pages via AllocatePageTablePage if the entry is not present.
*
* Supports 2MB large pages (default). If the page fault flag is set,
* uses 1GB pages instead.
*============================================================================*/
UINT64 PageFaultHandlerCore(void)
{
IDTR Idtr;
AsmReadIdtr(&Idtr);
/* Get stack frame from IDT base */
UINT64 *pFrame = (UINT64 *)(Idtr.Base - 104);
UINT64 Cr2 = AsmReadCr2();
DebugPrint(0x80000000, "CapsuleX64 - PageFaultHandler: Cr2 - %lx\n", Cr2);
/* Check if Cr2 is within our managed range */
if (Cr2 >= *(UINT64 *)((char *)pFrame + 4096))
return *(UINT64 *)((char *)pFrame - 8);
UINT64 FaultAddr = *(UINT64 *)pFrame & Cr2;
UINT64 PhysAddrMask = *(UINT64 *)pFrame;
UINT64 Cr3Val = PhysAddrMask & AsmReadCr3();
UINT64 AttrMask = *(UINT64 *)((char *)pFrame - 80);
PAGE_TABLE_CONTEXT *ptCtx = (PAGE_TABLE_CONTEXT *)((char *)pFrame - 112);
UINT8 b1GBPage = *(UINT8 *)((char *)pFrame);
/* PML4 lookup (bits 47:39) */
UINT64 Pml4Index = BitFieldReadU64(FaultAddr, 39, 47);
UINT64 *Pml4Entry = (UINT64 *)(Cr3Val + 8 * Pml4Index);
if (!(*Pml4Entry & 1))
AllocatePageTablePage(ptCtx, Pml4Entry);
UINT64 PageDirPtrTable = PhysAddrMask & *Pml4Entry;
UINT64 PdptIndex = BitFieldReadU64(FaultAddr, 30, 38);
if (b1GBPage) {
/* 1GB page */
*(UINT64 *)(PageDirPtrTable + 8 * PdptIndex) =
(AttrMask | Cr2) & 0xFFFFFFFFC0000000ULL | 0x83;
} else {
/* 2MB page */
UINT64 *PdptEntry = (UINT64 *)(PageDirPtrTable + 8 * PdptIndex);
if (!(*PdptEntry & 1))
AllocatePageTablePage(ptCtx, PdptEntry);
UINT64 PageDirTable = PhysAddrMask & *PdptEntry;
UINT64 PdIndex = BitFieldReadU64(FaultAddr, 21, 29);
*(UINT64 *)(PageDirTable + 8 * PdIndex) =
(AttrMask | Cr2) & 0xFFFFFFFFFFE00000ULL | 0x83;
}
return 0;
}
/*=============================================================================
* PageFaultHandlerWrapper
*============================================================================*/
void PageFaultHandlerWrapper(UINT64 a1, UINT64 a2,
UINT64 a3, UINT64 a4)
{
/* Set MXCSR to default */
__asm__ volatile("ldmxcsr %0" :: "m"(0x00001F80));
if (PageFaultHandlerCore())
return;
__asm__ volatile("iretq");
}
/*=============================================================================
* ValidateMemoryResource - check address range
*============================================================================*/
BOOLEAN ValidateMemoryResource(
MEMORY_RESOURCE_DESCRIPTOR *MemResources,
UINT64 Address, UINT64 Size)
{
if (Address > ~Size) {
DebugPrint(0x80000000,
"ERROR: Address(0x%lx) > (MAX_ADDRESS - Size(0x%lx))\n",
Address, Size);
return FALSE;
}
if (!MemResources || !MemResources[0].Length)
return TRUE;
UINT64 Index = 0;
while (MemResources[Index].Length) {
if (Address >= MemResources[Index].BaseAddress &&
Address + Size <= MemResources[Index].BaseAddress +
MemResources[Index].Length) {
DebugPrint(64,
"Address(0x%lx) Size(0x%lx) in MemoryResource[0x%x] "
"- Start(0x%lx) Length(0x%lx)\n",
Address, Size, Index,
MemResources[Index].BaseAddress,
MemResources[Index].Length);
return TRUE;
}
Index++;
}
DebugPrint(0x80000000,
"ERROR: Address(0x%lx) Size(0x%lx) not in any MemoryResource\n",
Address, Size);
return FALSE;
}
/*=============================================================================
* ValidateCapsuleIntegrity
*============================================================================*/
UINT64 ValidateCapsuleIntegrity(UINT64 BlockList,
MEMORY_RESOURCE_DESCRIPTOR *MemResources)
{
DebugPrint(64, "ValidateCapsuleIntegrity\n");
UINT64 TotalCapsuleSize = 0;
UINT64 CapsuleCount = 0;
if (!ValidateMemoryResource(MemResources, BlockList, 16))
return 0;
DebugPrint(64, "Ptr - 0x%x\n", BlockList);
while (1) {
EFI_CAPSULE_BLOCK_DESCRIPTOR *Desc =
(EFI_CAPSULE_BLOCK_DESCRIPTOR *)BlockList;
DebugPrint(64, "Ptr->Length - 0x%x\n", Desc->Length);
DebugPrint(64, "Ptr->Union - 0x%x\n", Desc->Union.DataBlock);
if (!Desc->Length && !Desc->Union.DataBlock)
break;
if (BlockList & 7) {
DebugPrint(0x80000000,
"ERROR: BlockList address failed alignment check\n");
return 0;
}
if (Desc->Length) {
if (!ValidateMemoryResource(MemResources,
Desc->Union.DataBlock,
Desc->Length))
return 0;
if (!TotalCapsuleSize) {
EFI_CAPSULE_HEADER *CapsuleHeader =
(EFI_CAPSULE_HEADER *)(UINT64)Desc->Union.DataBlock;
if (Desc->Length < sizeof(EFI_CAPSULE_HEADER)) {
DebugPrint(0x80000000,
"ERROR: Ptr->Length(0x%lx) < sizeof(EFI_CAPSULE_HEADER)\n",
Desc->Length);
return 0;
}
if (CapsuleHeader->HeaderSize >
CapsuleHeader->CapsuleImageSize) {
DebugPrint(0x80000000,
"ERROR: CapsuleHeader->HeaderSize(0x%x) > "
"CapsuleHeader->CapsuleImageSize(0x%x)\n",
CapsuleHeader->HeaderSize,
CapsuleHeader->CapsuleImageSize);
return 0;
}
if (!(CapsuleHeader->Flags & 0x10000) ||
(CapsuleHeader->Flags & 0x30000) == 0x20000 ||
(CapsuleHeader->Flags & 0x50000) == 0x40000)
return 0;
CapsuleCount++;
TotalCapsuleSize = CapsuleHeader->CapsuleImageSize;
}
if (TotalCapsuleSize < Desc->Length) {
DebugPrint(0x80000000,
"ERROR: CapsuleSize(0x%lx) < Ptr->Length(0x%lx)\n",
TotalCapsuleSize, Desc->Length);
return 0;
}
TotalCapsuleSize -= Desc->Length;
BlockList += 16;
if (!ValidateMemoryResource(MemResources, BlockList, 16))
return 0;
DebugPrint(64, "Ptr(B) - 0x%x\n", BlockList);
} else {
BlockList = Desc->Union.DataBlock;
if (!ValidateMemoryResource(MemResources, BlockList, 16))
return 0;
DebugPrint(64, "Ptr(C) - 0x%x\n", BlockList);
}
}
if (!CapsuleCount) {
DebugPrint(0x80000000, "ERROR: CapsuleCount(0x%x) == 0\n", 0);
return 0;
}
if (TotalCapsuleSize) {
DebugPrint(0x80000000, "ERROR: CapsuleSize(0x%lx) != 0\n",
TotalCapsuleSize);
return 0;
}
return BlockList;
}
/*=============================================================================
* GetCapsuleInfo - Get capsule count/size
*============================================================================*/
EFI_STATUS GetCapsuleInfo(EFI_CAPSULE_BLOCK_DESCRIPTOR *Desc,
UINT64 *CapsuleCount, UINT64 *TotalLength,
UINT64 *CapsuleNumber)
{
DebugPrint(64, "GetCapsuleInfo enter\n");
if (!Desc) {
AssertFail((UINT64)"e:\\hs\\MdeModulePkg\\Universal\\CapsulePei"
"\\Common\\CapsuleCoalesce.c", 656,
(UINT64)"Desc != ((void *) 0)");
}
EFI_CAPSULE_BLOCK_DESCRIPTOR *p = Desc;
UINT64 Count = 0;
UINT64 TotalSize = 0;
UINT64 NumCapsules = 0;
UINT64 ThisCapsuleImageSize = 0;
while (p->Union.DataBlock) {
if (p->Length) {
if (p->Length >= ~TotalSize) {
DebugPrint(0x80000000,
"ERROR: Desc->Length(0x%lx) >= (MAX_ADDRESS - Size(0x%x))\n",
p->Length, (UINT32)TotalSize);
return EFI_OUT_OF_RESOURCES;
}
TotalSize += p->Length;
Count++;
if (!ThisCapsuleImageSize) {
EFI_CAPSULE_HEADER *CapsuleHeader =
(EFI_CAPSULE_HEADER *)(UINT64)p->Union.DataBlock;
ThisCapsuleImageSize = CapsuleHeader->CapsuleImageSize;
NumCapsules++;
}
if (ThisCapsuleImageSize < p->Length) {
AssertFail((UINT64)"e:\\hs\\MdeModulePkg\\Universal"
"\\CapsulePei\\Common\\CapsuleCoalesce.c",
710,
(UINT64)"ThisCapsuleImageSize >= Desc->Length");
}
ThisCapsuleImageSize -= p->Length;
p++;
} else {
if (p == (EFI_CAPSULE_BLOCK_DESCRIPTOR *)
(UINT64)Desc->Union.DataBlock)
goto error;
p = (EFI_CAPSULE_BLOCK_DESCRIPTOR *)(UINT64)Desc->Union.DataBlock;
}
}
if (!Count)
goto error;
if (ThisCapsuleImageSize) {
AssertFail((UINT64)"e:\\hs\\MdeModulePkg\\Universal"
"\\CapsulePei\\Common\\CapsuleCoalesce.c",
730, (UINT64)"ThisCapsuleImageSize == 0");
}
if (CapsuleCount) *CapsuleCount = Count;
if (TotalLength) *TotalLength = TotalSize;
if (CapsuleNumber) *CapsuleNumber = NumCapsules;
return EFI_SUCCESS;
error:
DebugPrint(0x80000000, "ERROR: Count == 0\n");
return EFI_NOT_FOUND;
}
/*=============================================================================
* FindFreeMemoryInDescriptors
*
* Scans a memory resource descriptor array to find a free region
* large enough to hold a buffer of Size bytes. Handles overlapping
* descriptors and relocation fixups.
*============================================================================*/
UINT64 *FindFreeMemoryInDescriptors(UINT64 *ResourceArray,
UINT64 *Target,
UINT64 Size,
UINT64 SourceBase)
{
UINT64 *TargetEnd = (UINT64 *)((UINT8 *)Target + Size - (UINT64)Target);
UINT64 *Resource = ResourceArray;
for (; Resource != NULL; ) {
if (!Resource)
return Target;
UINT64 EntryCount = 2;
if (*Resource) {
do
EntryCount += 2;
while (Resource[EntryCount - 2]);
}
if ((UINT64 *)((UINT8 *)Target + (UINT64)Target) > Resource) {
UINT64 *NextEntry = (UINT64 *)&Resource[EntryCount];
if (Target < &Resource[EntryCount]) {
Target = &Resource[EntryCount];
Resource = ResourceArray;
if ((UINT64)NextEntry <= (UINT64)TargetEnd)
continue;
return 0;
}
}
while (1) {
UINT64 RangeBase = *Resource;
if (!RangeBase)
break;
UINT64 RangeSize = Resource[1];
if ((UINT64)Target + (UINT64)Target > RangeSize &&
(UINT64)Target < RangeBase + RangeSize) {
Target = (UINT64 *)(RangeSize + RangeBase);
Resource = ResourceArray;
if ((UINT64)(RangeSize + RangeBase) > (UINT64)TargetEnd)
return 0;
break;
}
Resource += 2;
}
if (!*Resource)
Resource = (UINT64 *)Resource[1];
}
return Target;
}
/*=============================================================================
* RelocateCapsuleDescriptors
*============================================================================*/
UINT64 *RelocateCapsuleDescriptors(
EFI_CAPSULE_BLOCK_DESCRIPTOR *Desc,
UINT64 *FlatBuffer,
UINT64 DescriptorCount,
UINT64 *ScratchBuffer,
UINT64 ScratchSize)
{
UINT64 TotalSize = 16 * DescriptorCount;
UINT64 *Cursor = (UINT64 *)Desc;
UINT64 FlatEnd = (UINT64)&FlatBuffer[TotalSize / 8];
if (ScratchSize < TotalSize)
return 0;
/* Walk linked list, fix up overlapping descriptor data */
while (Cursor[1]) {
if (*Cursor) {
UINT64 *Source = (UINT64 *)Cursor[1];
if ((UINT64 *)FlatEnd > Cursor[1] &&
FlatBuffer < (UINT64 *)((char *)Source + *Cursor)) {
UINT64 *Destination = FindFreeMemoryInDescriptors(
(UINT64 *)Cursor, (UINT64 *)FlatEnd,
ScratchSize - TotalSize, *Cursor);
if (!Destination)
return 0;
CopyMem(Destination, Source, *Cursor);
DebugPrint(64,
"Capsule relocate descriptors from/to/size "
"0x%lX 0x%lX 0x%lX\n",
(UINT64)Cursor[1], (UINT64)Destination, *Cursor);
Cursor[1] = (UINT64)Destination;
}
Cursor += 2;
} else {
Cursor = (UINT64 *)Cursor[1];
}
}
/* Second pass: fix up descriptor block addresses */
UINT64 *Walker = (UINT64 *)Cursor;
UINT64 *Previous = 0;
while (Walker) {
if (!Walker[1])
break;
UINT64 *Tail = Walker;
UINT64 Index;
for (Index = 2; *Tail; Index += 2)
Tail += 2;
if ((UINT64 *)FlatEnd > Walker && FlatBuffer < &Walker[Index]) {
UINT64 *Relocated = FindFreeMemoryInDescriptors(
(UINT64 *)Cursor, (UINT64 *)FlatEnd,
ScratchSize - TotalSize, Index * 8);
if (!Relocated)
return 0;
CopyMem(Relocated, Walker, Index * 8);
DebugPrint(64, "Capsule reloc descriptor block #2\n");
if (Previous)
Previous[1] = (UINT64)Relocated;
else
Cursor = Relocated;
}
Walker = (UINT64 *)Tail[1];
Previous = Tail;
}
/* Flatten to output buffer */
UINT64 *Output = FlatBuffer;
while (Cursor) {
UINT64 Next = Cursor[1];
if (!Next)
break;
if (*Cursor) {
Output[1] = Next;
*Output = *Cursor;
Output += 2;
Cursor += 2;
} else {
Cursor = (UINT64 *)Cursor[1];
}
}
Output[1] = 0;
*Output = 0;
return FlatBuffer;
}
/*=============================================================================
* DumpExceptionContext - dump CPU context
*============================================================================*/
UINT64 DumpExceptionContext(void *ExceptionType,
UINT64 *ContextRecord)
{
UINT64 ApicId = GetApicId();
DebugPrintWorker(
"!!!! X64 Exception Type - %02x(%a) CPU Apic ID - %08x !!!!\n",
(UINT32)(UINT64)ExceptionType, ExceptionType, ApicId);
if ((1 << (UINT32)(UINT64)ExceptionType) & 0x27D00) {
DebugPrintWorker("ExceptionData - %016lx", *ContextRecord);
if (ExceptionType == (void *)14) {
DebugPrintWorker(
" I:%x R:%x U:%x W:%x P:%x PK:%x S:%x",
(*ContextRecord >> 4) & 1,
(*ContextRecord >> 3) & 1,
(*ContextRecord >> 2) & 1,
(*ContextRecord >> 1) & 1,
*ContextRecord & 1,
(*ContextRecord >> 5) & 1,
(*ContextRecord >> 15) & 1);
}
DebugPrintWorker("\n");
}
DebugPrintWorker("RIP - %016lx, CS - %016lx, RFLAGS - %016lx\n",
ContextRecord[84], ContextRecord[89], ContextRecord[77]);
DebugPrintWorker("RAX - %016lx, RCX - %016lx, RDX - %016lx\n",
ContextRecord[98], ContextRecord[97], ContextRecord[96]);
DebugPrintWorker("RBX - %016lx, RSP - %016lx, RBP - %016lx\n",
ContextRecord[95], ContextRecord[94], ContextRecord[93]);
DebugPrintWorker("RSI - %016lx, RDI - %016lx\n",
ContextRecord[92], ContextRecord[91]);
DebugPrintWorker("R8 - %016lx, R9 - %016lx, R10 - %016lx\n",
ContextRecord[99], ContextRecord[100], ContextRecord[101]);
DebugPrintWorker("R11 - %016lx, R12 - %016lx, R13 - %016lx\n",
ContextRecord[102], ContextRecord[103], ContextRecord[104]);
DebugPrintWorker("R14 - %016lx, R15 - %016lx\n",
ContextRecord[105], ContextRecord[106]);
DebugPrintWorker("DS - %016lx, ES - %016lx, FS - %016lx\n",
ContextRecord[88], ContextRecord[87], ContextRecord[86]);
DebugPrintWorker("GS - %016lx, SS - %016lx\n",
ContextRecord[85], ContextRecord[90]);
DebugPrintWorker("CR0 - %016lx, CR2 - %016lx, CR3 - %016lx\n",
ContextRecord[71], ContextRecord[73], ContextRecord[74]);
DebugPrintWorker("CR4 - %016lx, CR8 - %016lx\n",
ContextRecord[75], ContextRecord[76]);
DebugPrintWorker("DR0 - %016lx, DR1 - %016lx, DR2 - %016lx\n",
ContextRecord[65], ContextRecord[66], ContextRecord[67]);
DebugPrintWorker("DR3 - %016lx, DR6 - %016lx, DR7 - %016lx\n",
ContextRecord[68], ContextRecord[69], ContextRecord[70]);
DebugPrintWorker("GDTR - %016lx %016lx, LDTR - %016lx\n",
ContextRecord[80], ContextRecord[81], ContextRecord[78]);
DebugPrintWorker("IDTR - %016lx %016lx, TR - %016lx\n",
ContextRecord[82], ContextRecord[83], ContextRecord[79]);
return DebugPrintWorker("FXSAVE_STATE - %016lx\n",
(UINT64)ContextRecord + 8);
}
/*=============================================================================
* X64ExceptionHandler
*============================================================================*/
void X64ExceptionHandler(UINT64 ExceptionType,
UINT64 *ContextRecord)
{
FindAndLogImageInfo((void *)ExceptionType, ContextRecord);
}
/*=============================================================================
* FindAndLogImageInfo - find PE/COFF from RIP
*============================================================================*/
void FindAndLogImageInfo(void *ExceptionType,
UINT64 *ContextRecord)
{
DumpExceptionContext(ExceptionType, ContextRecord);
UINT64 *Rip = (UINT64 *)ContextRecord[84];
char *v3 = (char *)((UINT64)Rip & 0xFFFFFFFFFFFFFFFCULL);
if (!v3)
goto not_found;
while (1) {
if (*(UINT16 *)v3 == 0x5A4D) {
char *v4 = &v3[*(UINT16 *)(v3 + 60)];
if (v4 > v3 && (UINT64)v4 < ContextRecord[84]) {
if (*(UINT32 *)v4 == 0x00004550)
goto found;
}
} else if (*(UINT16 *)v3 == 0x4550) {
UINT16 n332 = *(UINT16 *)(v3 + 2);
if (n332 == 0x14C || n332 == 0x200 ||
n332 == 0xEC0 || n332 == 0x8664 ||
n332 == 0xAA64 || n332 == 0x1C2)
goto found;
}
v3 -= 4;
if (!v3)
goto found;
}
found:
if (v3) {
DebugPrintWorker("!!!! Find image ");
char *pdbPath;
if (*(UINT16 *)v3 == 0x5A4D)
pdbPath = &v3[*(UINT16 *)(v3 + 60)];
else
pdbPath = v3;
UINT64 EntryPoint;
if (*(UINT16 *)pdbPath == 0x4550)
EntryPoint = (UINT64)&v3[*(UINT32 *)(pdbPath + 8) -
*(UINT16 *)(pdbPath + 6) + 40];
else if (*(UINT32 *)pdbPath == 0x00004550)
EntryPoint = (UINT64)&v3[*(UINT32 *)(pdbPath + 40)];
else
EntryPoint = 0;
char *Pdb = PeCoffGetPdbPath(v3);
if (Pdb)
DebugPrintWorker("%a", Pdb);
else
DebugPrintWorker("(No PDB) ");
DebugPrintWorker(" (ImageBase=%016lp, EntryPoint=%016p) !!!!\n",
v3, EntryPoint);
for (;;)
;
}
not_found:
DebugPrintWorker("!!!! Can't find image information. !!!!\n");
for (;;)
;
}
/*=============================================================================
* PeCoffGetPdbPath - Get PDB path from PE/COFF
*============================================================================*/
char *PeCoffGetPdbPath(char *Pe32Data)
{
char *v6;
if (!Pe32Data) {
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BasePeCoffGetEntryPointLib"
"\\PeCoffGetEntryPoint.c", 166,
(UINT64)"Pe32Data != ((void *) 0)");
}
if (*(UINT16 *)Pe32Data == 0x5A4D)
v6 = &Pe32Data[*(UINT16 *)(Pe32Data + 60)];
else
v6 = Pe32Data;
if (*(UINT16 *)v6 != 0x4550) {
if (*(UINT32 *)v6 != 0x00004550)
return 0;
UINT16 Machine = *(UINT16 *)(v6 + 4);
if (!(Machine == 0x14C || Machine == 0x200 ||
Machine == 0x8664 || Machine == 0x869C))
return 0;
}
UINT32 *DirEntry = (UINT32 *)(v6 + 168);
UINT32 NumDir = *(UINT32 *)(v6 + 116);
if (NumDir <= 6)
return 0;
UINT64 DirAddr = (UINT64)DirEntry[0];
UINT64 DirSize = (UINT64)DirEntry[1];
if (!DirAddr || !DirSize)
return 0;
for (UINT64 i = 0; i < DirSize; i += 28) {
UINT32 *pDbgEntry = (UINT32 *)((char *)Pe32Data + DirAddr + i);
if (pDbgEntry[3] == 2 && pDbgEntry[4]) {
char *Entry = &Pe32Data[pDbgEntry[5]];
if (*(UINT32 *)Entry == 0x53445352) /* RSDS */
return Entry + 20;
if (*(UINT32 *)Entry == 0x5342474D) /* NB10 */
return (char *)pDbgEntry + 16;
}
}
return 0;
}
/*=============================================================================
* GetApicId - Get APIC ID
*============================================================================*/
UINT64 GetApicId(void)
{
UINT32 Eax, Ebx, Ecx, Edx;
AsmCpuid(1, &Eax, &Ebx, 0, 0);
return (Ebx >> 24) & 0xFF;
}
/*=============================================================================
* ModuleEntryPoint - Main entry point
*
* Called by PEI dispatcher. Saves IDT, sets up page fault handler
* IDT entries, validates capsule integrity, coalesces capsule data,
* restores IDT, and calls AsmDisablePaging64 to enter capsule
* long-mode runtime.
*============================================================================*/
EFI_STATUS ModuleEntryPoint(EFI_HANDLE ImageHandle,
EFI_SYSTEM_TABLE *SystemTable)
{
IDTR Idtr;
UINT16 n543[8];
UINT8 v33[544];
UINT8 *Scratch = v33;
UINT64 IdtTable[4];
UINT8 v26;
UINT64 v27, v28, v29, v30;
char v31[64];
UINT32 n36_1;
UINT8 n36;
UINT16 v13;
UINT64 v14;
/* Save current IDT */
AsmReadIdtr(&Idtr);
/* Zero scratch buffer */
ZeroMem(v33, 544);
/* Load new IDT with limit = 543 (68 entries) */
n543[0] = 543;
AsmWriteIdtr(n543);
/* Read current IDT */
AsmReadIdtr(&Idtr);
UINT64 n32 = ((UINT64)Idtr.Limit + 1) >> 4;
if (n32 > 0x20)
n32 = 32;
UINT16 v6 = AsmReadCs();
InitPageFaultHandler((void (**)())IdtTable);
/* Fill IDT entries with page fault handler */
UINT64 n32_1 = 0;
if (n32) {
UINT64 v8 = (UINT64)&Idtr.Base + 6;
do {
*(UINT16 *)(v8 - 4) = v6; /* Segment selector = CS */
UINT64 v9 = n32_1 * (UINT64)IdtTable[1];
n32_1++;
UINT64 v10 = (UINT64)IdtTable[0] + v9;
*(UINT16 *)(v8 - 6) = (UINT16)v10;
*(UINT16 *)v8 = (UINT16)(v10 >> 16);
*(UINT32 *)(v8 + 2) = (UINT32)(v10 >> 32);
*(UINT8 *)(v8 - 1) = 0x8E; /* 32-bit interrupt gate */
v8 += 16;
} while (n32_1 < n32);
}
v26 = *((UINT8 *)ImageHandle + 64);
v30 = *((UINT64 *)((UINT8 *)ImageHandle + 65));
/* Get physical address width */
AsmCpuid(0x80000000, &n36_1, 0, 0, 0);
if (n36_1 < 0x80000008) {
n36 = 36;
} else {
AsmCpuid(0x80000008, &n36_1, 0, 0, 0);
n36 = (UINT8)n36_1;
}
v27 = (InternalLShiftU64(1, n36) - 1) & 0xFFFFFFFFF000ULL;
/* Set page fault handler address in scratch */
*((UINT16 *)Scratch + 112) = (UINT16)(UINT64)PageFaultHandlerWrapper;
v13 = AsmReadCs();
*((UINT32 *)Scratch + 59) = 0;
*((UINT16 *)Scratch + 113) = v13;
*((UINT16 *)Scratch + 115) = (UINT64)PageFaultHandlerWrapper >> 16;
*((UINT16 *)Scratch + 114) = 0x8E00;
*((UINT32 *)Scratch + 58) = (UINT64)PageFaultHandlerWrapper >> 32;
if (v26)
v14 = (v27 & AsmReadCr3()) + 0x2000;
else
v14 = (v27 & AsmReadCr3()) + 24576;
v29 = 0;
v28 = v14;
ZeroMem(v31, 64);
/* Coalesce the capsule data */
CapsuleCoalesce(
(UINT64)&SystemTable->Hdr.Revision + 2,
(UINT64 *)*((UINT64 *)ImageHandle + 4),
(UINT64 *)*((UINT64 *)ImageHandle + 5),
(UINT64 **)*((UINT64 *)ImageHandle + 6),
(UINT32 *)*((UINT64 *)ImageHandle + 7));
DebugPrint(64, "%a() Stack Base: 0x%lx, Stack Size: 0x%lx\n",
"ModuleEntryPoint",
*((UINT64 *)ImageHandle + 1),
*((UINT64 *)ImageHandle + 2));
/* Restore original IDT */
AsmWriteIdtr((UINT16 *)&Idtr);
UINT32 v16 = *((UINT32 *)ImageHandle + 4) +
*((UINT32 *)ImageHandle + 2);
UINT32 v17 = *(UINT32 *)((char *)&SystemTable->Hdr.Signature + 2);
UINT16 Signature = SystemTable->Hdr.Signature;
if (!v17)
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseLib\\"
"X86DisablePaging64.c", 60,
(UINT64)"EntryPoint != 0");
if (!v16)
AssertFail((UINT64)"e:\\hs\\MdePkg\\Library\\BaseLib\\"
"X86DisablePaging64.c", 61,
(UINT64)"NewStack != 0");
AsmDisablePaging64(Signature, v17,
(EFI_HANDLE)(UINT32)ImageHandle,
SystemTable, v16);
AssertFail((UINT64)"e:\\hs\\MdeModulePkg\\Universal\\"
"CapsulePei\\X64\\X64Entry.c", 309,
(UINT64)"((BOOLEAN)(0==1))");
return EFI_SUCCESS;
}