[PATCH 11/38] arm: implement p2m lookup

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

Signed-off-by: Ian Campbell <ian.campbell@citrix.com>
---
 xen/arch/arm/p2m.c        |   71 ++++++++++++++++++++++++++++++++++++++++++--
 xen/include/asm-arm/p2m.h |    3 ++
 2 files changed, 70 insertions(+), 4 deletions(-)

diff --git a/xen/arch/arm/p2m.c b/xen/arch/arm/p2m.c
index 095e608..9b40e93 100644
--- a/xen/arch/arm/p2m.c
+++ b/xen/arch/arm/p2m.c
@@ -10,10 +10,20 @@ void dump_p2m_lookup(struct domain *d, paddr_t addr)
     struct p2m_domain *p2m = &d->arch.p2m;
     lpae_t *first = NULL, *second = NULL, *third = NULL;
 
-    printk("dom%d IPA %#016llx\n", d->domain_id, addr);
+    printk("dom%d IPA %#"PRIpaddr"\n", d->domain_id, addr);
+
+    printk("P2M @ %p mfn:%#lx (%#03llx,%#03llx,%#03llx)\n",
+           p2m->first_level,
+           page_to_mfn(p2m->first_level),
+           first_table_offset(addr),
+           second_table_offset(addr),
+           third_table_offset(addr));
+
+    if ( first_table_offset(addr) >= LPAE_ENTRIES )
+        goto done;
 
     first = __map_domain_page(p2m->first_level);
-    printk("1ST[%#03llx] = %#016llx\n",
+    printk("1ST[%#03llx] = %#"PRIpaddr"\n",
            first_table_offset(addr),
            first[first_table_offset(addr)].bits);
     if ( !first[first_table_offset(addr)].p2m.valid ||
@@ -21,7 +31,7 @@ void dump_p2m_lookup(struct domain *d, paddr_t addr)
         goto done;
 
     second = map_domain_page(first[first_table_offset(addr)].p2m.base);
-    printk("2ND[%#03llx] = %#016llx\n",
+    printk("2ND[%#03llx] = %#"PRIpaddr"\n",
            second_table_offset(addr),
            second[second_table_offset(addr)].bits);
     if ( !second[second_table_offset(addr)].p2m.valid ||
@@ -29,7 +39,7 @@ void dump_p2m_lookup(struct domain *d, paddr_t addr)
         goto done;
 
     third = map_domain_page(second[second_table_offset(addr)].p2m.base);
-    printk("3RD[%#03llx] = %#016llx\n",
+    printk("3RD[%#03llx] = %#"PRIpaddr"\n",
            third_table_offset(addr),
            third[third_table_offset(addr)].bits);
 
@@ -51,6 +61,59 @@ 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)
+{
+    struct p2m_domain *p2m = &d->arch.p2m;
+    lpae_t pte, *first = NULL, *second = NULL, *third = NULL;
+    paddr_t maddr = INVALID_PADDR;
+
+    spin_lock(&p2m->lock);
+
+    first = __map_domain_page(p2m->first_level);
+    if ( !first[first_table_offset(paddr)].p2m.valid )
+        goto done_err;
+    if ( !first[first_table_offset(paddr)].p2m.table )
+    {
+        pte = first[first_table_offset(paddr)];
+        goto done;
+    }
+
+    second = map_domain_page(first[first_table_offset(paddr)].p2m.base);
+    if ( !second[second_table_offset(paddr)].p2m.valid )
+        goto done_err;
+    if ( !second[second_table_offset(paddr)].p2m.table )
+    {
+        pte = second[second_table_offset(paddr)];
+        goto done;
+    }
+
+    third = map_domain_page(second[second_table_offset(paddr)].p2m.base);
+    if ( !third[third_table_offset(paddr)].p2m.valid )
+        goto done_err;
+    if ( !third[third_table_offset(paddr)].p2m.table )
+        goto done_err;
+
+    pte = third[third_table_offset(paddr)];
+
+done:
+
+    maddr = (pte.bits & PADDR_MASK & PAGE_MASK) | (paddr & ~PAGE_MASK);
+done_err:
+    if (third) unmap_domain_page(third);
+    if (second) unmap_domain_page(second);
+    if (first) unmap_domain_page(first);
+
+    spin_unlock(&p2m->lock);
+
+    return maddr;
+}
+
 int guest_physmap_mark_populate_on_demand(struct domain *d,
                                           unsigned long gfn,
                                           unsigned int order)
diff --git a/xen/include/asm-arm/p2m.h b/xen/include/asm-arm/p2m.h
index 349923a..1afd5cb 100644
--- a/xen/include/asm-arm/p2m.h
+++ b/xen/include/asm-arm/p2m.h
@@ -32,6 +32,9 @@ int p2m_alloc_table(struct domain *d);
 /* */
 void p2m_load_VTTBR(struct domain *d);
 
+/* */
+paddr_t p2m_lookup(struct domain *d, paddr_t gpfn);
+
 /* Setup p2m RAM mapping for domain d from start-end. */
 int p2m_populate_ram(struct domain *d, paddr_t start, paddr_t end);
 /* Map MMIO regions in the p2m: start_gaddr and end_gaddr is the range
-- 
1.7.9.1