7 files changed, 620 insertions, 291 deletions

diff --git a/arch/arm/cpu/armv8/cache.S b/arch/arm/cpu/armv8/cache.S
index ab8c08917a..a9f4fec387 100644
--- a/arch/arm/cpu/armv8/cache.S
+++ b/arch/arm/cpu/armv8/cache.S
@@ -10,6 +10,7 @@
 #include <asm-offsets.h>
 #include <config.h>
 #include <asm/macro.h>
+#include <asm/system.h>
 #include <linux/linkage.h>
 
 /*
@@ -160,3 +161,56 @@ ENTRY(__asm_flush_l3_cache)
 	ret
 ENDPROC(__asm_flush_l3_cache)
 	.weak	__asm_flush_l3_cache
+
+/*
+ * void __asm_switch_ttbr(ulong new_ttbr)
+ *
+ * Safely switches to a new page table.
+ */
+ENTRY(__asm_switch_ttbr)
+	/* x2 = SCTLR (alive throghout the function) */
+	switch_el x4, 3f, 2f, 1f
+3:	mrs	x2, sctlr_el3
+	b	0f
+2:	mrs	x2, sctlr_el2
+	b	0f
+1:	mrs	x2, sctlr_el1
+0:
+
+	/* Unset CR_M | CR_C | CR_I from SCTLR to disable all caches */
+	movn	x1, #(CR_M | CR_C | CR_I)
+	and	x1, x2, x1
+	switch_el x4, 3f, 2f, 1f
+3:	msr	sctlr_el3, x1
+	b	0f
+2:	msr	sctlr_el2, x1
+	b	0f
+1:	msr	sctlr_el1, x1
+0:	isb
+
+	/* This call only clobbers x30 (lr) and x9 (unused) */
+	mov	x3, x30
+	bl	__asm_invalidate_tlb_all
+
+	/* From here on we're running safely with caches disabled */
+
+	/* Set TTBR to our first argument */
+	switch_el x4, 3f, 2f, 1f
+3:	msr	ttbr0_el3, x0
+	b	0f
+2:	msr	ttbr0_el2, x0
+	b	0f
+1:	msr	ttbr0_el1, x0
+0:	isb
+
+	/* Restore original SCTLR and thus enable caches again */
+	switch_el x4, 3f, 2f, 1f
+3:	msr	sctlr_el3, x2
+	b	0f
+2:	msr	sctlr_el2, x2
+	b	0f
+1:	msr	sctlr_el1, x2
+0:	isb
+
+	ret	x3
+ENDPROC(__asm_switch_ttbr)
diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
index 71f0020c7f..d1bd06bedf 100644
--- a/arch/arm/cpu/armv8/cache_v8.c
+++ b/arch/arm/cpu/armv8/cache_v8.c
@@ -2,6 +2,9 @@
  * (C) Copyright 2013
  * David Feng <fenghua@phytium.com.cn>
  *
+ * (C) Copyright 2016
+ * Alexander Graf <agraf@suse.de>
+ *
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
@@ -13,137 +16,388 @@ DECLARE_GLOBAL_DATA_PTR;
 
 #ifndef CONFIG_SYS_DCACHE_OFF
 
-#ifdef CONFIG_SYS_FULL_VA
-static void set_ptl1_entry(u64 index, u64 ptl2_entry)
+/*
+ *  With 4k page granule, a virtual address is split into 4 lookup parts
+ *  spanning 9 bits each:
+ *
+ *    _______________________________________________
+ *   |       |       |       |       |       |       |
+ *   |   0   |  Lv0  |  Lv1  |  Lv2  |  Lv3  |  off  |
+ *   |_______|_______|_______|_______|_______|_______|
+ *     63-48   47-39   38-30   29-21   20-12   11-00
+ *
+ *             mask        page size
+ *
+ *    Lv0: FF8000000000       --
+ *    Lv1:   7FC0000000       1G
+ *    Lv2:     3FE00000       2M
+ *    Lv3:       1FF000       4K
+ *    off:          FFF
+ */
+
+static u64 get_tcr(int el, u64 *pips, u64 *pva_bits)
 {
-	u64 *pgd = (u64 *)gd->arch.tlb_addr;
-	u64 value;
+	u64 max_addr = 0;
+	u64 ips, va_bits;
+	u64 tcr;
+	int i;
+
+	/* Find the largest address we need to support */
+	for (i = 0; mem_map[i].size || mem_map[i].attrs; i++)
+		max_addr = max(max_addr, mem_map[i].base + mem_map[i].size);
+
+	/* Calculate the maximum physical (and thus virtual) address */
+	if (max_addr > (1ULL << 44)) {
+		ips = 5;
+		va_bits = 48;
+	} else  if (max_addr > (1ULL << 42)) {
+		ips = 4;
+		va_bits = 44;
+	} else  if (max_addr > (1ULL << 40)) {
+		ips = 3;
+		va_bits = 42;
+	} else  if (max_addr > (1ULL << 36)) {
+		ips = 2;
+		va_bits = 40;
+	} else  if (max_addr > (1ULL << 32)) {
+		ips = 1;
+		va_bits = 36;
+	} else {
+		ips = 0;
+		va_bits = 32;
+	}
+
+	if (el == 1) {
+		tcr = TCR_EL1_RSVD | (ips << 32) | TCR_EPD1_DISABLE;
+	} else if (el == 2) {
+		tcr = TCR_EL2_RSVD | (ips << 16);
+	} else {
+		tcr = TCR_EL3_RSVD | (ips << 16);
+	}
 
-	value = ptl2_entry | PTL1_TYPE_TABLE;
-	pgd[index] = value;
+	/* PTWs cacheable, inner/outer WBWA and inner shareable */
+	tcr |= TCR_TG0_4K | TCR_SHARED_INNER | TCR_ORGN_WBWA | TCR_IRGN_WBWA;
+	tcr |= TCR_T0SZ(va_bits);
+
+	if (pips)
+		*pips = ips;
+	if (pva_bits)
+		*pva_bits = va_bits;
+
+	return tcr;
 }
 
-static void set_ptl2_block(u64 ptl1, u64 bfn, u64 address, u64 memory_attrs)
+#define MAX_PTE_ENTRIES 512
+
+static int pte_type(u64 *pte)
 {
-	u64 *pmd = (u64 *)ptl1;
-	u64 value;
+	return *pte & PTE_TYPE_MASK;
+}
 
-	value = address | PTL2_TYPE_BLOCK | PTL2_BLOCK_AF;
-	value |= memory_attrs;
-	pmd[bfn] = value;
+/* Returns the LSB number for a PTE on level <level> */
+static int level2shift(int level)
+{
+	/* Page is 12 bits wide, every level translates 9 bits */
+	return (12 + 9 * (3 - level));
 }
 
-static struct mm_region mem_map[] = CONFIG_SYS_MEM_MAP;
+static u64 *find_pte(u64 addr, int level)
+{
+	int start_level = 0;
+	u64 *pte;
+	u64 idx;
+	u64 va_bits;
+	int i;
+
+	debug("addr=%llx level=%d\n", addr, level);
+
+	get_tcr(0, NULL, &va_bits);
+	if (va_bits < 39)
+		start_level = 1;
+
+	if (level < start_level)
+		return NULL;
+
+	/* Walk through all page table levels to find our PTE */
+	pte = (u64*)gd->arch.tlb_addr;
+	for (i = start_level; i < 4; i++) {
+		idx = (addr >> level2shift(i)) & 0x1FF;
+		pte += idx;
+		debug("idx=%llx PTE %p at level %d: %llx\n", idx, pte, i, *pte);
+
+		/* Found it */
+		if (i == level)
+			return pte;
+		/* PTE is no table (either invalid or block), can't traverse */
+		if (pte_type(pte) != PTE_TYPE_TABLE)
+			return NULL;
+		/* Off to the next level */
+		pte = (u64*)(*pte & 0x0000fffffffff000ULL);
+	}
 
-#define PTL1_ENTRIES CONFIG_SYS_PTL1_ENTRIES
-#define PTL2_ENTRIES CONFIG_SYS_PTL2_ENTRIES
+	/* Should never reach here */
+	return NULL;
+}
 
-static void setup_pgtables(void)
+/* Returns and creates a new full table (512 entries) */
+static u64 *create_table(void)
 {
-	int l1_e, l2_e;
-	unsigned long pmd = 0;
-	unsigned long address;
-
-	/* Setup the PMD pointers */
-	for (l1_e = 0; l1_e < CONFIG_SYS_MEM_MAP_SIZE; l1_e++) {
-		gd->arch.pmd_addr[l1_e] = gd->arch.tlb_addr +
-						PTL1_ENTRIES * sizeof(u64);
-		gd->arch.pmd_addr[l1_e] += PTL2_ENTRIES * sizeof(u64) * l1_e;
-		gd->arch.pmd_addr[l1_e] = ALIGN(gd->arch.pmd_addr[l1_e],
-						0x10000UL);
+	u64 *new_table = (u64*)gd->arch.tlb_fillptr;
+	u64 pt_len = MAX_PTE_ENTRIES * sizeof(u64);
+
+	/* Allocate MAX_PTE_ENTRIES pte entries */
+	gd->arch.tlb_fillptr += pt_len;
+
+	if (gd->arch.tlb_fillptr - gd->arch.tlb_addr > gd->arch.tlb_size)
+		panic("Insufficient RAM for page table: 0x%lx > 0x%lx. "
+		      "Please increase the size in get_page_table_size()",
+			gd->arch.tlb_fillptr - gd->arch.tlb_addr,
+			gd->arch.tlb_size);
+
+	/* Mark all entries as invalid */
+	memset(new_table, 0, pt_len);
+
+	return new_table;
+}
+
+static void set_pte_table(u64 *pte, u64 *table)
+{
+	/* Point *pte to the new table */
+	debug("Setting %p to addr=%p\n", pte, table);
+	*pte = PTE_TYPE_TABLE | (ulong)table;
+}
+
+/* Add one mm_region map entry to the page tables */
+static void add_map(struct mm_region *map)
+{
+	u64 *pte;
+	u64 addr = map->base;
+	u64 size = map->size;
+	u64 attrs = map->attrs | PTE_TYPE_BLOCK | PTE_BLOCK_AF;
+	u64 blocksize;
+	int level;
+	u64 *new_table;
+
+	while (size) {
+		pte = find_pte(addr, 0);
+		if (pte && (pte_type(pte) == PTE_TYPE_FAULT)) {
+			debug("Creating table for addr 0x%llx\n", addr);
+			new_table = create_table();
+			set_pte_table(pte, new_table);
+		}
+
+		for (level = 1; level < 4; level++) {
+			pte = find_pte(addr, level);
+			blocksize = 1ULL << level2shift(level);
+			debug("Checking if pte fits for addr=%llx size=%llx "
+			      "blocksize=%llx\n", addr, size, blocksize);
+			if (size >= blocksize && !(addr & (blocksize - 1))) {
+				/* Page fits, create block PTE */
+				debug("Setting PTE %p to block addr=%llx\n",
+				      pte, addr);
+				*pte = addr | attrs;
+				addr += blocksize;
+				size -= blocksize;
+				break;
+			} else if ((pte_type(pte) == PTE_TYPE_FAULT)) {
+				/* Page doesn't fit, create subpages */
+				debug("Creating subtable for addr 0x%llx "
+				      "blksize=%llx\n", addr, blocksize);
+				new_table = create_table();
+				set_pte_table(pte, new_table);
+			}
+		}
+	}
+}
+
+/* Splits a block PTE into table with subpages spanning the old block */
+static void split_block(u64 *pte, int level)
+{
+	u64 old_pte = *pte;
+	u64 *new_table;
+	u64 i = 0;
+	/* level describes the parent level, we need the child ones */
+	int levelshift = level2shift(level + 1);
+
+	if (pte_type(pte) != PTE_TYPE_BLOCK)
+		panic("PTE %p (%llx) is not a block. Some driver code wants to "
+		      "modify dcache settings for an range not covered in "
+		      "mem_map.", pte, old_pte);
+
+	new_table = create_table();
+	debug("Splitting pte %p (%llx) into %p\n", pte, old_pte, new_table);
+
+	for (i = 0; i < MAX_PTE_ENTRIES; i++) {
+		new_table[i] = old_pte | (i << levelshift);
+
+		/* Level 3 block PTEs have the table type */
+		if ((level + 1) == 3)
+			new_table[i] |= PTE_TYPE_TABLE;
+
+		debug("Setting new_table[%lld] = %llx\n", i, new_table[i]);
 	}
 
-	/* Setup the page tables */
-	for (l1_e = 0; l1_e < PTL1_ENTRIES; l1_e++) {
-		if (mem_map[pmd].base ==
-			(uintptr_t)l1_e << PTL2_BITS) {
-			set_ptl1_entry(l1_e, gd->arch.pmd_addr[pmd]);
-
-			for (l2_e = 0; l2_e < PTL2_ENTRIES; l2_e++) {
-				address = mem_map[pmd].base
-					+ (uintptr_t)l2_e * BLOCK_SIZE;
-				set_ptl2_block(gd->arch.pmd_addr[pmd], l2_e,
-					       address, mem_map[pmd].attrs);
+	/* Set the new table into effect */
+	set_pte_table(pte, new_table);
+}
+
+enum pte_type {
+	PTE_INVAL,
+	PTE_BLOCK,
+	PTE_LEVEL,
+};
+
+/*
+ * This is a recursively called function to count the number of
+ * page tables we need to cover a particular PTE range. If you
+ * call this with level = -1 you basically get the full 48 bit
+ * coverage.
+ */
+static int count_required_pts(u64 addr, int level, u64 maxaddr)
+{
+	int levelshift = level2shift(level);
+	u64 levelsize = 1ULL << levelshift;
+	u64 levelmask = levelsize - 1;
+	u64 levelend = addr + levelsize;
+	int r = 0;
+	int i;
+	enum pte_type pte_type = PTE_INVAL;
+
+	for (i = 0; mem_map[i].size || mem_map[i].attrs; i++) {
+		struct mm_region *map = &mem_map[i];
+		u64 start = map->base;
+		u64 end = start + map->size;
+
+		/* Check if the PTE would overlap with the map */
+		if (max(addr, start) <= min(levelend, end)) {
+			start = max(addr, start);
+			end = min(levelend, end);
+
+			/* We need a sub-pt for this level */
+			if ((start & levelmask) || (end & levelmask)) {
+				pte_type = PTE_LEVEL;
+				break;
 			}
 
-			pmd++;
-		} else {
-			set_ptl1_entry(l1_e, 0);
+			/* Lv0 can not do block PTEs, so do levels here too */
+			if (level <= 0) {
+				pte_type = PTE_LEVEL;
+				break;
+			}
+
+			/* PTE is active, but fits into a block */
+			pte_type = PTE_BLOCK;
 		}
 	}
+
+	/*
+	 * Block PTEs at this level are already covered by the parent page
+	 * table, so we only need to count sub page tables.
+	 */
+	if (pte_type == PTE_LEVEL) {
+		int sublevel = level + 1;
+		u64 sublevelsize = 1ULL << level2shift(sublevel);
+
+		/* Account for the new sub page table ... */
+		r = 1;
+
+		/* ... and for all child page tables that one might have */
+		for (i = 0; i < MAX_PTE_ENTRIES; i++) {
+			r += count_required_pts(addr, sublevel, maxaddr);
+			addr += sublevelsize;
+
+			if (addr >= maxaddr) {
+				/*
+				 * We reached the end of address space, no need
+				 * to look any further.
+				 */
+				break;
+			}
+		}
+	}
+
+	return r;
 }
 
-#else
+/* Returns the estimated required size of all page tables */
+u64 get_page_table_size(void)
+{
+	u64 one_pt = MAX_PTE_ENTRIES * sizeof(u64);
+	u64 size = 0;
+	u64 va_bits;
+	int start_level = 0;
+
+	get_tcr(0, NULL, &va_bits);
+	if (va_bits < 39)
+		start_level = 1;
+
+	/* Account for all page tables we would need to cover our memory map */
+	size = one_pt * count_required_pts(0, start_level - 1, 1ULL << va_bits);
+
+	/*
+	 * We need to duplicate our page table once to have an emergency pt to
+	 * resort to when splitting page tables later on
+	 */
+	size *= 2;
+
+	/*
+	 * We may need to split page tables later on if dcache settings change,
+	 * so reserve up to 4 (random pick) page tables for that.
+	 */
+	size += one_pt * 4;
+
+	return size;
+}
 
-inline void set_pgtable_section(u64 *page_table, u64 index, u64 section,
-			 u64 memory_type, u64 attribute)
+static void setup_pgtables(void)
 {
-	u64 value;
+	int i;
 
-	value = section | PMD_TYPE_SECT | PMD_SECT_AF;
-	value |= PMD_ATTRINDX(memory_type);
-	value |= attribute;
-	page_table[index] = value;
+	/*
+	 * Allocate the first level we're on with invalidate entries.
+	 * If the starting level is 0 (va_bits >= 39), then this is our
+	 * Lv0 page table, otherwise it's the entry Lv1 page table.
+	 */
+	create_table();
+
+	/* Now add all MMU table entries one after another to the table */
+	for (i = 0; mem_map[i].size || mem_map[i].attrs; i++)
+		add_map(&mem_map[i]);
+
+	/* Create the same thing once more for our emergency page table */
+	create_table();
 }
 
-inline void set_pgtable_table(u64 *page_table, u64 index, u64 *table_addr)
+static void setup_all_pgtables(void)
 {
-	u64 value;
+	u64 tlb_addr = gd->arch.tlb_addr;
+
+	/* Reset the fill ptr */
+	gd->arch.tlb_fillptr = tlb_addr;
 
-	value = (u64)table_addr | PMD_TYPE_TABLE;
-	page_table[index] = value;
+	/* Create normal system page tables */
+	setup_pgtables();
+
+	/* Create emergency page tables */
+	gd->arch.tlb_addr = gd->arch.tlb_fillptr;
+	setup_pgtables();
+	gd->arch.tlb_emerg = gd->arch.tlb_addr;
+	gd->arch.tlb_addr = tlb_addr;
 }
-#endif
 
 /* to activate the MMU we need to set up virtual memory */
 __weak void mmu_setup(void)
 {
-#ifndef CONFIG_SYS_FULL_VA
-	bd_t *bd = gd->bd;
-	u64 *page_table = (u64 *)gd->arch.tlb_addr, i, j;
-#endif
 	int el;
 
-#ifdef CONFIG_SYS_FULL_VA
-	unsigned long coreid = read_mpidr() & CONFIG_COREID_MASK;
-
-	/* Set up page tables only on BSP */
-	if (coreid == BSP_COREID)
-		setup_pgtables();
-#else
-	/* Setup an identity-mapping for all spaces */
-	for (i = 0; i < (PGTABLE_SIZE >> 3); i++) {
-		set_pgtable_section(page_table, i, i << SECTION_SHIFT,
-				    MT_DEVICE_NGNRNE, PMD_SECT_NON_SHARE);
-	}
-
-	/* Setup an identity-mapping for all RAM space */
-	for (i = 0; i < CONFIG_NR_DRAM_BANKS; i++) {
-		ulong start = bd->bi_dram[i].start;
-		ulong end = bd->bi_dram[i].start + bd->bi_dram[i].size;
-		for (j = start >> SECTION_SHIFT;
-		     j < end >> SECTION_SHIFT; j++) {
-			set_pgtable_section(page_table, j, j << SECTION_SHIFT,
-					    MT_NORMAL, PMD_SECT_NON_SHARE);
-		}
-	}
+	/* Set up page tables only once */
+	if (!gd->arch.tlb_fillptr)
+		setup_all_pgtables();
 
-#endif
-	/* load TTBR0 */
 	el = current_el();
-	if (el == 1) {
-		set_ttbr_tcr_mair(el, gd->arch.tlb_addr,
-				  TCR_EL1_RSVD | TCR_FLAGS | TCR_EL1_IPS_BITS,
-				  MEMORY_ATTRIBUTES);
-	} else if (el == 2) {
-		set_ttbr_tcr_mair(el, gd->arch.tlb_addr,
-				  TCR_EL2_RSVD | TCR_FLAGS | TCR_EL2_IPS_BITS,
-				  MEMORY_ATTRIBUTES);
-	} else {
-		set_ttbr_tcr_mair(el, gd->arch.tlb_addr,
-				  TCR_EL3_RSVD | TCR_FLAGS | TCR_EL3_IPS_BITS,
-				  MEMORY_ATTRIBUTES);
-	}
+	set_ttbr_tcr_mair(el, gd->arch.tlb_addr, get_tcr(el, NULL, NULL),
+			  MEMORY_ATTRIBUTES);
+
 	/* enable the mmu */
 	set_sctlr(get_sctlr() | CR_M);
 }
@@ -228,36 +482,99 @@ u64 *__weak arch_get_page_table(void) {
 	return NULL;
 }
 
-#ifndef CONFIG_SYS_FULL_VA
+static bool is_aligned(u64 addr, u64 size, u64 align)
+{
+	return !(addr & (align - 1)) && !(size & (align - 1));
+}
+
+static u64 set_one_region(u64 start, u64 size, u64 attrs, int level)
+{
+	int levelshift = level2shift(level);
+	u64 levelsize = 1ULL << levelshift;
+	u64 *pte = find_pte(start, level);
+
+	/* Can we can just modify the current level block PTE? */
+	if (is_aligned(start, size, levelsize)) {
+		*pte &= ~PMD_ATTRINDX_MASK;
+		*pte |= attrs;
+		debug("Set attrs=%llx pte=%p level=%d\n", attrs, pte, level);
+
+		return levelsize;
+	}
+
+	/* Unaligned or doesn't fit, maybe split block into table */
+	debug("addr=%llx level=%d pte=%p (%llx)\n", start, level, pte, *pte);
+
+	/* Maybe we need to split the block into a table */
+	if (pte_type(pte) == PTE_TYPE_BLOCK)
+		split_block(pte, level);
+
+	/* And then double-check it became a table or already is one */
+	if (pte_type(pte) != PTE_TYPE_TABLE)
+		panic("PTE %p (%llx) for addr=%llx should be a table",
+		      pte, *pte, start);
+
+	/* Roll on to the next page table level */
+	return 0;
+}
+
 void mmu_set_region_dcache_behaviour(phys_addr_t start, size_t size,
 				     enum dcache_option option)
 {
-	u64 *page_table = arch_get_page_table();
-	u64 upto, end;
-
-	if (page_table == NULL)
-		return;
+	u64 attrs = PMD_ATTRINDX(option);
+	u64 real_start = start;
+	u64 real_size = size;
+
+	debug("start=%lx size=%lx\n", (ulong)start, (ulong)size);
+
+	/*
+	 * We can not modify page tables that we're currently running on,
+	 * so we first need to switch to the "emergency" page tables where
+	 * we can safely modify our primary page tables and then switch back
+	 */
+	__asm_switch_ttbr(gd->arch.tlb_emerg);
+
+	/*
+	 * Loop through the address range until we find a page granule that fits
+	 * our alignment constraints, then set it to the new cache attributes
+	 */
+	while (size > 0) {
+		int level;
+		u64 r;
+
+		for (level = 1; level < 4; level++) {
+			r = set_one_region(start, size, attrs, level);
+			if (r) {
+				/* PTE successfully replaced */
+				size -= r;
+				start += r;
+				break;
+			}
+		}
 
-	end = ALIGN(start + size, (1 << MMU_SECTION_SHIFT)) >>
-	      MMU_SECTION_SHIFT;
-	start = start >> MMU_SECTION_SHIFT;
-	for (upto = start; upto < end; upto++) {
-		page_table[upto] &= ~PMD_ATTRINDX_MASK;
-		page_table[upto] |= PMD_ATTRINDX(option);
 	}
-	asm volatile("dsb sy");
-	__asm_invalidate_tlb_all();
-	asm volatile("dsb sy");
-	asm volatile("isb");
-	start = start << MMU_SECTION_SHIFT;
-	end = end << MMU_SECTION_SHIFT;
-	flush_dcache_range(start, end);
-	asm volatile("dsb sy");
+
+	/* We're done modifying page tables, switch back to our primary ones */
+	__asm_switch_ttbr(gd->arch.tlb_addr);
+
+	/*
+	 * Make sure there's nothing stale in dcache for a region that might
+	 * have caches off now
+	 */
+	flush_dcache_range(real_start, real_start + real_size);
 }
-#endif
 
 #else	/* CONFIG_SYS_DCACHE_OFF */
 
+/*
+ * For SPL builds, we may want to not have dcache enabled. Any real U-Boot
+ * running however really wants to have dcache and the MMU active. Check that
+ * everything is sane and give the developer a hint if it isn't.
+ */
+#ifndef CONFIG_SPL_BUILD
+#error Please describe your MMU layout in CONFIG_SYS_MEM_MAP and enable dcache.
+#endif
+
 void invalidate_dcache_all(void)
 {
 }
diff --git a/arch/arm/cpu/armv8/exceptions.S b/arch/arm/cpu/armv8/exceptions.S
index baf9401e64..4f4f526f93 100644
--- a/arch/arm/cpu/armv8/exceptions.S
+++ b/arch/arm/cpu/armv8/exceptions.S
@@ -82,31 +82,65 @@ vectors:
 _do_bad_sync:
 	exception_entry
 	bl	do_bad_sync
+	b	exception_exit
 
 _do_bad_irq:
 	exception_entry
 	bl	do_bad_irq
+	b	exception_exit
 
 _do_bad_fiq:
 	exception_entry
 	bl	do_bad_fiq
+	b	exception_exit
 
 _do_bad_error:
 	exception_entry
 	bl	do_bad_error
+	b	exception_exit
 
 _do_sync:
 	exception_entry
 	bl	do_sync
+	b	exception_exit
 
 _do_irq:
 	exception_entry
 	bl	do_irq
+	b	exception_exit
 
 _do_fiq:
 	exception_entry
 	bl	do_fiq
+	b	exception_exit
 
 _do_error:
 	exception_entry
 	bl	do_error
+	b	exception_exit
+
+exception_exit:
+	ldp	x2, x0, [sp],#16
+	switch_el x11, 3f, 2f, 1f
+3:	msr	elr_el3, x2
+	b	0f
+2:	msr	elr_el2, x2
+	b	0f
+1:	msr	elr_el1, x2
+0:
+	ldp	x1, x2, [sp],#16
+	ldp	x3, x4, [sp],#16
+	ldp	x5, x6, [sp],#16
+	ldp	x7, x8, [sp],#16
+	ldp	x9, x10, [sp],#16
+	ldp	x11, x12, [sp],#16
+	ldp	x13, x14, [sp],#16
+	ldp	x15, x16, [sp],#16
+	ldp	x17, x18, [sp],#16
+	ldp	x19, x20, [sp],#16
+	ldp	x21, x22, [sp],#16
+	ldp	x23, x24, [sp],#16
+	ldp	x25, x26, [sp],#16
+	ldp	x27, x28, [sp],#16
+	ldp	x29, x30, [sp],#16
+	eret
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index 6ea28ed5cb..7404bd932a 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -26,6 +26,14 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+static struct mm_region layerscape_mem_map[] = {
+	{
+		/* List terminator */
+		0,
+	}
+};
+struct mm_region *mem_map = layerscape_mem_map;
+
 void cpu_name(char *name)
 {
 	struct ccsr_gur __iomem *gur = (void *)(CONFIG_SYS_FSL_GUTS_ADDR);
@@ -48,6 +56,25 @@ void cpu_name(char *name)
 }
 
 #ifndef CONFIG_SYS_DCACHE_OFF
+static void set_pgtable_section(u64 *page_table, u64 index, u64 section,
+			u64 memory_type, u64 attribute)
+{
+       u64 value;
+
+       value = section | PTE_TYPE_BLOCK | PTE_BLOCK_AF;
+       value |= PMD_ATTRINDX(memory_type);
+       value |= attribute;
+       page_table[index] = value;
+}
+
+static void set_pgtable_table(u64 *page_table, u64 index, u64 *table_addr)
+{
+       u64 value;
+
+       value = (u64)table_addr | PTE_TYPE_TABLE;
+       page_table[index] = value;
+}
+
 /*
  * Set the block entries according to the information of the table.
  */
@@ -114,10 +141,10 @@ static int find_table(const struct sys_mmu_table *list,
 
 		temp_base -= block_size;
 
-		if ((level_table[index - 1] & PMD_TYPE_MASK) ==
-		    PMD_TYPE_TABLE) {
+		if ((level_table[index - 1] & PTE_TYPE_MASK) ==
+		    PTE_TYPE_TABLE) {
 			level_table = (u64 *)(level_table[index - 1] &
-				      ~PMD_TYPE_MASK);
+				      ~PTE_TYPE_MASK);
 			level++;
 			continue;
 		} else {
@@ -220,7 +247,7 @@ static inline int final_secure_ddr(u64 *level0_table,
 	struct table_info table = {};
 	struct sys_mmu_table ddr_entry = {
 		0, 0, BLOCK_SIZE_L1, MT_NORMAL,
-		PMD_SECT_OUTER_SHARE | PMD_SECT_NS
+		PTE_BLOCK_OUTER_SHARE | PTE_BLOCK_NS
 	};
 	u64 index;
 
@@ -243,7 +270,7 @@ static inline int final_secure_ddr(u64 *level0_table,
 	ddr_entry.virt_addr = phys_addr;
 	ddr_entry.phys_addr = phys_addr;
 	ddr_entry.size = CONFIG_SYS_MEM_RESERVE_SECURE;
-	ddr_entry.attribute = PMD_SECT_OUTER_SHARE;
+	ddr_entry.attribute = PTE_BLOCK_OUTER_SHARE;
 	ret = find_table(&ddr_entry, &table, level0_table);
 	if (ret) {
 		printf("MMU error: could not find secure ddr table\n");
diff --git a/arch/arm/cpu/armv8/u-boot-spl.lds b/arch/arm/cpu/armv8/u-boot-spl.lds
index 4df339c84a..cc427c3583 100644
--- a/arch/arm/cpu/armv8/u-boot-spl.lds
+++ b/arch/arm/cpu/armv8/u-boot-spl.lds
@@ -54,6 +54,8 @@ SECTIONS
 		*(.__end)
 	} >.sram
 
+	_image_binary_end = .;
+
 	.bss_start : {
 		. = ALIGN(8);
 		KEEP(*(.__bss_start));
diff --git a/arch/arm/cpu/armv8/u-boot.lds b/arch/arm/cpu/armv8/u-boot.lds
index 4c12222370..fd15ad5963 100644
--- a/arch/arm/cpu/armv8/u-boot.lds
+++ b/arch/arm/cpu/armv8/u-boot.lds
@@ -42,6 +42,22 @@ SECTIONS
 
 	. = ALIGN(8);
 
+	.efi_runtime : {
+                __efi_runtime_start = .;
+		*(efi_runtime_text)
+		*(efi_runtime_data)
+                __efi_runtime_stop = .;
+	}
+
+	.efi_runtime_rel : {
+                __efi_runtime_rel_start = .;
+		*(.relaefi_runtime_text)
+		*(.relaefi_runtime_data)
+                __efi_runtime_rel_stop = .;
+	}
+
+	. = ALIGN(8);
+
 	.image_copy_end :
 	{
 		*(.__image_copy_end)
diff --git a/arch/arm/cpu/armv8/zynqmp/cpu.c b/arch/arm/cpu/armv8/zynqmp/cpu.c
index c71f29152d..5dd3cd86cf 100644
--- a/arch/arm/cpu/armv8/zynqmp/cpu.c
+++ b/arch/arm/cpu/armv8/zynqmp/cpu.c
@@ -8,6 +8,7 @@
 #include <common.h>
 #include <asm/arch/hardware.h>
 #include <asm/arch/sys_proto.h>
+#include <asm/armv8/mmu.h>
 #include <asm/io.h>
 
 #define ZYNQ_SILICON_VER_MASK	0xF000
@@ -15,6 +16,53 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+static struct mm_region zynqmp_mem_map[] = {
+	{
+		.base = 0x0UL,
+		.size = 0x80000000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
+			 PTE_BLOCK_INNER_SHARE
+	}, {
+		.base = 0x80000000UL,
+		.size = 0x70000000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE |
+			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+	}, {
+		.base = 0xf8000000UL,
+		.size = 0x07e00000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE |
+			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+	}, {
+		.base = 0xffe00000UL,
+		.size = 0x00200000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
+			 PTE_BLOCK_INNER_SHARE
+	}, {
+		.base = 0x400000000UL,
+		.size = 0x200000000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE |
+			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+	}, {
+		.base = 0x600000000UL,
+		.size = 0x800000000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_NORMAL) |
+			 PTE_BLOCK_INNER_SHARE
+	}, {
+		.base = 0xe00000000UL,
+		.size = 0xf200000000UL,
+		.attrs = PTE_BLOCK_MEMTYPE(MT_DEVICE_NGNRNE) |
+			 PTE_BLOCK_NON_SHARE |
+			 PTE_BLOCK_PXN | PTE_BLOCK_UXN
+	}, {
+		/* List terminator */
+		0,
+	}
+};
+struct mm_region *mem_map = zynqmp_mem_map;
+
 static unsigned int zynqmp_get_silicon_version_secure(void)
 {
 	u32 ver;
@@ -44,172 +92,3 @@ unsigned int zynqmp_get_silicon_version(void)
 
 	return ZYNQMP_CSU_VERSION_SILICON;
 }
-
-#ifndef CONFIG_SYS_DCACHE_OFF
-#include <asm/armv8/mmu.h>
-
-#define SECTION_SHIFT_L1	30UL
-#define SECTION_SHIFT_L2	21UL
-#define BLOCK_SIZE_L0		0x8000000000UL
-#define BLOCK_SIZE_L1		(1 << SECTION_SHIFT_L1)
-#define BLOCK_SIZE_L2		(1 << SECTION_SHIFT_L2)
-
-#define TCR_TG1_4K		(1 << 31)
-#define TCR_EPD1_DISABLE	(1 << 23)
-#define ZYNQMO_VA_BITS		40
-#define ZYNQMP_TCR		TCR_TG1_4K | \
-				TCR_EPD1_DISABLE | \
-				TCR_SHARED_OUTER | \
-				TCR_SHARED_INNER | \
-				TCR_IRGN_WBWA | \
-				TCR_ORGN_WBWA | \
-				TCR_T0SZ(ZYNQMO_VA_BITS)
-
-#define MEMORY_ATTR	PMD_SECT_AF | PMD_SECT_INNER_SHARE |	\
-			PMD_ATTRINDX(MT_NORMAL) |	\
-			PMD_TYPE_SECT
-#define DEVICE_ATTR	PMD_SECT_AF | PMD_SECT_PXN |	\
-			PMD_SECT_UXN | PMD_ATTRINDX(MT_DEVICE_NGNRNE) |	\
-			PMD_TYPE_SECT
-
-/* 4K size is required to place 512 entries in each level */
-#define TLB_TABLE_SIZE	0x1000
-
-struct attr_tbl {
-	u32 num;
-	u64 attr;
-};
-
-static struct attr_tbl attr_tbll1t0[4] = { {16, 0x0},
-					   {8, DEVICE_ATTR},
-					   {32, MEMORY_ATTR},
-					   {456, DEVICE_ATTR}
-					 };
-static struct attr_tbl attr_tbll2t3[4] = { {0x180, DEVICE_ATTR},
-					   {0x40, 0x0},
-					   {0x3F, DEVICE_ATTR},
-					   {0x1, MEMORY_ATTR}
-					 };
-
-/*
- * This mmu table looks as below
- * Level 0 table contains two entries to 512GB sizes. One is Level1 Table 0
- * and other Level1 Table1.
- * Level1 Table0 contains entries for each 1GB from 0 to 511GB.
- * Level1 Table1 contains entries for each 1GB from 512GB to 1TB.
- * Level2 Table0, Level2 Table1, Level2 Table2 and Level2 Table3 contains
- * entries for each 2MB starting from 0GB, 1GB, 2GB and 3GB respectively.
- */
-static void zynqmp_mmu_setup(void)
-{
-	int el;
-	u32 index_attr;
-	u64 i, section_l1t0, section_l1t1;
-	u64 section_l2t0, section_l2t1, section_l2t2, section_l2t3;
-	u64 *level0_table = (u64 *)gd->arch.tlb_addr;
-	u64 *level1_table_0 = (u64 *)(gd->arch.tlb_addr + TLB_TABLE_SIZE);
-	u64 *level1_table_1 = (u64 *)(gd->arch.tlb_addr + (2 * TLB_TABLE_SIZE));
-	u64 *level2_table_0 = (u64 *)(gd->arch.tlb_addr + (3 * TLB_TABLE_SIZE));
-	u64 *level2_table_1 = (u64 *)(gd->arch.tlb_addr + (4 * TLB_TABLE_SIZE));
-	u64 *level2_table_2 = (u64 *)(gd->arch.tlb_addr + (5 * TLB_TABLE_SIZE));
-	u64 *level2_table_3 = (u64 *)(gd->arch.tlb_addr + (6 * TLB_TABLE_SIZE));
-
-	level0_table[0] =
-		(u64)level1_table_0 | PMD_TYPE_TABLE;
-	level0_table[1] =
-		(u64)level1_table_1 | PMD_TYPE_TABLE;
-
-	/*
-	 * set level 1 table 0, covering 0 to 512GB
-	 * set level 1 table 1, covering 512GB to 1TB
-	 */
-	section_l1t0 = 0;
-	section_l1t1 = BLOCK_SIZE_L0;
-
-	index_attr = 0;
-	for (i = 0; i < 512; i++) {
-		level1_table_0[i] = section_l1t0;
-		level1_table_0[i] |= attr_tbll1t0[index_attr].attr;
-		attr_tbll1t0[index_attr].num--;
-		if (attr_tbll1t0[index_attr].num == 0)
-			index_attr++;
-		level1_table_1[i] = section_l1t1;
-		level1_table_1[i] |= DEVICE_ATTR;
-		section_l1t0 += BLOCK_SIZE_L1;
-		section_l1t1 += BLOCK_SIZE_L1;
-	}
-
-	level1_table_0[0] =
-		(u64)level2_table_0 | PMD_TYPE_TABLE;
-	level1_table_0[1] =
-		(u64)level2_table_1 | PMD_TYPE_TABLE;
-	level1_table_0[2] =
-		(u64)level2_table_2 | PMD_TYPE_TABLE;
-	level1_table_0[3] =
-		(u64)level2_table_3 | PMD_TYPE_TABLE;
-
-	section_l2t0 = 0;
-	section_l2t1 = section_l2t0 + BLOCK_SIZE_L1; /* 1GB */
-	section_l2t2 = section_l2t1 + BLOCK_SIZE_L1; /* 2GB */
-	section_l2t3 = section_l2t2 + BLOCK_SIZE_L1; /* 3GB */
-
-	index_attr = 0;
-
-	for (i = 0; i < 512; i++) {
-		level2_table_0[i] = section_l2t0 | MEMORY_ATTR;
-		level2_table_1[i] = section_l2t1 | MEMORY_ATTR;
-		level2_table_2[i] = section_l2t2 | DEVICE_ATTR;
-		level2_table_3[i] = section_l2t3 |
-				    attr_tbll2t3[index_attr].attr;
-		attr_tbll2t3[index_attr].num--;
-		if (attr_tbll2t3[index_attr].num == 0)
-			index_attr++;
-		section_l2t0 += BLOCK_SIZE_L2;
-		section_l2t1 += BLOCK_SIZE_L2;
-		section_l2t2 += BLOCK_SIZE_L2;
-		section_l2t3 += BLOCK_SIZE_L2;
-	}
-
-	/* flush new MMU table */
-	flush_dcache_range(gd->arch.tlb_addr,
-			   gd->arch.tlb_addr + gd->arch.tlb_size);
-
-	/* point TTBR to the new table */
-	el = current_el();
-	set_ttbr_tcr_mair(el, gd->arch.tlb_addr,
-			  ZYNQMP_TCR, MEMORY_ATTRIBUTES);
-
-	set_sctlr(get_sctlr() | CR_M);
-}
-
-int arch_cpu_init(void)
-{
-	icache_enable();
-	__asm_invalidate_dcache_all();
-	__asm_invalidate_tlb_all();
-	return 0;
-}
-
-/*
- * This function is called from lib/board.c.
- * It recreates MMU table in main memory. MMU and d-cache are enabled earlier.
- * There is no need to disable d-cache for this operation.
- */
-void enable_caches(void)
-{
-	/* The data cache is not active unless the mmu is enabled */
-	if (!(get_sctlr() & CR_M)) {
-		invalidate_dcache_all();
-		__asm_invalidate_tlb_all();
-		zynqmp_mmu_setup();
-	}
-	puts("Enabling Caches...\n");
-
-	set_sctlr(get_sctlr() | CR_C);
-}
-
-u64 *arch_get_page_table(void)
-{
-	return (u64 *)(gd->arch.tlb_addr + 0x3000);
-}
-#endif