5 files changed, 248 insertions, 12 deletions

diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index a1a7a979553..14b48b261e0 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -129,6 +129,99 @@ static u32 xhci_port_state_to_neutral(u32 state)
         return (state & XHCI_PORT_RO) | (state & XHCI_PORT_RWS);
 }
 
+/*
+ * find slot id based on port number.
+ */
+static int xhci_find_slot_id_by_port(struct xhci_hcd *xhci, u16 port)
+{
+        int slot_id;
+        int i;
+
+        slot_id = 0;
+        for (i = 0; i < MAX_HC_SLOTS; i++) {
+                if (!xhci->devs[i])
+                        continue;
+                if (xhci->devs[i]->port == port) {
+                        slot_id = i;
+                        break;
+                }
+        }
+
+        return slot_id;
+}
+
+/*
+ * Stop device
+ * It issues stop endpoint command for EP 0 to 30. And wait the last command
+ * to complete.
+ * suspend will set to 1, if suspend bit need to set in command.
+ */
+static int xhci_stop_device(struct xhci_hcd *xhci, int slot_id, int suspend)
+{
+        struct xhci_virt_device *virt_dev;
+        struct xhci_command *cmd;
+        unsigned long flags;
+        int timeleft;
+        int ret;
+        int i;
+
+        ret = 0;
+        virt_dev = xhci->devs[slot_id];
+        cmd = xhci_alloc_command(xhci, false, true, GFP_NOIO);
+        if (!cmd) {
+                xhci_dbg(xhci, "Couldn't allocate command structure.\n");
+                return -ENOMEM;
+        }
+
+        spin_lock_irqsave(&xhci->lock, flags);
+        for (i = LAST_EP_INDEX; i > 0; i--) {
+                if (virt_dev->eps[i].ring && virt_dev->eps[i].ring->dequeue)
+                        xhci_queue_stop_endpoint(xhci, slot_id, i, suspend);
+        }
+        cmd->command_trb = xhci->cmd_ring->enqueue;
+        list_add_tail(&cmd->cmd_list, &virt_dev->cmd_list);
+        xhci_queue_stop_endpoint(xhci, slot_id, 0, suspend);
+        xhci_ring_cmd_db(xhci);
+        spin_unlock_irqrestore(&xhci->lock, flags);
+
+        /* Wait for last stop endpoint command to finish */
+        timeleft = wait_for_completion_interruptible_timeout(
+                        cmd->completion,
+                        USB_CTRL_SET_TIMEOUT);
+        if (timeleft <= 0) {
+                xhci_warn(xhci, "%s while waiting for stop endpoint command\n",
+                                timeleft == 0 ? "Timeout" : "Signal");
+                spin_lock_irqsave(&xhci->lock, flags);
+                /* The timeout might have raced with the event ring handler, so
+                 * only delete from the list if the item isn't poisoned.
+                 */
+                if (cmd->cmd_list.next != LIST_POISON1)
+                        list_del(&cmd->cmd_list);
+                spin_unlock_irqrestore(&xhci->lock, flags);
+                ret = -ETIME;
+                goto command_cleanup;
+        }
+
+command_cleanup:
+        xhci_free_command(xhci, cmd);
+        return ret;
+}
+
+/*
+ * Ring device, it rings the all doorbells unconditionally.
+ */
+static void xhci_ring_device(struct xhci_hcd *xhci, int slot_id)
+{
+        int i;
+
+        for (i = 0; i < LAST_EP_INDEX + 1; i++)
+                if (xhci->devs[slot_id]->eps[i].ring &&
+                    xhci->devs[slot_id]->eps[i].ring->dequeue)
+                        xhci_ring_ep_doorbell(xhci, slot_id, i, 0);
+
+        return;
+}
+
 static void xhci_disable_port(struct xhci_hcd *xhci, u16 wIndex,
                 u32 __iomem *addr, u32 port_status)
 {
@@ -162,6 +255,10 @@ static void xhci_clear_port_change_bit(struct xhci_hcd *xhci, u16 wValue,
                 status = PORT_PEC;
                 port_change_bit = "enable/disable";
                 break;
+        case USB_PORT_FEAT_C_SUSPEND:
+                status = PORT_PLC;
+                port_change_bit = "suspend/resume";
+                break;
         default:
                 /* Should never happen */
                 return;
@@ -182,6 +279,7 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
         u32 temp, status;
         int retval = 0;
         u32 __iomem *addr;
+        int slot_id;
 
         ports = HCS_MAX_PORTS(xhci->hcs_params1);
 
@@ -211,9 +309,21 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
                 if ((temp & PORT_OCC))
                         status |= USB_PORT_STAT_C_OVERCURRENT << 16;
                 /*
-                 * FIXME ignoring suspend, reset, and USB 2.1/3.0 specific
+                 * FIXME ignoring reset and USB 2.1/3.0 specific
                  * changes
                  */
+                if ((temp & PORT_PLS_MASK) == XDEV_U3
+                        && (temp & PORT_POWER))
+                        status |= 1 << USB_PORT_FEAT_SUSPEND;
+                if ((temp & PORT_PLS_MASK) == XDEV_U0
+                        && (temp & PORT_POWER)
+                        && (xhci->suspended_ports[wIndex >> 5] &
+                            (1 << (wIndex & 31)))) {
+                        xhci->suspended_ports[wIndex >> 5] &=
+                                        ~(1 << (wIndex & 31));
+                        xhci->port_c_suspend[wIndex >> 5] |=
+                                        1 << (wIndex & 31);
+                }
                 if (temp & PORT_CONNECT) {
                         status |= USB_PORT_STAT_CONNECTION;
                         status |= xhci_port_speed(temp);
@@ -226,6 +336,8 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
                         status |= USB_PORT_STAT_RESET;
                 if (temp & PORT_POWER)
                         status |= USB_PORT_STAT_POWER;
+                if (xhci->port_c_suspend[wIndex >> 5] & (1 << (wIndex & 31)))
+                        status |= 1 << USB_PORT_FEAT_C_SUSPEND;
                 xhci_dbg(xhci, "Get port status returned 0x%x\n", status);
                 put_unaligned(cpu_to_le32(status), (__le32 *) buf);
                 break;
@@ -238,6 +350,42 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
                 temp = xhci_readl(xhci, addr);
                 temp = xhci_port_state_to_neutral(temp);
                 switch (wValue) {
+                case USB_PORT_FEAT_SUSPEND:
+                        temp = xhci_readl(xhci, addr);
+                        /* 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.
+                         */
+                        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");
+                                goto error;
+                        }
+
+                        slot_id = xhci_find_slot_id_by_port(xhci, wIndex + 1);
+                        if (!slot_id) {
+                                xhci_warn(xhci, "slot_id is zero\n");
+                                goto error;
+                        }
+                        /* 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);
+
+                        temp = xhci_port_state_to_neutral(temp);
+                        temp &= ~PORT_PLS_MASK;
+                        temp |= PORT_LINK_STROBE | XDEV_U3;
+                        xhci_writel(xhci, temp, addr);
+
+                        spin_unlock_irqrestore(&xhci->lock, flags);
+                        msleep(10); /* wait device to enter */
+                        spin_lock_irqsave(&xhci->lock, flags);
+
+                        temp = xhci_readl(xhci, addr);
+                        xhci->suspended_ports[wIndex >> 5] |=
+                                        1 << (wIndex & (31));
+                        break;
                 case USB_PORT_FEAT_POWER:
                         /*
                          * Turn on ports, even if there isn't per-port switching.
@@ -271,6 +419,52 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
                 temp = xhci_readl(xhci, addr);
                 temp = xhci_port_state_to_neutral(temp);
                 switch (wValue) {
+                case USB_PORT_FEAT_SUSPEND:
+                        temp = xhci_readl(xhci, addr);
+                        xhci_dbg(xhci, "clear USB_PORT_FEAT_SUSPEND\n");
+                        xhci_dbg(xhci, "PORTSC %04x\n", temp);
+                        if (temp & PORT_RESET)
+                                goto error;
+                        if (temp & XDEV_U3) {
+                                if ((temp & PORT_PE) == 0)
+                                        goto error;
+                                if (DEV_SUPERSPEED(temp)) {
+                                        temp = xhci_port_state_to_neutral(temp);
+                                        temp &= ~PORT_PLS_MASK;
+                                        temp |= PORT_LINK_STROBE | XDEV_U0;
+                                        xhci_writel(xhci, temp, addr);
+                                        xhci_readl(xhci, addr);
+                                } else {
+                                        temp = xhci_port_state_to_neutral(temp);
+                                        temp &= ~PORT_PLS_MASK;
+                                        temp |= PORT_LINK_STROBE | XDEV_RESUME;
+                                        xhci_writel(xhci, temp, addr);
+
+                                        spin_unlock_irqrestore(&xhci->lock,
+                                                               flags);
+                                        msleep(20);
+                                        spin_lock_irqsave(&xhci->lock, flags);
+
+                                        temp = xhci_readl(xhci, addr);
+                                        temp = xhci_port_state_to_neutral(temp);
+                                        temp &= ~PORT_PLS_MASK;
+                                        temp |= PORT_LINK_STROBE | XDEV_U0;
+                                        xhci_writel(xhci, temp, addr);
+                                }
+                                xhci->port_c_suspend[wIndex >> 5] |=
+                                                1 << (wIndex & 31);
+                        }
+
+                        slot_id = xhci_find_slot_id_by_port(xhci, wIndex + 1);
+                        if (!slot_id) {
+                                xhci_dbg(xhci, "slot_id is zero\n");
+                                goto error;
+                        }
+                        xhci_ring_device(xhci, slot_id);
+                        break;
+                case USB_PORT_FEAT_C_SUSPEND:
+                        xhci->port_c_suspend[wIndex >> 5] &=
+                                        ~(1 << (wIndex & 31));
                 case USB_PORT_FEAT_C_RESET:
                 case USB_PORT_FEAT_C_CONNECTION:
                 case USB_PORT_FEAT_C_OVER_CURRENT:
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index be901808e47..858a82867e1 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -867,6 +867,7 @@ int xhci_setup_addressable_virt_dev(struct xhci_hcd *xhci, struct usb_device *ud
                         top_dev = top_dev->parent)
                 /* Found device below root hub */;
         slot_ctx->dev_info2 |= (u32) ROOT_HUB_PORT(top_dev->portnum);
+        dev->port = top_dev->portnum;
         xhci_dbg(xhci, "Set root hub portnum to %d\n", top_dev->portnum);
 
         /* Is this a LS/FS device under a HS hub? */
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 48e60d166ff..b18e00ecb46 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -68,6 +68,10 @@
 #include <linux/slab.h>
 #include "xhci.h"
 
+static int handle_cmd_in_cmd_wait_list(struct xhci_hcd *xhci,
+                struct xhci_virt_device *virt_dev,
+                struct xhci_event_cmd *event);
+
 /*
  * Returns zero if the TRB isn't in this segment, otherwise it returns the DMA
  * address of the TRB.
@@ -313,7 +317,7 @@ void xhci_ring_cmd_db(struct xhci_hcd *xhci)
         xhci_readl(xhci, &xhci->dba->doorbell[0]);
 }
 
-static void ring_ep_doorbell(struct xhci_hcd *xhci,
+void xhci_ring_ep_doorbell(struct xhci_hcd *xhci,
                 unsigned int slot_id,
                 unsigned int ep_index,
                 unsigned int stream_id)
@@ -353,7 +357,7 @@ static void ring_doorbell_for_active_rings(struct xhci_hcd *xhci,
         /* A ring has pending URBs if its TD list is not empty */
         if (!(ep->ep_state & EP_HAS_STREAMS)) {
                 if (!(list_empty(&ep->ring->td_list)))
-                        ring_ep_doorbell(xhci, slot_id, ep_index, 0);
+                        xhci_ring_ep_doorbell(xhci, slot_id, ep_index, 0);
                 return;
         }
 
@@ -361,7 +365,8 @@ static void ring_doorbell_for_active_rings(struct xhci_hcd *xhci,
                         stream_id++) {
                 struct xhci_stream_info *stream_info = ep->stream_info;
                 if (!list_empty(&stream_info->stream_rings[stream_id]->td_list))
-                        ring_ep_doorbell(xhci, slot_id, ep_index, stream_id);
+                        xhci_ring_ep_doorbell(xhci, slot_id, ep_index,
+                                                stream_id);
         }
 }
 
@@ -626,10 +631,11 @@ static void xhci_giveback_urb_in_irq(struct xhci_hcd *xhci,
  *     bit cleared) so that the HW will skip over them.
  */
 static void handle_stopped_endpoint(struct xhci_hcd *xhci,
-                union xhci_trb *trb)
+                union xhci_trb *trb, struct xhci_event_cmd *event)
 {
         unsigned int slot_id;
         unsigned int ep_index;
+        struct xhci_virt_device *virt_dev;
         struct xhci_ring *ep_ring;
         struct xhci_virt_ep *ep;
         struct list_head *entry;
@@ -638,6 +644,21 @@ static void handle_stopped_endpoint(struct xhci_hcd *xhci,
 
         struct xhci_dequeue_state deq_state;
 
+        if (unlikely(TRB_TO_SUSPEND_PORT(
+                        xhci->cmd_ring->dequeue->generic.field[3]))) {
+                slot_id = TRB_TO_SLOT_ID(
+                        xhci->cmd_ring->dequeue->generic.field[3]);
+                virt_dev = xhci->devs[slot_id];
+                if (virt_dev)
+                        handle_cmd_in_cmd_wait_list(xhci, virt_dev,
+                                event);
+                else
+                        xhci_warn(xhci, "Stop endpoint command "
+                                "completion for disabled slot %u\n",
+                                slot_id);
+                return;
+        }
+
         memset(&deq_state, 0, sizeof(deq_state));
         slot_id = TRB_TO_SLOT_ID(trb->generic.field[3]);
         ep_index = TRB_TO_EP_INDEX(trb->generic.field[3]);
@@ -1091,7 +1112,7 @@ bandwidth_change:
                 complete(&xhci->addr_dev);
                 break;
         case TRB_TYPE(TRB_STOP_RING):
-                handle_stopped_endpoint(xhci, xhci->cmd_ring->dequeue);
+                handle_stopped_endpoint(xhci, xhci->cmd_ring->dequeue, event);
                 break;
         case TRB_TYPE(TRB_SET_DEQ):
                 handle_set_deq_completion(xhci, event, xhci->cmd_ring->dequeue);
@@ -2347,7 +2368,7 @@ static void giveback_first_trb(struct xhci_hcd *xhci, int slot_id,
          */
         wmb();
         start_trb->field[3] |= start_cycle;
-        ring_ep_doorbell(xhci, slot_id, ep_index, stream_id);
+        xhci_ring_ep_doorbell(xhci, slot_id, ep_index, stream_id);
 }
 
 /*
@@ -2931,7 +2952,7 @@ static int xhci_queue_isoc_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
         wmb();
         start_trb->field[3] |= start_cycle;
 
-        ring_ep_doorbell(xhci, slot_id, ep_index, urb->stream_id);
+        xhci_ring_ep_doorbell(xhci, slot_id, ep_index, urb->stream_id);
         return 0;
 }
 
@@ -3108,15 +3129,20 @@ int xhci_queue_evaluate_context(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
                         false);
 }
 
+/*
+ * Suspend is set to indicate "Stop Endpoint Command" is being issued to stop
+ * activity on an endpoint that is about to be suspended.
+ */
 int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, int slot_id,
-                unsigned int ep_index)
+                unsigned int ep_index, int suspend)
 {
         u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
         u32 trb_ep_index = EP_ID_FOR_TRB(ep_index);
         u32 type = TRB_TYPE(TRB_STOP_RING);
+        u32 trb_suspend = SUSPEND_PORT_FOR_TRB(suspend);
 
         return queue_command(xhci, 0, 0, 0,
-                        trb_slot_id | trb_ep_index | type, false);
+                        trb_slot_id | trb_ep_index | type | trb_suspend, false);
 }
 
 /* Set Transfer Ring Dequeue Pointer command.
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index caccecb7368..3d2af688157 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -968,7 +968,7 @@ int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
                 ep->stop_cmd_timer.expires = jiffies +
                         XHCI_STOP_EP_CMD_TIMEOUT * HZ;
                 add_timer(&ep->stop_cmd_timer);
-                xhci_queue_stop_endpoint(xhci, urb->dev->slot_id, ep_index);
+                xhci_queue_stop_endpoint(xhci, urb->dev->slot_id, ep_index, 0);
                 xhci_ring_cmd_db(xhci);
         }
 done:
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index a7181b491e6..73e5db3e89c 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -269,6 +269,10 @@ struct xhci_op_regs {
  * A read gives the current link PM state of the port,
  * a write with Link State Write Strobe set sets the link state.
  */
+#define PORT_PLS_MASK   (0xf << 5)
+#define XDEV_U0         (0x0 << 5)
+#define XDEV_U3         (0x3 << 5)
+#define XDEV_RESUME     (0xf << 5)
 /* true: port has power (see HCC_PPC) */
 #define PORT_POWER      (1 << 9)
 /* bits 10:13 indicate device speed:
@@ -510,6 +514,7 @@ struct xhci_slot_ctx {
 #define MAX_EXIT        (0xffff)
 /* Root hub port number that is needed to access the USB device */
 #define ROOT_HUB_PORT(p)        (((p) & 0xff) << 16)
+#define DEVINFO_TO_ROOT_HUB_PORT(p)     (((p) >> 16) & 0xff)
 /* Maximum number of ports under a hub device */
 #define XHCI_MAX_PORTS(p)       (((p) & 0xff) << 24)
 
@@ -754,6 +759,7 @@ struct xhci_virt_device {
         /* Status of the last command issued for this device */
         u32                             cmd_status;
         struct list_head                cmd_list;
+        u8                              port;
 };
 
 
@@ -884,6 +890,10 @@ struct xhci_event_cmd {
 #define TRB_TO_EP_INDEX(p)              ((((p) & (0x1f << 16)) >> 16) - 1)
 #define EP_ID_FOR_TRB(p)                ((((p) + 1) & 0x1f) << 16)
 
+#define SUSPEND_PORT_FOR_TRB(p)         (((p) & 1) << 23)
+#define TRB_TO_SUSPEND_PORT(p)          (((p) & (1 << 23)) >> 23)
+#define LAST_EP_INDEX                   30
+
 /* Set TR Dequeue Pointer command TRB fields */
 #define TRB_TO_STREAM_ID(p)             ((((p) & (0xffff << 16)) >> 16))
 #define STREAM_ID_FOR_TRB(p)            ((((p)) & 0xffff) << 16)
@@ -1202,6 +1212,9 @@ struct xhci_hcd {
 #define XHCI_LINK_TRB_QUIRK     (1 << 0)
 #define XHCI_RESET_EP_QUIRK     (1 << 1)
 #define XHCI_NEC_HOST           (1 << 2)
+        u32                     port_c_suspend[8];      /* port suspend change*/
+        u32                     suspended_ports[8];     /* which ports are
+                                                           suspended */
 };
 
 /* For testing purposes */
@@ -1409,7 +1422,7 @@ int xhci_queue_address_device(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
 int xhci_queue_vendor_command(struct xhci_hcd *xhci,
                 u32 field1, u32 field2, u32 field3, u32 field4);
 int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, int slot_id,
-                unsigned int ep_index);
+                unsigned int ep_index, int suspend);
 int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
                 int slot_id, unsigned int ep_index);
 int xhci_queue_bulk_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
@@ -1439,6 +1452,8 @@ void xhci_queue_config_ep_quirk(struct xhci_hcd *xhci,
                 unsigned int slot_id, unsigned int ep_index,
                 struct xhci_dequeue_state *deq_state);
 void xhci_stop_endpoint_command_watchdog(unsigned long arg);
+void xhci_ring_ep_doorbell(struct xhci_hcd *xhci, unsigned int slot_id,
+                unsigned int ep_index, unsigned int stream_id);
 
 /* xHCI roothub code */
 int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue, u16 wIndex,