/*
* prio_heap.c
*
- * Static-sized priority heap containing pointers. Based on CLRS,
- * chapter 6.
+ * Priority heap containing pointers. Based on CLRS, chapter 6.
*
* Copyright 2011 - Mathieu Desnoyers <mathieu.desnoyers@efficios.com>
*
*/
#include <linux/slab.h>
-#include <linux/prio_heap.h>
+#include "prio_heap.h"
-int heap_init(struct ptr_heap *heap, size_t size,
- gfp_t gfpmask, int gt(void *a, void *b))
+#ifdef DEBUG_HEAP
+void check_heap(const struct ptr_heap *heap)
{
- WARN_ON_ONCE(size == 0);
- heap->ptrs = kmalloc(size * sizeof(void *), gfpmask);
- if (!heap->ptrs)
+ 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) >> 1;
+}
+
+static
+size_t left(size_t i)
+{
+ return (i << 1) + 1;
+}
+
+static
+size_t right(size_t i)
+{
+ return (i << 1) + 2;
+}
+
+/*
+ * Copy of heap->ptrs pointer is invalid after heap_grow.
+ */
+static
+int heap_grow(struct ptr_heap *heap, size_t new_len)
+{
+ void **new_ptrs;
+
+ if (heap->alloc_len >= new_len)
+ return 0;
+
+ heap->alloc_len = max_t(size_t, new_len, heap->alloc_len << 1);
+ new_ptrs = kmalloc(heap->alloc_len * sizeof(void *), heap->gfpmask);
+ if (!new_ptrs)
return -ENOMEM;
- heap->size = 0;
- heap->max = size;
- heap->gt = gt;
+ if (heap->ptrs)
+ memcpy(new_ptrs, heap->ptrs, heap->len * sizeof(void *));
+ kfree(heap->ptrs);
+ heap->ptrs = new_ptrs;
+ return 0;
+}
+
+static
+int heap_set_len(struct ptr_heap *heap, size_t new_len)
+{
+ int ret;
+
+ ret = heap_grow(heap, new_len);
+ if (ret)
+ return ret;
+ heap->len = new_len;
return 0;
}
+int heap_init(struct ptr_heap *heap, size_t alloc_len,
+ gfp_t gfpmask, int gt(void *a, void *b))
+{
+ heap->ptrs = NULL;
+ heap->len = 0;
+ heap->alloc_len = 0;
+ heap->gt = gt;
+ /*
+ * Minimum size allocated is 1 entry to ensure memory allocation
+ * never fails within heap_replace_max.
+ */
+ return heap_grow(heap, max_t(size_t, 1, alloc_len));
+}
+
void heap_free(struct ptr_heap *heap)
{
kfree(heap->ptrs);
}
-static void heapify(struct ptr_heap *heap, int pos)
+static void heapify(struct ptr_heap *heap, size_t i)
{
- /* TODO */
+ void **ptrs = heap->ptrs;
+ size_t l, r, largest;
+
+ for (;;) {
+ void *tmp;
+
+ l = left(i);
+ r = right(i);
+ if (l < heap->len && heap->gt(ptrs[l], ptrs[i]))
+ largest = l;
+ else
+ largest = i;
+ if (r < heap->len && heap->gt(ptrs[r], ptrs[largest]))
+ largest = r;
+ 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)
{
void *res;
- void **ptrs = heap->ptrs;
- if (!heap->size) {
- ptrs[0] = p;
- heap->size = 1;
+ if (!heap->len) {
+ (void) heap_set_len(heap, 1);
+ heap->ptrs[0] = p;
+ check_heap(heap);
return NULL;
}
/* Replace the current max and heapify */
- res = ptrs[0];
- ptrs[0] = p;
+ res = heap->ptrs[0];
+ heap->ptrs[0] = p;
heapify(heap, 0);
return res;
}
-void *heap_insert(struct ptr_heap *heap, void *p)
+int heap_insert(struct ptr_heap *heap, void *p)
{
- void **ptrs = heap->ptrs;
- void *tmp = NULL;
+ void **ptrs;
+ size_t pos;
+ int ret;
- 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;
+ ret = heap_set_len(heap, heap->len + 1);
+ if (ret)
+ return ret;
+ ptrs = heap->ptrs;
+ pos = heap->len - 1;
+ 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);
}
- return tmp;
+ ptrs[pos] = p;
+ check_heap(heap);
+ return 0;
}
void *heap_remove(struct ptr_heap *heap)
{
- void **ptrs = heap->ptrs;
-
- switch (heap->size) {
+ switch (heap->len) {
case 0:
return NULL;
case 1:
- heap->size = 0;
- return ptrs[0];
+ (void) heap_set_len(heap, 0);
+ return heap->ptrs[0];
}
-
/* Shrink, replace the current max by previous last entry and heapify */
- return heap_replace_max(heap, ptrs[--heap->size]);
+ heap_set_len(heap, 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)
{
- void **ptrs = heap->ptrs;
- size_t pos, size = heap->size;
+ size_t pos, len = heap->len;
- for (pos = 0; pos < size; pos++)
- if (ptrs[pos] == p)
+ for (pos = 0; pos < len; pos++)
+ if (heap->ptrs[pos] == p)
goto found;
return NULL;
found:
- if (heap->size == 1) {
- heap->size = 0;
- return ptrs[0];
+ 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.
- */
- ptrs[pos] = ptrs[--heap->size];
+ /* Replace p with previous last entry and heapify. */
+ heap_set_len(heap, heap->len - 1);
+ /* len changed. previous last entry is at heap->len */
+ heap->ptrs[pos] = heap->ptrs[heap->len];
heapify(heap, pos);
return p;
}