1 files changed, 58 insertions, 55 deletions

diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index edbdf5d6962c..3ed4db334341 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -22,7 +22,8 @@
 #include <linux/kernel.h>
 #include <linux/bug.h>
 #include <linux/types.h>
-#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/export.h>
 #include <linux/slab.h>
 #include <linux/errno.h>
 #include <linux/iommu.h>
@@ -110,6 +111,27 @@ void iommu_device_unregister(struct iommu_device *iommu)
         spin_unlock(&iommu_device_lock);
 }
 
+int iommu_probe_device(struct device *dev)
+{
+        const struct iommu_ops *ops = dev->bus->iommu_ops;
+        int ret = -EINVAL;
+
+        WARN_ON(dev->iommu_group);
+
+        if (ops)
+                ret = ops->add_device(dev);
+
+        return ret;
+}
+
+void iommu_release_device(struct device *dev)
+{
+        const struct iommu_ops *ops = dev->bus->iommu_ops;
+
+        if (dev->iommu_group)
+                ops->remove_device(dev);
+}
+
 static struct iommu_domain *__iommu_domain_alloc(struct bus_type *bus,
                                                  unsigned type);
 static int __iommu_attach_device(struct iommu_domain *domain,
@@ -1117,16 +1139,7 @@ struct iommu_domain *iommu_group_default_domain(struct iommu_group *group)
 
 static int add_iommu_group(struct device *dev, void *data)
 {
-        struct iommu_callback_data *cb = data;
-        const struct iommu_ops *ops = cb->ops;
-        int ret;
-
-        if (!ops->add_device)
-                return 0;
-
-        WARN_ON(dev->iommu_group);
-
-        ret = ops->add_device(dev);
+        int ret = iommu_probe_device(dev);
 
         /*
          * We ignore -ENODEV errors for now, as they just mean that the
@@ -1141,11 +1154,7 @@ static int add_iommu_group(struct device *dev, void *data)
 
 static int remove_iommu_group(struct device *dev, void *data)
 {
-        struct iommu_callback_data *cb = data;
-        const struct iommu_ops *ops = cb->ops;
-
-        if (ops->remove_device && dev->iommu_group)
-                ops->remove_device(dev);
+        iommu_release_device(dev);
 
         return 0;
 }
@@ -1153,27 +1162,22 @@ static int remove_iommu_group(struct device *dev, void *data)
 static int iommu_bus_notifier(struct notifier_block *nb,
                               unsigned long action, void *data)
 {
+        unsigned long group_action = 0;
         struct device *dev = data;
-        const struct iommu_ops *ops = dev->bus->iommu_ops;
         struct iommu_group *group;
-        unsigned long group_action = 0;
 
         /*
          * ADD/DEL call into iommu driver ops if provided, which may
          * result in ADD/DEL notifiers to group->notifier
          */
         if (action == BUS_NOTIFY_ADD_DEVICE) {
-                if (ops->add_device) {
-                        int ret;
+                int ret;
 
-                        ret = ops->add_device(dev);
-                        return (ret) ? NOTIFY_DONE : NOTIFY_OK;
-                }
+                ret = iommu_probe_device(dev);
+                return (ret) ? NOTIFY_DONE : NOTIFY_OK;
         } else if (action == BUS_NOTIFY_REMOVED_DEVICE) {
-                if (ops->remove_device && dev->iommu_group) {
-                        ops->remove_device(dev);
-                        return 0;
-                }
+                iommu_release_device(dev);
+                return NOTIFY_OK;
         }
 
         /*
@@ -1712,33 +1716,32 @@ EXPORT_SYMBOL_GPL(iommu_unmap_fast);
 size_t 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, min_pagesz;
+        size_t len = 0, mapped = 0;
+        phys_addr_t start;
+        unsigned int i = 0;
         int ret;
 
-        if (unlikely(domain->pgsize_bitmap == 0UL))
-                return 0;
+        while (i <= nents) {
+                phys_addr_t s_phys = sg_phys(sg);
 
-        min_pagesz = 1 << __ffs(domain->pgsize_bitmap);
-
-        for_each_sg(sg, s, nents, i) {
-                phys_addr_t phys = page_to_phys(sg_page(s)) + s->offset;
+                if (len && s_phys != start + len) {
+                        ret = iommu_map(domain, iova + mapped, start, len, prot);
+                        if (ret)
+                                goto out_err;
 
-                /*
-                 * We are mapping on IOMMU page boundaries, so offset within
-                 * the page must be 0. However, the IOMMU may support pages
-                 * smaller than PAGE_SIZE, so s->offset may still represent
-                 * an offset of that boundary within the CPU page.
-                 */
-                if (!IS_ALIGNED(s->offset, min_pagesz))
-                        goto out_err;
+                        mapped += len;
+                        len = 0;
+                }
 
-                ret = iommu_map(domain, iova + mapped, phys, s->length, prot);
-                if (ret)
-                        goto out_err;
+                if (len) {
+                        len += sg->length;
+                } else {
+                        len = sg->length;
+                        start = s_phys;
+                }
 
-                mapped += s->length;
+                if (++i < nents)
+                        sg = sg_next(sg);
         }
 
         return mapped;
@@ -1976,7 +1979,7 @@ const struct iommu_ops *iommu_ops_from_fwnode(struct fwnode_handle *fwnode)
 int iommu_fwspec_init(struct device *dev, struct fwnode_handle *iommu_fwnode,
                       const struct iommu_ops *ops)
 {
-        struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+        struct iommu_fwspec *fwspec = dev_iommu_fwspec_get(dev);
 
         if (fwspec)
                 return ops == fwspec->ops ? 0 : -EINVAL;
@@ -1988,26 +1991,26 @@ int iommu_fwspec_init(struct device *dev, struct fwnode_handle *iommu_fwnode,
         of_node_get(to_of_node(iommu_fwnode));
         fwspec->iommu_fwnode = iommu_fwnode;
         fwspec->ops = ops;
-        dev->iommu_fwspec = fwspec;
+        dev_iommu_fwspec_set(dev, fwspec);
         return 0;
 }
 EXPORT_SYMBOL_GPL(iommu_fwspec_init);
 
 void iommu_fwspec_free(struct device *dev)
 {
-        struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+        struct iommu_fwspec *fwspec = dev_iommu_fwspec_get(dev);
 
         if (fwspec) {
                 fwnode_handle_put(fwspec->iommu_fwnode);
                 kfree(fwspec);
-                dev->iommu_fwspec = NULL;
+                dev_iommu_fwspec_set(dev, NULL);
         }
 }
 EXPORT_SYMBOL_GPL(iommu_fwspec_free);
 
 int iommu_fwspec_add_ids(struct device *dev, u32 *ids, int num_ids)
 {
-        struct iommu_fwspec *fwspec = dev->iommu_fwspec;
+        struct iommu_fwspec *fwspec = dev_iommu_fwspec_get(dev);
         size_t size;
         int i;
 
@@ -2016,11 +2019,11 @@ int iommu_fwspec_add_ids(struct device *dev, u32 *ids, int num_ids)
 
         size = offsetof(struct iommu_fwspec, ids[fwspec->num_ids + num_ids]);
         if (size > sizeof(*fwspec)) {
-                fwspec = krealloc(dev->iommu_fwspec, size, GFP_KERNEL);
+                fwspec = krealloc(fwspec, size, GFP_KERNEL);
                 if (!fwspec)
                         return -ENOMEM;
 
-                dev->iommu_fwspec = fwspec;
+                dev_iommu_fwspec_set(dev, fwspec);
         }
 
         for (i = 0; i < num_ids; i++)