[PATCH v2 1/3] xen/arm: Add 4-level page table for stage 2 translation

[vijay.kilari@gmail.com]

From: Vijaya Kumar K <Vijaya.Kumar@caviumnetworks.com>

To support 48-bit Physical Address support, add 4-level
page tables for stage 2 translation.
With this patch stage 1 and stage 2 translation at EL2 are
with 4-levels

Configure TCR_EL2.IPS and VTCR_EL2.PS based on platform
supported PA range at runtime

Signed-off-by: Vijaya Kumar K <Vijaya.Kumar@caviumnetworks.com>
---
 xen/arch/arm/arm64/head.S       |   14 ++--
 xen/arch/arm/mm.c               |   18 +++---
 xen/arch/arm/p2m.c              |  136 +++++++++++++++++++++++++++++++++------
 xen/include/asm-arm/p2m.h       |    5 +-
 xen/include/asm-arm/page.h      |   16 +++--
 xen/include/asm-arm/processor.h |  102 ++++++++++++++++++++++++++++-
 6 files changed, 248 insertions(+), 43 deletions(-)

diff --git a/xen/arch/arm/arm64/head.S b/xen/arch/arm/arm64/head.S
index 2a13527..8396268 100644
--- a/xen/arch/arm/arm64/head.S
+++ b/xen/arch/arm/arm64/head.S
@@ -224,13 +224,13 @@ skip_bss:
         ldr   x0, =MAIRVAL
         msr   mair_el2, x0
 
-        /* Set up the HTCR:
-         * PASize -- 40 bits / 1TB
-         * Top byte is used
-         * PT walks use Inner-Shareable accesses,
-         * PT walks are write-back, write-allocate in both cache levels,
-         * Full 64-bit address space goes through this table. */
-        ldr   x0, =0x80823500
+        mrs   x1, ID_AA64MMFR0_EL1
+
+        /* Set up the HTCR */
+        ldr   x0, =TCR_VAL_BASE
+
+        /* Set TCR_EL2.IPS based on ID_AA64MMFR0_EL1.PARange */
+        bfi   x0, x1, #16, #3
         msr   tcr_el2, x0
 
         /* Set up the SCTLR_EL2:
diff --git a/xen/arch/arm/mm.c b/xen/arch/arm/mm.c
index eac228c..04e3182 100644
--- a/xen/arch/arm/mm.c
+++ b/xen/arch/arm/mm.c
@@ -377,17 +377,17 @@ void __init arch_init_memory(void)
 void __cpuinit setup_virt_paging(void)
 {
     /* Setup Stage 2 address translation */
-    /* SH0=11 (Inner-shareable)
-     * ORGN0=IRGN0=01 (Normal memory, Write-Back Write-Allocate Cacheable)
-     * SL0=01 (Level-1)
-     * ARVv7: T0SZ=(1)1000 = -8 (32-(-8) = 40 bit physical addresses)
-     * ARMv8: T0SZ=01 1000 = 24 (64-24   = 40 bit physical addresses)
-     *        PS=010 == 40 bits
-     */
 #ifdef CONFIG_ARM_32
-    WRITE_SYSREG32(0x80003558, VTCR_EL2);
+    WRITE_SYSREG32(VTCR_VAL_BASE, VTCR_EL2);
 #else
-    WRITE_SYSREG32(0x80023558, VTCR_EL2);
+    /* Update IPA 48 bit and PA 48 bit */
+    if ( current_cpu_data.mm64.pa_range == VTCR_PS_48BIT_VAL )
+        WRITE_SYSREG32(VTCR_VAL_BASE | VTCR_TOSZ_48BIT | VTCR_PS_48BIT,
+                       VTCR_EL2);
+    else
+        /* default to IPA 48 bit and PA 40 bit */
+        WRITE_SYSREG32(VTCR_VAL_BASE | VTCR_TOSZ_40BIT | VTCR_PS_40BIT,
+                       VTCR_EL2);
 #endif
     isb();
 }
diff --git a/xen/arch/arm/p2m.c b/xen/arch/arm/p2m.c
index 603c097..045c003 100644
--- a/xen/arch/arm/p2m.c
+++ b/xen/arch/arm/p2m.c
@@ -10,29 +10,42 @@
 #include <asm/hardirq.h>
 #include <asm/page.h>
 
+#ifdef CONFIG_ARM_64
+/* Zeroeth level is of 1 page size */
+#define P2M_ROOT_ORDER 0
+#else
 /* First level P2M is 2 consecutive pages */
-#define P2M_FIRST_ORDER 1
-#define P2M_FIRST_ENTRIES (LPAE_ENTRIES<<P2M_FIRST_ORDER)
+#define P2M_ROOT_ORDER 1
+#endif
+#define P2M_FIRST_ENTRIES (LPAE_ENTRIES << P2M_ROOT_ORDER)
 
 void dump_p2m_lookup(struct domain *d, paddr_t addr)
 {
     struct p2m_domain *p2m = &d->arch.p2m;
-    lpae_t *first;
+    lpae_t *lookup;
 
     printk("dom%d IPA 0x%"PRIpaddr"\n", d->domain_id, addr);
 
-    if ( first_linear_offset(addr) > LPAE_ENTRIES )
+#ifdef CONFIG_ARM_64
+    if ( zeroeth_linear_offset(addr) > P2M_FIRST_ENTRIES )
+    {
+        printk("Beyond number of support entries at zeroeth level\n");
+        return;
+    }
+#else
+    if ( first_linear_offset(addr) > P2M_FIRST_ENTRIES )
     {
         printk("Cannot dump addresses in second of first level pages...\n");
         return;
     }
+#endif
 
     printk("P2M @ %p mfn:0x%lx\n",
-           p2m->first_level, page_to_mfn(p2m->first_level));
+           p2m->root_level, page_to_mfn(p2m->root_level));
 
-    first = __map_domain_page(p2m->first_level);
-    dump_pt_walk(first, addr);
-    unmap_domain_page(first);
+    lookup = __map_domain_page(p2m->root_level);
+    dump_pt_walk(lookup, addr);
+    unmap_domain_page(lookup);
 }
 
 static void p2m_load_VTTBR(struct domain *d)
@@ -72,6 +85,20 @@ void p2m_restore_state(struct vcpu *n)
     isb();
 }
 
+#ifdef CONFIG_ARM_64
+/*
+ * Map zeroeth level page that addr contains.
+ */
+static lpae_t *p2m_map_zeroeth(struct p2m_domain *p2m, paddr_t addr)
+{
+    if ( zeroeth_linear_offset(addr) >= LPAE_ENTRIES )
+        return NULL;
+
+    return __map_domain_page(p2m->root_level);
+}
+
+#else
+
 static int p2m_first_level_index(paddr_t addr)
 {
     /*
@@ -92,10 +119,11 @@ static lpae_t *p2m_map_first(struct p2m_domain *p2m, paddr_t addr)
     if ( first_linear_offset(addr) >= P2M_FIRST_ENTRIES )
         return NULL;
 
-    page = p2m->first_level + p2m_first_level_index(addr);
+    page = p2m->root_level + p2m_first_level_index(addr);
 
     return __map_domain_page(page);
 }
+#endif
 
 /*
  * Lookup the MFN corresponding to a domain's PFN.
@@ -107,6 +135,9 @@ paddr_t p2m_lookup(struct domain *d, paddr_t paddr, p2m_type_t *t)
 {
     struct p2m_domain *p2m = &d->arch.p2m;
     lpae_t pte, *first = NULL, *second = NULL, *third = NULL;
+#ifdef CONFIG_ARM_64
+    lpae_t *zeroeth = NULL;
+#endif
     paddr_t maddr = INVALID_PADDR;
     p2m_type_t _t;
 
@@ -117,9 +148,26 @@ paddr_t p2m_lookup(struct domain *d, paddr_t paddr, p2m_type_t *t)
 
     spin_lock(&p2m->lock);
 
+#ifdef CONFIG_ARM_64
+    zeroeth = p2m_map_zeroeth(p2m, paddr);
+    if ( !zeroeth )
+        goto err;
+
+    pte = zeroeth[zeroeth_table_offset(paddr)];
+    /* Zeroeth level does not support block translation
+     * so pte.p2m.table should be always set.
+     * Just check for valid bit
+     */
+    if ( !pte.p2m.valid )
+        goto done;
+
+    /* Map first level table */
+    first = map_domain_page(pte.p2m.base);
+#else
     first = p2m_map_first(p2m, paddr);
     if ( !first )
         goto err;
+#endif
 
     pte = first[first_table_offset(paddr)];
     if ( !pte.p2m.valid || !pte.p2m.table )
@@ -148,6 +196,9 @@ done:
     if (third) unmap_domain_page(third);
     if (second) unmap_domain_page(second);
     if (first) unmap_domain_page(first);
+#ifdef CONFIG_ARM_64
+    if (zeroeth) unmap_domain_page(zeroeth);
+#endif
 
 err:
     spin_unlock(&p2m->lock);
@@ -286,8 +337,14 @@ static int apply_p2m_changes(struct domain *d,
     struct p2m_domain *p2m = &d->arch.p2m;
     lpae_t *first = NULL, *second = NULL, *third = NULL;
     paddr_t addr;
-    unsigned long cur_first_page = ~0,
-                  cur_first_offset = ~0,
+#ifdef CONFIG_ARM_64
+    lpae_t *zeroeth = NULL;
+    unsigned long cur_zeroeth_page = ~0,
+                  cur_zeroeth_offset = ~0;
+#else
+    unsigned long cur_first_page = ~0;
+#endif
+    unsigned long cur_first_offset = ~0,
                   cur_second_offset = ~0;
     unsigned long count = 0;
     unsigned int flush = 0;
@@ -299,6 +356,44 @@ static int apply_p2m_changes(struct domain *d,
     addr = start_gpaddr;
     while ( addr < end_gpaddr )
     {
+#ifdef CONFIG_ARM_64
+        /* Find zeroeth offset and map zeroeth page */
+        if ( cur_zeroeth_page != zeroeth_table_offset(addr) )
+        {
+            if ( zeroeth ) unmap_domain_page(zeroeth);
+            zeroeth = p2m_map_zeroeth(p2m, addr);
+            if ( !zeroeth )
+            {
+                rc = -EINVAL;
+                goto out;
+            }
+            cur_zeroeth_page = zeroeth_table_offset(addr);
+        }
+
+        if ( !zeroeth[zeroeth_table_offset(addr)].p2m.valid )
+        {
+            if ( !populate )
+            {
+                addr = (addr + ZEROETH_SIZE) & ZEROETH_MASK;
+                continue;
+            }
+            rc = p2m_create_table(d, &zeroeth[zeroeth_table_offset(addr)]);
+            if ( rc < 0 )
+            {
+                printk("p2m_populate_ram: L0 failed\n");
+                goto out;
+            }
+        } 
+
+        BUG_ON(!zeroeth[zeroeth_table_offset(addr)].p2m.valid);
+
+        if ( cur_zeroeth_offset != zeroeth_table_offset(addr) )
+        {
+            if ( first ) unmap_domain_page(first);
+            first = map_domain_page(zeroeth[zeroeth_table_offset(addr)].p2m.base);
+            cur_zeroeth_offset = zeroeth_table_offset(addr);
+        }
+#else
         if ( cur_first_page != p2m_first_level_index(addr) )
         {
             if ( first ) unmap_domain_page(first);
@@ -310,7 +405,7 @@ static int apply_p2m_changes(struct domain *d,
             }
             cur_first_page = p2m_first_level_index(addr);
         }
-
+#endif
         if ( !first[first_table_offset(addr)].p2m.valid )
         {
             if ( !populate )
@@ -479,6 +574,9 @@ out:
     if (third) unmap_domain_page(third);
     if (second) unmap_domain_page(second);
     if (first) unmap_domain_page(first);
+#ifdef CONFIG_ARM_64
+    if ( zeroeth ) unmap_domain_page(zeroeth);
+#endif
 
     spin_unlock(&p2m->lock);
 
@@ -530,7 +628,7 @@ int p2m_alloc_table(struct domain *d)
     struct page_info *page;
     void *p;
 
-    page = alloc_domheap_pages(NULL, P2M_FIRST_ORDER, 0);
+    page = alloc_domheap_pages(NULL, P2M_ROOT_ORDER, 0);
     if ( page == NULL )
         return -ENOMEM;
 
@@ -541,13 +639,15 @@ int p2m_alloc_table(struct domain *d)
     clear_page(p);
     unmap_domain_page(p);
 
+#ifdef CONFIG_ARM_32
     p = __map_domain_page(page + 1);
     clear_page(p);
     unmap_domain_page(p);
+#endif
 
-    p2m->first_level = page;
+    p2m->root_level = page;
 
-    d->arch.vttbr = page_to_maddr(p2m->first_level)
+    d->arch.vttbr = page_to_maddr(p2m->root_level)
         | ((uint64_t)p2m->vmid&0xff)<<48;
 
     p2m_load_VTTBR(d);
@@ -628,9 +728,9 @@ void p2m_teardown(struct domain *d)
     while ( (pg = page_list_remove_head(&p2m->pages)) )
         free_domheap_page(pg);
 
-    free_domheap_pages(p2m->first_level, P2M_FIRST_ORDER);
+    free_domheap_pages(p2m->root_level, P2M_ROOT_ORDER);
 
-    p2m->first_level = NULL;
+    p2m->root_level = NULL;
 
     p2m_free_vmid(d);
 
@@ -654,7 +754,7 @@ int p2m_init(struct domain *d)
 
     d->arch.vttbr = 0;
 
-    p2m->first_level = NULL;
+    p2m->root_level = NULL;
 
     p2m->max_mapped_gfn = 0;
     p2m->lowest_mapped_gfn = ULONG_MAX;
diff --git a/xen/include/asm-arm/p2m.h b/xen/include/asm-arm/p2m.h
index bd71abe..c33fc4d 100644
--- a/xen/include/asm-arm/p2m.h
+++ b/xen/include/asm-arm/p2m.h
@@ -13,8 +13,9 @@ struct p2m_domain {
     /* Pages used to construct the p2m */
     struct page_list_head pages;
 
-    /* Root of p2m page tables, 2 contiguous pages */
-    struct page_info *first_level;
+    /* ARMv7: Root of p2m page tables, 2 contiguous pages */
+    /* ARMv8: Look up table is zeroeth level */
+    struct page_info *root_level;
 
     /* Current VMID in use */
     uint8_t vmid;
diff --git a/xen/include/asm-arm/page.h b/xen/include/asm-arm/page.h
index c38e9c9..31c08b4 100644
--- a/xen/include/asm-arm/page.h
+++ b/xen/include/asm-arm/page.h
@@ -6,7 +6,11 @@
 #include <public/xen.h>
 #include <asm/processor.h>
 
+#ifdef CONFIG_ARM_64
+#define PADDR_BITS              48
+#else
 #define PADDR_BITS              40
+#endif
 #define PADDR_MASK              ((1ULL << PADDR_BITS)-1)
 
 #define VADDR_BITS              32
@@ -110,8 +114,8 @@ typedef struct __packed {
     unsigned long ng:1;         /* Not-Global */
 
     /* The base address must be appropriately aligned for Block entries */
-    unsigned long base:28;      /* Base address of block or next table */
-    unsigned long sbz:12;       /* Must be zero */
+    unsigned long base:36;      /* Base address of block or next table */
+    unsigned long sbz:4;       /* Must be zero */
 
     /* These seven bits are only used in Block entries and are ignored
      * in Table entries. */
@@ -145,8 +149,8 @@ typedef struct __packed {
     unsigned long sbz4:1;
 
     /* The base address must be appropriately aligned for Block entries */
-    unsigned long base:28;      /* Base address of block or next table */
-    unsigned long sbz3:12;
+    unsigned long base:36;      /* Base address of block or next table */
+    unsigned long sbz3:4;
 
     /* These seven bits are only used in Block entries and are ignored
      * in Table entries. */
@@ -170,9 +174,9 @@ typedef struct __packed {
     unsigned long pad2:10;
 
     /* The base address must be appropriately aligned for Block entries */
-    unsigned long base:28;      /* Base address of block or next table */
+    unsigned long base:36;      /* Base address of block or next table */
 
-    unsigned long pad1:24;
+    unsigned long pad1:16;
 } lpae_walk_t;
 
 typedef union {
diff --git a/xen/include/asm-arm/processor.h b/xen/include/asm-arm/processor.h
index 5978b8a..23c2f66 100644
--- a/xen/include/asm-arm/processor.h
+++ b/xen/include/asm-arm/processor.h
@@ -31,6 +31,95 @@
 #define MPIDR_AFFINITY_LEVEL(mpidr, level) \
          ((mpidr >> MPIDR_LEVEL_SHIFT(level)) & MPIDR_LEVEL_MASK)
 
+/* 
+ * VTCR register configuration for stage 2 translation
+ */
+#define VTCR_T0SZ_SHIFT   0
+#define VTCR_TOSZ_40BIT  (24 << VTCR_T0SZ_SHIFT)
+#define VTCR_TOSZ_48BIT  (16 << VTCR_T0SZ_SHIFT)
+
+#define VTCR_SL0_SHIFT    6
+#define VTCR_SL0_0       (0x2 << VTCR_SL0_SHIFT)
+#define VTCR_SL0_1       (0x1 << VTCR_SL0_SHIFT)
+#define VTCR_SL0_2       (0x0 << VTCR_SL0_SHIFT)
+
+#define VTCR_IRGN0_SHIFT  8
+#define VTCR_IRGN0_NC    (0x0 << VTCR_IRGN0_SHIFT)
+#define VTCR_IRGN0_WBWA  (0x1 << VTCR_IRGN0_SHIFT)
+#define VTCR_IRGN0_WT    (0x2 << VTCR_IRGN0_SHIFT)
+#define VTCR_IRGN0_WB    (0x3 << VTCR_IRGN0_SHIFT)
+
+#define VTCR_ORGN0_SHIFT  10
+#define VTCR_ORGN0_NC    (0x0 << VTCR_ORGN0_SHIFT)
+#define VTCR_ORGN0_WBWA  (0x1 << VTCR_ORGN0_SHIFT)
+#define VTCR_ORGN0_WT    (0x2 << VTCR_ORGN0_SHIFT)
+#define VTCR_ORGN0_WB    (0x3 << VTCR_ORGN0_SHIFT)
+
+#define VTCR_SH0_SHIFT    12
+#define VTCR_SH0_NS      (0x0 << VTCR_SH0_SHIFT)
+#define VTCR_SH0_OS      (0x2 << VTCR_SH0_SHIFT)
+#define VTCR_SH0_IS      (0x3 << VTCR_SH0_SHIFT)
+
+#define VTCR_TG0_SHIFT    14
+#define VTCR_TG0_4K      (0x0 << VTCR_TG0_SHIFT)
+#define VTCR_TG0_64K     (0x1 << VTCR_TG0_SHIFT)
+
+#define VTCR_PS_SHIFT     16
+#define VTCR_PS_32BIT    (0x0 << VTCR_PS_SHIFT)
+#define VTCR_PS_40BIT    (0x2 << VTCR_PS_SHIFT)
+#define VTCR_PS_48BIT    (0x5 << VTCR_PS_SHIFT)
+#define VTCR_PS_48BIT_VAL   0x5
+
+#ifdef CONFIG_ARM_64
+/*
+ * SL0=10 => Level-0 initial look up level
+ * SH0=11 => Inner-shareable
+ * ORGN0=IRGN0=01 => Normal memory, Write-Back Write-Allocate Cacheable
+ * TG0=00 => 4K page granular size
+ */
+#define VTCR_VAL_BASE  ((VTCR_SL0_0)      | \
+                        (VTCR_IRGN0_WBWA) | \
+                        (VTCR_ORGN0_WBWA) | \
+                        (VTCR_SH0_OS)     | \
+                        (VTCR_TG0_4K))
+#else
+/*
+ * T0SZ=(1)1000 => -8 (32-(-8) = 40 bit IPA)
+ * SL0=01 => Level-1 initial look up level
+ * SH0=11 => Inner-shareable
+ * ORGN0=IRGN0=01 => Normal memory, Write-Back Write-Allocate Cacheable
+ * TG0=00 => 4K page granular size
+ * PS=010 => 40 bits
+ * 40 bit IPA and 32 bit PA
+ */
+#define VTCR_VAL_BASE  ((VTCR_TOSZ_40BIT) | \
+                       (VTCR_SL0_1)      | \
+                       (VTCR_IRGN0_WBWA) | \
+                       (VTCR_ORGN0_WBWA) | \
+                       (VTCR_SH0_OS)     | \
+                       (VTCR_TG0_4K)     | \
+                       (VTCR_PS_32BIT))
+#endif
+
+/* TCR register configuration for Xen Stage 1 translation*/
+
+#define TCR_TBI_SHIFT       20
+#define TCR_TBI_USE_TBYTE  (0x0 << TCR_TBI_SHIFT)
+
+#ifdef CONFIG_ARM_64
+/* 
+ * 48 bit Hypervisor - VA  to 40 bit PA
+ * if platform supports 48 bit PA update runtime in head.S
+ */
+#define TCR_VAL_BASE   ((VTCR_TOSZ_48BIT)   | \
+                       (VTCR_IRGN0_WBWA)   | \
+                       (VTCR_ORGN0_WBWA)   | \
+                       (VTCR_SH0_OS)       | \
+                       (VTCR_TG0_4K)       | \
+                       (VTCR_PS_40BIT)     | \
+                       (TCR_TBI_USE_TBYTE))
+#endif
+
 /* TTBCR Translation Table Base Control Register */
 #define TTBCR_EAE    _AC(0x80000000,U)
 #define TTBCR_N_MASK _AC(0x07,U)
@@ -202,8 +291,19 @@ struct cpuinfo_arm {
         uint64_t bits[2];
     } aux64;
 
-    struct {
+    union {
         uint64_t bits[2];
+        struct {
+            unsigned long pa_range:4;
+            unsigned long asid_bits:4;
+            unsigned long bigend:4;
+            unsigned long secure_ns:4;
+            unsigned long bigend_el0:4;
+            unsigned long tgranule_16K:4;
+            unsigned long tgranule_64K:4;
+            unsigned long tgranule_4K:4;
+            unsigned long __res0:32;
+       };
     } mm64;
 
     struct {
-- 
1.7.9.5