Use the limited scatter-gather capabilities of ehci, ohci and uhci

host controllers to avoid the need to allocate any multi-page
physically contiguous memory blocks. This makes it possible to use
USB devices reliably on low-memory systems or when memory is too
fragmented for contiguous allocations to succeed.

The USB subsystem now uses bus_dmamap_load() directly on the buffers
supplied by USB peripheral drivers, so this also avoids having to
copy data back and forth before and after transfers. The ehci and
ohci controllers support scatter/gather as long as the buffer is
contiguous in the virtual address space. For uhci the hardware
cannot handle a physical address discontinuity within a USB packet,
so it is necessary to copy small memory fragments at times.

1 files changed, 241 insertions, 184 deletions

diff --git a/sys/dev/usb/ohci.c b/sys/dev/usb/ohci.c
index 0b84a32..c8e6b17 100644
--- a/sys/dev/usb/ohci.c
+++ b/sys/dev/usb/ohci.c
@@ -217,6 +217,8 @@ Static void		ohci_device_isoc_done(usbd_xfer_handle);
 
 Static usbd_status	ohci_device_setintr(ohci_softc_t *sc,
 			    struct ohci_pipe *pipe, int ival);
+Static usbd_status	ohci_device_intr_insert(ohci_softc_t *sc,
+			    usbd_xfer_handle xfer);
 
 Static int		ohci_str(usb_string_descriptor_t *, int, const char *);
 
@@ -510,11 +512,11 @@ ohci_alloc_std_chain(struct ohci_pipe *opipe, ohci_softc_t *sc,
 		     ohci_soft_td_t *sp, ohci_soft_td_t **ep)
 {
 	ohci_soft_td_t *next, *cur;
-	ohci_physaddr_t dataphys;
+	ohci_physaddr_t dataphys, physend;
 	u_int32_t tdflags;
 	int offset = 0;
-	int len, curlen;
-	usb_dma_t *dma = &xfer->dmabuf;
+	int len, maxp, curlen, curlen2, seg, segoff;
+	struct usb_dma_mapping *dma = &xfer->dmamap;
 	u_int16_t flags = xfer->flags;
 
 	DPRINTFN(alen < 4096,("ohci_alloc_std_chain: start len=%d\n", alen));
@@ -522,22 +524,22 @@ ohci_alloc_std_chain(struct ohci_pipe *opipe, ohci_softc_t *sc,
 	len = alen;
 	cur = sp;
 
+	maxp = UGETW(opipe->pipe.endpoint->edesc->wMaxPacketSize);
 	tdflags = htole32(
 	    (rd ? OHCI_TD_IN : OHCI_TD_OUT) |
 	    (flags & USBD_SHORT_XFER_OK ? OHCI_TD_R : 0) |
 	    OHCI_TD_NOCC | OHCI_TD_TOGGLE_CARRY | OHCI_TD_SET_DI(6));
 
+	seg = 0;
+	segoff = 0;
 	for (;;) {
 		next = ohci_alloc_std(sc);
 		if (next == NULL)
 			goto nomem;
 
-		dataphys = DMAADDR(dma, offset);
-
 		/*
 		 * The OHCI hardware can handle at most one 4k crossing.
-		 * XXX - currently we only allocate contigous buffers, but
-		 * the OHCI spec says: If during the data transfer the buffer
+		 * The OHCI spec says: If during the data transfer the buffer
 		 * address contained in the HC's working copy of
 		 * CurrentBufferPointer crosses a 4K boundary, the upper 20
 		 * bits of Buffer End are copied to the working value of
@@ -545,34 +547,67 @@ ohci_alloc_std_chain(struct ohci_pipe *opipe, ohci_softc_t *sc,
 		 * be the 0th byte in the same 4K page that contains the
 		 * last byte of the buffer (the 4K boundary crossing may
 		 * occur within a data packet transfer.)
-		 *
-		 * If/when dma has multiple segments, this will need to
-		 * properly handle fragmenting TD's.
-		 * 
-		 * Note that if we are gathering data from multiple SMALL
-		 * segments, e.g. mbufs, we need to do special gymnastics,
-		 * e.g. bounce buffering or data aggregation,
-		 * BEFORE WE GET HERE because a bulk USB transfer must
-		 * consist of maximally sized packets right up to the end.
-		 * A shorter than maximal packet means that it is the end
-		 * of the transfer. If the data transfer length is a
-		 * multiple of the packet size, then a 0 byte
-		 * packet will be the signal of the end of transfer.
-		 * Since packets can't cross TDs this means that
-		 * each TD except the last one must cover an exact multiple
-		 * of the maximal packet length.
 		 */
-		if (OHCI_PAGE_OFFSET(dataphys) + len <= (2 * OHCI_PAGE_SIZE)) {
-			/* We can handle all that remains in this TD */
+		KASSERT(seg < dma->nsegs, ("ohci_alloc_std_chain: overrun"));
+		dataphys = dma->segs[seg].ds_addr + segoff;
+		curlen = dma->segs[seg].ds_len - segoff;
+		if (curlen > len)
 			curlen = len;
+		physend = dataphys + curlen - 1;
+		if (OHCI_PAGE(dataphys) != OHCI_PAGE(physend)) {
+			/* Truncate to two OHCI pages if there are more. */
+			if (curlen > 2 * OHCI_PAGE_SIZE -
+			    OHCI_PAGE_OFFSET(dataphys))
+				curlen = 2 * OHCI_PAGE_SIZE -
+				    OHCI_PAGE_OFFSET(dataphys);
+			if (curlen < len)
+				curlen -= curlen % maxp;
+			physend = dataphys + curlen - 1;
+		} else if (OHCI_PAGE_OFFSET(physend + 1) == 0 && curlen < len &&
+		    curlen + segoff == dma->segs[seg].ds_len) {
+			/* We can possibly include another segment. */
+			KASSERT(seg + 1 < dma->nsegs,
+			    ("ohci_alloc_std_chain: overrun2"));
+			seg++;
+
+			/* Determine how much of the second segment to use. */
+			curlen2 = dma->segs[seg].ds_len;
+			if (curlen + curlen2 > len)
+				curlen2 = len - curlen;
+			if (OHCI_PAGE(dma->segs[seg].ds_addr) !=
+			    OHCI_PAGE(dma->segs[seg].ds_addr + curlen2 - 1))
+				curlen2 = OHCI_PAGE_SIZE -
+				    OHCI_PAGE_OFFSET(dma->segs[seg].ds_addr);
+			if (curlen + curlen2 < len)
+				curlen2 -= (curlen + curlen2) % maxp;
+
+			if (curlen2 > 0) {
+				/* We can include a second segment */
+				segoff = curlen2;
+				physend = dma->segs[seg].ds_addr + curlen2 - 1;
+				curlen += curlen2;
+			} else {
+				/* Second segment not usable now. */
+				seg--;
+				segoff += curlen;
+			}
 		} else {
-			/* must use multiple TDs, fill as much as possible. */
-			curlen = 2 * OHCI_PAGE_SIZE -
-				 OHCI_PAGE_OFFSET(dataphys);
-			/* the length must be a multiple of the max size */
-			curlen -= curlen %
-			    UGETW(opipe->pipe.endpoint->edesc->wMaxPacketSize);
-			KASSERT((curlen != 0), ("ohci_alloc_std: curlen == 0"));
+			/* Simple case where there is just one OHCI page. */
+			segoff += curlen;
+		}
+		if (curlen == 0 && len != 0) {
+			/*
+			 * A maxp length packet would need to be split.
+			 * This shouldn't be possible if PAGE_SIZE >= 4k
+			 * and the buffer is contiguous in virtual memory.
+			 */
+			panic("ohci_alloc_std_chain: XXX need to copy");
+		}
+		if (segoff >= dma->segs[seg].ds_len) {
+			KASSERT(segoff == dma->segs[seg].ds_len,
+			    ("ohci_alloc_std_chain: overlap"));
+			seg++;
+			segoff = 0;
 		}
 		DPRINTFN(4,("ohci_alloc_std_chain: dataphys=0x%08x "
 			    "len=%d curlen=%d\n",
@@ -583,7 +618,7 @@ ohci_alloc_std_chain(struct ohci_pipe *opipe, ohci_softc_t *sc,
 		cur->td.td_cbp = htole32(dataphys);
 		cur->nexttd = next;
 		cur->td.td_nexttd = htole32(next->physaddr);
-		cur->td.td_be = htole32(DMAADDR(dma, offset + curlen - 1));
+		cur->td.td_be = htole32(physend);
 		cur->len = curlen;
 		cur->flags = OHCI_ADD_LEN;
 		cur->xfer = xfer;
@@ -1009,14 +1044,6 @@ void
 ohci_freex(struct usbd_bus *bus, usbd_xfer_handle xfer)
 {
 	struct ohci_softc *sc = (struct ohci_softc *)bus;
-	struct ohci_xfer *oxfer = (struct ohci_xfer *)xfer;
-	ohci_soft_itd_t *sitd;
-
-        if (oxfer->ohci_xfer_flags & OHCI_ISOC_DIRTY) {
-		for (sitd = xfer->hcpriv; sitd != NULL && sitd->xfer == xfer;
-		    sitd = sitd->nextitd)
-			ohci_free_sitd(sc, sitd);
-	}
 
 #ifdef DIAGNOSTIC
 	if (xfer->busy_free != XFER_BUSY) {
@@ -1535,6 +1562,11 @@ ohci_softintr(void *v)
 				if (sitd->flags & OHCI_CALL_DONE)
 					break;
 			}
+		 	for (sitd = xfer->hcpriv; sitd->xfer == xfer;
+			    sitd = next) {
+				next = sitd->nextitd;
+				ohci_free_sitd(sc, sitd);
+			}
 
 			s = splusb();
 			usb_transfer_complete(xfer);
@@ -1571,42 +1603,18 @@ ohci_device_intr_done(usbd_xfer_handle xfer)
 {
 	struct ohci_pipe *opipe = (struct ohci_pipe *)xfer->pipe;
 	ohci_softc_t *sc = (ohci_softc_t *)opipe->pipe.device->bus;
-	ohci_soft_ed_t *sed = opipe->sed;
-	ohci_soft_td_t *data, *tail;
-
+	usbd_status err;
 
 	DPRINTFN(10,("ohci_device_intr_done: xfer=%p, actlen=%d\n",
 		     xfer, xfer->actlen));
 
 	xfer->hcpriv = NULL;
-
 	if (xfer->pipe->repeat) {
-		data = opipe->tail.td;
-		tail = ohci_alloc_std(sc); /* XXX should reuse TD */
-		if (tail == NULL) {
-			xfer->status = USBD_NOMEM;
+		err = ohci_device_intr_insert(sc, xfer);
+		if (err) {
+			xfer->status = err;
 			return;
 		}
-		tail->xfer = NULL;
-
-		data->td.td_flags = htole32(
-			OHCI_TD_IN | OHCI_TD_NOCC |
-			OHCI_TD_SET_DI(1) | OHCI_TD_TOGGLE_CARRY);
-		if (xfer->flags & USBD_SHORT_XFER_OK)
-			data->td.td_flags |= htole32(OHCI_TD_R);
-		data->td.td_cbp = htole32(DMAADDR(&xfer->dmabuf, 0));
-		data->nexttd = tail;
-		data->td.td_nexttd = htole32(tail->physaddr);
-		data->td.td_be = htole32(le32toh(data->td.td_cbp) +
-			xfer->length - 1);
-		data->len = xfer->length;
-		data->xfer = xfer;
-		data->flags = OHCI_CALL_DONE | OHCI_ADD_LEN;
-		xfer->hcpriv = data;
-		xfer->actlen = 0;
-
-		sed->ed.ed_tailp = htole32(tail->physaddr);
-		opipe->tail.td = tail;
 	}
 }
 
@@ -1638,7 +1646,7 @@ ohci_rhsc(ohci_softc_t *sc, usbd_xfer_handle xfer)
 
 	pipe = xfer->pipe;
 
-	p = KERNADDR(&xfer->dmabuf, 0);
+	p = xfer->buffer;
 	m = min(sc->sc_noport, xfer->length * 8 - 1);
 	memset(p, 0, xfer->length);
 	for (i = 1; i <= m; i++) {
@@ -2284,6 +2292,7 @@ ohci_abort_xfer(usbd_xfer_handle xfer, usbd_status status)
 		usb_rem_task(xfer->pipe->device, &OXFER(xfer)->abort_task);
 		usb_transfer_complete(xfer);
 		splx(s);
+		return;
 	}
 
 	if (xfer->device->bus->intr_context || !curproc)
@@ -2519,7 +2528,7 @@ ohci_root_ctrl_start(usbd_xfer_handle xfer)
 	index = UGETW(req->wIndex);
 
 	if (len != 0)
-		buf = KERNADDR(&xfer->dmabuf, 0);
+		buf = xfer->buffer;
 
 #define C(x,y) ((x) | ((y) << 8))
 	switch(C(req->bRequest, req->bmRequestType)) {
@@ -3099,26 +3108,44 @@ Static usbd_status
 ohci_device_intr_start(usbd_xfer_handle xfer)
 {
 	struct ohci_pipe *opipe = (struct ohci_pipe *)xfer->pipe;
-	usbd_device_handle dev = opipe->pipe.device;
-	ohci_softc_t *sc = (ohci_softc_t *)dev->bus;
+	ohci_softc_t *sc = (ohci_softc_t *)opipe->pipe.device->bus;
 	ohci_soft_ed_t *sed = opipe->sed;
-	ohci_soft_td_t *data, *tail;
-	int len;
-	int s;
+	usbd_status err;
 
 	if (sc->sc_dying)
 		return (USBD_IOERROR);
 
-	DPRINTFN(3, ("ohci_device_intr_transfer: xfer=%p len=%d "
+	DPRINTFN(3, ("ohci_device_intr_start: xfer=%p len=%d "
 		     "flags=%d priv=%p\n",
 		     xfer, xfer->length, xfer->flags, xfer->priv));
 
 #ifdef DIAGNOSTIC
 	if (xfer->rqflags & URQ_REQUEST)
-		panic("ohci_device_intr_transfer: a request");
+		panic("ohci_device_intr_start: a request");
 #endif
 
-	len = xfer->length;
+	err = ohci_device_intr_insert(sc, xfer);
+	if (err)
+		return (err);
+
+	sed->ed.ed_flags &= htole32(~OHCI_ED_SKIP);
+
+	return (USBD_IN_PROGRESS);
+}
+
+/*
+ * Insert an interrupt transfer into an endpoint descriptor list
+ */
+Static usbd_status
+ohci_device_intr_insert(ohci_softc_t *sc, usbd_xfer_handle xfer)
+{
+	struct ohci_pipe *opipe = (struct ohci_pipe *)xfer->pipe;
+	ohci_soft_ed_t *sed = opipe->sed;
+	ohci_soft_td_t *data, *tail;
+	ohci_physaddr_t dataphys, physend;
+	int s;
+
+	DPRINTFN(4, ("ohci_device_intr_insert: xfer=%p", xfer));
 
 	data = opipe->tail.td;
 	tail = ohci_alloc_std(sc);
@@ -3131,18 +3158,43 @@ ohci_device_intr_start(usbd_xfer_handle xfer)
 		OHCI_TD_SET_DI(1) | OHCI_TD_TOGGLE_CARRY);
 	if (xfer->flags & USBD_SHORT_XFER_OK)
 		data->td.td_flags |= htole32(OHCI_TD_R);
-	data->td.td_cbp = htole32(DMAADDR(&xfer->dmabuf, 0));
+	/*
+	 * Assume a short mapping with no complications, which
+	 * should always be true for <= 4k buffers in contiguous
+	 * virtual memory. The data can take the following forms:
+	 *	1 segment in 1 OHCI page
+	 *	1 segment in 2 OHCI pages
+	 *	2 segments in 2 OHCI pages
+	 * (see comment in ohci_alloc_std_chain() for details)
+	 */
+	KASSERT(xfer->length > 0 && xfer->length <= OHCI_PAGE_SIZE,
+	    ("ohci_device_intr_insert: bad length %d", xfer->length));
+	dataphys = xfer->dmamap.segs[0].ds_addr;
+	physend = dataphys + xfer->length - 1;
+	if (xfer->dmamap.nsegs == 2) {
+		KASSERT(OHCI_PAGE_OFFSET(dataphys +
+		    xfer->dmamap.segs[0].ds_len) == 0,
+		    ("ohci_device_intr_insert: bad seg 0 termination"));
+		physend = xfer->dmamap.segs[1].ds_addr + xfer->length -
+		    xfer->dmamap.segs[0].ds_len - 1;
+	} else {
+		KASSERT(xfer->dmamap.nsegs == 1,
+		    ("ohci_device_intr_insert: bad seg count %d",
+		    (u_int)xfer->dmamap.nsegs));
+	}
+	data->td.td_cbp = htole32(dataphys);
 	data->nexttd = tail;
 	data->td.td_nexttd = htole32(tail->physaddr);
-	data->td.td_be = htole32(le32toh(data->td.td_cbp) + len - 1);
-	data->len = len;
+	data->td.td_be = htole32(physend);
+	data->len = xfer->length;
 	data->xfer = xfer;
 	data->flags = OHCI_CALL_DONE | OHCI_ADD_LEN;
 	xfer->hcpriv = data;
+	xfer->actlen = 0;
 
 #ifdef USB_DEBUG
 	if (ohcidebug > 5) {
-		DPRINTF(("ohci_device_intr_transfer:\n"));
+		DPRINTF(("ohci_device_intr_insert:\n"));
 		ohci_dump_ed(sed);
 		ohci_dump_tds(data);
 	}
@@ -3152,25 +3204,9 @@ ohci_device_intr_start(usbd_xfer_handle xfer)
 	s = splusb();
 	sed->ed.ed_tailp = htole32(tail->physaddr);
 	opipe->tail.td = tail;
-	sed->ed.ed_flags &= htole32(~OHCI_ED_SKIP);
-
-#if 0
-/*
- * This goes horribly wrong, printing thousands of descriptors,
- * because false references are followed due to the fact that the
- * TD is gone.
- */
-	if (ohcidebug > 5) {
-		usb_delay_ms(&sc->sc_bus, 5);
-		DPRINTF(("ohci_device_intr_transfer: status=%x\n",
-			 OREAD4(sc, OHCI_COMMAND_STATUS)));
-		ohci_dump_ed(sed);
-		ohci_dump_tds(data);
-	}
-#endif
 	splx(s);
 
-	return (USBD_IN_PROGRESS);
+	return (USBD_NORMAL_COMPLETION);
 }
 
 /* Abort a device control request. */
@@ -3325,10 +3361,10 @@ ohci_device_isoc_enter(usbd_xfer_handle xfer)
 	ohci_softc_t *sc = (ohci_softc_t *)dev->bus;
 	ohci_soft_ed_t *sed = opipe->sed;
 	struct iso *iso = &opipe->u.iso;
-	struct ohci_xfer *oxfer = (struct ohci_xfer *)xfer;
+	struct usb_dma_mapping *dma = &xfer->dmamap;
 	ohci_soft_itd_t *sitd, *nsitd;
-	ohci_physaddr_t buf, offs, noffs, bp0, tdphys;
-	int i, ncur, nframes;
+	ohci_physaddr_t dataphys, bp0, physend, prevpage;
+	int curlen, i, len, ncur, nframes, npages, seg, segoff;
 	int s;
 
 	DPRINTFN(1,("ohci_device_isoc_enter: used=%d next=%d xfer=%p "
@@ -3345,94 +3381,115 @@ ohci_device_isoc_enter(usbd_xfer_handle xfer)
 			    iso->next));
 	}
 
-	if (xfer->hcpriv) {
-		for (sitd = xfer->hcpriv; sitd != NULL && sitd->xfer == xfer;
-		    sitd = sitd->nextitd)
-			ohci_free_sitd(sc, sitd); /* Free ITDs in prev xfer*/
-
-		if (sitd == NULL) {
-			sitd = ohci_alloc_sitd(sc);
-			if (sitd == NULL)
-				panic("cant alloc isoc");
-			opipe->tail.itd = sitd;
-			tdphys = sitd->physaddr;
-			sed->ed.ed_flags |= htole32(OHCI_ED_SKIP); /* Stop*/
-			sed->ed.ed_headp =
-			sed->ed.ed_tailp = htole32(tdphys);
-			sed->ed.ed_flags &= htole32(~OHCI_ED_SKIP); /* Start.*/
-		}
-	}
-
 	sitd = opipe->tail.itd;
-	buf = DMAADDR(&xfer->dmabuf, 0);
-	bp0 = OHCI_PAGE(buf);
-	offs = OHCI_PAGE_OFFSET(buf);
 	nframes = xfer->nframes;
 	xfer->hcpriv = sitd;
-	for (i = ncur = 0; i < nframes; i++, ncur++) {
-		noffs = offs + xfer->frlengths[i];
-		if (ncur == OHCI_ITD_NOFFSET ||	/* all offsets used */
-		    OHCI_PAGE(buf + noffs) > bp0 + OHCI_PAGE_SIZE) { /* too many page crossings */
-
-			/* Allocate next ITD */
-			nsitd = ohci_alloc_sitd(sc);
-			if (nsitd == NULL) {
-				/* XXX what now? */
-				printf("%s: isoc TD alloc failed\n",
-				       USBDEVNAME(sc->sc_bus.bdev));
-				return;
+	seg = 0;
+	segoff = 0;
+	i = 0;
+	while (i < nframes) {
+		/*
+		 * Fill in as many ITD frames as possible.
+		 */
+		KASSERT(seg < dma->nsegs, ("ohci_device_isoc_enter: overrun"));
+		bp0 = dma->segs[seg].ds_addr + segoff;
+		sitd->itd.itd_bp0 = htole32(bp0);
+		prevpage = OHCI_PAGE(bp0);
+		npages = 1;
+
+		ncur = 0;
+		while (ncur < OHCI_ITD_NOFFSET && i < nframes) {
+			/* Find the frame start and end physical addresses. */
+			len = xfer->frlengths[i];
+			dataphys = dma->segs[seg].ds_addr + segoff;
+			curlen = dma->segs[seg].ds_len - segoff;
+			if (len > curlen) {
+				KASSERT(seg + 1 < dma->nsegs,
+				    ("ohci_device_isoc_enter: overrun2"));
+				seg++;
+				segoff = len - curlen;
+			} else {
+				segoff += len;
+			}
+			KASSERT(segoff <= dma->segs[seg].ds_len,
+			    ("ohci_device_isoc_enter: overrun3"));
+			physend = dma->segs[seg].ds_addr + segoff - 1;
+
+			/* Check if there would be more than 2 pages . */
+			if (OHCI_PAGE(dataphys) != prevpage) {
+				prevpage = OHCI_PAGE(dataphys);
+				npages++;
+			}
+			if (OHCI_PAGE(physend) != prevpage) {
+				prevpage = OHCI_PAGE(physend);
+				npages++;
+			}
+			if (npages > 2) {
+				/* We cannot fit this frame now. */
+				segoff -= len;
+				if (segoff < 0) {
+					seg--;
+					segoff += dma->segs[seg].ds_len;
+				}
+				break;
 			}
 
-			/* Fill current ITD */
+			sitd->itd.itd_be = htole32(physend);
+			sitd->itd.itd_offset[ncur] =
+			    htole16(OHCI_ITD_MK_OFFS(OHCI_PAGE(dataphys) ==
+			    OHCI_PAGE(bp0) ? 0 : 1, dataphys));
+			i++;
+			ncur++;
+		}
+		if (segoff >= dma->segs[seg].ds_len) {
+			KASSERT(segoff == dma->segs[seg].ds_len,
+			    ("ohci_device_isoc_enter: overlap"));
+			seg++;
+			segoff = 0;
+		}
+
+		/* Allocate next ITD */
+		nsitd = ohci_alloc_sitd(sc);
+		if (nsitd == NULL) {
+			/* XXX what now? */
+			printf("%s: isoc TD alloc failed\n",
+			       USBDEVNAME(sc->sc_bus.bdev));
+			return;
+		}
+
+		/* Fill out remaining fields of current ITD */
+		sitd->nextitd = nsitd;
+		sitd->itd.itd_nextitd = htole32(nsitd->physaddr);
+		sitd->xfer = xfer;
+		if (i < nframes) {
 			sitd->itd.itd_flags = htole32(
 				OHCI_ITD_NOCC |
 				OHCI_ITD_SET_SF(iso->next) |
 				OHCI_ITD_SET_DI(6) | /* delay intr a little */
 				OHCI_ITD_SET_FC(ncur));
-			sitd->itd.itd_bp0 = htole32(bp0);
-			sitd->nextitd = nsitd;
-			sitd->itd.itd_nextitd = htole32(nsitd->physaddr);
-			sitd->itd.itd_be = htole32(bp0 + offs - 1);
-			sitd->xfer = xfer;
 			sitd->flags = OHCI_ITD_ACTIVE;
-
-			sitd = nsitd;
-			iso->next = iso->next + ncur;
-			bp0 = OHCI_PAGE(buf + offs);
-			ncur = 0;
+		} else {
+			sitd->itd.itd_flags = htole32(
+				OHCI_ITD_NOCC |
+				OHCI_ITD_SET_SF(iso->next) |
+				OHCI_ITD_SET_DI(0) |
+				OHCI_ITD_SET_FC(ncur));
+			sitd->flags = OHCI_CALL_DONE | OHCI_ITD_ACTIVE;
 		}
-		sitd->itd.itd_offset[ncur] = htole16(OHCI_ITD_MK_OFFS(offs));
-		offs = noffs;
-	}
-	nsitd = ohci_alloc_sitd(sc);
-	if (nsitd == NULL) {
-		/* XXX what now? */
-		printf("%s: isoc TD alloc failed\n",
-		       USBDEVNAME(sc->sc_bus.bdev));
-		return;
+		iso->next += ncur;
+
+		sitd = nsitd;
 	}
-	/* Fixup last used ITD */
-	sitd->itd.itd_flags = htole32(
-		OHCI_ITD_NOCC |
-		OHCI_ITD_SET_SF(iso->next) |
-		OHCI_ITD_SET_DI(0) |
-		OHCI_ITD_SET_FC(ncur));
-	sitd->itd.itd_bp0 = htole32(bp0);
-	sitd->nextitd = nsitd;
-	sitd->itd.itd_nextitd = htole32(nsitd->physaddr);
-	sitd->itd.itd_be = htole32(bp0 + offs - 1);
-	sitd->xfer = xfer;
-	sitd->flags = OHCI_CALL_DONE | OHCI_ITD_ACTIVE;
-
-	iso->next = iso->next + ncur;
+
 	iso->inuse += nframes;
 
-	xfer->actlen = offs;	/* XXX pretend we did it all */
+	/* XXX pretend we did it all */
+	xfer->actlen = 0;
+	for (i = 0; i < nframes; i++)
+		xfer->actlen += xfer->frlengths[i];
 
 	xfer->status = USBD_IN_PROGRESS;
 
-	oxfer->ohci_xfer_flags |= OHCI_ISOC_DIRTY;
-
 #ifdef USB_DEBUG
 	if (ohcidebug > 5) {
 		DPRINTF(("ohci_device_isoc_enter: frame=%d\n",
@@ -3443,9 +3500,9 @@ ohci_device_isoc_enter(usbd_xfer_handle xfer)
 #endif
 
 	s = splusb();
-	opipe->tail.itd = nsitd;
+	opipe->tail.itd = sitd;
 	sed->ed.ed_flags &= htole32(~OHCI_ED_SKIP);
-	sed->ed.ed_tailp = htole32(nsitd->physaddr);
+	sed->ed.ed_tailp = htole32(sitd->physaddr);
 	splx(s);
 
 #ifdef USB_DEBUG
@@ -3493,7 +3550,7 @@ ohci_device_isoc_abort(usbd_xfer_handle xfer)
 	struct ohci_pipe *opipe = (struct ohci_pipe *)xfer->pipe;
 	ohci_softc_t *sc = (ohci_softc_t *)opipe->pipe.device->bus;
 	ohci_soft_ed_t *sed;
-	ohci_soft_itd_t *sitd, *tmp_sitd;
+	ohci_soft_itd_t *sitd, *sitdnext, *tmp_sitd;
 	int s,undone,num_sitds;
 
 	s = splusb();
@@ -3556,20 +3613,20 @@ ohci_device_isoc_abort(usbd_xfer_handle xfer)
 		}
 	} while( undone != 0 );
 
+	/* Free the sitds */
+ 	for (sitd = xfer->hcpriv; sitd->xfer == xfer;
+	    sitd = sitdnext) {
+		sitdnext = sitd->nextitd;
+		ohci_free_sitd(sc, sitd);
+	}
 
 	s = splusb();
 
 	/* Run callback. */
 	usb_transfer_complete(xfer);
 
-	if (sitd != NULL)
-		/*
-		 * Only if there is a `next' sitd in next xfer...
-		 * unlink this xfer's sitds.
-		 */
-		sed->ed.ed_headp = htole32(sitd->physaddr);
-	else
-		sed->ed.ed_headp = 0;
+	/* There is always a `next' sitd so link it up. */
+	sed->ed.ed_headp = htole32(sitd->physaddr);
 
 	sed->ed.ed_flags &= htole32(~OHCI_ED_SKIP); /* remove hardware skip */