diff options
| -rw-r--r-- | payloads/libpayload/arch/arm/virtual.c | 117 | ||||
| -rw-r--r-- | payloads/libpayload/include/arm/arch/cache.h | 9 | ||||
| -rw-r--r-- | payloads/libpayload/include/arm/arch/virtual.h | 5 | ||||
| -rw-r--r-- | payloads/libpayload/include/stddef.h | 4 | ||||
| -rw-r--r-- | payloads/libpayload/include/stdlib.h | 5 |
5 files changed, 139 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(); } diff --git a/payloads/libpayload/include/arm/arch/cache.h b/payloads/libpayload/include/arm/arch/cache.h index 470eb55108..647ec42ca2 100644 --- a/payloads/libpayload/include/arm/arch/cache.h +++ b/payloads/libpayload/include/arm/arch/cache.h @@ -117,6 +117,15 @@ static inline void write_ttbr0(uint32_t val) asm volatile ("mcr p15, 0, %0, c2, c0, 0" : : "r" (val) : "memory"); } +/* read translation table base register 0 (TTBR0) */ +static inline uint64_t read_ttbr0(void) +{ + uint32_t low, high; + asm volatile ("mrrc p15, 0, %[low], %[high], c2" : + [low] "=r" (low), [high] "=r" (high)); + return ((uint64_t)high << 32) | low; +} + /* read translation table base control register (TTBCR) */ static inline uint32_t read_ttbcr(void) { diff --git a/payloads/libpayload/include/arm/arch/virtual.h b/payloads/libpayload/include/arm/arch/virtual.h index 328c3aa34e..e51530d252 100644 --- a/payloads/libpayload/include/arm/arch/virtual.h +++ b/payloads/libpayload/include/arm/arch/virtual.h @@ -30,6 +30,8 @@ #ifndef _ARCH_VIRTUAL_H #define _ARCH_VIRTUAL_H +#include <arch/cache.h> + extern unsigned long virtual_offset; #define virt_to_phys(virt) ((unsigned long) (virt) + virtual_offset) @@ -38,4 +40,7 @@ extern unsigned long virtual_offset; #define virt_to_bus(addr) virt_to_phys(addr) #define bus_to_virt(addr) phys_to_virt(addr) +void *lpae_map_phys_addr(unsigned long pa_mb, enum dcache_policy policy); +void lpae_restore_map(void); + #endif diff --git a/payloads/libpayload/include/stddef.h b/payloads/libpayload/include/stddef.h index f9deaeb0dc..fca66ed247 100644 --- a/payloads/libpayload/include/stddef.h +++ b/payloads/libpayload/include/stddef.h @@ -15,3 +15,7 @@ typedef __SIZE_TYPE__ ssize_t; #define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *) 0)->MEMBER) +/* Standard units. */ +#define KiB (1<<10) +#define MiB (1<<20) +#define GiB (1<<30) diff --git a/payloads/libpayload/include/stdlib.h b/payloads/libpayload/include/stdlib.h index 7113b6f7d3..91d854ec67 100644 --- a/payloads/libpayload/include/stdlib.h +++ b/payloads/libpayload/include/stdlib.h @@ -35,6 +35,11 @@ #include <stddef.h> #include <string.h> +#define ALIGN(x,a) __ALIGN_MASK(x,(typeof(x))(a)-1UL) +#define __ALIGN_MASK(x,mask) (((x)+(mask))&~(mask)) +#define ALIGN_UP(x,a) ALIGN((x),(a)) +#define ALIGN_DOWN(x,a) ((x) & ~((typeof(x))(a)-1UL)) + /** * @defgroup malloc Memory allocation functions * @{ |