utun2/lib/twheel.c

// twheel.c — Tickless hierarchical timing wheel (adapted from timeout.c)
// Original: Copyright (c) 2013-2014 William Ahern, MIT license.

#include <limits.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>
#include <sys/queue.h>

#include "twheel.h"
#include "mem.h"
#include "debug_config.h"

#define WHEEL_BIT 6
#define WHEEL_NUM 4

#define WHEEL_LEN  (1U << WHEEL_BIT)
#define WHEEL_MAX  (WHEEL_LEN - 1)
#define WHEEL_MASK (WHEEL_LEN - 1)

#include "twheel_bitops.c"

#define ctz(n) ctz64(n)
#define clz(n) clz64(n)
#define fls(n) ((int)(64 - clz64(n)))

typedef uint64_t wheel_t;
#define WHEEL_C(n) UINT64_C(n)

#define countof(a) (sizeof(a) / sizeof *(a))

#ifndef TAILQ_CONCAT
#define TAILQ_CONCAT(head1, head2, field) do {                          \
    if (!TAILQ_EMPTY(head2)) {                                          \
        *(head1)->tqh_last = (head2)->tqh_first;                        \
        (head2)->tqh_first->field.tqe_prev = (head1)->tqh_last;         \
        (head1)->tqh_last = (head2)->tqh_last;                          \
        TAILQ_INIT((head2));                                            \
    }                                                                   \
} while (0)
#endif

#ifndef TAILQ_FOREACH_SAFE
#define TAILQ_FOREACH_SAFE(var, head, field, tvar)                      \
    for ((var) = TAILQ_FIRST(head);                                     \
        (var) && ((tvar) = TAILQ_NEXT(var, field), 1);                  \
        (var) = (tvar))
#endif

#ifndef MAX
#define MAX(a,b) (((a)>(b))?(a):(b))
#endif
#ifndef MIN
#define MIN(a,b) (((a)<(b))?(a):(b))
#endif

static inline wheel_t rotl(const wheel_t v, int c) {
    if (!(c &= (sizeof v * CHAR_BIT - 1))) return v;
    return (v << c) | (v >> (sizeof v * CHAR_BIT - c));
}

static inline wheel_t rotr(const wheel_t v, int c) {
    if (!(c &= (sizeof v * CHAR_BIT - 1))) return v;
    return (v >> c) | (v << (sizeof v * CHAR_BIT - c));
}

static struct twheels *twheels_init(struct twheels *T, twheel_t hz) {
    for (unsigned i = 0; i < WHEEL_NUM; i++) {
        for (unsigned j = 0; j < WHEEL_LEN; j++) {
            TAILQ_INIT(&T->wheel[i][j]);
        }
    }
    TAILQ_INIT(&T->expired);
    for (unsigned i = 0; i < WHEEL_NUM; i++) T->pending[i] = 0;
    T->curtime = 0;
    T->hertz = hz ? hz : TWHEEL_mHZ;
    return T;
}

struct twheels *twheels_open(twheel_t hz) {
    struct twheels *T = u_malloc(sizeof *T);
    if (!T) { DEBUG_ERROR(DEBUG_CATEGORY_TIMERS, "twheels_open: malloc failed"); return NULL; }
    return twheels_init(T, hz);
}

void twheels_close(struct twheels *T) {
    if (!T) return;
    struct twheel_list reset;
    TAILQ_INIT(&reset);
    for (unsigned i = 0; i < WHEEL_NUM; i++)
        for (unsigned j = 0; j < WHEEL_LEN; j++)
            TAILQ_CONCAT(&reset, &T->wheel[i][j], tqe);
    TAILQ_CONCAT(&reset, &T->expired, tqe);
    struct twheel *to;
    TAILQ_FOREACH(to, &reset, tqe) to->pending = NULL;
    u_free(T);
}

twheel_t twheels_hz(struct twheels *T) { return T->hertz; }

void twheels_del(struct twheels *T, struct twheel *to) {
    if (to->pending) {
        TAILQ_REMOVE(to->pending, to, tqe);
        if (to->pending != &T->expired && TAILQ_EMPTY(to->pending)) {
            ptrdiff_t index = to->pending - &T->wheel[0][0];
            int w = index / WHEEL_LEN, slot = index % WHEEL_LEN;
            T->pending[w] &= ~(WHEEL_C(1) << slot);
        }
        to->pending = NULL;
    }
}

static inline int timeout_wheel(twheel_t timeout) {
    return (fls(MIN(timeout, TWHEEL_MAX)) - 1) / WHEEL_BIT;
}

static inline int timeout_slot(int wheel, twheel_t expires) {
    return WHEEL_MASK & ((expires >> (wheel * WHEEL_BIT)) - !!wheel);
}

static void twheels_sched(struct twheels *T, struct twheel *to, twheel_t expires) {
    twheels_del(T, to);
    to->expires = expires;
    if (expires > T->curtime) {
        twheel_t rem = expires - T->curtime;
        int wheel = timeout_wheel(rem);
        int slot = timeout_slot(wheel, to->expires);
        to->pending = &T->wheel[wheel][slot];
        TAILQ_INSERT_TAIL(to->pending, to, tqe);
        T->pending[wheel] |= WHEEL_C(1) << slot;
    } else {
        to->pending = &T->expired;
        TAILQ_INSERT_TAIL(to->pending, to, tqe);
    }
}

void twheels_add(struct twheels *T, struct twheel *to, twheel_t timeout) {
    if (to->flags & TWHEEL_INT) to->interval = MAX(1, timeout);
    if (to->flags & TWHEEL_ABS) twheels_sched(T, to, timeout);
    else twheels_sched(T, to, T->curtime + timeout);
}

void twheels_update(struct twheels *T, twheel_t curtime) {
    twheel_t elapsed = curtime - T->curtime;
    struct twheel_list todo;
    TAILQ_INIT(&todo);

    for (int wheel = 0; wheel < WHEEL_NUM; wheel++) {
        wheel_t pending;
        if ((elapsed >> (wheel * WHEEL_BIT)) > WHEEL_MAX) {
            pending = (wheel_t)~WHEEL_C(0);
        } else {
            wheel_t _elapsed = WHEEL_MASK & (elapsed >> (wheel * WHEEL_BIT));
            int oslot = WHEEL_MASK & (T->curtime >> (wheel * WHEEL_BIT));
            pending = rotl(((UINT64_C(1) << _elapsed) - 1), oslot);
            int nslot = WHEEL_MASK & (curtime >> (wheel * WHEEL_BIT));
            pending |= rotr(rotl(((WHEEL_C(1) << _elapsed) - 1), nslot), _elapsed);
            pending |= WHEEL_C(1) << nslot;
        }
        while (pending & T->pending[wheel]) {
            int slot = ctz(pending & T->pending[wheel]);
            TAILQ_CONCAT(&todo, &T->wheel[wheel][slot], tqe);
            T->pending[wheel] &= ~(UINT64_C(1) << slot);
        }
        if (!(0x1 & pending)) break;
        elapsed = MAX(elapsed, (WHEEL_LEN << (wheel * WHEEL_BIT)));
    }
    T->curtime = curtime;

    struct twheel *to;
    while (!TAILQ_EMPTY(&todo)) {
        to = TAILQ_FIRST(&todo);
        TAILQ_REMOVE(&todo, to, tqe);
        to->pending = NULL;
        twheels_sched(T, to, to->expires);
    }
}

bool twheels_pending(struct twheels *T) {
    wheel_t pending = 0;
    for (int wheel = 0; wheel < WHEEL_NUM; wheel++) pending |= T->pending[wheel];
    return !!pending;
}

bool twheels_expired(struct twheels *T) { return !TAILQ_EMPTY(&T->expired); }

static twheel_t twheels_int(struct twheels *T) {
    twheel_t timeout = ~TWHEEL_C(0), _timeout, relmask = 0;
    for (int wheel = 0; wheel < WHEEL_NUM; wheel++) {
        if (T->pending[wheel]) {
            int slot = WHEEL_MASK & (T->curtime >> (wheel * WHEEL_BIT));
            _timeout = (ctz(rotr(T->pending[wheel], slot)) + !!wheel) << (wheel * WHEEL_BIT);
            _timeout -= relmask & T->curtime;
            timeout = MIN(_timeout, timeout);
        }
        relmask <<= WHEEL_BIT;
        relmask |= WHEEL_MASK;
    }
    return timeout;
}

twheel_t twheels_timeout(struct twheels *T) {
    if (!TAILQ_EMPTY(&T->expired)) return 0;
    return twheels_int(T);
}

struct twheel *twheels_get(struct twheels *T) {
    if (!TAILQ_EMPTY(&T->expired)) {
        struct twheel *to = TAILQ_FIRST(&T->expired);
        TAILQ_REMOVE(&T->expired, to, tqe);
        to->pending = NULL;
        return to;
    }
    return NULL;
}

struct twheel *twheel_init(struct twheel *to, int flags) {
    memset(to, 0, sizeof *to);
    to->flags = flags;
    return to;
}

bool twheel_pending(struct twheel *to) { return to->pending != NULL; }