src/basic/prioq.c

   1 /* SPDX-License-Identifier: LGPL-2.1+ */
   2
   3 /*
   4  * Priority Queue
   5  * The prioq object implements a priority queue. That is, it orders objects by
   6  * their priority and allows O(1) access to the object with the highest
   7  * priority. Insertion and removal are Θ(log n). Optionally, the caller can
   8  * provide a pointer to an index which will be kept up-to-date by the prioq.
   9  *
  10  * The underlying algorithm used in this implementation is a Heap.
  11  */
  12
  13 #include <errno.h>
  14 #include <stdlib.h>
  15
  16 #include "alloc-util.h"
  17 #include "hashmap.h"
  18 #include "prioq.h"
  19
  20 struct prioq_item {
  21         void *data;
  22         unsigned *idx;
  23 };
  24
  25 struct Prioq {
  26         compare_func_t compare_func;
  27         unsigned n_items, n_allocated;
  28
  29         struct prioq_item *items;
  30 };
  31
  32 Prioq *prioq_new(compare_func_t compare_func) {
  33         Prioq *q;
  34
  35         q = new0(Prioq, 1);
  36         if (!q)
  37                 return q;
  38
  39         q->compare_func = compare_func;
  40         return q;
  41 }
  42
  43 Prioq* prioq_free(Prioq *q) {
  44         if (!q)
  45                 return NULL;
  46
  47         free(q->items);
  48         return mfree(q);
  49 }
  50
  51 int prioq_ensure_allocated(Prioq **q, compare_func_t compare_func) {
  52         assert(q);
  53
  54         if (*q)
  55                 return 0;
  56
  57         *q = prioq_new(compare_func);
  58         if (!*q)
  59                 return -ENOMEM;
  60
  61         return 0;
  62 }
  63
  64 static void swap(Prioq *q, unsigned j, unsigned k) {
  65         void *saved_data;
  66         unsigned *saved_idx;
  67
  68         assert(q);
  69         assert(j < q->n_items);
  70         assert(k < q->n_items);
  71
  72         assert(!q->items[j].idx || *(q->items[j].idx) == j);
  73         assert(!q->items[k].idx || *(q->items[k].idx) == k);
  74
  75         saved_data = q->items[j].data;
  76         saved_idx = q->items[j].idx;
  77         q->items[j].data = q->items[k].data;
  78         q->items[j].idx = q->items[k].idx;
  79         q->items[k].data = saved_data;
  80         q->items[k].idx = saved_idx;
  81
  82         if (q->items[j].idx)
  83                 *q->items[j].idx = j;
  84
  85         if (q->items[k].idx)
  86                 *q->items[k].idx = k;
  87 }
  88
  89 static unsigned shuffle_up(Prioq *q, unsigned idx) {
  90         assert(q);
  91         assert(idx < q->n_items);
  92
  93         while (idx > 0) {
  94                 unsigned k;
  95
  96                 k = (idx-1)/2;
  97
  98                 if (q->compare_func(q->items[k].data, q->items[idx].data) <= 0)
  99                         break;
 100
 101                 swap(q, idx, k);
 102                 idx = k;
 103         }
 104
 105         return idx;
 106 }
 107
 108 static unsigned shuffle_down(Prioq *q, unsigned idx) {
 109         assert(q);
 110
 111         for (;;) {
 112                 unsigned j, k, s;
 113
 114                 k = (idx+1)*2; /* right child */
 115                 j = k-1;       /* left child */
 116
 117                 if (j >= q->n_items)
 118                         break;
 119
 120                 if (q->compare_func(q->items[j].data, q->items[idx].data) < 0)
 121
 122                         /* So our left child is smaller than we are, let's
 123                          * remember this fact */
 124                         s = j;
 125                 else
 126                         s = idx;
 127
 128                 if (k < q->n_items &&
 129                     q->compare_func(q->items[k].data, q->items[s].data) < 0)
 130
 131                         /* So our right child is smaller than we are, let's
 132                          * remember this fact */
 133                         s = k;
 134
 135                 /* s now points to the smallest of the three items */
 136
 137                 if (s == idx)
 138                         /* No swap necessary, we're done */
 139                         break;
 140
 141                 swap(q, idx, s);
 142                 idx = s;
 143         }
 144
 145         return idx;
 146 }
 147
 148 int prioq_put(Prioq *q, void *data, unsigned *idx) {
 149         struct prioq_item *i;
 150         unsigned k;
 151
 152         assert(q);
 153
 154         if (q->n_items >= q->n_allocated) {
 155                 unsigned n;
 156                 struct prioq_item *j;
 157
 158                 n = MAX((q->n_items+1) * 2, 16u);
 159                 j = reallocarray(q->items, n, sizeof(struct prioq_item));
 160                 if (!j)
 161                         return -ENOMEM;
 162
 163                 q->items = j;
 164                 q->n_allocated = n;
 165         }
 166
 167         k = q->n_items++;
 168         i = q->items + k;
 169         i->data = data;
 170         i->idx = idx;
 171
 172         if (idx)
 173                 *idx = k;
 174
 175         shuffle_up(q, k);
 176
 177         return 0;
 178 }
 179
 180 static void remove_item(Prioq *q, struct prioq_item *i) {
 181         struct prioq_item *l;
 182
 183         assert(q);
 184         assert(i);
 185
 186         l = q->items + q->n_items - 1;
 187
 188         if (i == l)
 189                 /* Last entry, let's just remove it */
 190                 q->n_items--;
 191         else {
 192                 unsigned k;
 193
 194                 /* Not last entry, let's replace the last entry with
 195                  * this one, and reshuffle */
 196
 197                 k = i - q->items;
 198
 199                 i->data = l->data;
 200                 i->idx = l->idx;
 201                 if (i->idx)
 202                         *i->idx = k;
 203                 q->n_items--;
 204
 205                 k = shuffle_down(q, k);
 206                 shuffle_up(q, k);
 207         }
 208 }
 209
 210 _pure_ static struct prioq_item* find_item(Prioq *q, void *data, unsigned *idx) {
 211         struct prioq_item *i;
 212
 213         assert(q);
 214
 215         if (q->n_items <= 0)
 216                 return NULL;
 217
 218         if (idx) {
 219                 if (*idx == PRIOQ_IDX_NULL ||
 220                     *idx >= q->n_items)
 221                         return NULL;
 222
 223                 i = q->items + *idx;
 224                 if (i->data != data)
 225                         return NULL;
 226
 227                 return i;
 228         } else {
 229                 for (i = q->items; i < q->items + q->n_items; i++)
 230                         if (i->data == data)
 231                                 return i;
 232                 return NULL;
 233         }
 234 }
 235
 236 int prioq_remove(Prioq *q, void *data, unsigned *idx) {
 237         struct prioq_item *i;
 238
 239         if (!q)
 240                 return 0;
 241
 242         i = find_item(q, data, idx);
 243         if (!i)
 244                 return 0;
 245
 246         remove_item(q, i);
 247         return 1;
 248 }
 249
 250 int prioq_reshuffle(Prioq *q, void *data, unsigned *idx) {
 251         struct prioq_item *i;
 252         unsigned k;
 253
 254         assert(q);
 255
 256         i = find_item(q, data, idx);
 257         if (!i)
 258                 return 0;
 259
 260         k = i - q->items;
 261         k = shuffle_down(q, k);
 262         shuffle_up(q, k);
 263         return 1;
 264 }
 265
 266 void *prioq_peek(Prioq *q) {
 267
 268         if (!q)
 269                 return NULL;
 270
 271         if (q->n_items <= 0)
 272                 return NULL;
 273
 274         return q->items[0].data;
 275 }
 276
 277 void *prioq_pop(Prioq *q) {
 278         void *data;
 279
 280         if (!q)
 281                 return NULL;
 282
 283         if (q->n_items <= 0)
 284                 return NULL;
 285
 286         data = q->items[0].data;
 287         remove_item(q, q->items);
 288         return data;
 289 }
 290
 291 unsigned prioq_size(Prioq *q) {
 292
 293         if (!q)
 294                 return 0;
 295
 296         return q->n_items;
 297 }
 298
 299 bool prioq_isempty(Prioq *q) {
 300
 301         if (!q)
 302                 return true;
 303
 304         return q->n_items <= 0;
 305 }