ipcpd: Add timerwheel

The timerwheel can be used to defer work to a certain timeslot in the
future.

5 files changed, 508 insertions, 2 deletions

diff --git a/src/ipcpd/CMakeLists.txt b/src/ipcpd/CMakeLists.txt
index 43af7a25..afce9441 100644
--- a/src/ipcpd/CMakeLists.txt
+++ b/src/ipcpd/CMakeLists.txt
@@ -2,6 +2,7 @@ set(IPCP_SOURCES
         # Add source files here
         ${CMAKE_CURRENT_SOURCE_DIR}/ipcp.c
         ${CMAKE_CURRENT_SOURCE_DIR}/ipcp-data.c
+        ${CMAKE_CURRENT_SOURCE_DIR}/timerwheel.c
 )
 
 add_subdirectory(local)
@@ -10,3 +11,4 @@ add_subdirectory(shim-udp)
 if(NOT APPLE)
   add_subdirectory(shim-eth-llc)
 endif()
+add_subdirectory(tests)
diff --git a/src/ipcpd/tests/CMakeLists.txt b/src/ipcpd/tests/CMakeLists.txt
index 68bd762d..57a910c8 100644
--- a/src/ipcpd/tests/CMakeLists.txt
+++ b/src/ipcpd/tests/CMakeLists.txt
@@ -1,8 +1,20 @@
-get_filename_component(tmp ".." ABSOLUTE)
-get_filename_component(src_folder "${tmp}" NAME)
+get_filename_component(CURRENT_SOURCE_PARENT_DIR
+  ${CMAKE_CURRENT_SOURCE_DIR} DIRECTORY)
+get_filename_component(CURRENT_BINARY_PARENT_DIR
+  ${CMAKE_CURRENT_BINARY_DIR} DIRECTORY)
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+
+include_directories(${CURRENT_SOURCE_PARENT_DIR})
+include_directories(${CURRENT_BINARY_PARENT_DIR})
+
+include_directories(${CMAKE_SOURCE_DIR}/include)
+include_directories(${CMAKE_BINARY_DIR}/include)
 
 create_test_sourcelist(${src_folder}_tests test_suite.c
                        # Add new tests here
+                       timerwheel_test.c
 )
 
 add_executable(${src_folder}_test EXCLUDE_FROM_ALL ${${src_folder}_tests})
diff --git a/src/ipcpd/tests/timerwheel_test.c b/src/ipcpd/tests/timerwheel_test.c
new file mode 100644
index 00000000..615e6e41
--- /dev/null
+++ b/src/ipcpd/tests/timerwheel_test.c
@@ -0,0 +1,92 @@
+/*
+ * Ouroboros - Copyright (C) 2016
+ *
+ * Test of the timer wheel
+ *
+ *    Dimitri Staessens <dimitri.staessens@intec.ugent.be>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "timerwheel.c"
+
+#include <pthread.h>
+#include <time.h>
+#include <stdlib.h>
+
+#define MAX_ELEMENTS   500
+#define MAX_RESOLUTION 10  /* ms */
+#define MAX_ADDITIONS  1000
+
+int total;
+
+int add(void * o)
+{
+        total += *((int *) o);
+        return 0;
+}
+
+int timerwheel_test(int argc, char ** argv)
+{
+        struct timerwheel * tw;
+        long resolution;
+        long elements;
+        struct timespec wait;
+
+        int additions;
+
+        int check_total = 0;
+
+        int i;
+
+        (void) argc;
+        (void) argv;
+
+        srand(time(NULL));
+
+        total = 0;
+
+        resolution = rand() % (MAX_RESOLUTION - 1) + 1;
+        elements = rand() % (MAX_ELEMENTS - 10) + 10;
+
+        tw = timerwheel_create(resolution, resolution * elements);
+        if (tw == NULL)
+                return -1;
+
+        wait.tv_sec = (resolution * elements) / 1000;
+        wait.tv_nsec = ((resolution * elements) % 1000) * MILLION;
+
+        additions = rand() % MAX_ADDITIONS + 1000;
+
+        for (i = 0; i < additions; ++i) {
+                int delay = rand() % (resolution * elements);
+                int var = rand() % 5;
+                check_total += var;
+                timerwheel_add(tw,
+                               (void (*)(void *)) add,
+                               (void *) &var,
+                               sizeof(var),
+                               delay);
+        }
+
+        nanosleep(&wait, NULL);
+
+        timerwheel_destroy(tw);
+
+        if (total != check_total)
+                return -1;
+
+        return 0;
+}
diff --git a/src/ipcpd/timerwheel.c b/src/ipcpd/timerwheel.c
new file mode 100644
index 00000000..f6785611
--- /dev/null
+++ b/src/ipcpd/timerwheel.c
@@ -0,0 +1,361 @@
+/*
+ * Ouroboros - Copyright (C) 2016
+ *
+ * Timerwheel
+ *
+ *    Dimitri Staessens <dimitri.staessens@intec.ugent.be>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <ouroboros/config.h>
+#include <ouroboros/time_utils.h>
+#include <ouroboros/errno.h>
+#include <ouroboros/list.h>
+
+#define OUROBOROS_PREFIX "timerwheel"
+
+#include <ouroboros/logs.h>
+
+#include <pthread.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+
+#define FRAC 10 /* accuracy of the timer */
+
+#define tw_used(tw) ((tw->head + tw->elements - tw->tail) & (tw->elements - 1));
+#define tw_free(tw) (tw_used(tw) + 1 < tw->elements)
+#define tw_empty(tw) (tw->head == tw->tail)
+
+enum tw_state {
+        TW_NULL = 0,
+        TW_RUNNING,
+        TW_DESTROY
+};
+
+struct tw_f {
+        struct list_head next;
+        void (* func)(void *);
+        void * arg;
+};
+
+struct tw_el {
+        struct list_head funcs;
+        struct timespec  expiry;
+};
+
+struct timerwheel {
+        struct tw_el *   wheel;
+
+        struct timespec  intv;
+
+        size_t           pos;
+
+        struct list_head wq;
+
+        pthread_cond_t   work;
+        pthread_mutex_t  lock;
+
+        int              resolution;
+        unsigned int     elements;
+
+        enum tw_state    state;
+        pthread_mutex_t  s_lock;
+
+        pthread_t        ticker;
+        pthread_t        worker;
+};
+
+static void tw_el_fini(struct tw_el * e)
+{
+        struct list_head * p;
+        struct list_head * h;
+
+        list_for_each_safe(p, h, &e->funcs) {
+                struct tw_f * f = list_entry(p, struct tw_f, next);
+                list_del(&f->next);
+                if (f->arg != NULL)
+                        free(f->arg);
+        }
+}
+
+static enum tw_state tw_get_state(struct timerwheel * tw)
+{
+        enum tw_state state;
+
+        assert(tw);
+
+        pthread_mutex_lock(&tw->s_lock);
+
+        state = tw->state;
+
+        pthread_mutex_unlock(&tw->s_lock);
+
+        return state;
+}
+
+static void tw_set_state(struct timerwheel * tw, enum tw_state state)
+{
+        assert(tw);
+        assert(state != TW_NULL);
+
+        pthread_mutex_lock(&tw->s_lock);
+
+        tw->state = state;
+
+        pthread_mutex_unlock(&tw->s_lock);
+}
+
+static void * worker(void * o)
+{
+        struct list_head * p;
+        struct list_head * h;
+
+        struct timerwheel * tw = (struct timerwheel *) o;
+        struct timespec dl;
+        struct timespec now;
+
+        clock_gettime(CLOCK_MONOTONIC, &now);
+
+        ts_add(&now, &tw->intv, &dl);
+
+        pthread_mutex_lock(&tw->lock);
+
+        while (tw_get_state(tw) == TW_RUNNING) {
+                if (pthread_cond_timedwait(&tw->work, &tw->lock, &dl)
+                    == ETIMEDOUT) {
+                        ts_add(&dl, &tw->intv, &dl);
+                        continue;
+                }
+
+                list_for_each_safe(p, h, &tw->wq) {
+                        struct tw_f * f = list_entry(p, struct tw_f, next);
+                        list_del(&f->next);
+                        pthread_mutex_unlock(&tw->lock);
+                        f->func(f->arg);
+                        if (f->arg != NULL)
+                                free(f->arg);
+                        free(f);
+                        pthread_mutex_lock(&tw->lock);
+                }
+        }
+
+        pthread_mutex_unlock(&tw->lock);
+
+        return (void *) o;
+}
+
+static void * movement(void * o)
+{
+        struct timerwheel * tw = (struct timerwheel *) o;
+        struct timespec now = {0, 0};
+        long ms = (tw->resolution * tw->elements);
+        struct timespec total = {ms / 1000,
+                                 (ms % 1000) * MILLION};
+        struct list_head * p;
+        struct list_head * h;
+
+        while (tw_get_state(tw) == TW_RUNNING) {
+                clock_gettime(CLOCK_MONOTONIC, &now);
+                if (ts_diff_us(&tw->wheel[tw->pos].expiry, &now) < 0) {
+                        nanosleep(&tw->intv, NULL);
+                        continue;
+                }
+
+                pthread_mutex_lock(&tw->lock);
+
+                list_for_each_safe(p, h, &tw->wheel[tw->pos].funcs) {
+                        struct tw_f * f = list_entry(p, struct tw_f, next);
+                        list_del(&f->next);
+                        list_add(&f->next, &tw->wq);
+                        pthread_cond_signal(&tw->work);
+                }
+
+                ts_add(&tw->wheel[tw->pos].expiry,
+                       &total,
+                       &tw->wheel[tw->pos].expiry);
+
+                tw->pos = (tw->pos + 1) & (tw->elements - 1);
+
+                pthread_mutex_unlock(&tw->lock);
+        }
+
+        return (void *) 0;
+}
+
+struct timerwheel * timerwheel_create(unsigned int resolution,
+                                      unsigned int max_delay)
+{
+        struct timespec now = {0, 0};
+        struct timespec res_ts = {resolution / 1000,
+                                  (resolution % 1000) * MILLION};
+        unsigned long i;
+
+        struct timerwheel * tw;
+
+        assert(resolution != 0);
+
+        tw = malloc(sizeof(*tw));
+        if (tw == NULL)
+                return NULL;
+
+        if (pthread_mutex_init(&tw->lock, NULL))
+                return NULL;
+
+        tw->elements = 1;
+
+        while (tw->elements < max_delay / resolution)
+                tw->elements <<= 1;
+
+        tw->wheel = malloc(sizeof(*tw->wheel) * tw->elements);
+        if (tw->wheel == NULL) {
+                free(tw);
+                return NULL;
+        }
+
+        tw->resolution = resolution;
+
+        tw->intv.tv_sec = tw->resolution / (1000 * FRAC);
+        tw->intv.tv_nsec = (tw->resolution % 1000) * (MILLION / FRAC);
+
+        INIT_LIST_HEAD(&tw->wq);
+
+        if (pthread_mutex_init(&tw->lock, NULL)) {
+                LOG_DBG("Could not init mutex.");
+                free(tw->wheel);
+                free(tw);
+                return NULL;
+        }
+
+        if (pthread_mutex_init(&tw->s_lock, NULL)) {
+                LOG_DBG("Could not init mutex.");
+                pthread_mutex_destroy(&tw->lock);
+                free(tw->wheel);
+                free(tw);
+                return NULL;
+        }
+
+        if (pthread_cond_init(&tw->work, NULL)) {
+                LOG_DBG("Could not init cond.");
+                pthread_mutex_destroy(&tw->s_lock);
+                pthread_mutex_destroy(&tw->lock);
+                free(tw->wheel);
+                free(tw);
+                return NULL;
+        }
+
+        tw->pos = 0;
+        tw->state = TW_RUNNING;
+
+        clock_gettime(CLOCK_MONOTONIC, &now);
+        now.tv_nsec -= (now.tv_nsec % MILLION);
+
+        for (i = 0; i < tw->elements; ++i) {
+                INIT_LIST_HEAD(&tw->wheel[i].funcs);
+                tw->wheel[i].expiry = now;
+                ts_add(&now, &res_ts, &now);
+        }
+
+        if (pthread_create(&tw->worker, NULL, worker, (void *) tw)) {
+                LOG_DBG("Could not create worker.");
+                pthread_cond_destroy(&tw->work);
+                pthread_mutex_destroy(&tw->s_lock);
+                pthread_mutex_destroy(&tw->lock);
+                free(tw->wheel);
+                free(tw);
+                return NULL;
+        }
+
+        if (pthread_create(&tw->ticker, NULL, movement, (void *) tw)) {
+                LOG_DBG("Could not create timer.");
+                tw_set_state(tw, TW_DESTROY);
+                pthread_join(tw->worker, NULL);
+                pthread_cond_destroy(&tw->work);
+                pthread_mutex_destroy(&tw->s_lock);
+                pthread_mutex_destroy(&tw->lock);
+                free(tw->wheel);
+                free(tw);
+                return NULL;
+        }
+
+        return tw;
+}
+
+void timerwheel_destroy(struct timerwheel * tw)
+{
+        unsigned long i;
+
+        struct list_head * p;
+        struct list_head * h;
+
+        tw_set_state(tw, TW_DESTROY);
+
+        pthread_join(tw->ticker, NULL);
+        pthread_join(tw->worker, NULL);
+
+        for (i = 0; i < tw->elements; ++i)
+                tw_el_fini(&tw->wheel[i]);
+
+        pthread_mutex_lock(&tw->lock);
+
+        list_for_each_safe(p, h, &tw->wq) {
+                struct tw_f * f = list_entry(p, struct tw_f, next);
+                list_del(&f->next);
+                if (f->arg != NULL)
+                        free(f->arg);
+                free(f);
+        }
+
+        pthread_mutex_unlock(&tw->lock);
+
+        pthread_cond_destroy(&tw->work);
+        pthread_mutex_destroy(&tw->lock);
+        pthread_mutex_destroy(&tw->s_lock);
+
+        free(tw->wheel);
+        free(tw);
+}
+
+int timerwheel_add(struct timerwheel * tw,
+                   void (* func)(void *),
+                   void * arg,
+                   size_t arg_len,
+                   unsigned int delay)
+{
+        int pos = (tw->pos + delay / tw->resolution) & (tw->elements - 1);
+        struct tw_f * f = malloc(sizeof(*f));
+        if (f == NULL)
+                return -ENOMEM;
+
+        f->func = func;
+        f->arg = malloc(arg_len);
+        if (f->arg == NULL) {
+                free(f);
+                return -ENOMEM;
+        }
+
+        memcpy(f->arg, arg, arg_len);
+
+        assert(delay < tw->elements * tw->resolution);
+
+        pthread_mutex_lock(&tw->lock);
+
+        list_add(&f->next, &tw->wheel[pos].funcs);
+
+        pthread_mutex_unlock(&tw->lock);
+
+        return 0;
+}
diff --git a/src/ipcpd/timerwheel.h b/src/ipcpd/timerwheel.h
new file mode 100644
index 00000000..d10410ca
--- /dev/null
+++ b/src/ipcpd/timerwheel.h
@@ -0,0 +1,39 @@
+/*
+ * Ouroboros - Copyright (C) 2016
+ *
+ * Ring buffer for incoming SDUs
+ *
+ *    Dimitri Staessens <dimitri.staessens@intec.ugent.be>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef OUROBOROS_IPCPD_TIMERWHEEL_H
+#define OUROBOROS_IPCPD_TIMERWHEEL_H
+
+struct timerwheel;
+
+struct timerwheel * timerwheel_create(unsigned int resolution,
+                                      unsigned int max_delay);
+
+void                timerwheel_destroy(struct timerwheel * tw);
+
+int                 timerwheel_add(struct timerwheel * tw,
+                                   void (* func)(void *),
+                                   void *       arg,
+                                   size_t       arg_len,
+                                   unsigned int delay); /* ms */
+
+#endif /* OUROBOROS_IPCPD_TIMERWHEEL_H */