Index: sys/arm64/arm64/locore.S
===================================================================
--- sys/arm64/arm64/locore.S
+++ sys/arm64/arm64/locore.S
@@ -36,6 +36,7 @@
 #include <machine/pte.h>
 
 #define	VIRT_BITS		39
+#define	L1_PAGE_COUNT	16
 
 	.globl	kernbase
 	.set	kernbase, KERNBASE
@@ -100,6 +101,13 @@
 	br	x15
 
 virtdone:
+
+	/* Update TCR to value expected by initarm */
+	ldr	x2, tcr
+	mrs	x3, id_aa64mmfr0_el1
+	bfi	x2, x3, #32, #3
+	msr	tcr_el1, x2
+
 	/* Set up the stack */
 	adr	x25, initstack_end
 	mov	sp, x25
@@ -164,10 +172,15 @@
 	/* Set the context id */
 	msr	contextidr_el1, x1
 
+	ldr	x2, tcr_early
+	mrs	x3, id_aa64mmfr0_el1
+	bfi	x2, x3, #32, #3
+	msr	tcr_el1, x2
+
 	/* Load the kernel page table */
 	adr	x26, pagetable_l1_ttbr1
 	/* Load the identity page table */
-	adr	x27, pagetable_l1_ttbr0
+	adr	x27, pagetable_l0_ttbr0
 
 	/* Enable the mmu */
 	bl	start_mmu
@@ -177,6 +190,12 @@
 	br	x15
 
 mp_virtdone:
+
+	ldr	x2, tcr
+	mrs	x3, id_aa64mmfr0_el1
+	bfi	x2, x3, #32, #3
+	msr	tcr_el1, x2
+
 	ldr	x4, =secondary_stacks
 	mov	x5, #(PAGE_SIZE * KSTACK_PAGES)
 	mul	x5, x0, x5
@@ -367,27 +386,63 @@
 	mov	x6, x26
 	bl	link_l1_pagetable
 
-
 	/*
 	 * Build the TTBR0 maps.
 	 */
-	add	x27, x26, #PAGE_SIZE
-
-#if defined(SOCDEV_PA) && defined(SOCDEV_VA)
-	/* Create a table for the UART */
-	mov	x6, x27		/* The initial page table */
-	mov	x7, #DEVICE_MEM
-	mov	x8, #(SOCDEV_VA)	/* VA start */
-	mov	x9, #(SOCDEV_PA)	/* PA start */
-	bl	build_section_pagetable
-#endif
+	adr	x27, pagetable_l0_ttbr0
+
+	/* Map L1 tables to L0 */
+	mov	x14, #0
+	mov	x15, #L1_PAGE_COUNT
+	adr	x8, pagetable_l1_ttbr0
+1:
+	/* pagetable_l1_ttbr0
+	 *  x6  = L0 table
+	 *  x8  = Virtual Address
+	 *  x9  = L1 PA (trashed)
+	 *  x11, x12 and x13 are trashed
+	 */
+	adr	x9, pagetable_l1_ttbr0
+	mov	x8, x14
+	lsl	x8, x8, #12
+	add x9, x9, x8
+
+	mov	x8, x14
+	lsl	x8, x8, L0_SHIFT
+
+	mov	x6, x27
+	bl link_l0_pagetable
+	add	x8, x8, #PAGE_SIZE
+	add	x14, x14, #1
+	cmp	x14, x15
+	b.lo	1b
 
-	/* Create the VA = PA map */
-	mov	x6, x27		/* The initial page table */
+	/* Build multiple 1GB identity maps for early boot */
+	mov	x14, #0
+	mov	x15, #(Ln_ENTRIES*L1_PAGE_COUNT)
+	adr	x25, pagetable_l1_ttbr0
+	mov	x8, 0
+1:
+	/*
+	 * Create the VA = PA map
+	 *  x6 = L1 table
+	 *  x7 = Type (0 = Device, 1 = Normal)
+	 *  x8 = VA start
+	 *  x9 = PA start (trashed)
+	 *  x11, x12 and x13 are trashed
+	 */
+	mov	x6, x14
+	lsl	x6, x6, #3
+	and	x6, x6, #(~(PAGE_SIZE - 1))
+	add	x6, x6, x25		/* The initial page table */
 	mov	x7, #NORMAL_UNCACHED /* Uncached as it's only needed early on */
-	mov	x9, x27
-	mov	x8, x9		/* VA start (== PA start) */
+	mov	x9, x8		/* VA start (== PA start) */
 	bl	build_section_pagetable
+	add	x14, x14, #1
+	mov	x9, #L1_SIZE
+	add	x8, x8, x9
+	cmp	x14, x15
+	b.lo	1b
 
 	/* Restore the Link register */
 	mov	x30, x5
@@ -424,6 +479,37 @@
 	ret
 
 /*
+ * Builds an L0 -> L1 table descriptor
+ *
+ * This is a link for a 512GiB block of memory with up to 1GiB regions mapped
+ * within it by build_block_pagetable.
+ *
+ *  x6  = L0 table
+ *  x8  = Virtual Address
+ *  x9  = L1 PA (trashed)
+ *  x11, x12 and x13 are trashed
+ */
+link_l0_pagetable:
+	/*
+	 * Link an L0 -> L1 table entry.
+	 */
+	/* Find the table index */
+	lsr	x11, x8, #L0_SHIFT
+	and	x11, x11, #Ln_ADDR_MASK
+
+	/* Build the L1 block entry */
+	mov	x12, #L0_TABLE
+
+	/* Only use the output address bits */
+	lsr	x9, x9, #12
+	orr	x12, x12, x9, lsl #12
+
+	/* Store the entry */
+	str	x12, [x6, x11, lsl #3]
+
+	ret
+
+/*
  * Builds an L1 -> L2 table descriptor
  *
  * This is a link for a 1GiB block of memory with up to 2MiB regions mapped
@@ -520,7 +606,7 @@
 	msr	mair_el1, x2
 
 	/* Setup TCR according to PARange bits from ID_AA64MMFR0_EL1 */
-	ldr	x2, tcr
+	ldr	x2, tcr_early
 	mrs	x3, id_aa64mmfr0_el1
 	bfi	x2, x3, #32, #3
 	msr	tcr_el1, x2
@@ -543,6 +629,9 @@
 tcr:
 	.quad (TCR_TxSZ(64 - VIRT_BITS) | TCR_ASID_16 | TCR_TG1_4K | \
 	    TCR_CACHE_ATTRS | TCR_SMP_ATTRS)
+tcr_early:
+	.quad (((64 - VIRT_BITS) << TCR_T1SZ_SHIFT) | ((64 - 48) << TCR_T0SZ_SHIFT) | \
+	    TCR_ASID_16 | TCR_TG1_4K | TCR_CACHE_ATTRS | TCR_SMP_ATTRS)
 sctlr_set:
 	/* Bits to set */
 	.quad (SCTLR_UCI | SCTLR_nTWE | SCTLR_nTWI | SCTLR_UCT | SCTLR_DZE | \
@@ -569,6 +658,8 @@
 pagetable_l1_ttbr1:
 	.space	PAGE_SIZE
 pagetable_l1_ttbr0:
+	.space	L1_PAGE_COUNT*PAGE_SIZE
+pagetable_l0_ttbr0:
 	.space	PAGE_SIZE
 pagetable_end: