Index: boot/arch/arm64/src/asm.S
===================================================================
--- boot/arch/arm64/src/asm.S	(revision 84176f3d26c423111021bb2b97b70bf0076fb9f4)
+++ boot/arch/arm64/src/asm.S	(revision 95b7d4df17053b0cb97e9406f5af392aeab0f70b)
@@ -33,4 +33,6 @@
 .section BOOTSTRAP
 
+#define DIRECTORY_ENTRIES 16
+
 /* MS-DOS stub */
 msdos_stub:
@@ -51,6 +53,8 @@
 	.long 0                         /* Number of symbols */
 	.short sec_table - opt_header   /* Size of optional header */
-	/* Characteristics = IMAGE_FILE_EXECUTABLE_IMAGE |
-	 *   IMAGE_FILE_LARGE_ADDRESS_AWARE */
+	/*
+	 * Characteristics = IMAGE_FILE_EXECUTABLE_IMAGE |
+	 *   IMAGE_FILE_LARGE_ADDRESS_AWARE
+	 */
 	.short 0x22
 
@@ -87,8 +91,9 @@
 	.quad 0                         /* Size of heap commit */
 	.long 0                         /* Loader flags */
-	.long 0                         /* Number of RVA and sizes */
+	.long DIRECTORY_ENTRIES         /* Number of RVA and sizes */
+	.space DIRECTORY_ENTRIES * 8    /* Directory entries */
 
 sec_table:
-        .ascii ".text\x0\x0\x0"         /* Name */
+	.ascii ".text\x0\x0\x0"         /* Name */
 	.long payload_end - start       /* Virtual size */
 	.long start - msdos_stub        /* Virtual address */
@@ -99,19 +104,21 @@
 	.short 0                        /* Number of relocations */
 	.short 0                        /* Number of line numbers */
-	/* Characteristics = IMAGE_SCN_CNT_CODE | IMAGE_SCN_MEM_EXECUTE |
-	 *   IMAGE_SCN_MEM_READ | IMAGE_SCN_MEM_WRITE */
+	/*
+	 * Characteristics = IMAGE_SCN_CNT_CODE | IMAGE_SCN_MEM_EXECUTE |
+	 *   IMAGE_SCN_MEM_READ | IMAGE_SCN_MEM_WRITE
+	 */
 	.long 0xe0000020
 
+/** Boot loader entry point
+ *
+ * @param x0 UEFI image handle.
+ * @param x1 Pointer to the UEFI system table.
+ *
+ */
 SYMBOL(start)
 	.hidden start
 
 	/*
-	 * Parameters:
-	 * x0 is the image handle.
-	 * x1 is a pointer to the UEFI system table.
-	 */
-
-	/*
-	 * Stay on the UEFI stack. Its size is at least 128 kB, plenty for this
+	 * Stay on the UEFI stack. Its size is at least 128 KiB, plenty for this
 	 * boot loader.
 	 */
@@ -128,5 +135,13 @@
 	add x1, x1, #:lo12:_DYNAMIC
 	bl self_relocate
-	cbnz x0, 0f
+	cbnz x0, __uefi_exit
+
+	/*
+	 * Flush the instruction cache of the relocated boot loader image.
+	 */
+	adr x0, msdos_stub
+	adrp x1, payload_end
+	sub x1, x1, x0
+	bl smc_coherence
 
 	/*
@@ -138,7 +153,7 @@
 	bl bootstrap
 
-0:
-	ldp x29, x30, [sp], #32
-	ret
+	__uefi_exit:
+		ldp x29, x30, [sp], #32
+		ret
 
 FUNCTION_BEGIN(halt)
@@ -148,19 +163,149 @@
 FUNCTION_END(halt)
 
+/** Flush instruction caches
+ *
+ * @param x0 Starting address of the flushing.
+ * @param x1 Number of bytes to flush.
+ *
+ */
+FUNCTION_BEGIN(smc_coherence)
+	.hidden smc_coherence
+
+	/* Initialize loop */
+	mov x9, x0
+	mov x10, xzr
+
+	__dc_loop:
+		/* Data or Unified Cache Line Clean */
+		dc cvau, x9
+		add x9, x9, #4
+		add x10, x10, #4
+		cmp x10, x1
+		blo __dc_loop
+
+	dsb ish
+
+	/* Initialize loop */
+	mov x9, x0
+	mov x10, xzr
+
+	__ic_loop:
+		/* Instruction Cache Line Invalidate */
+		ic ivau, x9
+		add x9, x9, #4
+		add x10, x10, #4
+		cmp x10, x1
+		blo __ic_loop
+
+	dsb ish
+	isb
+	ret
+FUNCTION_END(smc_coherence)
+
+/** Flush data caches
+ *
+ * @param x0 Starting address of the flushing.
+ * @param x1 Number of bytes to flush.
+ *
+ */
+FUNCTION_BEGIN(dcache_flush)
+	.hidden dcache_flush
+
+	mov x9, x0
+	mov x10, xzr
+
+	__dc_flush_loop:
+		/* Data or Unified Cache Line Clean */
+		dc cvau, x9
+		add x9, x9, #4
+		add x10, x10, #4
+		cmp x10, x1
+		blo __dc_flush_loop
+
+	dsb ish
+	isb
+	ret
+FUNCTION_END(dcache_flush)
+
+/** Kernel entry
+ *
+ * @param x0 Kernel entry point.
+ * @param x1 Pointer to the bootinfo structure.
+ *
+ */
 FUNCTION_BEGIN(jump_to_kernel)
 	.hidden jump_to_kernel
 
-	/*
-	 * Parameters:
-	 * x0 is kernel entry point.
-	 * x1 is pointer to the bootinfo structure.
-	 */
-
-	/* Disable MMU (removes the identity mapping provided by UEFI). */
-	mrs x2, sctlr_el1
-	bic x2, x2, #SCTLR_M_FLAG
-	msr sctlr_el1, x2
-	isb
-
-	br x0
+	mrs x9, CurrentEL
+	lsr x9, x9, 2
+
+	cmp x9, #3
+	b.eq __el3
+
+	cmp x9, #2
+	b.eq __el2
+
+	cmp x9, #1
+	b.eq __el1
+
+	b halt
+
+	__el3:
+		msr sctlr_el2, xzr
+		msr hcr_el2, xzr
+		isb
+
+		/* EL2 is AArch64, EL1 is Non-secure World */
+		mov x9, #(1 << 10)
+		orr x9, x9, #(1 << 0)
+		msr scr_el3, x9
+		isb
+
+		/* EL2h */
+		mov x9, #0x9
+		msr spsr_el3, x9
+		isb
+
+		adr x9, __el2
+		msr elr_el3, x9
+		isb
+
+		/* Switch to EL2 */
+		eret
+
+	__el2:
+		msr sctlr_el1, xzr
+		isb
+
+		/* EL1 is AArch64 */
+		mov x9, #(1 << 31)
+		msr hcr_el2, x9
+		isb
+
+		/* EL1h */
+		mov x9, #0x5
+		msr spsr_el2, x9
+		isb
+
+		adr x9, __el1
+		msr elr_el2, x9
+		isb
+
+		/* Switch to EL1 */
+		eret
+
+	__el1:
+		/* Do not trap on FPU instructions */
+		mrs x9, cpacr_el1
+		orr x9, x9, #(3 << 20)
+		msr cpacr_el1, x9
+		dmb ish
+
+		/* Disable MMU (removes the identity mapping provided by UEFI) */
+		mrs x9, sctlr_el1
+		bic x9, x9, #SCTLR_M_FLAG
+		msr sctlr_el1, x9
+		isb
+
+		br x0
 FUNCTION_END(jump_to_kernel)
