diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index 6b7c7dac65a2..39e52eb02d6d 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -913,6 +913,232 @@ static u32 xhci_get_port_status(struct usb_hcd *hcd,
 	return status;
 }
 
+#ifdef CONFIG_AMLOGIC_USB
+static void xhci_port_set_test_mode(struct xhci_hcd *xhci,
+	u16 test_mode, u16 wIndex)
+{
+	u32 temp;
+	__le32 __iomem **port_array;
+
+	port_array = xhci->usb2_ports;
+	temp = readl(port_array[wIndex] + PORTPMSC);
+	temp &= ~(0xf << PORT_TEST_MODE_SHIFT);
+	temp |= test_mode << PORT_TEST_MODE_SHIFT;
+	writel(temp, port_array[wIndex] + PORTPMSC);
+	xhci->test_mode = test_mode;
+	if (test_mode == TEST_FORCE_EN)
+		xhci_start(xhci);
+}
+
+int xhci_disable_slot(struct xhci_hcd *xhci, struct xhci_command *command,
+			u32 slot_id)
+{
+	u32 state;
+	int ret = 0;
+	struct xhci_virt_device *virt_dev;
+
+	virt_dev = xhci->devs[slot_id];
+	if (!virt_dev)
+		return -EINVAL;
+	if (!command)
+		command = xhci_alloc_command(xhci, false, false, GFP_KERNEL);
+	if (!command)
+		return -ENOMEM;
+
+	/* Don't disable the slot if the host controller is dead. */
+	state = readl(&xhci->op_regs->status);
+	if (state == 0xffffffff || (xhci->xhc_state & XHCI_STATE_DYING) ||
+			(xhci->xhc_state & XHCI_STATE_HALTED)) {
+		xhci_free_virt_device(xhci, slot_id);
+		xhci_free_command(xhci, command);
+
+		return ret;
+	}
+
+	ret = xhci_queue_slot_control(xhci, command, TRB_DISABLE_SLOT,
+				slot_id);
+	if (ret) {
+		xhci_err(xhci, "FIXME: allocate a command ring segment\n");
+
+		return ret;
+	}
+	xhci_ring_cmd_db(xhci);
+
+	return ret;
+}
+
+/*
+ * xhci_set_port_power() must be called with xhci->lock held.
+ * It will release and re-aquire the lock while calling ACPI
+ * method.
+ */
+static void xhci_set_port_power(struct xhci_hcd *xhci,
+	 struct usb_hcd *hcd, u16 index, bool on)
+{
+	__le32 __iomem **port_array;
+	u32 temp;
+	unsigned long flags = 0;
+
+	port_array = xhci->usb2_ports;
+	temp = readl(port_array[index]);
+	if (on) {
+		/* Power on */
+		writel(temp | PORT_POWER, port_array[index]);
+		temp = readl(port_array[index]);
+		xhci_err(xhci, "set port power, actual port %d status  = 0x%x\n",
+						index, temp);
+	} else {
+		/* Power off */
+		writel(temp & ~PORT_POWER, port_array[index]);
+	}
+
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	temp = usb_acpi_power_manageable(hcd->self.root_hub,
+					index);
+	if (temp)
+		usb_acpi_set_power_state(hcd->self.root_hub,
+			index, on);
+	spin_lock_irqsave(&xhci->lock, flags);
+}
+
+
+static int xhci_enter_test_mode(struct xhci_hcd *xhci,
+				u16 test_mode, u16 wIndex)
+{
+	int i, retval;
+
+	/* Disable all Device Slots */
+	xhci_err(xhci, "Disable all slots\n");
+	for (i = 1; i <= HCS_MAX_SLOTS(xhci->hcs_params1); i++) {
+		retval = xhci_disable_slot(xhci, NULL, i);
+		if (retval)
+			xhci_err(xhci, "Failed to disable slot %d, %d.\n",
+				 i, retval);
+	}
+	/* Put all ports to the Disable state by clear PP */
+	xhci_err(xhci, "Disable all port (PP = 0)\n");
+	/* Power off USB3 ports*/
+	for (i = 0; i < xhci->num_usb3_ports; i++)
+		xhci_set_port_power(xhci, xhci->shared_hcd, i, false);
+	/* Power off USB2 ports*/
+	for (i = 0; i < xhci->num_usb2_ports; i++)
+		xhci_set_port_power(xhci, xhci->main_hcd, i, false);
+	/* Stop the controller */
+	xhci_err(xhci, "Stop controller\n");
+	retval = xhci_halt(xhci);
+	if (retval)
+		return retval;
+	/* Disable runtime PM for test mode */
+	pm_runtime_forbid(xhci_to_hcd(xhci)->self.controller);
+	/* Set PORTPMSC.PTC field to enter selected test mode */
+	/* Port is selected by wIndex. port_id = wIndex + 1 */
+	xhci_err(xhci, "Enter Test Mode: %d, Port_id=%d\n",
+					test_mode, wIndex + 1);
+	xhci_port_set_test_mode(xhci, test_mode, wIndex);
+	return retval;
+}
+
+static int xhci_exit_test_mode(struct xhci_hcd *xhci)
+{
+	int retval;
+
+	if (!xhci->test_mode) {
+		xhci_err(xhci, "Not in test mode, do nothing.\n");
+		return 0;
+	}
+	if (xhci->test_mode == TEST_FORCE_EN &&
+		!(xhci->xhc_state & XHCI_STATE_HALTED)) {
+		retval = xhci_halt(xhci);
+		if (retval)
+			return retval;
+	}
+	pm_runtime_allow(xhci_to_hcd(xhci)->self.controller);
+	xhci->test_mode = 0;
+	return xhci_reset(xhci);
+}
+
+static int xhci_test_suspend_resume(struct usb_hcd *hcd,
+				u16 wIndex)
+{
+	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
+	unsigned long flags = 0;
+	u32 temp;
+	int slot_id;
+	__le32 __iomem **port_array = xhci->usb2_ports;
+
+	/* 15 second delay per the test spec */
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	xhci_err(xhci, "into suspend\n");
+	spin_lock_irqsave(&xhci->lock, flags);
+
+	/*suspend*/
+	temp = readl(port_array[wIndex]);
+	if ((temp & PORT_PLS_MASK) != XDEV_U0) {
+		/* Resume the port to U0 first */
+		xhci_set_link_state(xhci, port_array, wIndex,
+			XDEV_U0);
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		usleep_range(10000-1, 10000);
+		spin_lock_irqsave(&xhci->lock, flags);
+	}
+	/* In spec software should not attempt to suspend
+	 * a port unless the port reports that it is in the
+	 * enabled (PED = ‘1’,PLS < ‘3’) state.
+	 */
+	temp = readl(port_array[wIndex]);
+	if ((temp & PORT_PE) == 0 || (temp & PORT_RESET)
+		|| (temp & PORT_PLS_MASK) >= XDEV_U3) {
+		xhci_warn(xhci, "USB core suspending device not in U0/U1/U2.\n");
+		return -1;
+	}
+
+	slot_id = xhci_find_slot_id_by_port(hcd, xhci,
+			wIndex + 1);
+	if (!slot_id) {
+		xhci_warn(xhci, "slot_id is zero\n");
+		return -1;
+	}
+	/* unlock to execute stop endpoint commands */
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	xhci_stop_device(xhci, slot_id, 1);
+	spin_lock_irqsave(&xhci->lock, flags);
+
+	xhci_set_link_state(xhci, port_array, wIndex, XDEV_U3);
+
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	usleep_range(10000-1, 10000); /* wait device to enter */
+	spin_lock_irqsave(&xhci->lock, flags);
+
+	/* 15 second delay per the test spec */
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	xhci_err(xhci, "wait 15s\n");
+	msleep(15000);
+	xhci_err(xhci, "into resume\n");
+	spin_lock_irqsave(&xhci->lock, flags);
+
+	temp = readl(port_array[wIndex]);
+	xhci_dbg(xhci, "clear USB_PORT_FEAT_SUSPEND\n");
+	xhci_dbg(xhci, "PORTSC %04x\n", temp);
+	if (temp & PORT_RESET)
+		return -1;
+	if ((temp & PORT_PLS_MASK) == XDEV_U3) {
+		if ((temp & PORT_PE) == 0)
+			return -1;
+
+		xhci_set_link_state(xhci, port_array, wIndex,
+			XDEV_RESUME);
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		msleep(20);
+		spin_lock_irqsave(&xhci->lock, flags);
+		xhci_set_link_state(xhci, port_array, wIndex,
+			XDEV_U0);
+	}
+
+	xhci_ring_device(xhci, slot_id);
+	return 0;
+}
+#endif
+
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 		u16 wIndex, char *buf, u16 wLength)
 {
@@ -927,11 +1153,15 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 	u16 link_state = 0;
 	u16 wake_mask = 0;
 	u16 timeout = 0;
+#ifdef CONFIG_AMLOGIC_USB
+	u16 test_mode = 0;
+#endif
 
 	max_ports = xhci_get_ports(hcd, &port_array);
 	bus_state = &xhci->bus_state[hcd_index(hcd)];
 
 	spin_lock_irqsave(&xhci->lock, flags);
+
 	switch (typeReq) {
 	case GetHubStatus:
 		/* No power source, over-current reported per port */
@@ -1000,6 +1230,10 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 			link_state = (wIndex & 0xff00) >> 3;
 		if (wValue == USB_PORT_FEAT_REMOTE_WAKE_MASK)
 			wake_mask = wIndex & 0xff00;
+#ifdef CONFIG_AMLOGIC_USB
+		if (wValue == USB_PORT_FEAT_TEST)
+			test_mode = (wIndex & 0xff00) >> 8;
+#endif
 		/* The MSB of wIndex is the U1/U2 timeout */
 		timeout = (wIndex & 0xff00) >> 8;
 		wIndex &= 0xff;
@@ -1175,6 +1409,23 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 			temp |= PORT_U2_TIMEOUT(timeout);
 			writel(temp, port_array[wIndex] + PORTPMSC);
 			break;
+#ifdef CONFIG_AMLOGIC_USB
+		case USB_PORT_FEAT_TEST:
+			/* 4.19.6 Port Test Modes (USB2 Test Mode) */
+			if (hcd->speed != HCD_USB2)
+				goto error;
+			if (test_mode > 6 || test_mode < 1)
+				goto error;
+
+			if ((test_mode >= 1) && (test_mode <= 5))
+				retval = xhci_enter_test_mode(xhci,
+				test_mode, wIndex);
+			else if (test_mode == 6)
+				retval = xhci_test_suspend_resume(hcd, wIndex);
+			else
+				retval = 0;
+			break;
+#endif
 		default:
 			goto error;
 		}
@@ -1250,6 +1501,11 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 						wIndex, false);
 			spin_lock_irqsave(&xhci->lock, flags);
 			break;
+#ifdef CONFIG_AMLOGIC_USB
+		case USB_PORT_FEAT_TEST:
+			retval = xhci_exit_test_mode(xhci);
+			break;
+#endif
 		default:
 			goto error;
 		}
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 89a14d5f6ad8..6f67e3c6f577 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -3365,6 +3365,163 @@ int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 	return 0;
 }
 
+#ifdef CONFIG_AMLOGIC_USB
+/* Caller must have locked xhci->lock */
+int xhci_test_single_step(struct xhci_hcd *xhci, gfp_t mem_flags,
+		struct urb *urb, int slot_id,
+		unsigned int ep_index, int testflag)
+{
+	struct xhci_ring *ep_ring;
+	int num_trbs;
+	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;
+	unsigned long flags = 0;
+
+	ep_ring = xhci_urb_to_transfer_ring(xhci, urb);
+	if (!ep_ring)
+		return -EINVAL;
+
+	/*
+	 * Need to copy setup packet into setup TRB, so we can't use the setup
+	 * DMA address.
+	 */
+	if (!urb->setup_packet)
+		return -EINVAL;
+
+	/* 1 TRB for setup, 1 for status */
+	num_trbs = 2;
+	/*
+	 * Don't need to check if we need additional event data and normal TRBs,
+	 * since data in control transfers will never get bigger than 16MB
+	 * XXX: can we get a buffer that crosses 64KB boundaries?
+	 */
+	if (urb->transfer_buffer_length > 0)
+		num_trbs++;
+	ret = prepare_transfer(xhci, xhci->devs[slot_id],
+			ep_index, urb->stream_id,
+			num_trbs, urb, 0, mem_flags);
+	if (ret < 0)
+		return ret;
+
+	urb_priv = urb->hcpriv;
+	td = urb_priv->td[0];
+
+	/*
+	 * Don't give the first TRB to the hardware (by toggling the cycle bit)
+	 * until we've finished creating all the other TRBs.  The ring's cycle
+	 * state may change as we enqueue the other TRBs, so save it too.
+	 */
+	start_trb = &ep_ring->enqueue->generic;
+	start_cycle = ep_ring->cycle_state;
+
+	/* Queue setup TRB - see section 6.4.1.2.1 */
+	/* FIXME better way to translate setup_packet into two u32 fields? */
+	setup = (struct usb_ctrlrequest *) urb->setup_packet;
+	field = 0;
+	field |= TRB_IDT | TRB_TYPE(TRB_SETUP);
+	if (start_cycle == 0)
+		field |= 0x1;
+
+	/* xHCI 1.0/1.1 6.4.1.2.1: Transfer Type field */
+	if ((xhci->hci_version >= 0x100) || (xhci->quirks & XHCI_MTK_HOST)) {
+		if (urb->transfer_buffer_length > 0) {
+			if (setup->bRequestType & USB_DIR_IN)
+				field |= TRB_TX_TYPE(TRB_DATA_IN);
+			else
+				field |= TRB_TX_TYPE(TRB_DATA_OUT);
+		}
+	}
+
+	queue_trb(xhci, ep_ring, true,
+		  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, 0,
+			start_cycle, start_trb);
+
+	/* 15 second delay per the test spec */
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	xhci_err(xhci, "step 1\n");
+	msleep(15000);
+	spin_lock_irqsave(&xhci->lock, flags);
+
+	start_trb = &ep_ring->enqueue->generic;
+	start_cycle = ep_ring->cycle_state;
+
+	/* If there's data, queue data TRBs */
+	/* Only set interrupt on short packet for IN endpoints */
+	if (usb_urb_dir_in(urb))
+		field = TRB_ISP | TRB_TYPE(TRB_DATA);
+	else
+		field = 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) {
+		if (setup->bRequestType & USB_DIR_IN)
+			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);
+		giveback_first_trb(xhci, slot_id, ep_index, 0,
+			start_cycle, start_trb);
+	}
+
+	/* 15 second delay per the test spec */
+	spin_unlock_irqrestore(&xhci->lock, flags);
+	xhci_err(xhci, "step 2\n");
+	msleep(15000);
+	spin_lock_irqsave(&xhci->lock, flags);
+
+	/* Save the DMA address of the last TRB in the TD */
+	td->last_trb = ep_ring->enqueue;
+
+	/* Queue status TRB - see Table 7 and sections 4.11.2.2 and 6.4.1.2.3 */
+	/* If the device sent data, the status stage is an OUT transfer */
+	if (urb->transfer_buffer_length > 0 && setup->bRequestType & USB_DIR_IN)
+		field = 0;
+	else
+		field = TRB_DIR_IN;
+	queue_trb(xhci, ep_ring, false,
+			0,
+			0,
+			TRB_INTR_TARGET(0),
+			/* Event on completion */
+			field | TRB_IOC |
+			TRB_TYPE(TRB_STATUS) | ep_ring->cycle_state);
+
+	giveback_first_trb(xhci, slot_id, ep_index, 0,
+			start_cycle, start_trb);
+
+		/* 15 second delay per the test spec */
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		xhci_err(xhci, "step 3\n");
+		msleep(15000);
+		spin_lock_irqsave(&xhci->lock, flags);
+
+	return 0;
+
+}
+#endif
+
 /* Caller must have locked xhci->lock */
 int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		struct urb *urb, int slot_id, unsigned int ep_index)
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index a7d239f5fc5f..7d33e3103e83 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -125,7 +125,7 @@ int xhci_halt(struct xhci_hcd *xhci)
 /*
  * Set the run bit and wait for the host to be running.
  */
-static int xhci_start(struct xhci_hcd *xhci)
+int xhci_start(struct xhci_hcd *xhci)
 {
 	u32 temp;
 	int ret;
@@ -1339,7 +1339,9 @@ int xhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
 	unsigned int slot_id, ep_index;
 	struct urb_priv	*urb_priv;
 	int size, i;
-
+#ifdef CONFIG_AMLOGIC_USB
+	struct usb_ctrlrequest *setup;
+#endif
 	if (!urb || xhci_check_args(hcd, urb->dev, urb->ep,
 					true, true, __func__) <= 0)
 		return -EINVAL;
@@ -1404,6 +1406,25 @@ int xhci_urb_enqueue(struct usb_hcd *hcd, struct urb *urb, gfp_t mem_flags)
 		spin_lock_irqsave(&xhci->lock, flags);
 		if (xhci->xhc_state & XHCI_STATE_DYING)
 			goto dying;
+
+#ifdef CONFIG_AMLOGIC_USB
+		setup = (struct usb_ctrlrequest *) urb->setup_packet;
+		if ((setup->bRequestType == 0x80) && (setup->bRequest == 0x06)
+			&& (setup->wValue == 0x0100)
+			&& (setup->wIndex != 0x0)) {
+			if ((((setup->wIndex)>>8) & 0xff) == 7) {
+				setup->wIndex = 0;
+				ret = xhci_test_single_step(xhci,
+					GFP_ATOMIC, urb,
+					slot_id, ep_index, 1);
+			} else if ((((setup->wIndex)>>8)&0xff) == 8) {
+				setup->wIndex = 0;
+				ret = xhci_test_single_step(xhci,
+					GFP_ATOMIC, urb,
+					slot_id, ep_index, 2);
+			}
+		} else
+#endif
 		ret = xhci_queue_ctrl_tx(xhci, GFP_ATOMIC, urb,
 				slot_id, ep_index);
 		if (ret)
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 98e1616c312d..fd33bfc7164c 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -429,6 +429,9 @@ struct xhci_op_regs {
 /* USB3 Protocol PORTLI  Port Link Information */
 #define PORT_RX_LANES(p)	(((p) >> 16) & 0xf)
 #define PORT_TX_LANES(p)	(((p) >> 20) & 0xf)
+#ifdef CONFIG_AMLOGIC_USB
+#define PORT_TEST_MODE_SHIFT	28
+#endif
 
 /* USB2 Protocol PORTHLPMC */
 #define PORT_HIRDM(p)((p) & 3)
@@ -1689,6 +1692,9 @@ struct xhci_hcd {
 	/* Compliance Mode Recovery Data */
 	struct timer_list	comp_mode_recovery_timer;
 	u32			port_status_u0;
+#ifdef CONFIG_AMLOGIC_USB
+	u16			test_mode;
+#endif
 /* Compliance Mode Timer Triggered every 2 seconds */
 #define COMP_MODE_RCVRY_MSECS 2000
 
@@ -1988,6 +1994,7 @@ struct xhci_input_control_ctx *xhci_get_input_control_ctx(struct xhci_container_
 struct xhci_slot_ctx *xhci_get_slot_ctx(struct xhci_hcd *xhci, struct xhci_container_ctx *ctx);
 struct xhci_ep_ctx *xhci_get_ep_ctx(struct xhci_hcd *xhci, struct xhci_container_ctx *ctx, unsigned int ep_index);
 
+
 struct xhci_ring *xhci_triad_to_transfer_ring(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		unsigned int stream_id);
@@ -1999,4 +2006,12 @@ static inline struct xhci_ring *xhci_urb_to_transfer_ring(struct xhci_hcd *xhci,
 					urb->stream_id);
 }
 
+extern struct timer_list	xhci_reset_timer;
+#ifdef CONFIG_AMLOGIC_USB
+int xhci_start(struct xhci_hcd *xhci);
+int xhci_test_single_step(struct xhci_hcd *xhci, gfp_t mem_flags,
+		struct urb *urb, int slot_id,
+		unsigned int ep_index, int testflag);
+#endif
+
 #endif /* __LINUX_XHCI_HCD_H */