[PATCH RFC 7/9] xen: arm: handle variable p2m levels in apply_p2m_changes

[Ian Campbell <ian.campbell@citrix.com>]

As with prervious changes this involves conversion from a linear series of
lookups into a loop over the levels.

Signed-off-by: Ian Campbell <ian.campbell@citrix.com>
---
 xen/arch/arm/p2m.c |  178 +++++++++++++++++++++++++---------------------------
 1 file changed, 84 insertions(+), 94 deletions(-)

diff --git a/xen/arch/arm/p2m.c b/xen/arch/arm/p2m.c
index 557663f..e9938ae 100644
--- a/xen/arch/arm/p2m.c
+++ b/xen/arch/arm/p2m.c
@@ -15,7 +15,6 @@
 
 /* First level P2M is 2 consecutive pages */
 #define P2M_ROOT_ORDER 1
-#define P2M_ROOT_ENTRIES (LPAE_ENTRIES<<P2M_ROOT_ORDER)
 #define P2M_ROOT_PAGES    (1<<P2M_ROOT_ORDER)
 
 static bool_t p2m_valid(lpae_t pte)
@@ -115,31 +114,6 @@ void flush_tlb_domain(struct domain *d)
         p2m_load_VTTBR(current->domain);
 }
 
-static int p2m_first_level_index(paddr_t addr)
-{
-    /*
-     * 1st pages are concatenated so zeroeth offset gives us the
-     * index of the 1st page
-     */
-    return zeroeth_table_offset(addr);
-}
-
-/*
- * Map whichever of the first pages contain addr. The caller should
- * then use first_table_offset as an index.
- */
-static lpae_t *p2m_map_first(struct p2m_domain *p2m, paddr_t addr)
-{
-    struct page_info *page;
-
-    if ( first_linear_offset(addr) >= P2M_ROOT_ENTRIES )
-        return NULL;
-
-    page = p2m->root + p2m_first_level_index(addr);
-
-    return __map_domain_page(page);
-}
-
 /*
  * Lookup the MFN corresponding to a domain's PFN.
  *
@@ -707,14 +681,13 @@ static int apply_p2m_changes(struct domain *d,
                      int mattr,
                      p2m_type_t t)
 {
-    int rc, ret;
+    int level, rc, ret;
     struct p2m_domain *p2m = &d->arch.p2m;
-    lpae_t *first = NULL, *second = NULL, *third = NULL;
+    lpae_t *mappings[4] = { NULL, };
     paddr_t addr;
-    unsigned long cur_first_page = ~0,
-                  cur_first_offset = ~0,
-                  cur_second_offset = ~0;
-    unsigned long count = 0;
+    unsigned int cur_root_table = ~0;
+    unsigned int cur_offset[4] = { ~0, };
+    unsigned int count = 0;
     bool_t flush = false;
     bool_t flush_pt;
 
@@ -726,9 +699,27 @@ static int apply_p2m_changes(struct domain *d,
 
     spin_lock(&p2m->lock);
 
+    if ( P2M_ROOT_PAGES == 1 )
+    {
+        /* Static mapping. P2M_ROOT_PAGES > 1 are handled below */
+        mappings[P2M_ROOT_LEVEL] = __map_domain_page(p2m->root);
+        {
+            rc = -EINVAL;
+            goto out;
+        }
+    }
+
     addr = start_gpaddr;
     while ( addr < end_gpaddr )
     {
+        int root_table;
+        const unsigned int offsets[4] = {
+            zeroeth_table_offset(addr),
+            first_table_offset(addr),
+            second_table_offset(addr),
+            third_table_offset(addr)
+        };
+
         /*
          * Arbitrarily, preempt every 512 operations or 8192 nops.
          * 512*P2M_ONE_PROGRESS == 8192*P2M_ONE_PROGRESS_NOP == 0x2000
@@ -748,73 +739,71 @@ static int apply_p2m_changes(struct domain *d,
             count = 0;
         }
 
-        if ( cur_first_page != p2m_first_level_index(addr) )
+        if ( P2M_ROOT_PAGES > 1 )
         {
-            if ( first ) unmap_domain_page(first);
-            first = p2m_map_first(p2m, addr);
-            if ( !first )
+            int i;
+            /*
+             * Concatenated root-level tables. The table number will be the
+             * offset at the previous level. It is not possible to concetenate
+             * a level-0 root.
+             */
+            BUG_ON(P2M_ROOT_LEVEL == 0);
+            root_table = offsets[P2M_ROOT_LEVEL - 1];
+            if ( root_table >= P2M_ROOT_PAGES )
             {
                 rc = -EINVAL;
                 goto out;
             }
-            cur_first_page = p2m_first_level_index(addr);
-            /* Any mapping further down is now invalid */
-            cur_first_offset = cur_second_offset = ~0;
-        }
-
-        /* We only use a 3 level p2m at the moment, so no level 0,
-         * current hardware doesn't support super page mappings at
-         * level 0 anyway */
-
-        ret = apply_one_level(d, &first[first_table_offset(addr)],
-                              1, flush_pt, op,
-                              start_gpaddr, end_gpaddr,
-                              &addr, &maddr, &flush,
-                              mattr, t);
-        if ( ret < 0 ) { rc = ret ; goto out; }
-        count += ret;
-        if ( ret != P2M_ONE_DESCEND ) continue;
-
-        BUG_ON(!p2m_valid(first[first_table_offset(addr)]));
-
-        if ( cur_first_offset != first_table_offset(addr) )
-        {
-            if (second) unmap_domain_page(second);
-            second = map_domain_page(first[first_table_offset(addr)].p2m.base);
-            cur_first_offset = first_table_offset(addr);
-            /* Any mapping further down is now invalid */
-            cur_second_offset = ~0;
+            if ( cur_root_table != root_table )
+            {
+                if ( mappings[P2M_ROOT_LEVEL] )
+                    unmap_domain_page(mappings[P2M_ROOT_LEVEL]);
+                mappings[P2M_ROOT_LEVEL] =
+                    __map_domain_page(p2m->root + root_table);
+                if ( !mappings[P2M_ROOT_LEVEL] )
+                {
+                    rc = -EINVAL;
+                    goto out;
+                }
+                cur_root_table = root_table;
+                /* Any mapping further down is now invalid */
+                for ( i = P2M_ROOT_LEVEL; i < 4; i++ )
+                    cur_offset[i] = ~0;
+            }
         }
-        /* else: second already valid */
-
-        ret = apply_one_level(d,&second[second_table_offset(addr)],
-                              2, flush_pt, op,
-                              start_gpaddr, end_gpaddr,
-                              &addr, &maddr, &flush,
-                              mattr, t);
-        if ( ret < 0 ) { rc = ret ; goto out; }
-        count += ret;
-        if ( ret != P2M_ONE_DESCEND ) continue;
-
-        BUG_ON(!p2m_valid(second[second_table_offset(addr)]));
 
-        if ( cur_second_offset != second_table_offset(addr) )
+        for ( level = P2M_ROOT_LEVEL; level < 4; level++ )
         {
-            /* map third level */
-            if (third) unmap_domain_page(third);
-            third = map_domain_page(second[second_table_offset(addr)].p2m.base);
-            cur_second_offset = second_table_offset(addr);
+            unsigned offset = offsets[level];
+            lpae_t *entry = &mappings[level][offset];
+
+            ret = apply_one_level(d, entry,
+                                  level, flush_pt, op,
+                                  start_gpaddr, end_gpaddr,
+                                  &addr, &maddr, &flush,
+                                  mattr, t);
+            if ( ret < 0 ) { rc = ret ; goto out; }
+            count += ret;
+            /* L3 had better have done something! We cannot descend any further */
+            BUG_ON(level == 3 && ret == P2M_ONE_DESCEND);
+            if ( ret != P2M_ONE_DESCEND ) break;
+
+            BUG_ON(!p2m_valid(*entry));
+
+            if ( cur_offset[level] != offset )
+            {
+                /* Update mapping for next level */
+                int i;
+                if ( mappings[level+1] )
+                    unmap_domain_page(mappings[level+1]);
+                mappings[level+1] = map_domain_page(entry->p2m.base);
+                cur_offset[level] = offset;
+                /* Any mapping further down is now invalid */
+                for ( i = level+1; i < 4; i++ )
+                    cur_offset[i] = ~0;
+            }
+            /* else: next level already valid */
         }
-
-        ret = apply_one_level(d, &third[third_table_offset(addr)],
-                              3, flush_pt, op,
-                              start_gpaddr, end_gpaddr,
-                              &addr, &maddr, &flush,
-                              mattr, t);
-        if ( ret < 0 ) { rc = ret ; goto out; }
-        /* L3 had better have done something! We cannot descend any further */
-        BUG_ON(ret == P2M_ONE_DESCEND);
-        count += ret;
     }
 
     if ( flush )
@@ -838,10 +827,11 @@ static int apply_p2m_changes(struct domain *d,
     rc = 0;
 
 out:
-    if (third) unmap_domain_page(third);
-    if (second) unmap_domain_page(second);
-    if (first) unmap_domain_page(first);
-
+    for ( level = P2M_ROOT_LEVEL; level < 4; level ++ )
+    {
+        if ( mappings[level] )
+            unmap_domain_page(mappings[level]);
+    }
     spin_unlock(&p2m->lock);
 
     return rc;
-- 
1.7.10.4