@@ -1511,6 +1511,16 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
/* 4.19.6 Port Test Modes (USB2 Test Mode) */
if (hcd->speed != HCD_USB2)
goto error;
+
+#ifdef CONFIG_USB_HCD_TEST_MODE
+ if (test_mode == EHSET_TEST_SINGLE_STEP_SET_FEATURE) {
+ spin_unlock_irqrestore(&xhci->lock, flags);
+ retval = ehset_single_step_set_feature(hcd,
+ wIndex + 1);
+ spin_lock_irqsave(&xhci->lock, flags);
+ break;
+ }
+#endif
if (test_mode > USB_TEST_FORCE_ENABLE ||
test_mode < USB_TEST_J)
goto error;
@@ -3797,6 +3797,129 @@ int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
return 0;
}
+#ifdef CONFIG_USB_HCD_TEST_MODE
+/*
+ * This function prepare TRBs and submits them for the
+ * SINGLE_STEP_SET_FEATURE Test.
+ * This is done in two parts: first SETUP req for GetDesc is sent then
+ * 15 seconds later, the IN stage for GetDesc starts to req data from dev
+ *
+ * is_setup : argument decides which of the two stage needs to be
+ * performed; TRUE - SETUP and FALSE - IN+STATUS
+ * Returns 0 if success
+ */
+int xhci_submit_single_step_set_feature(struct usb_hcd *hcd,
+ struct urb *urb, int is_setup)
+{
+ int slot_id;
+ unsigned int ep_index;
+ struct xhci_ring *ep_ring;
+ int ret;
+ struct usb_ctrlrequest *setup;
+ struct xhci_generic_trb *start_trb;
+ int start_cycle;
+ u32 field, length_field, remainder;
+ struct urb_priv *urb_priv;
+ struct xhci_td *td;
+ struct xhci_hcd *xhci = hcd_to_xhci(hcd);
+
+ /* urb_priv will be free after transcation has completed */
+ urb_priv = kzalloc(sizeof(struct urb_priv) +
+ sizeof(struct xhci_td), GFP_KERNEL);
+ if (!urb_priv)
+ return -ENOMEM;
+
+ td = &urb_priv->td[0];
+ urb_priv->num_tds = 1;
+ urb_priv->num_tds_done = 0;
+ urb->hcpriv = urb_priv;
+
+ ep_ring = xhci_urb_to_transfer_ring(xhci, urb);
+ if (!ep_ring) {
+ ret = -EINVAL;
+ goto free_priv;
+ }
+
+ slot_id = urb->dev->slot_id;
+ ep_index = xhci_get_endpoint_index(&urb->ep->desc);
+
+ setup = (struct usb_ctrlrequest *) urb->setup_packet;
+ if (is_setup) {
+ ret = prepare_transfer(xhci, xhci->devs[slot_id],
+ ep_index, urb->stream_id,
+ 1, urb, 0, GFP_KERNEL);
+ if (ret < 0)
+ goto free_priv;
+
+ start_trb = &ep_ring->enqueue->generic;
+ start_cycle = ep_ring->cycle_state;
+ /* Save the DMA address of the last TRB in the TD */
+ td->last_trb = ep_ring->enqueue;
+ field = TRB_IOC | TRB_IDT | TRB_TYPE(TRB_SETUP) | start_cycle;
+ /* xHCI 1.0/1.1 6.4.1.2.1: Transfer Type field */
+ if ((xhci->hci_version >= 0x100) ||
+ (xhci->quirks & XHCI_MTK_HOST))
+ field |= TRB_TX_TYPE(TRB_DATA_IN);
+
+ queue_trb(xhci, ep_ring, false,
+ setup->bRequestType | setup->bRequest << 8 |
+ le16_to_cpu(setup->wValue) << 16,
+ le16_to_cpu(setup->wIndex) |
+ le16_to_cpu(setup->wLength) << 16,
+ TRB_LEN(8) | TRB_INTR_TARGET(0),
+ /* Immediate data in pointer */
+ field);
+ giveback_first_trb(xhci, slot_id, ep_index, urb->stream_id,
+ start_cycle, start_trb);
+ return 0;
+ }
+
+ ret = prepare_transfer(xhci, xhci->devs[slot_id],
+ ep_index, urb->stream_id,
+ 2, urb, 0, GFP_KERNEL);
+ if (ret < 0)
+ goto free_priv;
+
+ start_trb = &ep_ring->enqueue->generic;
+ start_cycle = ep_ring->cycle_state;
+ field = TRB_ISP | TRB_TYPE(TRB_DATA);
+
+ remainder = xhci_td_remainder(xhci, 0,
+ urb->transfer_buffer_length,
+ urb->transfer_buffer_length,
+ urb, 1);
+
+ length_field = TRB_LEN(urb->transfer_buffer_length) |
+ TRB_TD_SIZE(remainder) |
+ TRB_INTR_TARGET(0);
+
+ if (urb->transfer_buffer_length > 0) {
+ field |= TRB_DIR_IN;
+ queue_trb(xhci, ep_ring, true,
+ lower_32_bits(urb->transfer_dma),
+ upper_32_bits(urb->transfer_dma),
+ length_field,
+ field | ep_ring->cycle_state);
+ }
+
+ td->last_trb = ep_ring->enqueue;
+ field = TRB_IOC | TRB_TYPE(TRB_STATUS) | ep_ring->cycle_state;
+ queue_trb(xhci, ep_ring, false,
+ 0,
+ 0,
+ TRB_INTR_TARGET(0),
+ field);
+
+ giveback_first_trb(xhci, slot_id, ep_index, 0,
+ start_cycle, start_trb);
+
+ return 0;
+free_priv:
+ xhci_urb_free_priv(urb_priv);
+ return ret;
+}
+#endif /* CONFIG_USB_HCD_TEST_MODE */
+
/*
* The transfer burst count field of the isochronous TRB defines the number of
* bursts that are required to move all packets in this TD. Only SuperSpeed
@@ -5414,6 +5414,7 @@ static const struct hc_driver xhci_hc_driver = {
.disable_usb3_lpm_timeout = xhci_disable_usb3_lpm_timeout,
.find_raw_port_number = xhci_find_raw_port_number,
.clear_tt_buffer_complete = xhci_clear_tt_buffer_complete,
+ .submit_single_step_set_feature = xhci_submit_single_step_set_feature,
};
void xhci_init_driver(struct hc_driver *drv,
@@ -2165,6 +2165,16 @@ int xhci_find_raw_port_number(struct usb_hcd *hcd, int port1);
struct xhci_hub *xhci_get_rhub(struct usb_hcd *hcd);
void xhci_hc_died(struct xhci_hcd *xhci);
+#ifdef CONFIG_USB_HCD_TEST_MODE
+int xhci_submit_single_step_set_feature(struct usb_hcd *hcd,
+ struct urb *urb, int is_setup);
+#else
+static inline int xhci_submit_single_step_set_feature(struct usb_hcd *hcd,
+ struct urb *urb, int is_setup)
+{
+ return 0;
+}
+#endif
#ifdef CONFIG_PM
int xhci_bus_suspend(struct usb_hcd *hcd);