From c9891db2b888da5f93387125dde69174e9f0e916 Mon Sep 17 00:00:00 2001
From: Mathieu Desnoyers <mathieu.desnoyers@efficios.com>
Date: Mon, 23 May 2011 12:03:41 -0400
Subject: [PATCH] Implement missing prio heap functions (MIT-licensed)

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

diff --git a/lib/prio_heap/prio_heap.c b/lib/prio_heap/prio_heap.c
index e660b0cd..0c9bb607 100644
--- a/lib/prio_heap/prio_heap.c
+++ b/lib/prio_heap/prio_heap.c
@@ -20,9 +20,23 @@
 #include <linux/slab.h>
 #include <linux/prio_heap.h>
 
-/*
- * TODO implement heap_init, heap_free, heap_insert.
- */
+int heap_init(struct ptr_heap *heap, size_t size,
+	       gfp_t gfpmask, int gt(void *a, void *b))
+{
+	WARN_ON_ONCE(size == 0);
+	heap->ptrs = kmalloc(size * sizeof(void *), gfpmask);
+	if (!heap->ptrs)
+		return -ENOMEM;
+	heap->size = 0;
+	heap->max = size;
+	heap->gt = gt;
+	return 0;
+}
+
+void heap_free(struct ptr_heap *heap)
+{
+	kfree(heap->ptrs);
+}
 
 static void heapify(struct ptr_heap *heap, int pos)
 {
@@ -64,6 +78,34 @@ void *heap_replace_max(struct ptr_heap *heap, void *p)
 	return res;
 }
 
+void *heap_insert(struct ptr_heap *heap, void *p)
+{
+	void **ptrs = heap->ptrs;
+	void *tmp = NULL;
+
+	if (heap->size < heap->max) {
+		/* Add the element to the end */
+		heap->ptrs[heap->size++] = p;
+		/* rebalance */
+		heapify(heap, 0);
+		return NULL;
+	}
+
+	/*
+	 * Full. We need to replace the largest (if we are
+	 * smaller or equal to this element).
+	 */
+	if (heap->gt(ptrs[0], p)) {
+		tmp = ptrs[0];
+		ptrs[0] = p;
+		/* rebalance */
+		heapify(heap, 0);
+	} else {
+		tmp = p;
+	}
+	return tmp;
+}
+
 void *heap_remove(struct ptr_heap *heap)
 {
 	void **ptrs = heap->ptrs;
diff --git a/lib/prio_heap/prio_heap.h b/lib/prio_heap/prio_heap.h
index 12b1638f..3a318853 100644
--- a/lib/prio_heap/prio_heap.h
+++ b/lib/prio_heap/prio_heap.h
@@ -20,10 +20,13 @@
  * all copies or substantial portions of the Software.
  */
 
-/*
- * TODO:
- * implement struct ptr_head, heap_init, heap_free, heap_insert.
- */
+#include <linux/gfp.h>
+
+struct ptr_heap {
+	size_t size, max;
+	void **ptrs;
+	int (*gt)(void *a, void *b);
+};
 
 /**
  * heap_maximum - return the largest element in the heap
@@ -37,6 +40,33 @@ static inline void *heap_maximum(const struct ptr_heap *heap)
 	return heap->size ? heap->ptrs[0] : NULL;
 }
 
+/**
+ * heap_init - initialize the heap
+ * @heap: the heap to initialize
+ * @size: maximum number of elements
+ * @gfp: allocation flags
+ * @gt: function to compare the elements
+ *
+ * Returns -ENOMEM if out of memory.
+ */
+extern int heap_init(struct ptr_heap *heap, size_t size,
+		     gfp_t gfpmask, int gt(void *a, void *b));
+
+/**
+ * heap_free - free the heap
+ * @heap: the heap to free
+ */
+extern void heap_free(struct ptr_heap *heap);
+
+/**
+ * heap_insert - insert an element into the heap
+ * @heap: the heap to be operated on
+ *
+ * Insert an element into the heap. If the heap is full, return the
+ * largest element between those previously present in the heap and the
+ * element being added, else return NULL.
+ */
+extern void *heap_insert(struct ptr_heap *heap, void *p);
 
 /**
  * heap_remove - remove the largest element from the heap
-- 
2.34.1