[PATCH 4/8] arm/mpu: Support vCPU context switch on MPU systems

[Luca Fancellu <luca.fancellu@xxxxxxx>]

From: Penny Zheng <Penny.Zheng@xxxxxxx>

Implement the functions p2m_save_state and p2m_restore_state for MPU
systems. Unlike on MMU systems, where we simply update VTTBR_EL2 with
the incoming guest's p2m table on context switch, we have to disable the
outgoing guest's p2m memory regions and enable the incoming guest's p2m
memory regions.

Signed-off-by: Penny Zheng <penny.zheng@xxxxxxx>
Signed-off-by: Wei Chen <wei.chen@xxxxxxx>
Signed-off-by: Luca Fancellu <luca.fancellu@xxxxxxx>
Signed-off-by: Hari Limaye <hari.limaye@xxxxxxx>
Signed-off-by: Harry Ramsey <harry.ramsey@xxxxxxx>
---
 xen/arch/arm/include/asm/mpu/cpregs.h |  4 +++
 xen/arch/arm/mpu/mm.c                 | 11 +++++--
 xen/arch/arm/mpu/p2m.c                | 47 +++++++++++++++++++++++++--
 3 files changed, 57 insertions(+), 5 deletions(-)

diff --git a/xen/arch/arm/include/asm/mpu/cpregs.h 
b/xen/arch/arm/include/asm/mpu/cpregs.h
index 9f3b32acd79f..5a3d92cf5389 100644
--- a/xen/arch/arm/include/asm/mpu/cpregs.h
+++ b/xen/arch/arm/include/asm/mpu/cpregs.h
@@ -6,6 +6,9 @@
 /* CP15 CR0: MPU Type Register */
 #define HMPUIR          p15,4,c0,c0,4
 
+/* CP15 CR2: Virtualization System Control register */
+#define VSCTLR          p15,4,c2,c0,1
+
 /* CP15 CR6: Protection Region Enable Register */
 #define HPRENR          p15,4,c6,c1,1
 
@@ -88,6 +91,7 @@
 #define PRENR_EL2       HPRENR
 #define PRLAR_EL2       HPRLAR
 #define PRSELR_EL2      HPRSELR
+#define VSCTLR_EL2      VSCTLR
 #endif /* CONFIG_ARM_32 */
 
 #endif /* __ARM_MPU_CPREGS_H */
diff --git a/xen/arch/arm/mpu/mm.c b/xen/arch/arm/mpu/mm.c
index 4ee58ded5ad6..5ed77355a5f9 100644
--- a/xen/arch/arm/mpu/mm.c
+++ b/xen/arch/arm/mpu/mm.c
@@ -421,9 +421,14 @@ static int xen_mpumap_update_entry(paddr_t base, paddr_t 
limit,
     return 0;
 }
 
-int check_mpu_mapping(paddr_t base, paddr_t limit, unsigned int flags)
+static bool check_mpu_mapping(paddr_t base, paddr_t limit, unsigned int flags,
+                              bool p2m)
 {
-    if ( flags_has_rwx(flags) )
+    /*
+     * Mappings should not be both Writeable and Executable, unless
+     * it is for guest P2M mapping.
+     */
+    if ( flags_has_rwx(flags) && !p2m )
     {
         printk("Mappings should not be both Writeable and Executable\n");
         return false;
@@ -450,7 +455,7 @@ int xen_mpumap_update(paddr_t base, paddr_t limit, unsigned 
int flags, bool p2m)
 {
     int rc;
 
-    if ( !check_mpu_mapping(base, limit, flags) )
+    if ( !check_mpu_mapping(base, limit, flags, p2m) )
         return -EINVAL;
 
     spin_lock(&xen_mpumap_lock);
diff --git a/xen/arch/arm/mpu/p2m.c b/xen/arch/arm/mpu/p2m.c
index f2482237412b..bf87c65c106c 100644
--- a/xen/arch/arm/mpu/p2m.c
+++ b/xen/arch/arm/mpu/p2m.c
@@ -285,14 +285,57 @@ int p2m_init(struct domain *d)
     return 0;
 }
 
+static int p2m_xenmpu_update(struct p2m_domain *p2m, bool online)
+{
+    pr_t *p2m_table;
+    unsigned int flags = online ? _PAGE_PRESENT : 0;
+
+    p2m_table = (pr_t *)page_to_virt(p2m->root);
+    if ( !p2m_table )
+        return -EINVAL;
+
+    for ( unsigned int i = 0; i < p2m->nr_regions; i++ )
+    {
+        paddr_t base = pr_get_base(&p2m_table[i]);
+        paddr_t limit = pr_get_limit(&p2m_table[i]) + 1;
+        unsigned int region_flags;
+
+        region_flags = build_p2m_flags(region_get_p2m(&p2m_table[i])) | flags;
+        if ( xen_mpumap_update(base, limit, region_flags, true) )
+        {
+            printk(XENLOG_G_ERR "Unable to update MPU memory mapping with P2M 
region %#"PRIpaddr"-%#"PRIpaddr"\n",
+                   base, limit);
+            return -EINVAL;
+        }
+    }
+
+    return 0;
+}
+
+/* p2m_save_state and p2m_restore_state work in pair. */
 void p2m_save_state(struct vcpu *p)
 {
-    BUG_ON("unimplemented");
+    struct p2m_domain *p2m = p2m_get_hostp2m(p->domain);
+
+    p->arch.sctlr = READ_SYSREG(SCTLR_EL1);
+
+    if ( p2m_xenmpu_update(p2m, false) )
+        panic("Failed to offline P2M MPU memory mapping\n");
 }
 
 void p2m_restore_state(struct vcpu *n)
 {
-    BUG_ON("unimplemented");
+    struct p2m_domain *p2m = p2m_get_hostp2m(n->domain);
+    uint8_t *last_vcpu_ran = &p2m->last_vcpu_ran[smp_processor_id()];
+
+    WRITE_SYSREG(n->arch.sctlr, SCTLR_EL1);
+    WRITE_SYSREG(n->arch.hcr_el2, HCR_EL2);
+
+    WRITE_SYSREG(p2m->vsctlr, VSCTLR_EL2);
+    if ( p2m_xenmpu_update(p2m, true) )
+        panic("Failed to online P2M MPU memory mapping\n");
+
+    *last_vcpu_ran = n->vcpu_id;
 }
 
 void p2m_final_teardown(struct domain *d)
-- 
2.34.1