myxce/src/lib/delay.c

#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#include "delay.h"
#include "utils.h"

/* Return 1 if a ms-task's deadline has passed, 0 otherwise. */
static int ms_task_due(const delay_task_t* t)
{
    struct timespec now;

    clock_gettime(CLOCK_MONOTONIC, &now);
    if (now.tv_sec > t->nextrun_ts.tv_sec) {
        return 1;
    }
    if (now.tv_sec == t->nextrun_ts.tv_sec
        && now.tv_nsec >= t->nextrun_ts.tv_nsec) {
        return 1;
    }
    return 0;
}

static void* delay_worker(void* args)
{
    delaymgr_t* mgr = (delaymgr_t*)args;

    while (1) {
        ordq_item_t*  it;
        delay_task_t* t;
        time_t        now;
        int           is_ms_task;

        pthread_testcancel();
        {
            long ms = (long)mgr->precision_ms;
            struct timespec ts = {
                .tv_sec  = ms / 1000,
                .tv_nsec = (ms % 1000) * 1000000L,
            };
            nanosleep(&ts, NULL);
        }

        it = ordq_peek(&(mgr->tasks), NULL);
        if (it == NULL) {
            continue;
        }

        t          = (delay_task_t*)it->data;
        is_ms_task = (t->nextrun_ts.tv_nsec != 0 || t->nextrun_ts.tv_sec != 0);

        if (is_ms_task) {
            if (!ms_task_due(t)) {
                continue;
            }
        } else {
            now = time(NULL);
            if (t->nextrun > now) {
                continue;
            }
        }

        it = ordq_get(&(mgr->tasks), NULL);
        t  = (delay_task_t*)it->data;

        mgr->runner_fn(t->entry, t->args);
        if (t->runcount > 0) {
            t->runcount--;
        }

        if (t->runcount == 0) {
            FREEUP(t);
        } else {
            if (is_ms_task) {
                /* advance deadline by run_period ms */
                long add_ns = (long)t->run_period * 1000000L;

                t->nextrun_ts.tv_nsec += add_ns;
                t->nextrun_ts.tv_sec  += t->nextrun_ts.tv_nsec / 1000000000L;
                t->nextrun_ts.tv_nsec %= 1000000000L;
            } else {
                /* put back to taskq as it need to be run again */
                t->nextrun = now + t->run_period;
            }
            ordq_put(&(mgr->tasks), t);
        }

        FREEUP(it);
    }

    return NULL;
}

static int task_cmp(void* ctx, void* arg1, void* arg2)
{
    (void)ctx;
    delay_task_t* t1 = (delay_task_t*)arg1;
    delay_task_t* t2 = (delay_task_t*)arg2;

    /* ms-tasks: compare by nextrun_ts (nanosecond-precision deadline).
     * Both delay_task_ms and ordq_put use nextrun==0 as the ms-task marker. */
    int t1_is_ms = (t1->nextrun_ts.tv_nsec != 0 || t1->nextrun_ts.tv_sec != 0);
    int t2_is_ms = (t2->nextrun_ts.tv_nsec != 0 || t2->nextrun_ts.tv_sec != 0);

    if (t1_is_ms && t2_is_ms) {
        if (t1->nextrun_ts.tv_sec > t2->nextrun_ts.tv_sec) return 1;
        if (t1->nextrun_ts.tv_sec < t2->nextrun_ts.tv_sec) return -1;
        if (t1->nextrun_ts.tv_nsec > t2->nextrun_ts.tv_nsec) return 1;
        if (t1->nextrun_ts.tv_nsec < t2->nextrun_ts.tv_nsec) return -1;
        return 0;
    }

    /* Mixed or seconds-based: fall back to nextrun comparison. */
    time_t k1 = t1->nextrun;
    time_t k2 = t2->nextrun;
    if (k1 > k2) return 1;
    if (k1 == k2) return 0;
    return -1;
}

static void delay_task_killer(delay_task_t* t)
{
    if (t == NULL) {
        return;
    }

    FREEUP(t);
}

static int default_runner_fn(delay_task_entry_fn_t entry, void* arg)
{
    if (entry == NULL) {
        return 0;
    }

    return entry(arg);
}

int delaymgr_init(delaymgr_t* mgr, int precision, task_run_fn_t runner_fn)
{
    if ((mgr == NULL) || (precision <= 0)) {
        return -EINVAL;
    }

    if (runner_fn == NULL) {
        runner_fn = default_runner_fn;
    }

    mgr->precision_ms = precision;
    mgr->running   = 0;
    mgr->runner_fn = runner_fn;
    mgr->worker    = 0;

    ordq_init(&(mgr->tasks), (ordq_data_killer_t)delay_task_killer,
              (ordq_cmp_fn_t)task_cmp);

    return 0;
}

void delaymgr_stop(delaymgr_t* mgr)
{
    int rc;

    if (mgr == NULL) {
        return;
    }

    if (!(mgr->running)) {
        return;
    }

    if (mgr->worker != 0) {
        rc = pthread_cancel(mgr->worker);
        if (rc == 0) {
            void* res = NULL;

            pthread_join(mgr->worker, &res);

            if (res == PTHREAD_CANCELED) {
                mgr->worker = 0;
            }
        }
    }

    return;
}

void delaymgr_fini(delaymgr_t* mgr)
{
    if (mgr == NULL) {
        return;
    }

    if (mgr->running) {
        delaymgr_stop(mgr);
    }

    ordq_fini(&(mgr->tasks));
}

int delaymgr_start(delaymgr_t* mgr)
{
    int rc;

    if (mgr == NULL) {
        return -EINVAL;
    }

    rc = pthread_create(&(mgr->worker), NULL, &delay_worker, mgr);
    if (rc == 0) {
        mgr->running = 1;

    } else {
        SET_ERROR(errno);
        return -errno;
    }

    return 0;
}

int delay_task(delaymgr_t* mgr, int delay, int runcount,
               delay_task_entry_fn_t entry, void* args)
{
    delay_task_t* t;
    time_t        now = time(NULL);
    time_t        ts_nextrun;

    if ((mgr == NULL) || (delay <= 0) || (entry == NULL)) {
        return -EINVAL;
    }

    t = (delay_task_t*)calloc(1, sizeof(*t));
    if (t == NULL) {
        return -ENOMEM;
    }

    ts_nextrun = now + delay;

    t->nextrun    = ts_nextrun;
    t->runcount   = runcount;
    t->run_period = delay;
    t->entry      = entry;
    t->args       = args;

    return ordq_put(&(mgr->tasks), t);
}

int delay_task_ms(delaymgr_t* mgr, int delay_ms, int runcount,
                  delay_task_entry_fn_t entry, void* args)
{
    delay_task_t*   t;
    struct timespec now;
    long            add_ns;

    if ((mgr == NULL) || (delay_ms <= 0) || (entry == NULL)) {
        return -EINVAL;
    }

    t = (delay_task_t*)calloc(1, sizeof(*t));
    if (t == NULL) {
        return -ENOMEM;
    }

    clock_gettime(CLOCK_MONOTONIC, &now);

    add_ns = (long)delay_ms * 1000000L;
    t->nextrun_ts.tv_sec  = now.tv_sec  + (now.tv_nsec + add_ns) / 1000000000L;
    t->nextrun_ts.tv_nsec = (now.tv_nsec + add_ns) % 1000000000L;

    /* Ensure nextrun_ts is never all-zero so the ms-task flag is unambiguous.
     * tv_nsec == 0 can only arise when add_ns is an exact multiple of 1s and
     * now.tv_nsec == 0; in that edge case bump tv_nsec by 1 ns. */
    if (t->nextrun_ts.tv_nsec == 0) {
        t->nextrun_ts.tv_nsec = 1;
    }

    t->nextrun    = 0;          /* unused for ms-tasks */
    t->runcount   = runcount;
    t->run_period = delay_ms;
    t->entry      = entry;
    t->args       = args;

    return ordq_put(&(mgr->tasks), t);
}