[PATCH v4 2/4] xen/arm: introduce a generic p2m walker and use it in p2m_lookup

[Stefano Stabellini <stefano.stabellini@eu.citrix.com>]

Signed-off-by: Stefano Stabellini <stefano.stabellini@eu.citrix.com>
---
 xen/arch/arm/p2m.c |  104 +++++++++++++++++++++++++++++++++++++++-------------
 1 files changed, 78 insertions(+), 26 deletions(-)

diff --git a/xen/arch/arm/p2m.c b/xen/arch/arm/p2m.c
index 307c6d4..a77df9a 100644
--- a/xen/arch/arm/p2m.c
+++ b/xen/arch/arm/p2m.c
@@ -30,49 +30,101 @@ void p2m_load_VTTBR(struct domain *d)
     isb(); /* Ensure update is visible */
 }
 
-/*
- * Lookup the MFN corresponding to a domain's PFN.
- *
- * There are no processor functions to do a stage 2 only lookup therefore we
- * do a a software walk.
- */
-paddr_t p2m_lookup(struct domain *d, paddr_t paddr)
+static int p2m_walker(struct domain *d, paddr_t paddr, unsigned int order,
+                      int (*func)(lpae_t *pte, void *arg), void* arg)
 {
+    lpae_t *first = NULL, *second = NULL, *third = NULL;
     struct p2m_domain *p2m = &d->arch.p2m;
-    lpae_t pte, *first = NULL, *second = NULL, *third = NULL;
-    paddr_t maddr = INVALID_PADDR;
+    int rc = -EFAULT, i;
+    unsigned long cur_first_offset = ~0, cur_second_offset = ~0;
 
     spin_lock(&p2m->lock);
 
     first = __map_domain_page(p2m->first_level);
 
-    pte = first[first_table_offset(paddr)];
-    if ( !pte.p2m.valid || !pte.p2m.table )
-        goto done;
+    if ( !first ||
+            !first[first_table_offset(paddr)].p2m.valid ||
+            !first[first_table_offset(paddr)].p2m.table )
+        goto err;
+
+    for ( i = 0; i < (1UL << order); i++ )
+    {
+        if ( cur_first_offset != first_table_offset(paddr) )
+        {
+            if (second) unmap_domain_page(second);
+            second = map_domain_page(first[first_table_offset(paddr)].p2m.base);
+            cur_first_offset = first_table_offset(paddr);
+        }
+        if ( !second ||
+                !second[second_table_offset(paddr)].p2m.valid ||
+                !second[second_table_offset(paddr)].p2m.table )
+            goto err;
+
+        if ( cur_second_offset != second_table_offset(paddr) )
+        {
+            if (third) unmap_domain_page(third);
+            third = map_domain_page(second[second_table_offset(paddr)].p2m.base);
+            cur_second_offset = second_table_offset(paddr);
+        }
+        if ( !third ||
+                !third[third_table_offset(paddr)].p2m.valid )
+            goto err;
+
+        rc = func(&third[third_table_offset(paddr)], arg);
+        if ( rc != 0 )
+            return rc;
+
+        paddr += PAGE_SIZE;
+    }
+
+    rc = 0;
+
+err:
+    if ( third ) unmap_domain_page(third);
+    if ( second ) unmap_domain_page(second);
+    if ( first ) unmap_domain_page(first);
 
-    second = map_domain_page(pte.p2m.base);
-    pte = second[second_table_offset(paddr)];
-    if ( !pte.p2m.valid || !pte.p2m.table )
-        goto done;
+    spin_unlock(&p2m->lock);
+
+    return rc;
+}
+
+struct p2m_lookup_t {
+    paddr_t paddr;
+    paddr_t maddr;
+};
 
-    third = map_domain_page(pte.p2m.base);
-    pte = third[third_table_offset(paddr)];
+static int _p2m_lookup(lpae_t *ptep, void *arg)
+{
+    lpae_t pte;
+    struct p2m_lookup_t *p2m = (struct p2m_lookup_t *)arg;
+
+    pte = *ptep;
 
     /* This bit must be one in the level 3 entry */
     if ( !pte.p2m.table )
         pte.bits = 0;
 
-done:
     if ( pte.p2m.valid )
-        maddr = (pte.bits & PADDR_MASK & PAGE_MASK) | (paddr & ~PAGE_MASK);
-
-    if (third) unmap_domain_page(third);
-    if (second) unmap_domain_page(second);
-    if (first) unmap_domain_page(first);
+        p2m->maddr = (pte.bits & PADDR_MASK & PAGE_MASK) |
+            (p2m->paddr & ~PAGE_MASK);
+    return 0;
+}
+/*
+ * Lookup the MFN corresponding to a domain's PFN.
+ *
+ * There are no processor functions to do a stage 2 only lookup therefore we
+ * do a a software walk.
+ */
+paddr_t p2m_lookup(struct domain *d, paddr_t paddr)
+{
+    struct p2m_lookup_t p2m;
+    p2m.paddr = paddr;
+    p2m.maddr = INVALID_PADDR;
 
-    spin_unlock(&p2m->lock);
+    p2m_walker(d, paddr, 0, _p2m_lookup, &p2m);
 
-    return maddr;
+    return p2m.maddr;
 }
 
 int guest_physmap_mark_populate_on_demand(struct domain *d,
-- 
1.7.2.5