EFI_STATUS
InitializePL031 (
  VOID
  )
{
  EFI_STATUS    Status;

  // Prepare the hardware
  Status = IdentifyPL031();
  if (EFI_ERROR (Status)) {
    goto EXIT;
  }

  // Ensure interrupts are masked. We do not want RTC interrupts in UEFI
  if ((MmioRead32 (mPL031RtcBase + PL031_RTC_IMSC_IRQ_MASK_SET_CLEAR_REGISTER) & PL031_SET_IRQ_MASK) != PL031_SET_IRQ_MASK) {
    MmioOr32 (mPL031RtcBase + PL031_RTC_IMSC_IRQ_MASK_SET_CLEAR_REGISTER, PL031_SET_IRQ_MASK);
  }

  // Clear any existing interrupts
  if ((MmioRead32 (mPL031RtcBase + PL031_RTC_RIS_RAW_IRQ_STATUS_REGISTER) & PL031_IRQ_TRIGGERED) == PL031_IRQ_TRIGGERED) {
    MmioOr32 (mPL031RtcBase + PL031_RTC_ICR_IRQ_CLEAR_REGISTER, PL031_CLEAR_IRQ);
  }

  // Start the clock counter
  if ((MmioRead32 (mPL031RtcBase + PL031_RTC_CR_CONTROL_REGISTER) & PL031_RTC_ENABLED) != PL031_RTC_ENABLED) {
    MmioOr32 (mPL031RtcBase + PL031_RTC_CR_CONTROL_REGISTER, PL031_RTC_ENABLED);
  }

  mPL031Initialized = TRUE;

  EXIT:
  return Status;
}

/**
  Return PTP interface type.

  @param[in] Register                Pointer to PTP register.

  @return PTP interface type.
**/
UINT8
GetPtpInterface (
  IN VOID *Register
  )
{
  PTP_CRB_INTERFACE_IDENTIFIER  InterfaceId;
  PTP_FIFO_INTERFACE_CAPABILITY InterfaceCapability;

  //
  // Check interface id
  //
  InterfaceId.Uint32 = MmioRead32 ((UINTN)&((PTP_CRB_REGISTERS *)Register)->InterfaceId);
  InterfaceCapability.Uint32 = MmioRead32 ((UINTN)&((PTP_FIFO_REGISTERS *)Register)->InterfaceCapability);

  if ((InterfaceId.Bits.InterfaceType == PTP_INTERFACE_IDENTIFIER_INTERFACE_TYPE_CRB) &&
      (InterfaceId.Bits.InterfaceVersion == PTP_INTERFACE_IDENTIFIER_INTERFACE_VERSION_CRB) &&
      (InterfaceId.Bits.CapCRB != 0)) {
    return TPM_DEVICE_INTERFACE_PTP_CRB;
  }
  if ((InterfaceId.Bits.InterfaceType == PTP_INTERFACE_IDENTIFIER_INTERFACE_TYPE_FIFO) &&
      (InterfaceId.Bits.InterfaceVersion == PTP_INTERFACE_IDENTIFIER_INTERFACE_VERSION_FIFO) &&
      (InterfaceId.Bits.CapFIFO != 0) &&
      (InterfaceCapability.Bits.InterfaceVersion == INTERFACE_CAPABILITY_INTERFACE_VERSION_PTP)) {
    return TPM_DEVICE_INTERFACE_PTP_FIFO;
  }
  return TPM_DEVICE_INTERFACE_TIS;
}

// Setup SP810's Timer2 for managing delay functions. And Timer3 for Performance counter
// Note: ArmVE's Timer0 and Timer1 are used by TimerDxe.
RETURN_STATUS
EFIAPI
TimerConstructor (
  VOID
  )
{
  // Check if the Metronome Timer is already initialized
  if ((MmioRead32 (SP804_TIMER_METRONOME_BASE + SP804_TIMER_CONTROL_REG) & SP804_TIMER_CTRL_ENABLE) == 0) {
    // Configure the Metronome Timer for free running operation, 32 bits, no prescaler, and interrupt disabled
    MmioWrite32 (SP804_TIMER_METRONOME_BASE + SP804_TIMER_CONTROL_REG, SP804_TIMER_CTRL_32BIT | SP804_PRESCALE_DIV_1);

    // Start the Metronome Timer ticking
    MmioOr32 (SP804_TIMER_METRONOME_BASE + SP804_TIMER_CONTROL_REG, SP804_TIMER_CTRL_ENABLE);
  }

  // Check if the Performance Timer is already initialized
  if ((MmioRead32 (SP804_TIMER_PERFORMANCE_BASE + SP804_TIMER_CONTROL_REG) & SP804_TIMER_CTRL_ENABLE) == 0) {
    // Configure the Performance timer for free running operation, 32 bits, no prescaler, interrupt disabled
    MmioWrite32 (SP804_TIMER_PERFORMANCE_BASE + SP804_TIMER_CONTROL_REG, SP804_TIMER_CTRL_32BIT | SP804_PRESCALE_DIV_1);

    // Start the Performance Timer ticking
    MmioOr32 (SP804_TIMER_PERFORMANCE_BASE + SP804_TIMER_CONTROL_REG, SP804_TIMER_CTRL_ENABLE);
  }

  return RETURN_SUCCESS;
}

UINTN
EFIAPI
NanoSecondDelay (
  IN  UINTN NanoSeconds
  )
{
  UINT32  Delay;
  UINT32  StartTime;
  UINT32  CurrentTime;
  UINT32  ElapsedTime;
  UINT32  TimerCountRegister;

  Delay = (NanoSeconds / PcdGet32(PcdEmbeddedPerformanceCounterPeriodInNanoseconds)) + 1;
  
  TimerCountRegister = TimerBase(PcdGet32(PcdOmap35xxFreeTimer)) + GPTIMER_TCRR;

  StartTime = MmioRead32 (TimerCountRegister);

  do 
  {
    CurrentTime = MmioRead32 (TimerCountRegister);
    ElapsedTime = CurrentTime - StartTime;
  } while (ElapsedTime < Delay);

  NanoSeconds = ElapsedTime * PcdGet32(PcdEmbeddedPerformanceCounterPeriodInNanoseconds);

  return NanoSeconds;
}

/**
  This function retrieves the period of timer interrupts in 100 ns units,
  returns that value in TimerPeriod, and returns EFI_SUCCESS.  If TimerPeriod
  is NULL, then EFI_INVALID_PARAMETER is returned.  If a TimerPeriod of 0 is
  returned, then the timer is currently disabled.

  @param  This             The EFI_TIMER_ARCH_PROTOCOL instance.
  @param  TimerPeriod      A pointer to the timer period to retrieve in 100 ns units. If
                           0 is returned, then the timer is currently disabled.


  @retval EFI_SUCCESS           The timer period was returned in TimerPeriod.
  @retval EFI_INVALID_PARAMETER TimerPeriod is NULL.

**/
EFI_STATUS
EFIAPI
SP805GetTimerPeriod (
  IN CONST EFI_WATCHDOG_TIMER_ARCH_PROTOCOL   *This,
  OUT UINT64                                  *TimerPeriod
  )
{
  EFI_STATUS  Status = EFI_SUCCESS;
  UINT64      ReturnValue;

  if (TimerPeriod == NULL) {
    return EFI_INVALID_PARAMETER;
  }

  // Check if the watchdog is stopped
  if ( (MmioRead32(SP805_WDOG_CONTROL_REG) & SP805_WDOG_CTRL_INTEN) == 0 ) {
    // It is stopped, so return zero.
    ReturnValue = 0;
  } else {
    // Convert the Watchdog ticks into TimerPeriod
    // Ensure 64bit arithmetic throughout because the Watchdog ticks may already
    // be at the maximum 32 bit value and we still need to multiply that by 600.
    ReturnValue = MultU64x32( MmioRead32(SP805_WDOG_LOAD_REG), 600 );
  }

  *TimerPeriod = ReturnValue;

  return Status;
}

EFI_STATUS
MMCReceiveResponse (
  IN EFI_MMC_HOST_PROTOCOL     *This,
  IN MMC_RESPONSE_TYPE         Type,
  IN UINT32*                   Buffer
  )
{
  if (Buffer == NULL) {
    return EFI_INVALID_PARAMETER;
  }

  if (Type == MMC_RESPONSE_TYPE_R2) {
    Buffer[0] = MmioRead32 (MMCHS_RSP10);
    Buffer[1] = MmioRead32 (MMCHS_RSP32);
    Buffer[2] = MmioRead32 (MMCHS_RSP54);
    Buffer[3] = MmioRead32 (MMCHS_RSP76);
  } else {
    Buffer[0] = MmioRead32 (MMCHS_RSP10);
  }

  if (Type == MMC_RESPONSE_TYPE_CSD) {
    mMaxDataTransferRate = Buffer[3] & 0xFF;
  } else if (Type == MMC_RESPONSE_TYPE_RCA) {
    mRca = Buffer[0] >> 16;
  }

VOID
XhciSwitchSwid(BOOLEAN enable)
{
  UINTN                             XhciPciMmBase;
  EFI_PHYSICAL_ADDRESS              XhciMemBaseAddress;
  UINT32                            DualRoleCfg0;
  UINT32                            DualRoleCfg1;

  XhciPciMmBase = MmPciAddress (0, 0, xhci_path.Device, xhci_path.Function, 0);
  XhciMemBaseAddress = MmioRead32 ((UINTN) (XhciPciMmBase + R_XHCI_MEM_BASE)) & B_XHCI_MEM_BASE_BA;
  DEBUG ((DEBUG_INFO, "XhciPciMmBase=%x, XhciMemBaseAddress=%x\n", XhciPciMmBase, XhciMemBaseAddress));

  DualRoleCfg0 = MmioRead32 ((UINTN)(XhciMemBaseAddress + R_XHCI_MEM_DUAL_ROLE_CFG0));
  if (enable) {
    DualRoleCfg0 = DualRoleCfg0 | (1 << 24) | (1 << 21) | (1 << 20);
    DEBUG ((DEBUG_INFO, "DualRoleCfg0 : Set SW ID : 0x%x \n", DualRoleCfg0));
  }
  else {
    DualRoleCfg0 = DualRoleCfg0 & ~(1 << 24) & ~(1 << 21) & ~(1 << 20);
    DEBUG ((DEBUG_INFO, "DualRoleCfg0 : Clear SW ID : 0x%x \n", DualRoleCfg0));
  }
  MmioWrite32 ((UINTN)(XhciMemBaseAddress + R_XHCI_MEM_DUAL_ROLE_CFG0), DualRoleCfg0);

  DualRoleCfg1 = MmioRead32 ((UINTN)(XhciMemBaseAddress + R_XHCI_MEM_DUAL_ROLE_CFG1));
  DEBUG ((DEBUG_INFO, "DualRoleCfg1 : 0x%x \n", DualRoleCfg1));
}

/**
  EFI_CPU_INTERRUPT_HANDLER that is called when a processor interrupt occurs.

  @param  InterruptType    Defines the type of interrupt or exception that
                           occurred on the processor.This parameter is processor architecture specific.
  @param  SystemContext    A pointer to the processor context when
                           the interrupt occurred on the processor.

  @return None

**/
VOID
EFIAPI
IrqInterruptHandler (
  IN EFI_EXCEPTION_TYPE           InterruptType,
  IN EFI_SYSTEM_CONTEXT           SystemContext
  )
{
  UINT32                      GicInterrupt;
  UINT32                      uwDomainID;
  HARDWARE_INTERRUPT_HANDLER  InterruptHandler;

  uwDomainID = MmioRead32 (PcdGet32(PcdGicDistributorBase) + PV660_GICD_DOMAINIDR);

  GicInterrupt = MmioRead32 (PcdGet32(PcdGicInterruptInterfaceBase) + ARM_GIC_ICCIAR);
  GicInterrupt -= uwDomainID*128;

  // Special Interrupts (ID1020-ID1023) have an Interrupt ID greater than the number of interrupt (ie: Spurious interrupt).
  if (GicInterrupt >= mGicNumInterrupts) {
    // The special interrupt do not need to be acknowledge
    return;
  }
  
  InterruptHandler = gRegisteredInterruptHandlers[GicInterrupt];
  if (InterruptHandler != NULL) {
    // Call the registered interrupt handler.
    InterruptHandler (GicInterrupt, SystemContext);
  } else {
    DEBUG ((EFI_D_ERROR, "Spurious GIC interrupt: 0x%x\n", GicInterrupt));
  }

  (VOID)EndOfInterrupt (&gHardwareInterruptProtocol, GicInterrupt);
}

/**
  Reads memory-mapped registers.

  @param[in]  PeiServices  An indirect pointer to the PEI Services Table
                           published by the PEI Foundation.
  @param[in]  This         Pointer to local data for the interface.
  @param[in]  Width        The width of the access. Enumerated in bytes.
  @param[in]  Address      The physical address of the access.
  @param[in]  Count        The number of accesses to perform.
  @param[out] Buffer       A pointer to the buffer of data.

  @retval EFI_SUCCESS            The function completed successfully.
  @retval EFI_INVALID_PARAMETER  Width is invalid for this EFI system.
  @retval EFI_INVALID_PARAMETER  Buffer is NULL.
  @retval EFI_UNSUPPORTED        The address range specified by Address, Width,
                                 and Count is not valid for this EFI system.

**/
EFI_STATUS
EFIAPI
CpuMemoryServiceRead (
  IN  CONST EFI_PEI_SERVICES    **PeiServices,
  IN  CONST EFI_PEI_CPU_IO_PPI  *This,
  IN  EFI_PEI_CPU_IO_PPI_WIDTH  Width,
  IN  UINT64                    Address,
  IN  UINTN                     Count,
  OUT VOID                      *Buffer
  )
{
  EFI_STATUS                Status;
  UINT8                     InStride;
  UINT8                     OutStride;
  EFI_PEI_CPU_IO_PPI_WIDTH  OperationWidth;
  BOOLEAN                   Aligned;
  UINT8                     *Uint8Buffer;

  Status = CpuIoCheckParameter (TRUE, Width, Address, Count, Buffer);
  if (EFI_ERROR (Status)) {
    return Status;
  }

  //
  // Select loop based on the width of the transfer
  //
  InStride = mInStride[Width];
  OutStride = mOutStride[Width];
  OperationWidth = (EFI_PEI_CPU_IO_PPI_WIDTH) (Width & 0x03);
  Aligned = (BOOLEAN)(((UINTN)Buffer & (mInStride[OperationWidth] - 1)) == 0x00);
  for (Uint8Buffer = Buffer; Count > 0; Address += InStride, Uint8Buffer += OutStride, Count--) {
    if (OperationWidth == EfiPeiCpuIoWidthUint8) {
      *Uint8Buffer = MmioRead8 ((UINTN)Address);
    } else if (OperationWidth == EfiPeiCpuIoWidthUint16) {
      if (Aligned) {
        *((UINT16 *)Uint8Buffer) = MmioRead16 ((UINTN)Address);
      } else {
        WriteUnaligned16 ((UINT16 *)Uint8Buffer, MmioRead16 ((UINTN)Address));
      }
    } else if (OperationWidth == EfiPeiCpuIoWidthUint32) {
      if (Aligned) {
        *((UINT32 *)Uint8Buffer) = MmioRead32 ((UINTN)Address);
      } else {
        WriteUnaligned32 ((UINT32 *)Uint8Buffer, MmioRead32 ((UINTN)Address));
      }
    } else if (OperationWidth == EfiPeiCpuIoWidthUint64) {
      if (Aligned) {
        *((UINT64 *)Uint8Buffer) = MmioRead64 ((UINTN)Address);
      } else {
        WriteUnaligned64 ((UINT64 *)Uint8Buffer, MmioRead64 ((UINTN)Address));
      }
    }
  }
  return EFI_SUCCESS;
}

/**
  EFI_CPU_INTERRUPT_HANDLER that is called when a processor interrupt occurs.

  @param  InterruptType    Defines the type of interrupt or exception that
                           occurred on the processor.This parameter is processor architecture specific.
  @param  SystemContext    A pointer to the processor context when
                           the interrupt occurred on the processor.

  @return None

**/
VOID
EFIAPI
IrqInterruptHandler (
  IN EFI_EXCEPTION_TYPE           InterruptType,
  IN EFI_SYSTEM_CONTEXT           SystemContext
  )
{
  UINT32                     Vector;
  HARDWARE_INTERRUPT_HANDLER InterruptHandler;
  
  Vector = MmioRead32 (INTCPS_SIR_IRQ) & INTCPS_SIR_IRQ_MASK;

  // Needed to prevent infinite nesting when Time Driver lowers TPL
  MmioWrite32 (INTCPS_CONTROL, INTCPS_CONTROL_NEWIRQAGR);
  ArmDataSyncronizationBarrier ();
  
  InterruptHandler = gRegisteredInterruptHandlers[Vector];
  if (InterruptHandler != NULL) {
    // Call the registered interrupt handler.
    InterruptHandler (Vector, SystemContext);
  }
  
  // Needed to clear after running the handler
  MmioWrite32 (INTCPS_CONTROL, INTCPS_CONTROL_NEWIRQAGR);
  ArmDataSyncronizationBarrier ();
}

/**
  Register Handler for the specified interrupt source.

  @param This     Instance pointer for this protocol
  @param Source   Hardware source of the interrupt
  @param Handler  Callback for interrupt. NULL to unregister

  @retval EFI_SUCCESS Source was updated to support Handler.
  @retval EFI_DEVICE_ERROR  Hardware could not be programmed.

**/
EFI_STATUS
EFIAPI
RegisterInterruptSource (
  IN EFI_HARDWARE_INTERRUPT_PROTOCOL    *This,
  IN HARDWARE_INTERRUPT_SOURCE          Source,
  IN HARDWARE_INTERRUPT_HANDLER         Handler
  )
{
  if (Source > MAX_VECTOR) {
    ASSERT(FALSE);
    return EFI_UNSUPPORTED;
  } 
  
  if ((MmioRead32 (INTCPS_ILR(Source)) & INTCPS_ILR_FIQ) == INTCPS_ILR_FIQ) {
    // This vector has been programmed as FIQ so we can't use it for IRQ
    // EFI does not use FIQ, but the debugger can use it to check for 
    // ctrl-c. So this ASSERT means you have a conflict with the debug agent
    ASSERT (FALSE);
    return EFI_UNSUPPORTED;
  }
  
  if ((Handler == NULL) && (gRegisteredInterruptHandlers[Source] == NULL)) {
    return EFI_INVALID_PARAMETER;
  }

  if ((Handler != NULL) && (gRegisteredInterruptHandlers[Source] != NULL)) {
    return EFI_ALREADY_STARTED;
  }

  gRegisteredInterruptHandlers[Source] = Handler;
  return This->EnableInterruptSource(This, Source);
}

/**                                                                 
  Turn of DMA channel configured by EnableDma().
            
  @param  Channel               DMA Channel to configure
  @param  SuccesMask            Bits in DMA4_CSR register indicate EFI_SUCCESS
  @param  ErrorMask             Bits in DMA4_CSR register indicate EFI_DEVICE_ERROR
                                  
  @retval EFI_SUCCESS           DMA hardware disabled
  @retval EFI_INVALID_PARAMETER Channel is not valid
  @retval EFI_DEVICE_ERROR      The system hardware could not map the requested information.
                                   
**/
EFI_STATUS
EFIAPI
DisableDmaChannel (
  IN  UINTN       Channel,
  IN  UINT32      SuccessMask,
  IN  UINT32      ErrorMask
  )
{
  EFI_STATUS  Status = EFI_SUCCESS;
  UINT32      Reg;


  if (Channel > DMA4_MAX_CHANNEL) {
    return EFI_INVALID_PARAMETER;
  }

  do {
    Reg = MmioRead32 (DMA4_CSR(Channel));
    if ((Reg & ErrorMask) != 0) {
      Status = EFI_DEVICE_ERROR;
      DEBUG ((EFI_D_ERROR, "DMA Error (%d) %x\n", Channel, Reg));
      break;
    }
  } while ((Reg & SuccessMask) != SuccessMask);


  // Disable all status bits and clear them
  MmioWrite32 (DMA4_CICR (Channel), 0);
  MmioWrite32 (DMA4_CSR (Channel),  DMA4_CSR_RESET);

  MmioAnd32 (DMA4_CCR(0), ~(DMA4_CCR_ENABLE | DMA4_CCR_RD_ACTIVE | DMA4_CCR_WR_ACTIVE)); 
  return Status;
}

/**
  Return current state of interrupt source Source.

  @param This     Instance pointer for this protocol
  @param Source   Hardware source of the interrupt
  @param InterruptState  TRUE: source enabled, FALSE: source disabled.

  @retval EFI_SUCCESS       InterruptState is valid
  @retval EFI_DEVICE_ERROR  InterruptState is not valid

**/
EFI_STATUS
EFIAPI
GetInterruptSourceState (
  IN EFI_HARDWARE_INTERRUPT_PROTOCOL    *This,
  IN HARDWARE_INTERRUPT_SOURCE          Source,
  IN BOOLEAN                            *InterruptState
  )
{
  UINTN Bank;
  UINTN Bit;
  
  if (InterruptState == NULL) {
    return EFI_INVALID_PARAMETER;
  }
  
  if (Source > MAX_VECTOR) {
    ASSERT(FALSE);
    return EFI_UNSUPPORTED;
  }

  Bank = Source / 32;
  Bit  = 1UL << (Source % 32);
    
  if ((MmioRead32(INTCPS_MIR(Bank)) & Bit) == Bit) {
    *InterruptState = FALSE;
  } else {
    *InterruptState = TRUE;
  }
  
  return EFI_SUCCESS;
}

/**

  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);

  // If the interrupt is shared then we must check if this interrupt source is the one associated to this Timer
  if (MmioRead32 (SP804_TIMER_PERIODIC_BASE + SP804_TIMER_MSK_INT_STS_REG) != 0) {
    // Clear the periodic interrupt
    MmioWrite32 (SP804_TIMER_PERIODIC_BASE + SP804_TIMER_INT_CLR_REG, 0);

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

    if (mTimerNotifyFunction) {
      mTimerNotifyFunction (mTimerPeriod);
    }
  }

  gBS->RestoreTPL (OriginalTPL);
}

VOID
EFIAPI
PlatformSpecificInit (
  VOID
  )
{
  UINTN                 XhciPciMmBase;
  EFI_PHYSICAL_ADDRESS  XhciMemBaseAddress;

  XhciPciMmBase   = MmPciAddress (
                      0,
                      0,
                      xhci_path.Device,
                      xhci_path.Function,
                      0
                      );


  XhciMemBaseAddress = MmioRead32 ((UINTN) (XhciPciMmBase + R_XHCI_MEM_BASE)) & B_XHCI_MEM_BASE_BA;
  DEBUG ((DEBUG_INFO, "XhciPciMmBase=%x, XhciMemBaseAddress=%x\n", XhciPciMmBase, XhciMemBaseAddress));

  MmioWrite32 ((UINTN)(XhciMemBaseAddress + R_XHCI_MEM_DUAL_ROLE_CFG0), 0x1310800);

  return;
}

/**
  EFI_CPU_INTERRUPT_HANDLER that is called when a processor interrupt occurs.

  @param  InterruptType    Defines the type of interrupt or exception that
                           occurred on the processor.This parameter is processor
                           architecture specific.
  @param  SystemContext    A pointer to the processor context when
                           the interrupt occurred on the processor.

  @return None

**/
STATIC
VOID
EFIAPI
IrqInterruptHandler (
  IN EFI_EXCEPTION_TYPE           InterruptType,
  IN EFI_SYSTEM_CONTEXT           SystemContext
  )
{
  HARDWARE_INTERRUPT_HANDLER  InterruptHandler;
  HARDWARE_INTERRUPT_SOURCE   Source;
  UINT32                      RegVal;

  RegVal = MmioRead32 (RegBase + BCM2836_INTC_TIMER_PENDING_OFFSET) &
    ((1 << NUM_IRQS) - 1);
  Source = HighBitSet32 (RegVal);
  if (Source < 0) {
    return;
  }

  InterruptHandler = mRegisteredInterruptHandlers[Source];
  if (InterruptHandler != NULL) {
    // Call the registered interrupt handler.
    InterruptHandler (Source, SystemContext);
  }
}

EFIAPI
MmioReadBuffer32 (
  IN  UINTN       StartAddress,
  IN  UINTN       Length,
  OUT UINT32      *Buffer
  )
{
  UINT32    *ReturnBuffer;

  ASSERT ((StartAddress & (sizeof (UINT32) - 1)) == 0);

  ASSERT ((Length - 1) <=  (MAX_ADDRESS - StartAddress));
  ASSERT ((Length - 1) <=  (MAX_ADDRESS - (UINTN) Buffer));

  ASSERT ((Length & (sizeof (UINT32) - 1)) == 0);
  ASSERT (((UINTN) Buffer & (sizeof (UINT32) - 1)) == 0);

  ReturnBuffer = Buffer;

  while (Length > 0) {
    *(Buffer++) = MmioRead32 (StartAddress);
    StartAddress += sizeof (UINT32);
    Length -= sizeof (UINT32);
  }

  return ReturnBuffer;
}

/**
  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);
  }
}

EFI_STATUS
ConfigureSciSmiGpioRout (
  IN EFI_PLATFORM_INFO_HOB       *PlatformInfo)
{
	UINT32 	GPI_Routing;

	GPI_Routing = MmioRead32 (PMC_BASE_ADDRESS + R_PCH_PMC_GPI_ROUT);

	//
	// For FAB3, Route GPIO_CORE 0 to cause Runtime SCI, GPIO_SUS 0 to cause Wake SCI and GPIO_SUS 7 to cause EXTSMI
	//
	if(PlatformInfo->BoardRev == 3) {
	GPI_Routing = GPI_Routing & 0xfffc3ffc;
	GPI_Routing = GPI_Routing | 0x00024002;
	}

	//
	// For FAB2/1, Route GPIO_CORE 7 to cause Runtime SCI, GPIO_SUS 0 to cause Wake SCI and GPIO_SUS 7 to cause EXTSMI
	//
	else {
	GPI_Routing = GPI_Routing & 0x3fff3ffc;
	GPI_Routing = GPI_Routing | 0x80004002;
	}
	MmioWrite32((PMC_BASE_ADDRESS + R_PCH_PMC_GPI_ROUT), GPI_Routing);

	return EFI_SUCCESS;
}

/**
  Initialize controllers that must setup at the early stage

  Some peripherals must be initialized in Secure World.
  For example, some L2x0 requires to be initialized in Secure World

**/
RETURN_STATUS
ArmPlatformSecInitialize (
  IN  UINTN                     MpId
  )
{
  UINT32  Identification;

  // If it is not the primary core then there is nothing to do
  if (!ArmPlatformIsPrimaryCore (MpId)) {
    return RETURN_SUCCESS;
  }

  // Configure periodic timer (TIMER0) for 1MHz operation
  MmioOr32 (SP810_CTRL_BASE + SP810_SYS_CTRL_REG, SP810_SYS_CTRL_TIMER0_TIMCLK);
  // Configure 1MHz clock
  MmioOr32 (SP810_CTRL_BASE + SP810_SYS_CTRL_REG, SP810_SYS_CTRL_TIMER1_TIMCLK);
  // Configure SP810 to use 1MHz clock and disable
  MmioAndThenOr32 (SP810_CTRL_BASE + SP810_SYS_CTRL_REG, ~SP810_SYS_CTRL_TIMER2_EN, SP810_SYS_CTRL_TIMER2_TIMCLK);
  // Configure SP810 to use 1MHz clock and disable
  MmioAndThenOr32 (SP810_CTRL_BASE + SP810_SYS_CTRL_REG, ~SP810_SYS_CTRL_TIMER3_EN, SP810_SYS_CTRL_TIMER3_TIMCLK);

  // Read the GIC Identification Register
  Identification = MmioRead32 (PcdGet32(PcdGicInterruptInterfaceBase) + ARM_GIC_ICCIDR);

  // Check if we are GICv3
  if (ARM_GIC_ICCIDR_GET_ARCH_VERSION(Identification) >= 0x3) {
    InitializeGicV3 ();
  }

  return RETURN_SUCCESS;
}