xhci: Add a global command queue

[firefly-linux-kernel-4.4.55.git] / drivers / usb / host / xhci-ring.c

diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index b172a7dee6ac9d8d7737995f34eb4d8bfd512557..89b87456754809367cdd7e7643018a664db4427c 100644 (file)
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -1520,6 +1520,20 @@ static void xhci_handle_cmd_nec_get_fw(struct xhci_hcd *xhci,
                        NEC_FW_MINOR(le32_to_cpu(event->status)));
 }
 
+static void xhci_del_and_free_cmd(struct xhci_command *cmd)
+{
+       list_del(&cmd->cmd_list);
+       if (!cmd->completion)
+               kfree(cmd);
+}
+
+void xhci_cleanup_command_queue(struct xhci_hcd *xhci)
+{
+       struct xhci_command *cur_cmd, *tmp_cmd;
+       list_for_each_entry_safe(cur_cmd, tmp_cmd, &xhci->cmd_list, cmd_list)
+               xhci_del_and_free_cmd(cur_cmd);
+}
+
 static void handle_cmd_completion(struct xhci_hcd *xhci,
                struct xhci_event_cmd *event)
 {
@@ -1528,6 +1542,7 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
        dma_addr_t cmd_dequeue_dma;
        u32 cmd_comp_code;
        union xhci_trb *cmd_trb;
+       struct xhci_command *cmd;
        u32 cmd_type;
 
        cmd_dma = le64_to_cpu(event->cmd_trb);
@@ -1545,6 +1560,13 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
                return;
        }
 
+       cmd = list_entry(xhci->cmd_list.next, struct xhci_command, cmd_list);
+
+       if (cmd->command_trb != xhci->cmd_ring->dequeue) {
+               xhci_err(xhci,
+                        "Command completion event does not match command\n");
+               return;
+       }
        trace_xhci_cmd_completion(cmd_trb, (struct xhci_generic_trb *) event);
 
        cmd_comp_code = GET_COMP_CODE(le32_to_cpu(event->status));
@@ -1614,6 +1636,9 @@ static void handle_cmd_completion(struct xhci_hcd *xhci,
                xhci->error_bitmask |= 1 << 6;
                break;
        }
+
+       xhci_del_and_free_cmd(cmd);
+
        inc_deq(xhci, xhci->cmd_ring);
 }
 
@@ -3998,6 +4023,8 @@ static int queue_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
 {
        int reserved_trbs = xhci->cmd_ring_reserved_trbs;
        int ret;
+       if (xhci->xhc_state & XHCI_STATE_DYING)
+               return -ESHUTDOWN;
 
        if (!command_must_succeed)
                reserved_trbs++;
@@ -4011,10 +4038,9 @@ static int queue_command(struct xhci_hcd *xhci, struct xhci_command *cmd,
                                        "unfailable commands failed.\n");
                return ret;
        }
-       if (cmd->completion)
-               cmd->command_trb = xhci->cmd_ring->enqueue;
-       else
-               kfree(cmd);
+
+       cmd->command_trb = xhci->cmd_ring->enqueue;
+       list_add_tail(&cmd->cmd_list, &xhci->cmd_list);
 
        queue_trb(xhci, xhci->cmd_ring, false, field1, field2, field3,
                        field4 | xhci->cmd_ring->cycle_state);