x86/P2M: deal with partial success of p2m_set_entry()

M2P and PoD stats need to remain in sync with P2M; if an update succeeds
only partially, respective adjustments need to be made. If updates get
made before the call, they may also need undoing upon complete failure
(i.e. including the single-page case).

This is XSA-389.

Signed-off-by: Jan Beulich <jbeulich@suse.com>
Reviewed-by: Roger Pau Monné <roger.pau@citrix.com>

--- a/xen/arch/x86/mm/p2m.c
+++ b/xen/arch/x86/mm/p2m.c
@@ -546,8 +546,26 @@ p2m_remove_page(struct p2m_domain *p2m,
                 set_gpfn_from_mfn(mfn+i, INVALID_M2P_ENTRY);
         }
     }
-    return set_p2m_entry(p2m, gfn, _mfn(INVALID_MFN), page_order,
-                         p2m_invalid, p2m->default_access) ? 0 : -ENOMEM;
+
+    if ( set_p2m_entry(p2m, gfn, _mfn(INVALID_MFN), page_order, p2m_invalid,
+                       p2m->default_access) )
+        return 0;
+
+    if ( !mfn_valid(_mfn(mfn)) )
+        return -ENOMEM;
+
+    /*
+     * The operation may have partially succeeded. For the failed part we need
+     * to undo the M2P update.
+     */
+    for ( i = 0; i < (1UL << page_order); ++i )
+    {
+        p2m->get_entry(p2m, gfn + i, &t, &a, 0, NULL);
+        if ( !p2m_is_hole(t) && !p2m_is_grant(t) && !p2m_is_shared(t) )
+            set_gpfn_from_mfn(mfn + i, gfn + i);
+    }
+
+    return -ENOMEM;
 }
 
 int
@@ -707,11 +725,6 @@ guest_physmap_add_entry(struct domain *d
     /* Now, actually do the two-way mapping */
     if ( set_p2m_entry(p2m, gfn, _mfn(mfn), page_order, t, p2m->default_access) )
     {
-        pod_lock(p2m);
-        p2m->pod.entry_count -= pod_count;
-        BUG_ON(p2m->pod.entry_count < 0);
-        pod_unlock(p2m);
-
         if ( !p2m_is_grant(t) )
         {
             for ( i = 0; i < (1UL << page_order); i++ )
@@ -719,7 +732,40 @@ guest_physmap_add_entry(struct domain *d
         }
     }
     else
+    {
+        /*
+         * The operation may have partially succeeded. For the successful part
+         * we need to update the M2P, while for the failed part we may need to
+         * adjust PoD stats as well as undo the earlier M2P update.
+         */
+        for ( i = 0; i < (1UL << page_order); ++i )
+        {
+            omfn = p2m->get_entry(p2m, gfn + i, &ot, &a, 0, NULL);
+            if ( p2m_is_pod(ot) )
+            {
+                BUG_ON(!pod_count);
+                --pod_count;
+            }
+            else if ( mfn_x(omfn) == mfn + i && ot == t &&
+                      a == p2m->default_access && !p2m_is_grant(t) )
+                set_gpfn_from_mfn(mfn_x(omfn), gfn + i);
+            else if ( p2m_is_ram(ot) && !p2m_is_paged(ot) )
+            {
+                ASSERT(mfn_valid(omfn));
+                set_gpfn_from_mfn(mfn_x(omfn), gfn + i);
+            }
+        }
+
         rc = -EINVAL;
+    }
+
+    if ( pod_count )
+    {
+        pod_lock(p2m);
+        p2m->pod.entry_count -= pod_count;
+        BUG_ON(p2m->pod.entry_count < 0);
+        pod_unlock(p2m);
+    }
 
     p2m_unlock(p2m);
 
@@ -802,19 +848,27 @@ set_mmio_p2m_entry(struct domain *d, uns
         domain_crash(d);
         return 0;
     }
+
+    P2M_DEBUG("set mmio %lx %lx\n", gfn, mfn_x(mfn));
+    if ( !set_p2m_entry(p2m, gfn, mfn, PAGE_ORDER_4K, p2m_mmio_direct,
+                        p2m->default_access) )
+        gdprintk(XENLOG_ERR, "set_p2m_entry: %#lx failed (0x%"PRI_mfn")\n",
+                 gfn, mfn_x(mfn));
     else if ( p2m_is_ram(ot) )
     {
         ASSERT(mfn_valid(omfn));
         set_gpfn_from_mfn(mfn_x(omfn), INVALID_M2P_ENTRY);
     }
+    else if ( p2m_is_pod(ot) )
+    {
+        pod_lock(p2m);
+        p2m->pod.entry_count--;
+        BUG_ON(p2m->pod.entry_count < 0);
+        pod_unlock(p2m);
+    }
 
-    P2M_DEBUG("set mmio %lx %lx\n", gfn, mfn_x(mfn));
-    rc = set_p2m_entry(p2m, gfn, mfn, PAGE_ORDER_4K, p2m_mmio_direct, p2m->default_access);
     gfn_unlock(p2m, gfn, 0);
-    if ( 0 == rc )
-        gdprintk(XENLOG_ERR,
-            "set_mmio_p2m_entry: set_p2m_entry failed! mfn=%08lx\n",
-            mfn_x(get_gfn_query_unlocked(p2m->domain, gfn, &ot)));
+
     return rc;
 }
 
