4 files changed, 104 insertions, 73 deletions

diff --git a/lib/bitmap.c b/lib/bitmap.c
index 702565821c99..11bf49750583 100644
--- a/lib/bitmap.c
+++ b/lib/bitmap.c
@@ -271,6 +271,87 @@ int __bitmap_weight(const unsigned long *bitmap, int bits)
 }
 EXPORT_SYMBOL(__bitmap_weight);
 
+#define BITMAP_FIRST_WORD_MASK(start) (~0UL << ((start) % BITS_PER_LONG))
+
+void bitmap_set(unsigned long *map, int start, int nr)
+{
+        unsigned long *p = map + BIT_WORD(start);
+        const int size = start + nr;
+        int bits_to_set = BITS_PER_LONG - (start % BITS_PER_LONG);
+        unsigned long mask_to_set = BITMAP_FIRST_WORD_MASK(start);
+
+        while (nr - bits_to_set >= 0) {
+                *p |= mask_to_set;
+                nr -= bits_to_set;
+                bits_to_set = BITS_PER_LONG;
+                mask_to_set = ~0UL;
+                p++;
+        }
+        if (nr) {
+                mask_to_set &= BITMAP_LAST_WORD_MASK(size);
+                *p |= mask_to_set;
+        }
+}
+EXPORT_SYMBOL(bitmap_set);
+
+void bitmap_clear(unsigned long *map, int start, int nr)
+{
+        unsigned long *p = map + BIT_WORD(start);
+        const int size = start + nr;
+        int bits_to_clear = BITS_PER_LONG - (start % BITS_PER_LONG);
+        unsigned long mask_to_clear = BITMAP_FIRST_WORD_MASK(start);
+
+        while (nr - bits_to_clear >= 0) {
+                *p &= ~mask_to_clear;
+                nr -= bits_to_clear;
+                bits_to_clear = BITS_PER_LONG;
+                mask_to_clear = ~0UL;
+                p++;
+        }
+        if (nr) {
+                mask_to_clear &= BITMAP_LAST_WORD_MASK(size);
+                *p &= ~mask_to_clear;
+        }
+}
+EXPORT_SYMBOL(bitmap_clear);
+
+/*
+ * bitmap_find_next_zero_area - find a contiguous aligned zero area
+ * @map: The address to base the search on
+ * @size: The bitmap size in bits
+ * @start: The bitnumber to start searching at
+ * @nr: The number of zeroed bits we're looking for
+ * @align_mask: Alignment mask for zero area
+ *
+ * The @align_mask should be one less than a power of 2; the effect is that
+ * the bit offset of all zero areas this function finds is multiples of that
+ * power of 2. A @align_mask of 0 means no alignment is required.
+ */
+unsigned long bitmap_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 = __ALIGN_MASK(index, align_mask);
+
+        end = index + nr;
+        if (end > size)
+                return end;
+        i = find_next_bit(map, end, index);
+        if (i < end) {
+                start = i + 1;
+                goto again;
+        }
+        return index;
+}
+EXPORT_SYMBOL(bitmap_find_next_zero_area);
+
 /*
  * Bitmap printing & parsing functions: first version by Bill Irwin,
  * second version by Paul Jackson, third by Joe Korty.
diff --git a/lib/genalloc.c b/lib/genalloc.c
index eed2bdb865e7..e67f97495dd5 100644
--- a/lib/genalloc.c
+++ b/lib/genalloc.c
@@ -11,6 +11,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/bitmap.h>
 #include <linux/genalloc.h>
 
 
@@ -114,7 +115,7 @@ unsigned long gen_pool_alloc(struct gen_pool *pool, size_t size)
         struct gen_pool_chunk *chunk;
         unsigned long addr, flags;
         int order = pool->min_alloc_order;
-        int nbits, bit, start_bit, end_bit;
+        int nbits, start_bit, end_bit;
 
         if (size == 0)
                 return 0;
@@ -129,29 +130,19 @@ unsigned long gen_pool_alloc(struct gen_pool *pool, size_t size)
                 end_bit -= nbits + 1;
 
                 spin_lock_irqsave(&chunk->lock, flags);
-                bit = -1;
-                while (bit + 1 < end_bit) {
-                        bit = find_next_zero_bit(chunk->bits, end_bit, bit + 1);
-                        if (bit >= end_bit)
-                                break;
-
-                        start_bit = bit;
-                        if (nbits > 1) {
-                                bit = find_next_bit(chunk->bits, bit + nbits,
-                                                        bit + 1);
-                                if (bit - start_bit < nbits)
-                                        continue;
-                        }
-
-                        addr = chunk->start_addr +
-                                            ((unsigned long)start_bit << order);
-                        while (nbits--)
-                                __set_bit(start_bit++, chunk->bits);
+                start_bit = bitmap_find_next_zero_area(chunk->bits, end_bit, 0,
+                                                nbits, 0);
+                if (start_bit >= end_bit) {
                         spin_unlock_irqrestore(&chunk->lock, flags);
-                        read_unlock(&pool->lock);
-                        return addr;
+                        continue;
                 }
+
+                addr = chunk->start_addr + ((unsigned long)start_bit << order);
+
+                bitmap_set(chunk->bits, start_bit, nbits);
                 spin_unlock_irqrestore(&chunk->lock, flags);
+                read_unlock(&pool->lock);
+                return addr;
         }
         read_unlock(&pool->lock);
         return 0;
diff --git a/lib/iommu-helper.c b/lib/iommu-helper.c
index 75dbda03f4fb..c0251f4ad08b 100644
--- a/lib/iommu-helper.c
+++ b/lib/iommu-helper.c
@@ -3,41 +3,7 @@
  */
 
 #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;
-}
-
-void iommu_area_reserve(unsigned long *map, unsigned long i, int len)
-{
-        unsigned long end = i + len;
-        while (i < end) {
-                __set_bit(i, map);
-                i++;
-        }
-}
+#include <linux/bitmap.h>
 
 int iommu_is_span_boundary(unsigned int index, unsigned int nr,
                            unsigned long shift,
@@ -55,31 +21,24 @@ unsigned long iommu_area_alloc(unsigned long *map, unsigned long size,
                                unsigned long align_mask)
 {
         unsigned long index;
+
+        /* We don't want the last of the limit */
+        size -= 1;
 again:
-        index = find_next_zero_area(map, size, start, nr, align_mask);
-        if (index != -1) {
+        index = bitmap_find_next_zero_area(map, size, start, nr, align_mask);
+        if (index < size) {
                 if (iommu_is_span_boundary(index, nr, shift, boundary_size)) {
                         /* we could do more effectively */
                         start = index + 1;
                         goto again;
                 }
-                iommu_area_reserve(map, index, nr);
+                bitmap_set(map, index, nr);
+                return index;
         }
-        return index;
+        return -1;
 }
 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);
-
 unsigned long iommu_num_pages(unsigned long addr, unsigned long len,
                               unsigned long io_page_size)
 {
diff --git a/lib/swiotlb.c b/lib/swiotlb.c
index 5bc01803f8f8..437eedb5a53b 100644
--- a/lib/swiotlb.c
+++ b/lib/swiotlb.c
@@ -549,7 +549,7 @@ swiotlb_alloc_coherent(struct device *hwdev, size_t size,
                 dma_mask = hwdev->coherent_dma_mask;
 
         ret = (void *)__get_free_pages(flags, order);
-        if (ret && swiotlb_virt_to_bus(hwdev, ret) + size > dma_mask) {
+        if (ret && swiotlb_virt_to_bus(hwdev, ret) + size - 1 > dma_mask) {
                 /*
                  * The allocated memory isn't reachable by the device.
                  */
@@ -571,7 +571,7 @@ swiotlb_alloc_coherent(struct device *hwdev, size_t size,
         dev_addr = swiotlb_virt_to_bus(hwdev, ret);
 
         /* Confirm address can be DMA'd by device */
-        if (dev_addr + size > dma_mask) {
+        if (dev_addr + size - 1 > dma_mask) {
                 printk("hwdev DMA mask = 0x%016Lx, dev_addr = 0x%016Lx\n",
                        (unsigned long long)dma_mask,
                        (unsigned long long)dev_addr);