diff --git a/xen/common/grant_table.c b/xen/common/grant_table.c
index 464007e..67cbed9 100644
--- a/xen/common/grant_table.c
+++ b/xen/common/grant_table.c
@@ -727,7 +727,7 @@ __gnttab_map_grant_ref(
 
     double_gt_lock(lgt, rgt);
 
-    if ( gnttab_need_iommu_mapping(ld) )
+    if ( gnttab_need_iommu_mapping(ld) || gnttab_need_identity_mapping(ld) )
     {
         unsigned int wrc, rdc;
         int err = 0;
@@ -738,13 +738,23 @@ __gnttab_map_grant_ref(
              !(old_pin & (GNTPIN_hstw_mask|GNTPIN_devw_mask)) )
         {
             if ( wrc == 0 )
-                err = iommu_map_page(ld, frame, frame,
-                                     IOMMUF_readable|IOMMUF_writable);
+            {
+                if ( gnttab_need_identity_mapping(ld) )
+                    err = arch_grant_map_page_identity(ld, frame, 1);
+                else
+                    err = iommu_map_page(ld, frame, frame,
+                            IOMMUF_readable|IOMMUF_writable);
+            }
         }
         else if ( act_pin && !old_pin )
         {
             if ( (wrc + rdc) == 0 )
-                err = iommu_map_page(ld, frame, frame, IOMMUF_readable);
+            {
+                if ( gnttab_need_identity_mapping(ld) )
+                    err = arch_grant_map_page_identity(ld, frame, 0);
+                else
+                    err = iommu_map_page(ld, frame, frame, IOMMUF_readable);
+            }
         }
         if ( err )
         {
@@ -935,15 +945,24 @@ __gnttab_unmap_common(
             act->pin -= GNTPIN_hstw_inc;
     }
 
-    if ( gnttab_need_iommu_mapping(ld) )
+    if ( gnttab_need_iommu_mapping(ld) || gnttab_need_identity_mapping(ld) )
     {
         unsigned int wrc, rdc;
         int err = 0;
         mapcount(lgt, rd, op->frame, &wrc, &rdc);
         if ( (wrc + rdc) == 0 )
-            err = iommu_unmap_page(ld, op->frame);
-        else if ( wrc == 0 )
-            err = iommu_map_page(ld, op->frame, op->frame, IOMMUF_readable);
+        {
+            if ( gnttab_need_identity_mapping(ld) )
+                err = arch_grant_unmap_page_identity(ld, op->frame);
+            else
+                err = iommu_unmap_page(ld, op->frame);
+        } else if ( wrc == 0 )
+        {
+            if ( gnttab_need_identity_mapping(ld) )
+                err = arch_grant_map_page_identity(ld, op->frame, 0);
+            else
+                err = iommu_map_page(ld, op->frame, op->frame, IOMMUF_readable);
+        }
         if ( err )
         {
             rc = GNTST_general_error;
diff --git a/xen/common/kernel.c b/xen/common/kernel.c
index 7e83353..dacbe38 100644
--- a/xen/common/kernel.c
+++ b/xen/common/kernel.c
@@ -15,6 +15,7 @@
 #include <xen/guest_access.h>
 #include <xen/hypercall.h>
 #include <asm/current.h>
+#include <asm/grant_table.h>
 #include <public/nmi.h>
 #include <public/version.h>
 
@@ -325,6 +326,8 @@ DO(xen_version)(int cmd, XEN_GUEST_HANDLE_PARAM(void) arg)
                 break;
             }
 #endif
+            if ( gnttab_need_identity_mapping(d) )
+                fi.submap |= 1U << XENFEAT_grant_map_identity;
             break;
         default:
             return -EINVAL;
diff --git a/xen/include/asm-arm/grant_table.h b/xen/include/asm-arm/grant_table.h
index eac8a70..6f7ccd9 100644
--- a/xen/include/asm-arm/grant_table.h
+++ b/xen/include/asm-arm/grant_table.h
@@ -36,6 +36,9 @@ static inline int replace_grant_supported(void)
 #define gnttab_need_iommu_mapping(d)                    \
     (is_domain_direct_mapped(d) && need_iommu(d))
 
+#define gnttab_need_identity_mapping(d)                    \
+    (is_domain_direct_mapped(d) && !need_iommu(d))
+
 #endif /* __ASM_GRANT_TABLE_H__ */
 /*
  * Local variables:
diff --git a/xen/include/asm-x86/grant_table.h b/xen/include/asm-x86/grant_table.h
index 8c9bbcf..56861a7 100644
--- a/xen/include/asm-x86/grant_table.h
+++ b/xen/include/asm-x86/grant_table.h
@@ -68,6 +68,8 @@ static inline void gnttab_clear_flag(unsigned int nr, uint16_t *st)
 #define gnttab_need_iommu_mapping(d)                \
     (!paging_mode_translate(d) && need_iommu(d))
 
+#define gnttab_need_identity_mapping(d)     (0)
+
 static inline int replace_grant_supported(void)
 {
     return 1;
diff --git a/xen/include/public/features.h b/xen/include/public/features.h
index a149aa6..ba753a0 100644
--- a/xen/include/public/features.h
+++ b/xen/include/public/features.h
@@ -94,6 +94,9 @@
 /* operation as Dom0 is supported */
 #define XENFEAT_dom0                      11
 
+/* Xen also maps grant references at pfn = mfn */
+#define XENFEAT_grant_map_identity              12
+
 #define XENFEAT_NR_SUBMAPS 1
 
 #endif /* __XEN_PUBLIC_FEATURES_H__ */
