1 files changed, 28 insertions, 14 deletions

diff --git a/arch/powerpc/sysdev/msi_bitmap.c b/arch/powerpc/sysdev/msi_bitmap.c
index 2ff630267e9..0c75214b6f9 100644
--- a/arch/powerpc/sysdev/msi_bitmap.c
+++ b/arch/powerpc/sysdev/msi_bitmap.c
@@ -20,32 +20,37 @@ int msi_bitmap_alloc_hwirqs(struct msi_bitmap *bmp, int num)
 	int offset, order = get_count_order(num);
 
 	spin_lock_irqsave(&bmp->lock, flags);
-	/*
-	 * This is fast, but stricter than we need. We might want to add
-	 * a fallback routine which does a linear search with no alignment.
-	 */
-	offset = bitmap_find_free_region(bmp->bitmap, bmp->irq_count, order);
+
+	offset = bitmap_find_next_zero_area(bmp->bitmap, bmp->irq_count, 0,
+					    num, (1 << order) - 1);
+	if (offset > bmp->irq_count)
+		goto err;
+
+	bitmap_set(bmp->bitmap, offset, num);
 	spin_unlock_irqrestore(&bmp->lock, flags);
 
-	pr_debug("msi_bitmap: allocated 0x%x (2^%d) at offset 0x%x\n",
-		 num, order, offset);
+	pr_debug("msi_bitmap: allocated 0x%x at offset 0x%x\n", num, offset);
 
 	return offset;
+err:
+	spin_unlock_irqrestore(&bmp->lock, flags);
+	return -ENOMEM;
 }
+EXPORT_SYMBOL(msi_bitmap_alloc_hwirqs);
 
 void msi_bitmap_free_hwirqs(struct msi_bitmap *bmp, unsigned int offset,
 			    unsigned int num)
 {
 	unsigned long flags;
-	int order = get_count_order(num);
 
-	pr_debug("msi_bitmap: freeing 0x%x (2^%d) at offset 0x%x\n",
-		 num, order, offset);
+	pr_debug("msi_bitmap: freeing 0x%x at offset 0x%x\n",
+		 num, offset);
 
 	spin_lock_irqsave(&bmp->lock, flags);
-	bitmap_release_region(bmp->bitmap, offset, order);
+	bitmap_clear(bmp->bitmap, offset, num);
 	spin_unlock_irqrestore(&bmp->lock, flags);
 }
+EXPORT_SYMBOL(msi_bitmap_free_hwirqs);
 
 void msi_bitmap_reserve_hwirq(struct msi_bitmap *bmp, unsigned int hwirq)
 {
@@ -143,7 +148,7 @@ void msi_bitmap_free(struct msi_bitmap *bmp)
 #define check(x)	\
 	if (!(x)) printk("msi_bitmap: test failed at line %d\n", __LINE__);
 
-void __init test_basics(void)
+static void __init test_basics(void)
 {
 	struct msi_bitmap bmp;
 	int i, size = 512;
@@ -180,6 +185,15 @@ void __init test_basics(void)
 	msi_bitmap_free_hwirqs(&bmp, size / 2, 1);
 	check(msi_bitmap_alloc_hwirqs(&bmp, 1) == size / 2);
 
+	/* Check we get a naturally aligned offset */
+	check(msi_bitmap_alloc_hwirqs(&bmp, 2) % 2 == 0);
+	check(msi_bitmap_alloc_hwirqs(&bmp, 4) % 4 == 0);
+	check(msi_bitmap_alloc_hwirqs(&bmp, 8) % 8 == 0);
+	check(msi_bitmap_alloc_hwirqs(&bmp, 9) % 16 == 0);
+	check(msi_bitmap_alloc_hwirqs(&bmp, 3) % 4 == 0);
+	check(msi_bitmap_alloc_hwirqs(&bmp, 7) % 8 == 0);
+	check(msi_bitmap_alloc_hwirqs(&bmp, 121) % 128 == 0);
+
 	msi_bitmap_free(&bmp);
 
 	/* Clients may check bitmap == NULL for "not-allocated" */
@@ -188,7 +202,7 @@ void __init test_basics(void)
 	kfree(bmp.bitmap);
 }
 
-void __init test_of_node(void)
+static void __init test_of_node(void)
 {
 	u32 prop_data[] = { 10, 10, 25, 3, 40, 1, 100, 100, 200, 20 };
 	const char *expected_str = "0-9,20-24,28-39,41-99,220-255";
@@ -236,7 +250,7 @@ void __init test_of_node(void)
 	kfree(bmp.bitmap);
 }
 
-int __init msi_bitmap_selftest(void)
+static int __init msi_bitmap_selftest(void)
 {
 	printk(KERN_DEBUG "Running MSI bitmap self-tests ...\n");