diff --git a/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c b/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c
index 180a60b5aa..0b7141c4f1 100644
--- a/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c
+++ b/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.c
@@ -608,6 +608,148 @@ AhciBuildCommandFis (
   CmdFis->AhciCFisDevHead     = (UINT8) (AtaCommandBlock->AtaDeviceHead | 0xE0);
 }
 
+/**
+  Wait until SATA device reports it is ready for operation.
+
+  @param[in] PciIo    Pointer to AHCI controller PciIo.
+  @param[in] Port     SATA port index on which to reset.
+
+  @retval EFI_SUCCESS  Device ready for operation.
+  @retval EFI_TIMEOUT  Device failed to get ready within required period.
+**/
+EFI_STATUS
+AhciWaitDeviceReady (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT8                Port
+   )
+{
+  UINT32      PhyDetectDelay;
+  UINT32      Data;
+  UINT32      Offset;
+
+  //
+  // 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 = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SERR;
+    if (AhciReadReg(PciIo, Offset) != 0) {
+      AhciWriteReg (PciIo, Offset, AhciReadReg(PciIo, Offset));
+    }
+    Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+
+    Data = AhciReadReg (PciIo, Offset) & EFI_AHCI_PORT_TFD_MASK;
+    if (Data == 0) {
+      break;
+    }
+
+    MicroSecondDelay (1000);
+    PhyDetectDelay--;
+  } while (PhyDetectDelay > 0);
+
+  if (PhyDetectDelay == 0) {
+    DEBUG ((DEBUG_ERROR, "Port %d Device not ready (TFD=0x%X)\n", Port, Data));
+    return EFI_TIMEOUT;
+  } else {
+    return EFI_SUCCESS;
+  }
+}
+
+
+/**
+  Reset the SATA port. Algorithm follows AHCI spec 1.3.1 section 10.4.2
+
+  @param[in] PciIo    Pointer to AHCI controller PciIo.
+  @param[in] Port     SATA port index on which to reset.
+
+  @retval EFI_SUCCESS  Port reset.
+  @retval Others       Failed to reset the port.
+**/
+EFI_STATUS
+AhciResetPort (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT8                Port
+  )
+{
+  UINT32      Offset;
+  EFI_STATUS  Status;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SCTL;
+  AhciOrReg (PciIo, Offset, EFI_AHCI_PORT_SCTL_DET_INIT);
+  //
+  // SW is required to keep DET set to 0x1 at least for 1 milisecond to ensure that
+  // at least one COMRESET signal is sent.
+  //
+  MicroSecondDelay(1000);
+  AhciAndReg (PciIo, Offset, ~(UINT32)EFI_AHCI_PORT_SSTS_DET_MASK);
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SSTS;
+  Status = AhciWaitMmioSet (PciIo, Offset, EFI_AHCI_PORT_SSTS_DET_MASK, EFI_AHCI_PORT_SSTS_DET_PCE, ATA_ATAPI_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  return AhciWaitDeviceReady (PciIo, Port);
+}
+
+/**
+  Recovers the SATA port from error condition.
+  This function implements algorithm described in
+  AHCI spec 1.3.1 section 6.2.2
+
+  @param[in] PciIo    Pointer to AHCI controller PciIo.
+  @param[in] Port     SATA port index on which to check.
+
+  @retval EFI_SUCCESS  Port recovered.
+  @retval Others       Failed to recover port.
+**/
+EFI_STATUS
+AhciRecoverPortError (
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,
+  IN UINT8                Port
+  )
+{
+  UINT32      Offset;
+  UINT32      PortInterrupt;
+  UINT32      PortTfd;
+  EFI_STATUS  Status;
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_IS;
+  PortInterrupt = AhciReadReg (PciIo, Offset);
+  if ((PortInterrupt & EFI_AHCI_PORT_IS_FATAL_ERROR_MASK) == 0) {
+    //
+    // No fatal error detected. Exit with success as port should still be operational.
+    // No need to clear IS as it will be cleared when the next command starts.
+    //
+    return EFI_SUCCESS;
+  }
+
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_CMD;
+  AhciAndReg (PciIo, Offset, ~(UINT32)EFI_AHCI_PORT_CMD_ST);
+
+  Status = AhciWaitMmioSet (PciIo, Offset, EFI_AHCI_PORT_CMD_CR, 0, ATA_ATAPI_TIMEOUT);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Ahci port %d is in hung state, aborting recovery\n", Port));
+    return Status;
+  }
+
+  //
+  // If TFD.BSY or TFD.DRQ is still set it means that drive is hung and software has
+  // to reset it before sending any additional commands.
+  //
+  Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
+  PortTfd = AhciReadReg (PciIo, Offset);
+  if ((PortTfd & (EFI_AHCI_PORT_TFD_BSY | EFI_AHCI_PORT_TFD_DRQ)) != 0) {
+    Status = AhciResetPort (PciIo, Port);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "Failed to reset the port %d\n", Port));
+    }
+  }
+
+  return EFI_SUCCESS;
+}
+
 /**
   Checks if specified FIS has been received.
 
@@ -827,6 +969,10 @@ AhciPioTransfer (
     Status = AhciWaitUntilFisReceived (PciIo, Port, Timeout, SataFisD2H);
   }
 
+  if (Status == EFI_DEVICE_ERROR) {
+    AhciRecoverPortError (PciIo, Port);
+  }
+
 Exit:
   AhciStopCommand (
     PciIo,
@@ -1007,6 +1153,10 @@ AhciDmaTransfer (
     Status = AhciWaitUntilFisReceived (PciIo, Port, Timeout, SataFisD2H);
   }
 
+  if (Status == EFI_DEVICE_ERROR) {
+    AhciRecoverPortError (PciIo, Port);
+  }
+
 Exit:
   //
   // For Blocking mode, the command should be stopped, the Fis should be disabled
@@ -1119,6 +1269,9 @@ AhciNonDataTransfer (
   }
 
   Status = AhciWaitUntilFisReceived (PciIo, Port, Timeout, SataFisD2H);
+  if (Status == EFI_DEVICE_ERROR) {
+    AhciRecoverPortError (PciIo, Port);
+  }
 
 Exit:
   AhciStopCommand (
@@ -2583,29 +2736,8 @@ AhciModeInitialization (
         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 = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_SERR;
-        if (AhciReadReg(PciIo, Offset) != 0) {
-          AhciWriteReg (PciIo, Offset, AhciReadReg(PciIo, Offset));
-        }
-        Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
-
-        Data = AhciReadReg (PciIo, Offset) & EFI_AHCI_PORT_TFD_MASK;
-        if (Data == 0) {
-          break;
-        }
-
-        MicroSecondDelay (1000);
-        PhyDetectDelay--;
-      } while (PhyDetectDelay > 0);
-
-      if (PhyDetectDelay == 0) {
-        DEBUG ((EFI_D_ERROR, "Port %d Device presence detected but phy not ready (TFD=0x%X)\n", Port, Data));
+      Status = AhciWaitDeviceReady (PciIo, Port);
+      if (EFI_ERROR (Status)) {
         continue;
       }
 
diff --git a/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h b/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h
index 6bcff1bb7b..338447a55f 100644
--- a/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h
+++ b/MdeModulePkg/Bus/Ata/AtaAtapiPassThru/AhciMode.h
@@ -114,6 +114,7 @@ typedef union {
 #define   EFI_AHCI_PORT_IS_CLEAR               0xFFFFFFFF
 #define   EFI_AHCI_PORT_IS_FIS_CLEAR           0x0000000F
 #define   EFI_AHCI_PORT_IS_ERROR_MASK          (EFI_AHCI_PORT_IS_INFS | EFI_AHCI_PORT_IS_IFS | EFI_AHCI_PORT_IS_HBDS | EFI_AHCI_PORT_IS_HBFS | EFI_AHCI_PORT_IS_TFES)
+#define   EFI_AHCI_PORT_IS_FATAL_ERROR_MASK    (EFI_AHCI_PORT_IS_IFS | EFI_AHCI_PORT_IS_HBDS | EFI_AHCI_PORT_IS_HBFS | EFI_AHCI_PORT_IS_TFES)
 
 #define EFI_AHCI_PORT_IE                       0x0014
 #define EFI_AHCI_PORT_CMD                      0x0018
@@ -122,9 +123,11 @@ typedef union {
 #define   EFI_AHCI_PORT_CMD_SUD                BIT1
 #define   EFI_AHCI_PORT_CMD_POD                BIT2
 #define   EFI_AHCI_PORT_CMD_CLO                BIT3
-#define   EFI_AHCI_PORT_CMD_CR                 BIT15
 #define   EFI_AHCI_PORT_CMD_FRE                BIT4
+#define   EFI_AHCI_PORT_CMD_CCS_MASK           (BIT8 | BIT9 | BIT10 | BIT11 | BIT12)
+#define   EFI_AHCI_PORT_CMD_CCS_SHIFT          8
 #define   EFI_AHCI_PORT_CMD_FR                 BIT14
+#define   EFI_AHCI_PORT_CMD_CR                 BIT15
 #define   EFI_AHCI_PORT_CMD_MASK               ~(EFI_AHCI_PORT_CMD_ST | EFI_AHCI_PORT_CMD_FRE | EFI_AHCI_PORT_CMD_COL)
 #define   EFI_AHCI_PORT_CMD_PMA                BIT17
 #define   EFI_AHCI_PORT_CMD_HPCP               BIT18