EFI_STATUS
EFIAPI
FindFv (
  IN EFI_PEI_FIND_FV_PPI          *This,
  IN CONST EFI_PEI_SERVICES             **PeiServices,
  IN OUT UINT8                    *FvNumber,
  OUT EFI_FIRMWARE_VOLUME_HEADER  **FVAddress
  )
{
	//
  // At present, we only have one Fv to search
  //
  if (*FvNumber == 0) {
    *FvNumber = 1;
    *FVAddress = (EFI_FIRMWARE_VOLUME_HEADER *)(UINTN)FixedPcdGet32 (PcdFlashFvMainBase);
    return EFI_SUCCESS;
  }
  else if (*FvNumber == 1) {
    *FvNumber = 2;
    *FVAddress = (EFI_FIRMWARE_VOLUME_HEADER *)(UINTN)FixedPcdGet32 (PcdFlashFvRecovery2Base);
    return EFI_SUCCESS;
  }
  else { // Not the one Fv we care about
    return EFI_NOT_FOUND;
  }
}

/**
  Measure FV image. 
  Add it into the measured FV list after the FV is measured successfully. 

  @param[in]  FvBase            Base address of FV image.
  @param[in]  FvLength          Length of FV image.

  @retval EFI_SUCCESS           Fv image is measured successfully 
                                or it has been already measured.
  @retval EFI_OUT_OF_RESOURCES  No enough memory to log the new event.
  @retval EFI_DEVICE_ERROR      The command was unsuccessful.

**/
EFI_STATUS
EFIAPI
MeasureFvImage (
  IN EFI_PHYSICAL_ADDRESS           FvBase,
  IN UINT64                         FvLength
  )
{
  UINT32                            Index;
  EFI_STATUS                        Status;
  EFI_PLATFORM_FIRMWARE_BLOB        FvBlob;
  TCG_PCR_EVENT_HDR                 TcgEventHdr;
  TIS_TPM_HANDLE                    TpmHandle;

  TpmHandle = (TIS_TPM_HANDLE) (UINTN) TPM_BASE_ADDRESS;

  //
  // Check whether FV is in the measured FV list.
  //
  for (Index = 0; Index < mMeasuredFvIndex; Index ++) {
    if (mMeasuredFvInfo[Index].BlobBase == FvBase) {
      return EFI_SUCCESS;
    }
  }
  
  //
  // Measure and record the FV to the TPM
  //
  FvBlob.BlobBase   = FvBase;
  FvBlob.BlobLength = FvLength;

  DEBUG ((DEBUG_INFO, "The FV which is measured by TcgPei starts at: 0x%x\n", FvBlob.BlobBase));
  DEBUG ((DEBUG_INFO, "The FV which is measured by TcgPei has the size: 0x%x\n", FvBlob.BlobLength));

  TcgEventHdr.PCRIndex = 0;
  TcgEventHdr.EventType = EV_EFI_PLATFORM_FIRMWARE_BLOB;
  TcgEventHdr.EventSize = sizeof (FvBlob);

  Status = HashLogExtendEvent (
             (EFI_PEI_SERVICES **) GetPeiServicesTablePointer(),
             (UINT8*) (UINTN) FvBlob.BlobBase,
             (UINTN) FvBlob.BlobLength,
             TpmHandle,
             &TcgEventHdr,
             (UINT8*) &FvBlob
             );
  ASSERT_EFI_ERROR (Status);

  //
  // Add new FV into the measured FV list.
  //
  ASSERT (mMeasuredFvIndex < FixedPcdGet32 (PcdPeiCoreMaxFvSupported));
  if (mMeasuredFvIndex < FixedPcdGet32 (PcdPeiCoreMaxFvSupported)) {
    mMeasuredFvInfo[mMeasuredFvIndex].BlobBase   = FvBase;
    mMeasuredFvInfo[mMeasuredFvIndex++].BlobLength = FvLength;
  }

  return Status;
}

EFI_STATUS
PciRbConstructor (
  IN  PCI_HOST_BRIDGE_INSTANCE *HostBridge,
  IN  UINT32 PciAcpiUid,
  IN  UINT64 MemAllocAttributes
  )
{
  PCI_ROOT_BRIDGE_INSTANCE* RootBridge;
  EFI_STATUS Status;

  PCI_TRACE ("PciRbConstructor()");

  // Allocate Memory for the Instance from a Template
  RootBridge = AllocateZeroPool (sizeof (PCI_ROOT_BRIDGE_INSTANCE));
  if (RootBridge == NULL) {
    PCI_TRACE ("PciRbConstructor(): ERROR: Out of Resources");
    return EFI_OUT_OF_RESOURCES;
  }
  RootBridge->Signature = PCI_ROOT_BRIDGE_SIGNATURE;
  CopyMem (&(RootBridge->DevicePath), &gDevicePathTemplate, sizeof (EFI_PCI_ROOT_BRIDGE_DEVICE_PATH));
  CopyMem (&(RootBridge->Io), &gIoTemplate, sizeof (EFI_PCI_ROOT_BRIDGE_IO_PROTOCOL));

  // Set Parent Handle
  RootBridge->Io.ParentHandle = HostBridge->Handle;

  // Attach the Root Bridge to the PCI Host Bridge Instance
  RootBridge->HostBridge = HostBridge;

  // Set Device Path for this Root Bridge
  RootBridge->DevicePath.Acpi.UID = PciAcpiUid;

  RootBridge->BusStart  = FixedPcdGet32 (PcdPciBusMin);
  RootBridge->BusLength = FixedPcdGet32 (PcdPciBusMax) - FixedPcdGet32 (PcdPciBusMin) + 1;

  // PCI Attributes
  RootBridge->Supports = 0;
  RootBridge->Attributes = 0;

  // Install Protocol Instances. It will also generate a device handle for the PCI Root Bridge
  Status = gBS->InstallMultipleProtocolInterfaces (
                      &RootBridge->Handle,
                      &gEfiDevicePathProtocolGuid, &RootBridge->DevicePath,
                      &gEfiPciRootBridgeIoProtocolGuid, &RootBridge->Io,
                      NULL
                      );
  ASSERT (RootBridge->Signature == PCI_ROOT_BRIDGE_SIGNATURE);
  if (EFI_ERROR (Status)) {
    PCI_TRACE ("PciRbConstructor(): ERROR: Fail to install Protocol Interfaces");
    FreePool (RootBridge);
    return EFI_DEVICE_ERROR;
  }

  HostBridge->RootBridge = RootBridge;
  return EFI_SUCCESS;
}

EFI_STATUS
EFIAPI
StyxSataPlatformDxeEntryPoint (
  IN EFI_HANDLE         ImageHandle,
  IN EFI_SYSTEM_TABLE   *SystemTable
  )
{
  UINT32                  PortNum;
  EFI_STATUS              Status;

  //
  // Perform SATA workarounds
  //
  for (PortNum = 0; PortNum < FixedPcdGet8(PcdSata0PortCount); PortNum++) {
      SetCwMinSata0 (PortNum);
  }

  Status = InitializeSataController (FixedPcdGet32(PcdSata0CtrlAxiSlvPort),
             FixedPcdGet8(PcdSata0PortCount), 0);
  if (EFI_ERROR (Status)) {
    DEBUG ((DEBUG_WARN, "%a: failed to initialize primary SATA controller!\n",
      __FUNCTION__));
    return Status;
  }

  for (PortNum = 0; PortNum < FixedPcdGet8(PcdSata0PortCount); PortNum++) {
      SetPrdSingleSata0 (PortNum);
  }

  //
  // Ignore the second SATA controller on pre-B1 silicon
  //
  if ((PcdGet32 (PcdSocCpuId) & STYX_SOC_VERSION_MASK) < STYX_SOC_VERSION_B1) {
    return EFI_SUCCESS;
  }

  if (FixedPcdGet8(PcdSata1PortCount) > 0) {
    for (PortNum = 0; PortNum < FixedPcdGet8(PcdSata1PortCount); PortNum++) {
        SetCwMinSata1 (PortNum);
    }

    Status = InitializeSataController (FixedPcdGet32(PcdSata1CtrlAxiSlvPort),
               FixedPcdGet8(PcdSata1PortCount),
               FixedPcdGet8(PcdSata0PortCount));
    if (EFI_ERROR (Status)) {
      DEBUG ((DEBUG_WARN, "%a: failed to initialize secondary SATA controller!\n",
        __FUNCTION__));
    } else {
      for (PortNum = 0; PortNum < FixedPcdGet8(PcdSata1PortCount); PortNum++) {
          SetPrdSingleSata1 (PortNum);
      }
    }
  }
  return EFI_SUCCESS;
}

/**
  Measure and record the Firmware Volum Information once FvInfoPPI install.

  @param[in] PeiServices       An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
  @param[in] NotifyDescriptor  Address of the notification descriptor data structure.
  @param[in] Ppi               Address of the PPI that was installed.

  @retval EFI_SUCCESS          The FV Info is measured and recorded to TPM.
  @return Others               Fail to measure FV.

**/
EFI_STATUS
EFIAPI
FirmwareVolmeInfoPpiNotifyCallback (
  IN EFI_PEI_SERVICES               **PeiServices,
  IN EFI_PEI_NOTIFY_DESCRIPTOR      *NotifyDescriptor,
  IN VOID                           *Ppi
  )
{
  EFI_PEI_FIRMWARE_VOLUME_INFO_PPI  *Fv;
  EFI_STATUS                        Status;
  EFI_PEI_FIRMWARE_VOLUME_PPI       *FvPpi;
  UINTN                             Index;

  Fv = (EFI_PEI_FIRMWARE_VOLUME_INFO_PPI *) Ppi;

  //
  // The PEI Core can not dispatch or load files from memory mapped FVs that do not support FvPpi.
  //
  Status = PeiServicesLocatePpi (
             &Fv->FvFormat, 
             0, 
             NULL,
             (VOID**)&FvPpi
             );
  if (EFI_ERROR (Status)) {
    return EFI_SUCCESS;
  }
  
  //
  // This is an FV from an FFS file, and the parent FV must have already been measured,
  // No need to measure twice, so just record the FV and return
  //
  if (Fv->ParentFvName != NULL || Fv->ParentFileName != NULL ) {
    
    ASSERT (mMeasuredChildFvIndex < FixedPcdGet32 (PcdPeiCoreMaxFvSupported));
    if (mMeasuredChildFvIndex < FixedPcdGet32 (PcdPeiCoreMaxFvSupported)) {
      //
      // Check whether FV is in the measured child FV list.
      //
      for (Index = 0; Index < mMeasuredChildFvIndex; Index++) {
        if (mMeasuredChildFvInfo[Index].BlobBase == (EFI_PHYSICAL_ADDRESS) (UINTN) Fv->FvInfo) {
          return EFI_SUCCESS;
        }
      }
      mMeasuredChildFvInfo[mMeasuredChildFvIndex].BlobBase   = (EFI_PHYSICAL_ADDRESS) (UINTN) Fv->FvInfo;
      mMeasuredChildFvInfo[mMeasuredChildFvIndex].BlobLength = Fv->FvInfoSize;
      mMeasuredChildFvIndex++;
    }
    return EFI_SUCCESS;
  }

  return MeasureFvImage ((EFI_PHYSICAL_ADDRESS) (UINTN) Fv->FvInfo, Fv->FvInfoSize);
}

/**

  Initialize PPI services.

  @param PrivateData     Pointer to the PEI Core data.
  @param OldCoreData     Pointer to old PEI Core data. 
                         NULL if being run in non-permament memory mode.

**/
VOID
InitializePpiServices (
  IN PEI_CORE_INSTANCE *PrivateData,
  IN PEI_CORE_INSTANCE *OldCoreData
  )
{
  if (OldCoreData == NULL) {
    PrivateData->PpiData.NotifyListEnd = FixedPcdGet32 (PcdPeiCoreMaxPpiSupported)-1;
    PrivateData->PpiData.DispatchListEnd = FixedPcdGet32 (PcdPeiCoreMaxPpiSupported)-1;
    PrivateData->PpiData.LastDispatchedNotify = FixedPcdGet32 (PcdPeiCoreMaxPpiSupported)-1;
  }
}

/**
  Initialize the state information for the Timer Architectural Protocol and
  the Timer Debug support protocol that allows the debugger to break into a
  running program.

  @param  ImageHandle   of the loaded driver
  @param  SystemTable   Pointer to the System Table

  @retval EFI_SUCCESS           Protocol registered
  @retval EFI_OUT_OF_RESOURCES  Cannot allocate protocol data structure
  @retval EFI_DEVICE_ERROR      Hardware problems

**/
EFI_STATUS
EFIAPI
TimerInitialize (
  IN EFI_HANDLE         ImageHandle,
  IN EFI_SYSTEM_TABLE   *SystemTable
  )
{
  EFI_HANDLE  Handle = NULL;
  EFI_STATUS  Status;
  UINT32      TimerBaseAddress;

  // Find the interrupt controller protocol.  ASSERT if not found.
  Status = gBS->LocateProtocol (&gHardwareInterruptProtocolGuid, NULL, (VOID **)&gInterrupt);
  ASSERT_EFI_ERROR (Status);

  // Set up the timer registers
  TimerBaseAddress = TimerBase (FixedPcdGet32(PcdOmap35xxArchTimer));
  TISR = TimerBaseAddress + GPTIMER_TISR;
  TCLR = TimerBaseAddress + GPTIMER_TCLR;
  TLDR = TimerBaseAddress + GPTIMER_TLDR;
  TCRR = TimerBaseAddress + GPTIMER_TCRR;
  TIER = TimerBaseAddress + GPTIMER_TIER;

  // Disable the timer
  Status = TimerDriverSetTimerPeriod (&gTimer, 0);
  ASSERT_EFI_ERROR (Status);

  // Install interrupt handler
  gVector = InterruptVectorForTimer (FixedPcdGet32(PcdOmap35xxArchTimer));
  Status = gInterrupt->RegisterInterruptSource (gInterrupt, gVector, TimerInterruptHandler);
  ASSERT_EFI_ERROR (Status);

  // Turn on the functional clock for Timer
  MmioOr32 (CM_FCLKEN_PER, CM_FCLKEN_PER_EN_GPT3_ENABLE);

  // Set up default timer
  Status = TimerDriverSetTimerPeriod (&gTimer, FixedPcdGet32(PcdTimerPeriod));
  ASSERT_EFI_ERROR (Status);

  // Install the Timer Architectural Protocol onto a new handle
  Status = gBS->InstallMultipleProtocolInterfaces (
                  &Handle,
                  &gEfiTimerArchProtocolGuid,      &gTimer,
                  NULL
                  );
  ASSERT_EFI_ERROR(Status);

  return Status;
}

VOID
UartInit (
  VOID
  )
{
  UINTN   Uart            = FixedPcdGet32(PcdOmap35xxConsoleUart);
  UINT32  UartBaseAddress = UartBase(Uart);

  // Set MODE_SELECT=DISABLE before trying to initialize or modify DLL, DLH registers.
  MmioWrite32 (UartBaseAddress + UART_MDR1_REG, UART_MDR1_MODE_SELECT_DISABLE);

  // Put device in configuration mode.
  MmioWrite32 (UartBaseAddress + UART_LCR_REG, UART_LCR_DIV_EN_ENABLE);

  // Programmable divisor N = 48Mhz/16/115200 = 26
  MmioWrite32 (UartBaseAddress + UART_DLL_REG, 3000000/FixedPcdGet64 (PcdUartDefaultBaudRate)); // low divisor
  MmioWrite32 (UartBaseAddress + UART_DLH_REG,  0); // high divisor

  // Enter into UART operational mode.
  MmioWrite32 (UartBaseAddress + UART_LCR_REG, UART_LCR_DIV_EN_DISABLE | UART_LCR_CHAR_LENGTH_8);

  // Force DTR and RTS output to active
  MmioWrite32 (UartBaseAddress + UART_MCR_REG, UART_MCR_RTS_FORCE_ACTIVE | UART_MCR_DTR_FORCE_ACTIVE);

  // Clear & enable fifos
  MmioWrite32 (UartBaseAddress + UART_FCR_REG, UART_FCR_TX_FIFO_CLEAR | UART_FCR_RX_FIFO_CLEAR | UART_FCR_FIFO_ENABLE);  

  // Restore MODE_SELECT 
  MmioWrite32 (UartBaseAddress + UART_MDR1_REG, UART_MDR1_MODE_SELECT_UART_16X);
}

VOID
TimerInit (
  VOID
  )
{
  UINTN  Timer            = FixedPcdGet32(PcdOmap35xxFreeTimer);
  UINT32 TimerBaseAddress = TimerBase(Timer);

  // Set source clock for GPT3 & GPT4 to SYS_CLK
  MmioOr32 (CM_CLKSEL_PER, CM_CLKSEL_PER_CLKSEL_GPT3_SYS | CM_CLKSEL_PER_CLKSEL_GPT4_SYS);

  // Set count & reload registers
  MmioWrite32 (TimerBaseAddress + GPTIMER_TCRR, 0x00000000);
  MmioWrite32 (TimerBaseAddress + GPTIMER_TLDR, 0x00000000);

  // Disable interrupts
  MmioWrite32 (TimerBaseAddress + GPTIMER_TIER, TIER_TCAR_IT_DISABLE | TIER_OVF_IT_DISABLE | TIER_MAT_IT_DISABLE);

  // Start Timer
  MmioWrite32 (TimerBaseAddress + GPTIMER_TCLR, TCLR_AR_AUTORELOAD | TCLR_ST_ON);

  //Disable OMAP Watchdog timer (WDT2)
  MmioWrite32 (WDTIMER2_BASE + WSPR, 0xAAAA);
  DEBUG ((EFI_D_ERROR, "Magic delay to disable watchdog timers properly.\n"));
  MmioWrite32 (WDTIMER2_BASE + WSPR, 0x5555);
}

/**

  C Interrupt Handler called in the interrupt context when Source interrupt is active.


  @param Source         Source of the interrupt. Hardware routing off a specific platform defines
                        what source means.

  @param SystemContext  Pointer to system register context. Mostly used by debuggers and will
                        update the system context after the return from the interrupt if
                        modified. Don't change these values unless you know what you are doing

**/
VOID
EFIAPI
TimerInterruptHandler (
  IN  HARDWARE_INTERRUPT_SOURCE   Source,
  IN  EFI_SYSTEM_CONTEXT          SystemContext
  )
{
  EFI_TPL      OriginalTPL;

  //
  // DXE core uses this callback for the EFI timer tick. The DXE core uses locks
  // that raise to TPL_HIGH and then restore back to current level. Thus we need
  // to make sure TPL level is set to TPL_HIGH while we are handling the timer tick.
  //
  OriginalTPL = gBS->RaiseTPL (TPL_HIGH_LEVEL);

  // Check if the timer interrupt is active
  if ((ArmArchTimerGetTimerCtrlReg () ) & ARM_ARCH_TIMER_ISTATUS) {

    // Signal end of interrupt early to help avoid losing subsequent ticks from long duration handlers
    gInterrupt->EndOfInterrupt (gInterrupt, Source);

    if (mTimerNotifyFunction) {
      mTimerNotifyFunction (mTimerPeriod);
    }

    // Reload the Timer
    TimerDriverSetTimerPeriod (&gTimer, FixedPcdGet32(PcdTimerPeriod));
  }

  // Enable timer interrupts
  gInterrupt->EnableInterruptSource (gInterrupt, Source);

  gBS->RestoreTPL (OriginalTPL);
}

/**
  Initialize the state information for the Timer Architectural Protocol and
  the Timer Debug support protocol that allows the debugger to break into a
  running program.

  @param  ImageHandle   of the loaded driver
  @param  SystemTable   Pointer to the System Table

  @retval EFI_SUCCESS           Protocol registered
  @retval EFI_OUT_OF_RESOURCES  Cannot allocate protocol data structure
  @retval EFI_DEVICE_ERROR      Hardware problems

**/
EFI_STATUS
EFIAPI
TimerInitialize (
  IN EFI_HANDLE         ImageHandle,
  IN EFI_SYSTEM_TABLE   *SystemTable
  )
{
  EFI_HANDLE  Handle = NULL;
  EFI_STATUS  Status;
  UINTN TimerCtrlReg;

  if (ArmIsArchTimerImplemented () == 0) {
    DEBUG ((EFI_D_ERROR, "ARM Architectural Timer is not available in the CPU, hence cann't use this Driver \n"));
    ASSERT (0);
  }

  // Find the interrupt controller protocol.  ASSERT if not found.
  Status = gBS->LocateProtocol (&gHardwareInterruptProtocolGuid, NULL, (VOID **)&gInterrupt);
  ASSERT_EFI_ERROR (Status);

  // Disable the timer
  TimerCtrlReg = ArmArchTimerGetTimerCtrlReg ();
  TimerCtrlReg |= ARM_ARCH_TIMER_IMASK;
  TimerCtrlReg &= ~ARM_ARCH_TIMER_ENABLE;
  ArmArchTimerSetTimerCtrlReg (TimerCtrlReg);
  Status = TimerDriverSetTimerPeriod (&gTimer, 0);
  ASSERT_EFI_ERROR (Status);

  // Install secure and Non-secure interrupt handlers
  // Note: Because it is not possible to determine the security state of the
  // CPU dynamically, we just install interrupt handler for both sec and non-sec
  // timer PPI
  Status = gInterrupt->RegisterInterruptSource (gInterrupt, PcdGet32 (PcdArmArchTimerSecIntrNum), TimerInterruptHandler);
  ASSERT_EFI_ERROR (Status);

  Status = gInterrupt->RegisterInterruptSource (gInterrupt, PcdGet32 (PcdArmArchTimerIntrNum), TimerInterruptHandler);
  ASSERT_EFI_ERROR (Status);

  // Set up default timer
  Status = TimerDriverSetTimerPeriod (&gTimer, FixedPcdGet32(PcdTimerPeriod)); // TIMER_DEFAULT_PERIOD
  ASSERT_EFI_ERROR (Status);

  // Install the Timer Architectural Protocol onto a new handle
  Status = gBS->InstallMultipleProtocolInterfaces(
                  &Handle,
                  &gEfiTimerArchProtocolGuid,      &gTimer,
                  NULL
                  );
  ASSERT_EFI_ERROR(Status);

  // Everything is ready, unmask and enable timer interrupts
  TimerCtrlReg = ARM_ARCH_TIMER_ENABLE;
  ArmArchTimerSetTimerCtrlReg (TimerCtrlReg);

  // Register for an ExitBootServicesEvent
  Status = gBS->CreateEvent (EVT_SIGNAL_EXIT_BOOT_SERVICES, TPL_NOTIFY, ExitBootServicesEvent, NULL, &EfiExitBootServicesEvent);
  ASSERT_EFI_ERROR (Status);

  return Status;
}

/**
  Call FspInit API.

  @param[in] FspHeader FSP header pointer.
**/
VOID
PeiFspInit (
  IN FSP_INFO_HEADER *FspHeader
  )
{
  FSP_INIT_PARAMS           FspInitParams;
  FSP_INIT_RT_COMMON_BUFFER FspRtBuffer;
  UINT8                     FspUpdRgn[FixedPcdGet32 (PcdMaxUpdRegionSize)];
  UINT32                    UpdRegionSize;
  EFI_BOOT_MODE             BootMode;
  UINT64                    StackSize;
  EFI_PHYSICAL_ADDRESS      StackBase;
  EFI_STATUS                Status;

  DEBUG ((DEBUG_INFO, "PeiFspInit enter\n"));

  PeiServicesGetBootMode (&BootMode);
  DEBUG ((DEBUG_INFO, "BootMode - 0x%x\n", BootMode));

  GetStackInfo (BootMode, FALSE, &StackBase, &StackSize);
  DEBUG ((DEBUG_INFO, "StackBase - 0x%x\n", StackBase));
  DEBUG ((DEBUG_INFO, "StackSize - 0x%x\n", StackSize));

  ZeroMem (&FspRtBuffer, sizeof(FspRtBuffer));
  FspRtBuffer.StackTop = (UINT32 *)(UINTN)(StackBase + StackSize);

  FspRtBuffer.BootMode = BootMode;

  /* Platform override any UPD configs */
  UpdRegionSize = GetUpdRegionSize();
  DEBUG ((DEBUG_INFO, "UpdRegionSize - 0x%x\n", UpdRegionSize));
  DEBUG ((DEBUG_INFO, "sizeof(FspUpdRgn) - 0x%x\n", sizeof(FspUpdRgn)));
  ASSERT(sizeof(FspUpdRgn) >= UpdRegionSize);
  ZeroMem (FspUpdRgn, UpdRegionSize);
  FspRtBuffer.UpdDataRgnPtr = UpdateFspUpdConfigs (FspUpdRgn);
  FspRtBuffer.BootLoaderTolumSize = 0;

  ZeroMem (&FspInitParams, sizeof(FspInitParams));
  FspInitParams.NvsBufferPtr = GetNvsBuffer ();
  DEBUG ((DEBUG_INFO, "NvsBufferPtr - 0x%x\n", FspInitParams.NvsBufferPtr));
  FspInitParams.RtBufferPtr  = (VOID *)&FspRtBuffer;
  FspInitParams.ContinuationFunc = (CONTINUATION_PROC)ContinuationFunc;

  SaveSecContext (GetPeiServicesTablePointer ());

  DEBUG ((DEBUG_INFO, "FspInitParams      - 0x%x\n", &FspInitParams));
  DEBUG ((DEBUG_INFO, "  NvsBufferPtr     - 0x%x\n", FspInitParams.NvsBufferPtr));
  DEBUG ((DEBUG_INFO, "  RtBufferPtr      - 0x%x\n", FspInitParams.RtBufferPtr));
  DEBUG ((DEBUG_INFO, "    StackTop       - 0x%x\n", FspRtBuffer.StackTop));
  DEBUG ((DEBUG_INFO, "    BootMode       - 0x%x\n", FspRtBuffer.BootMode));
  DEBUG ((DEBUG_INFO, "    UpdDataRgnPtr  - 0x%x\n", FspRtBuffer.UpdDataRgnPtr));
  DEBUG ((DEBUG_INFO, "  ContinuationFunc - 0x%x\n", FspInitParams.ContinuationFunc));

  Status = CallFspInit (FspHeader, &FspInitParams);
  //
  // Should never return
  //
  DEBUG((DEBUG_ERROR, "FSP Init failed, status: 0x%x\n", Status));
  CpuDeadLoop ();
}

/**
  Sets the baud rate, receive FIFO depth, transmit/receice time out, parity,
  data bits, and stop bits on a serial device.

  @param BaudRate           The requested baud rate. A BaudRate value of 0 will use the
                            device's default interface speed.
                            On output, the value actually set.
  @param ReveiveFifoDepth   The requested depth of the FIFO on the receive side of the
                            serial interface. A ReceiveFifoDepth value of 0 will use
                            the device's default FIFO depth.
                            On output, the value actually set.
  @param Timeout            The requested time out for a single character in microseconds.
                            This timeout applies to both the transmit and receive side of the
                            interface. A Timeout value of 0 will use the device's default time
                            out value.
                            On output, the value actually set.
  @param Parity             The type of parity to use on this serial device. A Parity value of
                            DefaultParity will use the device's default parity value.
                            On output, the value actually set.
  @param DataBits           The number of data bits to use on the serial device. A DataBits
                            vaule of 0 will use the device's default data bit setting.
                            On output, the value actually set.
  @param StopBits           The number of stop bits to use on this serial device. A StopBits
                            value of DefaultStopBits will use the device's default number of
                            stop bits.
                            On output, the value actually set.

  @retval RETURN_SUCCESS            The new attributes were set on the serial device.
  @retval RETURN_UNSUPPORTED        The serial device does not support this operation.
  @retval RETURN_INVALID_PARAMETER  One or more of the attributes has an unsupported value.
  @retval RETURN_DEVICE_ERROR       The serial device is not functioning correctly.

**/
RETURN_STATUS
EFIAPI
SerialPortSetAttributes (
  IN OUT UINT64             *BaudRate,
  IN OUT UINT32             *ReceiveFifoDepth,
  IN OUT UINT32             *Timeout,
  IN OUT EFI_PARITY_TYPE    *Parity,
  IN OUT UINT8              *DataBits,
  IN OUT EFI_STOP_BITS_TYPE *StopBits
  )
{
  RETURN_STATUS Status;

  if (mSerialBaseAddress == 0) {
    Status = RETURN_UNSUPPORTED;
  } else {
    Status = PL011UartInitializePort (
               mSerialBaseAddress,
               FixedPcdGet32 (PL011UartClkInHz),
               BaudRate,
               ReceiveFifoDepth,
               Parity,
               DataBits,
               StopBits
               );
  }

  return Status;
}

/**
  Return clock in for PL011 Uart IP

  @return Pcd PL011UartClkInHz
**/
UINT32
EFIAPI
PL011UartClockGetFreq (
  VOID
  )
{
  return FixedPcdGet32 (PL011UartClkInHz);
}

/**

  This function reinstalls an interface in the PEI PPI database by GUID. 
  The purpose of the service is to publish an interface that other parties can 
  use to replace an interface of the same name in the protocol database with a 
  different interface.

  @param PeiServices            An indirect pointer to the EFI_PEI_SERVICES table published by the PEI Foundation.
  @param OldPpi                 Pointer to the old PEI PPI Descriptors.
  @param NewPpi                 Pointer to the new PEI PPI Descriptors.

  @retval EFI_SUCCESS           if the operation was successful
  @retval EFI_INVALID_PARAMETER if OldPpi or NewPpi is NULL
  @retval EFI_INVALID_PARAMETER if NewPpi is not valid
  @retval EFI_NOT_FOUND         if the PPI was not in the database

**/
EFI_STATUS
EFIAPI
PeiReInstallPpi (
  IN CONST EFI_PEI_SERVICES        **PeiServices,
  IN CONST EFI_PEI_PPI_DESCRIPTOR  *OldPpi,
  IN CONST EFI_PEI_PPI_DESCRIPTOR  *NewPpi
  )
{
  PEI_CORE_INSTANCE   *PrivateData;
  INTN                Index;


  if ((OldPpi == NULL) || (NewPpi == NULL)) {
    return EFI_INVALID_PARAMETER;
  }

  if ((NewPpi->Flags & EFI_PEI_PPI_DESCRIPTOR_PPI) == 0) {
    return  EFI_INVALID_PARAMETER;
  }

  PrivateData = PEI_CORE_INSTANCE_FROM_PS_THIS(PeiServices);

  //
  // Find the old PPI instance in the database.  If we can not find it,
  // return the EFI_NOT_FOUND error.
  //
  for (Index = 0; Index < PrivateData->PpiData.PpiListEnd; Index++) {
    if (OldPpi == PrivateData->PpiData.PpiListPtrs[Index].Ppi) {
      break;
    }
  }
  if (Index == PrivateData->PpiData.PpiListEnd) {
    return EFI_NOT_FOUND;
  }

  //
  // Remove the old PPI from the database, add the new one.
  //
  DEBUG((EFI_D_INFO, "Reinstall PPI: %g\n", NewPpi->Guid));
  ASSERT (Index < (INTN)(FixedPcdGet32 (PcdPeiCoreMaxPpiSupported)));
  PrivateData->PpiData.PpiListPtrs[Index].Ppi = (EFI_PEI_PPI_DESCRIPTOR *) NewPpi;

  //
  // Dispatch any callback level notifies for the newly installed PPI.
  //
  DispatchNotify (
    PrivateData,
    EFI_PEI_PPI_DESCRIPTOR_NOTIFY_CALLBACK,
    Index,
    Index+1,
    PrivateData->PpiData.DispatchListEnd,
    PrivateData->PpiData.NotifyListEnd
    );


  return EFI_SUCCESS;
}

VOID
ArchInitialize (
  VOID
  )
{
  // Enable Floating Point
  if (FixedPcdGet32 (PcdVFPEnabled)) {
    ArmEnableVFP ();
  }
}

/**

  Process the Notify List at dispatch level.

  @param PrivateData  PeiCore's private data structure.

**/
VOID
ProcessNotifyList (
  IN PEI_CORE_INSTANCE  *PrivateData
  )
{
  INTN                    TempValue;

  while (TRUE) {
    //
    // Check if the PEIM that was just dispatched resulted in any
    // Notifies getting installed.  If so, go process any dispatch
    // level Notifies that match the previouly installed PPIs.
    // Use "while" instead of "if" since DispatchNotify can modify
    // DispatchListEnd (with NotifyPpi) so we have to iterate until the same.
    //
    while (PrivateData->PpiData.LastDispatchedNotify != PrivateData->PpiData.DispatchListEnd) {
      TempValue = PrivateData->PpiData.DispatchListEnd;
      DispatchNotify (
        PrivateData,
        EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH,
        0,
        PrivateData->PpiData.LastDispatchedInstall,
        PrivateData->PpiData.LastDispatchedNotify,
        PrivateData->PpiData.DispatchListEnd
        );
      PrivateData->PpiData.LastDispatchedNotify = TempValue;
    }


    //
    // Check if the PEIM that was just dispatched resulted in any
    // PPIs getting installed.  If so, go process any dispatch
    // level Notifies that match the installed PPIs.
    // Use "while" instead of "if" since DispatchNotify can modify
    // PpiListEnd (with InstallPpi) so we have to iterate until the same.
    //
    while (PrivateData->PpiData.LastDispatchedInstall != PrivateData->PpiData.PpiListEnd) {
      TempValue = PrivateData->PpiData.PpiListEnd;
      DispatchNotify (
        PrivateData,
        EFI_PEI_PPI_DESCRIPTOR_NOTIFY_DISPATCH,
        PrivateData->PpiData.LastDispatchedInstall,
        PrivateData->PpiData.PpiListEnd,
        FixedPcdGet32 (PcdPeiCoreMaxPpiSupported)-1,
        PrivateData->PpiData.DispatchListEnd
        );
      PrivateData->PpiData.LastDispatchedInstall = TempValue;
    }

    if (PrivateData->PpiData.LastDispatchedNotify == PrivateData->PpiData.DispatchListEnd) {
      break;
    }
  }
  return;
}

/**

  Migrate PPI Pointers from the temporary memory stack to PEI installed memory.

  @param SecCoreData     Points to a data structure containing SEC to PEI handoff data, such as the size 
                         and location of temporary RAM, the stack location and the BFV location.
  @param PrivateData     Pointer to PeiCore's private data structure.

**/
VOID
ConvertPpiPointers (
  IN CONST EFI_SEC_PEI_HAND_OFF  *SecCoreData,
  IN PEI_CORE_INSTANCE           *PrivateData
  )
{
  UINT8                 Index;
  UINT8                 IndexHole;

  for (Index = 0; Index < FixedPcdGet32 (PcdPeiCoreMaxPpiSupported); Index++) {
    if (Index < PrivateData->PpiData.PpiListEnd || Index > PrivateData->PpiData.NotifyListEnd) {
      //
      // Convert PPI pointer in old Heap
      //
      ConverSinglePpiPointer (
        &PrivateData->PpiData.PpiListPtrs[Index],
        (UINTN)SecCoreData->PeiTemporaryRamBase,
        (UINTN)SecCoreData->PeiTemporaryRamBase + SecCoreData->PeiTemporaryRamSize,
        PrivateData->HeapOffset,
        PrivateData->HeapOffsetPositive
        );
        
      //
      // Convert PPI pointer in old Stack
      //
      ConverSinglePpiPointer (
        &PrivateData->PpiData.PpiListPtrs[Index],
        (UINTN)SecCoreData->StackBase,
        (UINTN)SecCoreData->StackBase + SecCoreData->StackSize,
        PrivateData->StackOffset,
        PrivateData->StackOffsetPositive
        );
        
      //
      // Convert PPI pointer in old TempRam Hole
      //
      for (IndexHole = 0; IndexHole < HOLE_MAX_NUMBER; IndexHole ++) {
        if (PrivateData->HoleData[IndexHole].Size == 0) {
          continue;
        }
        
        ConverSinglePpiPointer (
          &PrivateData->PpiData.PpiListPtrs[Index],
          (UINTN)PrivateData->HoleData[IndexHole].Base,
          (UINTN)PrivateData->HoleData[IndexHole].Base + PrivateData->HoleData[IndexHole].Size,
          PrivateData->HoleData[IndexHole].Offset,
          PrivateData->HoleData[IndexHole].OffsetPositive
          );
      }
    }
  }
}

VOID
ArchInitialize (
  VOID
  )
{
  // Enable Floating Point
  if (FixedPcdGet32 (PcdVFPEnabled)) {
    ArmEnableVFP ();
  }

  if (ArmReadCurrentEL () == AARCH64_EL2) {
    // Trap General Exceptions. All exceptions that would be routed to EL1 are routed to EL2
    ArmWriteHcr (ARM_HCR_TGE);
  }
}

/**
  Initialize the state information for the Timer Architectural Protocol and
  the Timer Debug support protocol that allows the debugger to break into a
  running program.

  @param  ImageHandle   of the loaded driver
  @param  SystemTable   Pointer to the System Table

  @retval EFI_SUCCESS           Protocol registered
  @retval EFI_OUT_OF_RESOURCES  Cannot allocate protocol data structure
  @retval EFI_DEVICE_ERROR      Hardware problems

**/
EFI_STATUS
EFIAPI
TimerInitialize (
  IN EFI_HANDLE         ImageHandle,
  IN EFI_SYSTEM_TABLE   *SystemTable
  )
{
  EFI_HANDLE  Handle = NULL;
  EFI_STATUS  Status;

  // Set the interrupt timer number
  gVector = PcdGet32(PcdSP804TimerPeriodicInterruptNum);

  // Find the interrupt controller protocol.  ASSERT if not found.
  Status = gBS->LocateProtocol (&gHardwareInterruptProtocolGuid, NULL, (VOID **)&gInterrupt);
  ASSERT_EFI_ERROR (Status);

  // Disable the timer
  Status = TimerDriverSetTimerPeriod (&gTimer, 0);
  ASSERT_EFI_ERROR (Status);

  // Install interrupt handler
  Status = gInterrupt->RegisterInterruptSource (gInterrupt, gVector, TimerInterruptHandler);
  ASSERT_EFI_ERROR (Status);

  // configure timer 0 for periodic operation, 32 bits, no prescaler, and interrupt enabled
  MmioWrite32 (SP804_TIMER_PERIODIC_BASE + SP804_TIMER_CONTROL_REG, SP804_TIMER_CTRL_PERIODIC | SP804_TIMER_CTRL_32BIT | SP804_PRESCALE_DIV_1 | SP804_TIMER_CTRL_INT_ENABLE);

  // Set up default timer
  Status = TimerDriverSetTimerPeriod (&gTimer, FixedPcdGet32(PcdTimerPeriod)); // TIMER_DEFAULT_PERIOD
  ASSERT_EFI_ERROR (Status);

  // Install the Timer Architectural Protocol onto a new handle
  Status = gBS->InstallMultipleProtocolInterfaces(
                  &Handle,
                  &gEfiTimerArchProtocolGuid,      &gTimer,
                  NULL
                  );
  ASSERT_EFI_ERROR(Status);

  // Register for an ExitBootServicesEvent
  Status = gBS->CreateEvent (EVT_SIGNAL_EXIT_BOOT_SERVICES, TPL_NOTIFY, ExitBootServicesEvent, NULL, &EfiExitBootServicesEvent);
  ASSERT_EFI_ERROR (Status);

  return Status;
}