AhciMode.c

Go to the documentation of this file.

 
#include "AhciPei.h"
 
#define ATA_CMD_TRUST_NON_DATA  0x5B
#define ATA_CMD_TRUST_RECEIVE   0x5C
#define ATA_CMD_TRUST_SEND      0x5E
 
//
// Look up table (IsWrite) for EFI_ATA_PASS_THRU_CMD_PROTOCOL
//
EFI_ATA_PASS_THRU_CMD_PROTOCOL  mAtaPassThruCmdProtocols[2] = {
  EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN,
  EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_OUT
};
 
//
// Look up table (Lba48Bit, IsIsWrite) for ATA_CMD
//
UINT8  mAtaCommands[2][2] = {
  {
    ATA_CMD_READ_SECTORS,            // 28-bit LBA; PIO read
    ATA_CMD_WRITE_SECTORS            // 28-bit LBA; PIO write
  },
  {
    ATA_CMD_READ_SECTORS_EXT,        // 48-bit LBA; PIO read
    ATA_CMD_WRITE_SECTORS_EXT        // 48-bit LBA; PIO write
  }
};
 
//
// Look up table (IsTrustSend) for ATA_CMD
//
UINT8  mAtaTrustCommands[2] = {
  ATA_CMD_TRUST_RECEIVE,    // PIO read
  ATA_CMD_TRUST_SEND        // PIO write
};
 
//
// Look up table (Lba48Bit) for maximum transfer block number
//
#define MAX_28BIT_TRANSFER_BLOCK_NUM  0x100
//
// Due to limited resource for VTd PEI DMA buffer on platforms, the driver
// limits the maximum transfer block number for 48-bit addressing.
// Here, setting to 0x800 means that for device with 512-byte block size, the
// maximum buffer for DMA mapping will be 1M bytes in size.
//
#define MAX_48BIT_TRANSFER_BLOCK_NUM  0x800
 
UINT32  mMaxTransferBlockNumber[2] = {
  MAX_28BIT_TRANSFER_BLOCK_NUM,
  MAX_48BIT_TRANSFER_BLOCK_NUM
};
 
//
// The maximum total sectors count in 28 bit addressing mode
//
#define MAX_28BIT_ADDRESSING_CAPACITY  0xfffffff
 
UINT32
AhciReadReg (
  IN UINTN   AhciBar,
  IN UINT32  Offset
  )
{
  UINT32  Data;
 
  Data = 0;
  Data = MmioRead32 (AhciBar + Offset);
 
  return Data;
}
 
VOID
AhciWriteReg (
  IN UINTN   AhciBar,
  IN UINT32  Offset,
  IN UINT32  Data
  )
{
  MmioWrite32 (AhciBar + Offset, Data);
}
 
VOID
AhciAndReg (
  IN UINTN   AhciBar,
  IN UINT32  Offset,
  IN UINT32  AndData
  )
{
  UINT32  Data;
 
  Data  = AhciReadReg (AhciBar, Offset);
  Data &= AndData;
 
  AhciWriteReg (AhciBar, Offset, Data);
}
 
VOID
AhciOrReg (
  IN UINTN   AhciBar,
  IN UINT32  Offset,
  IN UINT32  OrData
  )
{
  UINT32  Data;
 
  Data  = AhciReadReg (AhciBar, Offset);
  Data |= OrData;
 
  AhciWriteReg (AhciBar, Offset, Data);
}
 
EFI_STATUS
EFIAPI
AhciWaitMmioSet (
  IN UINTN   AhciBar,
  IN UINT32  Offset,
  IN UINT32  MaskValue,
  IN UINT32  TestValue,
  IN UINT64  Timeout
  )
{
  UINT32  Value;
  UINT32  Delay;
 
  Delay = (UINT32)(DivU64x32 (Timeout, 1000) + 1);
 
  do {
    Value = AhciReadReg (AhciBar, Offset) & MaskValue;
 
    if (Value == TestValue) {
      return EFI_SUCCESS;
    }
 
    //
    // Stall for 100 microseconds.
    //
    MicroSecondDelay (100);
 
    Delay--;
  } while (Delay > 0);
 
  return EFI_TIMEOUT;
}
 
EFI_STATUS
AhciCheckMemSet (
  IN UINTN   Address,
  IN UINT32  MaskValue,
  IN UINT32  TestValue
  )
{
  UINT32  Value;
 
  Value  = *(volatile UINT32 *)Address;
  Value &= MaskValue;
 
  if (Value == TestValue) {
    return EFI_SUCCESS;
  } else {
    return EFI_NOT_READY;
  }
}
 
EFI_STATUS
AhciWaitMemSet (
  IN  EFI_PHYSICAL_ADDRESS  Address,
  IN  UINT32                MaskValue,
  IN  UINT32                TestValue,
  IN  UINT64                Timeout
  )
{
  UINT32   Value;
  UINT64   Delay;
  BOOLEAN  InfiniteWait;
 
  if (Timeout == 0) {
    InfiniteWait = TRUE;
  } else {
    InfiniteWait = FALSE;
  }
 
  Delay =  DivU64x32 (Timeout, 1000) + 1;
 
  do {
    //
    // Access system memory to see if the value is the tested one.
    //
    // The system memory pointed by Address will be updated by the
    // SATA Host Controller, "volatile" is introduced to prevent
    // compiler from optimizing the access to the memory address
    // to only read once.
    //
    Value  = *(volatile UINT32 *)(UINTN)Address;
    Value &= MaskValue;
 
    if (Value == TestValue) {
      return EFI_SUCCESS;
    }
 
    //
    // Stall for 100 microseconds.
    //
    MicroSecondDelay (100);
 
    Delay--;
  } while (InfiniteWait || (Delay > 0));
 
  return EFI_TIMEOUT;
}
 
VOID
AhciClearPortStatus (
  IN UINTN  AhciBar,
  IN UINT8  Port
  )
{
  UINT32  Offset;
 
  //
  // Clear any error status
  //
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_SERR;
  AhciWriteReg (AhciBar, Offset, AhciReadReg (AhciBar, Offset));
 
  //
  // Clear any port interrupt status
  //
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_IS;
  AhciWriteReg (AhciBar, Offset, AhciReadReg (AhciBar, Offset));
 
  //
  // Clear any HBA interrupt status
  //
  AhciWriteReg (AhciBar, AHCI_IS_OFFSET, AhciReadReg (AhciBar, AHCI_IS_OFFSET));
}
 
EFI_STATUS
AhciEnableFisReceive (
  IN UINTN   AhciBar,
  IN UINT8   Port,
  IN UINT64  Timeout
  )
{
  UINT32  Offset;
 
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
  AhciOrReg (AhciBar, Offset, AHCI_PORT_CMD_FRE);
 
  return EFI_SUCCESS;
}
 
EFI_STATUS
AhciDisableFisReceive (
  IN UINTN   AhciBar,
  IN UINT8   Port,
  IN UINT64  Timeout
  )
{
  UINT32  Offset;
  UINT32  Data;
 
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
  Data   = AhciReadReg (AhciBar, Offset);
 
  //
  // Before disabling Fis receive, the DMA engine of the port should NOT be in
  // running status.
  //
  if ((Data & (AHCI_PORT_CMD_ST | AHCI_PORT_CMD_CR)) != 0) {
    return EFI_UNSUPPORTED;
  }
 
  //
  // Check if the Fis receive DMA engine for the port is running.
  //
  if ((Data & AHCI_PORT_CMD_FR) != AHCI_PORT_CMD_FR) {
    return EFI_SUCCESS;
  }
 
  AhciAndReg (AhciBar, Offset, (UINT32) ~(AHCI_PORT_CMD_FRE));
 
  return AhciWaitMmioSet (
           AhciBar,
           Offset,
           AHCI_PORT_CMD_FR,
           0,
           Timeout
           );
}
 
VOID
AhciBuildCommand (
  IN     PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private,
  IN     UINT8                             Port,
  IN     UINT8                             PortMultiplier,
  IN     UINT8                             FisIndex,
  IN     EFI_AHCI_COMMAND_FIS              *CommandFis,
  IN     EFI_AHCI_COMMAND_LIST             *CommandList,
  IN     UINT8                             CommandSlotNumber,
  IN OUT VOID                              *DataPhysicalAddr,
  IN     UINT32                            DataLength
  )
{
  EFI_AHCI_REGISTERS  *AhciRegisters;
  UINTN               AhciBar;
  UINT64              BaseAddr;
  UINT32              PrdtNumber;
  UINT32              PrdtIndex;
  UINTN               RemainedData;
  UINTN               MemAddr;
  DATA_64             Data64;
  UINT32              Offset;
 
  AhciRegisters = &Private->AhciRegisters;
  AhciBar       = Private->MmioBase;
 
  //
  // Filling the PRDT
  //
  PrdtNumber = (UINT32)DivU64x32 (
                         (UINT64)DataLength + AHCI_MAX_DATA_PER_PRDT - 1,
                         AHCI_MAX_DATA_PER_PRDT
                         );
 
  //
  // According to AHCI 1.3 spec, a PRDT entry can point to a maximum 4MB data block.
  // It also limits that the maximum amount of the PRDT entry in the command table
  // is 65535.
  // Current driver implementation supports up to a maximum of AHCI_MAX_PRDT_NUMBER
  // PRDT entries.
  //
  ASSERT (PrdtNumber <= AHCI_MAX_PRDT_NUMBER);
  if (PrdtNumber > AHCI_MAX_PRDT_NUMBER) {
    return;
  }
 
  Data64.Uint64 = (UINTN)(AhciRegisters->AhciRFis) + sizeof (EFI_AHCI_RECEIVED_FIS) * FisIndex;
 
  BaseAddr = Data64.Uint64;
 
  ZeroMem ((VOID *)((UINTN)BaseAddr), sizeof (EFI_AHCI_RECEIVED_FIS));
 
  ZeroMem (AhciRegisters->AhciCmdTable, sizeof (EFI_AHCI_COMMAND_TABLE));
 
  CommandFis->AhciCFisPmNum = PortMultiplier;
 
  CopyMem (&AhciRegisters->AhciCmdTable->CommandFis, CommandFis, sizeof (EFI_AHCI_COMMAND_FIS));
 
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
  AhciAndReg (AhciBar, Offset, (UINT32) ~(AHCI_PORT_CMD_DLAE | AHCI_PORT_CMD_ATAPI));
 
  RemainedData              = (UINTN)DataLength;
  MemAddr                   = (UINTN)DataPhysicalAddr;
  CommandList->AhciCmdPrdtl = PrdtNumber;
 
  for (PrdtIndex = 0; PrdtIndex < PrdtNumber; PrdtIndex++) {
    if (RemainedData < AHCI_MAX_DATA_PER_PRDT) {
      AhciRegisters->AhciCmdTable->PrdtTable[PrdtIndex].AhciPrdtDbc = (UINT32)RemainedData - 1;
    } else {
      AhciRegisters->AhciCmdTable->PrdtTable[PrdtIndex].AhciPrdtDbc = AHCI_MAX_DATA_PER_PRDT - 1;
    }
 
    Data64.Uint64                                                  = (UINT64)MemAddr;
    AhciRegisters->AhciCmdTable->PrdtTable[PrdtIndex].AhciPrdtDba  = Data64.Uint32.Lower32;
    AhciRegisters->AhciCmdTable->PrdtTable[PrdtIndex].AhciPrdtDbau = Data64.Uint32.Upper32;
    RemainedData                                                  -= AHCI_MAX_DATA_PER_PRDT;
    MemAddr                                                       += AHCI_MAX_DATA_PER_PRDT;
  }
 
  //
  // Set the last PRDT to Interrupt On Complete
  //
  if (PrdtNumber > 0) {
    AhciRegisters->AhciCmdTable->PrdtTable[PrdtNumber - 1].AhciPrdtIoc = 1;
  }
 
  CopyMem (
    (VOID *)((UINTN)AhciRegisters->AhciCmdList + (UINTN)CommandSlotNumber * sizeof (EFI_AHCI_COMMAND_LIST)),
    CommandList,
    sizeof (EFI_AHCI_COMMAND_LIST)
    );
 
  Data64.Uint64                                              = (UINT64)(UINTN)AhciRegisters->AhciCmdTable;
  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdCtba  = Data64.Uint32.Lower32;
  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdCtbau = Data64.Uint32.Upper32;
  AhciRegisters->AhciCmdList[CommandSlotNumber].AhciCmdPmp   = PortMultiplier;
}
 
VOID
AhciBuildCommandFis (
  IN OUT EFI_AHCI_COMMAND_FIS   *CmdFis,
  IN     EFI_ATA_COMMAND_BLOCK  *AtaCommandBlock
  )
{
  ZeroMem (CmdFis, sizeof (EFI_AHCI_COMMAND_FIS));
 
  CmdFis->AhciCFisType = AHCI_FIS_REGISTER_H2D;
  //
  // Indicator it's a command
  //
  CmdFis->AhciCFisCmdInd = 0x1;
  CmdFis->AhciCFisCmd    = AtaCommandBlock->AtaCommand;
 
  CmdFis->AhciCFisFeature    = AtaCommandBlock->AtaFeatures;
  CmdFis->AhciCFisFeatureExp = AtaCommandBlock->AtaFeaturesExp;
 
  CmdFis->AhciCFisSecNum    = AtaCommandBlock->AtaSectorNumber;
  CmdFis->AhciCFisSecNumExp = AtaCommandBlock->AtaSectorNumberExp;
 
  CmdFis->AhciCFisClyLow    = AtaCommandBlock->AtaCylinderLow;
  CmdFis->AhciCFisClyLowExp = AtaCommandBlock->AtaCylinderLowExp;
 
  CmdFis->AhciCFisClyHigh    = AtaCommandBlock->AtaCylinderHigh;
  CmdFis->AhciCFisClyHighExp = AtaCommandBlock->AtaCylinderHighExp;
 
  CmdFis->AhciCFisSecCount    = AtaCommandBlock->AtaSectorCount;
  CmdFis->AhciCFisSecCountExp = AtaCommandBlock->AtaSectorCountExp;
 
  CmdFis->AhciCFisDevHead = (UINT8)(AtaCommandBlock->AtaDeviceHead | 0xE0);
}
 
EFI_STATUS
AhciStopCommand (
  IN  UINTN   AhciBar,
  IN  UINT8   Port,
  IN  UINT64  Timeout
  )
{
  UINT32  Offset;
  UINT32  Data;
 
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
  Data   = AhciReadReg (AhciBar, Offset);
 
  if ((Data & (AHCI_PORT_CMD_ST | AHCI_PORT_CMD_CR)) == 0) {
    return EFI_SUCCESS;
  }
 
  if ((Data & AHCI_PORT_CMD_ST) != 0) {
    AhciAndReg (AhciBar, Offset, (UINT32) ~(AHCI_PORT_CMD_ST));
  }
 
  return AhciWaitMmioSet (
           AhciBar,
           Offset,
           AHCI_PORT_CMD_CR,
           0,
           Timeout
           );
}
 
EFI_STATUS
AhciStartCommand (
  IN  UINTN   AhciBar,
  IN  UINT8   Port,
  IN  UINT8   CommandSlot,
  IN  UINT64  Timeout
  )
{
  UINT32      CmdSlotBit;
  EFI_STATUS  Status;
  UINT32      PortStatus;
  UINT32      StartCmd;
  UINT32      PortTfd;
  UINT32      Offset;
  UINT32      Capability;
 
  //
  // Collect AHCI controller information
  //
  Capability = AhciReadReg (AhciBar, AHCI_CAPABILITY_OFFSET);
 
  CmdSlotBit = (UINT32)(1 << CommandSlot);
 
  AhciClearPortStatus (
    AhciBar,
    Port
    );
 
  Status = AhciEnableFisReceive (
             AhciBar,
             Port,
             Timeout
             );
  if (EFI_ERROR (Status)) {
    return Status;
  }
 
  Offset     = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
  PortStatus = AhciReadReg (AhciBar, Offset);
 
  StartCmd = 0;
  if ((PortStatus & AHCI_PORT_CMD_ALPE) != 0) {
    StartCmd  = AhciReadReg (AhciBar, Offset);
    StartCmd &= ~AHCI_PORT_CMD_ICC_MASK;
    StartCmd |= AHCI_PORT_CMD_ACTIVE;
  }
 
  Offset  = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_TFD;
  PortTfd = AhciReadReg (AhciBar, Offset);
 
  if ((PortTfd & (AHCI_PORT_TFD_BSY | AHCI_PORT_TFD_DRQ)) != 0) {
    if ((Capability & BIT24) != 0) {
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
      AhciOrReg (AhciBar, Offset, AHCI_PORT_CMD_CLO);
 
      AhciWaitMmioSet (
        AhciBar,
        Offset,
        AHCI_PORT_CMD_CLO,
        0,
        Timeout
        );
    }
  }
 
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
  AhciOrReg (AhciBar, Offset, AHCI_PORT_CMD_ST | StartCmd);
 
  //
  // Setting the command
  //
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CI;
  AhciAndReg (AhciBar, Offset, 0);
  AhciOrReg (AhciBar, Offset, CmdSlotBit);
 
  return EFI_SUCCESS;
}
 
EFI_STATUS
AhciPioTransfer (
  IN     PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private,
  IN     UINT8                             Port,
  IN     UINT8                             PortMultiplier,
  IN     UINT8                             FisIndex,
  IN     BOOLEAN                           Read,
  IN     EFI_ATA_COMMAND_BLOCK             *AtaCommandBlock,
  IN OUT EFI_ATA_STATUS_BLOCK              *AtaStatusBlock,
  IN OUT VOID                              *MemoryAddr,
  IN     UINT32                            DataCount,
  IN     UINT64                            Timeout
  )
{
  EFI_STATUS             Status;
  EDKII_IOMMU_OPERATION  MapOp;
  UINTN                  MapLength;
  EFI_PHYSICAL_ADDRESS   PhyAddr;
  VOID                   *MapData;
  EFI_AHCI_REGISTERS     *AhciRegisters;
  UINTN                  AhciBar;
  BOOLEAN                InfiniteWait;
  UINT32                 Offset;
  UINT32                 OldRfisLo;
  UINT32                 OldRfisHi;
  UINT32                 OldCmdListLo;
  UINT32                 OldCmdListHi;
  DATA_64                Data64;
  UINT32                 FisBaseAddr;
  UINT32                 Delay;
  EFI_AHCI_COMMAND_FIS   CFis;
  EFI_AHCI_COMMAND_LIST  CmdList;
  UINT32                 PortTfd;
  UINT32                 PrdCount;
  BOOLEAN                PioFisReceived;
  BOOLEAN                D2hFisReceived;
 
  //
  // Current driver implementation supports up to a maximum of AHCI_MAX_PRDT_NUMBER
  // PRDT entries.
  //
  if (DataCount / (UINT32)AHCI_MAX_PRDT_NUMBER > AHCI_MAX_DATA_PER_PRDT) {
    DEBUG ((
      DEBUG_ERROR,
      "%a: Driver only support a maximum of 0x%x PRDT entries, "
      "current number of data byte 0x%x is too large, maximum allowed is 0x%x.\n",
      __func__,
      AHCI_MAX_PRDT_NUMBER,
      DataCount,
      AHCI_MAX_PRDT_NUMBER * AHCI_MAX_DATA_PER_PRDT
      ));
    return EFI_UNSUPPORTED;
  }
 
  MapOp = Read ? EdkiiIoMmuOperationBusMasterWrite :
          EdkiiIoMmuOperationBusMasterRead;
  MapLength = DataCount;
  Status    = IoMmuMap (
                MapOp,
                MemoryAddr,
                &MapLength,
                &PhyAddr,
                &MapData
                );
  if (EFI_ERROR (Status) || (MapLength != DataCount)) {
    DEBUG ((DEBUG_ERROR, "%a: Fail to map data buffer.\n", __func__));
    return EFI_OUT_OF_RESOURCES;
  }
 
  AhciRegisters = &Private->AhciRegisters;
  AhciBar       = Private->MmioBase;
  InfiniteWait  = (Timeout == 0) ? TRUE : FALSE;
 
  //
  // Fill FIS base address register
  //
  Offset        = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_FB;
  OldRfisLo     = AhciReadReg (AhciBar, Offset);
  Offset        = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_FBU;
  OldRfisHi     = AhciReadReg (AhciBar, Offset);
  Data64.Uint64 = (UINTN)(AhciRegisters->AhciRFis) + sizeof (EFI_AHCI_RECEIVED_FIS) * FisIndex;
  Offset        = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_FB;
  AhciWriteReg (AhciBar, Offset, Data64.Uint32.Lower32);
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_FBU;
  AhciWriteReg (AhciBar, Offset, Data64.Uint32.Upper32);
 
  //
  // Single task environment, we only use one command table for all port
  //
  Offset        = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CLB;
  OldCmdListLo  = AhciReadReg (AhciBar, Offset);
  Offset        = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CLBU;
  OldCmdListHi  = AhciReadReg (AhciBar, Offset);
  Data64.Uint64 = (UINTN)(AhciRegisters->AhciCmdList);
  Offset        = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CLB;
  AhciWriteReg (AhciBar, Offset, Data64.Uint32.Lower32);
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CLBU;
  AhciWriteReg (AhciBar, Offset, Data64.Uint32.Upper32);
 
  //
  // Package read needed
  //
  AhciBuildCommandFis (&CFis, AtaCommandBlock);
 
  ZeroMem (&CmdList, sizeof (EFI_AHCI_COMMAND_LIST));
 
  CmdList.AhciCmdCfl = AHCI_FIS_REGISTER_H2D_LENGTH / 4;
  CmdList.AhciCmdW   = Read ? 0 : 1;
 
  AhciBuildCommand (
    Private,
    Port,
    PortMultiplier,
    FisIndex,
    &CFis,
    &CmdList,
    0,
    (VOID *)(UINTN)PhyAddr,
    DataCount
    );
 
  Status = AhciStartCommand (
             AhciBar,
             Port,
             0,
             Timeout
             );
  if (EFI_ERROR (Status)) {
    goto Exit;
  }
 
  //
  // Checking the status and wait the driver sending Data
  //
  FisBaseAddr = (UINT32)(UINTN)AhciRegisters->AhciRFis + sizeof (EFI_AHCI_RECEIVED_FIS) * FisIndex;
  if (Read) {
    //
    // Wait device sends the PIO setup fis before data transfer
    //
    Status = EFI_TIMEOUT;
    Delay  = (UINT32)DivU64x32 (Timeout, 1000) + 1;
    do {
      PioFisReceived = FALSE;
      D2hFisReceived = FALSE;
      Offset         = FisBaseAddr + AHCI_PIO_FIS_OFFSET;
      Status         = AhciCheckMemSet (Offset, AHCI_FIS_TYPE_MASK, AHCI_FIS_PIO_SETUP);
      if (!EFI_ERROR (Status)) {
        DEBUG ((DEBUG_INFO, "%a: PioFisReceived.\n", __func__));
        PioFisReceived = TRUE;
      }
 
      //
      // According to SATA 2.6 spec section 11.7, D2h FIS means an error encountered.
      // But Qemu and Marvel 9230 sata controller may just receive a D2h FIS from
      // device after the transaction is finished successfully.
      // To get better device compatibilities, we further check if the PxTFD's
      // ERR bit is set. By this way, we can know if there is a real error happened.
      //
      Offset = FisBaseAddr + AHCI_D2H_FIS_OFFSET;
      Status = AhciCheckMemSet (Offset, AHCI_FIS_TYPE_MASK, AHCI_FIS_REGISTER_D2H);
      if (!EFI_ERROR (Status)) {
        DEBUG ((DEBUG_INFO, "%a: D2hFisReceived.\n", __func__));
        D2hFisReceived = TRUE;
      }
 
      if (PioFisReceived || D2hFisReceived) {
        Offset  = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_TFD;
        PortTfd = AhciReadReg (AhciBar, (UINT32)Offset);
        //
        // PxTFD will be updated if there is a D2H or SetupFIS received.
        //
        if ((PortTfd & AHCI_PORT_TFD_ERR) != 0) {
          Status = EFI_DEVICE_ERROR;
          break;
        }
 
        PrdCount = *(volatile UINT32 *)(&(AhciRegisters->AhciCmdList[0].AhciCmdPrdbc));
        if (PrdCount == DataCount) {
          Status = EFI_SUCCESS;
          break;
        }
      }
 
      //
      // Stall for 100 microseconds.
      //
      MicroSecondDelay (100);
 
      Delay--;
      if (Delay == 0) {
        Status = EFI_TIMEOUT;
      }
    } while (InfiniteWait || (Delay > 0));
  } else {
    //
    // Wait for D2H Fis is received
    //
    Offset = FisBaseAddr + AHCI_D2H_FIS_OFFSET;
    Status = AhciWaitMemSet (
               Offset,
               AHCI_FIS_TYPE_MASK,
               AHCI_FIS_REGISTER_D2H,
               Timeout
               );
    if (EFI_ERROR (Status)) {
      DEBUG ((DEBUG_ERROR, "%a: AhciWaitMemSet (%r)\n", __func__, Status));
      goto Exit;
    }
 
    Offset  = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_TFD;
    PortTfd = AhciReadReg (AhciBar, (UINT32)Offset);
    if ((PortTfd & AHCI_PORT_TFD_ERR) != 0) {
      Status = EFI_DEVICE_ERROR;
    }
  }
 
Exit:
  AhciStopCommand (
    AhciBar,
    Port,
    Timeout
    );
 
  AhciDisableFisReceive (
    AhciBar,
    Port,
    Timeout
    );
 
  if (MapData != NULL) {
    IoMmuUnmap (MapData);
  }
 
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_FB;
  AhciWriteReg (AhciBar, Offset, OldRfisLo);
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_FBU;
  AhciWriteReg (AhciBar, Offset, OldRfisHi);
 
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CLB;
  AhciWriteReg (AhciBar, Offset, OldCmdListLo);
  Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CLBU;
  AhciWriteReg (AhciBar, Offset, OldCmdListHi);
 
  return Status;
}
 
EFI_STATUS
AhciNonDataTransfer (
  IN     PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private,
  IN     UINT8                             Port,
  IN     UINT8                             PortMultiplier,
  IN     UINT8                             FisIndex,
  IN     EFI_ATA_COMMAND_BLOCK             *AtaCommandBlock,
  IN OUT EFI_ATA_STATUS_BLOCK              *AtaStatusBlock,
  IN     UINT64                            Timeout
  )
{
  EFI_STATUS             Status;
  UINTN                  AhciBar;
  EFI_AHCI_REGISTERS     *AhciRegisters;
  UINTN                  FisBaseAddr;
  UINTN                  Offset;
  UINT32                 PortTfd;
  EFI_AHCI_COMMAND_FIS   CFis;
  EFI_AHCI_COMMAND_LIST  CmdList;
 
  AhciBar       = Private->MmioBase;
  AhciRegisters = &Private->AhciRegisters;
 
  //
  // Package read needed
  //
  AhciBuildCommandFis (&CFis, AtaCommandBlock);
 
  ZeroMem (&CmdList, sizeof (EFI_AHCI_COMMAND_LIST));
 
  CmdList.AhciCmdCfl = AHCI_FIS_REGISTER_H2D_LENGTH / 4;
 
  AhciBuildCommand (
    Private,
    Port,
    PortMultiplier,
    FisIndex,
    &CFis,
    &CmdList,
    0,
    NULL,
    0
    );
 
  Status = AhciStartCommand (
             AhciBar,
             Port,
             0,
             Timeout
             );
  if (EFI_ERROR (Status)) {
    goto Exit;
  }
 
  //
  // Wait device sends the Response Fis
  //
  FisBaseAddr = (UINTN)AhciRegisters->AhciRFis + sizeof (EFI_AHCI_RECEIVED_FIS) * FisIndex;
  Offset      = FisBaseAddr + AHCI_D2H_FIS_OFFSET;
  Status      = AhciWaitMemSet (
                  Offset,
                  AHCI_FIS_TYPE_MASK,
                  AHCI_FIS_REGISTER_D2H,
                  Timeout
                  );
 
  if (EFI_ERROR (Status)) {
    goto Exit;
  }
 
  Offset  = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_TFD;
  PortTfd = AhciReadReg (AhciBar, (UINT32)Offset);
  if ((PortTfd & AHCI_PORT_TFD_ERR) != 0) {
    Status = EFI_DEVICE_ERROR;
  }
 
Exit:
  AhciStopCommand (
    AhciBar,
    Port,
    Timeout
    );
 
  AhciDisableFisReceive (
    AhciBar,
    Port,
    Timeout
    );
 
  return Status;
}
 
EFI_STATUS
AhciReset (
  IN UINTN   AhciBar,
  IN UINT64  Timeout
  )
{
  UINT32  Delay;
  UINT32  Value;
  UINT32  Capability;
 
  //
  // Collect AHCI controller information
  //
  Capability = AhciReadReg (AhciBar, AHCI_CAPABILITY_OFFSET);
 
  //
  // Enable AE before accessing any AHCI registers if Supports AHCI Mode Only is not set
  //
  if ((Capability & AHCI_CAP_SAM) == 0) {
    AhciOrReg (AhciBar, AHCI_GHC_OFFSET, AHCI_GHC_ENABLE);
  }
 
  AhciOrReg (AhciBar, AHCI_GHC_OFFSET, AHCI_GHC_RESET);
 
  Delay = (UINT32)(DivU64x32 (Timeout, 1000) + 1);
 
  do {
    Value = AhciReadReg (AhciBar, AHCI_GHC_OFFSET);
    if ((Value & AHCI_GHC_RESET) == 0) {
      return EFI_SUCCESS;
    }
 
    //
    // Stall for 100 microseconds.
    //
    MicroSecondDelay (100);
 
    Delay--;
  } while (Delay > 0);
 
  return EFI_TIMEOUT;
}
 
EFI_STATUS
AhciIdentify (
  IN PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private,
  IN UINT8                             Port,
  IN UINT8                             PortMultiplier,
  IN UINT8                             FisIndex,
  IN ATA_IDENTIFY_DATA                 *Buffer
  )
{
  EFI_STATUS             Status;
  EFI_ATA_COMMAND_BLOCK  Acb;
  EFI_ATA_STATUS_BLOCK   Asb;
 
  if (Buffer == NULL) {
    return EFI_INVALID_PARAMETER;
  }
 
  ZeroMem (&Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
  ZeroMem (&Asb, sizeof (EFI_ATA_STATUS_BLOCK));
 
  Acb.AtaCommand     = ATA_CMD_IDENTIFY_DRIVE;
  Acb.AtaSectorCount = 1;
 
  Status = AhciPioTransfer (
             Private,
             Port,
             PortMultiplier,
             FisIndex,
             TRUE,
             &Acb,
             &Asb,
             Buffer,
             sizeof (ATA_IDENTIFY_DATA),
             ATA_TIMEOUT
             );
 
  return Status;
}
 
UINT8
AhciGetNumberOfPortsFromMap (
  IN UINT32  PortBitMap
  )
{
  UINT8  NumberOfPorts;
 
  NumberOfPorts = 0;
 
  while (PortBitMap != 0) {
    if ((PortBitMap & ((UINT32)BIT0)) != 0) {
      NumberOfPorts++;
    }
 
    PortBitMap = PortBitMap >> 1;
  }
 
  return NumberOfPorts;
}
 
EFI_STATUS
AhciGetPortFromMap (
  IN  UINT32  PortBitMap,
  IN  UINT8   PortIndex,
  OUT UINT8   *Port
  )
{
  if (PortIndex == 0) {
    return EFI_NOT_FOUND;
  }
 
  *Port = 0;
 
  while (PortBitMap != 0) {
    if ((PortBitMap & ((UINT32)BIT0)) != 0) {
      PortIndex--;
 
      //
      // Found the port specified by PortIndex.
      //
      if (PortIndex == 0) {
        return EFI_SUCCESS;
      }
    }
 
    PortBitMap = PortBitMap >> 1;
    *Port      = *Port + 1;
  }
 
  return EFI_NOT_FOUND;
}
 
EFI_STATUS
AhciCreateTransferDescriptor (
  IN OUT PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private
  )
{
  EFI_STATUS            Status;
  UINTN                 AhciBar;
  EFI_AHCI_REGISTERS    *AhciRegisters;
  EFI_PHYSICAL_ADDRESS  DeviceAddress;
  VOID                  *Base;
  VOID                  *Mapping;
  UINT32                Capability;
  UINT32                PortImplementBitMap;
  UINT8                 MaxPortNumber;
  UINT8                 MaxCommandSlotNumber;
  UINTN                 MaxRFisSize;
  UINTN                 MaxCmdListSize;
  UINTN                 MaxCmdTableSize;
 
  AhciBar       = Private->MmioBase;
  AhciRegisters = &Private->AhciRegisters;
 
  //
  // Collect AHCI controller information
  //
  Capability = AhciReadReg (AhciBar, AHCI_CAPABILITY_OFFSET);
 
  //
  // Get the number of command slots per port supported by this HBA.
  //
  MaxCommandSlotNumber = (UINT8)(((Capability & 0x1F00) >> 8) + 1);
  ASSERT (MaxCommandSlotNumber > 0);
  if (MaxCommandSlotNumber == 0) {
    return EFI_DEVICE_ERROR;
  }
 
  //
  // Get the highest bit of implemented ports which decides how many bytes are
  // allocated for recived FIS.
  //
  PortImplementBitMap = AhciReadReg (AhciBar, AHCI_PI_OFFSET);
  MaxPortNumber       = (UINT8)(UINTN)(HighBitSet32 (PortImplementBitMap) + 1);
  if (MaxPortNumber == 0) {
    return EFI_DEVICE_ERROR;
  }
 
  //
  // Get the number of ports that actually needed to be initialized.
  //
  MaxPortNumber = MIN (MaxPortNumber, AhciGetNumberOfPortsFromMap (Private->PortBitMap));
 
  //
  // Allocate memory for received FIS.
  //
  MaxRFisSize = MaxPortNumber * sizeof (EFI_AHCI_RECEIVED_FIS);
  Status      = IoMmuAllocateBuffer (
                  EFI_SIZE_TO_PAGES (MaxRFisSize),
                  &Base,
                  &DeviceAddress,
                  &Mapping
                  );
  if (EFI_ERROR (Status)) {
    return EFI_OUT_OF_RESOURCES;
  }
 
  ASSERT (DeviceAddress == ((EFI_PHYSICAL_ADDRESS)(UINTN)Base));
  AhciRegisters->AhciRFis    = Base;
  AhciRegisters->AhciRFisMap = Mapping;
  AhciRegisters->MaxRFisSize = MaxRFisSize;
  ZeroMem (AhciRegisters->AhciRFis, EFI_PAGE_SIZE * EFI_SIZE_TO_PAGES (MaxRFisSize));
 
  //
  // Allocate memory for command list.
  // Note that the implemenation is a single task model which only use a command
  // list for each port.
  //
  MaxCmdListSize = 1 * sizeof (EFI_AHCI_COMMAND_LIST);
  Status         = IoMmuAllocateBuffer (
                     EFI_SIZE_TO_PAGES (MaxCmdListSize),
                     &Base,
                     &DeviceAddress,
                     &Mapping
                     );
  if (EFI_ERROR (Status)) {
    Status = EFI_OUT_OF_RESOURCES;
    goto ErrorExit;
  }
 
  ASSERT (DeviceAddress == ((EFI_PHYSICAL_ADDRESS)(UINTN)Base));
  AhciRegisters->AhciCmdList    = Base;
  AhciRegisters->AhciCmdListMap = Mapping;
  AhciRegisters->MaxCmdListSize = MaxCmdListSize;
  ZeroMem (AhciRegisters->AhciCmdList, EFI_PAGE_SIZE * EFI_SIZE_TO_PAGES (MaxCmdListSize));
 
  //
  // Allocate memory for command table
  // According to AHCI 1.3 spec, a PRD table can contain maximum 65535 entries.
  //
  MaxCmdTableSize = sizeof (EFI_AHCI_COMMAND_TABLE);
  Status          = IoMmuAllocateBuffer (
                      EFI_SIZE_TO_PAGES (MaxCmdTableSize),
                      &Base,
                      &DeviceAddress,
                      &Mapping
                      );
  if (EFI_ERROR (Status)) {
    Status = EFI_OUT_OF_RESOURCES;
    goto ErrorExit;
  }
 
  ASSERT (DeviceAddress == ((EFI_PHYSICAL_ADDRESS)(UINTN)Base));
  AhciRegisters->AhciCmdTable    = Base;
  AhciRegisters->AhciCmdTableMap = Mapping;
  AhciRegisters->MaxCmdTableSize = MaxCmdTableSize;
  ZeroMem (AhciRegisters->AhciCmdTable, EFI_PAGE_SIZE * EFI_SIZE_TO_PAGES (MaxCmdTableSize));
 
  return EFI_SUCCESS;
 
ErrorExit:
  if (AhciRegisters->AhciRFisMap != NULL) {
    IoMmuFreeBuffer (
      EFI_SIZE_TO_PAGES (AhciRegisters->MaxRFisSize),
      AhciRegisters->AhciRFis,
      AhciRegisters->AhciRFisMap
      );
    AhciRegisters->AhciRFis = NULL;
  }
 
  if (AhciRegisters->AhciCmdListMap != NULL) {
    IoMmuFreeBuffer (
      EFI_SIZE_TO_PAGES (AhciRegisters->MaxCmdListSize),
      AhciRegisters->AhciCmdList,
      AhciRegisters->AhciCmdListMap
      );
    AhciRegisters->AhciCmdList = NULL;
  }
 
  return Status;
}
 
EFI_LBA
GetAtapi6Capacity (
  IN ATA_IDENTIFY_DATA  *IdentifyData
  )
{
  EFI_LBA  Capacity;
  EFI_LBA  TmpLba;
  UINTN    Index;
 
  if ((IdentifyData->command_set_supported_83 & BIT10) == 0) {
    //
    // The device doesn't support 48 bit addressing
    //
    return 0;
  }
 
  //
  // 48 bit address feature set is supported, get maximum capacity
  //
  Capacity = 0;
  for (Index = 0; Index < 4; Index++) {
    //
    // Lower byte goes first: word[100] is the lowest word, word[103] is highest
    //
    TmpLba    = IdentifyData->maximum_lba_for_48bit_addressing[Index];
    Capacity |= LShiftU64 (TmpLba, 16 * Index);
  }
 
  return Capacity;
}
 
EFI_STATUS
IdentifyAtaDevice (
  IN OUT PEI_AHCI_ATA_DEVICE_DATA  *DeviceData
  )
{
  ATA_IDENTIFY_DATA        *IdentifyData;
  EFI_PEI_BLOCK_IO2_MEDIA  *Media;
  EFI_LBA                  Capacity;
  UINT32                   MaxSectorCount;
  UINT16                   PhyLogicSectorSupport;
 
  IdentifyData = DeviceData->IdentifyData;
  Media        = &DeviceData->Media;
 
  if ((IdentifyData->config & BIT15) != 0) {
    DEBUG ((
      DEBUG_ERROR,
      "%a: Not a hard disk device on Port 0x%x PortMultiplierPort 0x%x\n",
      __func__,
      DeviceData->Port,
      DeviceData->PortMultiplier
      ));
    return EFI_UNSUPPORTED;
  }
 
  DEBUG ((
    DEBUG_INFO,
    "%a: Identify Device: Port 0x%x PortMultiplierPort 0x%x\n",
    __func__,
    DeviceData->Port,
    DeviceData->PortMultiplier
    ));
 
  //
  // Skip checking whether the WORD 88 (supported UltraDMA by drive), since the
  // driver only support PIO data transfer for now.
  //
 
  //
  // Get the capacity information of the device.
  //
  Capacity = GetAtapi6Capacity (IdentifyData);
  if (Capacity > MAX_28BIT_ADDRESSING_CAPACITY) {
    //
    // Capacity exceeds 120GB. 48-bit addressing is really needed
    //
    DeviceData->Lba48Bit = TRUE;
  } else {
    //
    // This is a hard disk <= 120GB capacity, treat it as normal hard disk
    //
    Capacity = ((UINT32)IdentifyData->user_addressable_sectors_hi << 16) |
               IdentifyData->user_addressable_sectors_lo;
    DeviceData->Lba48Bit = FALSE;
  }
 
  if (Capacity == 0) {
    DEBUG ((DEBUG_ERROR, "%a: Invalid Capacity (0) for ATA device.\n", __func__));
    return EFI_UNSUPPORTED;
  }
 
  Media->LastBlock = (EFI_PEI_LBA)(Capacity - 1);
 
  Media->BlockSize = 0x200;
  //
  // Check whether Long Physical Sector Feature is supported
  //
  PhyLogicSectorSupport = IdentifyData->phy_logic_sector_support;
  DEBUG ((
    DEBUG_INFO,
    "%a: PhyLogicSectorSupport = 0x%x\n",
    __func__,
    PhyLogicSectorSupport
    ));
  if ((PhyLogicSectorSupport & (BIT14 | BIT15)) == BIT14) {
    //
    // Check logical block size
    //
    if ((PhyLogicSectorSupport & BIT12) != 0) {
      Media->BlockSize = (UINT32)(((IdentifyData->logic_sector_size_hi << 16) |
                                   IdentifyData->logic_sector_size_lo) * sizeof (UINT16));
    }
  }
 
  //
  // Check BlockSize validity
  //
  MaxSectorCount = mMaxTransferBlockNumber[DeviceData->Lba48Bit];
  if ((Media->BlockSize == 0) || (Media->BlockSize > MAX_UINT32 / MaxSectorCount)) {
    DEBUG ((DEBUG_ERROR, "%a: Invalid BlockSize (0x%x).\n", __func__, Media->BlockSize));
    return EFI_UNSUPPORTED;
  }
 
  DEBUG ((
    DEBUG_INFO,
    "%a: BlockSize = 0x%x, LastBlock = 0x%lx\n",
    __func__,
    Media->BlockSize,
    Media->LastBlock
    ));
 
  if ((IdentifyData->trusted_computing_support & BIT0) != 0) {
    DEBUG ((DEBUG_INFO, "%a: Found Trust Computing feature support.\n", __func__));
    DeviceData->TrustComputing = TRUE;
  }
 
  Media->InterfaceType  = MSG_SATA_DP;
  Media->RemovableMedia = FALSE;
  Media->MediaPresent   = TRUE;
  Media->ReadOnly       = FALSE;
 
  return EFI_SUCCESS;
}
 
EFI_STATUS
CreateNewDevice (
  IN OUT PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private,
  IN     UINTN                             DeviceIndex,
  IN     UINT16                            Port,
  IN     UINT16                            PortMultiplier,
  IN     UINT8                             FisIndex,
  IN     ATA_IDENTIFY_DATA                 *IdentifyData
  )
{
  PEI_AHCI_ATA_DEVICE_DATA  *DeviceData;
  EFI_STATUS                Status;
 
  DeviceData = AllocateZeroPool (sizeof (PEI_AHCI_ATA_DEVICE_DATA));
  if (DeviceData == NULL) {
    return EFI_OUT_OF_RESOURCES;
  }
 
  if (IdentifyData != NULL) {
    DeviceData->IdentifyData = AllocateCopyPool (sizeof (ATA_IDENTIFY_DATA), IdentifyData);
    if (DeviceData->IdentifyData == NULL) {
      return EFI_OUT_OF_RESOURCES;
    }
  }
 
  DeviceData->Signature      = AHCI_PEI_ATA_DEVICE_DATA_SIGNATURE;
  DeviceData->Port           = Port;
  DeviceData->PortMultiplier = PortMultiplier;
  DeviceData->FisIndex       = FisIndex;
  DeviceData->DeviceIndex    = DeviceIndex;
  DeviceData->Private        = Private;
 
  Status = IdentifyAtaDevice (DeviceData);
  if (EFI_ERROR (Status)) {
    return Status;
  }
 
  if (DeviceData->TrustComputing) {
    Private->TrustComputingDevices++;
    DeviceData->TrustComputingDeviceIndex = Private->TrustComputingDevices;
  }
 
  Private->ActiveDevices++;
  InsertTailList (&Private->DeviceList, &DeviceData->Link);
 
  return EFI_SUCCESS;
}
 
EFI_STATUS
AhciModeInitialization (
  IN OUT PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private
  )
{
  EFI_STATUS          Status;
  UINTN               AhciBar;
  UINT32              Capability;
  UINT32              Value;
  UINT8               MaxPortNumber;
  UINT32              PortImplementBitMap;
  UINT32              PortInitializeBitMap;
  EFI_AHCI_REGISTERS  *AhciRegisters;
  UINT8               PortIndex;
  UINT8               Port;
  DATA_64             Data64;
  UINT32              Data;
  UINT32              Offset;
  UINT32              PhyDetectDelay;
  UINTN               DeviceIndex;
  ATA_IDENTIFY_DATA   IdentifyData;
 
  AhciBar = Private->MmioBase;
 
  Status = AhciReset (AhciBar, AHCI_PEI_RESET_TIMEOUT);
  if (EFI_ERROR (Status)) {
    DEBUG ((DEBUG_ERROR, "%a: AHCI HBA reset failed with %r.\n", __func__, Status));
    return EFI_DEVICE_ERROR;
  }
 
  //
  // Collect AHCI controller information
  //
  Capability = AhciReadReg (AhciBar, AHCI_CAPABILITY_OFFSET);
 
  //
  // Make sure that GHC.AE bit is set before accessing any AHCI registers.
  //
  Value = AhciReadReg (AhciBar, AHCI_GHC_OFFSET);
  if ((Value & AHCI_GHC_ENABLE) == 0) {
    AhciOrReg (AhciBar, AHCI_GHC_OFFSET, AHCI_GHC_ENABLE);
  }
 
  Status = AhciCreateTransferDescriptor (Private);
  if (EFI_ERROR (Status)) {
    DEBUG ((
      DEBUG_ERROR,
      "%a: Transfer-related data allocation failed with %r.\n",
      __func__,
      Status
      ));
    return EFI_OUT_OF_RESOURCES;
  }
 
  //
  // Get the number of command slots per port supported by this HBA.
  //
  MaxPortNumber = (UINT8)((Capability & 0x1F) + 1);
 
  //
  // Get the bit map of those ports exposed by this HBA.
  // It indicates which ports that the HBA supports are available for software
  // to use.
  //
  PortImplementBitMap = AhciReadReg (AhciBar, AHCI_PI_OFFSET);
 
  //
  // Get the number of ports that actually needed to be initialized.
  //
  MaxPortNumber = MIN (MaxPortNumber, (UINT8)(UINTN)(HighBitSet32 (PortImplementBitMap) + 1));
  MaxPortNumber = MIN (MaxPortNumber, AhciGetNumberOfPortsFromMap (Private->PortBitMap));
 
  PortInitializeBitMap = Private->PortBitMap & PortImplementBitMap;
  AhciRegisters        = &Private->AhciRegisters;
  DeviceIndex          = 0;
  //
  // Enumerate ATA ports
  //
  for (PortIndex = 1; PortIndex <= MaxPortNumber; PortIndex++) {
    Status = AhciGetPortFromMap (PortInitializeBitMap, PortIndex, &Port);
    if (EFI_ERROR (Status)) {
      //
      // No more available port, just break out of the loop.
      //
      break;
    }
 
    if ((PortImplementBitMap & (BIT0 << Port)) != 0) {
      //
      // Initialize FIS Base Address Register and Command List Base Address
      // Register for use.
      //
      Data64.Uint64 = (UINTN)(AhciRegisters->AhciRFis) +
                      sizeof (EFI_AHCI_RECEIVED_FIS) * (PortIndex - 1);
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_FB;
      AhciWriteReg (AhciBar, Offset, Data64.Uint32.Lower32);
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_FBU;
      AhciWriteReg (AhciBar, Offset, Data64.Uint32.Upper32);
 
      Data64.Uint64 = (UINTN)(AhciRegisters->AhciCmdList);
      Offset        = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CLB;
      AhciWriteReg (AhciBar, Offset, Data64.Uint32.Lower32);
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CLBU;
      AhciWriteReg (AhciBar, Offset, Data64.Uint32.Upper32);
 
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
      Data   = AhciReadReg (AhciBar, Offset);
      if ((Data & AHCI_PORT_CMD_CPD) != 0) {
        AhciOrReg (AhciBar, Offset, AHCI_PORT_CMD_POD);
      }
 
      if ((Capability & AHCI_CAP_SSS) != 0) {
        AhciOrReg (AhciBar, Offset, AHCI_PORT_CMD_SUD);
      }
 
      //
      // Disable aggressive power management.
      //
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_SCTL;
      AhciOrReg (AhciBar, Offset, AHCI_PORT_SCTL_IPM_INIT);
      //
      // Disable the reporting of the corresponding interrupt to system software.
      //
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_IE;
      AhciAndReg (AhciBar, Offset, 0);
 
      //
      // Enable FIS Receive DMA engine for the first D2H FIS.
      //
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
      AhciOrReg (AhciBar, Offset, AHCI_PORT_CMD_FRE);
 
      //
      // Wait no longer than 15 ms to wait the Phy to detect the presence of a device.
      //
      PhyDetectDelay = AHCI_BUS_PHY_DETECT_TIMEOUT;
      Offset         = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_SSTS;
      do {
        Data = AhciReadReg (AhciBar, Offset) & AHCI_PORT_SSTS_DET_MASK;
        if ((Data == AHCI_PORT_SSTS_DET_PCE) || (Data == AHCI_PORT_SSTS_DET)) {
          break;
        }
 
        MicroSecondDelay (1000);
        PhyDetectDelay--;
      } while (PhyDetectDelay > 0);
 
      if (PhyDetectDelay == 0) {
        //
        // No device detected at this port.
        // Clear PxCMD.SUD for those ports at which there are no device present.
        //
        Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_CMD;
        AhciAndReg (AhciBar, Offset, (UINT32) ~(AHCI_PORT_CMD_SUD));
        DEBUG ((DEBUG_ERROR, "%a: No device detected at Port %d.\n", __func__, Port));
        continue;
      }
 
      //
      // According to SATA1.0a spec section 5.2, we need to wait for PxTFD.BSY and PxTFD.DRQ
      // and PxTFD.ERR to be zero. The maximum wait time is 16s which is defined at ATA spec.
      //
      PhyDetectDelay = 16 * 1000;
      do {
        Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_SERR;
        if (AhciReadReg (AhciBar, Offset) != 0) {
          AhciWriteReg (AhciBar, Offset, AhciReadReg (AhciBar, Offset));
        }
 
        Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_TFD;
 
        Data = AhciReadReg (AhciBar, Offset) & AHCI_PORT_TFD_MASK;
        if (Data == 0) {
          break;
        }
 
        MicroSecondDelay (1000);
        PhyDetectDelay--;
      } while (PhyDetectDelay > 0);
 
      if (PhyDetectDelay == 0) {
        DEBUG ((
          DEBUG_ERROR,
          "%a: Port %d device presence detected but phy not ready (TFD=0x%x).\n",
          __func__,
          Port,
          Data
          ));
        continue;
      }
 
      //
      // When the first D2H register FIS is received, the content of PxSIG register is updated.
      //
      Offset = AHCI_PORT_START + Port * AHCI_PORT_REG_WIDTH + AHCI_PORT_SIG;
      Status = AhciWaitMmioSet (
                 AhciBar,
                 Offset,
                 0x0000FFFF,
                 0x00000101,
                 160000000
                 );
      if (EFI_ERROR (Status)) {
        DEBUG ((
          DEBUG_ERROR,
          "%a: Error occurred when waiting for the first D2H register FIS - %r\n",
          __func__,
          Status
          ));
        continue;
      }
 
      Data = AhciReadReg (AhciBar, Offset);
      if ((Data & AHCI_ATAPI_SIG_MASK) == AHCI_ATA_DEVICE_SIG) {
        Status = AhciIdentify (Private, Port, 0, PortIndex - 1, &IdentifyData);
        if (EFI_ERROR (Status)) {
          DEBUG ((DEBUG_ERROR, "%a: AhciIdentify() failed with %r\n", __func__, Status));
          continue;
        }
 
        DEBUG ((DEBUG_INFO, "%a: ATA hard disk found on Port %d.\n", __func__, Port));
      } else {
        continue;
      }
 
      //
      // Found an ATA hard disk device, add it into the device list.
      //
      DeviceIndex++;
      CreateNewDevice (
        Private,
        DeviceIndex,
        Port,
        0xFFFF,
        PortIndex - 1,
        &IdentifyData
        );
    }
  }
 
  return EFI_SUCCESS;
}
 
EFI_STATUS
TransferAtaDevice (
  IN     PEI_AHCI_ATA_DEVICE_DATA  *DeviceData,
  IN OUT VOID                      *Buffer,
  IN     EFI_LBA                   StartLba,
  IN     UINT32                    TransferLength,
  IN     BOOLEAN                   IsWrite
  )
{
  PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private;
  EDKII_PEI_ATA_PASS_THRU_PPI       *AtaPassThru;
  EFI_ATA_COMMAND_BLOCK             Acb;
  EFI_ATA_PASS_THRU_COMMAND_PACKET  Packet;
 
  Private     = DeviceData->Private;
  AtaPassThru = &Private->AtaPassThruPpi;
 
  //
  // Ensure Lba48Bit and IsWrite are valid boolean values
  //
  ASSERT ((UINTN)DeviceData->Lba48Bit < 2);
  ASSERT ((UINTN)IsWrite < 2);
  if (((UINTN)DeviceData->Lba48Bit >= 2) ||
      ((UINTN)IsWrite >= 2))
  {
    return EFI_INVALID_PARAMETER;
  }
 
  //
  // Prepare for ATA command block.
  //
  ZeroMem (&Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
  Acb.AtaCommand      = mAtaCommands[DeviceData->Lba48Bit][IsWrite];
  Acb.AtaSectorNumber = (UINT8)StartLba;
  Acb.AtaCylinderLow  = (UINT8)RShiftU64 (StartLba, 8);
  Acb.AtaCylinderHigh = (UINT8)RShiftU64 (StartLba, 16);
  Acb.AtaDeviceHead   = (UINT8)(BIT7 | BIT6 | BIT5 |
                                (DeviceData->PortMultiplier == 0xFFFF ?
                                 0 : (DeviceData->PortMultiplier << 4)));
  Acb.AtaSectorCount = (UINT8)TransferLength;
  if (DeviceData->Lba48Bit) {
    Acb.AtaSectorNumberExp = (UINT8)RShiftU64 (StartLba, 24);
    Acb.AtaCylinderLowExp  = (UINT8)RShiftU64 (StartLba, 32);
    Acb.AtaCylinderHighExp = (UINT8)RShiftU64 (StartLba, 40);
    Acb.AtaSectorCountExp  = (UINT8)(TransferLength >> 8);
  } else {
    Acb.AtaDeviceHead = (UINT8)(Acb.AtaDeviceHead | RShiftU64 (StartLba, 24));
  }
 
  //
  // Prepare for ATA pass through packet.
  //
  ZeroMem (&Packet, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
  if (IsWrite) {
    Packet.OutDataBuffer     = Buffer;
    Packet.OutTransferLength = TransferLength;
  } else {
    Packet.InDataBuffer     = Buffer;
    Packet.InTransferLength = TransferLength;
  }
 
  Packet.Asb      = NULL;
  Packet.Acb      = &Acb;
  Packet.Protocol = mAtaPassThruCmdProtocols[IsWrite];
  Packet.Length   = EFI_ATA_PASS_THRU_LENGTH_SECTOR_COUNT;
  //
  // |------------------------|-----------------|
  // | ATA PIO Transfer Mode  |  Transfer Rate  |
  // |------------------------|-----------------|
  // |       PIO Mode 0       |  3.3Mbytes/sec  |
  // |------------------------|-----------------|
  // |       PIO Mode 1       |  5.2Mbytes/sec  |
  // |------------------------|-----------------|
  // |       PIO Mode 2       |  8.3Mbytes/sec  |
  // |------------------------|-----------------|
  // |       PIO Mode 3       | 11.1Mbytes/sec  |
  // |------------------------|-----------------|
  // |       PIO Mode 4       | 16.6Mbytes/sec  |
  // |------------------------|-----------------|
  //
  // As AtaBus is used to manage ATA devices, we have to use the lowest transfer
  // rate to calculate the possible maximum timeout value for each read/write
  // operation. The timout value is rounded up to nearest integar and here an
  // additional 30s is added to follow ATA spec in which it mentioned that the
  // device may take up to 30s to respond commands in the Standby/Idle mode.
  //
  // Calculate the maximum timeout value for PIO read/write operation.
  //
  Packet.Timeout = TIMER_PERIOD_SECONDS (
                     DivU64x32 (
                       MultU64x32 (TransferLength, DeviceData->Media.BlockSize),
                       3300000
                       ) + 31
                     );
 
  return AtaPassThru->PassThru (
                        AtaPassThru,
                        DeviceData->Port,
                        DeviceData->PortMultiplier,
                        &Packet
                        );
}
 
EFI_STATUS
TrustTransferAtaDevice (
  IN     PEI_AHCI_ATA_DEVICE_DATA  *DeviceData,
  IN OUT VOID                      *Buffer,
  IN     UINT8                     SecurityProtocolId,
  IN     UINT16                    SecurityProtocolSpecificData,
  IN     UINTN                     TransferLength,
  IN     BOOLEAN                   IsTrustSend,
  IN     UINT64                    Timeout,
  OUT    UINTN                     *TransferLengthOut
  )
{
  PEI_AHCI_CONTROLLER_PRIVATE_DATA  *Private;
  EDKII_PEI_ATA_PASS_THRU_PPI       *AtaPassThru;
  EFI_ATA_COMMAND_BLOCK             Acb;
  EFI_ATA_PASS_THRU_COMMAND_PACKET  Packet;
  EFI_STATUS                        Status;
  VOID                              *NewBuffer;
 
  Private     = DeviceData->Private;
  AtaPassThru = &Private->AtaPassThruPpi;
 
  //
  // Ensure IsTrustSend are valid boolean values
  //
  ASSERT ((UINTN)IsTrustSend < 2);
  if ((UINTN)IsTrustSend >= 2) {
    return EFI_INVALID_PARAMETER;
  }
 
  //
  // Prepare for ATA command block.
  //
  ZeroMem (&Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
  if (TransferLength == 0) {
    Acb.AtaCommand = ATA_CMD_TRUST_NON_DATA;
  } else {
    Acb.AtaCommand = mAtaTrustCommands[IsTrustSend];
  }
 
  Acb.AtaFeatures     = SecurityProtocolId;
  Acb.AtaSectorCount  = (UINT8)(TransferLength / 512);
  Acb.AtaSectorNumber = (UINT8)((TransferLength / 512) >> 8);
  //
  // NOTE: ATA Spec has no explicitly definition for Security Protocol Specific layout.
  // Here use big endian for Cylinder register.
  //
  Acb.AtaCylinderHigh = (UINT8)SecurityProtocolSpecificData;
  Acb.AtaCylinderLow  = (UINT8)(SecurityProtocolSpecificData >> 8);
  Acb.AtaDeviceHead   = (UINT8)(BIT7 | BIT6 | BIT5 |
                                (DeviceData->PortMultiplier == 0xFFFF ?
                                 0 : (DeviceData->PortMultiplier << 4)));
 
  //
  // Prepare for ATA pass through packet.
  //
  ZeroMem (&Packet, sizeof (EFI_ATA_PASS_THRU_COMMAND_PACKET));
  if (TransferLength == 0) {
    Packet.InTransferLength  = 0;
    Packet.OutTransferLength = 0;
    Packet.Protocol          = EFI_ATA_PASS_THRU_PROTOCOL_ATA_NON_DATA;
  } else if (IsTrustSend) {
    //
    // Check the alignment of the incoming buffer prior to invoking underlying
    // ATA PassThru PPI.
    //
    if ((AtaPassThru->Mode->IoAlign > 1) &&
        !ADDRESS_IS_ALIGNED (Buffer, AtaPassThru->Mode->IoAlign))
    {
      NewBuffer = AllocateAlignedPages (
                    EFI_SIZE_TO_PAGES (TransferLength),
                    AtaPassThru->Mode->IoAlign
                    );
      if (NewBuffer == NULL) {
        return EFI_OUT_OF_RESOURCES;
      }
 
      CopyMem (NewBuffer, Buffer, TransferLength);
      Buffer = NewBuffer;
    }
 
    Packet.OutDataBuffer     = Buffer;
    Packet.OutTransferLength = (UINT32)TransferLength;
    Packet.Protocol          = mAtaPassThruCmdProtocols[IsTrustSend];
  } else {
    Packet.InDataBuffer     = Buffer;
    Packet.InTransferLength = (UINT32)TransferLength;
    Packet.Protocol         = mAtaPassThruCmdProtocols[IsTrustSend];
  }
 
  Packet.Asb     = NULL;
  Packet.Acb     = &Acb;
  Packet.Timeout = Timeout;
  Packet.Length  = EFI_ATA_PASS_THRU_LENGTH_BYTES;
 
  Status = AtaPassThru->PassThru (
                          AtaPassThru,
                          DeviceData->Port,
                          DeviceData->PortMultiplier,
                          &Packet
                          );
  if (TransferLengthOut != NULL) {
    if (!IsTrustSend) {
      *TransferLengthOut = Packet.InTransferLength;
    }
  }
 
  return Status;
}