1 files changed, 103 insertions, 31 deletions

diff --git a/hw/usb/hcd-xhci.c b/hw/usb/hcd-xhci.c
index 7b65741dd5..900abf5566 100644
--- a/hw/usb/hcd-xhci.c
+++ b/hw/usb/hcd-xhci.c
@@ -146,6 +146,21 @@ typedef struct XHCITRB {
     bool ccs;
 } XHCITRB;
 
+enum {
+    PLS_U0              =  0,
+    PLS_U1              =  1,
+    PLS_U2              =  2,
+    PLS_U3              =  3,
+    PLS_DISABLED        =  4,
+    PLS_RX_DETECT       =  5,
+    PLS_INACTIVE        =  6,
+    PLS_POLLING         =  7,
+    PLS_RECOVERY        =  8,
+    PLS_HOT_RESET       =  9,
+    PLS_COMPILANCE_MODE = 10,
+    PLS_TEST_MODE       = 11,
+    PLS_RESUME          = 15,
+};
 
 typedef enum TRBType {
     TRB_RESERVED = 0,
@@ -287,6 +302,16 @@ typedef enum TRBCCode {
 
 typedef struct XHCIState XHCIState;
 
+#define get_field(data, field)                  \
+    (((data) >> field##_SHIFT) & field##_MASK)
+
+#define set_field(data, newval, field) do {                     \
+        uint32_t val = *data;                                   \
+        val &= ~(field##_MASK << field##_SHIFT);                \
+        val |= ((newval) & field##_MASK) << field##_SHIFT;      \
+        *data = val;                                            \
+    } while (0)
+
 typedef enum EPType {
     ET_INVALID = 0,
     ET_ISO_OUT,
@@ -458,6 +483,8 @@ enum xhci_flags {
 
 static void xhci_kick_ep(XHCIState *xhci, unsigned int slotid,
                          unsigned int epid);
+static TRBCCode xhci_disable_ep(XHCIState *xhci, unsigned int slotid,
+                                unsigned int epid);
 static void xhci_event(XHCIState *xhci, XHCIEvent *event, int v);
 static void xhci_write_event(XHCIState *xhci, XHCIEvent *event, int v);
 
@@ -1050,8 +1077,7 @@ static TRBCCode xhci_enable_ep(XHCIState *xhci, unsigned int slotid,
 
     slot = &xhci->slots[slotid-1];
     if (slot->eps[epid-1]) {
-        fprintf(stderr, "xhci: slot %d ep %d already enabled!\n", slotid, epid);
-        return CC_TRB_ERROR;
+        xhci_disable_ep(xhci, slotid, epid);
     }
 
     epctx = g_malloc(sizeof(XHCIEPContext));
@@ -1894,6 +1920,9 @@ static TRBCCode xhci_address_slot(XHCIState *xhci, unsigned int slotid,
     }
 
     for (i = 0; i < xhci->numslots; i++) {
+        if (i == slotid-1) {
+            continue;
+        }
         if (xhci->slots[i].uport == uport) {
             fprintf(stderr, "xhci: port %s already assigned to slot %d\n",
                     uport->path, i+1);
@@ -1911,6 +1940,7 @@ static TRBCCode xhci_address_slot(XHCIState *xhci, unsigned int slotid,
         slot->devaddr = xhci->devaddr++;
         slot_ctx[3] = (SLOT_ADDRESSED << SLOT_STATE_SHIFT) | slot->devaddr;
         DPRINTF("xhci: device address is %d\n", slot->devaddr);
+        usb_device_reset(dev);
         usb_device_handle_control(dev, NULL,
                                   DeviceOutRequest | USB_REQ_SET_ADDRESS,
                                   slot->devaddr, 0, 0, NULL);
@@ -2312,35 +2342,86 @@ static void xhci_process_commands(XHCIState *xhci)
     }
 }
 
-static void xhci_update_port(XHCIState *xhci, XHCIPort *port, int is_detach)
+static bool xhci_port_have_device(XHCIPort *port)
+{
+    if (!port->uport->dev || !port->uport->dev->attached) {
+        return false; /* no device present */
+    }
+    if (!((1 << port->uport->dev->speed) & port->speedmask)) {
+        return false; /* speed mismatch */
+    }
+    return true;
+}
+
+static void xhci_port_notify(XHCIPort *port, uint32_t bits)
 {
+    XHCIEvent ev = { ER_PORT_STATUS_CHANGE, CC_SUCCESS,
+                     port->portnr << 24 };
+
+    if ((port->portsc & bits) == bits) {
+        return;
+    }
+    port->portsc |= bits;
+    if (!xhci_running(port->xhci)) {
+        return;
+    }
+    xhci_event(port->xhci, &ev, 0);
+}
+
+static void xhci_port_update(XHCIPort *port, int is_detach)
+{
+    uint32_t pls = PLS_RX_DETECT;
+
     port->portsc = PORTSC_PP;
-    if (port->uport->dev && port->uport->dev->attached && !is_detach &&
-        (1 << port->uport->dev->speed) & port->speedmask) {
+    if (!is_detach && xhci_port_have_device(port)) {
         port->portsc |= PORTSC_CCS;
         switch (port->uport->dev->speed) {
         case USB_SPEED_LOW:
             port->portsc |= PORTSC_SPEED_LOW;
+            pls = PLS_POLLING;
             break;
         case USB_SPEED_FULL:
             port->portsc |= PORTSC_SPEED_FULL;
+            pls = PLS_POLLING;
             break;
         case USB_SPEED_HIGH:
             port->portsc |= PORTSC_SPEED_HIGH;
+            pls = PLS_POLLING;
             break;
         case USB_SPEED_SUPER:
             port->portsc |= PORTSC_SPEED_SUPER;
+            port->portsc |= PORTSC_PED;
+            pls = PLS_U0;
             break;
         }
     }
+    set_field(&port->portsc, pls, PORTSC_PLS);
+    trace_usb_xhci_port_link(port->portnr, pls);
+    xhci_port_notify(port, PORTSC_CSC);
+}
+
+static void xhci_port_reset(XHCIPort *port)
+{
+    trace_usb_xhci_port_reset(port->portnr);
 
-    if (xhci_running(xhci)) {
-        port->portsc |= PORTSC_CSC;
-        XHCIEvent ev = { ER_PORT_STATUS_CHANGE, CC_SUCCESS,
-                         port->portnr << 24};
-        xhci_event(xhci, &ev, 0);
-        DPRINTF("xhci: port change event for port %d\n", port->portnr);
+    if (!xhci_port_have_device(port)) {
+        return;
     }
+
+    usb_device_reset(port->uport->dev);
+
+    switch (port->uport->dev->speed) {
+    case USB_SPEED_LOW:
+    case USB_SPEED_FULL:
+    case USB_SPEED_HIGH:
+        set_field(&port->portsc, PLS_U0, PORTSC_PLS);
+        trace_usb_xhci_port_link(port->portnr, PLS_U0);
+        port->portsc |= PORTSC_PED;
+        break;
+    }
+
+    port->portsc &= ~PORTSC_PR;
+    xhci_port_notify(port, PORTSC_PRC);
 }
 
 static void xhci_reset(DeviceState *dev)
@@ -2368,7 +2449,7 @@ static void xhci_reset(DeviceState *dev)
     }
 
     for (i = 0; i < xhci->numports; i++) {
-        xhci_update_port(xhci, xhci->ports + i, 0);
+        xhci_port_update(xhci->ports + i, 0);
     }
 
     for (i = 0; i < xhci->numintrs; i++) {
@@ -2499,19 +2580,18 @@ static void xhci_port_write(void *ptr, hwaddr reg,
                            PORTSC_PRC|PORTSC_PLC|PORTSC_CEC));
         if (val & PORTSC_LWS) {
             /* overwrite PLS only when LWS=1 */
-            portsc &= ~(PORTSC_PLS_MASK << PORTSC_PLS_SHIFT);
-            portsc |= val & (PORTSC_PLS_MASK << PORTSC_PLS_SHIFT);
+            uint32_t pls = get_field(val, PORTSC_PLS);
+            set_field(&portsc, pls, PORTSC_PLS);
+            trace_usb_xhci_port_link(port->portnr, pls);
         }
         /* read/write bits */
         portsc &= ~(PORTSC_PP|PORTSC_WCE|PORTSC_WDE|PORTSC_WOE);
         portsc |= (val & (PORTSC_PP|PORTSC_WCE|PORTSC_WDE|PORTSC_WOE));
+        port->portsc = portsc;
         /* write-1-to-start bits */
         if (val & PORTSC_PR) {
-            DPRINTF("xhci: port %d reset\n", port);
-            usb_device_reset(port->uport->dev);
-            portsc |= PORTSC_PRC | PORTSC_PED;
+            xhci_port_reset(port);
         }
-        port->portsc = portsc;
         break;
     case 0x04: /* PORTPMSC */
     case 0x08: /* PORTLI */
@@ -2815,7 +2895,7 @@ static void xhci_attach(USBPort *usbport)
     XHCIState *xhci = usbport->opaque;
     XHCIPort *port = xhci_lookup_port(xhci, usbport);
 
-    xhci_update_port(xhci, port, 0);
+    xhci_port_update(port, 0);
 }
 
 static void xhci_detach(USBPort *usbport)
@@ -2823,27 +2903,19 @@ static void xhci_detach(USBPort *usbport)
     XHCIState *xhci = usbport->opaque;
     XHCIPort *port = xhci_lookup_port(xhci, usbport);
 
-    xhci_update_port(xhci, port, 1);
+    xhci_port_update(port, 1);
 }
 
 static void xhci_wakeup(USBPort *usbport)
 {
     XHCIState *xhci = usbport->opaque;
     XHCIPort *port = xhci_lookup_port(xhci, usbport);
-    XHCIEvent ev = { ER_PORT_STATUS_CHANGE, CC_SUCCESS,
-                     port->portnr << 24};
-    uint32_t pls;
 
-    pls = (port->portsc >> PORTSC_PLS_SHIFT) & PORTSC_PLS_MASK;
-    if (pls != 3) {
-        return;
-    }
-    port->portsc |= 0xf << PORTSC_PLS_SHIFT;
-    if (port->portsc & PORTSC_PLC) {
+    if (get_field(port->portsc, PORTSC_PLS) != PLS_U3) {
         return;
     }
-    port->portsc |= PORTSC_PLC;
-    xhci_event(xhci, &ev, 0);
+    set_field(&port->portsc, PLS_RESUME, PORTSC_PLS);
+    xhci_port_notify(port, PORTSC_PLC);
 }
 
 static void xhci_complete(USBPort *port, USBPacket *packet)