[PATCH v3 3/3] arm/mpu: Implement p2m tables

[Luca Fancellu <luca.fancellu@xxxxxxx>]

From: Harry Ramsey <harry.ramsey@xxxxxxx>

Implement `p2m_alloc_table`, `p2m_init` and `p2m_final_teardown` for MPU
systems.

Signed-off-by: Harry Ramsey <harry.ramsey@xxxxxxx>
Signed-off-by: Luca Fancellu <luca.fancellu@xxxxxxx>
---
v3:
- Check for alloc_xenheap_pages allocation
- Clear additional page allocated for ARM64
- Add check for INVALID_VCPU_ID
- Remove unnecessary function generate_vsctlr
- Code formatting fixes
---
 xen/arch/arm/include/asm/arm32/mpu.h |  2 +
 xen/arch/arm/include/asm/arm64/mpu.h |  2 +
 xen/arch/arm/include/asm/mpu/p2m.h   | 12 +++++
 xen/arch/arm/include/asm/p2m.h       |  5 ++
 xen/arch/arm/mpu/p2m.c               | 78 ++++++++++++++++++++++++++--
 5 files changed, 96 insertions(+), 3 deletions(-)

diff --git a/xen/arch/arm/include/asm/arm32/mpu.h 
b/xen/arch/arm/include/asm/arm32/mpu.h
index 2cf0f8cbacae..d565230f84ee 100644
--- a/xen/arch/arm/include/asm/arm32/mpu.h
+++ b/xen/arch/arm/include/asm/arm32/mpu.h
@@ -11,6 +11,8 @@
  */
 #define MPU_REGION_RES0       0x0
 
+#define VSCTLR_VMID_SHIFT     16
+
 /* Hypervisor Protection Region Base Address Register */
 typedef union {
     struct {
diff --git a/xen/arch/arm/include/asm/arm64/mpu.h 
b/xen/arch/arm/include/asm/arm64/mpu.h
index 4f694190a8a3..8b86a03fee44 100644
--- a/xen/arch/arm/include/asm/arm64/mpu.h
+++ b/xen/arch/arm/include/asm/arm64/mpu.h
@@ -7,6 +7,8 @@
 
 #define MPU_REGION_RES0        (0xFFFFULL << 48)
 
+#define VSCTLR_VMID_SHIFT      48
+
 /* Protection Region Base Address Register */
 typedef union {
     struct __packed {
diff --git a/xen/arch/arm/include/asm/mpu/p2m.h 
b/xen/arch/arm/include/asm/mpu/p2m.h
index e46d9e757a1d..39fc0c944916 100644
--- a/xen/arch/arm/include/asm/mpu/p2m.h
+++ b/xen/arch/arm/include/asm/mpu/p2m.h
@@ -3,8 +3,20 @@
 #ifndef __ARM_MPU_P2M_H__
 #define __ARM_MPU_P2M_H__
 
+#include <xen/bitops.h>
+#include <xen/macros.h>
+#include <xen/page-size.h>
+
 struct p2m_domain;
 
+/*
+ * The architecture allows at most 255 EL2 MPU memory regions. The size of the
+ * MPU structure entry (pr_t) is 32 Bytes on AArch64 (requiring two 4KB pages)
+ * and 16 bytes on AArch32 (requiring one 4KB page).
+ */
+#define P2M_ROOT_PAGES DIV_ROUND_UP(255 * sizeof(pr_t), PAGE_SIZE)
+#define P2M_ROOT_ORDER get_count_order(P2M_ROOT_PAGES)
+
 static inline void p2m_clear_root_pages(struct p2m_domain *p2m) {}
 
 static inline void p2m_tlb_flush_sync(struct p2m_domain *p2m) {}
diff --git a/xen/arch/arm/include/asm/p2m.h b/xen/arch/arm/include/asm/p2m.h
index 010ce8c9ebbd..ed1b6dd40f40 100644
--- a/xen/arch/arm/include/asm/p2m.h
+++ b/xen/arch/arm/include/asm/p2m.h
@@ -48,8 +48,13 @@ struct p2m_domain {
     /* Current VMID in use */
     uint16_t vmid;
 
+#ifdef CONFIG_MMU
     /* Current Translation Table Base Register for the p2m */
     uint64_t vttbr;
+#else
+    /* Current Virtualization System Control Register for the p2m */
+    register_t vsctlr;
+#endif
 
     /* Highest guest frame that's ever been mapped in the p2m */
     gfn_t max_mapped_gfn;
diff --git a/xen/arch/arm/mpu/p2m.c b/xen/arch/arm/mpu/p2m.c
index f7fb58ab6aa8..ec8f630acd90 100644
--- a/xen/arch/arm/mpu/p2m.c
+++ b/xen/arch/arm/mpu/p2m.c
@@ -28,10 +28,69 @@ void p2m_dump_info(struct domain *d)
     BUG_ON("unimplemented");
 }
 
+static int p2m_alloc_table(struct domain *d)
+{
+    struct p2m_domain *p2m = p2m_get_hostp2m(d);
+    void *table = alloc_xenheap_pages(P2M_ROOT_ORDER, 0);
+
+    if ( !table )
+    {
+        printk(XENLOG_G_ERR "%pd: p2m: unable to allocate P2M MPU mapping 
table\n",
+               d);
+        return -ENOMEM;
+    }
+
+    p2m->root = virt_to_page(table);
+
+    for (int i = 0; i < P2M_ROOT_PAGES; i++)
+        clear_page(page_to_virt(p2m->root + i));
+
+    return 0;
+}
+
 int p2m_init(struct domain *d)
 {
-    BUG_ON("unimplemented");
-    return -EINVAL;
+    struct p2m_domain *p2m = p2m_get_hostp2m(d);
+    int rc = 0;
+    unsigned int cpu;
+
+    rwlock_init(&p2m->lock);
+
+    p2m->vmid = INVALID_VMID;
+    p2m->max_mapped_gfn = _gfn(0);
+    p2m->lowest_mapped_gfn = _gfn(ULONG_MAX);
+
+    p2m->default_access = p2m_access_rwx;
+    /* mem_access is NOT supported in MPU system. */
+    p2m->mem_access_enabled = false;
+
+    /* Ensure that the type chosen is large enough for MAX_VIRT_CPUS. */
+    BUILD_BUG_ON((1 << (sizeof(p2m->last_vcpu_ran[0]) * 8)) < MAX_VIRT_CPUS);
+    BUILD_BUG_ON((1 << (sizeof(p2m->last_vcpu_ran[0]) * 8)) < INVALID_VCPU_ID);
+
+    for_each_possible_cpu(cpu)
+        p2m->last_vcpu_ran[cpu] = INVALID_VCPU_ID;
+
+    /*
+     * "Trivial" initialization is now complete. Set the backpointer so that
+     * p2m_teardown() and related functions know to do something.
+     */
+    p2m->domain = d;
+
+    rc = p2m_alloc_vmid(d);
+    if ( rc )
+        return rc;
+
+    p2m->vsctlr = ((register_t)p2m->vmid << VSCTLR_VMID_SHIFT);
+
+    rc = p2m_alloc_table(d);
+    if ( rc )
+    {
+        p2m_free_vmid(d);
+        return rc;
+    }
+
+    return 0;
 }
 
 void p2m_save_state(struct vcpu *p)
@@ -46,7 +105,20 @@ void p2m_restore_state(struct vcpu *n)
 
 void p2m_final_teardown(struct domain *d)
 {
-    BUG_ON("unimplemented");
+    struct p2m_domain *p2m = p2m_get_hostp2m(d);
+
+    /* p2m not actually initialized */
+    if ( !p2m->domain )
+        return;
+
+    if ( p2m->root )
+        free_xenheap_pages(page_to_virt(p2m->root), P2M_ROOT_ORDER);
+
+    p2m->root = NULL;
+
+    p2m_free_vmid(d);
+
+    p2m->domain = NULL;
 }
 
 bool p2m_resolve_translation_fault(struct domain *d, gfn_t gfn)
-- 
2.34.1