2 files changed, 123 insertions, 25 deletions

diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index 7a2953d8f12e..b278458d5816 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -16,6 +16,8 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  */
 
+#define pr_fmt(fmt)    "%s: " fmt, __func__
+
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/bug.h>
@@ -47,6 +49,16 @@ int bus_set_iommu(struct bus_type *bus, struct iommu_ops *ops)
         if (bus->iommu_ops != NULL)
                 return -EBUSY;
 
+        /*
+         * Set the default pgsize values, which retain the existing
+         * IOMMU API behavior: drivers will be called to map
+         * regions that are sized/aligned to order of 4KiB pages.
+         *
+         * This will be removed once all drivers are migrated.
+         */
+        if (!ops->pgsize_bitmap)
+                ops->pgsize_bitmap = ~0xFFFUL;
+
         bus->iommu_ops = ops;
 
         /* Do IOMMU specific setup for this bus-type */
@@ -157,34 +169,125 @@ int iommu_domain_has_cap(struct iommu_domain *domain,
 EXPORT_SYMBOL_GPL(iommu_domain_has_cap);
 
 int iommu_map(struct iommu_domain *domain, unsigned long iova,
-              phys_addr_t paddr, int gfp_order, int prot)
+              phys_addr_t paddr, size_t size, int prot)
 {
-        size_t size;
+        unsigned long orig_iova = iova;
+        unsigned int min_pagesz;
+        size_t orig_size = size;
+        int ret = 0;
 
         if (unlikely(domain->ops->map == NULL))
                 return -ENODEV;
 
-        size         = PAGE_SIZE << gfp_order;
+        /* find out the minimum page size supported */
+        min_pagesz = 1 << __ffs(domain->ops->pgsize_bitmap);
+
+        /*
+         * both the virtual address and the physical one, as well as
+         * the size of the mapping, must be aligned (at least) to the
+         * size of the smallest page supported by the hardware
+         */
+        if (!IS_ALIGNED(iova | paddr | size, min_pagesz)) {
+                pr_err("unaligned: iova 0x%lx pa 0x%lx size 0x%lx min_pagesz "
+                        "0x%x\n", iova, (unsigned long)paddr,
+                        (unsigned long)size, min_pagesz);
+                return -EINVAL;
+        }
+
+        pr_debug("map: iova 0x%lx pa 0x%lx size 0x%lx\n", iova,
+                                (unsigned long)paddr, (unsigned long)size);
+
+        while (size) {
+                unsigned long pgsize, addr_merge = iova | paddr;
+                unsigned int pgsize_idx;
+
+                /* Max page size that still fits into 'size' */
+                pgsize_idx = __fls(size);
+
+                /* need to consider alignment requirements ? */
+                if (likely(addr_merge)) {
+                        /* Max page size allowed by both iova and paddr */
+                        unsigned int align_pgsize_idx = __ffs(addr_merge);
+
+                        pgsize_idx = min(pgsize_idx, align_pgsize_idx);
+                }
+
+                /* build a mask of acceptable page sizes */
+                pgsize = (1UL << (pgsize_idx + 1)) - 1;
+
+                /* throw away page sizes not supported by the hardware */
+                pgsize &= domain->ops->pgsize_bitmap;
 
-        BUG_ON(!IS_ALIGNED(iova | paddr, size));
+                /* make sure we're still sane */
+                BUG_ON(!pgsize);
 
-        return domain->ops->map(domain, iova, paddr, size, prot);
+                /* pick the biggest page */
+                pgsize_idx = __fls(pgsize);
+                pgsize = 1UL << pgsize_idx;
+
+                pr_debug("mapping: iova 0x%lx pa 0x%lx pgsize %lu\n", iova,
+                                        (unsigned long)paddr, pgsize);
+
+                ret = domain->ops->map(domain, iova, paddr, pgsize, prot);
+                if (ret)
+                        break;
+
+                iova += pgsize;
+                paddr += pgsize;
+                size -= pgsize;
+        }
+
+        /* unroll mapping in case something went wrong */
+        if (ret)
+                iommu_unmap(domain, orig_iova, orig_size - size);
+
+        return ret;
 }
 EXPORT_SYMBOL_GPL(iommu_map);
 
-int iommu_unmap(struct iommu_domain *domain, unsigned long iova, int gfp_order)
+size_t iommu_unmap(struct iommu_domain *domain, unsigned long iova, size_t size)
 {
-        size_t size, unmapped;
+        size_t unmapped_page, unmapped = 0;
+        unsigned int min_pagesz;
 
         if (unlikely(domain->ops->unmap == NULL))
                 return -ENODEV;
 
-        size         = PAGE_SIZE << gfp_order;
-
-        BUG_ON(!IS_ALIGNED(iova, size));
-
-        unmapped = domain->ops->unmap(domain, iova, size);
-
-        return get_order(unmapped);
+        /* find out the minimum page size supported */
+        min_pagesz = 1 << __ffs(domain->ops->pgsize_bitmap);
+
+        /*
+         * The virtual address, as well as the size of the mapping, must be
+         * aligned (at least) to the size of the smallest page supported
+         * by the hardware
+         */
+        if (!IS_ALIGNED(iova | size, min_pagesz)) {
+                pr_err("unaligned: iova 0x%lx size 0x%lx min_pagesz 0x%x\n",
+                                        iova, (unsigned long)size, min_pagesz);
+                return -EINVAL;
+        }
+
+        pr_debug("unmap this: iova 0x%lx size 0x%lx\n", iova,
+                                                        (unsigned long)size);
+
+        /*
+         * Keep iterating until we either unmap 'size' bytes (or more)
+         * or we hit an area that isn't mapped.
+         */
+        while (unmapped < size) {
+                size_t left = size - unmapped;
+
+                unmapped_page = domain->ops->unmap(domain, iova, left);
+                if (!unmapped_page)
+                        break;
+
+                pr_debug("unmapped: iova 0x%lx size %lx\n", iova,
+                                        (unsigned long)unmapped_page);
+
+                iova += unmapped_page;
+                unmapped += unmapped_page;
+        }
+
+        return unmapped;
 }
 EXPORT_SYMBOL_GPL(iommu_unmap);
diff --git a/drivers/iommu/omap-iovmm.c b/drivers/iommu/omap-iovmm.c
index e8fdb8830f69..0b7b14cb030b 100644
--- a/drivers/iommu/omap-iovmm.c
+++ b/drivers/iommu/omap-iovmm.c
@@ -409,7 +409,6 @@ static int map_iovm_area(struct iommu_domain *domain, struct iovm_struct *new,
         unsigned int i, j;
         struct scatterlist *sg;
         u32 da = new->da_start;
-        int order;
 
         if (!domain || !sgt)
                 return -EINVAL;
@@ -428,12 +427,10 @@ static int map_iovm_area(struct iommu_domain *domain, struct iovm_struct *new,
                 if (bytes_to_iopgsz(bytes) < 0)
                         goto err_out;
 
-                order = get_order(bytes);
-
                 pr_debug("%s: [%d] %08x %08x(%x)\n", __func__,
                          i, da, pa, bytes);
 
-                err = iommu_map(domain, da, pa, order, flags);
+                err = iommu_map(domain, da, pa, bytes, flags);
                 if (err)
                         goto err_out;
 
@@ -448,10 +445,9 @@ err_out:
                 size_t bytes;
 
                 bytes = sg->length + sg->offset;
-                order = get_order(bytes);
 
                 /* ignore failures.. we're already handling one */
-                iommu_unmap(domain, da, order);
+                iommu_unmap(domain, da, bytes);
 
                 da += bytes;
         }
@@ -466,7 +462,8 @@ static void unmap_iovm_area(struct iommu_domain *domain, struct omap_iommu *obj,
         size_t total = area->da_end - area->da_start;
         const struct sg_table *sgt = area->sgt;
         struct scatterlist *sg;
-        int i, err;
+        int i;
+        size_t unmapped;
 
         BUG_ON(!sgtable_ok(sgt));
         BUG_ON((!total) || !IS_ALIGNED(total, PAGE_SIZE));
@@ -474,13 +471,11 @@ static void unmap_iovm_area(struct iommu_domain *domain, struct omap_iommu *obj,
         start = area->da_start;
         for_each_sg(sgt->sgl, sg, sgt->nents, i) {
                 size_t bytes;
-                int order;
 
                 bytes = sg->length + sg->offset;
-                order = get_order(bytes);
 
-                err = iommu_unmap(domain, start, order);
-                if (err < 0)
+                unmapped = iommu_unmap(domain, start, bytes);
+                if (unmapped < bytes)
                         break;
 
                 dev_dbg(obj->dev, "%s: unmap %08x(%x) %08x\n",