Index: sys/dev/usb/ehci.c
===================================================================
RCS file: /cvsroot/src/sys/dev/usb/ehci.c,v
retrieving revision 1.177
diff -u -p -r1.177 ehci.c
--- sys/dev/usb/ehci.c	28 May 2011 15:47:17 -0000	1.177
+++ sys/dev/usb/ehci.c	6 Jun 2011 20:38:00 -0000
@@ -1256,7 +1256,7 @@ ehci_allocm(struct usbd_bus *bus, usb_dm
 	struct ehci_softc *sc = bus->hci_private;
 	usbd_status err;
 
-	err = usb_allocmem(&sc->sc_bus, size, 0, dma);
+	err = usb_allocmem_flags(&sc->sc_bus, size, 0, dma, USBMALLOC_MULTISEG);
 	if (err == USBD_NOMEM)
 		err = usb_reserve_allocm(&sc->sc_dma_reserve, dma, size);
 #ifdef EHCI_DEBUG
@@ -2634,8 +2634,9 @@ ehci_alloc_sqtd_chain(struct ehci_pipe *
 		     ehci_soft_qtd_t **sp, ehci_soft_qtd_t **ep)
 {
 	ehci_soft_qtd_t *next, *cur;
-	ehci_physaddr_t dataphys, dataphyspage, dataphyslastpage, nextphys;
+	ehci_physaddr_t nextphys;
 	u_int32_t qtdstatus;
+	bus_size_t curoffs;
 	int len, curlen, mps;
 	int i, tog;
 	usb_dma_t *dma = &xfer->dmabuf;
@@ -2643,9 +2644,8 @@ ehci_alloc_sqtd_chain(struct ehci_pipe *
 
 	DPRINTFN(alen<4*4096,("ehci_alloc_sqtd_chain: start len=%d\n", alen));
 
+	curoffs = 0;
 	len = alen;
-	dataphys = DMAADDR(dma, 0);
-	dataphyslastpage = EHCI_PAGE(dataphys + len - 1);
 	qtdstatus = EHCI_QTD_ACTIVE |
 	    EHCI_QTD_SET_PID(rd ? EHCI_QTD_PID_IN : EHCI_QTD_PID_OUT) |
 	    EHCI_QTD_SET_CERR(3)
@@ -2664,27 +2664,17 @@ ehci_alloc_sqtd_chain(struct ehci_pipe *
 	usb_syncmem(dma, 0, alen,
 	    rd ? BUS_DMASYNC_PREREAD : BUS_DMASYNC_PREWRITE);
 	for (;;) {
-		dataphyspage = EHCI_PAGE(dataphys);
 		/* The EHCI hardware can handle at most 5 pages. */
-		if (dataphyslastpage - dataphyspage <
-		    EHCI_QTD_NBUFFERS * EHCI_PAGE_SIZE) {
+		vaddr_t va_offs = (vaddr_t)KERNADDR(dma, curoffs);
+		va_offs = EHCI_PAGE_OFFSET(va_offs);
+		if (len - curoffs < EHCI_QTD_NBUFFERS * EHCI_PAGE_SIZE
+							- va_offs) {
 			/* we can handle it in this QTD */
-			curlen = len;
+			curlen = len - curoffs;
 		} else {
 			/* must use multiple TDs, fill as much as possible. */
-			curlen = EHCI_QTD_NBUFFERS * EHCI_PAGE_SIZE -
-				 EHCI_PAGE_OFFSET(dataphys);
-#ifdef DIAGNOSTIC
-			if (curlen > len) {
-				printf("ehci_alloc_sqtd_chain: curlen=0x%x "
-				       "len=0x%x offs=0x%x\n", curlen, len,
-				       EHCI_PAGE_OFFSET(dataphys));
-				printf("lastpage=0x%x page=0x%x phys=0x%x\n",
-				       dataphyslastpage, dataphyspage,
-				       dataphys);
-				curlen = len;
-			}
-#endif
+			curlen = EHCI_QTD_NBUFFERS * EHCI_PAGE_SIZE - va_offs;
+
 			/* the length must be a multiple of the max size */
 			curlen -= curlen % mps;
 			DPRINTFN(1,("ehci_alloc_sqtd_chain: multiple QTDs, "
@@ -2694,18 +2684,16 @@ ehci_alloc_sqtd_chain(struct ehci_pipe *
 				panic("ehci_alloc_sqtd_chain: curlen == 0");
 #endif
 		}
-		DPRINTFN(4,("ehci_alloc_sqtd_chain: dataphys=0x%08x "
-			    "dataphyslastpage=0x%08x len=%d curlen=%d\n",
-			    dataphys, dataphyslastpage,
-			    len, curlen));
-		len -= curlen;
+		DPRINTFN(4,("ehci_alloc_sqtd_chain: len=%d curlen=%d "
+				"curoffs=%d\n", len, curlen, curoffs));
 
 		/*
 		 * Allocate another transfer if there's more data left,
 		 * or if force last short transfer flag is set and we're
 		 * allocating a multiple of the max packet size.
 		 */
-		if (len != 0 ||
+
+		if (curoffs + curlen != len ||
 		    ((curlen % mps) == 0 && !rd && curlen != 0 &&
 		     (flags & USBD_FORCE_SHORT_XFER))) {
 			next = ehci_alloc_sqtd(sc);
@@ -2717,13 +2705,16 @@ ehci_alloc_sqtd_chain(struct ehci_pipe *
 			nextphys = EHCI_NULL;
 		}
 
-		for (i = 0; i * EHCI_PAGE_SIZE <
-		            curlen + EHCI_PAGE_OFFSET(dataphys); i++) {
-			ehci_physaddr_t a = dataphys + i * EHCI_PAGE_SIZE;
-			if (i != 0) /* use offset only in first buffer */
-				a = EHCI_PAGE(a);
-			cur->qtd.qtd_buffer[i] = htole32(a);
-			cur->qtd.qtd_buffer_hi[i] = 0;
+		/* Find number of pages we'll be using, insert dma addresses */
+		int pages = EHCI_PAGE(curlen + EHCI_PAGE_SIZE - 1) >> 12;
+		KASSERT(pages <= 5);
+		int pageoffs = EHCI_PAGE(curoffs);
+		for (i = 0; i < pages; i++) {
+			paddr_t a = DMAADDR(dma, pageoffs +
+						i * EHCI_PAGE_SIZE);
+			cur->qtd.qtd_buffer[i] = htole32(a & 0xFFFFF000);
+			/* Cast up to avoid compiler warnings */
+			cur->qtd.qtd_buffer_hi[i] = htole32((uint64_t)a >> 32);
 #ifdef DIAGNOSTIC
 			if (i >= EHCI_QTD_NBUFFERS) {
 				printf("ehci_alloc_sqtd_chain: i=%d\n", i);
@@ -2731,6 +2722,11 @@ ehci_alloc_sqtd_chain(struct ehci_pipe *
 			}
 #endif
 		}
+
+		/* First buffer pointer requires a page offset to start at */
+		vaddr_t va = (vaddr_t)KERNADDR(dma, curoffs);
+		cur->qtd.qtd_buffer[0] |= htole32(EHCI_PAGE_OFFSET(va));
+
 		cur->nextqtd = next;
 		cur->qtd.qtd_next = cur->qtd.qtd_altnext = nextphys;
 		cur->qtd.qtd_status =
@@ -2740,6 +2736,7 @@ ehci_alloc_sqtd_chain(struct ehci_pipe *
 
 		DPRINTFN(10,("ehci_alloc_sqtd_chain: cbp=0x%08x end=0x%08x\n",
 			    dataphys, dataphys + curlen));
+
 		/* adjust the toggle based on the number of packets in this
 		   qtd */
 		if (((curlen + mps - 1) / mps) & 1) {
@@ -2752,7 +2749,7 @@ ehci_alloc_sqtd_chain(struct ehci_pipe *
 		    BUS_DMASYNC_PREWRITE | BUS_DMASYNC_PREREAD);
 		DPRINTFN(10,("ehci_alloc_sqtd_chain: extend chain\n"));
 		if (len)
-			dataphys += curlen;
+			curoffs += curlen;
 		cur = next;
 	}
 	cur->qtd.qtd_status |= htole32(EHCI_QTD_IOC);
Index: sys/dev/usb/usb_mem.c
===================================================================
RCS file: /cvsroot/src/sys/dev/usb/usb_mem.c,v
retrieving revision 1.46
diff -u -p -r1.46 usb_mem.c
--- sys/dev/usb/usb_mem.c	20 Mar 2011 17:38:11 -0000	1.46
+++ sys/dev/usb/usb_mem.c	6 Jun 2011 20:38:00 -0000
@@ -87,7 +87,7 @@ struct usb_frag_dma {
 };
 
 Static usbd_status	usb_block_allocmem(bus_dma_tag_t, size_t, size_t,
-					   usb_dma_block_t **);
+					   usb_dma_block_t **, bool multiseg);
 Static void		usb_block_freemem(usb_dma_block_t *);
 
 Static LIST_HEAD(, usb_dma_block) usb_blk_freelist =
@@ -99,7 +99,7 @@ Static LIST_HEAD(, usb_frag_dma) usb_fra
 
 Static usbd_status
 usb_block_allocmem(bus_dma_tag_t tag, size_t size, size_t align,
-		   usb_dma_block_t **dmap)
+		   usb_dma_block_t **dmap, bool multiseg)
 {
 	int error;
         usb_dma_block_t *p;
@@ -118,6 +118,11 @@ usb_block_allocmem(bus_dma_tag_t tag, si
 	s = splusb();
 	/* First check the free list. */
 	LIST_FOREACH(p, &usb_blk_freelist, next) {
+
+		/* Don't allocate multiple segments to unwilling callers */
+		if (p->nsegs != 1 && !multiseg)
+			continue;
+
 		if (p->tag == tag && p->size >= size && p->align >= align) {
 			LIST_REMOVE(p, next);
 			usb_blk_nfree--;
@@ -145,8 +150,19 @@ usb_block_allocmem(bus_dma_tag_t tag, si
 	p->tag = tag;
 	p->size = size;
 	p->align = align;
-	error = bus_dmamem_alloc(tag, p->size, align, 0,
-				 p->segs, sizeof(p->segs)/sizeof(p->segs[0]),
+
+	p->nsegs = (size + (PAGE_SIZE-1)) / PAGE_SIZE;
+	if (!multiseg)
+		/* Caller wants one segment */
+		p->nsegs = 1;
+
+	p->segs = malloc(p->nsegs * sizeof(*p->segs), M_USB, M_NOWAIT);
+	if (p == NULL) {
+		free(p, M_USB);
+		return USBD_NOMEM;
+	}
+
+	error = bus_dmamem_alloc(tag, p->size, align, 0, p->segs, p->nsegs,
 				 &p->nsegs, BUS_DMA_NOWAIT);
 	if (error)
 		goto free0;
@@ -156,7 +172,7 @@ usb_block_allocmem(bus_dma_tag_t tag, si
 	if (error)
 		goto free1;
 
-	error = bus_dmamap_create(tag, p->size, 1, p->size,
+	error = bus_dmamap_create(tag, p->size, p->nsegs, p->size,
 				  0, BUS_DMA_NOWAIT, &p->map);
 	if (error)
 		goto unmap;
@@ -170,6 +186,7 @@ usb_block_allocmem(bus_dma_tag_t tag, si
 #ifdef USB_FRAG_DMA_WORKAROUND
 	memset(p->kaddr, 0, p->size);
 #endif
+
 	return (USBD_NORMAL_COMPLETION);
 
  destroy:
@@ -179,6 +196,7 @@ usb_block_allocmem(bus_dma_tag_t tag, si
  free1:
 	bus_dmamem_free(tag, p->segs, p->nsegs);
  free0:
+	free(p->segs, M_USB);
 	free(p, M_USB);
 	return (USBD_NOMEM);
 }
@@ -221,18 +239,29 @@ usb_block_freemem(usb_dma_block_t *p)
 usbd_status
 usb_allocmem(usbd_bus_handle bus, size_t size, size_t align, usb_dma_t *p)
 {
+
+	return usb_allocmem_flags(bus, size, align, p, 0);
+}
+
+usbd_status
+usb_allocmem_flags(usbd_bus_handle bus, size_t size, size_t align, usb_dma_t *p,
+			int flags)
+{
 	bus_dma_tag_t tag = bus->dmatag;
 	usbd_status err;
 	struct usb_frag_dma *f;
 	usb_dma_block_t *b;
 	int i;
 	int s;
+	bool frag;
+
+	frag = (flags & USBMALLOC_MULTISEG);
 
 	/* If the request is large then just use a full block. */
 	if (size > USB_MEM_SMALL || align > USB_MEM_SMALL) {
 		DPRINTFN(1, ("usb_allocmem: large alloc %d\n", (int)size));
 		size = (size + USB_MEM_BLOCK - 1) & ~(USB_MEM_BLOCK - 1);
-		err = usb_block_allocmem(tag, size, align, &p->block);
+		err = usb_block_allocmem(tag, size, align, &p->block, frag);
 		if (!err) {
 			p->block->flags = USB_DMA_FULLBLOCK;
 			p->offs = 0;
@@ -248,7 +277,8 @@ usb_allocmem(usbd_bus_handle bus, size_t
 	}
 	if (f == NULL) {
 		DPRINTFN(1, ("usb_allocmem: adding fragments\n"));
-		err = usb_block_allocmem(tag, USB_MEM_BLOCK, USB_MEM_SMALL,&b);
+		err = usb_block_allocmem(tag, USB_MEM_BLOCK, USB_MEM_SMALL, &b,
+									false);
 		if (err) {
 			splx(s);
 			return (err);
@@ -274,6 +304,7 @@ usb_allocmem(usbd_bus_handle bus, size_t
 	LIST_REMOVE(f, next);
 	splx(s);
 	DPRINTFN(5, ("usb_allocmem: use frag=%p size=%d\n", f, (int)size));
+
 	return (USBD_NORMAL_COMPLETION);
 }
 
@@ -304,6 +335,39 @@ usb_freemem(usbd_bus_handle bus, usb_dma
 	DPRINTFN(5, ("usb_freemem: frag=%p\n", f));
 }
 
+bus_addr_t
+usb_dmaaddr(usb_dma_t *dma, unsigned int offset)
+{
+	unsigned int i;
+	bus_size_t seg_offs;
+
+	offset += dma->offs;
+
+	KASSERT(offset < dma->block->size);
+
+	if (dma->block->nsegs == 1) {
+		KASSERT(dma->block->map->dm_segs[0].ds_len > offset);
+		return dma->block->map->dm_segs[0].ds_addr + offset;
+	}
+
+	/* Search for a bus_segment_t corresponding to this offset. With no
+	 * record of the offset in the map to a particular dma_segment_t, we
+	 * have to iterate from the start of the list each time. Could be
+	 * improved */
+	seg_offs = 0;
+	for (i = 0; i < dma->block->nsegs; i++) {
+		if (seg_offs + dma->block->map->dm_segs[i].ds_len > offset)
+			break;
+
+		seg_offs += dma->block->map->dm_segs[i].ds_len;
+	}
+
+	KASSERT(i != dma->block->nsegs);
+	offset -= seg_offs;
+	return dma->block->map->dm_segs[i].ds_addr + offset;
+}
+	
+
 void
 usb_syncmem(usb_dma_t *p, bus_addr_t offset, bus_size_t len, int ops)
 {
Index: sys/dev/usb/usb_mem.h
===================================================================
RCS file: /cvsroot/src/sys/dev/usb/usb_mem.h,v
retrieving revision 1.27
diff -u -p -r1.27 usb_mem.h
--- sys/dev/usb/usb_mem.h	28 Jun 2008 17:42:53 -0000	1.27
+++ sys/dev/usb/usb_mem.h	6 Jun 2011 20:38:00 -0000
@@ -36,7 +36,7 @@ typedef struct usb_dma_block {
 	bus_dma_tag_t tag;
 	bus_dmamap_t map;
         void *kaddr;
-        bus_dma_segment_t segs[1];
+        bus_dma_segment_t *segs;
         int nsegs;
         size_t size;
         size_t align;
@@ -46,14 +46,21 @@ typedef struct usb_dma_block {
 	LIST_ENTRY(usb_dma_block) next;
 } usb_dma_block_t;
 
-#define DMAADDR(dma, o) ((dma)->block->map->dm_segs[0].ds_addr + (dma)->offs + (o))
-#define KERNADDR(dma, o) \
-	((void *)((char *)(dma)->block->kaddr + (dma)->offs + (o)))
+#define USBMALLOC_MULTISEG		1
 
 usbd_status	usb_allocmem(usbd_bus_handle,size_t,size_t, usb_dma_t *);
+usbd_status	usb_allocmem_flags(usbd_bus_handle,size_t,size_t, usb_dma_t *,
+					int);
 void		usb_freemem(usbd_bus_handle, usb_dma_t *);
 void		usb_syncmem(usb_dma_t *, bus_addr_t, bus_size_t, int ops);
 
+bus_addr_t	usb_dmaaddr(usb_dma_t *, unsigned int);
+
+#define DMAADDR(dma, o) usb_dmaaddr((dma), (o))
+/*((dma)->block->map->dm_segs[0].ds_addr + (dma)->offs + (o))*/
+#define KERNADDR(dma, o) \
+	((void *)((char *)(dma)->block->kaddr + (dma)->offs + (o)))
+
 #ifdef __NetBSD__
 struct extent;