glib-aux: add NMPrioq priority queue (heap)

Taken from systemd's "Prioq".

Differences from Prioq:

- It is glib-ized, so certain operations cannot fail since g_malloc()
  never fails.

- Unlike Prioq, this structure is stack allocated. I think that makes
  sense, because we basically always want to embed the data structure
  in another object. There is never a need for passing this around as a
  pointer. And if you really want, you can box it yourself.

- The queue either accepts a GCompareFunc or a GComareDataFunc. This
  is for convenience. The prioq_ensure_allocated() and
  prioq_ensure_put() consequently are dropped, as they would be
  cumbersome with this pattern and don't seem useful.

Makefile.am

@@ -454,6 +454,8 @@ src_libnm_glib_aux_libnm_glib_aux_la_SOURCES = \
 	src/libnm-glib-aux/nm-logging-syslog.h \
 	src/libnm-glib-aux/nm-macros-internal.h \
 	src/libnm-glib-aux/nm-obj.h \
+	src/libnm-glib-aux/nm-prioq.c \
+	src/libnm-glib-aux/nm-prioq.h \
 	src/libnm-glib-aux/nm-random-utils.c \
 	src/libnm-glib-aux/nm-random-utils.h \
 	src/libnm-glib-aux/nm-ref-string.c \

src/libnm-glib-aux/meson.build

@@ -13,6 +13,7 @@ libnm_glib_aux = static_library(
     'nm-json-aux.c',
     'nm-keyfile-aux.c',
     'nm-logging-base.c',
+    'nm-prioq.c',
     'nm-random-utils.c',
     'nm-ref-string.c',
     'nm-secret-utils.c',

src/libnm-glib-aux/nm-prioq.c

@@ -0,0 +1,323 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+
+/*
+ * Taken from systemd's Prioq.
+ *
+ * Priority Queue
+ * The prioq object implements a priority queue. That is, it orders objects by
+ * their priority and allows O(1) access to the object with the highest
+ * priority. Insertion and removal are Θ(log n). Optionally, the caller can
+ * provide a pointer to an index which will be kept up-to-date by the prioq.
+ *
+ * The underlying algorithm used in this implementation is a Heap.
+ */
+
+#include "libnm-glib-aux/nm-default-glib-i18n-lib.h"
+
+#include "nm-prioq.h"
+
+#include <errno.h>
+#include <stdlib.h>
+
+/*****************************************************************************/
+
+struct _NMPrioqItem {
+    void     *data;
+    unsigned *idx;
+};
+
+/*****************************************************************************/
+
+void
+nm_prioq_init(NMPrioq *q, GCompareFunc compare_func)
+{
+    nm_assert(q);
+    nm_assert(compare_func);
+
+    *q = (NMPrioq){
+        ._priv =
+            {
+                .compare_func      = compare_func,
+                .compare_data      = NULL,
+                .compare_with_data = FALSE,
+                .items             = NULL,
+                .n_items           = 0,
+                .n_allocated       = 0,
+            },
+    };
+}
+
+void
+nm_prioq_init_with_data(NMPrioq *q, GCompareDataFunc compare_func, gpointer compare_data)
+{
+    nm_assert(q);
+    nm_assert(compare_func);
+
+    *q = (NMPrioq){
+        ._priv =
+            {
+                .compare_data_func = compare_func,
+                .compare_data      = compare_data,
+                .compare_with_data = TRUE,
+                .items             = NULL,
+                .n_items           = 0,
+                .n_allocated       = 0,
+            },
+    };
+}
+
+void
+nm_prioq_destroy(NMPrioq *q)
+{
+    if (!q || !q->_priv.compare_func)
+        return;
+
+    free(q->_priv.items);
+    q->_priv.compare_func = NULL;
+}
+
+/*****************************************************************************/
+
+static int
+compare(NMPrioq *q, unsigned a, unsigned b)
+{
+    nm_assert(q);
+    nm_assert(q->_priv.compare_func);
+    nm_assert(a != b);
+    nm_assert(a < q->_priv.n_items);
+    nm_assert(b < q->_priv.n_items);
+
+    if (q->_priv.compare_with_data) {
+        return q->_priv.compare_data_func(q->_priv.items[a].data,
+                                          q->_priv.items[b].data,
+                                          q->_priv.compare_data);
+    }
+
+    return q->_priv.compare_func(q->_priv.items[a].data, q->_priv.items[b].data);
+}
+
+static void
+swap(NMPrioq *q, unsigned j, unsigned k)
+{
+    nm_assert(q);
+    nm_assert(j < q->_priv.n_items);
+    nm_assert(k < q->_priv.n_items);
+
+    nm_assert(!q->_priv.items[j].idx || *(q->_priv.items[j].idx) == j);
+    nm_assert(!q->_priv.items[k].idx || *(q->_priv.items[k].idx) == k);
+
+    NM_SWAP(&q->_priv.items[j].data, &q->_priv.items[k].data);
+    NM_SWAP(&q->_priv.items[j].idx, &q->_priv.items[k].idx);
+
+    if (q->_priv.items[j].idx)
+        *q->_priv.items[j].idx = j;
+
+    if (q->_priv.items[k].idx)
+        *q->_priv.items[k].idx = k;
+}
+
+static unsigned
+shuffle_up(NMPrioq *q, unsigned idx)
+{
+    nm_assert(q);
+    nm_assert(idx < q->_priv.n_items);
+
+    while (idx > 0) {
+        unsigned k;
+
+        k = (idx - 1) / 2;
+
+        if (compare(q, k, idx) <= 0)
+            break;
+
+        swap(q, idx, k);
+        idx = k;
+    }
+
+    return idx;
+}
+
+static unsigned
+shuffle_down(NMPrioq *q, unsigned idx)
+{
+    nm_assert(q);
+
+    for (;;) {
+        unsigned j;
+        unsigned k;
+        unsigned s;
+
+        k = (idx + 1) * 2; /* right child */
+        j = k - 1;         /* left child */
+
+        if (j >= q->_priv.n_items)
+            break;
+
+        if (compare(q, j, idx) < 0) {
+            /* So our left child is smaller than we are, let's
+             * remember this fact */
+            s = j;
+        } else
+            s = idx;
+
+        if ((k < q->_priv.n_items) && compare(q, k, s) < 0) {
+            /* So our right child is smaller than we are, let's
+             * remember this fact */
+            s = k;
+        }
+
+        /* s now points to the smallest of the three items */
+
+        if (s == idx)
+            /* No swap necessary, we're done */
+            break;
+
+        swap(q, idx, s);
+        idx = s;
+    }
+
+    return idx;
+}
+
+void
+nm_prioq_put(NMPrioq *q, void *data, unsigned *idx)
+{
+    unsigned k;
+
+    nm_assert(q);
+
+    if (q->_priv.n_items >= q->_priv.n_allocated) {
+        q->_priv.n_allocated = NM_MAX((q->_priv.n_items + 1u) * 2u, 16u);
+        q->_priv.items       = g_renew(struct _NMPrioqItem, q->_priv.items, q->_priv.n_allocated);
+    }
+
+    k = q->_priv.n_items++;
+
+    q->_priv.items[k] = (struct _NMPrioqItem){
+        .data = data,
+        .idx  = idx,
+    };
+    if (idx)
+        *idx = k;
+
+    shuffle_up(q, k);
+}
+
+static void
+remove_item(NMPrioq *q, struct _NMPrioqItem *i)
+{
+    struct _NMPrioqItem *l;
+    unsigned             k;
+
+    nm_assert(q);
+    nm_assert(i);
+    nm_assert(q->_priv.n_items > 0);
+    nm_assert(i >= q->_priv.items);
+    nm_assert(i < &q->_priv.items[q->_priv.n_items]);
+
+    l = &q->_priv.items[q->_priv.n_items - 1u];
+
+    if (i == l) {
+        /* Last entry, let's just remove it */
+        q->_priv.n_items--;
+        return;
+    }
+
+    /* Not last entry, let's replace the last entry with
+     * this one, and reshuffle */
+    k = i - q->_priv.items;
+
+    *i = *l;
+    if (i->idx)
+        *i->idx = k;
+    q->_priv.n_items--;
+
+    k = shuffle_down(q, k);
+    shuffle_up(q, k);
+}
+
+_nm_pure static struct _NMPrioqItem *
+find_item(NMPrioq *q, void *data, unsigned *idx)
+{
+    struct _NMPrioqItem *i;
+
+    nm_assert(q);
+
+    if (q->_priv.n_items <= 0)
+        return NULL;
+
+    if (idx) {
+        if (*idx == NM_PRIOQ_IDX_NULL || *idx >= q->_priv.n_items)
+            return NULL;
+
+        i = &q->_priv.items[*idx];
+        if (i->data == data)
+            return i;
+    } else {
+        for (i = q->_priv.items; i < &q->_priv.items[q->_priv.n_items]; i++) {
+            if (i->data == data)
+                return i;
+        }
+    }
+
+    return NULL;
+}
+
+gboolean
+nm_prioq_remove(NMPrioq *q, void *data, unsigned *idx)
+{
+    struct _NMPrioqItem *i;
+
+    nm_assert(q);
+
+    i = find_item(q, data, idx);
+    if (!i)
+        return FALSE;
+
+    remove_item(q, i);
+    return TRUE;
+}
+
+gboolean
+nm_prioq_reshuffle(NMPrioq *q, void *data, unsigned *idx)
+{
+    struct _NMPrioqItem *i;
+    unsigned             k;
+
+    nm_assert(q);
+
+    i = find_item(q, data, idx);
+    if (!i)
+        return FALSE;
+
+    k = i - q->_priv.items;
+    k = shuffle_down(q, k);
+    shuffle_up(q, k);
+    return TRUE;
+}
+
+void *
+nm_prioq_peek_by_index(NMPrioq *q, unsigned idx)
+{
+    nm_assert(q);
+
+    if (idx >= q->_priv.n_items)
+        return NULL;
+
+    return q->_priv.items[idx].data;
+}
+
+void *
+nm_prioq_pop(NMPrioq *q)
+{
+    void *data;
+
+    nm_assert(q);
+
+    if (q->_priv.n_items <= 0)
+        return NULL;
+
+    data = q->_priv.items[0].data;
+    remove_item(q, &q->_priv.items[0]);
+    return data;
+}

src/libnm-glib-aux/nm-prioq.h

@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+
+#ifndef __NM_PRIOQ_H__
+#define __NM_PRIOQ_H__
+
+#define NM_PRIOQ_IDX_NULL G_MAXUINT
+
+struct _NMPrioqItem;
+
+typedef struct {
+    struct {
+        union {
+            GCompareDataFunc compare_data_func;
+            GCompareFunc     compare_func;
+        };
+
+        gpointer compare_data;
+
+        struct _NMPrioqItem *items;
+
+        unsigned n_items;
+        unsigned n_allocated;
+
+        bool compare_with_data;
+    } _priv;
+} NMPrioq;
+
+#define NM_PRIOQ_ZERO             \
+    {                             \
+        ._priv = {                \
+            .compare_func = NULL, \
+        },                        \
+    }
+
+void nm_prioq_init(NMPrioq *q, GCompareFunc compare_func);
+void nm_prioq_init_with_data(NMPrioq *q, GCompareDataFunc compare_func, gpointer compare_data);
+
+void nm_prioq_destroy(NMPrioq *q);
+
+#define nm_auto_prioq nm_auto(nm_prioq_destroy)
+
+void     nm_prioq_put(NMPrioq *q, void *data, unsigned *idx);
+gboolean nm_prioq_remove(NMPrioq *q, void *data, unsigned *idx);
+gboolean nm_prioq_reshuffle(NMPrioq *q, void *data, unsigned *idx);
+
+void *nm_prioq_peek_by_index(NMPrioq *q, unsigned idx) _nm_pure;
+
+static inline void *
+nm_prioq_peek(NMPrioq *q)
+{
+    return nm_prioq_peek_by_index(q, 0);
+}
+
+void *nm_prioq_pop(NMPrioq *q);
+
+#define NM_PRIOQ_FOREACH_ITEM(q, p) for (unsigned _i = 0; (p = nm_prioq_peek_by_index(q, _i)); _i++)
+
+_nm_pure static inline unsigned
+nm_prioq_size(NMPrioq *q)
+{
+    nm_assert(q);
+    return q->_priv.n_items;
+}
+
+_nm_pure static inline gboolean
+nm_prioq_isempty(NMPrioq *q)
+{
+    return nm_prioq_size(q) <= 0;
+}
+
+#endif /* __NM_PRIOQ_H__ */

src/libnm-glib-aux/tests/test-shared-general.c

@@ -11,6 +11,7 @@
 #include "libnm-glib-aux/nm-time-utils.h"
 #include "libnm-glib-aux/nm-ref-string.h"
 #include "libnm-glib-aux/nm-io-utils.h"
+#include "libnm-glib-aux/nm-prioq.h"
 
 #include "libnm-glib-aux/nm-test-utils.h"
 
@@ -2260,6 +2261,79 @@ test_garray(void)
 
 /*****************************************************************************/
 
+static int
+_prioq_cmp(gconstpointer a, gconstpointer b)
+{
+    NM_CMP_DIRECT(GPOINTER_TO_UINT(a), GPOINTER_TO_UINT(b));
+    return 0;
+}
+
+static int
+_prioq_cmp_with_data(gconstpointer a, gconstpointer b, gpointer user_data)
+{
+    return _prioq_cmp(a, b);
+}
+
+static void
+test_nm_prioq(void)
+{
+    nm_auto_prioq NMPrioq q = NM_PRIOQ_ZERO;
+    gpointer              data[200];
+    gpointer              data_pop[200];
+    guint                 data_idx[G_N_ELEMENTS(data)];
+    guint                 i;
+    guint                 n;
+    gpointer              p;
+
+    if (nmtst_get_rand_one_case_in(10))
+        return;
+
+    if (nmtst_get_rand_bool())
+        nm_prioq_init(&q, _prioq_cmp);
+    else
+        nm_prioq_init_with_data(&q, _prioq_cmp_with_data, NULL);
+
+    g_assert(nm_prioq_size(&q) == 0);
+
+    if (nmtst_get_rand_one_case_in(10))
+        return;
+
+    for (i = 0; i < G_N_ELEMENTS(data); i++) {
+        data[i]     = GUINT_TO_POINTER((nmtst_get_rand_uint32() % G_N_ELEMENTS(data)) + 1u);
+        data_idx[i] = NM_PRIOQ_IDX_NULL;
+    }
+
+    nm_prioq_put(&q, data[0], NULL);
+    g_assert(nm_prioq_size(&q) == 1);
+
+    p = nm_prioq_pop(&q);
+    g_assert(p == data[0]);
+    g_assert(nm_prioq_size(&q) == 0);
+
+    g_assert(!nm_prioq_pop(&q));
+
+    n = nmtst_get_rand_uint32() % G_N_ELEMENTS(data);
+    for (i = 0; i < n; i++)
+        nm_prioq_put(&q, data[i], &data_idx[i]);
+
+    g_assert_cmpint(nm_prioq_size(&q), ==, n);
+
+    if (nmtst_get_rand_one_case_in(10))
+        return;
+
+    for (i = 0; i < n; i++) {
+        data_pop[i] = nm_prioq_pop(&q);
+        g_assert(data_pop[i]);
+        if (i > 0)
+            g_assert(_prioq_cmp(data_pop[i - 1], data_pop[i]) <= 0);
+    }
+
+    g_assert(!nm_prioq_pop(&q));
+    g_assert(nm_prioq_size(&q) == 0);
+}
+
+/*****************************************************************************/
+
 NMTST_DEFINE();
 
 int
@@ -2306,6 +2380,7 @@ main(int argc, char **argv)
     g_test_add_func("/general/test_hostname_is_valid", test_hostname_is_valid);
     g_test_add_func("/general/test_inet_utils", test_inet_utils);
     g_test_add_func("/general/test_garray", test_garray);
+    g_test_add_func("/general/test_nm_prioq", test_nm_prioq);
 
     return g_test_run();
 }