src/basic/prioq.c

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