arm: Add support for HYP mode and LPAE page tables

We currently always modify the SVC versions of registers and only support
the short descriptor PTE format.

Some boards however (like the RPi2) run in HYP mode. There, we need to modify
the HYP version of system registers and HYP mode only supports the long
descriptor PTE format.

So this patch introduces support for both long descriptor PTEs and HYP mode
registers.

Signed-off-by: Alexander Graf <agraf@suse.de>

3 files changed, 161 insertions, 12 deletions

diff --git a/arch/arm/cpu/armv7/Kconfig b/arch/arm/cpu/armv7/Kconfig
index 6c5d5dd8e0..afeaac84de 100644
--- a/arch/arm/cpu/armv7/Kconfig
+++ b/arch/arm/cpu/armv7/Kconfig
@@ -31,4 +31,12 @@ config ARMV7_VIRT
 	---help---
 	Say Y here to boot in hypervisor (HYP) mode when booting non-secure.
 
+config ARMV7_LPAE
+	boolean "Use LPAE page table format" if EXPERT
+	depends on CPU_V7
+	default n
+	---help---
+	Say Y here to use the long descriptor page table format. This is
+	required if U-Boot runs in HYP mode.
+
 endif
diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
index 832c1dbd1c..9ae890a830 100644
--- a/arch/arm/include/asm/system.h
+++ b/arch/arm/include/asm/system.h
@@ -176,7 +176,9 @@ void smc_call(struct pt_regs *args);
 #define CR_AFE	(1 << 29)	/* Access flag enable			*/
 #define CR_TE	(1 << 30)	/* Thumb exception enable		*/
 
-#ifndef PGTABLE_SIZE
+#if defined(CONFIG_ARMV7_LPAE) && !defined(PGTABLE_SIZE)
+#define PGTABLE_SIZE		(4096 * 5)
+#elif !defined(PGTABLE_SIZE)
 #define PGTABLE_SIZE		(4096 * 4)
 #endif
 
@@ -233,17 +235,50 @@ void save_boot_params_ret(void);
 #define wfi()
 #endif
 
+static inline unsigned long get_cpsr(void)
+{
+	unsigned long cpsr;
+
+	asm volatile("mrs %0, cpsr" : "=r"(cpsr): );
+	return cpsr;
+}
+
+static inline int is_hyp(void)
+{
+#ifdef CONFIG_ARMV7_LPAE
+	/* HYP mode requires LPAE ... */
+	return ((get_cpsr() & 0x1f) == 0x1a);
+#else
+	/* ... so without LPAE support we can optimize all hyp code away */
+	return 0;
+#endif
+}
+
 static inline unsigned int get_cr(void)
 {
 	unsigned int val;
-	asm volatile("mrc p15, 0, %0, c1, c0, 0	@ get CR" : "=r" (val) : : "cc");
+
+	if (is_hyp())
+		asm volatile("mrc p15, 4, %0, c1, c0, 0	@ get CR" : "=r" (val)
+								  :
+								  : "cc");
+	else
+		asm volatile("mrc p15, 0, %0, c1, c0, 0	@ get CR" : "=r" (val)
+								  :
+								  : "cc");
 	return val;
 }
 
 static inline void set_cr(unsigned int val)
 {
-	asm volatile("mcr p15, 0, %0, c1, c0, 0	@ set CR"
-	  : : "r" (val) : "cc");
+	if (is_hyp())
+		asm volatile("mcr p15, 4, %0, c1, c0, 0	@ set CR" :
+								  : "r" (val)
+								  : "cc");
+	else
+		asm volatile("mcr p15, 0, %0, c1, c0, 0	@ set CR" :
+								  : "r" (val)
+								  : "cc");
 	isb();
 }
 
@@ -261,12 +296,59 @@ static inline void set_dacr(unsigned int val)
 	isb();
 }
 
-#ifdef CONFIG_CPU_V7
+#ifdef CONFIG_ARMV7_LPAE
+/* Long-Descriptor Translation Table Level 1/2 Bits */
+#define TTB_SECT_XN_MASK	(1ULL << 54)
+#define TTB_SECT_NG_MASK	(1 << 11)
+#define TTB_SECT_AF		(1 << 10)
+#define TTB_SECT_SH_MASK	(3 << 8)
+#define TTB_SECT_NS_MASK	(1 << 5)
+#define TTB_SECT_AP		(1 << 6)
+/* Note: TTB AP bits are set elsewhere */
+#define TTB_SECT_MAIR(x)	((x & 0x7) << 2) /* Index into MAIR */
+#define TTB_SECT		(1 << 0)
+#define TTB_PAGETABLE		(3 << 0)
+
+/* TTBCR flags */
+#define TTBCR_EAE		(1 << 31)
+#define TTBCR_T0SZ(x)		((x) << 0)
+#define TTBCR_T1SZ(x)		((x) << 16)
+#define TTBCR_USING_TTBR0	(TTBCR_T0SZ(0) | TTBCR_T1SZ(0))
+#define TTBCR_IRGN0_NC		(0 << 8)
+#define TTBCR_IRGN0_WBWA	(1 << 8)
+#define TTBCR_IRGN0_WT		(2 << 8)
+#define TTBCR_IRGN0_WBNWA	(3 << 8)
+#define TTBCR_IRGN0_MASK	(3 << 8)
+#define TTBCR_ORGN0_NC		(0 << 10)
+#define TTBCR_ORGN0_WBWA	(1 << 10)
+#define TTBCR_ORGN0_WT		(2 << 10)
+#define TTBCR_ORGN0_WBNWA	(3 << 10)
+#define TTBCR_ORGN0_MASK	(3 << 10)
+#define TTBCR_SHARED_NON	(0 << 12)
+#define TTBCR_SHARED_OUTER	(2 << 12)
+#define TTBCR_SHARED_INNER	(3 << 12)
+#define TTBCR_EPD0		(0 << 7)
+
+/*
+ * Memory types
+ */
+#define MEMORY_ATTRIBUTES	((0x00 << (0 * 8)) | (0x88 << (1 * 8)) | \
+				 (0xcc << (2 * 8)) | (0xff << (3 * 8)))
+
+/* options available for data cache on each page */
+enum dcache_option {
+	DCACHE_OFF = TTB_SECT | TTB_SECT_MAIR(0),
+	DCACHE_WRITETHROUGH = TTB_SECT | TTB_SECT_MAIR(1),
+	DCACHE_WRITEBACK = TTB_SECT | TTB_SECT_MAIR(2),
+	DCACHE_WRITEALLOC = TTB_SECT | TTB_SECT_MAIR(3),
+};
+#elif defined(CONFIG_CPU_V7)
 /* Short-Descriptor Translation Table Level 1 Bits */
 #define TTB_SECT_NS_MASK	(1 << 19)
 #define TTB_SECT_NG_MASK	(1 << 17)
 #define TTB_SECT_S_MASK		(1 << 16)
 /* Note: TTB AP bits are set elsewhere */
+#define TTB_SECT_AP		(3 << 10)
 #define TTB_SECT_TEX(x)		((x & 0x7) << 12)
 #define TTB_SECT_DOMAIN(x)	((x & 0xf) << 5)
 #define TTB_SECT_XN_MASK	(1 << 4)
@@ -282,6 +364,7 @@ enum dcache_option {
 	DCACHE_WRITEALLOC = DCACHE_WRITEBACK | TTB_SECT_TEX(1),
 };
 #else
+#define TTB_SECT_AP		(3 << 10)
 /* options available for data cache on each page */
 enum dcache_option {
 	DCACHE_OFF = 0x12,
@@ -293,7 +376,11 @@ enum dcache_option {
 
 /* Size of an MMU section */
 enum {
-	MMU_SECTION_SHIFT	= 20,
+#ifdef CONFIG_ARMV7_LPAE
+	MMU_SECTION_SHIFT	= 21, /* 2MB */
+#else
+	MMU_SECTION_SHIFT	= 20, /* 1MB */
+#endif
 	MMU_SECTION_SIZE	= 1 << MMU_SECTION_SHIFT,
 };
 
diff --git a/arch/arm/lib/cache-cp15.c b/arch/arm/lib/cache-cp15.c
index 8e185383a5..1121dc3a93 100644
--- a/arch/arm/lib/cache-cp15.c
+++ b/arch/arm/lib/cache-cp15.c
@@ -34,11 +34,22 @@ static void cp_delay (void)
 
 void set_section_dcache(int section, enum dcache_option option)
 {
+#ifdef CONFIG_ARMV7_LPAE
+	u64 *page_table = (u64 *)gd->arch.tlb_addr;
+	/* Need to set the access flag to not fault */
+	u64 value = TTB_SECT_AP | TTB_SECT_AF;
+#else
 	u32 *page_table = (u32 *)gd->arch.tlb_addr;
-	u32 value;
+	u32 value = TTB_SECT_AP;
+#endif
+
+	/* Add the page offset */
+	value |= ((u32)section << MMU_SECTION_SHIFT);
 
-	value = (section << MMU_SECTION_SHIFT) | (3 << 10);
+	/* Add caching bits */
 	value |= option;
+
+	/* Set PTE */
 	page_table[section] = value;
 }
 
@@ -68,8 +79,9 @@ __weak void dram_bank_mmu_setup(int bank)
 	int	i;
 
 	debug("%s: bank: %d\n", __func__, bank);
-	for (i = bd->bi_dram[bank].start >> 20;
-	     i < (bd->bi_dram[bank].start >> 20) + (bd->bi_dram[bank].size >> 20);
+	for (i = bd->bi_dram[bank].start >> MMU_SECTION_SHIFT;
+	     i < (bd->bi_dram[bank].start >> MMU_SECTION_SHIFT) +
+		 (bd->bi_dram[bank].size >> MMU_SECTION_SHIFT);
 	     i++) {
 #if defined(CONFIG_SYS_ARM_CACHE_WRITETHROUGH)
 		set_section_dcache(i, DCACHE_WRITETHROUGH);
@@ -89,14 +101,56 @@ static inline void mmu_setup(void)
 
 	arm_init_before_mmu();
 	/* Set up an identity-mapping for all 4GB, rw for everyone */
-	for (i = 0; i < 4096; i++)
+	for (i = 0; i < ((4096ULL * 1024 * 1024) >> MMU_SECTION_SHIFT); i++)
 		set_section_dcache(i, DCACHE_OFF);
 
 	for (i = 0; i < CONFIG_NR_DRAM_BANKS; i++) {
 		dram_bank_mmu_setup(i);
 	}
 
-#ifdef CONFIG_CPU_V7
+#ifdef CONFIG_ARMV7_LPAE
+	/* Set up 4 PTE entries pointing to our 4 1GB page tables */
+	for (i = 0; i < 4; i++) {
+		u64 *page_table = (u64 *)(gd->arch.tlb_addr + (4096 * 4));
+		u64 tpt = gd->arch.tlb_addr + (4096 * i);
+		page_table[i] = tpt | TTB_PAGETABLE;
+	}
+
+	reg = TTBCR_EAE;
+#if defined(CONFIG_SYS_ARM_CACHE_WRITETHROUGH)
+	reg |= TTBCR_ORGN0_WT | TTBCR_IRGN0_WT;
+#elif defined(CONFIG_SYS_ARM_CACHE_WRITEALLOC)
+	reg |= TTBCR_ORGN0_WBWA | TTBCR_IRGN0_WBWA;
+#else
+	reg |= TTBCR_ORGN0_WBNWA | TTBCR_IRGN0_WBNWA;
+#endif
+
+	if (is_hyp()) {
+		/* Set HCTR to enable LPAE */
+		asm volatile("mcr p15, 4, %0, c2, c0, 2"
+			: : "r" (reg) : "memory");
+		/* Set HTTBR0 */
+		asm volatile("mcrr p15, 4, %0, %1, c2"
+			:
+			: "r"(gd->arch.tlb_addr + (4096 * 4)), "r"(0)
+			: "memory");
+		/* Set HMAIR */
+		asm volatile("mcr p15, 4, %0, c10, c2, 0"
+			: : "r" (MEMORY_ATTRIBUTES) : "memory");
+	} else {
+		/* Set TTBCR to enable LPAE */
+		asm volatile("mcr p15, 0, %0, c2, c0, 2"
+			: : "r" (reg) : "memory");
+		/* Set 64-bit TTBR0 */
+		asm volatile("mcrr p15, 0, %0, %1, c2"
+			:
+			: "r"(gd->arch.tlb_addr + (4096 * 4)), "r"(0)
+			: "memory");
+		/* Set MAIR */
+		asm volatile("mcr p15, 0, %0, c10, c2, 0"
+			: : "r" (MEMORY_ATTRIBUTES) : "memory");
+	}
+#elif defined(CONFIG_CPU_V7)
 	/* Set TTBR0 */
 	reg = gd->arch.tlb_addr & TTBR0_BASE_ADDR_MASK;
 #if defined(CONFIG_SYS_ARM_CACHE_WRITETHROUGH)