drivers/staging/lttng/lib/prio_heap/lttng_prio_heap.c

   1 /*
   2  * lttng_prio_heap.c
   3  *
   4  * Priority heap containing pointers. Based on CLRS, chapter 6.
   5  *
   6  * Copyright 2011 - Mathieu Desnoyers <mathieu.desnoyers@efficios.com>
   7  *
   8  * Permission is hereby granted, free of charge, to any person obtaining a copy
   9  * of this software and associated documentation files (the "Software"), to deal
  10  * in the Software without restriction, including without limitation the rights
  11  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  12  * copies of the Software, and to permit persons to whom the Software is
  13  * furnished to do so, subject to the following conditions:
  14  *
  15  * The above copyright notice and this permission notice shall be included in
  16  * all copies or substantial portions of the Software.
  17  *
  18  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  19  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  20  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  21  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  22  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  23  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  24  * SOFTWARE.
  25  */
  26
  27 #include <linux/slab.h>
  28 #include "lttng_prio_heap.h"
  29
  30 #ifdef DEBUG_HEAP
  31 void lttng_check_heap(const struct lttng_ptr_heap *heap)
  32 {
  33         size_t i;
  34
  35         if (!heap->len)
  36                 return;
  37
  38         for (i = 1; i < heap->len; i++)
  39                 WARN_ON_ONCE(!heap->gt(heap->ptrs[i], heap->ptrs[0]));
  40 }
  41 #endif
  42
  43 static
  44 size_t parent(size_t i)
  45 {
  46         return (i -1) >> 1;
  47 }
  48
  49 static
  50 size_t left(size_t i)
  51 {
  52         return (i << 1) + 1;
  53 }
  54
  55 static
  56 size_t right(size_t i)
  57 {
  58         return (i << 1) + 2;
  59 }
  60
  61 /*
  62  * Copy of heap->ptrs pointer is invalid after heap_grow.
  63  */
  64 static
  65 int heap_grow(struct lttng_ptr_heap *heap, size_t new_len)
  66 {
  67         void **new_ptrs;
  68
  69         if (heap->alloc_len >= new_len)
  70                 return 0;
  71
  72         heap->alloc_len = max_t(size_t, new_len, heap->alloc_len << 1);
  73         new_ptrs = kmalloc(heap->alloc_len * sizeof(void *), heap->gfpmask);
  74         if (!new_ptrs)
  75                 return -ENOMEM;
  76         if (heap->ptrs)
  77                 memcpy(new_ptrs, heap->ptrs, heap->len * sizeof(void *));
  78         kfree(heap->ptrs);
  79         heap->ptrs = new_ptrs;
  80         return 0;
  81 }
  82
  83 static
  84 int heap_set_len(struct lttng_ptr_heap *heap, size_t new_len)
  85 {
  86         int ret;
  87
  88         ret = heap_grow(heap, new_len);
  89         if (ret)
  90                 return ret;
  91         heap->len = new_len;
  92         return 0;
  93 }
  94
  95 int lttng_heap_init(struct lttng_ptr_heap *heap, size_t alloc_len,
  96               gfp_t gfpmask, int gt(void *a, void *b))
  97 {
  98         heap->ptrs = NULL;
  99         heap->len = 0;
 100         heap->alloc_len = 0;
 101         heap->gt = gt;
 102         heap->gfpmask = gfpmask;
 103         /*
 104          * Minimum size allocated is 1 entry to ensure memory allocation
 105          * never fails within heap_replace_max.
 106          */
 107         return heap_grow(heap, max_t(size_t, 1, alloc_len));
 108 }
 109
 110 void lttng_heap_free(struct lttng_ptr_heap *heap)
 111 {
 112         kfree(heap->ptrs);
 113 }
 114
 115 static void heapify(struct lttng_ptr_heap *heap, size_t i)
 116 {
 117         void **ptrs = heap->ptrs;
 118         size_t l, r, largest;
 119
 120         for (;;) {
 121                 void *tmp;
 122
 123                 l = left(i);
 124                 r = right(i);
 125                 if (l < heap->len && heap->gt(ptrs[l], ptrs[i]))
 126                         largest = l;
 127                 else
 128                         largest = i;
 129                 if (r < heap->len && heap->gt(ptrs[r], ptrs[largest]))
 130                         largest = r;
 131                 if (largest == i)
 132                         break;
 133                 tmp = ptrs[i];
 134                 ptrs[i] = ptrs[largest];
 135                 ptrs[largest] = tmp;
 136                 i = largest;
 137         }
 138         lttng_check_heap(heap);
 139 }
 140
 141 void *lttng_heap_replace_max(struct lttng_ptr_heap *heap, void *p)
 142 {
 143         void *res;
 144
 145         if (!heap->len) {
 146                 (void) heap_set_len(heap, 1);
 147                 heap->ptrs[0] = p;
 148                 lttng_check_heap(heap);
 149                 return NULL;
 150         }
 151
 152         /* Replace the current max and heapify */
 153         res = heap->ptrs[0];
 154         heap->ptrs[0] = p;
 155         heapify(heap, 0);
 156         return res;
 157 }
 158
 159 int lttng_heap_insert(struct lttng_ptr_heap *heap, void *p)
 160 {
 161         void **ptrs;
 162         size_t pos;
 163         int ret;
 164
 165         ret = heap_set_len(heap, heap->len + 1);
 166         if (ret)
 167                 return ret;
 168         ptrs = heap->ptrs;
 169         pos = heap->len - 1;
 170         while (pos > 0 && heap->gt(p, ptrs[parent(pos)])) {
 171                 /* Move parent down until we find the right spot */
 172                 ptrs[pos] = ptrs[parent(pos)];
 173                 pos = parent(pos);
 174         }
 175         ptrs[pos] = p;
 176         lttng_check_heap(heap);
 177         return 0;
 178 }
 179
 180 void *lttng_heap_remove(struct lttng_ptr_heap *heap)
 181 {
 182         switch (heap->len) {
 183         case 0:
 184                 return NULL;
 185         case 1:
 186                 (void) heap_set_len(heap, 0);
 187                 return heap->ptrs[0];
 188         }
 189         /* Shrink, replace the current max by previous last entry and heapify */
 190         heap_set_len(heap, heap->len - 1);
 191         /* len changed. previous last entry is at heap->len */
 192         return lttng_heap_replace_max(heap, heap->ptrs[heap->len]);
 193 }
 194
 195 void *lttng_heap_cherrypick(struct lttng_ptr_heap *heap, void *p)
 196 {
 197         size_t pos, len = heap->len;
 198
 199         for (pos = 0; pos < len; pos++)
 200                 if (heap->ptrs[pos] == p)
 201                         goto found;
 202         return NULL;
 203 found:
 204         if (heap->len == 1) {
 205                 (void) heap_set_len(heap, 0);
 206                 lttng_check_heap(heap);
 207                 return heap->ptrs[0];
 208         }
 209         /* Replace p with previous last entry and heapify. */
 210         heap_set_len(heap, heap->len - 1);
 211         /* len changed. previous last entry is at heap->len */
 212         heap->ptrs[pos] = heap->ptrs[heap->len];
 213         heapify(heap, pos);
 214         return p;
 215 }