Extend and fix bus_dmamap_load_raw() implementation to make xhci(4)

work.  Copied from the 32-bit ARM version, with a few minor changes
for arm64.

1 files changed, 74 insertions, 19 deletions

diff --git a/sys/arch/arm64/arm64/bus_dma.c b/sys/arch/arm64/arm64/bus_dma.c
index 74e985bc5ec..ad351c481b2 100644
--- a/sys/arch/arm64/arm64/bus_dma.c
+++ b/sys/arch/arm64/arm64/bus_dma.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: bus_dma.c,v 1.4 2017/02/17 17:51:02 patrick Exp $ */
+/*	$OpenBSD: bus_dma.c,v 1.5 2017/02/18 14:14:19 patrick Exp $ */
 
 /*
  * Copyright (c) 2003-2004 Opsycon AB  (www.opsycon.se / www.opsycon.com)
@@ -254,30 +254,85 @@ int
 _dmamap_load_raw(bus_dma_tag_t t, bus_dmamap_t map, bus_dma_segment_t *segs,
     int nsegs, bus_size_t size, int flags)
 {
-	if (nsegs > map->_dm_segcnt || size > map->_dm_size)
-		return (EINVAL);
+	bus_addr_t paddr, baddr, bmask, lastaddr = 0;
+	bus_size_t plen, sgsize, mapsize;
+	vaddr_t vaddr;
+	int first = 1;
+	int i, seg = 0;
 
 	/*
-	 * Make sure we don't cross any boundaries.
+	 * Make sure that on error condition we return "no valid mappings".
 	 */
-	if (map->_dm_boundary) {
-		bus_addr_t bmask = ~(map->_dm_boundary - 1);
-		int i;
-
-		if (t->_dma_mask != 0)
-			bmask &= t->_dma_mask;
-		for (i = 0; i < nsegs; i++) {
-			if (segs[i].ds_len > map->_dm_maxsegsz)
-				return (EINVAL);
-			if ((segs[i].ds_addr & bmask) !=
-			    ((segs[i].ds_addr + segs[i].ds_len - 1) & bmask))
-				return (EINVAL);
+	map->dm_mapsize = 0;
+	map->dm_nsegs = 0;
+
+	if (nsegs > map->_dm_segcnt || size > map->_dm_size)
+		return (EINVAL);
+
+	mapsize = size;
+	bmask = ~(map->_dm_boundary - 1);
+
+	for (i = 0; i < nsegs && size > 0; i++) {
+		paddr = segs[i].ds_addr;
+		vaddr = segs[i]._ds_vaddr;
+		plen = MIN(segs[i].ds_len, size);
+
+		while (plen > 0) {
+			/*
+			 * Compute the segment size, and adjust counts.
+			 */
+			sgsize = PAGE_SIZE - ((u_long)paddr & PGOFSET);
+			if (plen < sgsize)
+				sgsize = plen;
+
+			/*
+			 * Make sure we don't cross any boundaries.
+			 */
+			if (map->_dm_boundary > 0) {
+				baddr = (paddr + map->_dm_boundary) & bmask;
+				if (sgsize > (baddr - paddr))
+					sgsize = (baddr - paddr);
+			}
+
+			/*
+			 * Insert chunk into a segment, coalescing with
+			 * previous segment if possible.
+			 */
+			if (first) {
+				map->dm_segs[seg].ds_addr = paddr;
+				map->dm_segs[seg].ds_len = sgsize;
+				map->dm_segs[seg]._ds_paddr = paddr;
+				map->dm_segs[seg]._ds_vaddr = vaddr;
+				first = 0;
+			} else {
+				if (paddr == lastaddr &&
+				    (map->dm_segs[seg].ds_len + sgsize) <=
+				     map->_dm_maxsegsz &&
+				     (map->_dm_boundary == 0 ||
+				     (map->dm_segs[seg].ds_addr & bmask) ==
+				     (paddr & bmask)))
+					map->dm_segs[seg].ds_len += sgsize;
+				else {
+					if (++seg >= map->_dm_segcnt)
+						return (EINVAL);
+					map->dm_segs[seg].ds_addr = paddr;
+					map->dm_segs[seg].ds_len = sgsize;
+					map->dm_segs[seg]._ds_paddr = paddr;
+					map->dm_segs[seg]._ds_vaddr = vaddr;
+				}
+			}
+
+			paddr += sgsize;
+			vaddr += sgsize;
+			plen -= sgsize;
+			size -= sgsize;
+
+			lastaddr = paddr;
 		}
 	}
 
-	bcopy(segs, map->dm_segs, nsegs * sizeof(*segs));
-	map->dm_nsegs = nsegs;
-	map->dm_mapsize = size;
+	map->dm_mapsize = mapsize;
+	map->dm_nsegs = seg + 1;
 	return (0);
 }