Index: sys/amd64/amd64/machdep.c
===================================================================
--- sys/amd64/amd64/machdep.c
+++ sys/amd64/amd64/machdep.c
@@ -1245,14 +1245,10 @@
 	 * Make hole for "AP -> long mode" bootstrap code.  The
 	 * mp_bootaddress vector is only available when the kernel
 	 * is configured to support APs and APs for the system start
-	 * in 32bit mode (e.g. SMP bare metal).
+	 * in real mode mode (e.g. SMP bare metal).
 	 */
-	if (init_ops.mp_bootaddress) {
-		if (physmap[1] >= 0x100000000)
-			panic(
-	"Basemem segment is not suitable for AP bootstrap code!");
-		physmap[1] = init_ops.mp_bootaddress(physmap[1] / 1024);
-	}
+	if (init_ops.mp_bootaddress)
+		init_ops.mp_bootaddress(physmap, &physmap_idx);
 
 	/*
 	 * Maxmem isn't the "maximum memory", it's one larger than the
Index: sys/amd64/amd64/mp_machdep.c
===================================================================
--- sys/amd64/amd64/mp_machdep.c
+++ sys/amd64/amd64/mp_machdep.c
@@ -102,18 +102,82 @@
 /*
  * Calculate usable address in base memory for AP trampoline code.
  */
-u_int
-mp_bootaddress(u_int basemem)
+void
+mp_bootaddress(vm_paddr_t *physmap, unsigned int *physmap_idx)
 {
+	unsigned int i;
+	bool allocated;
 
 	bootMP_size = mptramp_end - mptramp_start;
-	boot_address = trunc_page(basemem * 1024); /* round down to 4k boundary */
-	if (((basemem * 1024) - boot_address) < bootMP_size)
-		boot_address -= PAGE_SIZE;	/* not enough, lower by 4k */
-	/* 3 levels of page table pages */
-	mptramp_pagetables = boot_address - (PAGE_SIZE * 3);
+	allocated = false;
+	for (i = *physmap_idx; i <= *physmap_idx; i -= 2) {
+		/*
+		 * Find a memory region big enough and below the 1MB boundary
+		 * for the trampoline code.
+		 * NB: needs to be page aligned.
+		 */
+		if (physmap[i] >= MiB(1) ||
+		    (trunc_page(physmap[i + 1]) - round_page(physmap[i])) <
+		    round_page(bootMP_size))
+			continue;
+
+		allocated = true;
+		/*
+		 * Try to steal from the end of the region to mimic previous
+		 * behaviour, else fallback to steal from the start.
+		 */
+		if (physmap[i + 1] < MiB(1)) {
+			boot_address = trunc_page(physmap[i + 1]);
+			if ((physmap[i + 1] - boot_address) < bootMP_size)
+				boot_address -= round_page(bootMP_size);
+			physmap[i + 1] = boot_address;
+		} else {
+			boot_address = round_page(physmap[i]);
+			physmap[i] = boot_address + round_page(bootMP_size);
+		}
+		if (physmap[i] == physmap[i + 1] && *physmap_idx != 0) {
+			memmove(&physmap[i], &physmap[i + 2],
+			    sizeof(*physmap) * (*physmap_idx - i + 2));
+			*physmap_idx -= 2;
+		}
+	}
+
+	if (!allocated) {
+		boot_address = basemem * 1024 - bootMP_size;
+		if (bootverbose)
+			printf(
+"Cannot find enough space for the boot trampoline, placing it at %#x",
+			    boot_address);
+	}
 
-	return mptramp_pagetables;
+	allocated = false;
+	for (i = *physmap_idx; i <= *physmap_idx; i -= 2) {
+		/*
+		 * Find a memory region big enough below the 4GB boundary to
+		 * store the initial page tables. Note that it needs to be
+		 * aligned to a page boundary.
+		 */
+		if (physmap[i] >= GiB(4) ||
+		    (physmap[i + 1] - round_page(physmap[i])) < (PAGE_SIZE * 3))
+			continue;
+
+		allocated = true;
+		mptramp_pagetables = round_page(physmap[i]);
+		physmap[i] = round_page(physmap[i]) + (PAGE_SIZE * 3);
+		if (physmap[i] == physmap[i + 1] && *physmap_idx != 0) {
+			memmove(&physmap[i], &physmap[i + 2],
+			    sizeof(*physmap) * (*physmap_idx - i + 2));
+			*physmap_idx -= 2;
+		}
+	}
+
+	if (!allocated) {
+		mptramp_pagetables = trunc_page(boot_address) - (PAGE_SIZE * 3);
+		if (bootverbose)
+			printf(
+"Cannot find enough space for the initial AP page tables, placing them at %#x",
+			    mptramp_pagetables);
+	}
 }
 
 /*
Index: sys/i386/i386/machdep.c
===================================================================
--- sys/i386/i386/machdep.c
+++ sys/i386/i386/machdep.c
@@ -1902,7 +1902,7 @@
 
 #ifdef SMP
 	/* make hole for AP bootstrap code */
-	physmap[1] = mp_bootaddress(physmap[1]);
+	mp_bootaddress(physmap, &physmap_idx);
 #endif
 
 	/*
Index: sys/i386/i386/mp_machdep.c
===================================================================
--- sys/i386/i386/mp_machdep.c
+++ sys/i386/i386/mp_machdep.c
@@ -144,15 +144,52 @@
 /*
  * Calculate usable address in base memory for AP trampoline code.
  */
-u_int
-mp_bootaddress(u_int basemem)
+void
+mp_bootaddress(vm_paddr_t *physmap, unsigned int *physmap_idx)
 {
+	unsigned int i;
+	bool allocated;
+
+	allocated = false;
+	for (i = *physmap_idx; i <= *physmap_idx; i -= 2) {
+		/*
+		 * Find a memory region big enough and below the 1MB boundary
+		 * for the trampoline code.
+		 * NB: needs to be page aligned.
+		 */
+		if (physmap[i] >= MiB(1) ||
+		    (trunc_page(physmap[i + 1]) - round_page(physmap[i])) <
+		    round_page(bootMP_size))
+			continue;
+
+		allocated = true;
+		/*
+		 * Try to steal from the end of the region to mimic previous
+		 * behaviour, else fallback to steal from the start.
+		 */
+		if (physmap[i + 1] < MiB(1)) {
+			boot_address = trunc_page(physmap[i + 1]);
+			if ((physmap[i + 1] - boot_address) < bootMP_size)
+				boot_address -= round_page(bootMP_size);
+			physmap[i + 1] = boot_address;
+		} else {
+			boot_address = round_page(physmap[i]);
+			physmap[i] = boot_address + round_page(bootMP_size);
+		}
+		if (physmap[i] == physmap[i + 1] && *physmap_idx != 0) {
+			memmove(&physmap[i], &physmap[i + 2],
+			    sizeof(*physmap) * (*physmap_idx - i + 2));
+			*physmap_idx -= 2;
+		}
+	}
 
-	boot_address = trunc_page(basemem);	/* round down to 4k boundary */
-	if ((basemem - boot_address) < bootMP_size)
-		boot_address -= PAGE_SIZE;	/* not enough, lower by 4k */
-
-	return boot_address;
+	if (!allocated) {
+		boot_address = basemem * 1024 - bootMP_size;
+		if (bootverbose)
+			printf(
+"Cannot find enough space for the boot trampoline, placing it at %#x",
+			    boot_address);
+	}
 }
 
 /*
Index: sys/sys/param.h
===================================================================
--- sys/sys/param.h
+++ sys/sys/param.h
@@ -362,4 +362,8 @@
  */
 #define __PAST_END(array, offset) (((__typeof__(*(array)) *)(array))[offset])
 
+/* Unit conversion macros. */
+#define GiB(v) (v ## ULL << 30)
+#define MiB(v) (v ## ULL << 20)
+
 #endif	/* _SYS_PARAM_H_ */
Index: sys/x86/include/init.h
===================================================================
--- sys/x86/include/init.h
+++ sys/x86/include/init.h
@@ -41,7 +41,7 @@
 	void	(*early_clock_source_init)(void);
 	void	(*early_delay)(int);
 	void	(*parse_memmap)(caddr_t, vm_paddr_t *, int *);
-	u_int	(*mp_bootaddress)(u_int);
+	void	(*mp_bootaddress)(vm_paddr_t *, unsigned int *);
 	int	(*start_all_aps)(void);
 	void	(*msi_init)(void);
 };
Index: sys/x86/include/x86_smp.h
===================================================================
--- sys/x86/include/x86_smp.h
+++ sys/x86/include/x86_smp.h
@@ -95,7 +95,7 @@
 void	ipi_cpu(int cpu, u_int ipi);
 int	ipi_nmi_handler(void);
 void	ipi_selected(cpuset_t cpus, u_int ipi);
-u_int	mp_bootaddress(u_int);
+void	mp_bootaddress(vm_paddr_t *, unsigned int *);
 void	set_interrupt_apic_ids(void);
 void	smp_cache_flush(void);
 void	smp_masked_invlpg(cpuset_t mask, vm_offset_t addr, struct pmap *pmap);