1 files changed, 116 insertions, 1 deletions

diff --git a/payloads/libpayload/arch/arm/virtual.c b/payloads/libpayload/arch/arm/virtual.c
index 59768dbd66..0d53e50616 100644
--- a/payloads/libpayload/arch/arm/virtual.c
+++ b/payloads/libpayload/arch/arm/virtual.c
@@ -27,12 +27,127 @@
  * SUCH DAMAGE.
  */
 
+#include <assert.h>
+#include <die.h>
+#include <stdlib.h>
 #include <unistd.h>
+#include <arch/cache.h>
+#include <arch/virtual.h>
+#include <arch/io.h>
 
 unsigned long virtual_offset = 0;
+extern char _end[];
 
+/*
+ * MAIR Index
+ * (Originally defined in src/arch/arm/include/armv7/arch/cache.h)
+ */
+#define MAIR_INDX_NC	0
+#define MAIR_INDX_WT	1
+#define MAIR_INDX_WB	2
+
+/*
+ * Translation Table Attribute
+ * (Originally defined in src/arch/arm/include/armv7/arch/cache.h)
+ */
+#define ATTR_BASE (\
+	0ULL << 54 |	/* PN. 0:Not restricted */ \
+	0ULL << 53 |	/* PXN. 0:Not restricted */ \
+	1 << 10 |	/* AF. 1:Accessed. This is to prevent access \
+	 	 	 * fault when accessed for the first time */ \
+	0 << 6 |	/* AP[2:1]. 0b00:full access from PL1 */ \
+	0 << 5 |	/* NS. 0:Output address is in Secure space */ \
+	0 << 1 | 	/* block/table. 0:block entry */ \
+	1 << 0		/* validity. 1:valid */ \
+	)
+#define ATTR_NC		(ATTR_BASE | (MAIR_INDX_NC << 2))
+#define ATTR_WT		(ATTR_BASE | (MAIR_INDX_WT << 2))
+#define ATTR_WB		(ATTR_BASE | (MAIR_INDX_WB << 2))
+
+/* Translation Table Entry */
+typedef uint64_t pmd_t;
+typedef uint64_t pgd_t;
+
+#define SECTION_SHIFT	30
+#define BLOCK_SHIFT	21
+#define PAGE_SHIFT	12
+#define PGD_MASK	(~0 << PAGE_SHIFT)
+
+static pmd_t *ttb_buff = 0;
+static uintptr_t work_block;
+static pmd_t original_map;
 
 int getpagesize(void)
 {
-	return 4096;
+	return 1 << PAGE_SHIFT;
+}
+
+static void lpae_map_init(void)
+{
+	pgd_t *pgd;
+
+	die_if(!(read_ttbcr() >> 31), "LPAE is not enabled\n");
+
+	/* get work block address */
+	work_block = ALIGN_UP((uintptr_t)_end, 2*MiB);
+	assert(work_block);
+	printf("Work block for LPAE mapping is @ 0x%p\n", (void *)work_block);
+
+	/* get the address of the 1st pmd from pgd[0] */
+	pgd = (pgd_t *)((uintptr_t)read_ttbr0() & PGD_MASK);
+	ttb_buff = (pmd_t *)((uintptr_t)pgd[0] & PGD_MASK);
+	assert(ttb_buff);
+
+	original_map = ttb_buff[work_block >> BLOCK_SHIFT];
+}
+
+static void lpae_flush_work_block(void)
+{
+	dccmvac((uintptr_t)&ttb_buff[work_block >> BLOCK_SHIFT]);
+	dsb();
+	tlbimvaa(work_block);
+	dsb();
+	isb();
+}
+
+/**
+ * Maps a 2MB designated block to a requested physical address, and returns
+ * the address to the block or NULL on error.
+ *
+ * pa_mb: Physical address in MB. Has to be on a 2MB boundary.
+ * policy: Data chache policy
+ */
+void *lpae_map_phys_addr(unsigned long pa_mb, enum dcache_policy policy)
+{
+	pmd_t attr;
+
+	if (!ttb_buff)
+		lpae_map_init();
+
+	switch(policy) {
+	case DCACHE_OFF:
+		attr = ATTR_NC;
+		break;
+	case DCACHE_WRITEBACK:
+		attr = ATTR_WB;
+		break;
+	case DCACHE_WRITETHROUGH:
+		attr = ATTR_WT;
+		break;
+	default:
+		return NULL;
+	}
+
+	ttb_buff[work_block >> BLOCK_SHIFT] =
+			((pmd_t)pa_mb/2 << BLOCK_SHIFT) | attr;
+
+	lpae_flush_work_block();
+
+	return (void *)work_block;
+}
+
+void lpae_restore_map(void)
+{
+	ttb_buff[work_block >> BLOCK_SHIFT] = original_map;
+	lpae_flush_work_block();
 }