[PATCH v2 5/7] EmbeddedPkg: SiI3132: Break out FIS command submission

[Jeremy Linton <jeremy.linton@arm.com>]

The existing ATA pass-through routine builds the FIS and handles
submission to the hardware. Break out the FIS submission part
so that it can be utilized by the SCSI pass-through. Also,
tighten up the error handling a bit. Starting with removal
of the ASSERTs on errors. ATAPI like SCSI uses check
conditions to indicate device state changes. So these error
paths can get exercised on CD disk change/etc. Further
we want the clamp the timeouts within a range rather than
spinning forever if the port fails to become ready.

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Jeremy Linton <jeremy.linton@arm.com>
---
 .../Drivers/SataSiI3132Dxe/SiI3132AtaPassThru.c    | 225 +++++++++++++--------
 OpenPlatformPkg                                    |   2 +-
 2 files changed, 138 insertions(+), 89 deletions(-)

diff --git a/EmbeddedPkg/Drivers/SataSiI3132Dxe/SiI3132AtaPassThru.c b/EmbeddedPkg/Drivers/SataSiI3132Dxe/SiI3132AtaPassThru.c
index 2fb5fd6..b14a8aa 100644
--- a/EmbeddedPkg/Drivers/SataSiI3132Dxe/SiI3132AtaPassThru.c
+++ b/EmbeddedPkg/Drivers/SataSiI3132Dxe/SiI3132AtaPassThru.c
@@ -22,7 +22,8 @@ GetSataDevice (
   IN  SATA_SI3132_INSTANCE* SataInstance,
   IN  UINT16 Port,
   IN  UINT16 PortMultiplierPort
-) {
+  )
+{
   LIST_ENTRY              *List;
   SATA_SI3132_PORT        *SataPort;
   SATA_SI3132_DEVICE      *SataDevice;
@@ -44,6 +45,124 @@ GetSataDevice (
   return NULL;
 }
 
+UINT32
+SiI3231DeviceReady (
+  IN SATA_SI3132_PORT *SataPort,
+  IN EFI_PCI_IO_PROTOCOL *PciIo
+  )
+{
+  UINT32                  Value32;
+  UINT32                  Timeout;
+  Timeout = SI_DEFAULT_TIMEOUT;
+
+  do {
+    SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_STATUS_REG, &Value32);
+    Timeout--;
+  } while (Timeout && !(Value32 & SII3132_PORT_STATUS_PORTREADY));
+  if (Timeout == 0) {
+    DEBUG ((DEBUG_WARN, "SiI3132AtaPassThru() Device not ready, try anyway\n"));
+    //Consider doing a device reset here.
+  }
+
+  return Timeout;
+}
+
+EFI_STATUS
+SiI3132IssueCommand (
+  IN SATA_SI3132_PORT *SataPort,
+  EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT32 Timeout,
+  VOID *StatusBlock
+  )
+{
+  CONST UINT32 IrqMask;
+  UINT32       Value32, Error;
+  CONST UINTN  EmptySlot;
+  EFI_STATUS   Status;
+
+  IrqMask = (SII3132_PORT_INT_CMDCOMPL | SII3132_PORT_INT_CMDERR) << 16;
+  EmptySlot = 0;
+  SiI3231DeviceReady (SataPort, PciIo);
+  // Clear IRQ
+  SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, IrqMask);
+
+  if (!FeaturePcdGet (PcdSataSiI3132FeatureDirectCommandIssuing)) {
+    // Indirect Command Issuance
+
+    //TODO: Find which slot is free (maybe use the Cmd FIFO)
+    //SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_CMDEXECFIFO_REG, &EmptySlot);
+    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_CMDACTIV_REG + (EmptySlot * 8),
+                     (UINT32)(SataPort->PhysAddrHostPRB & 0xFFFFFFFF));
+    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_CMDACTIV_REG + (EmptySlot * 8) + 4,
+                     (UINT32)((SataPort->PhysAddrHostPRB >> 32) & 0xFFFFFFFF));
+  } else {
+    // Direct Command Issuance
+    DEBUG ((DEBUG_ERROR ,"SiI3132AtaPassThru() Untested path\n"));
+    Status = PciIo->Mem.Write (PciIo, EfiPciIoWidthUint32, 1, // Bar 1
+        SataPort->RegBase + (EmptySlot * 0x80),
+        sizeof (SATA_SI3132_PRB) / 4,
+        SataPort->HostPRB);
+    ASSERT_EFI_ERROR (Status);
+
+    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_CMDEXECFIFO_REG, EmptySlot);
+  }
+
+  // Clamp the timeout range
+  if (Timeout < 1) {
+    Timeout = SI_DEFAULT_TIMEOUT;
+  } else if (Timeout > SI_DEFAULT_TIMEOUT) {
+    Timeout = SI_DEFAULT_TIMEOUT;
+  }
+
+  SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, &Value32);
+  while (Timeout && !(Value32 & IrqMask)) {
+    gBS->Stall (1);
+    SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, &Value32);
+    Timeout--;
+  }
+
+  // Fill Packet Ata Status Block
+  Status = PciIo->Mem.Read (PciIo, EfiPciIoWidthUint32, 1, // Bar 1
+    SataPort->RegBase + 0x08,
+    sizeof (EFI_ATA_STATUS_BLOCK) / 4,
+    StatusBlock);
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "SiI3132AtaPassThru() status (%d) block %X %X\n",Status, SataPort->RegBase, StatusBlock));
+  }
+
+  if (Timeout == 0) {
+    DEBUG ((DEBUG_ERROR, "SiI3132AtaPassThru() Err:Timeout\n"));
+    // Flush the command, reinit port
+    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_CONTROLSET_REG, SII3132_PORT_CONTROL_INT);
+    SiI3231DeviceReady (SataPort, PciIo);
+    Status = EFI_TIMEOUT;
+
+  } else if (Value32 & (SII3132_PORT_INT_CMDERR << 16)) {
+    UINT32 Serror;
+
+    SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_CMDERROR_REG, &Error);
+    SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_SERROR_REG, &Serror);
+    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, Value32 & 0xFF00); //clear error bits
+
+    DEBUG ((DEBUG_INFO, "SiI3132AtaPassThru() CmdErr:0x%X (SiI3132 Err:0x%X) (STATUS: %X ERROR:%X) SERROR=%X\n",
+            Value32, Error, SataPort->HostPRB->Fis.Command,SataPort->HostPRB->Fis.Features, Serror));
+
+    // clear port status
+    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_CONTROLSET_REG, SII3132_PORT_CONTROL_INT);
+    SiI3231DeviceReady (SataPort, PciIo);
+    Status = EFI_DEVICE_ERROR;
+
+  } else if (Value32 & (SII3132_PORT_INT_CMDCOMPL << 16)) {
+    // Clear Command Complete
+    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, SII3132_PORT_INT_CMDCOMPL << 16);
+    Status = EFI_SUCCESS;
+
+  }
+
+  return Status;
+}
+
 EFI_STATUS
 SiI3132AtaPassThruCommand (
   IN     SATA_SI3132_INSTANCE             *SataSiI3132Instance,
@@ -58,18 +177,14 @@ SiI3132AtaPassThruCommand (
   UINTN                   InDataBufferLength = 0;
   EFI_PHYSICAL_ADDRESS    PhysOutDataBuffer;
   UINTN                   OutDataBufferLength;
-  CONST UINTN             EmptySlot = 0;
   UINTN                   Control = PRB_CTRL_ATA;
-  UINTN                   Protocol = 0;
-  UINT32                  Value32, Error, Timeout = 0;
-  CONST UINT32            IrqMask = (SII3132_PORT_INT_CMDCOMPL | SII3132_PORT_INT_CMDERR) << 16;
+  UINTN                   Protocol = PRB_PROT_DEFAULT;
   EFI_STATUS              Status;
   VOID*                   PciAllocMapping = NULL;
   EFI_PCI_IO_PROTOCOL     *PciIo;
 
   PciIo = SataSiI3132Instance->PciIo;
   ZeroMem (SataPort->HostPRB, sizeof (SATA_SI3132_PRB));
-
   // Construct Si3132 PRB
   switch (Packet->Protocol) {
   case EFI_ATA_PASS_THRU_PROTOCOL_ATA_HARDWARE_RESET:
@@ -92,7 +207,8 @@ SiI3132AtaPassThruCommand (
   case EFI_ATA_PASS_THRU_PROTOCOL_PIO_DATA_IN:
     // Fixup the size for block transfer. Following UEFI Specification, 'InTransferLength' should
     // be in number of bytes. But for most data transfer commands, the value is in number of blocks
-    if (Packet->Acb->AtaCommand == ATA_CMD_IDENTIFY_DRIVE) {
+    if ( (Packet->Acb->AtaCommand == ATA_CMD_IDENTIFY_DRIVE) ||
+         (Packet->Acb->AtaCommand == ATA_CMD_IDENTIFY_DEVICE) ) {
       InDataBufferLength = Packet->InTransferLength;
     } else {
       SataDevice = GetSataDevice (SataSiI3132Instance, SataPort->Index, PortMultiplierPort);
@@ -108,6 +224,7 @@ SiI3132AtaPassThruCommand (
                Packet->InDataBuffer, &InDataBufferLength, &PhysInDataBuffer, &PciAllocMapping
                );
     if (EFI_ERROR (Status)) {
+        DEBUG ((DEBUG_ERROR, "SiI map() failure %d\n", Status));
       return Status;
     }
 
@@ -121,8 +238,8 @@ SiI3132AtaPassThruCommand (
     CopyMem (&SataPort->HostPRB->Fis, Packet->Acb, sizeof (EFI_ATA_COMMAND_BLOCK));
 
     // Fixup the FIS
-    SataPort->HostPRB->Fis.FisType = 0x27; // Register - Host to Device FIS
-    SataPort->HostPRB->Fis.Control = 1 << 7; // Is a command
+    SataPort->HostPRB->Fis.FisType = SII_FIS_REGISTER_H2D; // Register - Host to Device FIS
+    SataPort->HostPRB->Fis.Control = SII_FIS_CONTROL_CMD; // Is a command
     if (FeaturePcdGet (PcdSataSiI3132FeaturePMPSupport)) {
       SataPort->HostPRB->Fis.Control |= PortMultiplierPort & 0xFF;
     }
@@ -188,81 +305,10 @@ SiI3132AtaPassThruCommand (
   SataPort->HostPRB->Control = Control;
   SataPort->HostPRB->ProtocolOverride = Protocol;
 
-  // Clear IRQ
-  SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, IrqMask);
-
-  if (!FeaturePcdGet (PcdSataSiI3132FeatureDirectCommandIssuing)) {
-    // Indirect Command Issuance
-
-    //TODO: Find which slot is free (maybe use the Cmd FIFO)
-    //SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_CMDEXECFIFO_REG, &EmptySlot);
-
-    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_CMDACTIV_REG + (EmptySlot * 8),
-                     (UINT32)(SataPort->PhysAddrHostPRB & 0xFFFFFFFF));
-    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_CMDACTIV_REG + (EmptySlot * 8) + 4,
-                     (UINT32)((SataPort->PhysAddrHostPRB >> 32) & 0xFFFFFFFF));
-  } else {
-    // Direct Command Issuance
-    Status = PciIo->Mem.Write (PciIo, EfiPciIoWidthUint32, 1, // Bar 1
-        SataPort->RegBase + (EmptySlot * 0x80),
-        sizeof (SATA_SI3132_PRB) / 4,
-        SataPort->HostPRB);
-    ASSERT_EFI_ERROR (Status);
-
-    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_CMDEXECFIFO_REG, EmptySlot);
-  }
-
-#if 0
-  // Could need to be implemented if we run multiple command in parallel to know which slot has been completed
-  SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_SLOTSTATUS_REG, &Value32);
-  Timeout = Packet->Timeout;
-  while (!Timeout && !Value32) {
-    gBS->Stall (1);
-    SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_SLOTSTATUS_REG, &Value32);
-    Timeout--;
-  }
-#else
-  SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, &Value32);
-  if (!Packet->Timeout) {
-    while (!(Value32 & IrqMask)) {
-      gBS->Stall (1);
-      SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, &Value32);
-    }
-  } else {
-    Timeout = Packet->Timeout;
-    while (Timeout && !(Value32 & IrqMask)) {
-      gBS->Stall (1);
-      SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, &Value32);
-      Timeout--;
-    }
-  }
-#endif
-  // Fill Packet Ata Status Block
-  Status = PciIo->Mem.Read (PciIo, EfiPciIoWidthUint32, 1, // Bar 1
-      SataPort->RegBase + 0x08,
-      sizeof (EFI_ATA_STATUS_BLOCK) / 4,
-      Packet->Asb);
-  ASSERT_EFI_ERROR (Status);
-
-
-  if ((Packet->Timeout != 0) && (Timeout == 0)) {
-    DEBUG ((EFI_D_ERROR, "SiI3132AtaPassThru() Err:Timeout\n"));
-    //ASSERT (0);
-    return EFI_TIMEOUT;
-  } else if (Value32 & (SII3132_PORT_INT_CMDERR << 16)) {
-    SATA_PORT_READ32 (SataPort->RegBase + SII3132_PORT_CMDERROR_REG, &Error);
-    DEBUG ((EFI_D_ERROR, "SiI3132AtaPassThru() CmdErr:0x%X (SiI3132 Err:0x%X)\n", Value32, Error));
-    ASSERT (0);
-    return EFI_DEVICE_ERROR;
-  } else if (Value32 & (SII3132_PORT_INT_CMDCOMPL << 16)) {
-    // Clear Command Complete
-    SATA_PORT_WRITE32 (SataPort->RegBase + SII3132_PORT_INTSTATUS_REG, SII3132_PORT_INT_CMDCOMPL << 16);
-
-    if (PciAllocMapping) {
-      Status = PciIo->Unmap (PciIo, PciAllocMapping);
-      ASSERT (!EFI_ERROR (Status));
-    }
+  Status = SiI3132IssueCommand (SataPort, PciIo, Packet->Timeout, Packet->Asb);
 
+  if (!EFI_ERROR (Status))
+  {
     // If the command was ATA_CMD_IDENTIFY_DRIVE then we need to update the BlockSize
     if (Packet->Acb->AtaCommand == ATA_CMD_IDENTIFY_DRIVE) {
       ATA_IDENTIFY_DATA *IdentifyData = (ATA_IDENTIFY_DATA*)Packet->InDataBuffer;
@@ -279,11 +325,14 @@ SiI3132AtaPassThruCommand (
         SataDevice->BlockSize = 0x200;
       }
     }
-    return EFI_SUCCESS;
-  } else {
-    ASSERT (0);
-    return EFI_DEVICE_ERROR;
   }
+
+  if (PciAllocMapping) {
+    Status = PciIo->Unmap (PciIo, PciAllocMapping);
+  }
+
+  return Status;
+
 }
 
 /**
@@ -339,8 +388,7 @@ SiI3132AtaPassThru (
   }
   SataPort = SataDevice->Port;
 
-  DEBUG ((EFI_D_INFO, "SiI3132AtaPassThru(%d,%d) : AtaCmd:0x%X Prot:%d\n", Port, PortMultiplierPort,
+  DEBUG ((DEBUG_VERBOSE, "SiI3132AtaPassThru(%p,%d,%d) : AtaCmd:0x%X Prot:%d\n", SataPort, Port, PortMultiplierPort,
          Packet->Acb->AtaCommand, Packet->Protocol));
 
   return SiI3132AtaPassThruCommand (SataSiI3132Instance, SataPort, PortMultiplierPort, Packet, Event);
-- 
2.9.3