1 files changed, 48 insertions, 2 deletions

diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index ed8b04867b1f..02e4313e937c 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -818,7 +818,15 @@ static int iommu_bus_init(struct bus_type *bus, const struct iommu_ops *ops)
                 kfree(nb);
                 return err;
         }
-        return bus_for_each_dev(bus, NULL, &cb, add_iommu_group);
+
+        err = bus_for_each_dev(bus, NULL, &cb, add_iommu_group);
+        if (err) {
+                bus_unregister_notifier(bus, nb);
+                kfree(nb);
+                return err;
+        }
+
+        return 0;
 }
 
 /**
@@ -836,13 +844,19 @@ static int iommu_bus_init(struct bus_type *bus, const struct iommu_ops *ops)
  */
 int bus_set_iommu(struct bus_type *bus, const struct iommu_ops *ops)
 {
+        int err;
+
         if (bus->iommu_ops != NULL)
                 return -EBUSY;
 
         bus->iommu_ops = ops;
 
         /* Do IOMMU specific setup for this bus-type */
-        return iommu_bus_init(bus, ops);
+        err = iommu_bus_init(bus, ops);
+        if (err)
+                bus->iommu_ops = NULL;
+
+        return err;
 }
 EXPORT_SYMBOL_GPL(bus_set_iommu);
 
@@ -1124,6 +1138,38 @@ size_t iommu_unmap(struct iommu_domain *domain, unsigned long iova, size_t size)
 }
 EXPORT_SYMBOL_GPL(iommu_unmap);
 
+size_t default_iommu_map_sg(struct iommu_domain *domain, unsigned long iova,
+                         struct scatterlist *sg, unsigned int nents, int prot)
+{
+        struct scatterlist *s;
+        size_t mapped = 0;
+        unsigned int i;
+        int ret;
+
+        for_each_sg(sg, s, nents, i) {
+                phys_addr_t phys = page_to_phys(sg_page(s));
+
+                /* We are mapping on page boundarys, so offset must be 0 */
+                if (s->offset)
+                        goto out_err;
+
+                ret = iommu_map(domain, iova + mapped, phys, s->length, prot);
+                if (ret)
+                        goto out_err;
+
+                mapped += s->length;
+        }
+
+        return mapped;
+
+out_err:
+        /* undo mappings already done */
+        iommu_unmap(domain, iova, mapped);
+
+        return 0;
+
+}
+EXPORT_SYMBOL_GPL(default_iommu_map_sg);
 
 int iommu_domain_window_enable(struct iommu_domain *domain, u32 wnd_nr,
                                phys_addr_t paddr, u64 size, int prot)