/** @file
 * CpuIoPei - CPU I/O PEI Module
 *
 * Source: UefiCpuPkg/CpuIoPei/CpuIoPei.c
 * This PEIM provides CPU I/O, MMIO, and port I/O access services
 * via the EFI_PEI_CPU_IO_PPI interface during the PEI phase.
 *
 * Decompiled from: HR650X BIOS, CpuIoPei.efi (PE32+ x86)
 * MD5:  f486e28500aeebac3792779ab72777cb
 * SHA256: 8bad4b7e3743a5a8074efd6de47d732e32e4bd1ccff56bb36cdc90aea7ae91bd
 * Image base: 0xffc18f74
 * Image size: 0x14a0
 * Image arch: 32-bit (Ia32)
 */

#include "CpuIoPei.h"

//
// Library helper: memset via compiler intrinsic
//
void * SetMem(void *buf, unsigned int count, char value)
{
  memset(buf, value, count);
  return buf;
}

//
// Library helper: memset32 via compiler intrinsic
//
void * SetMem32(void *buf, unsigned int count, int value)
{
  memset32(buf, value, count);
  return buf;
}

//
// Library helper: memset32 for 2-dword fill pattern
//
int  SetMem32Fill(int Buf, int Count, int Fill0, int Fill1)
{
  do
  {
    *(_DWORD *)(Buf + 8 * Count - 8) = Fill0;
    *(_DWORD *)(Buf + 8 * Count-- - 4) = Fill1;
  }
  while ( Count );
  return Buf;
}

//
// Library helper: memcpy with overlap handling
//
char * CopyMem(char *dst, char *src, unsigned int count)
{
  unsigned int count_1;
  char *dst_1;
  char *src_1;

  count_1 = count;
  if ( src < dst && &src[count - 1] >= dst )
  {
    // Overlapping: copy backwards
    src_1 = &src[count - 1];
    dst_1 = &dst[count - 1];
  }
  else
  {
    // No overlap: copy dword-aligned first, then remainder
    count_1 = count & 3;
    qmemcpy(dst, src, 4 * (count >> 2));
    src_1 = &src[4 * (count >> 2)];
    dst_1 = &dst[4 * (count >> 2)];
  }
  qmemcpy(dst_1, src_1, count_1);
  return dst;
}

//
// I/O Port Read - Byte string
//
__int64  IoReadBufferFx(unsigned __int16 Port, unsigned int Count, unsigned __int8 *Buffer)
{
  __int64 result;
  WORD2(result) = Port;
  __inbytestring(Port, Buffer, Count);
  return result;
}

//
// I/O Port Read - Word string
//
__int64  IoReadPortWordString(unsigned __int16 Port, unsigned int Count, unsigned __int16 *Buffer)
{
  __int64 result;
  WORD2(result) = Port;
  __inwordstring(Port, Buffer, Count);
  return result;
}

//
// I/O Port Read - Dword string
//
__int64  IoReadPortDwordString(unsigned __int16 Port, unsigned int Count, unsigned int *Buffer)
{
  __int64 result;
  WORD2(result) = Port;
  __indwordstring(Port, Buffer, Count);
  return result;
}

//
// I/O Port Write - Byte string
//
__int64  IoWriteBufferByte(unsigned __int16 Port, unsigned int Count, unsigned __int8 *Buffer)
{
  __int64 result;
  WORD2(result) = Port;
  __outbytestring(Port, Buffer, Count);
  return result;
}

//
// I/O Port Write - Word string
//
__int64  IoWritePortWordString(unsigned __int16 Port, unsigned int Count, unsigned __int16 *Buffer)
{
  __int64 result;
  WORD2(result) = Port;
  __outwordstring(Port, Buffer, Count);
  return result;
}

//
// I/O Port Write - Dword string
//
__int64  IoWritePortDwordString(unsigned __int16 Port, unsigned int Count, unsigned int *Buffer)
{
  __int64 result;
  WORD2(result) = Port;
  __outdwordstring(Port, Buffer, Count);
  return result;
}

//
// I/O Port Read - Word (single, aligned)
//
int  IoReadPortWord(unsigned __int16 Port)
{
  int DebugInstance;
  int result;

  if ( (Port & 1) != 0 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseIoLibIntrinsic\\IoLibMsc.c",
        133,
        "(Port & 1) == 0");
  }
  LOWORD(result) = __inword(Port);
  return (unsigned __int16)result;
}

//
// I/O Port Write - Word (single, aligned)
//
unsigned __int16  IoWritePortWord(unsigned __int16 Port, unsigned __int16 Value)
{
  int DebugInstance;

  if ( (Port & 1) != 0 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseIoLibIntrinsic\\IoLibMsc.c",
        163,
        "(Port & 1) == 0");
  }
  __outword(Port, Value);
  return Value;
}

//
// I/O Port Read - Dword (single, aligned)
//
unsigned __int32  IoReadPortDword(unsigned __int16 Port)
{
  int DebugInstance;

  if ( (Port & 3) != 0 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseIoLibIntrinsic\\IoLibMsc.c",
        193,
        "(Port & 3) == 0");
  }
  return __indword(Port);
}

//
// I/O Port Write - Dword (single, aligned)
//
unsigned int  IoWritePortDword(unsigned __int16 Port, unsigned int Value)
{
  int DebugInstance;

  if ( (Port & 3) != 0 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseIoLibIntrinsic\\IoLibMsc.c",
        223,
        "(Port & 3) == 0");
  }
  __outdword(Port, Value);
  return Value;
}

//
// MMIO Read - Word (aligned)
//
int  IoReadMmioWord(unsigned __int16 *Address)
{
  int DebugInstance;

  if ( ((unsigned __int8)Address & 1) != 0 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseIoLibIntrinsic\\IoLib.c",
        151,
        "(Address & 1) == 0");
  }
  return *Address;
}

//
// MMIO Write - Word (aligned)
//
__int16  IoWriteMmioWord(_WORD *Address, __int16 Value)
{
  int DebugInstance;

  if ( ((unsigned __int8)Address & 1) != 0 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseIoLibIntrinsic\\IoLib.c",
        183,
        "(Address & 1) == 0");
  }
  *Address = Value;
  return Value;
}

//
// MMIO Read - Qword (aligned)
//
__int64  IoReadMmioQword(void *Address)
{
  int DebugInstance;

  if ( ((unsigned __int8)Address & 7) != 0 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseIoLibIntrinsic\\IoLib.c",
        284,
        "(Address & 7) == 0");
  }
  return *(_QWORD *)Address;
}

//
// MMIO Write - Qword (aligned)
//
int  IoWriteMmioQword(void *Address, int Lo32, int Hi32)
{
  int DebugInstance;

  if ( ((unsigned __int8)Address & 7) != 0 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseIoLibIntrinsic\\IoLib.c",
        314,
        "(Address & 7) == 0");
  }
  *(_DWORD *)Address = Lo32;
  *((_DWORD *)Address + 1) = Hi32;
  return Lo32;
}

//
// Unaligned Read - 16-bit
//
__int16 __thiscall ReadUnaligned16(void *Buffer)
{
  int DebugInstance;

  if ( !Buffer )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseLib\\Unaligned.c",
        38,
        "Buffer != ((void *) 0)");
  }
  return *(_WORD *)Buffer;
}

//
// Unaligned Write - 16-bit
//
__int16  WriteUnaligned16(_WORD *Buffer, __int16 Value)
{
  int DebugInstance;

  if ( !Buffer )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseLib\\Unaligned.c",
        65,
        "Buffer != ((void *) 0)");
  }
  *Buffer = Value;
  return Value;
}

//
// Unaligned Read - 32-bit
//
int __thiscall ReadUnaligned32(void *Buffer)
{
  int DebugInstance;

  if ( !Buffer )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseLib\\Unaligned.c",
        141,
        "Buffer != ((void *) 0)");
  }
  return *(_DWORD *)Buffer;
}

//
// Unaligned Write - 32-bit
//
int  WriteUnaligned32(_DWORD *Buffer, int Value)
{
  int DebugInstance;

  if ( !Buffer )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseLib\\Unaligned.c",
        168,
        "Buffer != ((void *) 0)");
  }
  *Buffer = Value;
  return Value;
}

//
// Unaligned Read - 64-bit
//
__int64 __thiscall ReadUnaligned64(void *Buffer)
{
  int DebugInstance;

  if ( !Buffer )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseLib\\Unaligned.c",
        192,
        "Buffer != ((void *) 0)");
  }
  return *(_QWORD *)Buffer;
}

//
// Unaligned Write - 64-bit
//
int  WriteUnaligned64(_DWORD *Buffer, int Lo32, int Hi32)
{
  int DebugInstance;

  if ( !Buffer )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseLib\\Unaligned.c",
        219,
        "Buffer != ((void *) 0)");
  }
  *Buffer = Lo32;
  *(Buffer + 1) = Hi32;
  return Lo32;
}

//
// 64-bit Left Shift
//
__int64  LShiftU64(__int64 Value)
{
  unsigned int Count;
  int DebugInstance;
  __int64 Result;

  if ( Count >= 0x40 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseLib\\LShiftU64.c",
        39,
        "Count < 64");
  }
  LODWORD(Result) = 0;
  HIDWORD(Result) = Value;
  if ( (Count & 0x20) == 0 )
    Result = Value;
  return Result << (Count & 0x1F);
}

//
// 64-bit Right Shift
//
unsigned __int64  RShiftU64(unsigned __int64 Value)
{
  unsigned int Count;
  int DebugInstance;
  unsigned __int64 Result;

  if ( Count >= 0x40 )
  {
    DebugInstance = GetDebugInstance();
    if ( DebugInstance )
      (*(void ( **)(const char *, int, const char *))(DebugInstance + 4))(
        "e:\\hs\\MdePkg\\Library\\BaseLib\\RShiftU64.c",
        39,
        "Count < 64");
  }
  Result = HIDWORD(Value);
  if ( (Count & 0x20) == 0 )
    Result = Value;
  return Result >> (Count & 0x1F);
}

//
// CMOS Check Mode: determine debug printing capabilities
// Selects RTC register 0x4A to read the debug mode.
// Returns 0 (no debug), negative error codes, or status.
//
int CmosCheckMode()
{
  unsigned __int8 Save;
  char Mode;
  char Mode2;

  Save = __inbyte(0x70u);
  __outbyte(0x70u, Save & 0x80 | 0x4Au);
  Mode = __inbyte(0x71u);
  Mode2 = Mode;
  if ( (unsigned __int8)Mode <= 3u )
  {
LABEL_4:
    if ( !Mode2 )
      return 0;
    goto LABEL_5;
  }
  Mode2 = Mode;
  if ( !Mode )
  {
    Mode2 = MEMORY[0xFDAF0490] & 2 | 1;
    goto LABEL_4;
  }
LABEL_5:
  if ( Mode2 != -1 )
    return Mode2 != 1 ? -2147483578 : -2147483644;
  return 0;
}

//
// Get PEI Services Table Pointer
// Retrieves via IDT entry at offset -4 from IDT base.
//
int GetPeiServicesTable()
{
  int PeiServices;
  _BYTE Idtr[2];
  int IdtBase;

  ReadIdtr(Idtr);
  PeiServices = *(_DWORD *)(IdtBase - 4);
  if ( !PeiServices )
    DebugAssert(
      (int)"e:\\hs\\MdePkg\\Library\\PeiServicesTablePointerLibIdt\\PeiServicesTablePointer.c",
      48,
      (int)"PeiServices != ((void *) 0)");
  return PeiServices;
}

//
// Read IDTR (SIDT instruction)
//
void *__thiscall ReadIdtr(void *Idtr)
{
  if ( !Idtr )
    DebugAssert(
      (int)"e:\\hs\\MdePkg\\Library\\BaseLib\\X86ReadIdtr.c",
      37,
      (int)"Idtr != ((void *) 0)");
  __sidt(Idtr);
  return Idtr;
}

//
// Get Debug Instance from PEI Services
// Locates the gEfiPeiDebug2PpiGuid PPI.
//
int GetDebugInstance()
{
  int PeiServicesTable;
  _BYTE v2[4];
  int DebugInstance;

  PeiServicesTable = GetPeiServicesTable();
  if ( (*(int ( **)(int, void *, _DWORD, _BYTE *, int *))(*(_DWORD *)PeiServicesTable + 32))(
         PeiServicesTable,
         &gPeiDebug2PpiGuid,
         0,
         v2,
         &DebugInstance) >= 0 )
    return DebugInstance;
  else
    return 0;
}

//
// Debug Print (conditional on error level via CmosCheckMode)
//
int  DebugPrint(int ErrorLevel, const char *Format, ...)
{
  int result;
  int ( **DebugInstance)(int, const char *, char *);
  va_list va;

  va_start(va, Format);
  result = GetDebugInstance();
  DebugInstance = (int ( **)(int, const char *, char *))result;
  if ( result )
  {
    result = CmosCheckMode();
    if ( (result & ErrorLevel) != 0 )
      return (*DebugInstance)(ErrorLevel, Format, (char *)va);
  }
  return result;
}

//
// Debug Assert (via debug instance)
//
int  DebugAssert(
        const char *FileName,
        int LineNumber,
        const char *Assertion)
{
  int result;

  result = GetDebugInstance();
  if ( result )
    return (*(int ( **)(const char *, int, const char *))(result + 4))(
             FileName,
             LineNumber,
             Assertion);
  return result;
}

//
// CpuIoPei Entry Point
//
// Installs the CPU I/O PPI (EFI_PEI_CPU_IO_PPI) into the PEI database.
// On first invocation: installs via InstallPpi.
// On shadow (reload after memory): RegisterForShadow only.
//
EFI_STATUS
EFIAPI
CpuIoPeiEntryPoint(
  IN EFI_HANDLE        ImageHandle,
  IN EFI_SYSTEM_TABLE  *SystemTable
  )
{
  EFI_PEI_SERVICES    **PeiServices;
  EFI_STATUS          Status;

  PeiServices = GetPeiServicesTable();

  // RegisterForShadow detects if this is first-time or reinstall
  Status = (*PeiServices)->RegisterForShadow(ImageHandle);
  *(VOID **)((UINTN)SystemTable + 96) = &mCpuIoPpi;

  if (Status == EFI_ALREADY_STARTED) {
    DEBUG ((EFI_D_INFO,
      "CpuIO PPI has been loaded into memory.  Reinstalled PPI=0x%x\n",
      &mCpuIoPpi));
  } else {
    // First install: install the CPU I/O PPI descriptor
    PeiServices = GetPeiServicesTable();
    Status = (*PeiServices)->InstallPpi (&mCpuIoPpiDescriptor);
    ASSERT_EFI_ERROR (Status);
  }

  return EFI_SUCCESS;
}