#include <nuttx/config.h>

#include <stdint.h>

#include <nuttx/debug.h>

#include <assert.h>

#include <nuttx/arch.h>

#include <arch/barriers.h>

#include <arch/irq.h>

#include <arch/chip/chip.h>

#include "arm64_arch.h"

#include "arm64_internal.h"

#include "arm64_fatal.h"

#include "arm64_mmu.h"

#ifdef CONFIG_MMU_DEBUG

#define L0_SPACE ""

#define L1_SPACE " "

#define L2_SPACE " "

#define L3_SPACE " "

#define XLAT_TABLE_LEVEL_SPACE(level) \

(((level) == 0) ? L0_SPACE : \

((level) == 1) ? L1_SPACE : \

((level) == 2) ? L2_SPACE : L3_SPACE)

#ifdef CONFIG_MMU_ASSERT

#define __MMU_ASSERT(__cond, fmt, ...) \

do \

{ \

if (!(__cond)) \

{ \

trace_printf(fmt, ## __VA_ARGS__); \

PANIC(); \

} \

} \

while (false)

#define __MMU_ASSERT(test, fmt, ...)

#define PAGE_SIZE_SHIFT MMU_PAGE_SHIFT

#define PAGE_SIZE (1U << PAGE_SIZE_SHIFT)

#define XLAT_TABLE_SIZE_SHIFT PAGE_SIZE_SHIFT /* Size of one

#define XLAT_TABLE_SIZE (1U << XLAT_TABLE_SIZE_SHIFT)

#define XLAT_TABLE_ENTRY_SIZE_SHIFT 3U /* Each table entry is 8 bytes */

#define XLAT_TABLE_LEVEL_MAX MMU_PGT_LEVEL_MAX

#define XLAT_TABLE_ENTRIES_SHIFT \

(XLAT_TABLE_SIZE_SHIFT - XLAT_TABLE_ENTRY_SIZE_SHIFT)

#define XLAT_TABLE_ENTRIES (1U << XLAT_TABLE_ENTRIES_SHIFT)

#define L3_XLAT_VA_SIZE_SHIFT PAGE_SIZE_SHIFT

#define L2_XLAT_VA_SIZE_SHIFT \

(L3_XLAT_VA_SIZE_SHIFT + XLAT_TABLE_ENTRIES_SHIFT)

#define L1_XLAT_VA_SIZE_SHIFT \

(L2_XLAT_VA_SIZE_SHIFT + XLAT_TABLE_ENTRIES_SHIFT)

#define L0_XLAT_VA_SIZE_SHIFT \

(L1_XLAT_VA_SIZE_SHIFT + XLAT_TABLE_ENTRIES_SHIFT)

#define LEVEL_TO_VA_SIZE_SHIFT(level) \

(PAGE_SIZE_SHIFT + (XLAT_TABLE_ENTRIES_SHIFT * \

(XLAT_TABLE_LEVEL_MAX - (level))))

#define XLAT_TABLE_VA_IDX(va_addr, level) \

((va_addr >> LEVEL_TO_VA_SIZE_SHIFT(level)) & (XLAT_TABLE_ENTRIES - 1))

#define GET_XLAT_TABLE_BASE_LEVEL(va_bits) \

((va_bits > L0_XLAT_VA_SIZE_SHIFT) \

? 0U \

: (va_bits > L1_XLAT_VA_SIZE_SHIFT) \

? 1U \

: (va_bits > L2_XLAT_VA_SIZE_SHIFT) \

? 2U : 3U)

#define XLAT_TABLE_BASE_LEVEL GET_XLAT_TABLE_BASE_LEVEL(CONFIG_ARM64_VA_BITS)

#define GET_NUM_BASE_LEVEL_ENTRIES(va_bits) \

(1U << (va_bits - LEVEL_TO_VA_SIZE_SHIFT(XLAT_TABLE_BASE_LEVEL)))

#define NUM_BASE_LEVEL_ENTRIES GET_NUM_BASE_LEVEL_ENTRIES( \

CONFIG_ARM64_VA_BITS)

#ifdef CONFIG_BUILD_KERNEL

#define BASE_XLAT_TABLE_SIZE XLAT_TABLE_ENTRIES

#define BASE_XLAT_TABLE_ALIGN PAGE_SIZE

#define BASE_XLAT_TABLE_SIZE NUM_BASE_LEVEL_ENTRIES

#define BASE_XLAT_TABLE_ALIGN NUM_BASE_LEVEL_ENTRIES * sizeof(uint64_t)

#if (CONFIG_ARM64_PA_BITS == 52)

#define TCR_PS_BITS TCR_PS_BITS_4PB

#elif (CONFIG_ARM64_PA_BITS == 48)

#define TCR_PS_BITS TCR_PS_BITS_256TB

#elif (CONFIG_ARM64_PA_BITS == 44)

#define TCR_PS_BITS TCR_PS_BITS_16TB

#elif (CONFIG_ARM64_PA_BITS == 42)

#define TCR_PS_BITS TCR_PS_BITS_4TB

#elif (CONFIG_ARM64_PA_BITS == 40)

#define TCR_PS_BITS TCR_PS_BITS_1TB

#elif (CONFIG_ARM64_PA_BITS == 36)

#define TCR_PS_BITS TCR_PS_BITS_64GB

#define TCR_PS_BITS TCR_PS_BITS_4GB

#ifdef CONFIG_ARM64_TBI

#define TCR_TBI_FLAGS (TCR_TBI0 | TCR_TBI1 | TCR_ASID_8)

#define TCR_TBI_FLAGS 0

static uint64_t base_xlat_table[BASE_XLAT_TABLE_SIZE]

aligned_data(BASE_XLAT_TABLE_ALIGN);

static uint64_t xlat_tables[CONFIG_MAX_XLAT_TABLES][XLAT_TABLE_ENTRIES]

aligned_data(XLAT_TABLE_ENTRIES * sizeof(uint64_t));

static const struct arm_mmu_region g_mmu_nxrt_regions[] =

{

/* Mark text segment cacheable,read only and executable */

MMU_REGION_FLAT_ENTRY("nx_code",

(uint64_t)_stext,

(uint64_t)_sztext,

MT_CODE | MT_SECURE),

/* Mark rodata segment cacheable, read only and execute-never */

MMU_REGION_FLAT_ENTRY("nx_rodata",

(uint64_t)_srodata,

(uint64_t)_szrodata,

MT_RODATA | MT_SECURE),

/* Mark rest of the mirtos execution regions (data, bss, noinit, etc.)

MMU_REGION_FLAT_ENTRY("nx_data",

(uint64_t)_sdata,

(uint64_t)_szdata,

MT_NORMAL | MT_RW | MT_SECURE),

#ifdef CONFIG_BUILD_KERNEL

MMU_REGION_FLAT_ENTRY("nx_pgpool",

(uint64_t)CONFIG_ARCH_PGPOOL_PBASE,

(uint64_t)CONFIG_ARCH_PGPOOL_SIZE,

MT_NORMAL | MT_RW | MT_SECURE),

};

static const struct arm_mmu_config g_mmu_nxrt_config =

{

.num_regions = nitems(g_mmu_nxrt_regions),

.mmu_regions = g_mmu_nxrt_regions,

};

static const size_t g_pgt_sizes[] =

{

MMU_L0_PAGE_SIZE,

MMU_L1_PAGE_SIZE,

MMU_L2_PAGE_SIZE,

MMU_L3_PAGE_SIZE

};

uintptr_t g_kernel_mappings = (uintptr_t)&base_xlat_table;

uintptr_t g_kernel_pgt_pbase = (uintptr_t)&base_xlat_table;

static uint64_t get_tcr(int el)

{

uint64_t tcr;

uint64_t va_bits = CONFIG_ARM64_VA_BITS;

uint64_t tcr_ps_bits;

tcr_ps_bits = TCR_PS_BITS;

if (el == 1)

{

tcr = (tcr_ps_bits << TCR_EL1_IPS_SHIFT);

/* TCR_EL1.EPD1: Disable translation table walk for addresses

tcr |= TCR_EPD1_DISABLE;

}

else

{

tcr = (tcr_ps_bits << TCR_EL3_PS_SHIFT);

}

tcr |= TCR_T0SZ(va_bits);

/* Translation table walk is cacheable, inner/outer WBWA and

tcr |= TCR_TG0_4K | TCR_SHARED_INNER | TCR_ORGN_WBWA |

TCR_IRGN_WBWA | TCR_TBI_FLAGS;

#if (CONFIG_ARM64_PA_BITS == 52)

tcr |= TCR_DS;

return tcr;

}

static int pte_desc_type(uint64_t *pte)

{

return *pte & PTE_DESC_TYPE_MASK;

}

static uint64_t *calculate_pte_index(uint64_t addr, int level)

{

int base_level = XLAT_TABLE_BASE_LEVEL;

uint64_t *pte;

uint64_t idx;

unsigned int i;

/* Walk through all translation tables to find pte index */

pte = (uint64_t *)base_xlat_table;

for (i = base_level; i <= XLAT_TABLE_LEVEL_MAX; i++)

{

idx = XLAT_TABLE_VA_IDX(addr, i);

pte += idx;

/* Found pte index */

if (i == level)

{

return pte;

}

/* if PTE is not table desc, can't traverse */

if (pte_desc_type(pte) != PTE_TABLE_DESC)

{

return NULL;

}

/* Move to the next translation table level */

pte = (uint64_t *)(*pte & 0x0000fffffffff000);

}

return NULL;

}

static void set_pte_table_desc(uint64_t *pte, uint64_t *table,

unsigned int level)

{

#ifdef CONFIG_MMU_DEBUG

sinfo("%s", XLAT_TABLE_LEVEL_SPACE(level));

sinfo("%p: [Table] %p\n", pte, table);

/* Point pte to new table */

*pte = PTE_TABLE_DESC | (uint64_t)table;

}

static void set_pte_block_desc(uint64_t *pte, uint64_t addr_pa,

unsigned int attrs, unsigned int level)

{

uint64_t desc = addr_pa;

unsigned int mem_type;

desc |= (level == 3) ? PTE_PAGE_DESC : PTE_BLOCK_DESC;

/* NS bit for security memory access from secure state */

desc |= (attrs & MT_NS) ? PTE_BLOCK_DESC_NS : 0;

/* AP bits for Data access permission */

desc |= (attrs & MT_RW) ? PTE_BLOCK_DESC_AP_RW : PTE_BLOCK_DESC_AP_RO;

/* the access flag */

desc |= PTE_BLOCK_DESC_AF;

/* memory attribute index field */

mem_type = MT_TYPE(attrs);

desc |= PTE_BLOCK_DESC_MEMTYPE(mem_type);

switch (mem_type)

{

case MT_DEVICE_NGNRNE:

case MT_DEVICE_NGNRE:

case MT_DEVICE_GRE:

{

/* Access to Device memory and non-cacheable memory are coherent

desc |= PTE_BLOCK_DESC_OUTER_SHARE;

/* Map device memory as execute-never */

desc |= PTE_BLOCK_DESC_PXN;

desc |= PTE_BLOCK_DESC_UXN;

break;

}

case MT_NORMAL_NC:

case MT_NORMAL:

{

/* Make Normal RW memory as execute never */

if (attrs & MT_EXECUTE_NEVER)

{

desc |= PTE_BLOCK_DESC_PXN;

}

if (mem_type == MT_NORMAL)

{

desc |= PTE_BLOCK_DESC_INNER_SHARE;

}

else

{

desc |= PTE_BLOCK_DESC_OUTER_SHARE;

}

}

}

#if defined(CONFIG_MMU_DEBUG) && defined(CONFIG_MMU_DUMP_PTE)

sinfo("%s ", XLAT_TABLE_LEVEL_SPACE(level));

sinfo("%p: ", pte);

sinfo("%s ",

(mem_type ==

MT_NORMAL) ? "MEM" :((mem_type == MT_NORMAL_NC) ? "NC" : "DEV"));

sinfo("%s ", (attrs & MT_RW) ? "-RW" : "-RO");

sinfo("%s ", (attrs & MT_NS) ? "-NS" : "-S");

sinfo("%s ", (attrs & MT_EXECUTE_NEVER) ? "-XN" : "-EXEC");

sinfo("\n");

*pte = desc;

}

static uint64_t *new_prealloc_table(void)

{

static unsigned int table_idx;

__MMU_ASSERT(table_idx < CONFIG_MAX_XLAT_TABLES,

"Enough xlat tables not allocated");

return (uint64_t *)(xlat_tables[table_idx++]);

}

static void split_pte_block_desc(uint64_t *pte, int level)

{

uint64_t old_block_desc = *pte;

uint64_t *new_table;

unsigned int i = 0;

/* get address size shift bits for next level */

int levelshift = LEVEL_TO_VA_SIZE_SHIFT(level + 1);

#ifdef CONFIG_MMU_DEBUG

sinfo("Splitting existing PTE %p(L%d)\n", pte, level);

new_table = new_prealloc_table();

for (i = 0; i < XLAT_TABLE_ENTRIES; i++)

{

new_table[i] = old_block_desc | (i << levelshift);

if (level == 2)

{

new_table[i] |= PTE_PAGE_DESC;

}

}

/* Overwrite existing PTE set the new table into effect */

set_pte_table_desc(pte, new_table, level);

}

static void init_xlat_tables(const struct arm_mmu_region *region)

{

unsigned int level = XLAT_TABLE_BASE_LEVEL;

uint64_t virt = region->base_va;

uint64_t phys = region->base_pa;

uint64_t size = region->size;

uint64_t attrs = region->attrs;

uint64_t *pte;

uint64_t *new_table;

uint64_t level_size;

#ifdef CONFIG_MMU_DEBUG

sinfo("mmap: virt %lux phys %lux size %lux\n", virt, phys, size);

/* check minimum alignment requirement for given mmap region */

__MMU_ASSERT(((virt & (PAGE_SIZE - 1)) == 0) &&

((size & (PAGE_SIZE - 1)) == 0),

"address/size are not page aligned\n");

while (size)

{

__MMU_ASSERT(level <= XLAT_TABLE_LEVEL_MAX,

"max translation table level exceeded\n");

/* Locate PTE for given virtual address and page table level */

pte = calculate_pte_index(virt, level);

__MMU_ASSERT(pte != NULL, "pte not found\n");

level_size = 1ULL << LEVEL_TO_VA_SIZE_SHIFT(level);

if (size >= level_size && !(virt & (level_size - 1))

&& ((level == 0 && CONFIG_ARM64_PA_BITS == 52) || level != 0))

{

/* Given range fits into level size,

set_pte_block_desc(pte, phys, attrs, level);

virt += level_size;

phys += level_size;

size -= level_size;

/* Range is mapped, start again for next range */

level = XLAT_TABLE_BASE_LEVEL;

}

else if (pte_desc_type(pte) == PTE_INVALID_DESC)

{

/* Range doesn't fit, create subtable */

new_table = new_prealloc_table();

set_pte_table_desc(pte, new_table, level);

level++;

}

else if (pte_desc_type(pte) == PTE_BLOCK_DESC)

{

split_pte_block_desc(pte, level);

level++;

}

else if (pte_desc_type(pte) == PTE_TABLE_DESC)

{

level++;

}

}

}

static void setup_page_tables(void)

{

uint64_t max_va = 0, max_pa = 0;

const struct arm_mmu_region *region;

unsigned int index;

for (index = 0; index < g_mmu_config.num_regions; index++)

{

region = &g_mmu_config.mmu_regions[index];

max_va = MAX(max_va, region->base_va + region->size);

max_pa = MAX(max_pa, region->base_pa + region->size);

}

__MMU_ASSERT(max_va <= (1ULL << CONFIG_ARM64_VA_BITS),

"Maximum VA not supported\n");

__MMU_ASSERT(max_pa <= (1ULL << CONFIG_ARM64_PA_BITS),

"Maximum PA not supported\n");

/* create translation tables for user provided platform regions */

for (index = 0; index < g_mmu_config.num_regions; index++)

{

region = &g_mmu_config.mmu_regions[index];

if (region->size || region->attrs)

{

init_xlat_tables(region);

}

}

/* setup translation table for mirtos execution regions */

for (index = 0; index < g_mmu_nxrt_config.num_regions; index++)

{

region = &g_mmu_nxrt_config.mmu_regions[index];

if (region->size || region->attrs)

{

init_xlat_tables(region);

}

}

}

#if CONFIG_ARCH_ARM64_EXCEPTION_LEVEL == 3

static void enable_mmu_el3(unsigned int flags)

{

uint64_t value;

UNUSED(flags);

/* Set MAIR, TCR and TBBR registers */

write_sysreg(MEMORY_ATTRIBUTES, mair_el3);

write_sysreg(get_tcr(3), tcr_el3);

write_sysreg((uint64_t)base_xlat_table, ttbr0_el3);

/* Ensure these changes are seen before MMU is enabled */

UP_MB();

/* Enable the MMU and data cache */

value = read_sysreg(sctlr_el3);

write_sysreg((value | SCTLR_M_BIT

#ifndef CONFIG_ARM64_DCACHE_DISABLE

| SCTLR_C_BIT

), sctlr_el3);

/* Ensure the MMU enable takes effect immediately */

UP_ISB();

#ifdef CONFIG_MMU_DEBUG

sinfo("MMU enabled with dcache\n");

}

static void enable_mmu_el1(unsigned int flags)

{

uint64_t value;

UNUSED(flags);

/* Set MAIR, TCR and TBBR registers */

write_sysreg(MEMORY_ATTRIBUTES, mair_el1);

write_sysreg(get_tcr(1), tcr_el1);

write_sysreg((uint64_t)base_xlat_table, ttbr0_el1);

/* Ensure these changes are seen before MMU is enabled */

UP_MB();

/* Enable the MMU and data cache */

value = read_sysreg(sctlr_el1);

write_sysreg((value | SCTLR_M_BIT

#ifndef CONFIG_ARM64_DCACHE_DISABLE

| SCTLR_C_BIT

), sctlr_el1);

/* Ensure the MMU enable takes effect immediately */

UP_ISB();

#ifdef CONFIG_MMU_DEBUG

sinfo("MMU enabled with dcache\n");

}

int arm64_mmu_set_memregion(const struct arm_mmu_region *region)

{

uint64_t virt = region->base_va;

uint64_t size = region->size;

if (((virt & (PAGE_SIZE - 1)) == 0) &&

((size & (PAGE_SIZE - 1)) == 0))

{

init_xlat_tables(region);

}

else

{

sinfo("address/size are not page aligned\n");

return -EINVAL;

}

return 0;

}

int arm64_mmu_init(bool is_primary_core)

{

uint64_t val;

uint64_t el;

unsigned flags = 0;

/* Current MMU code supports EL1 and EL3 */

__asm__ volatile ("mrs %0, CurrentEL" : "=r" (el));

#if CONFIG_ARCH_ARM64_EXCEPTION_LEVEL == 3

__MMU_ASSERT(GET_EL(el) == MODE_EL3,

"Exception level not EL3, MMU not enabled!\n");

/* Ensure that MMU is already not enabled */

__asm__ volatile ("mrs %0, sctlr_el3" : "=r" (val));

__MMU_ASSERT((val & SCTLR_M_BIT) == 0, "MMU is already enabled\n");

__MMU_ASSERT(GET_EL(el) == MODE_EL1,

"Exception level not EL1, MMU not enabled!\n");

/* Ensure that MMU is already not enabled */

__asm__ volatile ("mrs %0, sctlr_el1" : "=r" (val));

__MMU_ASSERT((val & SCTLR_M_BIT) == 0, "MMU is already enabled\n");

#ifdef CONFIG_MMU_DEBUG

sinfo("xlat tables:\n");

sinfo("base table(L%d): %p, %d entries\n", XLAT_TABLE_BASE_LEVEL,

(uint64_t *)base_xlat_table, NUM_BASE_LEVEL_ENTRIES);

for (int idx = 0; idx < CONFIG_MAX_XLAT_TABLES; idx++)

{

sinfo("%d: %p\n", idx, (uint64_t *)(xlat_tables + idx));

}

if (is_primary_core)

{

setup_page_tables();

}

/* Currently EL1 and EL3 are supported */

#if CONFIG_ARCH_ARM64_EXCEPTION_LEVEL == 3

enable_mmu_el3(flags);

enable_mmu_el1(flags);

return 0;

}

void mmu_ln_setentry(uint32_t ptlevel, uintptr_t lnvaddr, uintptr_t paddr,

uintptr_t vaddr, uint64_t mmuflags)

{

uintptr_t *lntable = (uintptr_t *)lnvaddr;

uint32_t index;

DEBUGASSERT(ptlevel >= XLAT_TABLE_BASE_LEVEL &&

ptlevel <= XLAT_TABLE_LEVEL_MAX);

/* Calculate index for lntable */

index = XLAT_TABLE_VA_IDX(vaddr, ptlevel);

/* Save it */

lntable[index] = (paddr | mmuflags);

/* Update with memory by flushing the cache */

up_flush_dcache((uintptr_t)&lntable[index],

(uintptr_t)&lntable[index] + sizeof(uintptr_t));

/* Remove TLB entry for the modified page */

mmu_invalidate_tlb_by_vaddr(vaddr);

}

uintptr_t mmu_ln_getentry(uint32_t ptlevel, uintptr_t lnvaddr,

uintptr_t vaddr)

{

uintptr_t *lntable = (uintptr_t *)lnvaddr;

uint32_t index;

DEBUGASSERT(ptlevel >= XLAT_TABLE_BASE_LEVEL &&

ptlevel <= XLAT_TABLE_LEVEL_MAX);

index = XLAT_TABLE_VA_IDX(vaddr, ptlevel);

/* Invalidate D-Cache so that we read from the physical memory */

up_invalidate_dcache((uintptr_t)&lntable[index],

(uintptr_t)&lntable[index] + sizeof(uintptr_t));

return lntable[index];

}

void mmu_ln_restore(uint32_t ptlevel, uintptr_t lnvaddr, uintptr_t vaddr,

uintptr_t entry)

{

uintptr_t *lntable = (uintptr_t *)lnvaddr;

uint32_t index;

DEBUGASSERT(ptlevel >= XLAT_TABLE_BASE_LEVEL &&

ptlevel <= XLAT_TABLE_LEVEL_MAX);

index = XLAT_TABLE_VA_IDX(vaddr, ptlevel);

lntable[index] = entry;

/* Update with memory by flushing the cache */

up_flush_dcache((uintptr_t)&lntable[index],

(uintptr_t)&lntable[index] + sizeof(uintptr_t));

/* Remove TLB entry for the modified page */

mmu_invalidate_tlb_by_vaddr(vaddr);

}

size_t mmu_get_region_size(uint32_t ptlevel)

{

DEBUGASSERT(ptlevel >= XLAT_TABLE_BASE_LEVEL &&

ptlevel <= XLAT_TABLE_LEVEL_MAX);

return g_pgt_sizes[ptlevel];

}

uintptr_t mmu_get_base_pgt_level(void)

{

return XLAT_TABLE_BASE_LEVEL;

}