10 files changed, 263 insertions, 323 deletions

diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index f7841d44feda..689433cdef25 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -1790,6 +1790,9 @@ int usb_set_usb2_hardware_lpm(struct usb_device *udev, int enable)
         struct usb_hcd *hcd = bus_to_hcd(udev->bus);
         int ret = -EPERM;
 
+        if (enable && !udev->usb2_hw_lpm_allowed)
+                return 0;
+
         if (hcd->driver->set_usb2_hw_lpm) {
                 ret = hcd->driver->set_usb2_hw_lpm(hcd, udev, enable);
                 if (!ret)
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 92dde941fdbe..06cec635e703 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -1112,16 +1112,13 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
                         /*
                          * USB3 protocol ports will automatically transition
                          * to Enabled state when detect an USB3.0 device attach.
-                         * Do not disable USB3 protocol ports.
+                         * Do not disable USB3 protocol ports, just pretend
+                         * power was lost
                          */
-                        if (!hub_is_superspeed(hdev)) {
+                        portstatus &= ~USB_PORT_STAT_ENABLE;
+                        if (!hub_is_superspeed(hdev))
                                 usb_clear_port_feature(hdev, port1,
                                                    USB_PORT_FEAT_ENABLE);
-                                portstatus &= ~USB_PORT_STAT_ENABLE;
-                        } else {
-                                /* Pretend that power was lost for USB3 devs */
-                                portstatus &= ~USB_PORT_STAT_ENABLE;
-                        }
                 }
 
                 /* Clear status-change flags; we'll debounce later */
@@ -3958,6 +3955,32 @@ static int hub_set_address(struct usb_device *udev, int devnum)
         return retval;
 }
 
+/*
+ * There are reports of USB 3.0 devices that say they support USB 2.0 Link PM
+ * when they're plugged into a USB 2.0 port, but they don't work when LPM is
+ * enabled.
+ *
+ * Only enable USB 2.0 Link PM if the port is internal (hardwired), or the
+ * device says it supports the new USB 2.0 Link PM errata by setting the BESL
+ * support bit in the BOS descriptor.
+ */
+static void hub_set_initial_usb2_lpm_policy(struct usb_device *udev)
+{
+        int connect_type;
+
+        if (!udev->usb2_hw_lpm_capable)
+                return;
+
+        connect_type = usb_get_hub_port_connect_type(udev->parent,
+                        udev->portnum);
+
+        if ((udev->bos->ext_cap->bmAttributes & USB_BESL_SUPPORT) ||
+                        connect_type == USB_PORT_CONNECT_TYPE_HARD_WIRED) {
+                udev->usb2_hw_lpm_allowed = 1;
+                usb_set_usb2_hardware_lpm(udev, 1);
+        }
+}
+
 /* Reset device, (re)assign address, get device descriptor.
  * Device connection must be stable, no more debouncing needed.
  * Returns device in USB_STATE_ADDRESS, except on error.
@@ -4251,6 +4274,7 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
         /* notify HCD that we have a device connected and addressed */
         if (hcd->driver->update_device)
                 hcd->driver->update_device(hcd, udev);
+        hub_set_initial_usb2_lpm_policy(udev);
 fail:
         if (retval) {
                 hub_port_disable(hub, port1, 0);
@@ -5095,6 +5119,12 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
         }
         parent_hub = usb_hub_to_struct_hub(parent_hdev);
 
+        /* Disable USB2 hardware LPM.
+         * It will be re-enabled by the enumeration process.
+         */
+        if (udev->usb2_hw_lpm_enabled == 1)
+                usb_set_usb2_hardware_lpm(udev, 0);
+
         bos = udev->bos;
         udev->bos = NULL;
 
@@ -5202,6 +5232,7 @@ static int usb_reset_and_verify_device(struct usb_device *udev)
 
 done:
         /* Now that the alt settings are re-installed, enable LTM and LPM. */
+        usb_set_usb2_hardware_lpm(udev, 1);
         usb_unlocked_enable_lpm(udev);
         usb_enable_ltm(udev);
         usb_release_bos_descriptor(udev);
diff --git a/drivers/usb/core/message.c b/drivers/usb/core/message.c
index 82927e1ed27d..bb315970e475 100644
--- a/drivers/usb/core/message.c
+++ b/drivers/usb/core/message.c
@@ -1182,8 +1182,12 @@ void usb_disable_device(struct usb_device *dev, int skip_ep0)
                         put_device(&dev->actconfig->interface[i]->dev);
                         dev->actconfig->interface[i] = NULL;
                 }
+
+                if (dev->usb2_hw_lpm_enabled == 1)
+                        usb_set_usb2_hardware_lpm(dev, 0);
                 usb_unlocked_disable_lpm(dev);
                 usb_disable_ltm(dev);
+
                 dev->actconfig = NULL;
                 if (dev->state == USB_STATE_CONFIGURED)
                         usb_set_device_state(dev, USB_STATE_ADDRESS);
diff --git a/drivers/usb/core/sysfs.c b/drivers/usb/core/sysfs.c
index 5cf431b0424c..52a97adf02a0 100644
--- a/drivers/usb/core/sysfs.c
+++ b/drivers/usb/core/sysfs.c
@@ -458,7 +458,7 @@ static ssize_t usb2_hardware_lpm_show(struct device *dev,
         struct usb_device *udev = to_usb_device(dev);
         const char *p;
 
-        if (udev->usb2_hw_lpm_enabled == 1)
+        if (udev->usb2_hw_lpm_allowed == 1)
                 p = "enabled";
         else
                 p = "disabled";
@@ -478,8 +478,10 @@ static ssize_t usb2_hardware_lpm_store(struct device *dev,
 
         ret = strtobool(buf, &value);
 
-        if (!ret)
+        if (!ret) {
+                udev->usb2_hw_lpm_allowed = value;
                 ret = usb_set_usb2_hardware_lpm(udev, value);
+        }
 
         usb_unlock_device(udev);
 
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index e8b4c56dcf62..805f2348eeba 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -296,7 +296,7 @@ static int xhci_stop_device(struct xhci_hcd *xhci, int slot_id, int suspend)
         /* Wait for last stop endpoint command to finish */
         timeleft = wait_for_completion_interruptible_timeout(
                         cmd->completion,
-                        USB_CTRL_SET_TIMEOUT);
+                        XHCI_CMD_DEFAULT_TIMEOUT);
         if (timeleft <= 0) {
                 xhci_warn(xhci, "%s while waiting for stop endpoint command\n",
                                 timeleft == 0 ? "Timeout" : "Signal");
@@ -524,7 +524,8 @@ static void xhci_hub_report_usb3_link_state(u32 *status, u32 status_reg)
  * the compliance mode timer is deleted. A port won't enter
  * compliance mode if it has previously entered U0.
  */
-void xhci_del_comp_mod_timer(struct xhci_hcd *xhci, u32 status, u16 wIndex)
+static void xhci_del_comp_mod_timer(struct xhci_hcd *xhci, u32 status,
+                                    u16 wIndex)
 {
         u32 all_ports_seen_u0 = ((1 << xhci->num_usb3_ports)-1);
         bool port_in_u0 = ((status & PORT_PLS_MASK) == XDEV_U0);
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 83bcd13622c3..49b8bd063fab 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -1693,9 +1693,7 @@ void xhci_free_command(struct xhci_hcd *xhci,
 void xhci_mem_cleanup(struct xhci_hcd *xhci)
 {
         struct pci_dev  *pdev = to_pci_dev(xhci_to_hcd(xhci)->self.controller);
-        struct dev_info *dev_info, *next;
         struct xhci_cd  *cur_cd, *next_cd;
-        unsigned long   flags;
         int size;
         int i, j, num_ports;
 
@@ -1756,13 +1754,6 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 
         scratchpad_free(xhci);
 
-        spin_lock_irqsave(&xhci->lock, flags);
-        list_for_each_entry_safe(dev_info, next, &xhci->lpm_failed_devs, list) {
-                list_del(&dev_info->list);
-                kfree(dev_info);
-        }
-        spin_unlock_irqrestore(&xhci->lock, flags);
-
         if (!xhci->rh_bw)
                 goto no_bw;
 
@@ -2231,7 +2222,6 @@ int xhci_mem_init(struct xhci_hcd *xhci, gfp_t flags)
         u32 page_size, temp;
         int i;
 
-        INIT_LIST_HEAD(&xhci->lpm_failed_devs);
         INIT_LIST_HEAD(&xhci->cancel_cmd_list);
 
         page_size = xhci_readl(xhci, &xhci->op_regs->page_size);
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 6bfbd80ec2b9..1e2f3f495843 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -178,7 +178,7 @@ static void inc_deq(struct xhci_hcd *xhci, struct xhci_ring *ring)
                         if (ring->type == TYPE_EVENT &&
                                         last_trb_on_last_seg(xhci, ring,
                                                 ring->deq_seg, ring->dequeue)) {
-                                ring->cycle_state = (ring->cycle_state ? 0 : 1);
+                                ring->cycle_state ^= 1;
                         }
                         ring->deq_seg = ring->deq_seg->next;
                         ring->dequeue = ring->deq_seg->trbs;
@@ -726,7 +726,7 @@ static void xhci_stop_watchdog_timer_in_irq(struct xhci_hcd *xhci,
 
 /* Must be called with xhci->lock held in interrupt context */
 static void xhci_giveback_urb_in_irq(struct xhci_hcd *xhci,
-                struct xhci_td *cur_td, int status, char *adjective)
+                struct xhci_td *cur_td, int status)
 {
         struct usb_hcd *hcd;
         struct urb      *urb;
@@ -765,10 +765,9 @@ static void xhci_giveback_urb_in_irq(struct xhci_hcd *xhci,
  *  2. Otherwise, we turn all the TRBs in the TD into No-op TRBs (with the chain
  *     bit cleared) so that the HW will skip over them.
  */
-static void handle_stopped_endpoint(struct xhci_hcd *xhci,
+static void xhci_handle_cmd_stop_ep(struct xhci_hcd *xhci, int slot_id,
                 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;
@@ -779,10 +778,7 @@ static void handle_stopped_endpoint(struct xhci_hcd *xhci,
 
         struct xhci_dequeue_state deq_state;
 
-        if (unlikely(TRB_TO_SUSPEND_PORT(
-                             le32_to_cpu(xhci->cmd_ring->dequeue->generic.field[3])))) {
-                slot_id = TRB_TO_SLOT_ID(
-                        le32_to_cpu(xhci->cmd_ring->dequeue->generic.field[3]));
+        if (unlikely(TRB_TO_SUSPEND_PORT(le32_to_cpu(trb->generic.field[3])))) {
                 virt_dev = xhci->devs[slot_id];
                 if (virt_dev)
                         handle_cmd_in_cmd_wait_list(xhci, virt_dev,
@@ -795,7 +791,6 @@ static void handle_stopped_endpoint(struct xhci_hcd *xhci,
         }
 
         memset(&deq_state, 0, sizeof(deq_state));
-        slot_id = TRB_TO_SLOT_ID(le32_to_cpu(trb->generic.field[3]));
         ep_index = TRB_TO_EP_INDEX(le32_to_cpu(trb->generic.field[3]));
         ep = &xhci->devs[slot_id]->eps[ep_index];
 
@@ -891,7 +886,7 @@ remove_finished_td:
                 /* Doesn't matter what we pass for status, since the core will
                  * just overwrite it (because the URB has been unlinked).
                  */
-                xhci_giveback_urb_in_irq(xhci, cur_td, 0, "cancelled");
+                xhci_giveback_urb_in_irq(xhci, cur_td, 0);
 
                 /* Stop processing the cancelled list if the watchdog timer is
                  * running.
@@ -1001,7 +996,7 @@ void xhci_stop_endpoint_command_watchdog(unsigned long arg)
                                 if (!list_empty(&cur_td->cancelled_td_list))
                                         list_del_init(&cur_td->cancelled_td_list);
                                 xhci_giveback_urb_in_irq(xhci, cur_td,
-                                                -ESHUTDOWN, "killed");
+                                                -ESHUTDOWN);
                         }
                         while (!list_empty(&temp_ep->cancelled_td_list)) {
                                 cur_td = list_first_entry(
@@ -1010,7 +1005,7 @@ void xhci_stop_endpoint_command_watchdog(unsigned long arg)
                                                 cancelled_td_list);
                                 list_del_init(&cur_td->cancelled_td_list);
                                 xhci_giveback_urb_in_irq(xhci, cur_td,
-                                                -ESHUTDOWN, "killed");
+                                                -ESHUTDOWN);
                         }
                 }
         }
@@ -1077,11 +1072,9 @@ static void update_ring_for_set_deq_completion(struct xhci_hcd *xhci,
  * endpoint doorbell to restart the ring, but only if there aren't more
  * cancellations pending.
  */
-static void handle_set_deq_completion(struct xhci_hcd *xhci,
-                struct xhci_event_cmd *event,
-                union xhci_trb *trb)
+static void xhci_handle_cmd_set_deq(struct xhci_hcd *xhci, int slot_id,
+                union xhci_trb *trb, u32 cmd_comp_code)
 {
-        unsigned int slot_id;
         unsigned int ep_index;
         unsigned int stream_id;
         struct xhci_ring *ep_ring;
@@ -1089,7 +1082,6 @@ static void handle_set_deq_completion(struct xhci_hcd *xhci,
         struct xhci_ep_ctx *ep_ctx;
         struct xhci_slot_ctx *slot_ctx;
 
-        slot_id = TRB_TO_SLOT_ID(le32_to_cpu(trb->generic.field[3]));
         ep_index = TRB_TO_EP_INDEX(le32_to_cpu(trb->generic.field[3]));
         stream_id = TRB_TO_STREAM_ID(le32_to_cpu(trb->generic.field[2]));
         dev = xhci->devs[slot_id];
@@ -1107,11 +1099,11 @@ static void handle_set_deq_completion(struct xhci_hcd *xhci,
         ep_ctx = xhci_get_ep_ctx(xhci, dev->out_ctx, ep_index);
         slot_ctx = xhci_get_slot_ctx(xhci, dev->out_ctx);
 
-        if (GET_COMP_CODE(le32_to_cpu(event->status)) != COMP_SUCCESS) {
+        if (cmd_comp_code != COMP_SUCCESS) {
                 unsigned int ep_state;
                 unsigned int slot_state;
 
-                switch (GET_COMP_CODE(le32_to_cpu(event->status))) {
+                switch (cmd_comp_code) {
                 case COMP_TRB_ERR:
                         xhci_warn(xhci, "WARN Set TR Deq Ptr cmd invalid because "
                                         "of stream ID configuration\n");
@@ -1134,7 +1126,7 @@ static void handle_set_deq_completion(struct xhci_hcd *xhci,
                 default:
                         xhci_warn(xhci, "WARN Set TR Deq Ptr cmd with unknown "
                                         "completion code of %u.\n",
-                                  GET_COMP_CODE(le32_to_cpu(event->status)));
+                                  cmd_comp_code);
                         break;
                 }
                 /* OK what do we do now?  The endpoint state is hosed, and we
@@ -1171,21 +1163,17 @@ static void handle_set_deq_completion(struct xhci_hcd *xhci,
         ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
 }
 
-static void handle_reset_ep_completion(struct xhci_hcd *xhci,
-                struct xhci_event_cmd *event,
-                union xhci_trb *trb)
+static void xhci_handle_cmd_reset_ep(struct xhci_hcd *xhci, int slot_id,
+                union xhci_trb *trb, u32 cmd_comp_code)
 {
-        int slot_id;
         unsigned int ep_index;
 
-        slot_id = TRB_TO_SLOT_ID(le32_to_cpu(trb->generic.field[3]));
         ep_index = TRB_TO_EP_INDEX(le32_to_cpu(trb->generic.field[3]));
         /* This command will only fail if the endpoint wasn't halted,
          * but we don't care.
          */
         xhci_dbg_trace(xhci, trace_xhci_dbg_reset_ep,
-                "Ignoring reset ep completion code of %u",
-                 GET_COMP_CODE(le32_to_cpu(event->status)));
+                "Ignoring reset ep completion code of %u", cmd_comp_code);
 
         /* HW with the reset endpoint quirk needs to have a configure endpoint
          * command complete before the endpoint can be used.  Queue that here
@@ -1386,21 +1374,149 @@ static int handle_stopped_cmd_ring(struct xhci_hcd *xhci,
         return cur_trb_is_good;
 }
 
+static void xhci_handle_cmd_enable_slot(struct xhci_hcd *xhci, int slot_id,
+                u32 cmd_comp_code)
+{
+        if (cmd_comp_code == COMP_SUCCESS)
+                xhci->slot_id = slot_id;
+        else
+                xhci->slot_id = 0;
+        complete(&xhci->addr_dev);
+}
+
+static void xhci_handle_cmd_disable_slot(struct xhci_hcd *xhci, int slot_id)
+{
+        struct xhci_virt_device *virt_dev;
+
+        virt_dev = xhci->devs[slot_id];
+        if (!virt_dev)
+                return;
+        if (xhci->quirks & XHCI_EP_LIMIT_QUIRK)
+                /* Delete default control endpoint resources */
+                xhci_free_device_endpoint_resources(xhci, virt_dev, true);
+        xhci_free_virt_device(xhci, slot_id);
+}
+
+static void xhci_handle_cmd_config_ep(struct xhci_hcd *xhci, int slot_id,
+                struct xhci_event_cmd *event, u32 cmd_comp_code)
+{
+        struct xhci_virt_device *virt_dev;
+        struct xhci_input_control_ctx *ctrl_ctx;
+        unsigned int ep_index;
+        unsigned int ep_state;
+        u32 add_flags, drop_flags;
+
+        virt_dev = xhci->devs[slot_id];
+        if (handle_cmd_in_cmd_wait_list(xhci, virt_dev, event))
+                return;
+        /*
+         * Configure endpoint commands can come from the USB core
+         * configuration or alt setting changes, or because the HW
+         * needed an extra configure endpoint command after a reset
+         * endpoint command or streams were being configured.
+         * If the command was for a halted endpoint, the xHCI driver
+         * is not waiting on the configure endpoint command.
+         */
+        ctrl_ctx = xhci_get_input_control_ctx(xhci, virt_dev->in_ctx);
+        if (!ctrl_ctx) {
+                xhci_warn(xhci, "Could not get input context, bad type.\n");
+                return;
+        }
+
+        add_flags = le32_to_cpu(ctrl_ctx->add_flags);
+        drop_flags = le32_to_cpu(ctrl_ctx->drop_flags);
+        /* Input ctx add_flags are the endpoint index plus one */
+        ep_index = xhci_last_valid_endpoint(add_flags) - 1;
+
+        /* A usb_set_interface() call directly after clearing a halted
+         * condition may race on this quirky hardware.  Not worth
+         * worrying about, since this is prototype hardware.  Not sure
+         * if this will work for streams, but streams support was
+         * untested on this prototype.
+         */
+        if (xhci->quirks & XHCI_RESET_EP_QUIRK &&
+                        ep_index != (unsigned int) -1 &&
+                        add_flags - SLOT_FLAG == drop_flags) {
+                ep_state = virt_dev->eps[ep_index].ep_state;
+                if (!(ep_state & EP_HALTED))
+                        goto bandwidth_change;
+                xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
+                                "Completed config ep cmd - "
+                                "last ep index = %d, state = %d",
+                                ep_index, ep_state);
+                /* Clear internal halted state and restart ring(s) */
+                virt_dev->eps[ep_index].ep_state &= ~EP_HALTED;
+                ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
+                return;
+        }
+bandwidth_change:
+        xhci_dbg_trace(xhci,  trace_xhci_dbg_context_change,
+                        "Completed config ep cmd");
+        virt_dev->cmd_status = cmd_comp_code;
+        complete(&virt_dev->cmd_completion);
+        return;
+}
+
+static void xhci_handle_cmd_eval_ctx(struct xhci_hcd *xhci, int slot_id,
+                struct xhci_event_cmd *event, u32 cmd_comp_code)
+{
+        struct xhci_virt_device *virt_dev;
+
+        virt_dev = xhci->devs[slot_id];
+        if (handle_cmd_in_cmd_wait_list(xhci, virt_dev, event))
+                return;
+        virt_dev->cmd_status = cmd_comp_code;
+        complete(&virt_dev->cmd_completion);
+}
+
+static void xhci_handle_cmd_addr_dev(struct xhci_hcd *xhci, int slot_id,
+                u32 cmd_comp_code)
+{
+        xhci->devs[slot_id]->cmd_status = cmd_comp_code;
+        complete(&xhci->addr_dev);
+}
+
+static void xhci_handle_cmd_reset_dev(struct xhci_hcd *xhci, int slot_id,
+                struct xhci_event_cmd *event)
+{
+        struct xhci_virt_device *virt_dev;
+
+        xhci_dbg(xhci, "Completed reset device command.\n");
+        virt_dev = xhci->devs[slot_id];
+        if (virt_dev)
+                handle_cmd_in_cmd_wait_list(xhci, virt_dev, event);
+        else
+                xhci_warn(xhci, "Reset device command completion "
+                                "for disabled slot %u\n", slot_id);
+}
+
+static void xhci_handle_cmd_nec_get_fw(struct xhci_hcd *xhci,
+                struct xhci_event_cmd *event)
+{
+        if (!(xhci->quirks & XHCI_NEC_HOST)) {
+                xhci->error_bitmask |= 1 << 6;
+                return;
+        }
+        xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
+                        "NEC firmware version %2x.%02x",
+                        NEC_FW_MAJOR(le32_to_cpu(event->status)),
+                        NEC_FW_MINOR(le32_to_cpu(event->status)));
+}
+
 static void handle_cmd_completion(struct xhci_hcd *xhci,
                 struct xhci_event_cmd *event)
 {
         int slot_id = TRB_TO_SLOT_ID(le32_to_cpu(event->flags));
         u64 cmd_dma;
         dma_addr_t cmd_dequeue_dma;
-        struct xhci_input_control_ctx *ctrl_ctx;
-        struct xhci_virt_device *virt_dev;
-        unsigned int ep_index;
-        struct xhci_ring *ep_ring;
-        unsigned int ep_state;
+        u32 cmd_comp_code;
+        union xhci_trb *cmd_trb;
+        u32 cmd_type;
 
         cmd_dma = le64_to_cpu(event->cmd_trb);
+        cmd_trb = xhci->cmd_ring->dequeue;
         cmd_dequeue_dma = xhci_trb_virt_to_dma(xhci->cmd_ring->deq_seg,
-                        xhci->cmd_ring->dequeue);
+                        cmd_trb);
         /* Is the command ring deq ptr out of sync with the deq seg ptr? */
         if (cmd_dequeue_dma == 0) {
                 xhci->error_bitmask |= 1 << 4;
@@ -1412,19 +1528,17 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
                 return;
         }
 
-        trace_xhci_cmd_completion(&xhci->cmd_ring->dequeue->generic,
-                                        (struct xhci_generic_trb *) event);
+        trace_xhci_cmd_completion(cmd_trb, (struct xhci_generic_trb *) event);
 
-        if ((GET_COMP_CODE(le32_to_cpu(event->status)) == COMP_CMD_ABORT) ||
-                (GET_COMP_CODE(le32_to_cpu(event->status)) == COMP_CMD_STOP)) {
+        cmd_comp_code = GET_COMP_CODE(le32_to_cpu(event->status));
+        if (cmd_comp_code == COMP_CMD_ABORT || cmd_comp_code == COMP_CMD_STOP) {
                 /* If the return value is 0, we think the trb pointed by
                  * command ring dequeue pointer is a good trb. The good
                  * trb means we don't want to cancel the trb, but it have
                  * been stopped by host. So we should handle it normally.
                  * Otherwise, driver should invoke inc_deq() and return.
                  */
-                if (handle_stopped_cmd_ring(xhci,
-                                GET_COMP_CODE(le32_to_cpu(event->status)))) {
+                if (handle_stopped_cmd_ring(xhci, cmd_comp_code)) {
                         inc_deq(xhci, xhci->cmd_ring);
                         return;
                 }
@@ -1436,117 +1550,47 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
                         return;
         }
 
-        switch (le32_to_cpu(xhci->cmd_ring->dequeue->generic.field[3])
-                & TRB_TYPE_BITMASK) {
-        case TRB_TYPE(TRB_ENABLE_SLOT):
-                if (GET_COMP_CODE(le32_to_cpu(event->status)) == COMP_SUCCESS)
-                        xhci->slot_id = slot_id;
-                else
-                        xhci->slot_id = 0;
-                complete(&xhci->addr_dev);
+        cmd_type = TRB_FIELD_TO_TYPE(le32_to_cpu(cmd_trb->generic.field[3]));
+        switch (cmd_type) {
+        case TRB_ENABLE_SLOT:
+                xhci_handle_cmd_enable_slot(xhci, slot_id, cmd_comp_code);
                 break;
-        case TRB_TYPE(TRB_DISABLE_SLOT):
-                if (xhci->devs[slot_id]) {
-                        if (xhci->quirks & XHCI_EP_LIMIT_QUIRK)
-                                /* Delete default control endpoint resources */
-                                xhci_free_device_endpoint_resources(xhci,
-                                                xhci->devs[slot_id], true);
-                        xhci_free_virt_device(xhci, slot_id);
-                }
+        case TRB_DISABLE_SLOT:
+                xhci_handle_cmd_disable_slot(xhci, slot_id);
                 break;
-        case TRB_TYPE(TRB_CONFIG_EP):
-                virt_dev = xhci->devs[slot_id];
-                if (handle_cmd_in_cmd_wait_list(xhci, virt_dev, event))
-                        break;
-                /*
-                 * Configure endpoint commands can come from the USB core
-                 * configuration or alt setting changes, or because the HW
-                 * needed an extra configure endpoint command after a reset
-                 * endpoint command or streams were being configured.
-                 * If the command was for a halted endpoint, the xHCI driver
-                 * is not waiting on the configure endpoint command.
-                 */
-                ctrl_ctx = xhci_get_input_control_ctx(xhci,
-                                virt_dev->in_ctx);
-                if (!ctrl_ctx) {
-                        xhci_warn(xhci, "Could not get input context, bad type.\n");
-                        break;
-                }
-                /* Input ctx add_flags are the endpoint index plus one */
-                ep_index = xhci_last_valid_endpoint(le32_to_cpu(ctrl_ctx->add_flags)) - 1;
-                /* A usb_set_interface() call directly after clearing a halted
-                 * condition may race on this quirky hardware.  Not worth
-                 * worrying about, since this is prototype hardware.  Not sure
-                 * if this will work for streams, but streams support was
-                 * untested on this prototype.
-                 */
-                if (xhci->quirks & XHCI_RESET_EP_QUIRK &&
-                                ep_index != (unsigned int) -1 &&
-                    le32_to_cpu(ctrl_ctx->add_flags) - SLOT_FLAG ==
-                    le32_to_cpu(ctrl_ctx->drop_flags)) {
-                        ep_ring = xhci->devs[slot_id]->eps[ep_index].ring;
-                        ep_state = xhci->devs[slot_id]->eps[ep_index].ep_state;
-                        if (!(ep_state & EP_HALTED))
-                                goto bandwidth_change;
-                        xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
-                                        "Completed config ep cmd - "
-                                        "last ep index = %d, state = %d",
-                                        ep_index, ep_state);
-                        /* Clear internal halted state and restart ring(s) */
-                        xhci->devs[slot_id]->eps[ep_index].ep_state &=
-                                ~EP_HALTED;
-                        ring_doorbell_for_active_rings(xhci, slot_id, ep_index);
-                        break;
-                }
-bandwidth_change:
-                xhci_dbg_trace(xhci,  trace_xhci_dbg_context_change,
-                                "Completed config ep cmd");
-                xhci->devs[slot_id]->cmd_status =
-                        GET_COMP_CODE(le32_to_cpu(event->status));
-                complete(&xhci->devs[slot_id]->cmd_completion);
+        case TRB_CONFIG_EP:
+                xhci_handle_cmd_config_ep(xhci, slot_id, event, cmd_comp_code);
                 break;
-        case TRB_TYPE(TRB_EVAL_CONTEXT):
-                virt_dev = xhci->devs[slot_id];
-                if (handle_cmd_in_cmd_wait_list(xhci, virt_dev, event))
-                        break;
-                xhci->devs[slot_id]->cmd_status = GET_COMP_CODE(le32_to_cpu(event->status));
-                complete(&xhci->devs[slot_id]->cmd_completion);
+        case TRB_EVAL_CONTEXT:
+                xhci_handle_cmd_eval_ctx(xhci, slot_id, event, cmd_comp_code);
                 break;
-        case TRB_TYPE(TRB_ADDR_DEV):
-                xhci->devs[slot_id]->cmd_status = GET_COMP_CODE(le32_to_cpu(event->status));
-                complete(&xhci->addr_dev);
+        case TRB_ADDR_DEV:
+                xhci_handle_cmd_addr_dev(xhci, slot_id, cmd_comp_code);
                 break;
-        case TRB_TYPE(TRB_STOP_RING):
-                handle_stopped_endpoint(xhci, xhci->cmd_ring->dequeue, event);
+        case TRB_STOP_RING:
+                WARN_ON(slot_id != TRB_TO_SLOT_ID(
+                                le32_to_cpu(cmd_trb->generic.field[3])));
+                xhci_handle_cmd_stop_ep(xhci, slot_id, cmd_trb, event);
                 break;
-        case TRB_TYPE(TRB_SET_DEQ):
-                handle_set_deq_completion(xhci, event, xhci->cmd_ring->dequeue);
+        case TRB_SET_DEQ:
+                WARN_ON(slot_id != TRB_TO_SLOT_ID(
+                                le32_to_cpu(cmd_trb->generic.field[3])));
+                xhci_handle_cmd_set_deq(xhci, slot_id, cmd_trb, cmd_comp_code);
                 break;
-        case TRB_TYPE(TRB_CMD_NOOP):
+        case TRB_CMD_NOOP:
                 break;
-        case TRB_TYPE(TRB_RESET_EP):
-                handle_reset_ep_completion(xhci, event, xhci->cmd_ring->dequeue);
+        case TRB_RESET_EP:
+                WARN_ON(slot_id != TRB_TO_SLOT_ID(
+                                le32_to_cpu(cmd_trb->generic.field[3])));
+                xhci_handle_cmd_reset_ep(xhci, slot_id, cmd_trb, cmd_comp_code);
                 break;
-        case TRB_TYPE(TRB_RESET_DEV):
-                xhci_dbg(xhci, "Completed reset device command.\n");
-                slot_id = TRB_TO_SLOT_ID(
-                        le32_to_cpu(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, "Reset device command completion "
-                                        "for disabled slot %u\n", slot_id);
+        case TRB_RESET_DEV:
+                WARN_ON(slot_id != TRB_TO_SLOT_ID(
+                                le32_to_cpu(cmd_trb->generic.field[3])));
+                xhci_handle_cmd_reset_dev(xhci, slot_id, event);
                 break;
-        case TRB_TYPE(TRB_NEC_GET_FW):
-                if (!(xhci->quirks & XHCI_NEC_HOST)) {
-                        xhci->error_bitmask |= 1 << 6;
-                        break;
-                }
-                xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
-                        "NEC firmware version %2x.%02x",
-                         NEC_FW_MAJOR(le32_to_cpu(event->status)),
-                         NEC_FW_MINOR(le32_to_cpu(event->status)));
+        case TRB_NEC_GET_FW:
+                xhci_handle_cmd_nec_get_fw(xhci, event);
                 break;
         default:
                 /* Skip over unknown commands on the event ring */
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 6e0d886bcce5..4265b48856f6 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -3459,7 +3459,7 @@ int xhci_discover_or_reset_device(struct usb_hcd *hcd, struct usb_device *udev)
         /* Wait for the Reset Device command to finish */
         timeleft = wait_for_completion_interruptible_timeout(
                         reset_device_cmd->completion,
-                        USB_CTRL_SET_TIMEOUT);
+                        XHCI_CMD_DEFAULT_TIMEOUT);
         if (timeleft <= 0) {
                 xhci_warn(xhci, "%s while waiting for reset device command\n",
                                 timeleft == 0 ? "Timeout" : "Signal");
@@ -3583,11 +3583,6 @@ void xhci_free_dev(struct usb_hcd *hcd, struct usb_device *udev)
                 del_timer_sync(&virt_dev->eps[i].stop_cmd_timer);
         }
 
-        if (udev->usb2_hw_lpm_enabled) {
-                xhci_set_usb2_hardware_lpm(hcd, udev, 0);
-                udev->usb2_hw_lpm_enabled = 0;
-        }
-
         spin_lock_irqsave(&xhci->lock, flags);
         /* Don't disable the slot if the host controller is dead. */
         state = xhci_readl(xhci, &xhci->op_regs->status);
@@ -3721,9 +3716,6 @@ disable_slot:
  * the device).
  * We should be protected by the usb_address0_mutex in khubd's hub_port_init, so
  * we should only issue and wait on one address command at the same time.
- *
- * We add one to the device address issued by the hardware because the USB core
- * uses address 1 for the root hubs (even though they're not really devices).
  */
 int xhci_address_device(struct usb_hcd *hcd, struct usb_device *udev)
 {
@@ -3868,16 +3860,13 @@ int xhci_address_device(struct usb_hcd *hcd, struct usb_device *udev)
         slot_ctx = xhci_get_slot_ctx(xhci, virt_dev->out_ctx);
         trace_xhci_address_ctx(xhci, virt_dev->out_ctx,
                                 slot_ctx->dev_info >> 27);
-        /* Use kernel assigned address for devices; store xHC assigned
-         * address locally. */
-        virt_dev->address = (le32_to_cpu(slot_ctx->dev_state) & DEV_ADDR_MASK)
-                + 1;
         /* Zero the input context control for later use */
         ctrl_ctx->add_flags = 0;
         ctrl_ctx->drop_flags = 0;
 
         xhci_dbg_trace(xhci, trace_xhci_dbg_address,
-                        "Internal device address = %d", virt_dev->address);
+                       "Internal device address = %d",
+                       le32_to_cpu(slot_ctx->dev_state) & DEV_ADDR_MASK);
 
         return 0;
 }
@@ -4025,133 +4014,6 @@ static int xhci_calculate_usb2_hw_lpm_params(struct usb_device *udev)
         return PORT_BESLD(besld) | PORT_L1_TIMEOUT(l1) | PORT_HIRDM(hirdm);
 }
 
-static int xhci_usb2_software_lpm_test(struct usb_hcd *hcd,
-                                        struct usb_device *udev)
-{
-        struct xhci_hcd *xhci = hcd_to_xhci(hcd);
-        struct dev_info *dev_info;
-        __le32 __iomem  **port_array;
-        __le32 __iomem  *addr, *pm_addr;
-        u32             temp, dev_id;
-        unsigned int    port_num;
-        unsigned long   flags;
-        int             hird;
-        int             ret;
-
-        if (hcd->speed == HCD_USB3 || !xhci->sw_lpm_support ||
-                        !udev->lpm_capable)
-                return -EINVAL;
-
-        /* we only support lpm for non-hub device connected to root hub yet */
-        if (!udev->parent || udev->parent->parent ||
-                        udev->descriptor.bDeviceClass == USB_CLASS_HUB)
-                return -EINVAL;
-
-        spin_lock_irqsave(&xhci->lock, flags);
-
-        /* Look for devices in lpm_failed_devs list */
-        dev_id = le16_to_cpu(udev->descriptor.idVendor) << 16 |
-                        le16_to_cpu(udev->descriptor.idProduct);
-        list_for_each_entry(dev_info, &xhci->lpm_failed_devs, list) {
-                if (dev_info->dev_id == dev_id) {
-                        ret = -EINVAL;
-                        goto finish;
-                }
-        }
-
-        port_array = xhci->usb2_ports;
-        port_num = udev->portnum - 1;
-
-        if (port_num > HCS_MAX_PORTS(xhci->hcs_params1)) {
-                xhci_dbg(xhci, "invalid port number %d\n", udev->portnum);
-                ret = -EINVAL;
-                goto finish;
-        }
-
-        /*
-         * Test USB 2.0 software LPM.
-         * FIXME: some xHCI 1.0 hosts may implement a new register to set up
-         * hardware-controlled USB 2.0 LPM. See section 5.4.11 and 4.23.5.1.1.1
-         * in the June 2011 errata release.
-         */
-        xhci_dbg(xhci, "test port %d software LPM\n", port_num);
-        /*
-         * Set L1 Device Slot and HIRD/BESL.
-         * Check device's USB 2.0 extension descriptor to determine whether
-         * HIRD or BESL shoule be used. See USB2.0 LPM errata.
-         */
-        pm_addr = port_array[port_num] + PORTPMSC;
-        hird = xhci_calculate_hird_besl(xhci, udev);
-        temp = PORT_L1DS(udev->slot_id) | PORT_HIRD(hird);
-        xhci_writel(xhci, temp, pm_addr);
-
-        /* Set port link state to U2(L1) */
-        addr = port_array[port_num];
-        xhci_set_link_state(xhci, port_array, port_num, XDEV_U2);
-
-        /* wait for ACK */
-        spin_unlock_irqrestore(&xhci->lock, flags);
-        msleep(10);
-        spin_lock_irqsave(&xhci->lock, flags);
-
-        /* Check L1 Status */
-        ret = xhci_handshake(xhci, pm_addr,
-                        PORT_L1S_MASK, PORT_L1S_SUCCESS, 125);
-        if (ret != -ETIMEDOUT) {
-                /* enter L1 successfully */
-                temp = xhci_readl(xhci, addr);
-                xhci_dbg(xhci, "port %d entered L1 state, port status 0x%x\n",
-                                port_num, temp);
-                ret = 0;
-        } else {
-                temp = xhci_readl(xhci, pm_addr);
-                xhci_dbg(xhci, "port %d software lpm failed, L1 status %d\n",
-                                port_num, temp & PORT_L1S_MASK);
-                ret = -EINVAL;
-        }
-
-        /* Resume the port */
-        xhci_set_link_state(xhci, port_array, port_num, XDEV_U0);
-
-        spin_unlock_irqrestore(&xhci->lock, flags);
-        msleep(10);
-        spin_lock_irqsave(&xhci->lock, flags);
-
-        /* Clear PLC */
-        xhci_test_and_clear_bit(xhci, port_array, port_num, PORT_PLC);
-
-        /* Check PORTSC to make sure the device is in the right state */
-        if (!ret) {
-                temp = xhci_readl(xhci, addr);
-                xhci_dbg(xhci, "resumed port %d status 0x%x\n", port_num, temp);
-                if (!(temp & PORT_CONNECT) || !(temp & PORT_PE) ||
-                                (temp & PORT_PLS_MASK) != XDEV_U0) {
-                        xhci_dbg(xhci, "port L1 resume fail\n");
-                        ret = -EINVAL;
-                }
-        }
-
-        if (ret) {
-                /* Insert dev to lpm_failed_devs list */
-                xhci_warn(xhci, "device LPM test failed, may disconnect and "
-                                "re-enumerate\n");
-                dev_info = kzalloc(sizeof(struct dev_info), GFP_ATOMIC);
-                if (!dev_info) {
-                        ret = -ENOMEM;
-                        goto finish;
-                }
-                dev_info->dev_id = dev_id;
-                INIT_LIST_HEAD(&dev_info->list);
-                list_add(&dev_info->list, &xhci->lpm_failed_devs);
-        } else {
-                xhci_ring_device(xhci, udev->slot_id);
-        }
-
-finish:
-        spin_unlock_irqrestore(&xhci->lock, flags);
-        return ret;
-}
-
 int xhci_set_usb2_hardware_lpm(struct usb_hcd *hcd,
                         struct usb_device *udev, int enable)
 {
@@ -4228,7 +4090,7 @@ int xhci_set_usb2_hardware_lpm(struct usb_hcd *hcd,
                 }
 
                 pm_val &= ~PORT_HIRD_MASK;
-                pm_val |= PORT_HIRD(hird) | PORT_RWE;
+                pm_val |= PORT_HIRD(hird) | PORT_RWE | PORT_L1DS(udev->slot_id);
                 xhci_writel(xhci, pm_val, pm_addr);
                 pm_val = xhci_readl(xhci, pm_addr);
                 pm_val |= PORT_HLE;
@@ -4236,7 +4098,7 @@ int xhci_set_usb2_hardware_lpm(struct usb_hcd *hcd,
                 /* flush write */
                 xhci_readl(xhci, pm_addr);
         } else {
-                pm_val &= ~(PORT_HLE | PORT_RWE | PORT_HIRD_MASK);
+                pm_val &= ~(PORT_HLE | PORT_RWE | PORT_HIRD_MASK | PORT_L1DS_MASK);
                 xhci_writel(xhci, pm_val, pm_addr);
                 /* flush write */
                 xhci_readl(xhci, pm_addr);
@@ -4279,24 +4141,26 @@ static int xhci_check_usb2_port_capability(struct xhci_hcd *xhci, int port,
 int xhci_update_device(struct usb_hcd *hcd, struct usb_device *udev)
 {
         struct xhci_hcd *xhci = hcd_to_xhci(hcd);
-        int             ret;
         int             portnum = udev->portnum - 1;
 
-        ret = xhci_usb2_software_lpm_test(hcd, udev);
-        if (!ret) {
-                xhci_dbg(xhci, "software LPM test succeed\n");
-                if (xhci->hw_lpm_support == 1 &&
-                    xhci_check_usb2_port_capability(xhci, portnum, XHCI_HLC)) {
-                        udev->usb2_hw_lpm_capable = 1;
-                        udev->l1_params.timeout = XHCI_L1_TIMEOUT;
-                        udev->l1_params.besl = XHCI_DEFAULT_BESL;
-                        if (xhci_check_usb2_port_capability(xhci, portnum,
-                                                            XHCI_BLC))
-                                udev->usb2_hw_lpm_besl_capable = 1;
-                        ret = xhci_set_usb2_hardware_lpm(hcd, udev, 1);
-                        if (!ret)
-                                udev->usb2_hw_lpm_enabled = 1;
-                }
+        if (hcd->speed == HCD_USB3 || !xhci->sw_lpm_support ||
+                        !udev->lpm_capable)
+                return 0;
+
+        /* we only support lpm for non-hub device connected to root hub yet */
+        if (!udev->parent || udev->parent->parent ||
+                        udev->descriptor.bDeviceClass == USB_CLASS_HUB)
+                return 0;
+
+        if (xhci->hw_lpm_support == 1 &&
+                        xhci_check_usb2_port_capability(
+                                xhci, portnum, XHCI_HLC)) {
+                udev->usb2_hw_lpm_capable = 1;
+                udev->l1_params.timeout = XHCI_L1_TIMEOUT;
+                udev->l1_params.besl = XHCI_DEFAULT_BESL;
+                if (xhci_check_usb2_port_capability(xhci, portnum,
+                                        XHCI_BLC))
+                        udev->usb2_hw_lpm_besl_capable = 1;
         }
 
         return 0;
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 941d5f59e4dc..03c74b7965f8 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -383,6 +383,7 @@ struct xhci_op_regs {
 #define PORT_RWE                (1 << 3)
 #define PORT_HIRD(p)            (((p) & 0xf) << 4)
 #define PORT_HIRD_MASK          (0xf << 4)
+#define PORT_L1DS_MASK          (0xff << 8)
 #define PORT_L1DS(p)            (((p) & 0xff) << 8)
 #define PORT_HLE                (1 << 16)
 
@@ -934,8 +935,6 @@ struct xhci_virt_device {
         /* Rings saved to ensure old alt settings can be re-instated */
         struct xhci_ring                **ring_cache;
         int                             num_rings_cached;
-        /* Store xHC assigned device address */
-        int                             address;
 #define XHCI_MAX_RINGS_CACHED   31
         struct xhci_virt_ep             eps[31];
         struct completion               cmd_completion;
diff --git a/include/linux/usb.h b/include/linux/usb.h
index 055ba74bee80..7454865ad148 100644
--- a/include/linux/usb.h
+++ b/include/linux/usb.h
@@ -475,7 +475,8 @@ struct usb3_lpm_parameters {
  * @lpm_capable: device supports LPM
  * @usb2_hw_lpm_capable: device can perform USB2 hardware LPM
  * @usb2_hw_lpm_besl_capable: device can perform USB2 hardware BESL LPM
- * @usb2_hw_lpm_enabled: USB2 hardware LPM enabled
+ * @usb2_hw_lpm_enabled: USB2 hardware LPM is enabled
+ * @usb2_hw_lpm_allowed: Userspace allows USB 2.0 LPM to be enabled
  * @usb3_lpm_enabled: USB3 hardware LPM enabled
  * @string_langid: language ID for strings
  * @product: iProduct string, if present (static)
@@ -548,6 +549,7 @@ struct usb_device {
         unsigned usb2_hw_lpm_capable:1;
         unsigned usb2_hw_lpm_besl_capable:1;
         unsigned usb2_hw_lpm_enabled:1;
+        unsigned usb2_hw_lpm_allowed:1;
         unsigned usb3_lpm_enabled:1;
         int string_langid;