diff --git a/xen/arch/arm/mm.c b/xen/arch/arm/mm.c
index a810a056d7..6dad167a8e 100644
--- a/xen/arch/arm/mm.c
+++ b/xen/arch/arm/mm.c
@@ -24,6 +24,7 @@
 #include <xen/mm.h>
 #include <xen/preempt.h>
 #include <xen/errno.h>
+#include <xen/sched.h>
 #include <xen/grant_table.h>
 #include <xen/softirq.h>
 #include <xen/event.h>
@@ -34,7 +35,6 @@
 #include <asm/current.h>
 #include <asm/flushtlb.h>
 #include <public/memory.h>
-#include <xen/sched.h>
 #include <xen/vmap.h>
 #include <xsm/xsm.h>
 #include <xen/pfn.h>
@@ -1229,37 +1229,11 @@ int xenmem_add_to_physmap_one(
     switch ( space )
     {
     case XENMAPSPACE_grant_table:
-        grant_write_lock(d->grant_table);
-
-        if ( d->grant_table->gt_version == 0 )
-            d->grant_table->gt_version = 1;
-
-        if ( d->grant_table->gt_version == 2 &&
-                (idx & XENMAPIDX_grant_table_status) )
-        {
-            idx &= ~XENMAPIDX_grant_table_status;
-            if ( idx < nr_status_frames(d->grant_table) )
-                mfn = virt_to_mfn(d->grant_table->status[idx]);
-            else
-                return -EINVAL;
-        }
-        else
-        {
-            if ( (idx >= nr_grant_frames(d->grant_table)) &&
-                 (idx < max_grant_frames) )
-                gnttab_grow_table(d, idx + 1);
-
-            if ( idx < nr_grant_frames(d->grant_table) )
-                mfn = virt_to_mfn(d->grant_table->shared_raw[idx]);
-            else
-                return -EINVAL;
-        }
-
-        d->arch.grant_table_gfn[idx] = gfn;
+        rc = gnttab_map_frame(d, idx, gfn, &mfn);
+        if ( rc )
+            return rc;
 
         t = p2m_ram_rw;
-
-        grant_write_unlock(d->grant_table);
         break;
     case XENMAPSPACE_shared_info:
         if ( idx != 0 )
diff --git a/xen/arch/x86/mm.c b/xen/arch/x86/mm.c
index ed77270586..e339987c01 100644
--- a/xen/arch/x86/mm.c
+++ b/xen/arch/x86/mm.c
@@ -4601,40 +4601,19 @@ int xenmem_add_to_physmap_one(
 {
     struct page_info *page = NULL;
     unsigned long gfn = 0; /* gcc ... */
-    unsigned long prev_mfn, mfn = 0, old_gpfn;
+    unsigned long prev_mfn, old_gpfn;
     int rc = 0;
+    mfn_t mfn = INVALID_MFN;
     p2m_type_t p2mt;
 
     switch ( space )
     {
         case XENMAPSPACE_shared_info:
             if ( idx == 0 )
-                mfn = virt_to_mfn(d->shared_info);
+                mfn = _mfn(virt_to_mfn(d->shared_info));
             break;
         case XENMAPSPACE_grant_table:
-            grant_write_lock(d->grant_table);
-
-            if ( d->grant_table->gt_version == 0 )
-                d->grant_table->gt_version = 1;
-
-            if ( d->grant_table->gt_version == 2 &&
-                 (idx & XENMAPIDX_grant_table_status) )
-            {
-                idx &= ~XENMAPIDX_grant_table_status;
-                if ( idx < nr_status_frames(d->grant_table) )
-                    mfn = virt_to_mfn(d->grant_table->status[idx]);
-            }
-            else
-            {
-                if ( (idx >= nr_grant_frames(d->grant_table)) &&
-                     (idx < max_grant_frames) )
-                    gnttab_grow_table(d, idx + 1);
-
-                if ( idx < nr_grant_frames(d->grant_table) )
-                    mfn = virt_to_mfn(d->grant_table->shared_raw[idx]);
-            }
-
-            grant_write_unlock(d->grant_table);
+            gnttab_map_frame(d, idx, gpfn, &mfn);
             break;
         case XENMAPSPACE_gmfn_range:
         case XENMAPSPACE_gmfn:
@@ -4651,8 +4630,8 @@ int xenmem_add_to_physmap_one(
             }
             if ( !get_page_from_mfn(_mfn(idx), d) )
                 break;
-            mfn = idx;
-            page = mfn_to_page(_mfn(mfn));
+            mfn = _mfn(idx);
+            page = mfn_to_page(mfn);
             break;
         }
         case XENMAPSPACE_gmfn_foreign:
@@ -4661,7 +4640,7 @@ int xenmem_add_to_physmap_one(
             break;
     }
 
-    if ( !paging_mode_translate(d) || (mfn == 0) )
+    if ( !paging_mode_translate(d) || mfn_eq(mfn, INVALID_MFN) )
     {
         rc = -EINVAL;
         goto put_both;
@@ -4685,16 +4664,16 @@ int xenmem_add_to_physmap_one(
         goto put_both;
 
     /* Unmap from old location, if any. */
-    old_gpfn = get_gpfn_from_mfn(mfn);
+    old_gpfn = get_gpfn_from_mfn(mfn_x(mfn));
     ASSERT( old_gpfn != SHARED_M2P_ENTRY );
     if ( space == XENMAPSPACE_gmfn || space == XENMAPSPACE_gmfn_range )
         ASSERT( old_gpfn == gfn );
     if ( old_gpfn != INVALID_M2P_ENTRY )
-        rc = guest_physmap_remove_page(d, _gfn(old_gpfn), _mfn(mfn), PAGE_ORDER_4K);
+        rc = guest_physmap_remove_page(d, _gfn(old_gpfn), mfn, PAGE_ORDER_4K);
 
     /* Map at new location. */
     if ( !rc )
-        rc = guest_physmap_add_page(d, gpfn, _mfn(mfn), PAGE_ORDER_4K);
+        rc = guest_physmap_add_page(d, gpfn, mfn, PAGE_ORDER_4K);
 
  put_both:
     /* In the XENMAPSPACE_gmfn, we took a ref of the gfn at the top */
diff --git a/xen/common/grant_table.c b/xen/common/grant_table.c
index fb3859ce8e..9e18dc0493 100644
--- a/xen/common/grant_table.c
+++ b/xen/common/grant_table.c
@@ -3607,6 +3607,44 @@ int mem_sharing_gref_to_gfn(struct grant_table *gt, grant_ref_t ref,
 }
 #endif
 
+int gnttab_map_frame(struct domain *d, unsigned long idx, gfn_t gfn,
+                     mfn_t *mfn)
+{
+    int rc = 0;
+
+    grant_write_lock(d->grant_table);
+
+    if ( d->grant_table->gt_version == 0 )
+        d->grant_table->gt_version = 1;
+
+    if ( d->grant_table->gt_version == 2 &&
+         (idx & XENMAPIDX_grant_table_status) )
+    {
+        idx &= ~XENMAPIDX_grant_table_status;
+        if ( idx < nr_status_frames(d->grant_table) )
+            *mfn = _mfn(virt_to_mfn(d->grant_table->status[idx]));
+        else
+            rc = -EINVAL;
+    }
+    else
+    {
+        if ( (idx >= nr_grant_frames(d->grant_table)) &&
+             (idx < max_grant_frames) )
+            gnttab_grow_table(d, idx + 1);
+
+        if ( idx < nr_grant_frames(d->grant_table) )
+            *mfn = _mfn(virt_to_mfn(d->grant_table->shared_raw[idx]));
+        else
+            rc = -EINVAL;
+    }
+
+    gnttab_set_frame_gfn(d, idx, gfn);
+
+    grant_write_unlock(d->grant_table);
+
+    return rc;
+}
+
 static void gnttab_usage_print(struct domain *rd)
 {
     int first = 1;
diff --git a/xen/include/asm-arm/grant_table.h b/xen/include/asm-arm/grant_table.h
index bc4d61a940..581c4bf928 100644
--- a/xen/include/asm-arm/grant_table.h
+++ b/xen/include/asm-arm/grant_table.h
@@ -21,6 +21,12 @@ static inline int replace_grant_supported(void)
     return 1;
 }
 
+static inline void gnttab_set_frame_gfn(struct domain *d, unsigned long idx,
+                                        gfn_t gfn)
+{
+    d->arch.grant_table_gfn[idx] = gfn;
+}
+
 #define gnttab_create_shared_page(d, t, i)                               \
     do {                                                                 \
         share_xen_page_with_guest(                                       \
diff --git a/xen/include/asm-x86/grant_table.h b/xen/include/asm-x86/grant_table.h
index 33b2f88b96..c865999a33 100644
--- a/xen/include/asm-x86/grant_table.h
+++ b/xen/include/asm-x86/grant_table.h
@@ -75,6 +75,11 @@ static inline void gnttab_clear_flag(unsigned int nr, uint16_t *st)
     asm volatile ("lock btrw %w1,%0" : "=m" (*st) : "Ir" (nr), "m" (*st));
 }
 
+static inline void gnttab_set_frame_gfn(struct domain *d, unsigned long idx,
+                                        gfn_t gfn)
+{
+}
+
 /* Foreign mappings of HHVM-guest pages do not modify the type count. */
 #define gnttab_host_mapping_get_page_type(ro, ld, rd)   \
     (!(ro) && (((ld) == (rd)) || !paging_mode_external(rd)))
diff --git a/xen/include/xen/grant_table.h b/xen/include/xen/grant_table.h
index af269a108d..43ec6c4d80 100644
--- a/xen/include/xen/grant_table.h
+++ b/xen/include/xen/grant_table.h
@@ -136,4 +136,7 @@ static inline unsigned int grant_to_status_frames(int grant_frames)
 int mem_sharing_gref_to_gfn(struct grant_table *gt, grant_ref_t ref,
                             gfn_t *gfn, uint16_t *status);
 
+int gnttab_map_frame(struct domain *d, unsigned long idx, gfn_t gfn,
+                     mfn_t *mfn);
+
 #endif /* __XEN_GRANT_TABLE_H__ */
