Index: sys/dev/iommu/iommu_gas.c
===================================================================
--- sys/dev/iommu/iommu_gas.c
+++ sys/dev/iommu/iommu_gas.c
@@ -299,14 +299,12 @@
  */
 static bool
 iommu_gas_match_one(struct iommu_gas_match_args *a, iommu_gaddr_t beg,
-    iommu_gaddr_t end, iommu_gaddr_t maxaddr)
+    iommu_gaddr_t end)
 {
 	iommu_gaddr_t bs, start;
 
 	a->entry->start = roundup2(beg + IOMMU_PAGE_SIZE,
 	    a->common->alignment);
-	if (a->entry->start + a->offset + a->size > maxaddr)
-		return (false);
 
 	/* IOMMU_PAGE_SIZE to create gap after new entry. */
 	if (a->entry->start < beg + IOMMU_PAGE_SIZE ||
@@ -328,7 +326,6 @@
 	start = roundup2(bs, a->common->alignment);
 	/* IOMMU_PAGE_SIZE to create gap after new entry. */
 	if (start + a->offset + a->size + IOMMU_PAGE_SIZE <= end &&
-	    start + a->offset + a->size <= maxaddr &&
 	    vm_addr_bound_ok(start + a->offset, a->size,
 	    a->common->boundary)) {
 		a->entry->start = start;
@@ -338,7 +335,6 @@
 	/*
 	 * Not enough space to align at the requested boundary, or
 	 * boundary is smaller than the size, but allowed to split.
-	 * We already checked that start + size does not overlap maxaddr.
 	 *
 	 * XXXKIB. It is possible that bs is exactly at the start of
 	 * the next entry, then we do not have gap.  Ignore for now.
@@ -374,72 +370,98 @@
 }
 
 static int
-iommu_gas_lowermatch(struct iommu_gas_match_args *a, struct iommu_map_entry *entry)
+iommu_gas_match(struct iommu_gas_match_args *a, struct iommu_map_entry *entry)
 {
-	struct iommu_map_entry *child;
+	struct iommu_map_entry *first, *root;
+	iommu_gaddr_t hi[3], lo[3], min_free;
+	int i, n;
 
+	root = entry;
 	/*
 	 * If the subtree doesn't have free space for the requested allocation
-	 * plus two guard pages, give up.
+	 * plus two guard pages, skip it.
 	 */
-	if (entry->free_down < 2 * IOMMU_PAGE_SIZE +
-	    roundup2(a->size + a->offset, IOMMU_PAGE_SIZE))
-		return (ENOMEM);
-	if (entry->first >= a->common->lowaddr)
-		return (ENOMEM);
-	child = RB_RIGHT(entry, rb_entry);
-	if (child != NULL && 0 == iommu_gas_lowermatch(a, child))
-		return (0);
-	if (child != NULL && entry->end < a->common->lowaddr &&
-	    iommu_gas_match_one(a, entry->end, child->first,
-	    a->common->lowaddr)) {
-		iommu_gas_match_insert(a);
-		return (0);
-	}
-	child = RB_LEFT(entry, rb_entry);
-	if (child != NULL && child->last < a->common->lowaddr &&
-	    iommu_gas_match_one(a, child->last, entry->start,
-	    a->common->lowaddr)) {
-		iommu_gas_match_insert(a);
-		return (0);
-	}
-	if (child != NULL && 0 == iommu_gas_lowermatch(a, child))
-		return (0);
-	return (ENOMEM);
-}
+	min_free = 2 * IOMMU_PAGE_SIZE +
+	    roundup2(a->size + a->offset, IOMMU_PAGE_SIZE);
 
-static int
-iommu_gas_uppermatch(struct iommu_gas_match_args *a, struct iommu_map_entry *entry)
-{
-	struct iommu_map_entry *child;
+	/* Find the first entry that could contain a big-enough range. */
+	first = NULL;
+	while (entry != NULL && entry->free_down >= min_free) {
+		first = entry;
+		entry = RB_LEFT(entry, rb_entry);
+	}
 
-	/*
-	 * If the subtree doesn't have free space for the requested allocation
-	 * plus two guard pages, give up.
+	/* Walk through the big-enough free ranges, until one is found with
+	 * no alignment problems.  Skip over the [lowaddr, highaddr] range.
 	 */
-	if (entry->free_down < 2 * IOMMU_PAGE_SIZE +
-	    roundup2(a->size + a->offset, IOMMU_PAGE_SIZE))
-		return (ENOMEM);
-	if (entry->last < a->common->highaddr)
-		return (ENOMEM);
-	child = RB_LEFT(entry, rb_entry);
-	if (child != NULL && 0 == iommu_gas_uppermatch(a, child))
-		return (0);
-	if (child != NULL && child->last >= a->common->highaddr &&
-	    iommu_gas_match_one(a, child->last, entry->start,
-	    a->domain->end)) {
-		iommu_gas_match_insert(a);
-		return (0);
-	}
-	child = RB_RIGHT(entry, rb_entry);
-	if (child != NULL && entry->end >= a->common->highaddr &&
-	    iommu_gas_match_one(a, entry->end, child->first,
-	    a->domain->end)) {
-		iommu_gas_match_insert(a);
-		return (0);
-	}
-	if (child != NULL && 0 == iommu_gas_uppermatch(a, child))
-		return (0);
+	entry = first;
+	while (entry != NULL) {
+		/*
+		 * Enlist the [begin, end] values for the gaps adjacent to this
+		 * entry, in order.  Since the skipped range could lie within
+		 * one of the gaps adjacent to this entry, we may enlist three
+		 * gaps in all.
+		 */
+		n = 0;
+		if ((first = RB_LEFT(entry, rb_entry)) != NULL &&
+		    first->last < a->common->lowaddr) {
+			lo[n] = first->last;
+			hi[n] = ummin(entry->start, a->common->lowaddr);
+			++n;
+		}
+		if ((first = RB_LEFT(entry, rb_entry)) != NULL &&
+		    a->common->highaddr < entry->start) {
+			lo[n] = ummax(a->common->highaddr, first->last);
+			hi[n] = entry->start;
+			++n;
+		}
+		if ((first = RB_RIGHT(entry, rb_entry)) != NULL &&
+		    entry->end < a->common->lowaddr) {
+			lo[n] = entry->end;
+			hi[n] = ummin(first->first, a->common->lowaddr);
+			++n;
+		}
+		if ((first = RB_RIGHT(entry, rb_entry)) != NULL &&
+		    a->common->highaddr < first->first) {
+			lo[n] = ummax(a->common->highaddr, entry->end);
+			hi[n] = first->first;
+			++n;
+		}
+		for (i = 0; i < n; ++i) {
+			if (iommu_gas_match_one(a, lo[i], hi[i])) {
+				iommu_gas_match_insert(a);
+				return (0);
+			}
+		}
+		if (n == 0) {
+			/*
+			 * Find the first entry more than common->highaddr that
+			 * could contain a big-enough free range.
+			 */
+			entry = root;
+			first = NULL;
+			while (entry != NULL && entry->free_down >= min_free) {
+				if (entry->start > a->common->highaddr) {
+					first = entry;
+					entry = RB_LEFT(entry, rb_entry);
+				} else
+					entry = RB_RIGHT(entry, rb_entry);
+			}
+			entry = first;
+		} else if ((first = RB_RIGHT(entry, rb_entry)) != NULL &&
+			   first->free_down >= min_free) {
+			do
+				entry = first;
+			while ((first = RB_LEFT(entry, rb_entry)) != NULL &&
+			    first->free_down >= min_free);
+		} else {
+			while ((first = RB_PARENT(entry, rb_entry)) != NULL &&
+			    entry == RB_RIGHT(first, rb_entry))
+				entry = first;
+			entry = first;
+		}
+	}		
+
 	return (ENOMEM);
 }
 
@@ -461,21 +483,13 @@
 	a.gas_flags = flags;
 	a.entry = entry;
 
-	/* Handle lower region. */
-	if (common->lowaddr > 0) {
-		error = iommu_gas_lowermatch(&a,
-		    RB_ROOT(&domain->rb_root));
-		if (error == 0)
-			return (0);
-		KASSERT(error == ENOMEM,
-		    ("error %d from iommu_gas_lowermatch", error));
-	}
-	/* Handle upper region. */
+	error = iommu_gas_match(&a, RB_ROOT(&domain->rb_root));
+	if (error == 0)
+		return (0);
+	KASSERT(error == ENOMEM,
+	    ("error %d from iommu_gas_match", error));
 	if (common->highaddr >= domain->end)
 		return (ENOMEM);
-	error = iommu_gas_uppermatch(&a, RB_ROOT(&domain->rb_root));
-	KASSERT(error == ENOMEM,
-	    ("error %d from iommu_gas_uppermatch", error));
 	return (error);
 }