shared: add simple priority queue implementation

5 files changed, 500 insertions, 1 deletions

diff --git a/.gitignore b/.gitignore
index 2d95f149c0..c3bb81b1f7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -111,6 +111,7 @@
 /test-loopback
 /test-mmap-cache
 /test-ns
+/test-prioq
 /test-replace-var
 /test-sched-prio
 /test-sleep
diff --git a/Makefile.am b/Makefile.am
index d8e9b16be0..13eba27fed 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -620,6 +620,8 @@ libsystemd_shared_la_SOURCES = \
 	src/shared/set.h \
 	src/shared/fdset.c \
 	src/shared/fdset.h \
+	src/shared/prioq.c \
+	src/shared/prioq.h \
 	src/shared/strv.c \
 	src/shared/strv.h \
 	src/shared/env-util.c \
@@ -1075,7 +1077,8 @@ noinst_tests += \
 	test-sched-prio \
 	test-calendarspec \
 	test-strip-tab-ansi \
-	test-cgroup-util
+	test-cgroup-util \
+	test-prioq
 
 EXTRA_DIST += \
 	test/sched_idle_bad.service \
@@ -1166,6 +1169,15 @@ test_util_CFLAGS = \
 test_util_LDADD = \
 	libsystemd-core.la
 
+test_prioq_SOURCES = \
+	src/test/test-prioq.c
+
+test_prioq_CFLAGS = \
+	$(AM_CFLAGS)
+
+test_prioq_LDADD = \
+	libsystemd-core.la
+
 test_log_SOURCES = \
 	src/test/test-log.c
 
diff --git a/src/shared/prioq.c b/src/shared/prioq.c
new file mode 100644
index 0000000000..fe91324d9b
--- /dev/null
+++ b/src/shared/prioq.c
@@ -0,0 +1,281 @@
+/*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
+
+/***
+  This file is part of systemd.
+
+  Copyright 2013 Lennart Poettering
+
+  systemd is free software; you can redistribute it and/or modify it
+  under the terms of the GNU Lesser General Public License as published by
+  the Free Software Foundation; either version 2.1 of the License, or
+  (at your option) any later version.
+
+  systemd is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with systemd; If not, see <http://www.gnu.org/licenses/>.
+***/
+
+#include "util.h"
+#include "prioq.h"
+
+struct prioq_item {
+        void *data;
+        unsigned *idx;
+};
+
+struct Prioq {
+        compare_func_t compare_func;
+        unsigned n_items, n_allocated;
+
+        struct prioq_item *items;
+};
+
+Prioq *prioq_new(compare_func_t compare_func) {
+        Prioq *q;
+
+        q = new0(Prioq, 1);
+        if (!q)
+                return q;
+
+        q->compare_func = compare_func;
+        return q;
+}
+
+void prioq_free(Prioq *q) {
+        if (!q)
+                return;
+
+        free(q->items);
+        free(q);
+}
+
+static void swap(Prioq *q, unsigned j, unsigned k) {
+        void *saved_data;
+        unsigned *saved_idx;
+
+        assert(q);
+        assert(j < q->n_items);
+        assert(k < q->n_items);
+
+        assert(!q->items[j].idx || *(q->items[j].idx) == j);
+        assert(!q->items[k].idx || *(q->items[k].idx) == k);
+
+        saved_data = q->items[j].data;
+        saved_idx = q->items[j].idx;
+        q->items[j].data = q->items[k].data;
+        q->items[j].idx = q->items[k].idx;
+        q->items[k].data = saved_data;
+        q->items[k].idx = saved_idx;
+
+        if (q->items[j].idx)
+                *q->items[j].idx = j;
+
+        if (q->items[k].idx)
+                *q->items[k].idx = k;
+}
+
+static unsigned shuffle_up(Prioq *q, unsigned idx) {
+        assert(q);
+
+        while (idx > 0) {
+                unsigned k;
+
+                k = (idx-1)/2;
+
+                if (q->compare_func(q->items[k].data, q->items[idx].data) < 0)
+                        break;
+
+                swap(q, idx, k);
+                idx = k;
+        }
+
+        return idx;
+}
+
+static unsigned shuffle_down(Prioq *q, unsigned idx) {
+        assert(q);
+
+        for (;;) {
+                unsigned j, k, s;
+
+                k = (idx+1)*2; /* right child */
+                j = k-1;       /* left child */
+
+                if (j >= q->n_items)
+                        break;
+
+                if (q->compare_func(q->items[j].data, q->items[idx].data) < 0)
+
+                        /* So our left child is smaller than we are, let's
+                         * remember this fact */
+                        s = j;
+                else
+                        s = idx;
+
+                if (k < q->n_items &&
+                    q->compare_func(q->items[k].data, q->items[s].data) < 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;
+}
+
+int prioq_put(Prioq *q, void *data, unsigned *idx) {
+        struct prioq_item *i;
+        unsigned k;
+
+        assert(q);
+
+        if (q->n_items >= q->n_allocated) {
+                unsigned n;
+                struct prioq_item *j;
+
+                n = MAX((q->n_items+1) * 2, 16);
+                j = realloc(q->items, sizeof(struct prioq_item) * n);
+                if (!j)
+                        return -ENOMEM;
+
+                q->items = j;
+                q->n_allocated = n;
+        }
+
+        k = q->n_items++;
+        i = q->items + k;
+        i->data = data;
+        i->idx = idx;
+
+        if (idx)
+                *idx = k;
+
+        shuffle_up(q, k);
+
+        return 0;
+}
+
+static void remove_item(Prioq *q, struct prioq_item *i) {
+        struct prioq_item *l;
+
+        assert(q);
+        assert(i);
+
+        l = q->items + q->n_items - 1;
+
+        if (i == l)
+                /* Last entry, let's just remove it */
+                q->n_items--;
+        else {
+                unsigned k;
+
+                /* Not last entry, let's replace the last entry with
+                 * this one, and reshuffle */
+
+                k = i - q->items;
+
+                i->data = l->data;
+                i->idx = l->idx;
+                if (i->idx)
+                        *i->idx = k;
+                q->n_items--;
+
+                k = shuffle_down(q, k);
+                shuffle_up(q, k);
+        }
+}
+
+static struct prioq_item* find_item(Prioq *q, void *data, unsigned *idx) {
+        struct prioq_item *i;
+
+        assert(q);
+
+        if (idx) {
+                assert(*idx < q->n_items);
+                i = q->items + *idx;
+                assert(i->data == data);
+
+                return i;
+        } else {
+                for (i = q->items; i < q->items + q->n_items; i++)
+                        if (i->data == data)
+                                return i;
+                return NULL;
+        }
+}
+
+int prioq_remove(Prioq *q, void *data, unsigned *idx) {
+        struct prioq_item *i;
+
+        assert(q);
+
+        i = find_item(q, data, idx);
+        if (!i)
+                return 0;
+
+        remove_item(q, i);
+        return 1;
+}
+
+int prioq_reshuffle(Prioq *q, void *data, unsigned *idx) {
+        struct prioq_item *i;
+        unsigned k;
+
+        assert(q);
+
+        i = find_item(q, data, idx);
+        if (!i)
+                return 0;
+
+        k = i - q->items;
+        k = shuffle_down(q, k);
+        shuffle_up(q, k);
+        return 1;
+}
+
+void *prioq_peek(Prioq *q) {
+        assert(q);
+
+        if (q->n_items <= 0)
+                return NULL;
+
+        return q->items[0].data;
+}
+
+void *prioq_pop(Prioq *q) {
+        void *data;
+
+        assert(q);
+
+        if (q->n_items <= 0)
+                return NULL;
+
+        data = q->items[0].data;
+        remove_item(q, q->items);
+        return data;
+}
+
+unsigned prioq_size(Prioq *q) {
+        assert(q);
+
+        return q->n_items;
+
+}
+bool prioq_isempty(Prioq *q) {
+        assert(q);
+
+        return q->n_items <= 0;
+}
diff --git a/src/shared/prioq.h b/src/shared/prioq.h
new file mode 100644
index 0000000000..1b5b05e7c7
--- /dev/null
+++ b/src/shared/prioq.h
@@ -0,0 +1,39 @@
+/*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
+
+#pragma once
+
+/***
+  This file is part of systemd.
+
+  Copyright 2013 Lennart Poettering
+
+  systemd is free software; you can redistribute it and/or modify it
+  under the terms of the GNU Lesser General Public License as published by
+  the Free Software Foundation; either version 2.1 of the License, or
+  (at your option) any later version.
+
+  systemd is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with systemd; If not, see <http://www.gnu.org/licenses/>.
+***/
+
+#include "hashmap.h"
+
+typedef struct Prioq Prioq;
+
+Prioq *prioq_new(compare_func_t compare);
+void prioq_free(Prioq *q);
+
+int prioq_put(Prioq *q, void *data, unsigned *idx);
+int prioq_remove(Prioq *q, void *data, unsigned *idx);
+int prioq_reshuffle(Prioq *q, void *data, unsigned *idx);
+
+void *prioq_peek(Prioq *q);
+void *prioq_pop(Prioq *q);
+
+unsigned prioq_size(Prioq *q);
+bool prioq_isempty(Prioq *q);
diff --git a/src/test/test-prioq.c b/src/test/test-prioq.c
new file mode 100644
index 0000000000..73c640840a
--- /dev/null
+++ b/src/test/test-prioq.c
@@ -0,0 +1,166 @@
+/*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
+
+/***
+  This file is part of systemd.
+
+  Copyright 2013 Lennart Poettering
+
+  systemd is free software; you can redistribute it and/or modify it
+  under the terms of the GNU Lesser General Public License as published by
+  the Free Software Foundation; either version 2.1 of the License, or
+  (at your option) any later version.
+
+  systemd is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with systemd; If not, see <http://www.gnu.org/licenses/>.
+***/
+
+#include <stdlib.h>
+
+#include "util.h"
+#include "set.h"
+#include "prioq.h"
+
+#define SET_SIZE 1024*4
+
+static int unsigned_compare(const void *a, const void *b) {
+        const unsigned *x = a, *y = b;
+
+        if (*x < *y)
+                return -1;
+
+        if (*x > *y)
+                return 1;
+
+        return 0;
+}
+
+static void test_unsigned(void) {
+        unsigned buffer[SET_SIZE], i;
+        Prioq *q;
+
+        srand(0);
+
+        q = prioq_new(trivial_compare_func);
+        assert_se(q);
+
+        for (i = 0; i < ELEMENTSOF(buffer); i++) {
+                unsigned u;
+
+                u = (unsigned) rand();
+                buffer[i] = u;
+                assert_se(prioq_put(q, UINT_TO_PTR(u), NULL) >= 0);
+        }
+
+        qsort(buffer, ELEMENTSOF(buffer), sizeof(buffer[0]), unsigned_compare);
+
+        for (i = 0; i < ELEMENTSOF(buffer); i++) {
+                unsigned u;
+
+                assert_se(prioq_size(q) == ELEMENTSOF(buffer) - i);
+
+                u = PTR_TO_UINT(prioq_pop(q));
+                assert_se(buffer[i] == u);
+        }
+
+        assert_se(prioq_isempty(q));
+        prioq_free(q);
+}
+
+struct test {
+        unsigned value;
+        unsigned idx;
+};
+
+static int test_compare(const void *a, const void *b) {
+        const struct test *x = a, *y = b;
+
+        if (x->value < y->value)
+                return -1;
+
+        if (x->value > y->value)
+                return 1;
+
+        return 0;
+}
+
+static unsigned test_hash(const void *a) {
+        const struct test *x = a;
+
+        return x->value;
+}
+
+static void test_struct(void) {
+        Prioq *q;
+        Set *s;
+        unsigned previous = 0, i;
+        int r;
+
+        srand(0);
+
+        q = prioq_new(test_compare);
+        assert_se(q);
+
+        s = set_new(test_hash, test_compare);
+        assert_se(s);
+
+        for (i = 0; i < SET_SIZE; i++) {
+                struct test *t;
+
+                t = new0(struct test, 1);
+                assert_se(t);
+                t->value = (unsigned) rand();
+
+                r = prioq_put(q, t, &t->idx);
+                assert_se(r >= 0);
+
+                if (i % 4 == 0) {
+                        r = set_put(s, t);
+                        assert_se(r >= 0);
+                }
+        }
+
+        for (;;) {
+                struct test *t;
+
+                t = set_steal_first(s);
+                if (!t)
+                        break;
+
+                r = prioq_remove(q, t, &t->idx);
+                assert_se(r > 0);
+
+                free(t);
+        }
+
+        for (i = 0; i < SET_SIZE * 3 / 4; i++) {
+                struct test *t;
+
+                assert_se(prioq_size(q) == (SET_SIZE * 3 / 4) - i);
+
+                t = prioq_pop(q);
+                assert_se(t);
+
+                assert_se(previous <= t->value);
+                previous = t->value;
+                free(t);
+        }
+
+        assert_se(prioq_isempty(q));
+        prioq_free(q);
+
+        assert_se(set_isempty(s));
+        set_free(s);
+}
+
+int main(int argc, char* argv[]) {
+
+        test_unsigned();
+        test_struct();
+
+        return 0;
+}