From 0b6d2ab30831e9385a4796d45012ebb749a63cae Mon Sep 17 00:00:00 2001
From: Mathieu Desnoyers <mathieu.desnoyers@efficios.com>
Date: Mon, 23 May 2011 21:04:42 -0400
Subject: [PATCH] Add heap debug option, fix heap.

Signed-off-by: Mathieu Desnoyers <mathieu.desnoyers@efficios.com>
---
 lib/prio_heap/prio_heap.c | 75 ++++++++++++++++++++-------------------
 lib/prio_heap/prio_heap.h | 10 ++++++
 2 files changed, 48 insertions(+), 37 deletions(-)

diff --git a/lib/prio_heap/prio_heap.c b/lib/prio_heap/prio_heap.c
index 29af3033..f6b81580 100644
--- a/lib/prio_heap/prio_heap.c
+++ b/lib/prio_heap/prio_heap.c
@@ -19,22 +19,35 @@
 #include <linux/slab.h>
 #include "prio_heap.h"
 
+#ifdef DEBUG_HEAP
+void check_heap(const struct ptr_heap *heap)
+{
+	size_t i;
+
+	if (!heap->len)
+		return;
+
+	for (i = 1; i < heap->len; i++)
+		WARN_ON_ONCE(!heap->gt(heap->ptrs[i], heap->ptrs[0]));
+}
+#endif
+
 static
 size_t parent(size_t i)
 {
-	return i >> 1;
+	return (i -1) >> 1;
 }
 
 static
 size_t left(size_t i)
 {
-	return i << 1;
+	return (i << 1) + 1;
 }
 
 static
 size_t right(size_t i)
 {
-	return (i << 1) + 1;
+	return (i << 1) + 2;
 }
 
 /*
@@ -96,26 +109,24 @@ static void heapify(struct ptr_heap *heap, size_t i)
 	size_t l, r, largest;
 
 	for (;;) {
+		void *tmp;
+
 		l = left(i);
 		r = right(i);
-		if (l <= heap->len && ptrs[l] > ptrs[i])
+		if (l < heap->len && heap->gt(ptrs[l], ptrs[i]))
 			largest = l;
 		else
 			largest = i;
-		if (r <= heap->len && ptrs[r] > ptrs[largest])
+		if (r < heap->len && heap->gt(ptrs[r], ptrs[largest]))
 			largest = r;
-		if (largest != i) {
-			void *tmp;
-
-			tmp = ptrs[i];
-			ptrs[i] = ptrs[largest];
-			ptrs[largest] = tmp;
-			i = largest;
-			continue;
-		} else {
+		if (largest == i)
 			break;
-		}
+		tmp = ptrs[i];
+		ptrs[i] = ptrs[largest];
+		ptrs[largest] = tmp;
+		i = largest;
 	}
+	check_heap(heap);
 }
 
 void *heap_replace_max(struct ptr_heap *heap, void *p)
@@ -125,6 +136,7 @@ void *heap_replace_max(struct ptr_heap *heap, void *p)
 	if (!heap->len) {
 		(void) heap_set_len(heap, 1);
 		heap->ptrs[0] = p;
+		check_heap(heap);
 		return NULL;
 	}
 
@@ -145,28 +157,14 @@ int heap_insert(struct ptr_heap *heap, void *p)
 	if (ret)
 		return ret;
 	ptrs = heap->ptrs;
-	/* Add the element to the end */
-	ptrs[heap->len - 1] = p;
 	pos = heap->len - 1;
-	/* Bubble it up to the appropriate position. */
-	for (;;) {
-		if (pos > 0 && heap->gt(ptrs[pos], ptrs[parent(pos)])) {
-			void *tmp;
-
-			/* Need to exchange */
-			tmp = ptrs[pos];
-			ptrs[pos] = ptrs[parent(pos)];
-			ptrs[parent(pos)] = tmp;
-			pos = parent(pos);
-			/*
-			 * No need to rebalance: if we are larger than
-			 * our parent, we are necessarily larger than
-			 * its other child.
-			 */
-		} else {
-			break;
-		}
+	while (pos > 0 && heap->gt(p, ptrs[parent(pos)])) {
+		/* Move parent down until we find the right spot */
+		ptrs[pos] = ptrs[parent(pos)];
+		pos = parent(pos);
 	}
+	ptrs[pos] = p;
+	check_heap(heap);
 	return 0;
 }
 
@@ -181,7 +179,8 @@ void *heap_remove(struct ptr_heap *heap)
 	}
 	/* Shrink, replace the current max by previous last entry and heapify */
 	heap_set_len(heap, heap->len - 1);
-	return heap_replace_max(heap, heap->ptrs[heap->len - 1]);
+	/* len changed. previous last entry is at heap->len */
+	return heap_replace_max(heap, heap->ptrs[heap->len]);
 }
 
 void *heap_cherrypick(struct ptr_heap *heap, void *p)
@@ -195,11 +194,13 @@ void *heap_cherrypick(struct ptr_heap *heap, void *p)
 found:
 	if (heap->len == 1) {
 		(void) heap_set_len(heap, 0);
+		check_heap(heap);
 		return heap->ptrs[0];
 	}
 	/* Replace p with previous last entry and heapify. */
 	heap_set_len(heap, heap->len - 1);
-	heap->ptrs[pos] = heap->ptrs[heap->len - 1];
+	/* len changed. previous last entry is at heap->len */
+	heap->ptrs[pos] = heap->ptrs[heap->len];
 	heapify(heap, pos);
 	return p;
 }
diff --git a/lib/prio_heap/prio_heap.h b/lib/prio_heap/prio_heap.h
index 2674db21..2b55adf2 100644
--- a/lib/prio_heap/prio_heap.h
+++ b/lib/prio_heap/prio_heap.h
@@ -28,6 +28,15 @@ struct ptr_heap {
 	gfp_t gfpmask;
 };
 
+#ifdef DEBUG_HEAP
+void check_heap(const struct ptr_heap *heap);
+#else
+static inline
+void check_heap(const struct ptr_heap *heap)
+{
+}
+#endif
+
 /**
  * heap_maximum - return the largest element in the heap
  * @heap: the heap to be operated on
@@ -37,6 +46,7 @@ struct ptr_heap {
  */
 static inline void *heap_maximum(const struct ptr_heap *heap)
 {
+	check_heap(heap);
 	return heap->len ? heap->ptrs[0] : NULL;
 }
 
-- 
2.34.1