src/lib/heap.c

   1 #include <freeradius-devel/ident.h>
   2 RCSID("$Id$")
   3
   4 #include <freeradius-devel/libradius.h>
   5 #include <freeradius-devel/heap.h>
   6
   7 /*
   8  *      A heap entry is made of a pointer to the object, which
   9  *      contains the key.  The heap itself is an array of pointers.
  10  *
  11  *      Heaps normally support only ordered insert, and extraction
  12  *      of the minimum element.  The heap entry can contain an "int"
  13  *      field that holds the entries position in the heap.  The offset
  14  *      of the field is held inside of the heap structure.
  15  */
  16
  17 struct fr_heap_t {
  18         int size;
  19         int num_elements;
  20         size_t offset;
  21         fr_heap_cmp_t cmp;
  22         void **p;
  23 };
  24
  25 /*
  26  *      First node in a heap is element 0. Children of i are 2i+1 and
  27  *      2i+2.  These macros wrap the logic, so the code is more
  28  *      descriptive.
  29  */
  30 #define HEAP_PARENT(x) ( ( (x) - 1 ) / 2 )
  31 #define HEAP_LEFT(x) ( 2*(x) + 1 )
  32 #define HEAP_RIGHT(x) ( 2*(x) + 2 )
  33 #define HEAP_SWAP(a, b) { void *_tmp = a; a = b; b = _tmp; }
  34
  35 static int fr_heap_bubble(fr_heap_t *hp, int child);
  36
  37 void fr_heap_delete(fr_heap_t *hp)
  38 {
  39         if (!hp) return;
  40
  41         free(hp->p);
  42         free(hp);
  43 }
  44
  45 fr_heap_t *fr_heap_create(fr_heap_cmp_t cmp, size_t offset)
  46 {
  47         fr_heap_t *fh;
  48
  49         if (!cmp) return NULL;
  50
  51         fh = malloc(sizeof(*fh));
  52         if (!fh) return NULL;
  53
  54         memset(fh, 0, sizeof(*fh));
  55
  56         fh->size = 2048;
  57         fh->p = malloc(sizeof(*(fh->p)) * fh->size);
  58         if (!fh->p) {
  59                 free(fh);
  60                 return NULL;
  61         }
  62
  63         fh->cmp = cmp;
  64         fh->offset = offset;
  65
  66         return fh;
  67 }
  68
  69 /*
  70  *      Insert element in heap. Normally, p != NULL, we insert p in a
  71  *      new position and bubble up. If p == NULL, then the element is
  72  *      already in place, and key is the position where to start the
  73  *      bubble-up.
  74  *
  75  *      Returns 1 on failure (cannot allocate new heap entry)
  76  *
  77  *      If offset > 0 the position (index, int) of the element in the
  78  *      heap is also stored in the element itself at the given offset
  79  *      in bytes.
  80  */
  81 #define SET_OFFSET(heap, node) \
  82     if (heap->offset) \
  83             *((int *)(((uint8_t *)heap->p[node]) + heap->offset)) = node
  84
  85 /*
  86  *      RESET_OFFSET is used for sanity checks. It sets offset to an
  87  *      invalid value.
  88  */
  89 #define RESET_OFFSET(heap, node) \
  90     if (heap->offset) \
  91             *((int *)(((uint8_t *)heap->p[node]) + heap->offset)) = -1
  92
  93 int fr_heap_insert(fr_heap_t *hp, void *data)
  94 {
  95         int child = hp->num_elements;
  96
  97         /*
  98          *      Heap is full.  Double it's size.
  99          */
 100         if (child == hp->size) {
 101                 void **p;
 102
 103                 p = malloc(2 * hp->size * sizeof(*p));
 104                 if (!p) return 0;
 105
 106                 memcpy(p, hp->p, sizeof(*p) * hp->size);
 107                 free(hp->p);
 108                 hp->p = p;
 109                 hp->size *= 2;
 110         }
 111
 112         hp->p[child] = data;
 113         hp->num_elements++;
 114
 115         return fr_heap_bubble(hp, child);
 116 }
 117
 118
 119 static int fr_heap_bubble(fr_heap_t *hp, int child)
 120 {
 121         /*
 122          *      Bubble up the element.
 123          */
 124         while (child > 0) {
 125                 int parent = HEAP_PARENT(child);
 126
 127                 /*
 128                  *      Parent is smaller than the child.  We're done.
 129                  */
 130                 if (hp->cmp(hp->p[parent], hp->p[child]) < 0) break;
 131
 132                 /*
 133                  *      Child is smaller than the parent, repeat.
 134                  */
 135                 HEAP_SWAP(hp->p[child], hp->p[parent]);
 136                 SET_OFFSET(hp, child);
 137                 child = parent;
 138         }
 139         SET_OFFSET(hp, child);
 140
 141         return 1;
 142 }
 143
 144
 145 /*
 146  *      Remove the top element, or object.
 147  */
 148 int fr_heap_extract(fr_heap_t *hp, void *data)
 149 {
 150         int child, parent;
 151         int max;
 152
 153         if (!hp || (hp->num_elements == 0)) return 0;
 154
 155         max = hp->num_elements - 1;
 156
 157         /*
 158          *      Extract element.  Default is the first one.
 159          */
 160         if (!data) {
 161                 parent = 0;
 162
 163         } else {                /* extract from the middle */
 164                 if (!hp->offset) return 0;
 165
 166                 parent = *((int *)(((uint8_t *)data) + hp->offset));
 167
 168                 /*
 169                  *      Out of bounds.
 170                  */
 171                 if (parent < 0 || parent >= hp->num_elements) return 0;
 172         }
 173
 174         RESET_OFFSET(hp, parent);
 175         child = HEAP_LEFT(parent);
 176         while (child <= max) {
 177                 /*
 178                  *      Maybe take the right child.
 179                  */
 180                 if ((child != max) &&
 181                     (hp->cmp(hp->p[child + 1], hp->p[child]) < 0)) {
 182                         child = child + 1;
 183                 }
 184                 hp->p[parent] = hp->p[child];
 185                 SET_OFFSET(hp, parent);
 186                 parent = child;
 187                 child = HEAP_LEFT(child);
 188         }
 189         hp->num_elements--;
 190
 191         /*
 192          *      We didn't end up at the last element in the heap.
 193          *      This element has to be re-inserted.
 194          */
 195         if (parent != max) {
 196                 /*
 197                  *      Fill hole with last entry and bubble up,
 198                  *      reusing the insert code
 199                  */
 200                 hp->p[parent] = hp->p[max];
 201                 return fr_heap_bubble(hp, parent);
 202         }
 203
 204         return 1;
 205 }
 206
 207
 208 void *fr_heap_peek(fr_heap_t *hp)
 209 {
 210         if (!hp || (hp->num_elements == 0)) return NULL;
 211
 212         /*
 213          *      If this is NULL, we have a problem.
 214          */
 215         return hp->p[0];
 216 }
 217
 218 int fr_heap_num_elements(fr_heap_t *hp)
 219 {
 220         if (!hp) return 0;
 221
 222         return hp->num_elements;
 223 }
 224
 225
 226 #ifdef TESTING
 227 /*
 228  *  cc -g -DTESTING -I .. heap.c -o heap
 229  *
 230  *  ./heap
 231  */
 232
 233 #include <stdio.h>
 234 #include <stdlib.h>
 235
 236
 237 static int heap_cmp(const void *a, const void *b)
 238 {
 239         return *(int *)a - *(int *) b;
 240
 241 }
 242
 243
 244 int main(int argc, char **arg)
 245 {
 246         fr_heap_t *hp;
 247         int i, array[1024];
 248
 249         hp = fr_heap_create(heap_cmp, 0);
 250         if (!hp) {
 251                 fprintf(stderr, "Failed creating heap!\n");
 252                 exit(1);
 253         }
 254
 255         for (i = 0; i < 1024; i++) {
 256                 array[i] = (i * 257) % 65537;
 257                 if (!fr_heap_insert(hp, &array[i])) {
 258                         fprintf(stderr, "Failed inserting %d\n", i);
 259                         exit(1);
 260                 }
 261         }
 262
 263         for (i = 0; i < 1024; i++) {
 264                 int *p = fr_heap_peek(hp);
 265
 266                 if (!p) {
 267                         fprintf(stderr, "Failed peeking %d\n", i);
 268                         exit(1);
 269                 }
 270
 271                 printf("%d\t%d\n", i, *p);
 272
 273                 if (!fr_heap_extract(hp, NULL)) {
 274                         fprintf(stderr, "Failed extracting %d\n", i);
 275                         exit(1);
 276                 }
 277         }
 278
 279         fr_heap_delete(hp);
 280
 281         return 0;
 282 }
 283 #endif