Index: boot/arch/arm64/include/arch/barrier.h
===================================================================
--- boot/arch/arm64/include/arch/barrier.h	(revision 99589a9a53db598a3a043217657db813eea50a62)
+++ boot/arch/arm64/include/arch/barrier.h	(revision 95b7d4df17053b0cb97e9406f5af392aeab0f70b)
@@ -1,1 +1,47 @@
-../../../../../kernel/arch/arm64/include/arch/barrier.h
+/*
+ * Copyright (c) 2015 Petr Pavlu
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @addtogroup boot_arm64
+ * @{
+ */
+/** @file
+ * @brief Memory barriers.
+ */
+
+#ifndef BOOT_arm64_BARRIER_H_
+#define BOOT_arm64_BARRIER_H_
+
+#include <stddef.h>
+
+extern void smc_coherence(void *, size_t);
+extern void dcache_flush(void *, size_t);
+
+#endif
+
+/** @}
+ */
Index: boot/arch/arm64/src/asm.S
===================================================================
--- boot/arch/arm64/src/asm.S	(revision 99589a9a53db598a3a043217657db813eea50a62)
+++ boot/arch/arm64/src/asm.S	(revision 95b7d4df17053b0cb97e9406f5af392aeab0f70b)
@@ -135,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
 
 	/*
@@ -145,7 +153,7 @@
 	bl bootstrap
 
-0:
-	ldp x29, x30, [sp], #32
-	ret
+	__uefi_exit:
+		ldp x29, x30, [sp], #32
+		ret
 
 FUNCTION_BEGIN(halt)
@@ -155,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)
Index: boot/arch/arm64/src/main.c
===================================================================
--- boot/arch/arm64/src/main.c	(revision 99589a9a53db598a3a043217657db813eea50a62)
+++ boot/arch/arm64/src/main.c	(revision 95b7d4df17053b0cb97e9406f5af392aeab0f70b)
@@ -116,5 +116,4 @@
 {
 	efi_status_t status;
-	uint64_t current_el;
 	uint64_t memmap = 0;
 	sysarg_t memmap_size;
@@ -143,13 +142,4 @@
 	printf(" %p|%p: UEFI system table\n", efi_system_table_in,
 	    efi_system_table_in);
-
-	/* Validate the exception level. */
-	current_el = CurrentEL_read();
-	if (current_el != CURRENT_EL_EL1) {
-		printf("Error: Unexpected CurrentEL value %0#18" PRIx64 ".\n",
-		    current_el);
-		status = EFI_UNSUPPORTED;
-		goto fail;
-	}
 
 	/* Obtain memory map. */
@@ -187,5 +177,5 @@
 
 	/*
-	 * Check that everything is aligned on a 4kB boundary and the kernel can
+	 * Check that everything is aligned on a 4 KiB boundary and the kernel can
 	 * be placed by the decompression code at a correct address.
 	 */
@@ -199,9 +189,9 @@
 	 * Dynamically check the memory base. The condition should be always
 	 * true because UEFI guarantees each physical/virtual address in the
-	 * memory map is aligned on a 4kB boundary.
+	 * memory map is aligned on a 4 KiB boundary.
 	 */
 	if (!IS_ALIGNED(memory_base, PAGE_SIZE)) {
 		printf("Error: Start of usable RAM (%p) is not aligned on a "
-		    "4kB boundary.\n", (void *) memory_base);
+		    "4 KiB boundary.\n", (void *) memory_base);
 		status = EFI_UNSUPPORTED;
 		goto fail;
@@ -243,5 +233,5 @@
 
 	extract_payload(&bootinfo->taskmap, kernel_dest, ram_end,
-	    (uintptr_t) kernel_dest, ensure_visibility);
+	    (uintptr_t) kernel_dest, smc_coherence);
 
 	/* Get final memory map. */
@@ -301,4 +291,7 @@
 	bootinfo->memmap.cnt = cnt;
 
+	/* Flush the data cache containing bootinfo. */
+	dcache_flush(bootinfo, sizeof(*bootinfo));
+
 	uintptr_t entry = check_kernel_translated((void *) decompress_base,
 	    BOOT_OFFSET);