/**
  Initialize debug agent.

  This function is used to set up debug environment to support source level debugging.
  If certain Debug Agent Library instance has to save some private data in the stack,
  this function must work on the mode that doesn't return to the caller, then
  the caller needs to wrap up all rest of logic after InitializeDebugAgent() into one
  function and pass it into InitializeDebugAgent(). InitializeDebugAgent() is
  responsible to invoke the passing-in function at the end of InitializeDebugAgent().

  If the parameter Function is not NULL, Debug Agent Libary instance will invoke it by
  passing in the Context to be its parameter.

  If Function() is NULL, Debug Agent Library instance will return after setup debug
  environment.

  @param[in] InitFlag     Init flag is used to decide the initialize process.
  @param[in] Context      Context needed according to InitFlag; it was optional.
  @param[in] Function     Continue function called by debug agent library; it was
                          optional.

**/
VOID
EFIAPI
InitializeDebugAgent (
  IN UINT32                InitFlag,
  IN VOID                  *Context, OPTIONAL
  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
  )
{
  UINTN                Offset;
  UINTN                Length;
  BOOLEAN              IrqEnabled;
  UINT64               *VectorBase;


  //
  // Disable interrupts
  //
  IrqEnabled = ArmGetInterruptState ();
  ArmDisableInterrupts ();
  ArmDisableFiq ();

  //
  // Copy an implementation of the ARM exception vectors to PcdCpuVectorBaseAddress.
  //
  Length = (UINTN)ExceptionHandlersEnd - (UINTN)ExceptionHandlersStart;

  //
  // Reserve space for the exception handlers
  //
  VectorBase = (UINT64 *)(UINTN)PcdGet64 (PcdCpuVectorBaseAddress);


  // Copy our assembly code into the page that contains the exception vectors.
  CopyMem ((VOID *)VectorBase, (VOID *)ExceptionHandlersStart, Length);

  //
  // Patch in the common Assembly exception handler
  //
  Offset = (UINTN)CommonExceptionEntry - (UINTN)ExceptionHandlersStart;
  *(UINTN *) (((UINT8 *)VectorBase) + Offset) = (UINTN)AsmCommonExceptionEntry;

  // Flush Caches since we updated executable stuff
  InvalidateInstructionCacheRange ((VOID *)PcdGet64(PcdCpuVectorBaseAddress), Length);

  // setup a timer so gdb can break in via ctrl-c
  DebugAgentTimerIntialize ();

  if (IrqEnabled) {
    ArmEnableInterrupts ();
  }

  if (Function != NULL) {
    Function (Context);
  }

  return;
}

EFI_STATUS
EFIAPI
PeimInitializeFlashMap (
  IN       EFI_PEI_FILE_HANDLE       FileHandle,
  IN CONST EFI_PEI_SERVICES          **PeiServices
  )
/*++

Routine Description:
  Build GUIDed HOBs for platform specific flash map

Arguments:
  FfsHeader   - A pointer to the EFI_FFS_FILE_HEADER structure.
  PeiServices - General purpose services available to every PEIM.

Returns:
  EFI_STATUS

**/
{
  EFI_STATUS              Status;
  EMU_THUNK_PPI           *Thunk;
  EFI_PEI_PPI_DESCRIPTOR  *PpiDescriptor;
  EFI_PHYSICAL_ADDRESS    FdBase;
  EFI_PHYSICAL_ADDRESS    FdFixUp;
  UINT64                  FdSize;

  DEBUG ((EFI_D_ERROR, "EmulatorPkg Flash Map PEIM Loaded\n"));

  //
  // Get the Fwh Information PPI
  //
  Status = PeiServicesLocatePpi (
            &gEmuThunkPpiGuid, // GUID
            0,                 // INSTANCE
            &PpiDescriptor,     // EFI_PEI_PPI_DESCRIPTOR
            (VOID **)&Thunk       // PPI
            );
  ASSERT_EFI_ERROR (Status);

  //
  // Assume that FD0 contains the Flash map.
  //
  Status = Thunk->FirmwareDevices (0, &FdBase, &FdSize, &FdFixUp);
  if (EFI_ERROR (Status)) {
    return Status;
  }

  PcdSet64 (PcdFlashNvStorageVariableBase64, PcdGet64 (PcdEmuFlashNvStorageVariableBase) + FdFixUp);
  PcdSet64 (PcdFlashNvStorageFtwWorkingBase64, PcdGet64 (PcdEmuFlashNvStorageFtwWorkingBase) + FdFixUp);
  PcdSet64 (PcdFlashNvStorageFtwSpareBase64, PcdGet64 (PcdEmuFlashNvStorageFtwSpareBase) + FdFixUp);

  return EFI_SUCCESS;
}

/**
  Initialize debug agent.

  This function is used to set up debug environment to support source level debugging.
  If certain Debug Agent Library instance has to save some private data in the stack,
  this function must work on the mode that doesn't return to the caller, then
  the caller needs to wrap up all rest of logic after InitializeDebugAgent() into one
  function and pass it into InitializeDebugAgent(). InitializeDebugAgent() is
  responsible to invoke the passing-in function at the end of InitializeDebugAgent().

  If the parameter Function is not NULL, Debug Agent Library instance will invoke it by
  passing in the Context to be its parameter.

  If Function() is NULL, Debug Agent Library instance will return after setup debug
  environment.

  @param[in] InitFlag     Init flag is used to decide the initialize process.
  @param[in] Context      Context needed according to InitFlag; it was optional.
  @param[in] Function     Continue function called by debug agent library; it was
                          optional.

**/
VOID
EFIAPI
InitializeDebugAgent (
  IN UINT32                InitFlag,
  IN VOID                  *Context, OPTIONAL
  IN DEBUG_AGENT_CONTINUE  Function  OPTIONAL
  )
{
  EFI_STATUS            Status;
  EFI_FFS_FILE_HEADER   *FfsHeader;
  PE_COFF_LOADER_IMAGE_CONTEXT  ImageContext;

  // We use InitFlag to know if DebugAgent has been initialized from
  // Sec (DEBUG_AGENT_INIT_PREMEM_SEC) or PrePi (DEBUG_AGENT_INIT_POSTMEM_SEC)
  // modules
  if (InitFlag == DEBUG_AGENT_INIT_PREMEM_SEC) {
    //
    // Get the Sec or PrePeiCore module (defined as SEC type module)
    //
    Status = GetFfsFile ((EFI_FIRMWARE_VOLUME_HEADER*)(UINTN)PcdGet64 (PcdSecureFvBaseAddress), EFI_FV_FILETYPE_SECURITY_CORE, &FfsHeader);
    if (!EFI_ERROR(Status)) {
      Status = GetImageContext (FfsHeader,&ImageContext);
      if (!EFI_ERROR(Status)) {
        PeCoffLoaderRelocateImageExtraAction (&ImageContext);
      }
    }
  } else if (InitFlag == DEBUG_AGENT_INIT_POSTMEM_SEC) {
    //
    // Get the PrePi or PrePeiCore module (defined as SEC type module)
    //
    Status = GetFfsFile ((EFI_FIRMWARE_VOLUME_HEADER*)(UINTN)PcdGet64 (PcdFvBaseAddress), EFI_FV_FILETYPE_SECURITY_CORE, &FfsHeader);
    if (!EFI_ERROR(Status)) {
      Status = GetImageContext (FfsHeader,&ImageContext);
      if (!EFI_ERROR(Status)) {
        PeCoffLoaderRelocateImageExtraAction (&ImageContext);
      }
    }

    //
    // Get the PeiCore module (defined as PEI_CORE type module)
    //
    Status = GetFfsFile ((EFI_FIRMWARE_VOLUME_HEADER*)(UINTN)PcdGet64 (PcdFvBaseAddress), EFI_FV_FILETYPE_PEI_CORE, &FfsHeader);
    if (!EFI_ERROR(Status)) {
      Status = GetImageContext (FfsHeader,&ImageContext);
      if (!EFI_ERROR(Status)) {
        PeCoffLoaderRelocateImageExtraAction (&ImageContext);
      }
    }
  }
}

/**
  Return the Virtual Memory Map of your platform

  This Virtual Memory Map is used by MemoryInitPei Module to initialize the MMU on your platform.

  @param[out]   VirtualMemoryMap    Array of ARM_MEMORY_REGION_DESCRIPTOR describing a Physical-to-
                                    Virtual Memory mapping. This array must be ended by a zero-filled
                                    entry

**/
VOID
ArmPlatformGetVirtualMemoryMap (
  IN ARM_MEMORY_REGION_DESCRIPTOR** VirtualMemoryMap
  )
{
  ARM_MEMORY_REGION_ATTRIBUTES  CacheAttributes;
  UINTN                         Index = 0;
  ARM_MEMORY_REGION_DESCRIPTOR  *VirtualMemoryTable;

  ASSERT(VirtualMemoryMap != NULL);

  VirtualMemoryTable = (ARM_MEMORY_REGION_DESCRIPTOR*)AllocatePages(EFI_SIZE_TO_PAGES (sizeof(ARM_MEMORY_REGION_DESCRIPTOR) * MAX_VIRTUAL_MEMORY_MAP_DESCRIPTORS));
  if (VirtualMemoryTable == NULL) {
    return;
  }

  if (FeaturePcdGet(PcdCacheEnable) == TRUE) {
    CacheAttributes = DDR_ATTRIBUTES_CACHED;
  } else {
    CacheAttributes = DDR_ATTRIBUTES_UNCACHED;
  }

  // ReMap (Either NOR Flash or DRAM)
  VirtualMemoryTable[Index].PhysicalBase = PcdGet64 (PcdSystemMemoryBase);
  VirtualMemoryTable[Index].VirtualBase  = PcdGet64 (PcdSystemMemoryBase);
  VirtualMemoryTable[Index].Length       = PcdGet64 (PcdSystemMemorySize);
  VirtualMemoryTable[Index].Attributes   = CacheAttributes;

  // SOC Registers. L3 interconnects
  VirtualMemoryTable[++Index].PhysicalBase = SOC_REGISTERS_L3_PHYSICAL_BASE;
  VirtualMemoryTable[Index].VirtualBase  = SOC_REGISTERS_L3_PHYSICAL_BASE;
  VirtualMemoryTable[Index].Length       = SOC_REGISTERS_L3_PHYSICAL_LENGTH;
  VirtualMemoryTable[Index].Attributes   = SOC_REGISTERS_L3_ATTRIBUTES;

  // SOC Registers. L4 interconnects
  VirtualMemoryTable[++Index].PhysicalBase = SOC_REGISTERS_L4_PHYSICAL_BASE;
  VirtualMemoryTable[Index].VirtualBase  = SOC_REGISTERS_L4_PHYSICAL_BASE;
  VirtualMemoryTable[Index].Length       = SOC_REGISTERS_L4_PHYSICAL_LENGTH;
  VirtualMemoryTable[Index].Attributes   = SOC_REGISTERS_L4_ATTRIBUTES;

  // End of Table
  VirtualMemoryTable[++Index].PhysicalBase = 0;
  VirtualMemoryTable[Index].VirtualBase  = 0;
  VirtualMemoryTable[Index].Length       = 0;
  VirtualMemoryTable[Index].Attributes   = (ARM_MEMORY_REGION_ATTRIBUTES)0;

  ASSERT((Index + 1) == MAX_VIRTUAL_MEMORY_MAP_DESCRIPTORS);

  *VirtualMemoryMap = VirtualMemoryTable;
}

EFI_STATUS
EFIAPI
MemoryPeim (
  IN EFI_PHYSICAL_ADDRESS               MemoryBase,
  IN UINT64                             MemorySize
  )
{
  EFI_RESOURCE_ATTRIBUTE_TYPE ResourceAttributes;
  UINT64 Base,Size;

  //
  // For now we simply declare the IMA memory given to us, we will
  // do things a bit more smartly when I understand UEFI memory
  // management a bit better.
  //
  // Now, the permanent memory has been installed, we can call AllocatePages()
  //
  ResourceAttributes = (
      EFI_RESOURCE_ATTRIBUTE_PRESENT |
      EFI_RESOURCE_ATTRIBUTE_INITIALIZED |
      EFI_RESOURCE_ATTRIBUTE_UNCACHEABLE |
      EFI_RESOURCE_ATTRIBUTE_WRITE_COMBINEABLE |
      EFI_RESOURCE_ATTRIBUTE_WRITE_THROUGH_CACHEABLE |
      EFI_RESOURCE_ATTRIBUTE_WRITE_BACK_CACHEABLE |
      EFI_RESOURCE_ATTRIBUTE_TESTED
  );

  // TODO, use device-tree
  DEBUG((DEBUG_INIT, "System Memory Hob: %lx, %lx\n", MemoryBase, MemorySize));
  BuildResourceDescriptorHob (
      EFI_RESOURCE_SYSTEM_MEMORY,
      ResourceAttributes,
      MemoryBase, MemorySize);

  // Reserve ourselves (TODO: reserve map in DT ?)
  Base = PcdGet64(PcdFdBaseAddress) & ~(EFI_PAGE_SIZE - 1);
  Size = ((PcdGet64(PcdFdBaseAddress) + PcdGet64(PcdFdSize)) + (EFI_PAGE_SIZE - 1)) & ~(EFI_PAGE_SIZE - 1);
  DEBUG((DEBUG_INIT, "Reserve Hob: %lx, %lx\n", Base, Size));
  BuildMemoryAllocationHob (Base, Size, EfiBootServicesData);

  // Initialize MMU
  InitMmu ();

  if (FeaturePcdGet (PcdPrePiProduceMemoryTypeInformationHob)) {
    // Optional feature that helps prevent EFI memory map fragmentation.
    BuildMemoryTypeInformationHob ();
  }

  return EFI_SUCCESS;
}

EFI_STATUS
EFIAPI
InitializePlatformBootManagerLib (
  IN EFI_HANDLE        ImageHandle,
  IN EFI_SYSTEM_TABLE  *SystemTable
  )
{
  EFI_GUID  *TerminalTypeGuid;

  //
  // Update UART device path nodes based on UART PCD settings
  //
  gPciUartDevicePath0.Uart.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
  gPciUartDevicePath0.Uart.DataBits = PcdGet8 (PcdUartDefaultDataBits);
  gPciUartDevicePath0.Uart.Parity   = PcdGet8 (PcdUartDefaultParity);
  gPciUartDevicePath0.Uart.StopBits = PcdGet8 (PcdUartDefaultStopBits);
  gPciUartDevicePath1.Uart.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
  gPciUartDevicePath1.Uart.DataBits = PcdGet8 (PcdUartDefaultDataBits);
  gPciUartDevicePath1.Uart.Parity   = PcdGet8 (PcdUartDefaultParity);
  gPciUartDevicePath1.Uart.StopBits = PcdGet8 (PcdUartDefaultStopBits);

  //
  // Update Vendor device path nodes based on terminal type PCD settings
  //
  switch (PcdGet8 (PcdDefaultTerminalType)) {
  case PCANSITYPE:
    TerminalTypeGuid = &gEfiPcAnsiGuid;
    break;
  case VT100TYPE:
    TerminalTypeGuid = &gEfiVT100Guid;
    break;
  case VT100PLUSTYPE:
    TerminalTypeGuid = &gEfiVT100PlusGuid;
    break;
  case VTUTF8TYPE:
    TerminalTypeGuid = &gEfiVTUTF8Guid;
    break;
  case TTYTERMTYPE:
    TerminalTypeGuid = &gEfiTtyTermGuid;
    break;
  default:
    TerminalTypeGuid = &gEfiPcAnsiGuid;
    break;
  }
  CopyGuid (&gPciUartDevicePath0.TerminalType.Guid, TerminalTypeGuid);
  CopyGuid (&gPciUartDevicePath1.TerminalType.Guid, TerminalTypeGuid);

  return EFI_SUCCESS;
}

/**
  Notification function on EFI PCD protocol to install EFI Serial IO protocol based
  on Debug Communication Library. 

  @param[in]  Event    The event of notify protocol.
  @param[in]  Context  Notify event context.

**/
VOID
EFIAPI
InstallSerialIoNotification (
  IN EFI_EVENT     Event,
  IN VOID          *Context
  )
{
  EFI_STATUS  Status;

  //
  // Get Debug Port parameters from PCDs
  //
  mSerialIoDevicePath.UartDevicePath.BaudRate = PcdGet64 (PcdUartDefaultBaudRate);
  mSerialIoDevicePath.UartDevicePath.DataBits = PcdGet8 (PcdUartDefaultDataBits);
  mSerialIoDevicePath.UartDevicePath.Parity   = PcdGet8 (PcdUartDefaultParity);
  mSerialIoDevicePath.UartDevicePath.StopBits = PcdGet8 (PcdUartDefaultStopBits);

  mSerialIoMode.BaudRate = mSerialIoDevicePath.UartDevicePath.BaudRate;
  mSerialIoMode.DataBits = mSerialIoDevicePath.UartDevicePath.DataBits;
  mSerialIoMode.Parity   = mSerialIoDevicePath.UartDevicePath.Parity;
  mSerialIoMode.StopBits = mSerialIoDevicePath.UartDevicePath.StopBits;

  Status = gBS->InstallMultipleProtocolInterfaces (
                  &mSerialIoHandle,
                  &gEfiDevicePathProtocolGuid, &mSerialIoDevicePath,
                  &gEfiSerialIoProtocolGuid,   &mSerialIo,
                  NULL
                  );
  if (EFI_ERROR (Status)) {
    DEBUG ((EFI_D_ERROR, "Debug Agent: Failed to install EFI Serial IO Protocol on Debug Port!\n"));
  }
}

VOID
CreatePpiList (
  OUT UINTN                   *PpiListSize,
  OUT EFI_PEI_PPI_DESCRIPTOR  **PpiList
  )
{
  EFI_PEI_PPI_DESCRIPTOR *PlatformPpiList;
  UINTN                   PlatformPpiListSize;
  UINTN                   ListBase;
  EFI_PEI_PPI_DESCRIPTOR *LastPpi;

  // Get the Platform PPIs
  PlatformPpiListSize = 0;
  ArmPlatformGetPlatformPpiList (&PlatformPpiListSize, &PlatformPpiList);

  // Copy the Common and Platform PPis in Temporrary Memory
  ListBase = PcdGet64 (PcdCPUCoresStackBase);
  CopyMem ((VOID*)ListBase, gCommonPpiTable, sizeof(gCommonPpiTable));
  CopyMem ((VOID*)(ListBase + sizeof(gCommonPpiTable)), PlatformPpiList, PlatformPpiListSize);

  // Set the Terminate flag on the last PPI entry
  LastPpi = (EFI_PEI_PPI_DESCRIPTOR*)ListBase + ((sizeof(gCommonPpiTable) + PlatformPpiListSize) / sizeof(EFI_PEI_PPI_DESCRIPTOR)) - 1;
  LastPpi->Flags |= EFI_PEI_PPI_DESCRIPTOR_TERMINATE_LIST;

  *PpiList     = (EFI_PEI_PPI_DESCRIPTOR*)ListBase;
  *PpiListSize = sizeof(gCommonPpiTable) + PlatformPpiListSize;
}

/*++

Routine Description:
  Return the FD Size and base address. Since the FD is loaded from a
  file into host memory only the SEC will know it's address.

Arguments:
  Index  - Which FD, starts at zero.
  FdSize - Size of the FD in bytes
  FdBase - Start address of the FD. Assume it points to an FV Header
  FixUp  - Difference between actual FD address and build address

Returns:
  EFI_SUCCESS     - Return the Base address and size of the FV
  EFI_UNSUPPORTED - Index does nto map to an FD in the system

**/
EFI_STATUS
SecUnixFdAddress (
  IN     UINTN                 Index,
  IN OUT EFI_PHYSICAL_ADDRESS  *FdBase,
  IN OUT UINT64                *FdSize,
  IN OUT EFI_PHYSICAL_ADDRESS  *FixUp
  )
{
  if (Index >= gFdInfoCount) {
    return EFI_UNSUPPORTED;
  }

  *FdBase = gFdInfo[Index].Address;
  *FdSize = gFdInfo[Index].Size;
  *FixUp  = 0;

  if (*FdBase == 0 && *FdSize == 0) {
    return EFI_UNSUPPORTED;
  }

  if (Index == 0) {
    //
    // FD 0 has XIP code and well known PCD values
    // If the memory buffer could not be allocated at the FD build address
    // the Fixup is the difference.
    //
    *FixUp = *FdBase - PcdGet64 (PcdEmuFdBaseAddress);
  }

  return EFI_SUCCESS;
}

/**
  The constructor function to setup globals and goto virtual mode notify.

  @param  ImageHandle   The firmware allocated handle for the EFI image.
  @param  SystemTable   A pointer to the EFI System Table.

  @retval EFI_SUCCESS   The constructor completed successfully.
  @retval Other value   The constructor did not complete successfully.

**/
EFI_STATUS
EFIAPI
DxeRuntimeQncAccessLibConstructor (
  IN EFI_HANDLE        ImageHandle,
  IN EFI_SYSTEM_TABLE  *SystemTable
  )
{
  EFI_STATUS  Status;

  //
  // Cache the physical address of the PCI Express MMIO range into a module global variable
  //
  mDxeRuntimeQncAccessLibPciExpressBaseAddress = (UINTN) PcdGet64(PcdPciExpressBaseAddress);

  //
  // Register SetVirtualAddressMap () notify function
  //
  Status = gBS->CreateEventEx (
                  EVT_NOTIFY_SIGNAL,
                  TPL_NOTIFY,
                  DxeRuntimeQncAccessLibVirtualNotify,
                  NULL,
                  &gEfiEventVirtualAddressChangeGuid,
                  &mDxeRuntimeQncAccessLibVirtualNotifyEvent
                  );
  ASSERT_EFI_ERROR (Status);

  return Status;
}

VOID
EFIAPI
PrimaryMain (
  IN  EFI_PEI_CORE_ENTRY_POINT  PeiCoreEntryPoint
  )
{
  EFI_SEC_PEI_HAND_OFF        SecCoreData;
  UINTN                       PpiListSize;
  EFI_PEI_PPI_DESCRIPTOR      *PpiList;
  UINTN                       TemporaryRamBase;
  UINTN                       TemporaryRamSize;

  CreatePpiList (&PpiListSize, &PpiList);

  // Enable the GIC Distributor
  ArmGicEnableDistributor (PcdGet32(PcdGicDistributorBase));

  // If ArmVe has not been built as Standalone then we need to wake up the secondary cores
  if (FeaturePcdGet (PcdSendSgiToBringUpSecondaryCores)) {
    // Sending SGI to all the Secondary CPU interfaces
    ArmGicSendSgiTo (PcdGet32(PcdGicDistributorBase), ARM_GIC_ICDSGIR_FILTER_EVERYONEELSE, 0x0E, PcdGet32 (PcdGicSgiIntId));
  }

  // Adjust the Temporary Ram as the new Ppi List (Common + Platform Ppi Lists) is created at
  // the base of the primary core stack
  PpiListSize = ALIGN_VALUE(PpiListSize, CPU_STACK_ALIGNMENT);
  TemporaryRamBase = (UINTN)PcdGet64 (PcdCPUCoresStackBase) + PpiListSize;
  TemporaryRamSize = (UINTN)PcdGet32 (PcdCPUCorePrimaryStackSize) - PpiListSize;

  //
  // Bind this information into the SEC hand-off state
  // Note: this must be in sync with the stuff in the asm file
  // Note also:  HOBs (pei temp ram) MUST be above stack
  //
  SecCoreData.DataSize               = sizeof(EFI_SEC_PEI_HAND_OFF);
  SecCoreData.BootFirmwareVolumeBase = (VOID *)(UINTN)PcdGet64 (PcdFvBaseAddress);
  SecCoreData.BootFirmwareVolumeSize = PcdGet32 (PcdFvSize);
  SecCoreData.TemporaryRamBase       = (VOID *)TemporaryRamBase; // We run on the primary core (and so we use the first stack)
  SecCoreData.TemporaryRamSize       = TemporaryRamSize;
  SecCoreData.PeiTemporaryRamBase    = SecCoreData.TemporaryRamBase;
  SecCoreData.PeiTemporaryRamSize    = ALIGN_VALUE (SecCoreData.TemporaryRamSize / 2, CPU_STACK_ALIGNMENT);
  SecCoreData.StackBase              = (VOID *)((UINTN)SecCoreData.TemporaryRamBase + SecCoreData.PeiTemporaryRamSize);
  SecCoreData.StackSize              = (TemporaryRamBase + TemporaryRamSize) - (UINTN)SecCoreData.StackBase;

  // Jump to PEI core entry point
  PeiCoreEntryPoint (&SecCoreData, PpiList);
}

/**
  Calling this function causes a system-wide initialization. The processors
  are set to their initial state, and pending cycles are not corrupted.

  System reset should not return, if it returns, it means the system does
  not support warm reset.
**/
VOID
EFIAPI
ResetWarm (
  VOID
  )
{
  IoWrite8 ((UINTN) PcdGet64 (PcdResetControlRegister), PcdGet8 (PcdResetControlValueColdReset));
}

STATIC
PHYSICAL_ADDRESS
HostToDeviceAddress (
  IN  PHYSICAL_ADDRESS  HostAddress
  )
{
  return HostAddress + PcdGet64 (PcdArmDmaDeviceOffset);
}

/**
  Allocate private data for FTW driver and initialize it.

  @param[out] FtwData           Pointer to the FTW device structure

  @retval EFI_SUCCESS           Initialize the FTW device successfully.
  @retval EFI_OUT_OF_RESOURCES  Allocate memory error
  @retval EFI_INVALID_PARAMETER Workspace or Spare block does not exist

**/
EFI_STATUS
InitFtwDevice (
  OUT EFI_FTW_DEVICE               **FtwData 
  )
{
  EFI_FTW_DEVICE                   *FtwDevice;
  
  //
  // Allocate private data of this driver,
  // Including the FtwWorkSpace[FTW_WORK_SPACE_SIZE].
  //
  FtwDevice = AllocateZeroPool (sizeof (EFI_FTW_DEVICE) + PcdGet32 (PcdFlashNvStorageFtwWorkingSize));
  if (FtwDevice == NULL) {
    return EFI_OUT_OF_RESOURCES;
  }

  //
  // Initialize other parameters, and set WorkSpace as FTW_ERASED_BYTE.
  //
  FtwDevice->WorkSpaceLength  = (UINTN) PcdGet32 (PcdFlashNvStorageFtwWorkingSize);
  FtwDevice->SpareAreaLength  = (UINTN) PcdGet32 (PcdFlashNvStorageFtwSpareSize);
  if ((FtwDevice->WorkSpaceLength == 0) || (FtwDevice->SpareAreaLength == 0)) {
    DEBUG ((EFI_D_ERROR, "Ftw: Workspace or Spare block does not exist!\n"));
    FreePool (FtwDevice);
    return EFI_INVALID_PARAMETER;
  }

  FtwDevice->Signature        = FTW_DEVICE_SIGNATURE;
  FtwDevice->FtwFvBlock       = NULL;
  FtwDevice->FtwBackupFvb     = NULL;
  FtwDevice->FtwWorkSpaceLba  = (EFI_LBA) (-1);
  FtwDevice->FtwSpareLba      = (EFI_LBA) (-1);

  FtwDevice->WorkSpaceAddress = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageFtwWorkingBase64);
  if (FtwDevice->WorkSpaceAddress == 0) {
    FtwDevice->WorkSpaceAddress = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageFtwWorkingBase);
  }
  
  FtwDevice->SpareAreaAddress = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageFtwSpareBase64);
  if (FtwDevice->SpareAreaAddress == 0) {
    FtwDevice->SpareAreaAddress = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageFtwSpareBase);
  }  

  *FtwData = FtwDevice;
  return EFI_SUCCESS;
}

/**
  Get the serial device control bits.

  @param  Control                 Control signals read from the serial device.

  @retval EFI_SUCCESS             The control bits were read from the serial device.
  @retval EFI_DEVICE_ERROR        The serial device is not functioning correctly.

**/
RETURN_STATUS
EFIAPI
SerialPortGetControl (
  OUT UINT32                  *Control
  )
{
  return PL011UartGetControl ((UINTN)PcdGet64 (PcdSerialRegisterBase), Control);
}

/**
  Call before jumping to Normal World

  This function allows the firmware platform to do extra actions before
  jumping to the Normal World

**/
VOID
ArmPlatformSecExtraAction (
  IN  UINTN         MpId,
  OUT UINTN*        JumpAddress
  )
{
  *JumpAddress = PcdGet64 (PcdFvBaseAddress);
}

VOID CoreSelectBoot(VOID)
{
    if (!PcdGet64 (PcdTrustedFirmwareEnable))
    {
        StartupAp ();
    }

    return;
}

RETURN_STATUS
EFIAPI
PciExpressLibInitialize (
  VOID
  )
{
  mPciExpressBaseAddress = PcdGet64 (PcdPciExpressBaseAddress);
  return RETURN_SUCCESS;
}

/**
  HandlerInfo table address is set by PcdGuidedExtractHandlerTableAddress, which is used to store 
  the registered guid and Handler list. When it is initialized, it will be directly returned. 
  Or, HandlerInfo table will be initialized in this function.

  @param[in, out]  InfoPointer   The pointer to the handler information structure.

  @retval  RETURN_SUCCESS            HandlerInfo table can be used to store guid and function tables.
  @retval  RETURN_OUT_OF_RESOURCES   HandlerInfo table address is not writable.
**/
RETURN_STATUS
GetExtractGuidedSectionHandlerInfo (
  IN OUT EXTRACT_GUIDED_SECTION_HANDLER_INFO **InfoPointer
  )
{
  EXTRACT_GUIDED_SECTION_HANDLER_INFO *HandlerInfo;
  
  //
  // Set the available memory address to handler info.
  //
  HandlerInfo = (EXTRACT_GUIDED_SECTION_HANDLER_INFO*)(VOID*)(UINTN) PcdGet64 (PcdGuidedExtractHandlerTableAddress);
  if (HandlerInfo == NULL) {
    *InfoPointer = NULL;
    return EFI_OUT_OF_RESOURCES;
  }

  //
  // First check whether the handler information structure is initialized.
  //
  if (HandlerInfo->Signature == EXTRACT_HANDLER_INFO_SIGNATURE) {
    //
    // The handler information has been initialized and is returned.
    //
    *InfoPointer = HandlerInfo;
    return RETURN_SUCCESS;
  }

  //
  // Try to initialize the handler information structure
  //
  HandlerInfo->Signature = EXTRACT_HANDLER_INFO_SIGNATURE;
  if (HandlerInfo->Signature != EXTRACT_HANDLER_INFO_SIGNATURE) {
    //
    // The handler information structure was not writeable because the memory is not ready.
    //
    *InfoPointer = NULL;
    return RETURN_OUT_OF_RESOURCES;
  }

  //
  // Init HandlerInfo structure
  //
  HandlerInfo->NumberOfExtractHandler     = 0;
  HandlerInfo->ExtractHandlerGuidTable    = (GUID *) (HandlerInfo + 1);
  HandlerInfo->ExtractDecodeHandlerTable  = (EXTRACT_GUIDED_SECTION_DECODE_HANDLER *) (
                                              (UINT8 *)HandlerInfo->ExtractHandlerGuidTable + 
                                              PcdGet32 (PcdMaximumGuidedExtractHandler) * sizeof (GUID)
                                             );
  HandlerInfo->ExtractGetInfoHandlerTable = (EXTRACT_GUIDED_SECTION_GET_INFO_HANDLER *) (
                                              (UINT8 *)HandlerInfo->ExtractDecodeHandlerTable + 
                                              PcdGet32 (PcdMaximumGuidedExtractHandler) * 
                                              sizeof (EXTRACT_GUIDED_SECTION_DECODE_HANDLER)
                                             );
  *InfoPointer = HandlerInfo;
  return RETURN_SUCCESS;
}

/**
  SMM Fault Tolerant Write protocol notification event handler.

  Non-Volatile variable write may needs FTW protocol to reclaim when 
  writting variable.
  
  @param  Protocol   Points to the protocol's unique identifier
  @param  Interface  Points to the interface instance
  @param  Handle     The handle on which the interface was installed

  @retval EFI_SUCCESS   SmmEventCallback runs successfully
  @retval EFI_NOT_FOUND The Fvb protocol for variable is not found.
  
 **/
EFI_STATUS
EFIAPI
SmmFtwNotificationEvent (
  IN CONST EFI_GUID                       *Protocol,
  IN VOID                                 *Interface,
  IN EFI_HANDLE                           Handle
  )
{
  EFI_STATUS                              Status;
  EFI_SMM_FIRMWARE_VOLUME_BLOCK_PROTOCOL  *FvbProtocol;
  EFI_SMM_FAULT_TOLERANT_WRITE_PROTOCOL   *FtwProtocol;
  EFI_PHYSICAL_ADDRESS                    NvStorageVariableBase;
  
  if (mVariableModuleGlobal->FvbInstance != NULL) {
    return EFI_SUCCESS;
  }

  //
  // Ensure SMM FTW protocol is installed.
  //
  Status = GetFtwProtocol ((VOID **)&FtwProtocol);
  if (EFI_ERROR (Status)) {
    return Status;
  }

  //
  // Find the proper FVB protocol for variable.
  //
  NvStorageVariableBase = (EFI_PHYSICAL_ADDRESS) PcdGet64 (PcdFlashNvStorageVariableBase64);
  if (NvStorageVariableBase == 0) {
    NvStorageVariableBase = (EFI_PHYSICAL_ADDRESS) PcdGet32 (PcdFlashNvStorageVariableBase);
  }
  Status = GetFvbInfoByAddress (NvStorageVariableBase, NULL, &FvbProtocol);
  if (EFI_ERROR (Status)) {
    return EFI_NOT_FOUND;
  }

  mVariableModuleGlobal->FvbInstance = FvbProtocol;
  
  Status = VariableWriteServiceInitialize ();
  ASSERT_EFI_ERROR (Status);
 
  //
  // Notify the variable wrapper driver the variable write service is ready
  //
  Status = gBS->InstallProtocolInterface (
                  &mSmmVariableHandle,
                  &gSmmVariableWriteGuid,
                  EFI_NATIVE_INTERFACE,
                  NULL
                  );
  ASSERT_EFI_ERROR (Status);
  
  return EFI_SUCCESS;
}