diff --git a/xen/common/pv_iommu.c b/xen/common/pv_iommu.c
index 304fccf..91485f3 100644
--- a/xen/common/pv_iommu.c
+++ b/xen/common/pv_iommu.c
@@ -17,13 +17,239 @@
  * along with this program; If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <asm/p2m.h>
+#include <asm/event.h>
 #include <xen/guest_access.h>
+#include <public/pv-iommu.h>
 
 #define ret_t long
 
+static int get_paged_frame(unsigned long gfn, unsigned long *frame,
+                           struct page_info **page, int readonly,
+                           struct domain *rd)
+{
+    int rc = 0;
+#if defined(P2M_PAGED_TYPES) || defined(P2M_SHARED_TYPES)
+    p2m_type_t p2mt;
+
+    *page = get_page_from_gfn(rd, gfn, &p2mt,
+                             (readonly) ? P2M_ALLOC : P2M_UNSHARE);
+    if ( !(*page) )
+    {
+        *frame = INVALID_MFN;
+        if ( p2m_is_shared(p2mt) )
+            return -EIO;
+        if ( p2m_is_paging(p2mt) )
+        {
+            p2m_mem_paging_populate(rd, gfn);
+            return -EIO;
+        }
+        return -EIO;
+    }
+    *frame = page_to_mfn(*page);
+#else
+    *frame = gmfn_to_mfn(rd, gfn);
+    *page = mfn_valid(*frame) ? mfn_to_page(*frame) : NULL;
+    if ( (!(page)) || (!get_page*page, rd) )
+    {
+        *frame = INVALID_MFN;
+        *page = NULL;
+        rc = -EIO;
+    }
+#endif
+
+    return rc;
+}
+
+int can_use_iommu_check(struct domain *d)
+{
+    if ( !iommu_enabled || (!is_hardware_domain(d) && !need_iommu(d)) )
+        return 0;
+
+    if ( is_hardware_domain(d) && iommu_passthrough )
+        return 0;
+
+    if ( !domain_hvm_iommu(d)->platform_ops->lookup_page )
+        return 0;
+
+    return 1;
+}
+
+void do_iommu_sub_op(struct pv_iommu_op *op)
+{
+    struct domain *d = current->domain;
+    struct domain *rd = NULL;
+
+    /* Only order 0 pages supported */
+    if ( IOMMU_get_page_order(op->flags) != 0 )
+    {
+        op->status = -ENOSPC;
+        goto finish;
+    }
+
+    switch ( op->subop_id )
+    {
+        case IOMMUOP_query_caps:
+        {
+            op->flags = 0;
+            op->status = 0;
+            if ( can_use_iommu_check(d) )
+                op->flags |= IOMMU_QUERY_map_cap;
+
+            break;
+        }
+        case IOMMUOP_map_page:
+        {
+            unsigned long mfn, tmp;
+            unsigned int flags = 0;
+            struct page_info *page = NULL;
+
+            /* Check if calling domain can create IOMMU mappings */
+            if ( !can_use_iommu_check(d) )
+            {
+                op->status = -EPERM;
+                goto finish;
+            }
+
+            /* Check we are the owner of the page */
+            if ( !is_hardware_domain(d) &&
+                 ( maddr_get_owner(op->u.map_page.gfn) != d ) )
+            {
+                op->status = -EPERM;
+                goto finish;
+            }
+
+            /* Lookup page struct backing gfn */
+            if ( is_hardware_domain(d) &&
+                (op->flags & IOMMU_MAP_OP_no_ref_cnt) )
+            {
+                mfn = op->u.map_page.gfn;
+                page = mfn_to_page(mfn);
+                if (!page)
+                {
+                    op->status = -EPERM;
+                    goto finish;
+                }
+            } else if ( get_paged_frame(op->u.map_page.gfn, &mfn, &page, 0, d) )
+            {
+                op->status = -EPERM;
+                goto finish;
+            }
+
+            /* Check for conflict with existing BFN mappings */
+            if ( !iommu_lookup_page(d, op->u.map_page.bfn, &tmp) )
+            {
+                if ( !(op->flags & IOMMU_MAP_OP_no_ref_cnt) )
+                    put_page(page);
+                op->status = -EPERM;
+                goto finish;
+            }
+
+            if ( op->flags & IOMMU_OP_readable )
+                flags |= IOMMUF_readable;
+
+            if ( op->flags & IOMMU_OP_writeable )
+                flags |= IOMMUF_writable;
+
+            if ( iommu_map_page(d, op->u.map_page.bfn, mfn, flags) )
+            {
+                if ( !(op->flags & IOMMU_MAP_OP_no_ref_cnt) )
+                    put_page(page);
+                op->status = -EIO;
+                goto finish;
+            }
+
+            op->status = 0;
+            break;
+        }
+
+        case IOMMUOP_unmap_page:
+        {
+            unsigned long mfn;
+
+            /* Check if there is a valid BFN mapping for this domain */
+            if ( iommu_lookup_page(d, op->u.unmap_page.bfn, &mfn) )
+            {
+                op->status = -ENOENT;
+                goto finish;
+            }
+
+            if ( iommu_unmap_page(d, op->u.unmap_page.bfn) )
+            {
+                op->status = -EIO;
+                goto finish;
+            }
+
+            op->status = 0;
+            break;
+        }
+
+        default:
+            op->status = -ENODEV;
+            break;
+    }
+
+finish:
+    if ( rd )
+        rcu_unlock_domain(rd);
+
+    return;
+}
+
 ret_t do_iommu_op(XEN_GUEST_HANDLE_PARAM(void) arg, unsigned int count)
 {
-    return -ENOSYS;
+    ret_t ret = 0;
+    int i;
+    struct pv_iommu_op op;
+    struct domain *d = current->domain;
+
+    if ( !is_hardware_domain(d) )
+        return -ENOSYS;
+
+    if ( (int)count < 0 )
+        return -EINVAL;
+
+    if ( count > 1 )
+        this_cpu(iommu_dont_flush_iotlb) = 1;
+
+    for ( i = 0; i < count; i++ )
+    {
+        if ( i && hypercall_preempt_check() )
+        {
+            ret =  i;
+            goto flush_pages;
+        }
+        if ( unlikely(__copy_from_guest_offset(&op, arg, i, 1)) )
+        {
+            ret = -EFAULT;
+            goto flush_pages;
+        }
+        do_iommu_sub_op(&op);
+        if ( unlikely(__copy_to_guest_offset(arg, i, &op, 1)) )
+        {
+            ret = -EFAULT;
+            goto flush_pages;
+        }
+    }
+
+flush_pages:
+    if ( ret > 0 )
+    {
+        XEN_GUEST_HANDLE_PARAM(pv_iommu_op_t) op =
+            guest_handle_cast(arg, pv_iommu_op_t);
+        ASSERT(ret < count);
+        guest_handle_add_offset(op, i);
+        arg = guest_handle_cast(op, void);
+        ret = hypercall_create_continuation(__HYPERVISOR_iommu_op,
+                                           "hi", arg, count - i);
+    }
+    if ( count > 1 )
+    {
+        this_cpu(iommu_dont_flush_iotlb) = 0;
+        if ( i )
+            iommu_iotlb_flush_all(d);
+    }
+    return ret;
 }
 
 /*
diff --git a/xen/include/public/pv-iommu.h b/xen/include/public/pv-iommu.h
new file mode 100644
index 0000000..0f19f96
--- /dev/null
+++ b/xen/include/public/pv-iommu.h
@@ -0,0 +1,69 @@
+/*
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to
+ * deal in the Software without restriction, including without limitation the
+ * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+ * sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef __XEN_PUBLIC_PV_IOMMU_H__
+#define __XEN_PUBLIC_PV_IOMMU_H__
+
+#include "xen.h"
+
+#define IOMMUOP_query_caps            1
+#define IOMMUOP_map_page              2
+#define IOMMUOP_unmap_page            3
+
+struct pv_iommu_op {
+    uint16_t subop_id;
+
+#define IOMMU_page_order (0xf1 << 10)
+#define IOMMU_get_page_order(flags) ((flags & IOMMU_page_order) >> 10)
+#define IOMMU_QUERY_map_cap (1 << 0)
+#define IOMMU_QUERY_map_all_mfns (1 << 1)
+#define IOMMU_OP_readable (1 << 0)
+#define IOMMU_OP_writeable (1 << 1)
+#define IOMMU_MAP_OP_no_ref_cnt (1 << 2)
+    uint16_t flags;
+    int32_t status;
+
+    union {
+        struct {
+            uint64_t bfn;
+            uint64_t gfn;
+        } map_page;
+
+        struct {
+            uint64_t bfn;
+        } unmap_page;
+    } u;
+};
+
+
+typedef struct pv_iommu_op pv_iommu_op_t;
+DEFINE_XEN_GUEST_HANDLE(pv_iommu_op_t);
+
+#endif
+
+/*
+ * Local variables:
+ * mode: C
+ * c-file-style: "BSD"
+ * c-basic-offset: 4
+ * tab-width: 4
+ * indent-tabs-mode: nil
+ * End:
+ */
