/**
  Detect whether the disk in the drive is changed or not.
  
  @param[in] FdcDev  A pointer to FDC_BLK_IO_DEV
  
  @retval  EFI_SUCCESS        No disk media change
  @retval  EFI_DEVICE_ERROR   Fail to do the recalibrate or seek operation
  @retval  EFI_NO_MEDIA       No disk in the drive
  @retval  EFI_MEDIA_CHANGED  There is a new disk in the drive
**/
EFI_STATUS
DisketChanged (
  IN FDC_BLK_IO_DEV  *FdcDev
  )
{
  EFI_STATUS  Status;
  UINT8       Data;

  //
  // Check change line
  //
  Data = FdcReadPort (FdcDev, FDC_REGISTER_DIR);

  //
  // Io delay
  //
  MicroSecondDelay (50);

  if ((Data & DIR_DCL) == 0x80) {
    //
    // disk change line is active
    //
    if (FdcDev->PresentCylinderNumber != 0) {
      Status = Recalibrate (FdcDev);
    } else {
      Status = Seek (FdcDev, 0x30);
    }

    if (EFI_ERROR (Status)) {
      FdcDev->ControllerState->NeedRecalibrate = TRUE;
      return EFI_DEVICE_ERROR;
      //
      // Fail to do the seek or recalibrate operation
      //
    }

    Data = FdcReadPort (FdcDev, FDC_REGISTER_DIR);

    //
    // Io delay
    //
    MicroSecondDelay (50);

    if ((Data & DIR_DCL) == 0x80) {
      return EFI_NO_MEDIA;
    }

    return EFI_MEDIA_CHANGED;
  }

  return EFI_SUCCESS;
}

EFI_STATUS
OhciSetMemoryPointer(
  IN USB_OHCI_HC_DEV      *Ohc,
  IN UINTN                PointerType,
  IN VOID                 *Value
  )
{
  EFI_STATUS              Status;
  UINT32                  Verify;

  Status = OhciSetOperationalReg (Ohc, PointerType, (UINT32*)&Value);

  if (EFI_ERROR (Status)) {
    return Status;
  }

  Verify = OhciGetOperationalReg (Ohc, PointerType);

  while (Verify != (UINT32) Value) {
    MicroSecondDelay (HC_1_MILLISECOND);
    Verify = OhciGetOperationalReg (Ohc, PointerType);
  };


  return Status;
}

/**
  Get BurstCount by reading the burstCount field of a TIS regiger 
  in the time of default TIS_TIMEOUT_D.

  @param[in]  TisReg                Pointer to TIS register.
  @param[out] BurstCount            Pointer to a buffer to store the got BurstConut.

  @retval     EFI_SUCCESS           Get BurstCount.
  @retval     EFI_INVALID_PARAMETER TisReg is NULL or BurstCount is NULL.
  @retval     EFI_TIMEOUT           BurstCount can't be got in time.
**/
EFI_STATUS
EFIAPI
TisPcReadBurstCount (
  IN      TIS_PC_REGISTERS_PTR      TisReg,
     OUT  UINT16                    *BurstCount
  )
{
  UINT32                            WaitTime;
  UINT8                             DataByte0;
  UINT8                             DataByte1;

  if (BurstCount == NULL || TisReg == NULL) {
    return EFI_INVALID_PARAMETER;
  }

  WaitTime = 0;
  do {
    //
    // TIS_PC_REGISTERS_PTR->burstCount is UINT16, but it is not 2bytes aligned,
    // so it needs to use MmioRead8 to read two times
    //
    DataByte0   = MmioRead8 ((UINTN)&TisReg->BurstCount);
    DataByte1   = MmioRead8 ((UINTN)&TisReg->BurstCount + 1);
    *BurstCount = (UINT16)((DataByte1 << 8) + DataByte0);
    if (*BurstCount != 0) {
      return EFI_SUCCESS;
    }
    MicroSecondDelay (30);
    WaitTime += 30;
  } while (WaitTime < TIS_TIMEOUT_D);

  return EFI_TIMEOUT;
}

/**
  Write command byte to Data Register of FDC.
  
  @param FdcDev  Pointer to instance of FDC_BLK_IO_DEV
  @param Pointer Be used to save command byte written to FDC
  
  @retval  EFI_SUCCESS:    Write command byte to FDC successfully
  @retval  EFI_DEVICE_ERROR: The FDC is not ready to be written

**/
EFI_STATUS
DataOutByte (
  IN FDC_BLK_IO_DEV  *FdcDev,
  IN UINT8           *Pointer
  )
{
  UINT8 Data;

  //
  // wait for 1ms and detect the FDC is ready to be written
  //
  if (EFI_ERROR (FddDRQReady (FdcDev, DATA_OUT, 1))) {
    //
    // Not ready
    //
    return EFI_DEVICE_ERROR;
  }

  Data = *Pointer;

  FdcWritePort (FdcDev, FDC_REGISTER_DTR, Data);

  //
  // Io delay
  //
  MicroSecondDelay (50);

  return EFI_SUCCESS;
}

/**
  When the Timer(2s) off, turn the drive's motor off.
  
  @param Event EFI_EVENT: Event(the timer) whose notification function is being
                     invoked
  @param Context VOID *:  Pointer to the notification function's context

**/
VOID
EFIAPI
FddTimerProc (
  IN EFI_EVENT  Event,
  IN VOID       *Context
  )
{
  FDC_BLK_IO_DEV  *FdcDev;
  UINT8           Data;

  FdcDev = (FDC_BLK_IO_DEV *) Context;

  //
  // Get the motor status
  //
  Data = FdcReadPort (FdcDev, FDC_REGISTER_DOR);

  if (((FdcDev->Disk == FdcDisk0) && ((Data & 0x10) != 0x10)) ||
      ((FdcDev->Disk == FdcDisk1) && ((Data & 0x21) != 0x21))
      ) {
    return ;
  }
  //
  // the motor is on, so need motor off
  //
  Data = 0x0C;
  Data = (UINT8) (Data | (SELECT_DRV & FdcDev->Disk));
  FdcWritePort (FdcDev, FDC_REGISTER_DOR, Data);
  MicroSecondDelay (500);
}

/**
  Read result byte from Data Register of FDC.
  
  @param FdcDev   Pointer to instance of FDC_BLK_IO_DEV
  @param Pointer  Buffer to store the byte read from FDC
  
  @retval EFI_SUCCESS       Read result byte from FDC successfully
  @retval EFI_DEVICE_ERROR  The FDC is not ready to be read

**/
EFI_STATUS
DataInByte (
  IN  FDC_BLK_IO_DEV  *FdcDev,
  OUT UINT8           *Pointer
  )
{
  UINT8 Data;

  //
  // wait for 1ms and detect the FDC is ready to be read
  //
  if (EFI_ERROR (FddDRQReady (FdcDev, DATA_IN, 1))) {
    return EFI_DEVICE_ERROR;
    //
    // is not ready
    //
  }

  Data = FdcReadPort (FdcDev, FDC_REGISTER_DTR);

  //
  // Io delay
  //
  MicroSecondDelay (50);

  *Pointer = Data;
  return EFI_SUCCESS;
}

EFI_STATUS
RtcWaitToUpdate (
  UINTN Timeout
  )
{
  RTC_REGISTER_A  RegisterA;
  RTC_REGISTER_D  RegisterD;

  //
  // See if the RTC is functioning correctly
  //
  RegisterD.Data = RtcRead (RTC_ADDRESS_REGISTER_D);

  if (RegisterD.Bits.VRT == 0) {
    return EFI_DEVICE_ERROR;
  }
  //
  // Wait for up to 0.1 seconds for the RTC to be ready.
  //
  Timeout         = (Timeout / 10) + 1;
  RegisterA.Data  = RtcRead (RTC_ADDRESS_REGISTER_A);
  while (RegisterA.Bits.UIP == 1 && Timeout > 0) {
    MicroSecondDelay (10);
    RegisterA.Data = RtcRead (RTC_ADDRESS_REGISTER_A);
    Timeout--;
  }

  RegisterD.Data = RtcRead (RTC_ADDRESS_REGISTER_D);
  if (Timeout == 0 || RegisterD.Bits.VRT == 0) {
    return EFI_DEVICE_ERROR;
  }

  return EFI_SUCCESS;
}

/** Early initialisation of the PCIe controller.

  @param   PlatformType     See EFI_PLATFORM_TYPE enum definitions.

  @retval   EFI_SUCCESS               Operation success.

**/
EFI_STATUS
EFIAPI
PlatformPciExpressEarlyInit (
  IN CONST EFI_PLATFORM_TYPE              PlatformType
  )
{

  //
  // Release and wait for PCI controller to come out of reset.
  //
  SocUnitReleasePcieControllerPreWaitPllLock (PlatformType);
  MicroSecondDelay (PCIEXP_DELAY_US_WAIT_PLL_LOCK);
  SocUnitReleasePcieControllerPostPllLock (PlatformType);

  //
  // Early PCIe initialisation
  //
  SocUnitEarlyInitialisation ();

  //
  // Do North cluster early PCIe init.
  //
  PciExpressEarlyInit ();

  return EFI_SUCCESS;
}

/**
  Get BurstCount by reading the burstCount field of a TIS register
  in the time of default TIS_TIMEOUT_D.

  @param[out] BurstCount  Pointer to a buffer to store the got BurstConut.

  @retval EFI_SUCCESS            Get BurstCount.
  @retval EFI_INVALID_PARAMETER  BurstCount is NULL.
  @retval EFI_TIMEOUT            BurstCount can't be got in time.
**/
EFI_STATUS
TisPcReadBurstCount (
    OUT UINT16  *BurstCount
)
{
    UINT32  WaitTime;
    UINT8   DataByte0;
    UINT8   DataByte1;

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

    WaitTime = 0;
    do {
        //
        // BurstCount is UINT16, but it is not 2bytes aligned,
        // so it needs to use TpmReadByte to read two times
        //
        DataByte0   = TpmReadByte (INFINEON_TPM_BURST0_COUNT_0_DEFAULT);
        DataByte1   = TpmReadByte (INFINEON_TPM_BURST1_COUNT_0_DEFAULT);
        *BurstCount = (UINT16)((DataByte1 << 8) + DataByte0);
        if (*BurstCount != 0) {
            return EFI_SUCCESS;
        }
        MicroSecondDelay (30);
        WaitTime += 30;
    } while (WaitTime < TIS_TIMEOUT_D);

    return EFI_TIMEOUT;
}

/**
  Set the data rate and so on.
 
  @param  FdcDev  A pointer to FDC_BLK_IO_DEV

  @retval EFI_SUCCESS success to set the data rate
**/
EFI_STATUS
Setup (
  IN FDC_BLK_IO_DEV  *FdcDev
  )
{
  EFI_STATUS  Status;

  //
  // Set data rate 500kbs
  //
  FdcWritePort (FdcDev, FDC_REGISTER_CCR, 0x0);

  //
  // Io delay
  //
  MicroSecondDelay (50);

  Status = Specify (FdcDev);

  if (EFI_ERROR (Status)) {
    return EFI_DEVICE_ERROR;
  }

  return EFI_SUCCESS;
}

BOOLEAN
WaitForSpiCycleComplete (
  IN     EFI_SPI_PROTOCOL   *This,
  IN     BOOLEAN            ErrorCheck
  )
/*++

Routine Description:

  Wait execution cycle to complete on the SPI interface. Check both Hardware
  and Software Sequencing status registers

Arguments:

  This                - The SPI protocol instance
  UseSoftwareSequence - TRUE if this is a Hardware Sequencing operation
  ErrorCheck          - TRUE if the SpiCycle needs to do the error check

Returns:

  TRUE       SPI cycle completed on the interface.
  FALSE      Time out while waiting the SPI cycle to complete.
             It's not safe to program the next command on the SPI interface.

--*/
{
  UINT64        WaitTicks;
  UINT64        WaitCount;
  UINT16        Data16;
  SPI_INSTANCE  *SpiInstance;
  UINTN         PchRootComplexBar;

  SpiInstance       = SPI_INSTANCE_FROM_SPIPROTOCOL (This);
  PchRootComplexBar = SpiInstance->PchRootComplexBar;

  //
  // Convert the wait period allowed into to tick count
  //
  WaitCount = WAIT_TIME / WAIT_PERIOD;

  //
  // Wait for the SPI cycle to complete.
  //
  for (WaitTicks = 0; WaitTicks < WaitCount; WaitTicks++) {
    Data16 = MmioRead16 (PchRootComplexBar + R_QNC_RCRB_SPIS);
    if ((Data16 & B_QNC_RCRB_SPIS_SCIP) == 0) {
      MmioWrite16 (PchRootComplexBar + R_QNC_RCRB_SPIS, (B_QNC_RCRB_SPIS_BAS | B_QNC_RCRB_SPIS_CDS));
      if ((Data16 & B_QNC_RCRB_SPIS_BAS) && (ErrorCheck == TRUE)) {
        return FALSE;
      } else {
        return TRUE;
      }
    }

    MicroSecondDelay (WAIT_PERIOD);
  }

  return FALSE;
}

EFI_STATUS
SwitchRtcI2cChannelAndLock (
  VOID
  )
{
  UINT8   Temp;
  UINT8   Count;

  for (Count = 0; Count < 20; Count++) {
    Temp = ReadCpldReg (CPLD_I2C_SWITCH_FLAG);

    if ((Temp & BMC_I2C_STATUS) != 0) {
      //The I2C channel is shared with BMC,
      //Check if BMC has taken ownership of I2C.
      //If so, wait 30ms, then try again.
      //If not, start using I2C.
      //And the CPLD_I2C_SWITCH_FLAG will be set to CPU_GET_I2C_CONTROL
      //BMC will check this flag to decide to use I2C or not.
      MicroSecondDelay (30000);
      continue;
    }

    Temp = ReadCpldReg (CPLD_I2C_SWITCH_FLAG);
    Temp = Temp | CPU_GET_I2C_CONTROL;
    WriteCpldReg (CPLD_I2C_SWITCH_FLAG, Temp);

    //This is empirical value,give cpld some time to make sure the
    //value is wrote in
    MicroSecondDelay (2);
    Temp = ReadCpldReg (CPLD_I2C_SWITCH_FLAG);

    if ((Temp & CPU_GET_I2C_CONTROL) == CPU_GET_I2C_CONTROL) {
      return EFI_SUCCESS;
    }

    //There need 30ms to keep consistent with the previous loops if the CPU failed
    //to get control of I2C
    MicroSecondDelay (30000);
  }

  Temp = ReadCpldReg (CPLD_I2C_SWITCH_FLAG);
  Temp = Temp & ~CPU_GET_I2C_CONTROL;
  WriteCpldReg (CPLD_I2C_SWITCH_FLAG, Temp);

  return EFI_NOT_READY;
}

/*---------------------------------------------------------------------------------------*/
EFI_STATUS
EFIAPI
RelocateSmmBase (
  IN       UINTN                      Core,
  IN       EFI_PHYSICAL_ADDRESS       NewSmmBase,
  IN       EFI_PHYSICAL_ADDRESS       TsegBase,
  IN       UINTN                      TsegSize
  )
{
  EFI_PHYSICAL_ADDRESS    SmmRelocBase;
  SMM_RELOC_INFO          *RelocInfo;
  EFI_STATUS              Status;
  UINTN                   Timeout;

  Timeout = 0;
  //Allocate memory to relocate SMM base
  SmmRelocBase = SMM_DEFAULT_BASE;
  Status = gBS->AllocatePages (
                  AllocateAddress,
                  EfiBootServicesData,
                  1,
                  &SmmRelocBase
                  );

  if (EFI_ERROR (Status)) {
    return Status;
  }

  // Copy SMM relocate code in this allocated memory. This is Smm16Relocate.asm copied to SmmRelocBase
  Status = PrepareSmmRelocateVector (
              SmmRelocBase,
              &RelocInfo
              );


  RelocInfo->NewSmmBase  = (UINT32) NewSmmBase;
  RelocInfo->TsegBase    = (UINT32) TsegBase;
  RelocInfo->TsegSize    = (UINT32) TsegSize;

  // Send SIPI to relocate SMI. Smm16Relocate.asm code will get SMM control, perform necessary SMM relocation
  // and set the flag to indicate completion
  SendSmiIpl (GetCoreApicId (Core));
  while (RelocInfo->NewSmmBase) {
  // Wait for AP to run and clear this value to indicate compition
    MicroSecondDelay (1);
    Timeout++;
    if (Timeout > 10000) {
    // If > 10 sec. bailout. Enough??
      Status = EFI_UNSUPPORTED;
      break;
    }
  }

  // Release the memory
  gBS->FreePages (SmmRelocBase, 1);

  return Status;
}

/**
  This function provides a blocking stall for reset at least the given number of microseconds
  stipulated in the final argument.

  @param  PeiServices General purpose services available to every PEIM.

  @param  this Pointer to the local data for the interface.

  @param  Microseconds number of microseconds for which to stall.

  @retval EFI_SUCCESS the function provided at least the required stall.
**/
EFI_STATUS
EFIAPI
Stall (
  IN CONST EFI_PEI_SERVICES   **PeiServices,
  IN CONST EFI_PEI_STALL_PPI  *This,
  IN UINTN                    Microseconds
  )
{
  MicroSecondDelay (Microseconds);
  return EFI_SUCCESS;
}

/**
  Calling this function causes a system-wide reset. This sets
  all circuitry within the system to its initial state. This type of reset
  is asynchronous to system operation and operates without regard to
  cycle boundaries.

  System reset should not return, if it returns, it means the system does
  not support cold reset.
**/
VOID
EFIAPI
ResetCold (
  VOID
  )
{
  IoWrite8 (0xCF9, BIT2 | BIT1); // 1st choice: PIIX3 RCR, RCPU|SRST
  MicroSecondDelay (50);

  IoWrite8 (0x64, 0xfe);         // 2nd choice: keyboard controller
  CpuDeadLoop ();
}

/**
  According to errata FE-3124064, write to RTC TIME RTC_ALARMx registers
  may fail. As a workaround, before actual writing to those registers,
  issue a dummy write of 0x0 twice to RTC Status register.
  Also, according to the datasheet, the OS should wait 5us after every
  register write to the RTC hard macro, so that the required update
  can occur without holding off the system bus.
**/
STATIC
VOID
RtcDelayedWrite (
  IN UINT32        Offset,
  IN UINT32        Value
  )
{
  MmioWrite32 (mArmadaRtcBase + RTC_STATUS_REG, 0);
  MmioWrite32 (mArmadaRtcBase + RTC_STATUS_REG, 0);
  MmioWrite32 (mArmadaRtcBase + Offset, Value);
  MicroSecondDelay (5);
}

VOID OemPcieResetAndOffReset(void)
  {
    WriteCpldReg(CPU0_PCIE1_RESET_REG,0x0);
    WriteCpldReg(CPU0_PCIE2_RESET_REG,0x0);
    WriteCpldReg(CPU1_PCIE1_RESET_REG,0x0);
    WriteCpldReg(CPU1_PCIE2_RESET_REG,0x0);
    MicroSecondDelay(100000);
    WriteCpldReg(CPU0_PCIE1_RESET_REG,0x55);
    WriteCpldReg(CPU0_PCIE2_RESET_REG,0x55);
    WriteCpldReg(CPU1_PCIE1_RESET_REG,0x55);
    WriteCpldReg(CPU1_PCIE2_RESET_REG,0x55);
    return;
  }

/**
  Do the Sense Drive Status command.
  
  @param  FdcDev FDC_BLK_IO_DEV *: A pointer to FDC_BLK_IO_DEV
  @param  Lba EFI_LBA     : Logic block address
  
  @retval  EFI_SUCCESS:    Execute the Sense Drive Status command successfully
  @retval  EFI_DEVICE_ERROR: Fail to execute the command
  @retval  EFI_WRITE_PROTECTED:The disk is write protected

**/
EFI_STATUS
SenseDrvStatus (
  IN FDC_BLK_IO_DEV  *FdcDev,
  IN EFI_LBA         Lba
  )
{
  FDD_COMMAND_PACKET2 Command;
  UINT8               Head;
  UINT8               EndOfTrack;
  UINTN               Index;
  UINT8               StatusRegister3;
  UINT8               *CommandPointer;

  //
  // Sense Drive Status command obtains drive status information,
  // it has not execution phase and goes directly to the result phase from the
  // command phase, Status Register 3 contains the drive status information
  //
  ZeroMem (&Command, sizeof (FDD_COMMAND_PACKET2));
  Command.CommandCode = SENSE_DRV_STATUS_CMD;

  if (FdcDev->Disk == FdcDisk0) {
    Command.DiskHeadSel = 0;
  } else {
    Command.DiskHeadSel = 1;
  }

  EndOfTrack  = DISK_1440K_EOT;
  Head        = (UINT8) ((UINTN) Lba / EndOfTrack % 2);
  Command.DiskHeadSel = (UINT8) (Command.DiskHeadSel | (Head << 2));

  CommandPointer = (UINT8 *) (&Command);
  for (Index = 0; Index < sizeof (FDD_COMMAND_PACKET2); Index++) {
    if (EFI_ERROR (DataOutByte (FdcDev, CommandPointer++))) {
      return EFI_DEVICE_ERROR;
    }
  }

  if (EFI_ERROR (DataInByte (FdcDev, &StatusRegister3))) {
    return EFI_DEVICE_ERROR;
  }
  //
  // Io delay
  //
  MicroSecondDelay (50);

  //
  // Check Status Register 3 to get drive status information
  //
  return CheckStatus3 (StatusRegister3);
}

/**

  Routine Description:  Determine whether FDC is ready to write or read.
  
  @param  FdcDev Pointer to instance of FDC_BLK_IO_DEV
  @param  Dio BOOLEAN:      Indicate the FDC is waiting to write or read
  @param  TimeoutInSeconds UINTN: The time period for waiting
  
  @retval EFI_SUCCESS:  FDC is ready to write or read
  @retval EFI_NOT_READY:  FDC is not ready within the specified time period

**/
EFI_STATUS
FddDRQReady (
  IN FDC_BLK_IO_DEV  *FdcDev,
  IN BOOLEAN         Dio,
  IN  UINTN          TimeoutInSeconds
  )
{
  UINTN Delay;
  UINT8 StatusRegister;
  UINT8 DataInOut;

  //
  // Before writing to FDC or reading from FDC, the Host must examine
  // the bit7(RQM) and bit6(DIO) of the Main Status Register.
  // That is to say:
  //  command bytes can not be written to Data Register
  //  unless RQM is 1 and DIO is 0
  //  result bytes can not be read from Data Register
  //  unless RQM is 1 and DIO is 1
  //
  DataInOut = (UINT8) (Dio << 6);
  //
  // in order to compare bit6
  //
  Delay = ((TimeoutInSeconds * STALL_1_MSECOND) / 50) + 1;
  do {
    StatusRegister = FdcReadPort (FdcDev, FDC_REGISTER_MSR);
    if ((StatusRegister & MSR_RQM) == MSR_RQM && (StatusRegister & MSR_DIO) == DataInOut) {
      break;
      //
      // FDC is ready
      //
    }

    MicroSecondDelay (50);
    //
    // Stall for 50 us
    //
    Delay = Delay - 1;
  } while (Delay > 0);

  if (Delay == 0) {
    return EFI_NOT_READY;
    //
    // FDC is not ready within the specified time period
    //
  }

  return EFI_SUCCESS;
}

/**
  Stalls the CPU for at least the given number of nanoseconds.

  Stalls the CPU for the number of nanoseconds specified by NanoSeconds.

  When the timer frequency is 1MHz, each tick corresponds to 1 microsecond.
  Therefore, the nanosecond delay will be rounded up to the nearest 1 microsecond.

  @param  NanoSeconds The minimum number of nanoseconds to delay.

  @return The value of NanoSeconds inputed.

**/
UINTN
EFIAPI
NanoSecondDelay (
  IN  UINTN NanoSeconds
  )
{
  UINTN  MicroSeconds;

  // Round up to 1us Tick Number
  MicroSeconds = NanoSeconds / 1000;
  MicroSeconds += ((NanoSeconds % 1000) == 0) ? 0 : 1;

  MicroSecondDelay (MicroSeconds);

  return NanoSeconds;
}