diff --git a/cmake-tool/helpers/application_settings.cmake b/cmake-tool/helpers/application_settings.cmake
index 01580404..e4365c04 100644
--- a/cmake-tool/helpers/application_settings.cmake
+++ b/cmake-tool/helpers/application_settings.cmake
@@ -14,7 +14,7 @@ function(ApplyData61ElfLoaderSettings kernel_platform kernel_sel4_arch)
         binary_list
         "tx1;hikey;odroidc2;odroidc4;imx8mq-evk;imx8mm-evk;hifive;tqma8xqp1gb;bcm2711;rocketchip"
     )
-    set(efi_list "tk1;rockpro64;quartz64")
+    set(efi_list "tk1;rockpro64;quartz64;orin")
     set(uimage_list "tx2;am335x")
     if(
         ${kernel_platform} IN_LIST efi_list
diff --git a/elfloader-tool/include/arch-arm/64/mode/aarch64.h b/elfloader-tool/include/arch-arm/64/mode/aarch64.h
new file mode 100644
index 00000000..89fdcfd6
--- /dev/null
+++ b/elfloader-tool/include/arch-arm/64/mode/aarch64.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright 2023, NIO GmbH
+ *
+ * SPDX-License-Identifier: GPL-2.0-only
+ */
+#pragma once
+
+/* This file contains useful defines for assembly and C code. */
+
+#define PSR_F_BIT         0x00000040
+#define PSR_I_BIT         0x00000080
+#define PSR_A_BIT         0x00000100
+#define PSR_D_BIT         0x00000200
+
+#define PSR_MODE_EL0t     0x00000000
+#define PSR_MODE_EL1t     0x00000004
+#define PSR_MODE_EL1h     0x00000005
+#define PSR_MODE_EL2t     0x00000008
+#define PSR_MODE_EL2h     0x00000009
+#define PSR_MODE_SVC_32   0x00000013
+
+#define TCR_T0SZ(x)       ((64 - (x)))
+#define TCR_T1SZ(x)       ((64 - (x)) << 16)
+#define TCR_TxSZ(x)       (TCR_T0SZ(x) | TCR_T1SZ(x))
+
+#define TCR_IRGN0_WBWC    (1 << 8)
+#define TCR_IRGN_NC       ((0 << 8) | (0 << 24))
+#define TCR_IRGN_WBWA     ((1 << 8) | (1 << 24))
+#define TCR_IRGN_WT       ((2 << 8) | (2 << 24))
+#define TCR_IRGN_WBnWA    ((3 << 8) | (3 << 24))
+#define TCR_IRGN_MASK     ((3 << 8) | (3 << 24))
+
+#define TCR_ORGN0_WBWC    (1 << 10)
+#define TCR_ORGN_NC       ((0 << 10) | (0 << 26))
+#define TCR_ORGN_WBWA     ((1 << 10) | (1 << 26))
+#define TCR_ORGN_WT       ((2 << 10) | (2 << 26))
+#define TCR_ORGN_WBnWA    ((3 << 10) | (3 << 26))
+#define TCR_ORGN_MASK     ((3 << 10) | (3 << 26))
+
+#define TCR_SH0_ISH       (3 << 12)
+#define TCR_SHARED        ((3 << 12) | (3 << 28))
+
+#define TCR_TG0_4K        (0 << 14)
+#define TCR_TG0_64K       (1 << 14)
+#define TCR_TG1_4K        (2 << 30)
+#define TCR_TG1_64K       (3 << 30)
+
+#define TCR_PS_4G         (0 << 16)
+#define TCR_PS_64G        (1 << 16)
+#define TCR_PS_1T         (2 << 16)
+#define TCR_PS_4T         (3 << 16)
+#define TCR_PS_16T        (4 << 16)
+#define TCR_PS_256T       (5 << 16)
+
+/* bits are reserved as 1 */
+#define TCR_EL2_RES1      ((1 << 23) | (1 << 31))
+#define TCR_ASID16        (1 << 36)
+
+#define MT_DEVICE_nGnRnE  0
+#define MT_DEVICE_nGnRE   1
+#define MT_DEVICE_GRE     2
+#define MT_NORMAL_NC      3
+#define MT_NORMAL         4
+#define MT_NORMAL_WT      5
+#define MAIR(_attr, _mt)  ((_attr) << ((_mt) * 8))
+
+#define IS_DEV_MEM_INDEX(_idx) ((_idx) <= MT_DEVICE_GRE)
diff --git a/elfloader-tool/include/arch-arm/64/mode/assembler.h b/elfloader-tool/include/arch-arm/64/mode/assembler.h
index 4f9972c0..1d235a7e 100644
--- a/elfloader-tool/include/arch-arm/64/mode/assembler.h
+++ b/elfloader-tool/include/arch-arm/64/mode/assembler.h
@@ -9,65 +9,17 @@
 /* This file contains useful macros for assembly code. */
 
 #ifdef __ASSEMBLER__
+#include <mode/aarch64.h>
 
-#define PSR_F_BIT         0x00000040
-#define PSR_I_BIT         0x00000080
-#define PSR_A_BIT         0x00000100
-#define PSR_D_BIT         0x00000200
-
-#define PSR_MODE_EL0t     0x00000000
-#define PSR_MODE_EL1t     0x00000004
-#define PSR_MODE_EL1h     0x00000005
-#define PSR_MODE_EL2t     0x00000008
-#define PSR_MODE_EL2h     0x00000009
-#define PSR_MODE_SVC_32   0x00000013
-
-#define TCR_T0SZ(x)       ((64 - (x)))
-#define TCR_T1SZ(x)       ((64 - (x)) << 16)
-#define TCR_TxSZ(x)       (TCR_T0SZ(x) | TCR_T1SZ(x))
-
-#define TCR_IRGN0_WBWC    (1 << 8)
-#define TCR_IRGN_NC       ((0 << 8) | (0 << 24))
-#define TCR_IRGN_WBWA     ((1 << 8) | (1 << 24))
-#define TCR_IRGN_WT       ((2 << 8) | (2 << 24))
-#define TCR_IRGN_WBnWA    ((3 << 8) | (3 << 24))
-#define TCR_IRGN_MASK     ((3 << 8) | (3 << 24))
-
-#define TCR_ORGN0_WBWC    (1 << 10)
-#define TCR_ORGN_NC       ((0 << 10) | (0 << 26))
-#define TCR_ORGN_WBWA     ((1 << 10) | (1 << 26))
-#define TCR_ORGN_WT       ((2 << 10) | (2 << 26))
-#define TCR_ORGN_WBnWA    ((3 << 10) | (3 << 26))
-#define TCR_ORGN_MASK     ((3 << 10) | (3 << 26))
-
-#define TCR_SH0_ISH       (3 << 12)
-#define TCR_SHARED        ((3 << 12) | (3 << 28))
-
-#define TCR_TG0_4K        (0 << 14)
-#define TCR_TG0_64K       (1 << 14)
-#define TCR_TG1_4K        (2 << 30)
-#define TCR_TG1_64K       (3 << 30)
-
-#define TCR_PS_4G         (0 << 16)
-#define TCR_PS_64G        (1 << 16)
-#define TCR_PS_1T         (2 << 16)
-#define TCR_PS_4T         (3 << 16)
-#define TCR_PS_16T        (4 << 16)
-#define TCR_PS_256T       (5 << 16)
-
-/* bits are reserved as 1 */
-#define TCR_EL2_RES1      ((1 << 23) | (1 << 31))
-#define TCR_ASID16        (1 << 36)
-
-#define MT_DEVICE_nGnRnE  0
-#define MT_DEVICE_nGnRE   1
-#define MT_DEVICE_GRE     2
-#define MT_NORMAL_NC      3
-#define MT_NORMAL         4
-#define MT_NORMAL_WT      5
-#define MAIR(_attr, _mt)  ((_attr) << ((_mt) * 8))
+.macro disable_mmu sctlr tmp
+    __disable_mmu \sctlr, \tmp
+    ic      ialluis
+    dsb     sy
+    isb
+.endm
 
 .macro enable_mmu sctlr tmp
+    dsb     sy
     mrs     \tmp, \sctlr
     orr     \tmp, \tmp, #(1 << 0)
     orr     \tmp, \tmp, #(1 << 2)
@@ -76,17 +28,11 @@
     isb
 .endm
 
-.macro disable_mmu sctlr tmp
-    mrs     \tmp, \sctlr
-    bic     \tmp, \tmp, #(1 << 0)
-    bic     \tmp, \tmp, #(1 << 2)
-    bic     \tmp, \tmp, #(1 << 12)
-    msr     \sctlr, \tmp
+.macro __disable_mmu sctlr tmp
+    dsb     sy
     isb
-.endm
-
-.macro disable_id_cache sctlr tmp
     mrs     \tmp, \sctlr
+    bic     \tmp, \tmp, #(1 << 0)
     bic     \tmp, \tmp, #(1 << 2)
     bic     \tmp, \tmp, #(1 << 12)
     msr     \sctlr, \tmp
diff --git a/elfloader-tool/include/arch-arm/64/mode/structures.h b/elfloader-tool/include/arch-arm/64/mode/structures.h
index f77ef93d..dbc7a49f 100644
--- a/elfloader-tool/include/arch-arm/64/mode/structures.h
+++ b/elfloader-tool/include/arch-arm/64/mode/structures.h
@@ -6,6 +6,12 @@
 
 #pragma once
 
+/* ARM VMSAv8-64 (with a fully populated last level) has the same number of PTEs
+ * in all levels (we don't use concatenated pagetables in ELFloader) and each
+ * table entry is always eight bytes large.
+ */
+#define BITS_PER_LEVEL          (PAGE_BITS - 3)
+
 #define ARM_1GB_BLOCK_BITS      30
 #define ARM_2MB_BLOCK_BITS      21
 
@@ -21,14 +27,10 @@
 #define PMD_BITS                9
 #define PMD_SIZE_BITS           (PMD_BITS + PMDE_SIZE_BITS)
 
-#define GET_PGD_INDEX(x)        (((x) >> (ARM_2MB_BLOCK_BITS + PMD_BITS + PUD_BITS)) & MASK(PGD_BITS))
-#define GET_PUD_INDEX(x)        (((x) >> (ARM_2MB_BLOCK_BITS + PMD_BITS)) & MASK(PUD_BITS))
-#define GET_PMD_INDEX(x)        (((x) >> (ARM_2MB_BLOCK_BITS)) & MASK(PMD_BITS))
+#define GET_PGD_INDEX(x)        (((word_t)(x) >> (ARM_2MB_BLOCK_BITS + PMD_BITS + PUD_BITS)) & MASK(PGD_BITS))
+#define GET_PUD_INDEX(x)        (((word_t)(x) >> (ARM_2MB_BLOCK_BITS + PMD_BITS)) & MASK(PUD_BITS))
+#define GET_PMD_INDEX(x)        (((word_t)(x) >> (ARM_2MB_BLOCK_BITS)) & MASK(PMD_BITS))
 
 extern uint64_t _boot_pgd_up[BIT(PGD_BITS)];
-extern uint64_t _boot_pud_up[BIT(PUD_BITS)];
-extern uint64_t _boot_pmd_up[BIT(PMD_BITS)];
-
 extern uint64_t _boot_pgd_down[BIT(PGD_BITS)];
-extern uint64_t _boot_pud_down[BIT(PUD_BITS)];
 
diff --git a/elfloader-tool/include/arch-arm/cpuid.h b/elfloader-tool/include/arch-arm/cpuid.h
index f84612be..c0e1a6ce 100644
--- a/elfloader-tool/include/arch-arm/cpuid.h
+++ b/elfloader-tool/include/arch-arm/cpuid.h
@@ -12,7 +12,7 @@
 uint32_t read_cpuid_id(void);
 
 /* read MP ID register from CPUID */
-uint32_t read_cpuid_mpidr(void);
+word_t read_cpuid_mpidr(void);
 
 /* check if CPU is in HYP/EL2 mode */
 word_t is_hyp_mode(void);
diff --git a/elfloader-tool/include/arch-arm/elfloader.h b/elfloader-tool/include/arch-arm/elfloader.h
index 93293a75..ceab4796 100644
--- a/elfloader-tool/include/arch-arm/elfloader.h
+++ b/elfloader-tool/include/arch-arm/elfloader.h
@@ -22,7 +22,22 @@ typedef void (*init_arm_kernel_t)(word_t ui_p_reg_start,
 
 /* Enable the mmu. */
 extern void arm_enable_mmu(void);
+
+/* These functions are very similar however, there are some small differences
+ * between the ARMv8 and legacy implementation.
+ *
+ * New ARMv8 implementation:
+ * - Does the MMU disabling. This is to keep the time spent with MMU off low.
+ * - Is only meant if seL4 runs in EL2.
+ */
+#if defined(CONFIG_ARCH_AARCH64)
+/* Switches MMU-related stuff: pagetables, MAIR & TCR etc. Works also if the MMU
+ * was off initially. EL2 translation regime only.
+ */
+extern void arm_switch_to_hyp_tables(void);
+#else
 extern void arm_enable_hyp_mmu(void);
+#endif
 
 
 /* Setup boot VSpace. */
diff --git a/elfloader-tool/include/drivers/uart.h b/elfloader-tool/include/drivers/uart.h
index 1fa9f970..74ce4b16 100644
--- a/elfloader-tool/include/drivers/uart.h
+++ b/elfloader-tool/include/drivers/uart.h
@@ -6,6 +6,7 @@
 
 #pragma once
 
+#include <autoconf.h>
 #include <drivers/common.h>
 
 #define dev_get_uart(dev) ((struct elfloader_uart_ops *)(dev->drv->ops))
@@ -16,3 +17,7 @@ struct elfloader_uart_ops {
 
 volatile void *uart_get_mmio(void);
 void uart_set_out(struct elfloader_device *out);
+#if defined(CONFIG_ARCH_AARCH64)
+/* Implemented in mmu.c */
+void mmu_set_uart_base(volatile void *base);
+#endif
diff --git a/elfloader-tool/include/elfloader_common.h b/elfloader-tool/include/elfloader_common.h
index 7a54c316..c80f143c 100644
--- a/elfloader-tool/include/elfloader_common.h
+++ b/elfloader-tool/include/elfloader_common.h
@@ -14,7 +14,7 @@ typedef uintptr_t vaddr_t;
 
 #define PAGE_BITS           12
 
-#define BIT(x)              (1 << (x))
+#define BIT(x)              (1ul << (x))
 #define MASK(n)             (BIT(n) - 1)
 #define MIN(a, b)           (((a) < (b)) ? (a) : (b))
 #define IS_ALIGNED(n, b)    (!((n) & MASK(b)))
diff --git a/elfloader-tool/src/arch-arm/64/cpuid.c b/elfloader-tool/src/arch-arm/64/cpuid.c
index 66d9d09b..6c979a0f 100644
--- a/elfloader-tool/src/arch-arm/64/cpuid.c
+++ b/elfloader-tool/src/arch-arm/64/cpuid.c
@@ -6,6 +6,7 @@
 
 #include <armv/machine.h>
 #include <types.h>
+#include <cpuid.h>
 
 /* we only care about the affinity bits */
 #define MPIDR_MASK (0xff00ffffff)
diff --git a/elfloader-tool/src/arch-arm/64/crt0.S b/elfloader-tool/src/arch-arm/64/crt0.S
index acd4de92..7d1249be 100644
--- a/elfloader-tool/src/arch-arm/64/crt0.S
+++ b/elfloader-tool/src/arch-arm/64/crt0.S
@@ -29,7 +29,7 @@ BEGIN_FUNC(_start)
     bl      fixup_image_base
     mov     x2, x0
     /* restore original arguments for next step */
-    ldp     x0, x1, [sp, #-16]!
+    ldp     x0, x1, [sp], #16
     /* fixup_image_base returns 1 if no need to move */
     cmp     x2, #1
     beq     1f
diff --git a/elfloader-tool/src/arch-arm/64/mmu.c b/elfloader-tool/src/arch-arm/64/mmu.c
index 75d3b0a5..149f2f32 100644
--- a/elfloader-tool/src/arch-arm/64/mmu.c
+++ b/elfloader-tool/src/arch-arm/64/mmu.c
@@ -11,81 +11,418 @@
 #include <mode/structures.h>
 #include <printf.h>
 #include <abort.h>
+#include <assert.h>
+#include <mode/aarch64.h>
+#include <armv/machine.h>     /* dsb() */
+#include <drivers/uart.h>
 
-/*
-* Create a "boot" page table, which contains a 1:1 mapping below
-* the kernel's first vaddr, and a virtual-to-physical mapping above the
-* kernel's first vaddr.
-*/
-void init_boot_vspace(struct image_info *kernel_info)
+/* Note: "typeof()" is a GCC extension that is supported by Clang, too. */
+#define READ_ONCE(x)    (*(const volatile typeof(x) *)&(x))
+#define WRITE_ONCE(var, value) \
+    *((volatile typeof(var) *)(&(var))) = (value);
+
+
+//#define DEBUG_PAGETABLES
+
+#ifndef DEBUG_PAGETABLES
+#define dbg_printf(...) /* empty */
+static void dgb_print_2M_mapping_details(const char *map_name UNUSED,
+                                         paddr_t pa UNUSED, size_t size UNUSED) {}
+#else
+#define dbg_printf(...) printf(__VA_ARGS__)
+
+static int dgb_print_2M_mapping_indices(paddr_t pa)
 {
-    word_t i;
+    return printf("%u.%u.%u.X",
+                  GET_PGD_INDEX(pa),
+                  GET_PUD_INDEX(pa),
+                  GET_PMD_INDEX(pa));
+}
 
-    vaddr_t first_vaddr = kernel_info->virt_region_start;
-    vaddr_t last_vaddr = kernel_info->virt_region_end;
-    paddr_t first_paddr = kernel_info->phys_region_start;
+static void dgb_print_2M_mapping_details(const char *map_name, paddr_t pa, size_t size)
+{
+    int cnt = 0;
+    paddr_t pa_start = pa;
+    size_t orig_sz   = size;
 
-    _boot_pgd_down[0] = ((uintptr_t)_boot_pud_down) | BIT(1) | BIT(0); /* its a page table */
+    pa    = ROUND_DOWN(pa, ARM_2MB_BLOCK_BITS);
+    size += (pa_start - pa);
+    size  = ROUND_UP(size, ARM_2MB_BLOCK_BITS);
 
-    for (i = 0; i < BIT(PUD_BITS); i++) {
-        _boot_pud_down[i] = (i << ARM_1GB_BLOCK_BITS)
-                            | BIT(10) /* access flag */
-                            | (0 << 2) /* strongly ordered memory */
-                            | BIT(0); /* 1G block */
+    cnt += dgb_print_2M_mapping_indices(pa);
+    if (orig_sz) {
+        while (cnt < 11) {
+            printf(" ");
+            cnt++;
+        }
+        cnt += printf("--");
+        while (cnt < 16) {
+            printf(" ");
+            cnt++;
+        }
+        cnt += dgb_print_2M_mapping_indices(pa + size - 1);
+    }
+    while (cnt < 27) {
+        printf(" ");
+        cnt++;
+    }
+    if (orig_sz) {
+        printf("PA 0x%lx - 0x%lx (size: %lu MiB): %s\n", pa, pa + size - 1, size / 1024u / 1024, map_name);
+    } else {
+        /* No range given, just a single 2 MiB page */
+        printf("PA 0x%lx: %s\n", pa, map_name);
     }
+}
+#endif /* DEBUG_PAGETABLES */
 
-    _boot_pgd_up[GET_PGD_INDEX(first_vaddr)]
-        = ((uintptr_t)_boot_pud_up) | BIT(1) | BIT(0); /* its a page table */
+/* Page allocator. Contains a fixed number of pages. All page-aligned. No returning possible. */
 
-    _boot_pud_up[GET_PUD_INDEX(first_vaddr)]
-        = ((uintptr_t)_boot_pmd_up) | BIT(1) | BIT(0); /* its a page table */
+#define NUM_PAGES    7
+static char pages[BIT(PAGE_BITS) * NUM_PAGES] ALIGN(BIT(PGD_SIZE_BITS));
+static unsigned page_cnt;
 
-    /* We only map in 1 GiB, so check that the kernel doesn't cross 1GiB boundary. */
-    if ((first_vaddr & ~MASK(ARM_1GB_BLOCK_BITS)) != (last_vaddr & ~MASK(ARM_1GB_BLOCK_BITS))) {
-        printf("We only map 1GiB, but kernel vaddr range covers multiple GiB.\n");
-        abort();
+static void *get_page(void)
+{
+    void *ret = NULL;
+
+    if (page_cnt == 0) {
+        dbg_printf("get_page(): pages @ 0x%p\n", pages);
     }
-    for (i = GET_PMD_INDEX(first_vaddr); i < BIT(PMD_BITS); i++) {
-        _boot_pmd_up[i] = first_paddr
-                          | BIT(10) /* access flag */
-#if CONFIG_MAX_NUM_NODES > 1
-                          | (3 << 8) /* make sure the shareability is the same as the kernel's */
-#endif
-                          | (4 << 2) /* MT_NORMAL memory */
-                          | BIT(0); /* 2M block */
-        first_paddr += BIT(ARM_2MB_BLOCK_BITS);
+
+    if (page_cnt < NUM_PAGES) {
+        ret = &pages[BIT(PAGE_BITS) * page_cnt];
+        dbg_printf("get_page(): ret: 0x%p (%u->%u)\n", ret, page_cnt, page_cnt + 1);
+        page_cnt ++;
     }
+
+    return ret;
 }
 
-void init_hyp_boot_vspace(struct image_info *kernel_info)
+/* Translate a PA to a VA such that when accessing the VA we end up at that PA.
+ * Usually done in OS kernels via a physical memory map which has a constant
+ * virt-to-phys offset. Here this is the same, since either the MMU is off or
+ * we're running on the identity mapping.
+ */
+static inline uint64_t pa_to_va(uint64_t pa)
+{
+    return pa;
+}
+
+static inline uint64_t va_to_pa(uint64_t va)
 {
-    word_t i;
-    word_t pmd_index;
+    return va;
+}
+
+typedef uint64_t pte_t;
+
+/* This can be used to clear unwanted bits from a PA that is supposed to be put
+ * into a PTE/PDE; or it can be used to extract the PA from a PTE/PDE.
+ */
+static inline uint64_t mask_pa(uint64_t pa)
+{
+    /* Mask out the upper 16 bits and lower 12 bits. Only 48-bit OA for now. */
+    return (pa & 0x0000FFFFFFFFF000);
+}
+
+static inline uintptr_t pde_to_paddr(uint64_t pde_val)
+{
+    /* ARM DDI ARM DDI 0487I.a, page D8-5124 */
+    return mask_pa(pde_val);
+}
+
+static inline uint64_t make_pde(uintptr_t pa)
+{
+    /* For now we set all (upper) attributes to zero */
+    return (mask_pa(pa) | BIT(1) | BIT(0));
+}
+
+/* Accept a pointer, otherwise same as make_pde() */
+static inline uint64_t make_pde_from_ptr(pte_t *pagetable_target)
+{
+    return make_pde(va_to_pa((uintptr_t)pagetable_target));
+}
+
+/* ARM DDI 0487J.a, section D8.5.2 */
+#define INNER_SHAREABLE   3
+static inline uint64_t make_pte(paddr_t pa, uint8_t mem_attr_index)
+{
+    /* As per R_PYFVQ from the ARM spec, we can always safely set the shareability
+     * to inner, even for Device memory type.
+     * For exec-never bit(s), see Table D8-46 for EL2 translation regime (TR)
+     * and Table D8-45 for all others.
+     * PXN (bit 53) is RES0 for EL2 TR (Figure D8-16), so we simply always set it.
+     */
+    uint64_t xn = (IS_DEV_MEM_INDEX(mem_attr_index)) ? (BIT(54) | BIT(53)) : 0;
+    return xn
+           | mask_pa(pa)
+           | BIT(10) /* access flag */
+           | (INNER_SHAREABLE << 8)
+           | (mem_attr_index << 2)
+           | BIT(0); /* valid page/block mapping */
+}
+
+static inline _Bool pte_is_valid(pte_t pte)
+{
+    return (pte & 1);
+}
+
+static inline _Bool pte_is_block(pte_t pte)
+{
+    return ((pte & 3) == 1);
+}
+
+/* Take care about atomicity */
+static inline void pte_set(pte_t *ptep, pte_t val)
+{
+    WRITE_ONCE(*ptep, val);
+}
+
+static inline pte_t pte_get(pte_t *ptep)
+{
+    return READ_ONCE(*ptep);
+}
+
+static_assert(PGD_BITS == BITS_PER_LEVEL, "Mismatch in expected pagetable size");
+static_assert(PUD_BITS == BITS_PER_LEVEL, "Mismatch in expected pagetable size");
+static_assert(PMD_BITS == BITS_PER_LEVEL, "Mismatch in expected pagetable size");
+/* ARM VMSAv8-64: Each table entry is always eight bytes large */
+static_assert(PAGE_BITS == (BITS_PER_LEVEL + 3), "Mismatch in expected page size");
+
+/* A valid PA can be maximum 48 or 52 bit large, so upper bits are always zero */
+#define INVALID_PA     ((uint64_t)-1)
+static paddr_t walk_pagetables(vaddr_t va, uint64_t *l0_table,
+                               unsigned *level, pte_t **fault_pde)
+{
+    paddr_t ret = INVALID_PA;
+    /* All levels have the same size and therefore number of index bits
+     * (9 for 4kiB Translation Granule) on ARMv8.
+     */
+    uint64_t index_mask_bits = PGD_BITS + PUD_BITS + PMD_BITS + PAGE_BITS;
+    uint64_t *tbl = l0_table;
+
+    unsigned idx, lvl;
+    paddr_t pa;
+    pte_t pte;
+
+    /* Walk up to four levels */
+    for (lvl = 0; lvl <= 3; lvl++) {
+        idx = (va >> index_mask_bits) & MASK(BITS_PER_LEVEL);
+        pte = pte_get(&tbl[idx]);
+
+        if (!pte_is_valid(pte)) {
+            goto err_out;
+        } else if (pte_is_block(pte)) {
+            /* L0 giant pages (512 GiB) are not allowed by the architecture for
+             * 4kiB Granule size and 48 bit OA. We don't support 52 bit OA.
+             */
+            if (lvl == 0) {
+                goto err_out;
+            }
+            break;
+        }
+        if (lvl == 3) {
+            /* ARM DDI 0487I.a, page D8-5126 (I_WYRBP), D8-5131 (I_VKPKF):
+             * If the PTE in the last level is valid, it is interpreted as a page
+             * table, irrespectively of bit 1. This allows for the "loopback
+             * trick" - described in every (good) OS lecture at university :-)
+             * Other architectures like RISC-V have screwed this up with their
+             * pagetable format.
+             */
+            break;
+        }
+        /* We have a table descriptor. Descent to the next lower level */
+        pa = pde_to_paddr(pte);
+        vaddr_t va_next = pa_to_va(pa);
+        tbl = (uint64_t *)va_next;
+
+        index_mask_bits -= BITS_PER_LEVEL;
+    }
+
+    ret = (pa | (va & (MASK(index_mask_bits))));
+
+err_out:
+    *level     = lvl;
+    *fault_pde = &tbl[idx];
+    return ret;
+}
+
+/* Returns NULL if there is already something mappped at the requested VA. Fills
+ * in page tables if needed until the desired level is reached.
+ */
+static pte_t *fill_pt_tree(vaddr_t va, uint64_t *l0_table, unsigned target_lvl)
+{
+    paddr_t pa;
+    unsigned lvl;
+    pte_t *fault_pde;
+
+    pa = walk_pagetables(va, l0_table, &lvl, &fault_pde);
+
+    while ((lvl < target_lvl) && (pa == INVALID_PA)) {
+        /* fault_pde points to the entry to write. Add a new pagetable */
+        pte_set(fault_pde, make_pde_from_ptr(get_page()));
+
+        pa = walk_pagetables(va, l0_table, &lvl, &fault_pde);
+    }
+
+    if ((lvl == target_lvl) && fault_pde && !pte_is_valid(pte_get(fault_pde))) {
+        return fault_pde;
+    }
+    return NULL;
+}
+
+extern char _text[];
+extern char _end[];
+
+extern size_t dtb_size;
+
+static inline void clean_inval_cl(void *addr)
+{
+    asm volatile("dc civac, %0\n\t" :: "r"(addr));
+}
+
+static void clean_inval_pagetables(void)
+{
+    dsb();
+    /* Whole image for now; EFI case: Maybe our image is loaded on the boot
+     * CPU with caches enabled (and still being dirty), but the secondary CPUs
+     * start with caches disabled. Further, assume CL size is >= 64 Bytes.
+     * Maybe this is too cautious. Can we relax this?
+     */
+    for (vaddr_t va = (vaddr_t)_text; va < (vaddr_t)(_end); va += 64) {
+        clean_inval_cl((void *)va);
+    }
+    dsb();
+}
+
+static void map_uart(paddr_t base)
+{
+    pte_t *pte;
+
+    base = ROUND_DOWN(base, ARM_2MB_BLOCK_BITS);
+    pte  = fill_pt_tree(base, _boot_pgd_down, 2);
+    if (pte) {
+        pte_set(pte, make_pte(base, MT_DEVICE_nGnRnE));
+    } else {
+        printf("Unable to map the UART at PA 0x%lx\n", base);
+        abort();
+    }
+    dbg_printf("Done mapping UART at PA: 0x%lx\n", base);
+}
+
+
+static paddr_t uart_base_mmio;
+void mmu_set_uart_base(volatile void *base)
+{
+    uart_base_mmio = (paddr_t)base;
+}
+
+/*
+ * Create a "boot" page table, which contains a 1:1 mapping for the ELFloader and
+ * the DTB. Moreover create a mapping for the kernel image at the desired VA with the
+ * physical memory that was used when extracting the kernel from the elfloader
+ * image previously.
+ */
+static void init_boot_vspace_impl(const struct image_info *kernel_info, _Bool has_one_va_range)
+{
+    /* We may be running with MMU & caches off. Before we write new values
+     * make sure to clean & invalidate all previous data in those locations.
+     */
+    clean_inval_pagetables();
+
+    /* Map UART, using strongly ordered memory; one 2 MiB page; 1:1 VA/PA */
+    paddr_t uart_base = ROUND_DOWN(uart_base_mmio, ARM_2MB_BLOCK_BITS);
+    map_uart(uart_base);
+
+    /* Map Elfloader image, using NORMAL memory; 1:1 VA/PA */
+    paddr_t start_paddr = ROUND_DOWN(((paddr_t)_text), ARM_2MB_BLOCK_BITS);
+    paddr_t end_paddr   = ROUND_UP(((paddr_t)_end), ARM_2MB_BLOCK_BITS);
+
+    for (paddr_t pa = start_paddr; pa < end_paddr; pa += BIT(ARM_2MB_BLOCK_BITS)) {
+        pte_t *pte = fill_pt_tree(pa, _boot_pgd_down, 2);
+        if (pte) {
+            pte_set(pte, make_pte(pa, MT_NORMAL));
+        } else {
+            printf("Unable to map ELFloader at PA: 0x%lx\n", pa);
+            abort();
+        }
+        dbg_printf("Map Elfloader PA: 0x%lx\n", pa);
+    }
+    dbg_printf("Done mapping Elfloader\n");
+
+    paddr_t dtb_map_start, dtb_map_end;
+    if (dtb && (dtb_size > 0)) {
+        /* Device Tree Blob (DTB):
+         * An UEFI-supplied DTB lies outside of the image memory => Add mapping.
+         * For other DTBs the ELFloader of course saves the *target* address of
+         * the copied DTB in "dtb".
+         * So we also need to add a mapping here in those cases.
+         */
+        paddr_t dtb_end = (paddr_t)dtb + dtb_size;
+
+        dtb_map_start = ROUND_DOWN((paddr_t)dtb, ARM_2MB_BLOCK_BITS);
+        dtb_map_end   = ROUND_UP(dtb_end, ARM_2MB_BLOCK_BITS);
+        for (paddr_t pa = dtb_map_start; pa < dtb_map_end; pa += BIT(ARM_2MB_BLOCK_BITS)) {
+            pte_t *pte = fill_pt_tree(pa, _boot_pgd_down, 2);
+            if (pte) {
+                pte_set(pte, make_pte(pa, MT_NORMAL));
+            } else {
+                printf("Unable to map DTB at PA: 0x%lx\n", pa);
+            }
+            dbg_printf("Map DTB PA: 0x%lx\n", pa);
+        }
+        dbg_printf("Done mapping DTB\n");
+    }
+
+    /* Map the kernel */
     vaddr_t first_vaddr = kernel_info->virt_region_start;
+    vaddr_t last_vaddr  = kernel_info->virt_region_end;
     paddr_t first_paddr = kernel_info->phys_region_start;
-    _boot_pgd_down[0] = ((uintptr_t)_boot_pud_down) | BIT(1) | BIT(0);
 
-    for (i = 0; i < BIT(PUD_BITS); i++) {
-        _boot_pud_down[i] = (i << ARM_1GB_BLOCK_BITS)
-                            | BIT(10) /* access flag */
-                            | (0 << 2) /* strongly ordered memory */
-                            | BIT(0); /* 1G block */
+    uint64_t *l0_table = has_one_va_range ? _boot_pgd_down : _boot_pgd_up;
+    paddr_t pa = first_paddr;
+    for (vaddr_t va = first_vaddr; va < last_vaddr;
+         va += BIT(ARM_2MB_BLOCK_BITS),
+         pa += BIT(ARM_2MB_BLOCK_BITS)) {
+
+        pte_t *pte = fill_pt_tree(va, l0_table, 2);
+        if (pte) {
+            pte_set(pte, make_pte(pa, MT_NORMAL));
+        } else {
+            printf("Unable to map kernel at VA/PA: 0x%lx / 0x%lx\n", va, pa);
+        }
+        dbg_printf("Map kernel VA -> PA: 0x%lx -> 0x%lx\n", va, pa);
     }
+    dbg_printf("Done mapping kernel\n");
 
-    _boot_pgd_down[GET_PGD_INDEX(first_vaddr)]
-        = ((uintptr_t)_boot_pud_up) | BIT(1) | BIT(0); /* its a page table */
-
-    _boot_pud_up[GET_PUD_INDEX(first_vaddr)]
-        = ((uintptr_t)_boot_pmd_up) | BIT(1) | BIT(0); /* its a page table */
-
-    pmd_index = GET_PMD_INDEX(first_vaddr);
-    for (i = pmd_index; i < BIT(PMD_BITS); i++) {
-        _boot_pmd_up[i] = (((i - pmd_index) << ARM_2MB_BLOCK_BITS) + first_paddr)
-                          | BIT(10) /* access flag */
-#if CONFIG_MAX_NUM_NODES > 1
-                          | (3 << 8)
-#endif
-                          | (4 << 2) /* MT_NORMAL memory */
-                          | BIT(0); /* 2M block */
+    dbg_printf("Mapping indices:\n");
+    dgb_print_2M_mapping_details("UART", uart_base, /* one 2 MiB page */ 2u * 1024 * 1024);
+    dgb_print_2M_mapping_details("ELFloader image", (paddr_t)_text, (paddr_t)_end - (paddr_t)_text);
+    if (dtb && (dtb_size > 0)) {
+        dgb_print_2M_mapping_details("dtb", dtb_map_start, dtb_map_end - dtb_map_start - 1);
     }
+
+
+    /* Architecturally required barrier to make all writes to pagetable memories
+     * visible to the pagetable walker. See ARM DDI 0487I.a, section D8.2.6.
+     */
+    dsb();
+
+    /* Maintenance again, just to be sure. This is only necessary for the secondary
+     * CPUs; they may come up with caches & MMU disabled. What they should usually
+     * do is enable caches & MMU together! The following code is only necessary
+     * if they enable ONLY the MMU first and after that they enable the cache.
+     * That would be totally ... well ... suboptimal, but we play "better safe
+     * than sorry" here.
+     */
+    clean_inval_pagetables();
+}
+
+void init_boot_vspace(struct image_info *kernel_info)
+{
+    init_boot_vspace_impl(kernel_info, 0);
+}
+
+void init_hyp_boot_vspace(struct image_info *kernel_info)
+{
+    init_boot_vspace_impl(kernel_info, 1);
 }
diff --git a/elfloader-tool/src/arch-arm/64/structures.c b/elfloader-tool/src/arch-arm/64/structures.c
index 654fc7dc..e24680f4 100644
--- a/elfloader-tool/src/arch-arm/64/structures.c
+++ b/elfloader-tool/src/arch-arm/64/structures.c
@@ -8,11 +8,6 @@
 #include <types.h>
 #include <mode/structures.h>
 
-/* Paging structures for kernel mapping */
+/* Top-level paging structures for kernel and identity mapping */
 uint64_t _boot_pgd_up[BIT(PGD_BITS)] ALIGN(BIT(PGD_SIZE_BITS));
-uint64_t _boot_pud_up[BIT(PUD_BITS)] ALIGN(BIT(PUD_SIZE_BITS));
-uint64_t _boot_pmd_up[BIT(PMD_BITS)] ALIGN(BIT(PMD_SIZE_BITS));
-
-/* Paging structures for identity mapping */
 uint64_t _boot_pgd_down[BIT(PGD_BITS)] ALIGN(BIT(PGD_SIZE_BITS));
-uint64_t _boot_pud_down[BIT(PUD_BITS)] ALIGN(BIT(PUD_SIZE_BITS));
diff --git a/elfloader-tool/src/arch-arm/armv/armv8-a/64/mmu-hyp.S b/elfloader-tool/src/arch-arm/armv/armv8-a/64/mmu-hyp.S
index 31cb8a27..eb091f81 100644
--- a/elfloader-tool/src/arch-arm/armv/armv8-a/64/mmu-hyp.S
+++ b/elfloader-tool/src/arch-arm/armv/armv8-a/64/mmu-hyp.S
@@ -21,31 +21,32 @@
 .extern invalidate_icache
 .extern _boot_pgd_down
 
-BEGIN_FUNC(disable_caches_hyp)
-    stp     x29, x30, [sp, #-16]!
-    mov     x29, sp
-    bl      flush_dcache
-    disable_id_cache sctlr_el2, x9
-    ldp     x29, x30, [sp], #16
+BEGIN_FUNC(clean_dcache_by_range)
+    /* Ordering needed for strongly-ordered mem, not needed for NORMAL mem.
+     * See ARM DDI 0487I.a, page D7-5063.
+     */
+    dmb     sy
+
+    /* Extract minimum DCache CL size into x3 and CL mask into x4 */
+    mrs x2, ctr_el0
+    ubfx x4, x2, #16, #4
+    mov x3, #4
+    lsl x3, x3, x4
+    sub x4, x3, #1
+
+    /* Apply mask to start address before entering the loop */
+    bic x4, x0, x4
+clean_dcache_by_range_loop:
+    dc cvac, x4
+    add x4, x4, x3
+    cmp x4, x1
+    b.lt clean_dcache_by_range_loop
+    dsb sy
+    isb
     ret
-END_FUNC(disable_caches_hyp)
+END_FUNC(clean_dcache_by_range)
 
 BEGIN_FUNC(leave_hyp)
-    /* We call nested functions, follow the ABI. */
-    stp     x29, x30, [sp, #-16]!
-    mov     x29, sp
-
-    bl      flush_dcache
-
-    /* Ensure I-cache, D-cache and mmu are disabled for EL2/Stage1 */
-    disable_mmu sctlr_el2, x9
-
-    /*
-     * Invalidate the local I-cache so that any instructions fetched
-     * speculatively are discarded.
-     */
-    bl      invalidate_icache
-
     /* Ensure I-cache, D-cache and mmu are disabled for EL1/Stage2 */
     mov     x9, #(1 << 31)
     msr     hcr_el2, x9
@@ -63,23 +64,16 @@ BEGIN_FUNC(leave_hyp)
     msr     spsr_el2, x9
 
     /* Let's the caller use our stack, in case it needs to pop something */
-    ldp     x29, x30, [sp], #16
     mov     x10, sp
     msr     sp_el1, x10
     msr     elr_el2, x30
     eret
 END_FUNC(leave_hyp)
 
-BEGIN_FUNC(arm_enable_hyp_mmu)
-    stp     x29, x30, [sp, #-16]!
-    mov     x29, sp
-
-    bl      flush_dcache
-
-    disable_mmu sctlr_el2, x8
-
-    bl      invalidate_icache
-
+BEGIN_FUNC(arm_switch_to_hyp_tables)
+    /* Load MAIR & TCR values; construct TTBR address before disabling and re-
+     * enabling the MMU & caches.
+     */
     /*
      *   DEVICE_nGnRnE      000     00000000
      *   DEVICE_nGnRE       001     00000100
@@ -94,26 +88,38 @@ BEGIN_FUNC(arm_enable_hyp_mmu)
                  MAIR(0x44, MT_NORMAL_NC) | \
                  MAIR(0xff, MT_NORMAL) | \
                  MAIR(0xaa, MT_NORMAL_WT)
-    msr     mair_el2, x5
+
     ldr     x8, =TCR_T0SZ(48) | TCR_IRGN0_WBWC | TCR_ORGN0_WBWC | TCR_SH0_ISH | TCR_TG0_4K | TCR_PS | TCR_EL2_RES1
+
+    /* Use x16 as temp register */
+    disable_mmu  sctlr_el2, x16
+
+    msr     mair_el2, x5
     msr     tcr_el2, x8
     isb
 
+    /* For non-VHE the "down" contains both the the kernel mapping and 1:1 mapping. */
     adrp    x8, _boot_pgd_down
     msr     ttbr0_el2, x8
     isb
 
+     /* Invalidate TLBs */
+    dsb     sy
     tlbi    alle2is
+    tlbi    vmalls12e1
+    dsb     sy
+
+    tlbi    vmalle1is
     dsb     ish
     isb
 
-    enable_mmu  sctlr_el2, x8
+    /* Invalidate icache */
     ic  ialluis
-    dsb ish
+    dsb sy
     isb
-    tlbi    alle2is
-    dsb     ish
-    isb
-    ldp     x29, x30, [sp], #16
+
+    enable_mmu  sctlr_el2, x8
+    /* NOTE: enable_mmu already contains an isb after enabling. */
+
     ret
-END_FUNC(arm_enable_hyp_mmu)
+END_FUNC(arm_switch_to_hyp_tables)
diff --git a/elfloader-tool/src/arch-arm/armv/armv8-a/64/mmu.S b/elfloader-tool/src/arch-arm/armv/armv8-a/64/mmu.S
index ee161192..705c5671 100644
--- a/elfloader-tool/src/arch-arm/armv/armv8-a/64/mmu.S
+++ b/elfloader-tool/src/arch-arm/armv/armv8-a/64/mmu.S
@@ -39,21 +39,6 @@ BEGIN_FUNC(flush_dcache)
 END_FUNC(flush_dcache)
 
 BEGIN_FUNC(arm_enable_mmu)
-    /* We call nested functions, follow the ABI. */
-    stp     x29, x30, [sp, #-16]!
-    mov     x29, sp
-
-    bl      flush_dcache
-
-    /* Ensure I-cache, D-cache and mmu are disabled for EL1/Stage1 */
-    disable_mmu sctlr_el1 , x8
-
-    /*
-     * Invalidate the local I-cache so that any instructions fetched
-     * speculatively are discarded.
-     */
-    bl      invalidate_icache
-
     /*
      *   DEVICE_nGnRnE      000     00000000
      *   DEVICE_nGnRE       001     00000100
@@ -92,6 +77,5 @@ BEGIN_FUNC(arm_enable_mmu)
     adrp    x8, arm_vector_table
     msr     vbar_el1, x8
 
-    ldp     x29, x30, [sp], #16
     ret
 END_FUNC(arm_enable_mmu)
diff --git a/elfloader-tool/src/arch-arm/armv/armv8-a/64/smp.c b/elfloader-tool/src/arch-arm/armv/armv8-a/64/smp.c
index a86b02b4..edbf07e5 100644
--- a/elfloader-tool/src/arch-arm/armv/armv8-a/64/smp.c
+++ b/elfloader-tool/src/arch-arm/armv/armv8-a/64/smp.c
@@ -35,6 +35,12 @@ void core_entry(uint64_t sp)
 
 int is_core_up(int i)
 {
+    /* Secondary core may be booted with caches disabled,
+     * this value might be written in memory, invalidate our
+     * copy and get a new one. */
+    asm volatile("dc ivac, %0\n\t"
+                 "dmb nsh\n\t"
+                 :: "r"(&core_up[i]));
     return core_up[i] == i;
 }
 
diff --git a/elfloader-tool/src/arch-arm/drivers/smp-psci.c b/elfloader-tool/src/arch-arm/drivers/smp-psci.c
index ef3ea012..ae5fe951 100644
--- a/elfloader-tool/src/arch-arm/drivers/smp-psci.c
+++ b/elfloader-tool/src/arch-arm/drivers/smp-psci.c
@@ -3,6 +3,7 @@
  *
  * SPDX-License-Identifier: GPL-2.0-only
  */
+#include <autoconf.h>
 #include <elfloader_common.h>
 #include <devices_gen.h>
 #include <drivers/common.h>
@@ -24,7 +25,13 @@ static int smp_psci_cpu_on(UNUSED struct elfloader_device *dev,
     }
     secondary_data.entry = entry;
     secondary_data.stack = stack;
-    dmb();
+#if defined(CONFIG_ARCH_AARCH64)
+    /* If the secondary core caches are off, need to make sure that the info
+     * is clean to the physical memory so that the sedcondary cores can read it.
+     */
+    asm volatile("dc cvac, %0" :: "r"(&secondary_data));
+    dsb();
+#endif
     int ret = psci_cpu_on(cpu->cpu_id, (unsigned long)&secondary_startup, 0);
     if (ret != PSCI_SUCCESS) {
         printf("Failed to bring up core 0x%x with error %d\n", cpu->cpu_id, ret);
diff --git a/elfloader-tool/src/arch-arm/smp_boot.c b/elfloader-tool/src/arch-arm/smp_boot.c
index d429d113..f795f3a7 100644
--- a/elfloader-tool/src/arch-arm/smp_boot.c
+++ b/elfloader-tool/src/arch-arm/smp_boot.c
@@ -24,7 +24,7 @@ static volatile int non_boot_lock = 0;
 void arm_disable_dcaches(void);
 
 extern void const *dtb;
-extern uint32_t dtb_size;
+extern size_t dtb_size;
 
 /* Entry point for all CPUs other than the initial. */
 void non_boot_main(void)
@@ -34,7 +34,11 @@ void non_boot_main(void)
 #endif
     /* Spin until the first CPU has finished initialisation. */
     while (!non_boot_lock) {
-#ifndef CONFIG_ARCH_AARCH64
+#ifdef CONFIG_ARCH_AARCH64
+        /* The compiler may optimize this loop away, add a dsb()
+         * to force a reload. */
+        dsb();
+#else
         cpu_idle();
 #endif
     }
@@ -45,23 +49,27 @@ void non_boot_main(void)
         abort();
     }
 
-#ifndef CONFIG_ARM_HYPERVISOR_SUPPORT
     if (is_hyp_mode()) {
         extern void leave_hyp(void);
+#ifndef CONFIG_ARM_HYPERVISOR_SUPPORT
         leave_hyp();
-    }
 #endif
+    }
     /* Enable the MMU, and enter the kernel. */
     if (is_hyp_mode()) {
+#if defined(CONFIG_ARCH_AARCH64)
+        arm_switch_to_hyp_tables();
+#else
         arm_enable_hyp_mmu();
+#endif
     } else {
         arm_enable_mmu();
     }
 
-    /* Jump to the kernel. */
+    /* Jump to the kernel. Note: Our DTB is smaller than 4 GiB. */
     ((init_arm_kernel_t)kernel_info.virt_entry)(user_info.phys_region_start,
                                                 user_info.phys_region_end, user_info.phys_virt_offset,
-                                                user_info.virt_entry, (paddr_t)dtb, dtb_size);
+                                                user_info.virt_entry, (paddr_t)dtb, (uint32_t)dtb_size);
 
     printf("AP Kernel returned back to the elf-loader.\n");
     abort();
@@ -117,7 +125,13 @@ WEAK void init_cpus(void)
             abort();
         }
 
-        while (!is_core_up(num_cpus));
+        while (!is_core_up(num_cpus)) {
+#if defined(CONFIG_ARCH_AARCH64)
+            /* The compiler may optimize this loop away, add a dsb()
+             * to force a reload. */
+            dsb();
+#endif
+        }
         printf("Core %d is up with logic id %d\n", elfloader_cpus[i].cpu_id, num_cpus);
         num_cpus++;
     }
@@ -134,6 +148,13 @@ void smp_boot(void)
     arm_disable_dcaches();
 #endif
     init_cpus();
+#if defined(CONFIG_ARCH_AARCH64)
+    dsb();
     non_boot_lock = 1;
+    /* Secondary CPUs may still run with MMU & caches off. Force the update to be visible. */
+    asm volatile("dc civac, %0\n\t" :: "r"(&non_boot_lock) : "memory");;
+#else
+    non_boot_lock = 1;
+#endif
 }
 #endif /* CONFIG_MAX_NUM_NODES */
diff --git a/elfloader-tool/src/arch-arm/sys_boot.c b/elfloader-tool/src/arch-arm/sys_boot.c
index bf98aaf2..ff8c5ad5 100644
--- a/elfloader-tool/src/arch-arm/sys_boot.c
+++ b/elfloader-tool/src/arch-arm/sys_boot.c
@@ -23,7 +23,7 @@
 #define DTB_MAGIC (0xedfe0dd0)
 
 /* Maximum alignment we need to preserve when relocating (64K) */
-#define MAX_ALIGN_BITS (14)
+#define MAX_ALIGN_BITS (16)
 
 #ifdef CONFIG_IMAGE_EFI
 ALIGN(BIT(PAGE_BITS)) VISIBLE
@@ -168,6 +168,92 @@ void main(UNUSED void *arg)
     abort();
 }
 
+/* ARMv8 64-bit specific implementation of continue_boot() */
+#if defined(CONFIG_ARCH_AARCH64)
+void continue_boot(int was_relocated)
+{
+    if (was_relocated) {
+        printf("ELF loader relocated, continuing boot...\n");
+    }
+
+    /*
+     * If we were relocated, we need to re-initialise the
+     * driver model so all its pointers are set up properly.
+     */
+    if (was_relocated) {
+        initialise_devices();
+    }
+
+    /* Setup MMU. */
+#if defined(CONFIG_ARM_HYPERVISOR_SUPPORT)
+    init_hyp_boot_vspace(&kernel_info);
+#else
+    init_boot_vspace(&kernel_info);
+#endif
+
+    if (is_hyp_mode()) {
+        extern void clean_dcache_by_range(paddr_t start, paddr_t end);
+
+        paddr_t start = kernel_info.phys_region_start;
+        paddr_t end = kernel_info.phys_region_end;
+        clean_dcache_by_range(start, end);
+
+        start = (paddr_t)user_info.phys_region_start;
+        end = (paddr_t)user_info.phys_region_end;
+        clean_dcache_by_range(start, end);
+
+        start = (paddr_t)_text;
+        end = (paddr_t)_end;
+        clean_dcache_by_range(start, end);
+
+        if (dtb) {
+            start = (paddr_t)dtb;
+            end = start + dtb_size;
+            clean_dcache_by_range(start, end);
+        }
+
+#if defined(CONFIG_ARM_HYPERVISOR_SUPPORT)
+        printf("Switch to hypervisor mapping\n");
+        arm_switch_to_hyp_tables();
+#else
+        extern void leave_hyp(void);
+        /* Switch to EL1, assume EL2 MMU already disabled for ARMv8. */
+        leave_hyp();
+#endif
+    }
+
+#if CONFIG_MAX_NUM_NODES > 1
+    smp_boot();
+#endif /* CONFIG_MAX_NUM_NODES */
+
+    if (is_hyp_mode()) {
+        /* Nothing to be done here, we already switched above */
+    } else {
+        printf("Enabling MMU and paging\n");
+        arm_enable_mmu();
+    }
+
+    /* Enter kernel. The UART may no longer be accessible here. */
+    if ((uintptr_t)uart_get_mmio() < kernel_info.virt_region_start) {
+        printf("Jumping to kernel-image entry point...\n\n");
+    }
+
+    /* Clear D&A in DAIF */
+    asm volatile("msr daifclr, #0xC\n\t");
+
+    /* Jump to the kernel. Note: Our DTB is smaller than 4 GiB. */
+    ((init_arm_kernel_t)kernel_info.virt_entry)(user_info.phys_region_start,
+                                                user_info.phys_region_end,
+                                                user_info.phys_virt_offset,
+                                                user_info.virt_entry,
+                                                (word_t)dtb,
+                                                (uint32_t)dtb_size);
+
+    /* We should never get here. */
+    printf("ERROR: Kernel returned back to the ELF Loader\n");
+    abort();
+}
+#else
 void continue_boot(int was_relocated)
 {
     if (was_relocated) {
@@ -232,3 +318,4 @@ void continue_boot(int was_relocated)
     printf("ERROR: Kernel returned back to the ELF Loader\n");
     abort();
 }
+#endif
diff --git a/elfloader-tool/src/binaries/efi/efi_init.c b/elfloader-tool/src/binaries/efi/efi_init.c
index a177c083..47dc2651 100644
--- a/elfloader-tool/src/binaries/efi/efi_init.c
+++ b/elfloader-tool/src/binaries/efi/efi_init.c
@@ -4,18 +4,28 @@
  * SPDX-License-Identifier: GPL-2.0-only
  */
 
+#include <autoconf.h>
 #include <binaries/efi/efi.h>
 #include <elfloader_common.h>
 
 void *__application_handle = NULL;             // current efi application handler
 efi_system_table_t *__efi_system_table = NULL; // current efi system table
 
+static unsigned long efi_exit_bs_result = EFI_SUCCESS;
+static unsigned long exit_boot_services(void);
+
+unsigned long efi_exit_boot_services(void)
+{
+    return efi_exit_bs_result;
+}
+
 extern void _start(void);
 unsigned int efi_main(uintptr_t application_handle, uintptr_t efi_system_table)
 {
     clear_bss();
     __application_handle = (void *)application_handle;
     __efi_system_table = (efi_system_table_t *)efi_system_table;
+    efi_exit_bs_result = exit_boot_services();
     _start();
     return 0;
 }
@@ -41,7 +51,7 @@ void *efi_get_fdt(void)
  * This means boot time services are not available anymore. We should store
  * system information e.g. current memory map and pass them to kernel.
  */
-unsigned long efi_exit_boot_services(void)
+static unsigned long exit_boot_services(void)
 {
     unsigned long status;
     efi_memory_desc_t *memory_map;
@@ -52,31 +62,44 @@ unsigned long efi_exit_boot_services(void)
     efi_boot_services_t *bts = get_efi_boot_services();
 
     /*
-     * As the number of existing memeory segments are unknown,
-     * we need to resort to a trial and error to guess that.
-     * We start from 32 and increase it by one until get a valid value.
+     * As the number of existing memory segments are unknown,
+     * we need to start somewhere. The API then tells us how much space we need
+     * if it is not enough.
      */
     map_size = sizeof(*memory_map) * 32;
 
-again:
-    status = bts->allocate_pool(EFI_LOADER_DATA, map_size, (void **)&memory_map);
+    do {
+        status = bts->allocate_pool(EFI_LOADER_DATA, map_size, (void **)&memory_map);
+        /* If the allocation fails, there is something wrong and we cannot continue */
+        if (status != EFI_SUCCESS) {
+            return status;
+        }
+
+        status = bts->get_memory_map(&map_size, memory_map, &key, &desc_size, &desc_version);
+        if (status != EFI_SUCCESS) {
+            bts->free_pool(memory_map);
+            memory_map = NULL;
+
+            if (status == EFI_BUFFER_TOO_SMALL) {
+                /* Note: "map_size" is an IN/OUT-parameter and has been updated to the
+                 *       required size. We still add one more entry ("desc_size" is in bytes)
+                 *       due to the hint from the spec ("since allocation of the new buffer
+                 *       may potentially increase memory map size.").
+                 */
+                map_size += desc_size;
+            } else {
+                /* some other error; bail out! */
+                return status;
+            }
+        }
+    } while (status == EFI_BUFFER_TOO_SMALL);
 
-    if (status != EFI_SUCCESS)
-        return status;
-
-    status = bts->get_memory_map(&map_size, memory_map, &key, &desc_size, &desc_version);
-    if (status == EFI_BUFFER_TOO_SMALL) {
-        bts->free_pool(memory_map);
-
-        map_size += sizeof(*memory_map);
-        goto again;
-    }
+    status = bts->exit_boot_services(__application_handle, key);
 
-    if (status != EFI_SUCCESS){
-        bts->free_pool(memory_map);
-        return status;
-    }
+#if defined(CONFIG_ARCH_AARCH64)
+    /* Now that we're free, mask all exceptions until we enter the kernel */
+    asm volatile("msr daifset, #0xF\n\t");
+#endif
 
-    status = bts->exit_boot_services(__application_handle, key);
     return status;
 }
diff --git a/elfloader-tool/src/binaries/efi/gnuefi/crt0-efi-aarch64.S b/elfloader-tool/src/binaries/efi/gnuefi/crt0-efi-aarch64.S
index 13792b87..c735616b 100644
--- a/elfloader-tool/src/binaries/efi/gnuefi/crt0-efi-aarch64.S
+++ b/elfloader-tool/src/binaries/efi/gnuefi/crt0-efi-aarch64.S
@@ -64,7 +64,7 @@ extra_header_fields:
 	.short	0				// MinorSubsystemVersion
 	.long	0				// Win32VersionValue
 
-	.long	_edata - ImageBase		// SizeOfImage
+	.long	_end - ImageBase		// SizeOfImage
 
 	// Everything before the kernel image is considered part of the header
 	.long	_gnuefi_start - ImageBase		// SizeOfHeaders
diff --git a/elfloader-tool/src/binaries/efi/gnuefi/elf_aarch64_efi.lds b/elfloader-tool/src/binaries/efi/gnuefi/elf_aarch64_efi.lds
index bbbc502f..5ee11d7e 100644
--- a/elfloader-tool/src/binaries/efi/gnuefi/elf_aarch64_efi.lds
+++ b/elfloader-tool/src/binaries/efi/gnuefi/elf_aarch64_efi.lds
@@ -31,6 +31,9 @@ SECTIONS
    *(.data)
    *(.data1)
    *(.data.*)
+   __start__driver_list = .;
+   *(_driver_list)
+   __stop__driver_list = .;
    *(.got.plt)
    *(.got)
 
@@ -61,6 +64,7 @@ SECTIONS
   .dynstr   : { *(.dynstr) }
   . = ALIGN(4096);
   .note.gnu.build-id : { *(.note.gnu.build-id) }
+  _end = .;
   /DISCARD/ :
   {
     *(.rel.reloc)
diff --git a/elfloader-tool/src/common.c b/elfloader-tool/src/common.c
index 9846422f..d351df06 100644
--- a/elfloader-tool/src/common.c
+++ b/elfloader-tool/src/common.c
@@ -468,29 +468,29 @@ int load_images(
         /* keep it page aligned */
         next_phys_addr = dtb_phys_start = ROUND_UP(kernel_phys_end, PAGE_BITS);
 
-        size_t dtb_size = fdt_size(dtb);
-        if (0 == dtb_size) {
+        size_t dtb_sz = fdt_size(dtb);
+        if (0 == dtb_sz) {
             printf("ERROR: Invalid device tree blob supplied\n");
             return -1;
         }
 
         /* Make sure this is a sane thing to do */
         ret = ensure_phys_range_valid(next_phys_addr,
-                                      next_phys_addr + dtb_size);
+                                      next_phys_addr + dtb_sz);
         if (0 != ret) {
             printf("ERROR: Physical address of DTB invalid\n");
             return -1;
         }
 
-        memmove((void *)next_phys_addr, dtb, dtb_size);
-        next_phys_addr += dtb_size;
+        memmove((void *)next_phys_addr, dtb, dtb_sz);
+        next_phys_addr += dtb_sz;
         next_phys_addr = ROUND_UP(next_phys_addr, PAGE_BITS);
         dtb_phys_end = next_phys_addr;
 
         printf("Loaded DTB from %p.\n", dtb);
         printf("   paddr=[%p..%p]\n", dtb_phys_start, dtb_phys_end - 1);
         *chosen_dtb = (void *)dtb_phys_start;
-        *chosen_dtb_size = dtb_size;
+        *chosen_dtb_size = dtb_sz;
     } else {
         next_phys_addr = ROUND_UP(kernel_phys_end, PAGE_BITS);
     }
diff --git a/elfloader-tool/src/drivers/uart/common.c b/elfloader-tool/src/drivers/uart/common.c
index a5c914ff..b6ec30af 100644
--- a/elfloader-tool/src/drivers/uart/common.c
+++ b/elfloader-tool/src/drivers/uart/common.c
@@ -18,6 +18,9 @@ void uart_set_out(struct elfloader_device *out)
         return;
     }
     uart_out = out;
+#if defined(CONFIG_ARCH_AARCH64)
+    mmu_set_uart_base(out->region_bases[0]);
+#endif
 }
 
 volatile void *uart_get_mmio(void)
diff --git a/elfloader-tool/src/drivers/uart/tegra-uart.c b/elfloader-tool/src/drivers/uart/tegra-uart.c
new file mode 100644
index 00000000..7bf0f62a
--- /dev/null
+++ b/elfloader-tool/src/drivers/uart/tegra-uart.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright 2023， NIO
+ *
+ * SPDX-License-Identifier: GPL-2.0-only
+ */
+
+#include <devices_gen.h>
+#include <drivers/common.h>
+#include <drivers/uart.h>
+#include <elfloader_common.h>
+
+#define NUM_BYTES_FIELD_BIT     (24U)
+#define FLUSH_BIT               (26U)
+#define INTR_TRIGGER_BIT        (31U)
+#define UART_REG(mmio, x)       ((volatile uint32_t *)(mmio + (x)))
+
+static int tegra_uart_putchar(struct elfloader_device *dev, unsigned int c)
+{
+    uint32_t reg_val;
+
+    reg_val = (uint32_t)(1UL << NUM_BYTES_FIELD_BIT);
+    reg_val |= BIT(INTR_TRIGGER_BIT);
+    reg_val |= c;
+
+    if (c == '\r' || c == '\n') {
+        reg_val |= BIT(FLUSH_BIT);
+    }
+
+    while (*UART_REG(dev->region_bases[0], 0) & BIT(INTR_TRIGGER_BIT));
+
+    *UART_REG(dev->region_bases[0], 0) = reg_val;
+    return 0;
+}
+
+static int tegra_uart_init(struct elfloader_device *dev, UNUSED void *match_data)
+{
+    uart_set_out(dev);
+    *UART_REG(dev->region_bases[0], 0) = 0;
+
+    return 0;
+}
+
+static const struct dtb_match_table tegra_uart_matches[] = {
+    { .compatible = "nvidia,tegra194-tcu" },
+    { .compatible = NULL /* sentinel */ },
+};
+
+static const struct elfloader_uart_ops tegra_uart_ops = {
+    .putc = &tegra_uart_putchar,
+};
+
+static const struct elfloader_driver tegra_uart = {
+    .match_table = tegra_uart_matches,
+    .type = DRIVER_UART,
+    .init = &tegra_uart_init,
+    .ops = &tegra_uart_ops,
+};
+
+ELFLOADER_DRIVER(tegra_uart);