myxce/src/lib/delay.c

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

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

/* Flat CLOCK_MONOTONIC "now", in milliseconds. The single time source every
 * task deadline (delay_task_t.nextrun_ms) is compared against. */
static uint64_t monotonic_ms(void)
{
    struct timespec now;

    clock_gettime(CLOCK_MONOTONIC, &now);
    return (uint64_t)now.tv_sec * 1000ULL + (uint64_t)now.tv_nsec / 1000000ULL;
}

/* Return 1 if a task's deadline has passed, 0 otherwise. Every task in the
 * queue uses this single millisecond time domain (see delay.h) -- there is
 * no separate "seconds task" due-check anymore. */
static int task_due(const delay_task_t* t)
{
    return monotonic_ms() >= t->nextrun_ms;
}

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

    while (1) {
        ordq_item_t*  it;
        delay_task_t* t;

        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;
        if (!task_due(t)) {
            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 {
            /* advance deadline by run_period_ms -- flat integer add, no
             * sec/nsec carry arithmetic needed. */
            t->nextrun_ms += (uint64_t)t->run_period_ms;
            ordq_put(&(mgr->tasks), t);
        }

        FREEUP(it);
    }

    return NULL;
}

/* Single normalized comparison: every task carries one flat millisecond
 * deadline (nextrun_ms), so ordering is always apples-to-apples regardless
 * of whether the task was scheduled via delay_task() or delay_task_ms(). */
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;

    if (t1->nextrun_ms > t2->nextrun_ms) return 1;
    if (t1->nextrun_ms < t2->nextrun_ms) return -1;
    return 0;
}

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;
}

/* delay_task: seconds-granularity scheduling API. A thin wrapper over
 * delay_task_ms() -- every task, regardless of which API scheduled it, is
 * stored and compared on the single CLOCK_MONOTONIC timescale (delay.h), so
 * a seconds-task and an ms-task in the same delaymgr are always correctly
 * ordered against each other (previously a mixed wall-clock/monotonic
 * comparison meant a seconds-task could be starved indefinitely behind a
 * steady stream of ms-tasks -- see the bug this simplification fixes).
 * INT_MAX/1000 seconds (~24 days) is far beyond any real caller's delay; a
 * value that would overflow the ms conversion is clamped rather than
 * wrapped. */
int delay_task(delaymgr_t* mgr, int delay, int runcount,
               delay_task_entry_fn_t entry, void* args)
{
    if (delay <= 0) {
        return -EINVAL;
    }
    long delay_ms = (long)delay * 1000L;
    if (delay_ms > INT_MAX) {
        delay_ms = INT_MAX;
    }
    return delay_task_ms(mgr, (int)delay_ms, runcount, entry, args);
}

int delay_task_ms(delaymgr_t* mgr, int delay_ms, int runcount,
                  delay_task_entry_fn_t entry, void* args)
{
    delay_task_t* t;

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

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

    t->nextrun_ms     = monotonic_ms() + (uint64_t)delay_ms;
    t->runcount       = runcount;
    t->run_period_ms  = delay_ms;
    t->entry          = entry;
    t->args           = args;

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