/**
  To check memory usage bit map array to figure out if the memory range in which the image will be loaded is available or not. If 
  memory range is avaliable, the function will mark the correponding bits to 1 which indicates the memory range is used.
  The function is only invoked when load modules at fixed address feature is enabled. 
  
  @param  ImageBase                The base addres the image will be loaded at.
  @param  ImageSize                The size of the image
  
  @retval EFI_SUCCESS              The memory range the image will be loaded in is available
  @retval EFI_NOT_FOUND            The memory range the image will be loaded in is not available
**/
EFI_STATUS
CheckAndMarkFixLoadingMemoryUsageBitMap (
  IN  EFI_PHYSICAL_ADDRESS          ImageBase,
  IN  UINTN                         ImageSize
  )
{
   UINT32                             SmmCodePageNumber;
   UINT64                             SmmCodeSize; 
   EFI_PHYSICAL_ADDRESS               SmmCodeBase;
   UINTN                              BaseOffsetPageNumber;
   UINTN                              TopOffsetPageNumber;
   UINTN                              Index;
   //
   // Build tool will calculate the smm code size and then patch the PcdLoadFixAddressSmmCodePageNumber
   //
   SmmCodePageNumber = PcdGet32(PcdLoadFixAddressSmmCodePageNumber);
   SmmCodeSize = EFI_PAGES_TO_SIZE (SmmCodePageNumber);
   SmmCodeBase = gLoadModuleAtFixAddressSmramBase;
   
   //
   // If the memory usage bit map is not initialized,  do it. Every bit in the array 
   // indicate the status of the corresponding memory page, available or not
   // 
   if (mSmmCodeMemoryRangeUsageBitMap == NULL) {
     mSmmCodeMemoryRangeUsageBitMap = AllocateZeroPool(((SmmCodePageNumber / 64) + 1)*sizeof(UINT64));
   }
   //
   // If the Dxe code memory range is not allocated or the bit map array allocation failed, return EFI_NOT_FOUND
   //
   if (mSmmCodeMemoryRangeUsageBitMap == NULL) {
     return EFI_NOT_FOUND;
   }
   //
   // see if the memory range for loading the image is in the SMM code range.
   //
   if (SmmCodeBase + SmmCodeSize <  ImageBase + ImageSize || SmmCodeBase >  ImageBase) {
     return EFI_NOT_FOUND;   
   }   
   //
   // Test if the memory is avalaible or not.
   // 
   BaseOffsetPageNumber = (UINTN)EFI_SIZE_TO_PAGES((UINT32)(ImageBase - SmmCodeBase));
   TopOffsetPageNumber  = (UINTN)EFI_SIZE_TO_PAGES((UINT32)(ImageBase + ImageSize - SmmCodeBase));
   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
     if ((mSmmCodeMemoryRangeUsageBitMap[Index / 64] & LShiftU64(1, (Index % 64))) != 0) {
       //
       // This page is already used.
       //
       return EFI_NOT_FOUND;  
     }
   }
   
   //
   // Being here means the memory range is available.  So mark the bits for the memory range
   // 
   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
     mSmmCodeMemoryRangeUsageBitMap[Index / 64] |= LShiftU64(1, (Index % 64));
   }
   return  EFI_SUCCESS;   
}

UINTN
FindSpace (
  UINTN                       NoPages,
  IN UINTN                    *NumberOfMemoryMapEntries,
  IN EFI_MEMORY_DESCRIPTOR    *EfiMemoryDescriptor,
  EFI_MEMORY_TYPE             Type,
  UINT64                      Attribute
  )
{
  EFI_PHYSICAL_ADDRESS        MaxPhysicalStart;
  UINT64                      MaxNoPages;
  UINTN                       Index;
  EFI_MEMORY_DESCRIPTOR       *CurrentMemoryDescriptor;

  MaxPhysicalStart = 0;
  MaxNoPages       = 0;
  CurrentMemoryDescriptor = NULL;
  for (Index = 0; Index < *NumberOfMemoryMapEntries; Index++) {
    if (EfiMemoryDescriptor[Index].PhysicalStart + LShiftU64(EfiMemoryDescriptor[Index].NumberOfPages, EFI_PAGE_SHIFT) <= 0x100000) {
      continue;
    }
    if ((EfiMemoryDescriptor[Index].Type == EfiConventionalMemory) && 
        (EfiMemoryDescriptor[Index].NumberOfPages >= NoPages)) {
      if (EfiMemoryDescriptor[Index].PhysicalStart > MaxPhysicalStart) {
        if (EfiMemoryDescriptor[Index].PhysicalStart + LShiftU64(EfiMemoryDescriptor[Index].NumberOfPages, EFI_PAGE_SHIFT) <= 0x100000000ULL) {
          MaxPhysicalStart = EfiMemoryDescriptor[Index].PhysicalStart;
          MaxNoPages       = EfiMemoryDescriptor[Index].NumberOfPages;
          CurrentMemoryDescriptor = &EfiMemoryDescriptor[Index];
        }
      }
    }
    if ((EfiMemoryDescriptor[Index].Type == EfiReservedMemoryType) ||
        (EfiMemoryDescriptor[Index].Type >= EfiACPIReclaimMemory) ) {
      continue;
    }
    if ((EfiMemoryDescriptor[Index].Type == EfiRuntimeServicesCode) ||
        (EfiMemoryDescriptor[Index].Type == EfiRuntimeServicesData)) {
      break;
    }
  }
 
  if (MaxPhysicalStart == 0) {
    return 0;
  }

  if (MaxNoPages != NoPages) {
    CurrentMemoryDescriptor->NumberOfPages = MaxNoPages - NoPages;
    EfiMemoryDescriptor[*NumberOfMemoryMapEntries].Type          = Type;
    EfiMemoryDescriptor[*NumberOfMemoryMapEntries].PhysicalStart = MaxPhysicalStart + LShiftU64(MaxNoPages - NoPages, EFI_PAGE_SHIFT);
    EfiMemoryDescriptor[*NumberOfMemoryMapEntries].NumberOfPages = NoPages;
    EfiMemoryDescriptor[*NumberOfMemoryMapEntries].VirtualStart  = 0;
    EfiMemoryDescriptor[*NumberOfMemoryMapEntries].Attribute     = Attribute;
    *NumberOfMemoryMapEntries = *NumberOfMemoryMapEntries + 1;
  } else {
    CurrentMemoryDescriptor->Type      = Type;
    CurrentMemoryDescriptor->Attribute = Attribute;
  }

  return (UINTN)(MaxPhysicalStart + LShiftU64(MaxNoPages - NoPages, EFI_PAGE_SHIFT));
}

/**
  Determine the MTRR numbers used to program a memory range.

  This function first checks the alignment of the base address. If the alignment of the base address <= Length,
  cover the memory range (BaseAddress, alignment) by a MTRR, then BaseAddress += alignment and Length -= alignment.
  Repeat the step until alignment > Length.

  Then this function determines which direction of programming the variable MTRRs for the remaining length
  will use fewer MTRRs.

  @param  BaseAddress Length of Memory to program MTRR
  @param  Length      Length of Memory to program MTRR
  @param  MtrrNumber  Pointer to the number of necessary MTRRs

  @retval TRUE        Positive direction is better.
          FALSE       Negtive direction is better.

**/
BOOLEAN
GetMtrrNumberAndDirection (
  IN UINT64      BaseAddress,
  IN UINT64      Length,
  IN UINTN       *MtrrNumber
  )
{
  UINT64  TempQword;
  UINT64  Alignment;
  UINT32  Positive;
  UINT32  Subtractive;

  *MtrrNumber = 0;

  if (BaseAddress != 0) {
    do {
      //
      // Calculate the alignment of the base address.
      //
      Alignment = LShiftU64 (1, (UINTN)LowBitSet64 (BaseAddress));

      if (Alignment > Length) {
        break;
      }

      (*MtrrNumber)++;
      BaseAddress += Alignment;
      Length -= Alignment;
    } while (TRUE);

    if (Length == 0) {
      return TRUE;
    }
  }

  TempQword   = Length;
  Positive    = 0;
  Subtractive = 0;

  do {
    TempQword -= Power2MaxMemory (TempQword);
    Positive++;
  } while (TempQword != 0);

  TempQword = Power2MaxMemory (LShiftU64 (Length, 1)) - Length;
  Subtractive++;
  do {
    TempQword -= Power2MaxMemory (TempQword);
    Subtractive++;
  } while (TempQword != 0);

  if (Positive <= Subtractive) {
    *MtrrNumber += Positive;
    return TRUE;
  } else {
    *MtrrNumber += Subtractive;
    return FALSE;
  }
}

/**
  Process DMAR RMRR table.

  @param[in]  VTdInfo   The VTd engine context information.
  @param[in]  DmarRmrr  The RMRR table.
**/
VOID
ProcessRmrr (
  IN VTD_INFO                   *VTdInfo,
  IN EFI_ACPI_DMAR_RMRR_HEADER  *DmarRmrr
  )
{
  EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER       *DmarDevScopeEntry;
  UINTN                                             VTdIndex;
  UINT64                                            RmrrMask;
  UINTN                                             LowBottom;
  UINTN                                             LowTop;
  UINTN                                             HighBottom;
  UINT64                                            HighTop;
  EFI_ACPI_DMAR_HEADER                              *AcpiDmarTable;

  AcpiDmarTable = VTdInfo->AcpiDmarTable;

  DEBUG ((DEBUG_INFO,"  RMRR (Base 0x%016lx, Limit 0x%016lx)\n", DmarRmrr->ReservedMemoryRegionBaseAddress, DmarRmrr->ReservedMemoryRegionLimitAddress));

  if ((DmarRmrr->ReservedMemoryRegionBaseAddress == 0) ||
      (DmarRmrr->ReservedMemoryRegionLimitAddress == 0)) {
    return ;
  }

  DmarDevScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)(DmarRmrr + 1));
  while ((UINTN)DmarDevScopeEntry < (UINTN)DmarRmrr + DmarRmrr->Header.Length) {
    ASSERT (DmarDevScopeEntry->Type == EFI_ACPI_DEVICE_SCOPE_ENTRY_TYPE_PCI_ENDPOINT);

    VTdIndex = GetVTdEngineFromDevScopeEntry (AcpiDmarTable, DmarRmrr->SegmentNumber, DmarDevScopeEntry);
    if (VTdIndex != (UINTN)-1) {
      RmrrMask = LShiftU64 (1, VTdIndex);

      LowBottom = 0;
      LowTop = (UINTN)DmarRmrr->ReservedMemoryRegionBaseAddress;
      HighBottom = (UINTN)DmarRmrr->ReservedMemoryRegionLimitAddress + 1;
      HighTop = LShiftU64 (1, VTdInfo->HostAddressWidth + 1);

      SetDmaProtectedRange (
        VTdInfo,
        RmrrMask,
        0,
        (UINT32)(LowTop - LowBottom),
        HighBottom,
        HighTop - HighBottom
        );

      //
      // Remove the engine from the engine mask.
      // The assumption is that any other PEI driver does not access
      // the device covered by this engine.
      //
      VTdInfo->EngineMask = VTdInfo->EngineMask & (~RmrrMask);
    }

    DmarDevScopeEntry = (EFI_ACPI_DMAR_DEVICE_SCOPE_STRUCTURE_HEADER *)((UINTN)DmarDevScopeEntry + DmarDevScopeEntry->Length);
  }
}

/**
  To check memory usage bit map arry to figure out if the memory range the image will be loaded in is available or not. If 
  memory range is avaliable, the function will mark the correponding bits to 1 which indicates the memory range is used.
  The function is only invoked when load modules at fixed address feature is enabled. 
  
  @param  Private                  Pointer to the private data passed in from caller
  @param  ImageBase                The base addres the image will be loaded at.
  @param  ImageSize                The size of the image
  
  @retval EFI_SUCCESS              The memory range the image will be loaded in is available
  @retval EFI_NOT_FOUND            The memory range the image will be loaded in is not available
**/
EFI_STATUS
CheckAndMarkFixLoadingMemoryUsageBitMap (
  IN  PEI_CORE_INSTANCE             *Private,
  IN  EFI_PHYSICAL_ADDRESS          ImageBase,
  IN  UINT32                        ImageSize
  )
{
   UINT32                             DxeCodePageNumber;
   UINT64                             ReservedCodeSize;
   EFI_PHYSICAL_ADDRESS               PeiCodeBase;
   UINT32                             BaseOffsetPageNumber;
   UINT32                             TopOffsetPageNumber;
   UINT32                             Index;
   UINT64                             *MemoryUsageBitMap;
   

   //
   // The reserved code range includes RuntimeCodePage range, Boot time code range and PEI code range.
   //
   DxeCodePageNumber = PcdGet32(PcdLoadFixAddressBootTimeCodePageNumber);
   DxeCodePageNumber += PcdGet32(PcdLoadFixAddressRuntimeCodePageNumber);
   ReservedCodeSize  = EFI_PAGES_TO_SIZE(DxeCodePageNumber + PcdGet32(PcdLoadFixAddressPeiCodePageNumber));
   PeiCodeBase       = Private->LoadModuleAtFixAddressTopAddress - ReservedCodeSize;
   
   //
   // Test the memory range for loading the image in the PEI code range.
   //
   if ((Private->LoadModuleAtFixAddressTopAddress - EFI_PAGES_TO_SIZE(DxeCodePageNumber)) < (ImageBase + ImageSize) ||
       (PeiCodeBase > ImageBase)) {         
     return EFI_NOT_FOUND; 
   }
   
   //
   // Test if the memory is avalaible or not.
   //
   MemoryUsageBitMap    = Private->PeiCodeMemoryRangeUsageBitMap;  
   BaseOffsetPageNumber = EFI_SIZE_TO_PAGES((UINT32)(ImageBase - PeiCodeBase));
   TopOffsetPageNumber  = EFI_SIZE_TO_PAGES((UINT32)(ImageBase + ImageSize - PeiCodeBase));
   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
     if ((MemoryUsageBitMap[Index / 64] & LShiftU64(1, (Index % 64))) != 0) {
       //
       // This page is already used.
       //
       return EFI_NOT_FOUND;  
     }
   }
   
   //
   // Being here means the memory range is available.  So mark the bits for the memory range
   // 
   for (Index = BaseOffsetPageNumber; Index < TopOffsetPageNumber; Index ++) {
     MemoryUsageBitMap[Index / 64] |= LShiftU64(1, (Index % 64));
   }
   return  EFI_SUCCESS;   
}

EFI_RUNTIMESERVICE
EFI_STATUS
EFIAPI
MonotonicCounterDriverGetNextHighMonotonicCount (
  OUT UINT32  *HighCount
  )
/*++

Routine Description:

Arguments:

Returns:

--*/
{
  EFI_STATUS  Status;
  EFI_TPL     OldTpl;

  //
  // Check input parameters
  //
  if (HighCount == NULL) {
    return EFI_INVALID_PARAMETER;
  }

  if (!mEfiAtRuntime) {
    //
    // Use a lock if called before ExitBootServices()
    //
    OldTpl      = gBS->RaiseTPL (EFI_TPL_HIGH_LEVEL);
    *HighCount  = (UINT32) RShiftU64 (mEfiMtc, 32) + 1;
    mEfiMtc     = LShiftU64 (*HighCount, 32);
    gBS->RestoreTPL (OldTpl);
  } else {
    *HighCount  = (UINT32) RShiftU64 (mEfiMtc, 32) + 1;
    mEfiMtc     = LShiftU64 (*HighCount, 32);
  }
  //
  // Update the NvRam store to match the new high part
  //
  Status = gRT->SetVariable (
                  mEfiMtcName,
                  &mEfiMtcGuid,
                  EFI_VARIABLE_NON_VOLATILE | EFI_VARIABLE_RUNTIME_ACCESS | EFI_VARIABLE_BOOTSERVICE_ACCESS,
                  sizeof (UINT32),
                  HighCount
                  );

  return Status;
}

/**
  Initializes the Intel VTd PMR for all memory.

  @retval EFI_SUCCESS            Usb bot driver is successfully initialized.
  @retval EFI_OUT_OF_RESOURCES   Can't initialize the driver.

**/
EFI_STATUS
InitVTdPmrForAll (
  VOID
  )
{
  EFI_STATUS                  Status;
  VOID                        *Hob;
  VTD_INFO                    *VTdInfo;
  UINTN                       LowBottom;
  UINTN                       LowTop;
  UINTN                       HighBottom;
  UINT64                      HighTop;

  Hob = GetFirstGuidHob (&mVTdInfoGuid);
  VTdInfo = GET_GUID_HOB_DATA(Hob);

  LowBottom = 0;
  LowTop = 0;
  HighBottom = 0;
  HighTop = LShiftU64 (1, VTdInfo->HostAddressWidth + 1);

  Status = SetDmaProtectedRange (
             VTdInfo,
             VTdInfo->EngineMask,
             (UINT32)LowBottom,
             (UINT32)(LowTop - LowBottom),
             HighBottom,
             HighTop - HighBottom
             );

  return Status;
}

/**
  Convert a packed value from cbuint64 to a UINT64 value.

  @param  val      The pointer to packed data.

  @return          the UNIT64 value after convertion.

**/
UINT64 
cb_unpack64 (
  IN struct cbuint64 val
  )
{
  return LShiftU64 (val.hi, 32) | val.lo;
}

UINT64
Power2MaxMemory (
  IN UINT64                     MemoryLength
  )
/*++

Routine Description:

  TODO: Add function description

Arguments:

  MemoryLength  - TODO: add argument description

Returns:

  TODO: add return values

--*/
{
  UINT64  Result;

  if (RShiftU64 (MemoryLength, 32)) {
    Result = LShiftU64 ((UINT64) SetPower2 ((UINT32) RShiftU64 (MemoryLength, 32)), 32);
  } else {
    Result = (UINT64) SetPower2 ((UINT32) MemoryLength);
  }

  return Result;
}

UINT64 __aeabi_uldivmod(unsigned numerator, unsigned denominator)
{
	UINT64  Return;
	Return = __udivsi3 (numerator, denominator);
	Return |= LShiftU64 (__umodsi3 (numerator, denominator), 32);
	return Return;
}

/**
  This is the standard EFI driver point that detects whether there is a
  MemoryConfigurationData Variable and, if so, reports memory configuration info
  to the DataHub.

  @param  ImageHandle  Handle for the image of this driver
  @param  SystemTable  Pointer to the EFI System Table

  @return EFI_SUCCESS if the data is successfully reported
  @return EFI_NOT_FOUND if the HOB list could not be located.

**/
EFI_STATUS
LogMemorySmbiosRecord (
  VOID
  )
{
  EFI_STATUS                      Status;
  UINT64                          TotalMemorySize;
  UINT8                           NumSlots;
  SMBIOS_TABLE_TYPE19             *Type19Record;
  EFI_SMBIOS_HANDLE               MemArrayMappedAddrSmbiosHandle;
  EFI_SMBIOS_PROTOCOL             *Smbios;
  CHAR16                          *MemString;

  Status = gBS->LocateProtocol (&gEfiSmbiosProtocolGuid, NULL, (VOID**)&Smbios);
  ASSERT_EFI_ERROR (Status);

  NumSlots        = 1;

  //
  // Process Memory String in form size!size ...
  // So 64!64 is 128 MB
  //
  MemString   = (CHAR16 *)PcdGetPtr (PcdEmuMemorySize);
  for (TotalMemorySize = 0; *MemString != '\0';) {
    TotalMemorySize += StrDecimalToUint64 (MemString);
    while (*MemString != '\0') {
      if (*MemString == '!') {
        MemString++;
        break;
      }
      MemString++;
    }
  }

  //
  // Convert Total Memory Size to based on KiloByte
  //
  TotalMemorySize = LShiftU64 (TotalMemorySize, 20);
  //
  // Generate Memory Array Mapped Address info
  //
  Type19Record = AllocateZeroPool(sizeof (SMBIOS_TABLE_TYPE19) + 2);
  Type19Record->Hdr.Type = EFI_SMBIOS_TYPE_MEMORY_ARRAY_MAPPED_ADDRESS;
  Type19Record->Hdr.Length = sizeof(SMBIOS_TABLE_TYPE19);
  Type19Record->Hdr.Handle = 0;
  Type19Record->StartingAddress = 0;
  Type19Record->EndingAddress =  (UINT32)RShiftU64(TotalMemorySize, 10) - 1;
  Type19Record->MemoryArrayHandle = 0;
  Type19Record->PartitionWidth = (UINT8)(NumSlots);

  //
  // Generate Memory Array Mapped Address info (TYPE 19)
  //
  Status = AddSmbiosRecord (Smbios, &MemArrayMappedAddrSmbiosHandle, (EFI_SMBIOS_TABLE_HEADER*) Type19Record);

  FreePool(Type19Record);
  ASSERT_EFI_ERROR (Status);

  return Status;
}

/**
  Accumulate the MD5 value of every data segment and generate the finial
  result according to MD5 algorithm.

  @param[in, out]   Md5Ctx  The data structure of storing the original data
                            segment and the final result.
  @param[out]      HashVal  The final 128-bits output.

  @retval EFI_SUCCESS  The transform is ok.
  @retval Others       Other errors as indicated.
**/
EFI_STATUS
MD5Final (
  IN  OUT MD5_CTX  *Md5Ctx,
  OUT UINT8        *HashVal
  )
{
  UINTN PadLength;

  if (Md5Ctx->Status == EFI_ALREADY_STARTED) {
    //
    // Store Hashed value & Zeroize sensitive context information.
    //
    CopyMem (HashVal, (UINT8 *) Md5Ctx->States, MD5_HASHSIZE);
    ZeroMem ((UINT8 *)Md5Ctx, sizeof (*Md5Ctx));
    
    return EFI_SUCCESS;
  }

  if (EFI_ERROR (Md5Ctx->Status)) {
    return Md5Ctx->Status;
  }

  PadLength  = Md5Ctx->Count >= 56 ? 120 : 56;
  PadLength -= Md5Ctx->Count;
  MD5UpdateBlock (Md5Ctx, Md5HashPadding, PadLength);
  Md5Ctx->Length = LShiftU64 (Md5Ctx->Length, 3);
  MD5UpdateBlock (Md5Ctx, (CONST UINT8 *) &Md5Ctx->Length, 8);

  ZeroMem (Md5Ctx->M, sizeof (Md5Ctx->M));
  Md5Ctx->Length  = 0;
  Md5Ctx->Status  = EFI_ALREADY_STARTED;
  return MD5Final (Md5Ctx, HashVal);
}

/**
  This function handles S3 resume task at the end of PEI

  @param[in] PeiServices    Pointer to PEI Services Table.
  @param[in] NotifyDesc     Pointer to the descriptor for the Notification event that
                            caused this function to execute.
  @param[in] Ppi            Pointer to the PPI data associated with this function.

  @retval EFI_STATUS        Always return EFI_SUCCESS
**/
EFI_STATUS
EFIAPI
S3EndOfPeiNotify(
  IN EFI_PEI_SERVICES          **PeiServices,
  IN EFI_PEI_NOTIFY_DESCRIPTOR *NotifyDesc,
  IN VOID                      *Ppi
  )
{
  VOID                        *Hob;
  VTD_INFO                    *VTdInfo;
  UINT64                      EngineMask;

  DEBUG((DEBUG_INFO, "VTdPmr S3EndOfPeiNotify\n"));

  if ((PcdGet8(PcdVTdPolicyPropertyMask) & BIT1) == 0) {
    Hob = GetFirstGuidHob (&mVTdInfoGuid);
    if (Hob == NULL) {
      return EFI_SUCCESS;
    }
    VTdInfo = GET_GUID_HOB_DATA(Hob);

    EngineMask = LShiftU64 (1, VTdInfo->VTdEngineCount) - 1;
    DisableDmaProtection (VTdInfo, EngineMask);
  }
  return EFI_SUCCESS;
}

/**
  Send an IPI by writing to ICR.

  This function returns after the IPI has been accepted by the target processor. 

  @param  IcrLow 32-bit value to be written to the low half of ICR.
  @param  ApicId APIC ID of the target processor if this IPI is targeted for a specific processor.
**/
VOID
SendIpi (
  IN UINT32          IcrLow,
  IN UINT32          ApicId
  )
{
  UINT64             MsrValue;
  LOCAL_APIC_ICR_LOW IcrLowReg;

  if (GetApicMode () == LOCAL_APIC_MODE_XAPIC) {
    ASSERT (ApicId <= 0xff);

    //
    // For xAPIC, the act of writing to the low doubleword of the ICR causes the IPI to be sent.
    //
    MmioWrite32 (PcdGet32 (PcdCpuLocalApicBaseAddress) + XAPIC_ICR_HIGH_OFFSET, ApicId << 24);
    MmioWrite32 (PcdGet32 (PcdCpuLocalApicBaseAddress) + XAPIC_ICR_LOW_OFFSET, IcrLow);
    do {
      IcrLowReg.Uint32 = MmioRead32 (PcdGet32 (PcdCpuLocalApicBaseAddress) + XAPIC_ICR_LOW_OFFSET);
    } while (IcrLowReg.Bits.DeliveryStatus != 0);
  } else {
    //
    // For x2APIC, A single MSR write to the Interrupt Command Register is required for dispatching an 
    // interrupt in x2APIC mode.
    //
    MsrValue = LShiftU64 ((UINT64) ApicId, 32) | IcrLow;
    AsmWriteMsr64 (X2APIC_MSR_ICR_ADDRESS, MsrValue);
  }
}

/**
  Converts a number of EFI_PAGEs to a size in bytes.

  NOTE: Do not use EFI_PAGES_TO_SIZE because it handles UINTN only.

  @param  Pages     The number of EFI_PAGES.

  @return  The number of bytes associated with the number of EFI_PAGEs specified
           by Pages.
**/
STATIC
UINT64
EfiPagesToSize (
  IN UINT64 Pages
  )
{
  return LShiftU64 (Pages, EFI_PAGE_SHIFT);
}

/**

  This function return VTd engine index by PCI bus/device/function.

  @param Bus           Pci bus
  @param Device        Pci device
  @param Function      Pci function

  @return VTd engine index

**/
UINTN
FindVtdIndexByPciDevice (
  IN UINT8          Bus,
  IN UINT8          Device,
  IN UINT8          Function
  )
{
  UINTN                   VtdIndex;
  VTD_ROOT_ENTRY          *RootEntry;
  VTD_CONTEXT_ENTRY       *ContextEntryTable;
  VTD_CONTEXT_ENTRY       *ContextEntry;
  VTD_EXT_ROOT_ENTRY      *ExtRootEntry;
  VTD_EXT_CONTEXT_ENTRY   *ExtContextEntryTable;
  VTD_EXT_CONTEXT_ENTRY   *ExtContextEntry;
  UINT32                  PciDescriptorIndex;

  for (VtdIndex = 0; VtdIndex < mVtdUnitNumber; VtdIndex++) {
    PciDescriptorIndex = GetPciDescriptor (VtdIndex, Bus, Device, Function);
    if (PciDescriptorIndex == 0) {
      continue;
    }

//    DEBUG ((EFI_D_INFO,"FindVtdIndex(0x%x) for B%02x D%02x F%02x\n", VtdIndex, Bus, Device, Function));

    if (mVtdUnitInformation[VtdIndex].ExtRootEntryTable != 0) {
      ExtRootEntry = &mVtdUnitInformation[VtdIndex].ExtRootEntryTable[Bus];
      ExtContextEntryTable = (VTD_EXT_CONTEXT_ENTRY *)(UINTN)LShiftU64 (ExtRootEntry->Bits.LowerContextTablePointer, 12) ;
      ExtContextEntry      = &ExtContextEntryTable[(Device << 3) | Function];
      if (ExtContextEntry->Bits.DomainIdentifier != UEFI_DOMAIN_ID) {
        continue;
      }
    } else {
      RootEntry = &mVtdUnitInformation[VtdIndex].RootEntryTable[Bus];
      ContextEntryTable = (VTD_CONTEXT_ENTRY *)(UINTN)LShiftU64 (RootEntry->Bits.ContextTablePointer, 12) ;
      ContextEntry      = &ContextEntryTable[(Device << 3) | Function];
      if (ContextEntry->Bits.DomainIdentifier != UEFI_DOMAIN_ID) {
        continue;
      }
    }

    return VtdIndex;
  }

  return (UINTN)-1;
}

EFI_STATUS
EFIAPI    
PostPmInitCallBack (
  IN EFI_EVENT Event,
  IN VOID      *Context
  )
{
  UINT64      OriginalGTTMMADR;
  UINT32      LoGTBaseAddress;
  UINT32      HiGTBaseAddress;

  //
  // Enable Bus Master, I/O and Memory access on 0:2:0
  //
  PciOr8 (PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_R_CMD), (BIT2 | BIT1));

  //
  // only 32bit read/write is legal for device 0:2:0
  //
  OriginalGTTMMADR  = (UINT64) PciRead32 (PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_R_GTTMMADR));
  OriginalGTTMMADR  = LShiftU64 ((UINT64) PciRead32 (PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_R_GTTMMADR + 4)), 32) | (OriginalGTTMMADR);

  //
  // 64bit GTTMADR does not work for S3 save script table since it is executed in PEIM phase
  // Program temporarily 32bits GTTMMADR for POST and S3 resume
  //
  LoGTBaseAddress                   = (UINT32) (GTTMMADR & 0xFFFFFFFF);
  HiGTBaseAddress                   = (UINT32) RShiftU64 ((GTTMMADR & 0xFFFFFFFF00000000), 32);
  S3PciWrite32(PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_R_GTTMMADR), LoGTBaseAddress);
  S3PciWrite32(PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_R_GTTMMADR+4), HiGTBaseAddress);



  //
  // Restore original GTTMMADR
  //
  LoGTBaseAddress                   = (UINT32) (OriginalGTTMMADR & 0xFFFFFFFF);
  HiGTBaseAddress                   = (UINT32) RShiftU64 ((OriginalGTTMMADR & 0xFFFFFFFF00000000), 32);

  S3PciWrite32(PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_R_GTTMMADR), LoGTBaseAddress);
  S3PciWrite32(PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_R_GTTMMADR+4), HiGTBaseAddress);


  //
  // Lock the following registers, GGC, BDSM, BGSM
  //
  PciOr32 (PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_MGGC_OFFSET), LockBit);
  PciOr32 (PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_BSM_OFFSET), LockBit);
  PciOr32 (PCI_LIB_ADDRESS(0, IGD_DEV, 0,IGD_R_BGSM), LockBit);

  gBS->CloseEvent (Event);

  //
  // Return final status
  //
  return EFI_SUCCESS;
}

/**
  Set corresponding bits in bitmap table to 1 according to the address.

  @param[in]  Address     Start address to set for.
  @param[in]  BitNumber   Number of bits to set.
  @param[in]  BitMap      Pointer to bitmap which covers the Address.

  @return VOID
**/
STATIC
VOID
SetBits (
  IN EFI_PHYSICAL_ADDRESS    Address,
  IN UINTN                   BitNumber,
  IN UINT64                  *BitMap
  )
{
  UINTN           Lsbs;
  UINTN           Qwords;
  UINTN           Msbs;
  UINTN           StartBit;
  UINTN           EndBit;

  StartBit  = (UINTN)GUARDED_HEAP_MAP_ENTRY_BIT_INDEX (Address);
  EndBit    = (StartBit + BitNumber - 1) % GUARDED_HEAP_MAP_ENTRY_BITS;

  if ((StartBit + BitNumber) >= GUARDED_HEAP_MAP_ENTRY_BITS) {
    Msbs    = (GUARDED_HEAP_MAP_ENTRY_BITS - StartBit) %
              GUARDED_HEAP_MAP_ENTRY_BITS;
    Lsbs    = (EndBit + 1) % GUARDED_HEAP_MAP_ENTRY_BITS;
    Qwords  = (BitNumber - Msbs) / GUARDED_HEAP_MAP_ENTRY_BITS;
  } else {
    Msbs    = BitNumber;
    Lsbs    = 0;
    Qwords  = 0;
  }

  if (Msbs > 0) {
    *BitMap |= LShiftU64 (LShiftU64 (1, Msbs) - 1, StartBit);
    BitMap  += 1;
  }

  if (Qwords > 0) {
    SetMem64 ((VOID *)BitMap, Qwords * GUARDED_HEAP_MAP_ENTRY_BYTES,
              (UINT64)-1);
    BitMap += Qwords;
  }

  if (Lsbs > 0) {
    *BitMap |= (LShiftU64 (1, Lsbs) - 1);
  }
}

/**
  Get the PCI-E Address from a PCI address format 0x0000ssbbddffrrr
  where ss is SEGMENT, bb is BUS, dd is DEVICE, ff is FUNCTION
  and rrr is REGISTER (extension format for PCI-E).

  @param[in] InputAddress       PCI address format on input.
  @param[out]PciEAddress        PCI-E address extention format.
**/
VOID
EFIAPI
GetPciEAddressFromInputAddress (
  IN UINT64                 InputAddress,
  OUT UINT64                *PciEAddress
  )
{
  *PciEAddress = RShiftU64(InputAddress & ~(UINT64) 0xFFF, 4);
  *PciEAddress += LShiftU64((UINT16) InputAddress & 0x0FFF, 32);
}

/**
  Reads a bit field from a 64-bit value, performs a bitwise AND, and returns
  the result.

  Performs a bitwise AND between the bit field specified by StartBit and EndBit
  in Operand and the value specified by AndData. All other bits in Operand are
  preserved. The new 64-bit value is returned.

  If 64-bit operations are not supported, then ASSERT().
  If StartBit is greater than 63, then ASSERT().
  If EndBit is greater than 63, then ASSERT().
  If EndBit is less than StartBit, then ASSERT().

  @param  Operand   Operand on which to perform the bitfield operation.
  @param  StartBit  The ordinal of the least significant bit in the bit field.
                    Range 0..63.
  @param  EndBit    The ordinal of the most significant bit in the bit field.
                    Range 0..63.
  @param  AndData   The value to AND with the read value from the value

  @return The new 64-bit value.

**/
UINT64
EFIAPI
BitFieldAnd64 (
  IN      UINT64                    Operand,
  IN      UINTN                     StartBit,
  IN      UINTN                     EndBit,
  IN      UINT64                    AndData
  )
{
  UINT64  Value1;
  UINT64  Value2;
  
  ASSERT (EndBit < sizeof (Operand) * 8);
  ASSERT (StartBit <= EndBit);

  Value1 = LShiftU64 (~AndData, StartBit);
  Value2 = LShiftU64 ((UINT64)-2, EndBit);

  return Operand & ~(Value1 & ~Value2);
}