MdeModulePkg/Xhci: Remove TDs from transfer ring when timeout happens

The error handling for timeout case is enhanced to remove TDs from
transfer ring. The original code only removed s/w URB, but the h/w
transfer descriptor TDs didn't get removed. It would cause data lost
for data stream peripheral, such as usb-to-serial device, from the
s/w perspective.

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Feng Tian <feng.tian@intel.com>
Reviewed-by: Star Zeng <star.zeng@intel.com>
Reviewed-by: Baraneedharan Anbazhagan <anbazhagan@hp.com>

git-svn-id: https://svn.code.sf.net/p/edk2/code/trunk/edk2@18313 6f19259b-4bc3-4df7-8a09-765794883524

MdeModulePkg/Bus/Pci/XhciDxe/Xhci.c

@@ -905,17 +905,28 @@ XhcControlTransfer (
   *TransferResult = Urb->Result;
   *DataLength     = Urb->Completed;
 
-  if (*TransferResult == EFI_USB_NOERROR) {
+  if (Status == EFI_TIMEOUT) {
-    Status = EFI_SUCCESS;
+    //
-  } else if (*TransferResult == EFI_USB_ERR_STALL) {
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
-    RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+    //
-    if (EFI_ERROR (RecoveryStatus)) {
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
-      DEBUG ((EFI_D_ERROR, "XhcControlTransfer: XhcRecoverHaltedEndpoint failed\n"));
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcControlTransfer: XhcDequeueTrbFromEndpoint failed\n"));
     }
-    Status = EFI_DEVICE_ERROR;
     goto FREE_URB;
   } else {
-    goto FREE_URB;
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcControlTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+      goto FREE_URB;
+    } else {
+      goto FREE_URB;
+    }
   }
 
   Xhc->PciIo->Flush (Xhc->PciIo);
@@ -1241,14 +1252,24 @@ XhcBulkTransfer (
   *TransferResult = Urb->Result;
   *DataLength     = Urb->Completed;
 
-  if (*TransferResult == EFI_USB_NOERROR) {
+  if (Status == EFI_TIMEOUT) {
-    Status = EFI_SUCCESS;
+    //
-  } else if (*TransferResult == EFI_USB_ERR_STALL) {
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
-    RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+    //
-    if (EFI_ERROR (RecoveryStatus)) {
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
-      DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: XhcRecoverHaltedEndpoint failed\n"));
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcBulkTransfer: XhcDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcBulkTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
     }
-    Status = EFI_DEVICE_ERROR;
   }
 
   Xhc->PciIo->Flush (Xhc->PciIo);
@@ -1538,14 +1559,24 @@ XhcSyncInterruptTransfer (
   *TransferResult = Urb->Result;
   *DataLength     = Urb->Completed;
 
-  if (*TransferResult == EFI_USB_NOERROR) {
+  if (Status == EFI_TIMEOUT) {
-    Status = EFI_SUCCESS;
+    //
-  } else if (*TransferResult == EFI_USB_ERR_STALL) {
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
-    RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+    //
-    if (EFI_ERROR (RecoveryStatus)) {
+    RecoveryStatus = XhcDequeueTrbFromEndpoint(Xhc, Urb);
-      DEBUG ((EFI_D_ERROR, "XhcSyncInterruptTransfer: XhcRecoverHaltedEndpoint failed\n"));
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcSyncInterruptTransfer: XhcDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcSyncInterruptTransfer: XhcRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
     }
-    Status = EFI_DEVICE_ERROR;
   }
 
   Xhc->PciIo->Flush (Xhc->PciIo);

MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.c

@@ -645,12 +645,8 @@ XhcRecoverHaltedEndpoint (
   )
 {
   EFI_STATUS                  Status;
-  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
-  CMD_TRB_RESET_ENDPOINT      CmdTrbResetED;
-  CMD_SET_TR_DEQ_POINTER      CmdSetTRDeq;
   UINT8                       Dci;
   UINT8                       SlotId;
-  EFI_PHYSICAL_ADDRESS        PhyAddr;
 
   Status = EFI_SUCCESS;
   SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
@@ -665,17 +661,7 @@ XhcRecoverHaltedEndpoint (
   //
   // 1) Send Reset endpoint command to transit from halt to stop state
   //
-  ZeroMem (&CmdTrbResetED, sizeof (CmdTrbResetED));
+  Status = XhcResetEndpoint(Xhc, SlotId, Dci);
-  CmdTrbResetED.CycleBit = 1;
-  CmdTrbResetED.Type     = TRB_TYPE_RESET_ENDPOINT;
-  CmdTrbResetED.EDID     = Dci;
-  CmdTrbResetED.SlotId   = SlotId;
-  Status = XhcCmdTransfer (
-             Xhc,
-             (TRB_TEMPLATE *) (UINTN) &CmdTrbResetED,
-             XHC_GENERIC_TIMEOUT,
-             (TRB_TEMPLATE **) (UINTN) &EvtTrb
-             );
   if (EFI_ERROR(Status)) {
     DEBUG ((EFI_D_ERROR, "XhcRecoverHaltedEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
     goto Done;
@@ -684,22 +670,70 @@ XhcRecoverHaltedEndpoint (
   //
   // 2)Set dequeue pointer
   //
-  ZeroMem (&CmdSetTRDeq, sizeof (CmdSetTRDeq));
+  Status = XhcSetTrDequeuePointer(Xhc, SlotId, Dci, Urb);
-  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Urb->Ring->RingEnqueue, sizeof (CMD_SET_TR_DEQ_POINTER));
-  CmdSetTRDeq.PtrLo    = XHC_LOW_32BIT (PhyAddr) | Urb->Ring->RingPCS;
-  CmdSetTRDeq.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
-  CmdSetTRDeq.CycleBit = 1;
-  CmdSetTRDeq.Type     = TRB_TYPE_SET_TR_DEQUE;
-  CmdSetTRDeq.Endpoint = Dci;
-  CmdSetTRDeq.SlotId   = SlotId;
-  Status = XhcCmdTransfer (
-             Xhc,
-             (TRB_TEMPLATE *) (UINTN) &CmdSetTRDeq,
-             XHC_GENERIC_TIMEOUT,
-             (TRB_TEMPLATE **) (UINTN) &EvtTrb
-             );
   if (EFI_ERROR(Status)) {
-    DEBUG ((EFI_D_ERROR, "XhcRecoverHaltedEndpoint: Set Dequeue Pointer Failed, Status = %r\n", Status));
+    DEBUG ((EFI_D_ERROR, "XhcRecoverHaltedEndpoint: Set Transfer Ring Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3)Ring the doorbell to transit from stop to active
+  //
+  XhcRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDequeueTrbFromEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcEndpointToDci (Urb->Ep.EpAddr, (UINT8)(Urb->Ep.Direction));
+  ASSERT (Dci < 32);
+  
+  DEBUG ((EFI_D_INFO, "Stop Slot = %x,Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Stop endpoint command to stop xHC from executing of the TDs on the endpoint
+  //
+  Status = XhcStopEndpoint(Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDequeueTrbFromEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2)Set dequeue pointer
+  //
+  Status = XhcSetTrDequeuePointer(Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcDequeueTrbFromEndpoint: Set Transfer Ring Dequeue Pointer Failed, Status = %r\n", Status));
     goto Done;
   }
 
@@ -3061,6 +3095,105 @@ XhcStopEndpoint (
   return Status;
 }
 
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Reset endpoint successfully.
+  @retval Others                Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcResetEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_TRB_RESET_ENDPOINT      CmdTrbResetED;
+
+  DEBUG ((EFI_D_INFO, "XhcResetEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbResetED, sizeof (CmdTrbResetED));
+  CmdTrbResetED.CycleBit = 1;
+  CmdTrbResetED.Type     = TRB_TYPE_RESET_ENDPOINT;
+  CmdTrbResetED.EDID     = Dci;
+  CmdTrbResetED.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbResetED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcResetEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+  @param  Urb                   The dequeue pointer of the transfer ring specified
+                                by the urb to be updated.
+
+  @retval EFI_SUCCESS           Set transfer ring dequeue pointer succeeds.
+  @retval Others                Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetTrDequeuePointer (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci,
+  IN URB                    *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_SET_TR_DEQ_POINTER      CmdSetTRDeq;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  DEBUG ((EFI_D_INFO, "XhcSetTrDequeuePointer: Slot = 0x%x, Dci = 0x%x, Urb = 0x%x\n", SlotId, Dci, Urb));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdSetTRDeq, sizeof (CmdSetTRDeq));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Urb->Ring->RingEnqueue, sizeof (CMD_SET_TR_DEQ_POINTER));
+  CmdSetTRDeq.PtrLo    = XHC_LOW_32BIT (PhyAddr) | Urb->Ring->RingPCS;
+  CmdSetTRDeq.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdSetTRDeq.CycleBit = 1;
+  CmdSetTRDeq.Type     = TRB_TYPE_SET_TR_DEQUE;
+  CmdSetTRDeq.Endpoint = Dci;
+  CmdSetTRDeq.SlotId   = SlotId;
+  Status = XhcCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdSetTRDeq,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcSetTrDequeuePointer: Set TR Dequeue Pointer Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
 /**
   Set interface through XHCI's Configure_Endpoint cmd.
 

MdeModulePkg/Bus/Pci/XhciDxe/XhciSched.h

@@ -1317,6 +1317,86 @@ XhcRecoverHaltedEndpoint (
   IN  URB                 *Urb
   );
 
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcDequeueTrbFromEndpoint (
+  IN  USB_XHCI_INSTANCE   *Xhc,
+  IN  URB                 *Urb
+  );
+
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Stop endpoint successfully.
+  @retval Others                Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcStopEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  );
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+
+  @retval EFI_SUCCESS           Reset endpoint successfully.
+  @retval Others                Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcResetEndpoint (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci
+  );
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc                   The XHCI Instance.
+  @param  SlotId                The slot id to be configured.
+  @param  Dci                   The device context index of endpoint.
+  @param  Urb                   The dequeue pointer of the transfer ring specified
+                                by the urb to be updated.
+
+  @retval EFI_SUCCESS           Set transfer ring dequeue pointer succeeds.
+  @retval Others                Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcSetTrDequeuePointer (
+  IN USB_XHCI_INSTANCE      *Xhc,
+  IN UINT8                  SlotId,
+  IN UINT8                  Dci,
+  IN URB                    *Urb
+  );
+
 /**
   Create a new URB for a new transaction.
 

MdeModulePkg/Bus/Pci/XhciPei/XhcPeim.c

@@ -648,17 +648,28 @@ XhcPeiControlTransfer (
   *TransferResult = Urb->Result;
   *DataLength     = Urb->Completed;
 
-  if (*TransferResult == EFI_USB_NOERROR) {
+  if (Status == EFI_TIMEOUT) {
-    Status = EFI_SUCCESS;
+    //
-  } else if (*TransferResult == EFI_USB_ERR_STALL) {
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
-    RecoveryStatus = XhcPeiRecoverHaltedEndpoint(Xhc, Urb);
+    //
-    if (EFI_ERROR (RecoveryStatus)) {
+    RecoveryStatus = XhcPeiDequeueTrbFromEndpoint(Xhc, Urb);
-      DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: XhcPeiRecoverHaltedEndpoint failed\n"));
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcPeiControlTransfer: XhcPeiDequeueTrbFromEndpoint failed\n"));
     }
-    Status = EFI_DEVICE_ERROR;
     goto FREE_URB;
   } else {
-    goto FREE_URB;
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcPeiRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcPeiControlTransfer: XhcPeiRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
+      goto FREE_URB;
+    } else {
+      goto FREE_URB;
+    }
   }
 
   //
@@ -960,14 +971,24 @@ XhcPeiBulkTransfer (
   *TransferResult = Urb->Result;
   *DataLength     = Urb->Completed;
 
-  if (*TransferResult == EFI_USB_NOERROR) {
+  if (Status == EFI_TIMEOUT) {
-    Status = EFI_SUCCESS;
+    //
-  } else if (*TransferResult == EFI_USB_ERR_STALL) {
+    // The transfer timed out. Abort the transfer by dequeueing of the TD.
-    RecoveryStatus = XhcPeiRecoverHaltedEndpoint(Xhc, Urb);
+    //
-    if (EFI_ERROR (RecoveryStatus)) {
+    RecoveryStatus = XhcPeiDequeueTrbFromEndpoint(Xhc, Urb);
-      DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: XhcPeiRecoverHaltedEndpoint failed\n"));
+    if (EFI_ERROR(RecoveryStatus)) {
+      DEBUG((EFI_D_ERROR, "XhcPeiBulkTransfer: XhcPeiDequeueTrbFromEndpoint failed\n"));
+    }
+  } else {
+    if (*TransferResult == EFI_USB_NOERROR) {
+      Status = EFI_SUCCESS;
+    } else if (*TransferResult == EFI_USB_ERR_STALL) {
+      RecoveryStatus = XhcPeiRecoverHaltedEndpoint(Xhc, Urb);
+      if (EFI_ERROR (RecoveryStatus)) {
+        DEBUG ((EFI_D_ERROR, "XhcPeiBulkTransfer: XhcPeiRecoverHaltedEndpoint failed\n"));
+      }
+      Status = EFI_DEVICE_ERROR;
     }
-    Status = EFI_DEVICE_ERROR;
   }
 
   XhcPeiFreeUrb (Xhc, Urb);

MdeModulePkg/Bus/Pci/XhciPei/XhciSched.c

@@ -444,12 +444,8 @@ XhcPeiRecoverHaltedEndpoint (
   )
 {
   EFI_STATUS                  Status;
-  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
-  CMD_TRB_RESET_ENDPOINT      CmdTrbResetED;
-  CMD_SET_TR_DEQ_POINTER      CmdSetTRDeq;
   UINT8                       Dci;
   UINT8                       SlotId;
-  EFI_PHYSICAL_ADDRESS        PhyAddr;
 
   Status = EFI_SUCCESS;
   SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
@@ -463,17 +459,7 @@ XhcPeiRecoverHaltedEndpoint (
   //
   // 1) Send Reset endpoint command to transit from halt to stop state
   //
-  ZeroMem (&CmdTrbResetED, sizeof (CmdTrbResetED));
+  Status = XhcPeiResetEndpoint (Xhc, SlotId, Dci);
-  CmdTrbResetED.CycleBit = 1;
-  CmdTrbResetED.Type     = TRB_TYPE_RESET_ENDPOINT;
-  CmdTrbResetED.EDID     = Dci;
-  CmdTrbResetED.SlotId   = SlotId;
-  Status = XhcPeiCmdTransfer (
-             Xhc,
-             (TRB_TEMPLATE *) (UINTN) &CmdTrbResetED,
-             XHC_GENERIC_TIMEOUT,
-             (TRB_TEMPLATE **) (UINTN) &EvtTrb
-             );
   if (EFI_ERROR(Status)) {
     DEBUG ((EFI_D_ERROR, "XhcPeiRecoverHaltedEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
     goto Done;
@@ -482,20 +468,7 @@ XhcPeiRecoverHaltedEndpoint (
   //
   // 2) Set dequeue pointer
   //
-  ZeroMem (&CmdSetTRDeq, sizeof (CmdSetTRDeq));
+  Status = XhcPeiSetTrDequeuePointer (Xhc, SlotId, Dci, Urb);
-  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Urb->Ring->RingEnqueue, sizeof (CMD_SET_TR_DEQ_POINTER));
-  CmdSetTRDeq.PtrLo    = XHC_LOW_32BIT (PhyAddr) | Urb->Ring->RingPCS;
-  CmdSetTRDeq.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
-  CmdSetTRDeq.CycleBit = 1;
-  CmdSetTRDeq.Type     = TRB_TYPE_SET_TR_DEQUE;
-  CmdSetTRDeq.Endpoint = Dci;
-  CmdSetTRDeq.SlotId   = SlotId;
-  Status = XhcPeiCmdTransfer (
-             Xhc,
-             (TRB_TEMPLATE *) (UINTN) &CmdSetTRDeq,
-             XHC_GENERIC_TIMEOUT,
-             (TRB_TEMPLATE **) (UINTN) &EvtTrb
-             );
   if (EFI_ERROR(Status)) {
     DEBUG ((EFI_D_ERROR, "XhcPeiRecoverHaltedEndpoint: Set Dequeue Pointer Failed, Status = %r\n", Status));
     goto Done;
@@ -510,6 +483,65 @@ Done:
   return Status;
 }
 
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+XhcPeiDequeueTrbFromEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  UINT8                       Dci;
+  UINT8                       SlotId;
+
+  Status = EFI_SUCCESS;
+  SlotId = XhcPeiBusDevAddrToSlotId (Xhc, Urb->Ep.BusAddr);
+  if (SlotId == 0) {
+    return EFI_DEVICE_ERROR;
+  }
+  Dci = XhcPeiEndpointToDci (Urb->Ep.EpAddr, (UINT8) (Urb->Ep.Direction));
+
+  DEBUG ((EFI_D_INFO, "XhcPeiDequeueTrbFromEndpoint: Stop Slot = %x, Dci = %x\n", SlotId, Dci));
+
+  //
+  // 1) Send Stop endpoint command to stop endpoint.
+  //
+  Status = XhcPeiStopEndpoint (Xhc, SlotId, Dci);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDequeueTrbFromEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 2) Set dequeue pointer
+  //
+  Status = XhcPeiSetTrDequeuePointer (Xhc, SlotId, Dci, Urb);
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiDequeueTrbFromEndpoint: Set Dequeue Pointer Failed, Status = %r\n", Status));
+    goto Done;
+  }
+
+  //
+  // 3) Ring the doorbell to transit from stop to active
+  //
+  XhcPeiRingDoorBell (Xhc, SlotId, Dci);
+
+Done:
+  return Status;
+}
+
 /**
   Check if the Trb is a transaction of the URB.
 
@@ -2253,6 +2285,151 @@ XhcPeiConfigHubContext64 (
   return Status;
 }
 
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Stop endpoint successfully.
+  @retval Others        Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiStopEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  )
+{
+  EFI_STATUS                    Status;
+  EVT_TRB_COMMAND_COMPLETION    *EvtTrb;
+  CMD_TRB_STOP_ENDPOINT         CmdTrbStopED;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiStopEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbStopED, sizeof (CmdTrbStopED));
+  CmdTrbStopED.CycleBit = 1;
+  CmdTrbStopED.Type     = TRB_TYPE_STOP_ENDPOINT;
+  CmdTrbStopED.EDID     = Dci;
+  CmdTrbStopED.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbStopED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiStopEndpoint: Stop Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Reset endpoint successfully.
+  @retval Others        Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiResetEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_TRB_RESET_ENDPOINT      CmdTrbResetED;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiResetEndpoint: Slot = 0x%x, Dci = 0x%x\n", SlotId, Dci));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdTrbResetED, sizeof (CmdTrbResetED));
+  CmdTrbResetED.CycleBit = 1;
+  CmdTrbResetED.Type     = TRB_TYPE_RESET_ENDPOINT;
+  CmdTrbResetED.EDID     = Dci;
+  CmdTrbResetED.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdTrbResetED,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiResetEndpoint: Reset Endpoint Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+  @param  Urb           The dequeue pointer of the transfer ring specified
+                        by the urb to be updated.
+
+  @retval EFI_SUCCESS   Set transfer ring dequeue pointer succeeds.
+  @retval Others        Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiSetTrDequeuePointer (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci,
+  IN URB                *Urb
+  )
+{
+  EFI_STATUS                  Status;
+  EVT_TRB_COMMAND_COMPLETION  *EvtTrb;
+  CMD_SET_TR_DEQ_POINTER      CmdSetTRDeq;
+  EFI_PHYSICAL_ADDRESS        PhyAddr;
+
+  DEBUG ((EFI_D_INFO, "XhcPeiSetTrDequeuePointer: Slot = 0x%x, Dci = 0x%x, Urb = 0x%x\n", SlotId, Dci, Urb));
+
+  //
+  // Send stop endpoint command to transit Endpoint from running to stop state
+  //
+  ZeroMem (&CmdSetTRDeq, sizeof (CmdSetTRDeq));
+  PhyAddr = UsbHcGetPciAddrForHostAddr (Xhc->MemPool, Urb->Ring->RingEnqueue, sizeof (CMD_SET_TR_DEQ_POINTER));
+  CmdSetTRDeq.PtrLo    = XHC_LOW_32BIT (PhyAddr) | Urb->Ring->RingPCS;
+  CmdSetTRDeq.PtrHi    = XHC_HIGH_32BIT (PhyAddr);
+  CmdSetTRDeq.CycleBit = 1;
+  CmdSetTRDeq.Type     = TRB_TYPE_SET_TR_DEQUE;
+  CmdSetTRDeq.Endpoint = Dci;
+  CmdSetTRDeq.SlotId   = SlotId;
+  Status = XhcPeiCmdTransfer (
+             Xhc,
+             (TRB_TEMPLATE *) (UINTN) &CmdSetTRDeq,
+             XHC_GENERIC_TIMEOUT,
+             (TRB_TEMPLATE **) (UINTN) &EvtTrb
+             );
+  if (EFI_ERROR(Status)) {
+    DEBUG ((EFI_D_ERROR, "XhcPeiSetTrDequeuePointer: Set TR Dequeue Pointer Failed, Status = %r\n", Status));
+  }
+
+  return Status;
+}
+
 /**
   Check if there is a new generated event.
 

MdeModulePkg/Bus/Pci/XhciPei/XhciSched.h

@@ -1,7 +1,7 @@
 /** @file
 Private Header file for Usb Host Controller PEIM
 
-Copyright (c) 2014, Intel Corporation. All rights reserved.<BR>
+Copyright (c) 2014 - 2015, Intel Corporation. All rights reserved.<BR>
 
 This program and the accompanying materials
 are licensed and made available under the terms and conditions
@@ -938,6 +938,66 @@ XhcPeiSetConfigCmd64 (
   IN USB_CONFIG_DESCRIPTOR      *ConfigDesc
   );
 
+/**
+  Stop endpoint through XHCI's Stop_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Stop endpoint successfully.
+  @retval Others        Failed to stop endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiStopEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  );
+
+/**
+  Reset endpoint through XHCI's Reset_Endpoint cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+
+  @retval EFI_SUCCESS   Reset endpoint successfully.
+  @retval Others        Failed to reset endpoint.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiResetEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci
+  );
+
+/**
+  Set transfer ring dequeue pointer through XHCI's Set_Tr_Dequeue_Pointer cmd.
+
+  @param  Xhc           The XHCI device.
+  @param  SlotId        The slot id of the target device.
+  @param  Dci           The device context index of the target slot or endpoint.
+  @param  Urb           The dequeue pointer of the transfer ring specified
+                        by the urb to be updated.
+
+  @retval EFI_SUCCESS   Set transfer ring dequeue pointer succeeds.
+  @retval Others        Failed to set transfer ring dequeue pointer.
+
+**/
+EFI_STATUS
+EFIAPI
+XhcPeiSetTrDequeuePointer (
+  IN PEI_XHC_DEV        *Xhc,
+  IN UINT8              SlotId,
+  IN UINT8              Dci,
+  IN URB                *Urb
+  );
+
 /**
   Assign and initialize the device slot for a new device.
 
@@ -1066,6 +1126,25 @@ XhcPeiRecoverHaltedEndpoint (
   IN URB                *Urb
   );
 
+/**
+  System software shall use a Stop Endpoint Command (section 4.6.9) and the Set TR Dequeue Pointer
+  Command (section 4.6.10) to remove the timed-out TDs from the xHC transfer ring. The next write to
+  the Doorbell of the Endpoint will transition the Endpoint Context from the Stopped to the Running
+  state.
+
+  @param  Xhc                   The XHCI device.
+  @param  Urb                   The urb which doesn't get completed in a specified timeout range.
+
+  @retval EFI_SUCCESS           The dequeuing of the TDs is successful.
+  @retval Others                Failed to stop the endpoint and dequeue the TDs.
+
+**/
+EFI_STATUS
+XhcPeiDequeueTrbFromEndpoint (
+  IN PEI_XHC_DEV        *Xhc,
+  IN URB                *Urb
+  );
+
 /**
   Create a new URB for a new transaction.