armv8: New MMU setup code allowing to use 48+ bits PA/VA

This patch adds code which sets up 2-level page tables on ARM64 thus
extending available VA space. CPUs implementing 64k translation
granule are able to use direct PA-VA mapping of the whole 48 bit
address space.
It also adds the ability to reset the SCTRL register at the very beginning
of execution to avoid interference from stale mappings set up by early
firmware/loaders/etc.

Signed-off-by: Sergey Temerkhanov <s.temerkhanov@gmail.com>
Signed-off-by: Radha Mohan Chintakuntla <rchintakuntla@cavium.com>

2 files changed, 113 insertions, 0 deletions

diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
index 53bac3b449..71f0020c7f 100644
--- a/arch/arm/cpu/armv8/cache_v8.c
+++ b/arch/arm/cpu/armv8/cache_v8.c
@@ -12,6 +12,69 @@
 DECLARE_GLOBAL_DATA_PTR;
 
 #ifndef CONFIG_SYS_DCACHE_OFF
+
+#ifdef CONFIG_SYS_FULL_VA
+static void set_ptl1_entry(u64 index, u64 ptl2_entry)
+{
+	u64 *pgd = (u64 *)gd->arch.tlb_addr;
+	u64 value;
+
+	value = ptl2_entry | PTL1_TYPE_TABLE;
+	pgd[index] = value;
+}
+
+static void set_ptl2_block(u64 ptl1, u64 bfn, u64 address, u64 memory_attrs)
+{
+	u64 *pmd = (u64 *)ptl1;
+	u64 value;
+
+	value = address | PTL2_TYPE_BLOCK | PTL2_BLOCK_AF;
+	value |= memory_attrs;
+	pmd[bfn] = value;
+}
+
+static struct mm_region mem_map[] = CONFIG_SYS_MEM_MAP;
+
+#define PTL1_ENTRIES CONFIG_SYS_PTL1_ENTRIES
+#define PTL2_ENTRIES CONFIG_SYS_PTL2_ENTRIES
+
+static void setup_pgtables(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);
+	}
+
+	/* 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);
+			}
+
+			pmd++;
+		} else {
+			set_ptl1_entry(l1_e, 0);
+		}
+	}
+}
+
+#else
+
 inline void set_pgtable_section(u64 *page_table, u64 index, u64 section,
 			 u64 memory_type, u64 attribute)
 {
@@ -30,14 +93,24 @@ inline void set_pgtable_table(u64 *page_table, u64 index, u64 *table_addr)
 	value = (u64)table_addr | PMD_TYPE_TABLE;
 	page_table[index] = value;
 }
+#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,
@@ -55,6 +128,7 @@ __weak void mmu_setup(void)
 		}
 	}
 
+#endif
 	/* load TTBR0 */
 	el = current_el();
 	if (el == 1) {
@@ -154,6 +228,7 @@ u64 *__weak arch_get_page_table(void) {
 	return NULL;
 }
 
+#ifndef CONFIG_SYS_FULL_VA
 void mmu_set_region_dcache_behaviour(phys_addr_t start, size_t size,
 				     enum dcache_option option)
 {
@@ -179,6 +254,8 @@ void mmu_set_region_dcache_behaviour(phys_addr_t start, size_t size,
 	flush_dcache_range(start, end);
 	asm volatile("dsb sy");
 }
+#endif
+
 #else	/* CONFIG_SYS_DCACHE_OFF */
 
 void invalidate_dcache_all(void)
diff --git a/arch/arm/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
index da45d984d0..2ee60d60f1 100644
--- a/arch/arm/cpu/armv8/start.S
+++ b/arch/arm/cpu/armv8/start.S
@@ -43,6 +43,9 @@ _bss_end_ofs:
 	.quad	__bss_end - _start
 
 reset:
+#ifdef CONFIG_SYS_RESET_SCTRL
+	bl reset_sctrl
+#endif
 	/*
 	 * Could be EL3/EL2/EL1, Initial State:
 	 * Little Endian, MMU Disabled, i/dCache Disabled
@@ -99,6 +102,39 @@ master_cpu:
 
 	bl	_main
 
+#ifdef CONFIG_SYS_RESET_SCTRL
+reset_sctrl:
+	switch_el x1, 3f, 2f, 1f
+3:
+	mrs	x0, sctlr_el3
+	b	0f
+2:
+	mrs	x0, sctlr_el2
+	b	0f
+1:
+	mrs	x0, sctlr_el1
+
+0:
+	ldr	x1, =0xfdfffffa
+	and	x0, x0, x1
+
+	switch_el x1, 6f, 5f, 4f
+6:
+	msr	sctlr_el3, x0
+	b	7f
+5:
+	msr	sctlr_el2, x0
+	b	7f
+4:
+	msr	sctlr_el1, x0
+
+7:
+	dsb	sy
+	isb
+	b	__asm_invalidate_tlb_all
+	ret
+#endif
+
 /*-----------------------------------------------------------------------*/
 
 WEAK(apply_core_errata)