usb-xhci: usb3 streams

Add streams support to the xhci emulation.  No secondary streams yet,
only linear stream arays are supported for now.

Signed-off-by: Gerd Hoffmann <kraxel@redhat.com>

hw/usb/hcd-xhci.c

@@ -34,8 +34,8 @@
 #else
 #define DPRINTF(...) do {} while (0)
 #endif
-#define FIXME() do { fprintf(stderr, "FIXME %s:%d\n", \
+#define FIXME(_msg) do { fprintf(stderr, "FIXME %s:%d %s\n", \
-                             __func__, __LINE__); abort(); } while (0)
+                                 __func__, __LINE__, _msg); abort(); } while (0)
 
 #define MAXPORTS_2 15
 #define MAXPORTS_3 15
@@ -301,6 +301,8 @@ typedef enum TRBCCode {
 #define SLOT_CONTEXT_ENTRIES_SHIFT 27
 
 typedef struct XHCIState XHCIState;
+typedef struct XHCIStreamContext XHCIStreamContext;
+typedef struct XHCIEPContext XHCIEPContext;
 
 #define get_field(data, field)                  \
     (((data) >> field##_SHIFT) & field##_MASK)
@@ -351,6 +353,7 @@ typedef struct XHCITransfer {
     unsigned int iso_pkts;
     unsigned int slotid;
     unsigned int epid;
+    unsigned int streamid;
     bool in_xfer;
     bool iso_xfer;
 
@@ -367,7 +370,14 @@ typedef struct XHCITransfer {
     uint64_t mfindex_kick;
 } XHCITransfer;
 
-typedef struct XHCIEPContext {
+struct XHCIStreamContext {
+    dma_addr_t pctx;
+    unsigned int sct;
+    XHCIRing ring;
+    XHCIStreamContext *sstreams;
+};
+
+struct XHCIEPContext {
     XHCIState *xhci;
     unsigned int slotid;
     unsigned int epid;
@@ -382,11 +392,17 @@ typedef struct XHCIEPContext {
     unsigned int max_psize;
     uint32_t state;
 
+    /* streams */
+    unsigned int max_pstreams;
+    bool         lsa;
+    unsigned int nr_pstreams;
+    XHCIStreamContext *pstreams;
+
     /* iso xfer scheduling */
     unsigned int interval;
     int64_t mfindex_last;
     QEMUTimer *kick_timer;
-} XHCIEPContext;
+};
 
 typedef struct XHCISlot {
     bool enabled;
@@ -482,7 +498,7 @@ enum xhci_flags {
 };
 
 static void xhci_kick_ep(XHCIState *xhci, unsigned int slotid,
-                         unsigned int epid);
+                         unsigned int epid, unsigned int streamid);
 static TRBCCode xhci_disable_ep(XHCIState *xhci, unsigned int slotid,
                                 unsigned int epid);
 static void xhci_event(XHCIState *xhci, XHCIEvent *event, int v);
@@ -1068,18 +1084,116 @@ static void xhci_stop(XHCIState *xhci)
     xhci->crcr_low &= ~CRCR_CRR;
 }
 
+static XHCIStreamContext *xhci_alloc_stream_contexts(unsigned count,
+                                                     dma_addr_t base)
+{
+    XHCIStreamContext *stctx;
+    unsigned int i;
+
+    stctx = g_new0(XHCIStreamContext, count);
+    for (i = 0; i < count; i++) {
+        stctx[i].pctx = base + i * 16;
+        stctx[i].sct = -1;
+    }
+    return stctx;
+}
+
+static void xhci_reset_streams(XHCIEPContext *epctx)
+{
+    unsigned int i;
+
+    for (i = 0; i < epctx->nr_pstreams; i++) {
+        epctx->pstreams[i].sct = -1;
+        g_free(epctx->pstreams[i].sstreams);
+    }
+}
+
+static void xhci_alloc_streams(XHCIEPContext *epctx, dma_addr_t base)
+{
+    assert(epctx->pstreams == NULL);
+    epctx->nr_pstreams = 2 << epctx->max_pstreams;
+    epctx->pstreams = xhci_alloc_stream_contexts(epctx->nr_pstreams, base);
+}
+
+static void xhci_free_streams(XHCIEPContext *epctx)
+{
+    int i;
+
+    assert(epctx->pstreams != NULL);
+
+    if (!epctx->lsa) {
+        for (i = 0; i < epctx->nr_pstreams; i++) {
+            g_free(epctx->pstreams[i].sstreams);
+        }
+    }
+    g_free(epctx->pstreams);
+    epctx->pstreams = NULL;
+    epctx->nr_pstreams = 0;
+}
+
+static XHCIStreamContext *xhci_find_stream(XHCIEPContext *epctx,
+                                           unsigned int streamid,
+                                           uint32_t *cc_error)
+{
+    XHCIStreamContext *sctx;
+    dma_addr_t base;
+    uint32_t ctx[2], sct;
+
+    assert(streamid != 0);
+    if (epctx->lsa) {
+        if (streamid >= epctx->nr_pstreams) {
+            *cc_error = CC_INVALID_STREAM_ID_ERROR;
+            return NULL;
+        }
+        sctx = epctx->pstreams + streamid;
+    } else {
+        FIXME("secondary streams not implemented yet");
+    }
+
+    if (sctx->sct == -1) {
+        xhci_dma_read_u32s(epctx->xhci, sctx->pctx, ctx, sizeof(ctx));
+        fprintf(stderr, "%s: init sctx #%d @ %lx: %08x %08x\n", __func__,
+                streamid, sctx->pctx, ctx[0], ctx[1]);
+        sct = (ctx[0] >> 1) & 0x07;
+        if (epctx->lsa && sct != 1) {
+            *cc_error = CC_INVALID_STREAM_TYPE_ERROR;
+            return NULL;
+        }
+        sctx->sct = sct;
+        base = xhci_addr64(ctx[0] & ~0xf, ctx[1]);
+        xhci_ring_init(epctx->xhci, &sctx->ring, base);
+    }
+    return sctx;
+}
+
 static void xhci_set_ep_state(XHCIState *xhci, XHCIEPContext *epctx,
-                              uint32_t state)
+                              XHCIStreamContext *sctx, uint32_t state)
 {
     uint32_t ctx[5];
+    uint32_t ctx2[2];
 
+    fprintf(stderr, "%s: epid %d, state %d\n",
+            __func__, epctx->epid, state);
     xhci_dma_read_u32s(xhci, epctx->pctx, ctx, sizeof(ctx));
     ctx[0] &= ~EP_STATE_MASK;
     ctx[0] |= state;
+
+    /* update ring dequeue ptr */
+    if (epctx->nr_pstreams) {
+        if (sctx != NULL) {
+            xhci_dma_read_u32s(xhci, sctx->pctx, ctx2, sizeof(ctx2));
+            ctx2[0] &= 0xe;
+            ctx2[0] |= sctx->ring.dequeue | sctx->ring.ccs;
+            ctx2[1] = (sctx->ring.dequeue >> 16) >> 16;
+            xhci_dma_write_u32s(xhci, sctx->pctx, ctx2, sizeof(ctx2));
+        }
+    } else {
         ctx[2] = epctx->ring.dequeue | epctx->ring.ccs;
         ctx[3] = (epctx->ring.dequeue >> 16) >> 16;
         DPRINTF("xhci: set epctx: " DMA_ADDR_FMT " state=%d dequeue=%08x%08x\n",
                 epctx->pctx, state, ctx[3], ctx[2]);
+    }
+
     xhci_dma_write_u32s(xhci, epctx->pctx, ctx, sizeof(ctx));
     epctx->state = state;
 }
@@ -1087,7 +1201,7 @@ static void xhci_set_ep_state(XHCIState *xhci, XHCIEPContext *epctx,
 static void xhci_ep_kick_timer(void *opaque)
 {
     XHCIEPContext *epctx = opaque;
-    xhci_kick_ep(epctx->xhci, epctx->slotid, epctx->epid);
+    xhci_kick_ep(epctx->xhci, epctx->slotid, epctx->epid, 0);
 }
 
 static TRBCCode xhci_enable_ep(XHCIState *xhci, unsigned int slotid,
@@ -1117,16 +1231,22 @@ static TRBCCode xhci_enable_ep(XHCIState *xhci, unsigned int slotid,
     slot->eps[epid-1] = epctx;
 
     dequeue = xhci_addr64(ctx[2] & ~0xf, ctx[3]);
-    xhci_ring_init(xhci, &epctx->ring, dequeue);
-    epctx->ring.ccs = ctx[2] & 1;
 
     epctx->type = (ctx[1] >> EP_TYPE_SHIFT) & EP_TYPE_MASK;
     DPRINTF("xhci: endpoint %d.%d type is %d\n", epid/2, epid%2, epctx->type);
     epctx->pctx = pctx;
     epctx->max_psize = ctx[1]>>16;
     epctx->max_psize *= 1+((ctx[1]>>8)&0xff);
+    epctx->max_pstreams = (ctx[0] >> 10) & 0xf;
+    epctx->lsa = (ctx[0] >> 15) & 1;
     DPRINTF("xhci: endpoint %d.%d max transaction (burst) size is %d\n",
             epid/2, epid%2, epctx->max_psize);
+    if (epctx->max_pstreams) {
+        xhci_alloc_streams(epctx, dequeue);
+    } else {
+        xhci_ring_init(xhci, &epctx->ring, dequeue);
+        epctx->ring.ccs = ctx[2] & 1;
+    }
     for (i = 0; i < ARRAY_SIZE(epctx->transfers); i++) {
         usb_packet_init(&epctx->transfers[i].packet);
     }
@@ -1227,7 +1347,11 @@ static TRBCCode xhci_disable_ep(XHCIState *xhci, unsigned int slotid,
 
     epctx = slot->eps[epid-1];
 
-    xhci_set_ep_state(xhci, epctx, EP_DISABLED);
+    if (epctx->nr_pstreams) {
+        xhci_free_streams(epctx);
+    }
+
+    xhci_set_ep_state(xhci, epctx, NULL, EP_DISABLED);
 
     qemu_free_timer(epctx->kick_timer);
     g_free(epctx);
@@ -1264,7 +1388,11 @@ static TRBCCode xhci_stop_ep(XHCIState *xhci, unsigned int slotid,
 
     epctx = slot->eps[epid-1];
 
-    xhci_set_ep_state(xhci, epctx, EP_STOPPED);
+    xhci_set_ep_state(xhci, epctx, NULL, EP_STOPPED);
+
+    if (epctx->nr_pstreams) {
+        xhci_reset_streams(epctx);
+    }
 
     return CC_SUCCESS;
 }
@@ -1315,16 +1443,22 @@ static TRBCCode xhci_reset_ep(XHCIState *xhci, unsigned int slotid,
         return CC_USB_TRANSACTION_ERROR;
     }
 
-    xhci_set_ep_state(xhci, epctx, EP_STOPPED);
+    xhci_set_ep_state(xhci, epctx, NULL, EP_STOPPED);
+
+    if (epctx->nr_pstreams) {
+        xhci_reset_streams(epctx);
+    }
 
     return CC_SUCCESS;
 }
 
 static TRBCCode xhci_set_ep_dequeue(XHCIState *xhci, unsigned int slotid,
-                                    unsigned int epid, uint64_t pdequeue)
+                                    unsigned int epid, unsigned int streamid,
+                                    uint64_t pdequeue)
 {
     XHCISlot *slot;
     XHCIEPContext *epctx;
+    XHCIStreamContext *sctx;
     dma_addr_t dequeue;
 
     assert(slotid >= 1 && slotid <= xhci->numslots);
@@ -1334,7 +1468,7 @@ static TRBCCode xhci_set_ep_dequeue(XHCIState *xhci, unsigned int slotid,
         return CC_TRB_ERROR;
     }
 
-    trace_usb_xhci_ep_set_dequeue(slotid, epid, pdequeue);
+    trace_usb_xhci_ep_set_dequeue(slotid, epid, streamid, pdequeue);
     dequeue = xhci_mask64(pdequeue);
 
     slot = &xhci->slots[slotid-1];
@@ -1346,16 +1480,26 @@ static TRBCCode xhci_set_ep_dequeue(XHCIState *xhci, unsigned int slotid,
 
     epctx = slot->eps[epid-1];
 
-
     if (epctx->state != EP_STOPPED) {
         fprintf(stderr, "xhci: set EP dequeue pointer while EP %d not stopped\n", epid);
         return CC_CONTEXT_STATE_ERROR;
     }
 
+    if (epctx->nr_pstreams) {
+        uint32_t err;
+        sctx = xhci_find_stream(epctx, streamid, &err);
+        if (sctx == NULL) {
+            return err;
+        }
+        xhci_ring_init(xhci, &sctx->ring, dequeue & ~0xf);
+        sctx->ring.ccs = dequeue & 1;
+    } else {
+        sctx = NULL;
         xhci_ring_init(xhci, &epctx->ring, dequeue & ~0xF);
         epctx->ring.ccs = dequeue & 1;
+    }
 
-    xhci_set_ep_state(xhci, epctx, EP_STOPPED);
+    xhci_set_ep_state(xhci, epctx, sctx, EP_STOPPED);
 
     return CC_SUCCESS;
 }
@@ -1484,12 +1628,22 @@ static void xhci_stall_ep(XHCITransfer *xfer)
     XHCIState *xhci = xfer->xhci;
     XHCISlot *slot = &xhci->slots[xfer->slotid-1];
     XHCIEPContext *epctx = slot->eps[xfer->epid-1];
+    uint32_t err;
+    XHCIStreamContext *sctx;
 
+    if (epctx->nr_pstreams) {
+        sctx = xhci_find_stream(epctx, xfer->streamid, &err);
+        if (sctx == NULL) {
+            return;
+        }
+        sctx->ring.dequeue = xfer->trbs[0].addr;
+        sctx->ring.ccs = xfer->trbs[0].ccs;
+        xhci_set_ep_state(xhci, epctx, sctx, EP_HALTED);
+    } else {
         epctx->ring.dequeue = xfer->trbs[0].addr;
         epctx->ring.ccs = xfer->trbs[0].ccs;
-    xhci_set_ep_state(xhci, epctx, EP_HALTED);
+        xhci_set_ep_state(xhci, epctx, NULL, EP_HALTED);
-    DPRINTF("xhci: stalled slot %d ep %d\n", xfer->slotid, xfer->epid);
+    }
-    DPRINTF("xhci: will continue at "DMA_ADDR_FMT"\n", epctx->ring.dequeue);
 }
 
 static int xhci_submit(XHCIState *xhci, XHCITransfer *xfer,
@@ -1518,7 +1672,7 @@ static int xhci_setup_packet(XHCITransfer *xfer)
     }
 
     xhci_xfer_create_sgl(xfer, dir == USB_TOKEN_IN); /* Also sets int_req */
-    usb_packet_setup(&xfer->packet, dir, ep, 0,
+    usb_packet_setup(&xfer->packet, dir, ep, xfer->streamid,
                      xfer->trbs[0].addr, false, xfer->int_req);
     usb_packet_map(&xfer->packet, &xfer->sgl);
     DPRINTF("xhci: setup packet pid 0x%x addr %d ep %d\n",
@@ -1572,7 +1726,7 @@ static int xhci_complete_packet(XHCITransfer *xfer)
     default:
         fprintf(stderr, "%s: FIXME: status = %d\n", __func__,
                 xfer->packet.status);
-        FIXME();
+        FIXME("unhandled USB_RET_*");
     }
     return 0;
 }
@@ -1585,7 +1739,7 @@ static int xhci_fire_ctl_transfer(XHCIState *xhci, XHCITransfer *xfer)
     trb_setup = &xfer->trbs[0];
     trb_status = &xfer->trbs[xfer->trb_count-1];
 
-    trace_usb_xhci_xfer_start(xfer, xfer->slotid, xfer->epid);
+    trace_usb_xhci_xfer_start(xfer, xfer->slotid, xfer->epid, xfer->streamid);
 
     /* at most one Event Data TRB allowed after STATUS */
     if (TRB_TYPE(*trb_status) == TR_EVDATA && xfer->trb_count > 2) {
@@ -1627,7 +1781,7 @@ static int xhci_fire_ctl_transfer(XHCIState *xhci, XHCITransfer *xfer)
 
     xhci_complete_packet(xfer);
     if (!xfer->running_async && !xfer->running_retry) {
-        xhci_kick_ep(xhci, xfer->slotid, xfer->epid);
+        xhci_kick_ep(xhci, xfer->slotid, xfer->epid, 0);
     }
     return 0;
 }
@@ -1710,26 +1864,29 @@ static int xhci_submit(XHCIState *xhci, XHCITransfer *xfer, XHCIEPContext *epctx
 
     xhci_complete_packet(xfer);
     if (!xfer->running_async && !xfer->running_retry) {
-        xhci_kick_ep(xhci, xfer->slotid, xfer->epid);
+        xhci_kick_ep(xhci, xfer->slotid, xfer->epid, xfer->streamid);
     }
     return 0;
 }
 
 static int xhci_fire_transfer(XHCIState *xhci, XHCITransfer *xfer, XHCIEPContext *epctx)
 {
-    trace_usb_xhci_xfer_start(xfer, xfer->slotid, xfer->epid);
+    trace_usb_xhci_xfer_start(xfer, xfer->slotid, xfer->epid, xfer->streamid);
     return xhci_submit(xhci, xfer, epctx);
 }
 
-static void xhci_kick_ep(XHCIState *xhci, unsigned int slotid, unsigned int epid)
+static void xhci_kick_ep(XHCIState *xhci, unsigned int slotid,
+                         unsigned int epid, unsigned int streamid)
 {
+    XHCIStreamContext *stctx;
     XHCIEPContext *epctx;
+    XHCIRing *ring;
     USBEndpoint *ep = NULL;
     uint64_t mfindex;
     int length;
     int i;
 
-    trace_usb_xhci_ep_kick(slotid, epid);
+    trace_usb_xhci_ep_kick(slotid, epid, streamid);
     assert(slotid >= 1 && slotid <= xhci->numslots);
     assert(epid >= 1 && epid <= 31);
 
@@ -1782,14 +1939,28 @@ static void xhci_kick_ep(XHCIState *xhci, unsigned int slotid, unsigned int epid
         return;
     }
 
-    xhci_set_ep_state(xhci, epctx, EP_RUNNING);
+
+    if (epctx->nr_pstreams) {
+        uint32_t err;
+        stctx = xhci_find_stream(epctx, streamid, &err);
+        if (stctx == NULL) {
+            return;
+        }
+        ring = &stctx->ring;
+        xhci_set_ep_state(xhci, epctx, stctx, EP_RUNNING);
+    } else {
+        ring = &epctx->ring;
+        streamid = 0;
+        xhci_set_ep_state(xhci, epctx, NULL, EP_RUNNING);
+    }
+    assert(ring->base != 0);
 
     while (1) {
         XHCITransfer *xfer = &epctx->transfers[epctx->next_xfer];
         if (xfer->running_async || xfer->running_retry) {
             break;
         }
-        length = xhci_ring_chain_length(xhci, &epctx->ring);
+        length = xhci_ring_chain_length(xhci, ring);
         if (length < 0) {
             break;
         } else if (length == 0) {
@@ -1808,11 +1979,12 @@ static void xhci_kick_ep(XHCIState *xhci, unsigned int slotid, unsigned int epid
         xfer->trb_count = length;
 
         for (i = 0; i < length; i++) {
-            assert(xhci_ring_fetch(xhci, &epctx->ring, &xfer->trbs[i], NULL));
+            assert(xhci_ring_fetch(xhci, ring, &xfer->trbs[i], NULL));
         }
         xfer->xhci = xhci;
         xfer->epid = epid;
         xfer->slotid = slotid;
+        xfer->streamid = streamid;
 
         if (epid == 1) {
             if (xhci_fire_ctl_transfer(xhci, xfer) >= 0) {
@@ -2357,11 +2529,14 @@ static void xhci_process_commands(XHCIState *xhci)
             }
             break;
         case CR_SET_TR_DEQUEUE:
+            fprintf(stderr, "%s: CR_SET_TR_DEQUEUE\n", __func__);
             slotid = xhci_get_slot(xhci, &event, &trb);
             if (slotid) {
                 unsigned int epid = (trb.control >> TRB_CR_EPID_SHIFT)
                     & TRB_CR_EPID_MASK;
-                event.ccode = xhci_set_ep_dequeue(xhci, slotid, epid,
+                unsigned int streamid = (trb.status >> 16) & 0xffff;
+                event.ccode = xhci_set_ep_dequeue(xhci, slotid,
+                                                  epid, streamid,
                                                   trb.parameter);
             }
             break;
@@ -2554,9 +2729,9 @@ static uint64_t xhci_cap_read(void *ptr, hwaddr reg, unsigned size)
         break;
     case 0x10: /* HCCPARAMS */
         if (sizeof(dma_addr_t) == 4) {
-            ret = 0x00081000;
+            ret = 0x00087000;
         } else {
-            ret = 0x00081001;
+            ret = 0x00087001;
         }
         break;
     case 0x14: /* DBOFF */
@@ -2880,6 +3055,7 @@ static void xhci_doorbell_write(void *ptr, hwaddr reg,
                                 uint64_t val, unsigned size)
 {
     XHCIState *xhci = ptr;
+    unsigned int epid, streamid;
 
     trace_usb_xhci_doorbell_write(reg, val);
 
@@ -2898,13 +3074,15 @@ static void xhci_doorbell_write(void *ptr, hwaddr reg,
                     (uint32_t)val);
         }
     } else {
+        epid = val & 0xff;
+        streamid = (val >> 16) & 0xffff;
         if (reg > xhci->numslots) {
             fprintf(stderr, "xhci: bad doorbell %d\n", (int)reg);
-        } else if (val > 31) {
+        } else if (epid > 31) {
             fprintf(stderr, "xhci: bad doorbell %d write: 0x%x\n",
                     (int)reg, (uint32_t)val);
         } else {
-            xhci_kick_ep(xhci, reg, val);
+            xhci_kick_ep(xhci, reg, epid, streamid);
         }
     }
 }
@@ -2988,7 +3166,7 @@ static void xhci_complete(USBPort *port, USBPacket *packet)
         return;
     }
     xhci_complete_packet(xfer);
-    xhci_kick_ep(xfer->xhci, xfer->slotid, xfer->epid);
+    xhci_kick_ep(xfer->xhci, xfer->slotid, xfer->epid, xfer->streamid);
 }
 
 static void xhci_child_detach(USBPort *uport, USBDevice *child)
@@ -3045,7 +3223,7 @@ static void xhci_wakeup_endpoint(USBBus *bus, USBEndpoint *ep,
         DPRINTF("%s: oops, no slot for dev %d\n", __func__, ep->dev->addr);
         return;
     }
-    xhci_kick_ep(xhci, slotid, xhci_find_epid(ep));
+    xhci_kick_ep(xhci, slotid, xhci_find_epid(ep), stream);
 }
 
 static USBBusOps xhci_bus_ops = {

trace-events

@@ -370,11 +370,11 @@ usb_xhci_slot_evaluate(uint32_t slotid) "slotid %d"
 usb_xhci_slot_reset(uint32_t slotid) "slotid %d"
 usb_xhci_ep_enable(uint32_t slotid, uint32_t epid) "slotid %d, epid %d"
 usb_xhci_ep_disable(uint32_t slotid, uint32_t epid) "slotid %d, epid %d"
-usb_xhci_ep_set_dequeue(uint32_t slotid, uint32_t epid, uint64_t param) "slotid %d, epid %d, ptr %016" PRIx64
+usb_xhci_ep_set_dequeue(uint32_t slotid, uint32_t epid, uint32_t streamid, uint64_t param) "slotid %d, epid %d, streamid %d, ptr %016" PRIx64
-usb_xhci_ep_kick(uint32_t slotid, uint32_t epid) "slotid %d, epid %d"
+usb_xhci_ep_kick(uint32_t slotid, uint32_t epid, uint32_t streamid) "slotid %d, epid %d, streamid %d"
 usb_xhci_ep_stop(uint32_t slotid, uint32_t epid) "slotid %d, epid %d"
 usb_xhci_ep_reset(uint32_t slotid, uint32_t epid) "slotid %d, epid %d"
-usb_xhci_xfer_start(void *xfer, uint32_t slotid, uint32_t epid) "%p: slotid %d, epid %d"
+usb_xhci_xfer_start(void *xfer, uint32_t slotid, uint32_t epid, uint32_t streamid) "%p: slotid %d, epid %d, streamid %d"
 usb_xhci_xfer_async(void *xfer) "%p"
 usb_xhci_xfer_nak(void *xfer) "%p"
 usb_xhci_xfer_retry(void *xfer) "%p"