Index: sys/dev/iommu/iommu_gas.c
===================================================================
--- sys/dev/iommu/iommu_gas.c
+++ sys/dev/iommu/iommu_gas.c
@@ -292,15 +292,18 @@
 
 /*
  * The interval [beg, end) is a free interval between two iommu_map_entries.
- * maxaddr is an upper bound on addresses that can be allocated. Try to
- * allocate space in the free interval, subject to the conditions expressed
- * by a, and return 'true' if and only if the allocation attempt succeeds.
+ * minaddr is a lower bound on addresses that can be allocated.  maxaddr is an
+ * upper bound on addresses that can be allocated. Try to allocate space in the
+ * free interval, subject to the conditions expressed by a, and return 'true' if
+ * and only if the allocation attempt succeeds.
  */
 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 minaddr, iommu_gaddr_t maxaddr)
 {
-	iommu_gaddr_t bs, start;
+	iommu_gaddr_t first, size, start;
+	bool found __diagused;
+	int offset = a->offset;
 
 	/*
 	 * The prev->end is always aligned on the page size, which
@@ -309,70 +312,78 @@
 	 * A page sized gap is created between consecutive
 	 * allocations to ensure that out-of-bounds accesses fault.
 	 */
-	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 ||
-	    a->entry->start + a->size + a->offset + IOMMU_PAGE_SIZE > end)
-		return (false);
+	/* IOMMU_PAGE_SIZE to create gaps before, after new entry. */
+	beg = ummax(beg + IOMMU_PAGE_SIZE, minaddr);
+	end = ummin(end - IOMMU_PAGE_SIZE, maxaddr);
+	first = start = roundup2(beg, a->common->alignment);
 
-	/* No boundary crossing. */
-	if (vm_addr_bound_ok(a->entry->start + a->offset, a->size,
-	    a->common->boundary))
-		return (true);
+	size = a->size;
+	if (start < beg || start + size + offset > end)
+		return (false);
 
-	/*
-	 * The start + offset to start + offset + size region crosses
-	 * the boundary.  Check if there is enough space after the
-	 * next boundary after the beg.
-	 */
-	bs = rounddown2(a->entry->start + a->offset + a->common->boundary,
-	    a->common->boundary);
-	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;
-		return (true);
+	/* Check for and try to skip past boundary crossing. */
+	if (!vm_addr_bound_ok(start + offset, size, a->common->boundary)) {
+		/*
+		 * The start + offset to start + offset + size region crosses
+		 * the boundary.  Check if there is enough space after the next
+		 * boundary after the beg.
+		 */
+		beg = roundup2(start + offset + 1, a->common->boundary);
+		start = roundup2(beg, a->common->alignment);
 	}
 
-	/*
-	 * 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.
-	 */
-	if ((a->gas_flags & IOMMU_MF_CANSPLIT) != 0) {
-		a->size = bs - a->entry->start - a->offset;
-		return (true);
+	if (start + offset + size > end ||
+	    !vm_addr_bound_ok(start + offset, size, a->common->boundary)) {
+		/*
+		 * 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.
+		 */
+		if ((a->gas_flags & IOMMU_MF_CANSPLIT) == 0)
+			return (false);
+
+		size = beg - first - offset;
+		start = first;
 	}
-
-	return (false);
+	a->entry->start = start;
+	a->entry->end = start + roundup2(size + offset, IOMMU_PAGE_SIZE);
+	a->entry->flags = IOMMU_MAP_ENTRY_MAP;
+	found = iommu_gas_rb_insert(a->domain, a->entry);
+	KASSERT(found, ("found dup %p start %jx size %jx",
+	    a->domain, (uintmax_t)start, (uintmax_t)size));
+	return (true);
 }
 
-static void
-iommu_gas_match_insert(struct iommu_gas_match_args *a)
+/* Find the next entry that might abut a big-enough range. */
+static struct iommu_map_entry *
+iommu_gas_next(struct iommu_map_entry *entry, iommu_gaddr_t min_free)
 {
-	bool found __diagused;
-
-	a->entry->end = a->entry->start +
-	    roundup2(a->size + a->offset, IOMMU_PAGE_SIZE);
+	struct iommu_map_entry *first;
 
-	found = iommu_gas_rb_insert(a->domain, a->entry);
-	KASSERT(found, ("found dup %p start %jx size %jx",
-	    a->domain, (uintmax_t)a->entry->start, (uintmax_t)a->size));
-	a->entry->flags = IOMMU_MAP_ENTRY_MAP;
+	/* Find the next entry that might abut a big-enough range. */
+	if ((first = RB_RIGHT(entry, rb_entry)) != NULL &&
+	    first->free_down >= min_free) {
+		/* Find next entry in right subtree. */
+		do
+			entry = first;
+		while ((first = RB_LEFT(entry, rb_entry)) != NULL &&
+		       first->free_down >= min_free);
+	} else {
+		/* Find next entry in a left-parent ancestor. */
+		while ((first = RB_PARENT(entry, rb_entry)) != NULL &&
+		       entry == RB_RIGHT(first, rb_entry))
+			entry = first;
+		entry = first;
+	}
+	return (entry);
 }
 
 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 *first;
 	iommu_gaddr_t min_free;
@@ -395,74 +406,56 @@
 	 * Walk the big-enough ranges until one satisfies alignment
 	 * requirements, or violates lowaddr address requirement.
 	 */
-	entry = first;
-	while (entry != NULL) {
+	for (entry = first; entry != NULL;
+	    entry = iommu_gas_next(entry, min_free)) {
 		if ((first = RB_LEFT(entry, rb_entry)) != NULL &&
 		    iommu_gas_match_one(a, first->last, entry->start,
-		    a->common->lowaddr)) {
-			iommu_gas_match_insert(a);
+		    0, a->common->lowaddr))
 			return (0);
-		}
 		if (entry->end >= a->common->lowaddr) {
 			/* All remaining ranges >= lowaddr */
 			break;
 		}
 		if ((first = RB_RIGHT(entry, rb_entry)) != NULL &&
 		    iommu_gas_match_one(a, entry->end, first->first,
-		    a->common->lowaddr)) {
-			iommu_gas_match_insert(a);
+		    0, a->common->lowaddr))
 			return (0);
-		}
-		/* Find the next entry that might abut a big-enough range. */
-		if (first != NULL && first->free_down >= min_free) {
-			/* Find next entry in right subtree. */
-			do
-				entry = first;
-			while ((first = RB_LEFT(entry, rb_entry)) != NULL &&
-			    first->free_down >= min_free);
-		} else {
-			/* Find next entry in a left-parent ancestor. */
-			while ((first = RB_PARENT(entry, rb_entry)) != NULL &&
-			    entry == RB_RIGHT(first, rb_entry))
-				entry = first;
-			entry = first;
-		}
 	}
-	return (ENOMEM);
-}
 
-static int
-iommu_gas_uppermatch(struct iommu_gas_match_args *a, struct iommu_map_entry *entry)
-{
-	struct iommu_map_entry *child;
+	/* Climb to the nearest ancestor that spans highaddr. */
+	while (entry != NULL && entry->last <= a->common->highaddr)
+		entry = RB_PARENT(entry, rb_entry);
 
 	/*
-	 * If the subtree doesn't have free space for the requested allocation
-	 * plus two guard pages, give up.
+	 * Find the last entry before highaddr that could abut a big-enough
+	 * 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);
+	first = NULL;
+	while (entry != NULL && entry->free_down >= min_free) {
+		if (a->common->highaddr <= entry->end)
+			entry = RB_LEFT(entry, rb_entry);
+		else {
+			first = entry;
+			entry = RB_RIGHT(entry, rb_entry);
+		}
 	}
-	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);
+
+	/*
+	 * Walk the remaining big-enough ranges until one satisfies alignment
+	 * requirements.
+	 */
+	for (entry = first; entry != NULL;
+	    entry = iommu_gas_next(entry, min_free)) {
+		if ((first = RB_LEFT(entry, rb_entry)) != NULL &&
+		    iommu_gas_match_one(a, first->last, entry->start,
+		    a->common->highaddr, a->domain->end))
+			return (0);
+		if ((first = RB_RIGHT(entry, rb_entry)) != NULL &&
+		    iommu_gas_match_one(a, entry->end, first->first,
+		    a->common->highaddr, a->domain->end))
+			return (0);
 	}
-	if (child != NULL && 0 == iommu_gas_uppermatch(a, child))
-		return (0);
+
 	return (ENOMEM);
 }
 
@@ -484,20 +477,11 @@
 	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. */
-	if (common->highaddr >= domain->end)
-		return (ENOMEM);
-	error = iommu_gas_uppermatch(&a, RB_ROOT(&domain->rb_root));
-	KASSERT(error == 0 || error == ENOMEM,
-	    ("error %d from iommu_gas_uppermatch", error));
+	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));
 	return (error);
 }