4 files changed, 127 insertions, 10 deletions

diff --git a/lib/Makefile b/lib/Makefile
index 543f2502b60a..a18062e4633f 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -65,6 +65,7 @@ obj-$(CONFIG_SMP) += pcounter.o
 obj-$(CONFIG_AUDIT_GENERIC) += audit.o
 
 obj-$(CONFIG_SWIOTLB) += swiotlb.o
+obj-$(CONFIG_IOMMU_HELPER) += iommu-helper.o
 obj-$(CONFIG_FAULT_INJECTION) += fault-inject.o
 
 lib-$(CONFIG_GENERIC_BUG) += bug.o
diff --git a/lib/iommu-helper.c b/lib/iommu-helper.c
new file mode 100644
index 000000000000..495575a59ca6
--- /dev/null
+++ b/lib/iommu-helper.c
@@ -0,0 +1,80 @@
+/*
+ * IOMMU helper functions for the free area management
+ */
+
+#include <linux/module.h>
+#include <linux/bitops.h>
+
+static unsigned long find_next_zero_area(unsigned long *map,
+                                         unsigned long size,
+                                         unsigned long start,
+                                         unsigned int nr,
+                                         unsigned long align_mask)
+{
+        unsigned long index, end, i;
+again:
+        index = find_next_zero_bit(map, size, start);
+
+        /* Align allocation */
+        index = (index + align_mask) & ~align_mask;
+
+        end = index + nr;
+        if (end >= size)
+                return -1;
+        for (i = index; i < end; i++) {
+                if (test_bit(i, map)) {
+                        start = i+1;
+                        goto again;
+                }
+        }
+        return index;
+}
+
+static inline void set_bit_area(unsigned long *map, unsigned long i,
+                                int len)
+{
+        unsigned long end = i + len;
+        while (i < end) {
+                __set_bit(i, map);
+                i++;
+        }
+}
+
+static inline int is_span_boundary(unsigned int index, unsigned int nr,
+                                   unsigned long shift,
+                                   unsigned long boundary_size)
+{
+        shift = (shift + index) & (boundary_size - 1);
+        return shift + nr > boundary_size;
+}
+
+unsigned long iommu_area_alloc(unsigned long *map, unsigned long size,
+                               unsigned long start, unsigned int nr,
+                               unsigned long shift, unsigned long boundary_size,
+                               unsigned long align_mask)
+{
+        unsigned long index;
+again:
+        index = find_next_zero_area(map, size, start, nr, align_mask);
+        if (index != -1) {
+                if (is_span_boundary(index, nr, shift, boundary_size)) {
+                        /* we could do more effectively */
+                        start = index + 1;
+                        goto again;
+                }
+                set_bit_area(map, index, nr);
+        }
+        return index;
+}
+EXPORT_SYMBOL(iommu_area_alloc);
+
+void iommu_area_free(unsigned long *map, unsigned long start, unsigned int nr)
+{
+        unsigned long end = start + nr;
+
+        while (start < end) {
+                __clear_bit(start, map);
+                start++;
+        }
+}
+EXPORT_SYMBOL(iommu_area_free);
diff --git a/lib/radix-tree.c b/lib/radix-tree.c
index 48c250fe2233..65f0e758ec38 100644
--- a/lib/radix-tree.c
+++ b/lib/radix-tree.c
@@ -95,14 +95,17 @@ static inline gfp_t root_gfp_mask(struct radix_tree_root *root)
 static struct radix_tree_node *
 radix_tree_node_alloc(struct radix_tree_root *root)
 {
-        struct radix_tree_node *ret;
+        struct radix_tree_node *ret = NULL;
         gfp_t gfp_mask = root_gfp_mask(root);
 
-        ret = kmem_cache_alloc(radix_tree_node_cachep,
-                                set_migrateflags(gfp_mask, __GFP_RECLAIMABLE));
-        if (ret == NULL && !(gfp_mask & __GFP_WAIT)) {
+        if (!(gfp_mask & __GFP_WAIT)) {
                 struct radix_tree_preload *rtp;
 
+                /*
+                 * Provided the caller has preloaded here, we will always
+                 * succeed in getting a node here (and never reach
+                 * kmem_cache_alloc)
+                 */
                 rtp = &__get_cpu_var(radix_tree_preloads);
                 if (rtp->nr) {
                         ret = rtp->nodes[rtp->nr - 1];
@@ -110,6 +113,10 @@ radix_tree_node_alloc(struct radix_tree_root *root)
                         rtp->nr--;
                 }
         }
+        if (ret == NULL)
+                ret = kmem_cache_alloc(radix_tree_node_cachep,
+                                set_migrateflags(gfp_mask, __GFP_RECLAIMABLE));
+
         BUG_ON(radix_tree_is_indirect_ptr(ret));
         return ret;
 }
diff --git a/lib/swiotlb.c b/lib/swiotlb.c
index 1a8050ade861..4bb5a11e18a2 100644
--- a/lib/swiotlb.c
+++ b/lib/swiotlb.c
@@ -282,6 +282,15 @@ address_needs_mapping(struct device *hwdev, dma_addr_t addr)
         return (addr & ~mask) != 0;
 }
 
+static inline unsigned int is_span_boundary(unsigned int index,
+                                            unsigned int nslots,
+                                            unsigned long offset_slots,
+                                            unsigned long max_slots)
+{
+        unsigned long offset = (offset_slots + index) & (max_slots - 1);
+        return offset + nslots > max_slots;
+}
+
 /*
  * Allocates bounce buffer and returns its kernel virtual address.
  */
@@ -292,6 +301,16 @@ map_single(struct device *hwdev, char *buffer, size_t size, int dir)
         char *dma_addr;
         unsigned int nslots, stride, index, wrap;
         int i;
+        unsigned long start_dma_addr;
+        unsigned long mask;
+        unsigned long offset_slots;
+        unsigned long max_slots;
+
+        mask = dma_get_seg_boundary(hwdev);
+        start_dma_addr = virt_to_bus(io_tlb_start) & mask;
+
+        offset_slots = ALIGN(start_dma_addr, 1 << IO_TLB_SHIFT) >> IO_TLB_SHIFT;
+        max_slots = ALIGN(mask + 1, 1 << IO_TLB_SHIFT) >> IO_TLB_SHIFT;
 
         /*
          * For mappings greater than a page, we limit the stride (and
@@ -311,10 +330,17 @@ map_single(struct device *hwdev, char *buffer, size_t size, int dir)
          */
         spin_lock_irqsave(&io_tlb_lock, flags);
         {
-                wrap = index = ALIGN(io_tlb_index, stride);
-
+                index = ALIGN(io_tlb_index, stride);
                 if (index >= io_tlb_nslabs)
-                        wrap = index = 0;
+                        index = 0;
+
+                while (is_span_boundary(index, nslots, offset_slots,
+                                        max_slots)) {
+                        index += stride;
+                        if (index >= io_tlb_nslabs)
+                                index = 0;
+                }
+                wrap = index;
 
                 do {
                         /*
@@ -341,9 +367,12 @@ map_single(struct device *hwdev, char *buffer, size_t size, int dir)
 
                                 goto found;
                         }
-                        index += stride;
-                        if (index >= io_tlb_nslabs)
-                                index = 0;
+                        do {
+                                index += stride;
+                                if (index >= io_tlb_nslabs)
+                                        index = 0;
+                        } while (is_span_boundary(index, nslots, offset_slots,
+                                                  max_slots));
                 } while (index != wrap);
 
                 spin_unlock_irqrestore(&io_tlb_lock, flags);