software-watchdog/src/software_watchdog.c

// reference: http://man7.org/linux/man-pages/man2/timer_create.2.html

#include <stdio.h>
#include <string.h>
#include <time.h>
#include <signal.h>
#include <stdlib.h>
#include <stdbool.h>
#include "software_watchdog.h"
#ifdef _WIN32
    #define CLOCK_REALTIME 0
    #define NOMINMAX
    #define WIN32_LEAN_AND_MEAN
    #include <windows.h>

    int nanosleep(const struct timespec* req, struct timespec* rem) {
        double ms;
        ms = req->tv_sec * 1000.0 + req->tv_nsec / 1000000;
        Sleep(ms);
        return 0;
    }

    LARGE_INTEGER
    getFILETIMEoffset()
    {
        SYSTEMTIME s;
        FILETIME f;
        LARGE_INTEGER t;

        s.wYear = 1970;
        s.wMonth = 1;
        s.wDay = 1;
        s.wHour = 0;
        s.wMinute = 0;
        s.wSecond = 0;
        s.wMilliseconds = 0;
        SystemTimeToFileTime(&s, &f);
        t.QuadPart = f.dwHighDateTime;
        t.QuadPart <<= 32;
        t.QuadPart |= f.dwLowDateTime;
        return (t);
    }

    int
    clock_gettime(int X, struct timeval *tv)
    {
        LARGE_INTEGER           t;
        FILETIME            f;
        double                  microseconds;
        static LARGE_INTEGER    offset;
        static double           frequencyToMicroseconds;
        static int              initialized = 0;
        static BOOL             usePerformanceCounter = 0;

        if (!initialized) {
            LARGE_INTEGER performanceFrequency;
            initialized = 1;
            usePerformanceCounter = QueryPerformanceFrequency(&performanceFrequency);
            if (usePerformanceCounter) {
                QueryPerformanceCounter(&offset);
                frequencyToMicroseconds = (double)performanceFrequency.QuadPart / 1000000.;
            } else {
                offset = getFILETIMEoffset();
                frequencyToMicroseconds = 10.;
            }
        }
        if (usePerformanceCounter) QueryPerformanceCounter(&t);
        else {
            GetSystemTimeAsFileTime(&f);
            t.QuadPart = f.dwHighDateTime;
            t.QuadPart <<= 32;
            t.QuadPart |= f.dwLowDateTime;
        }

        t.QuadPart -= offset.QuadPart;
        microseconds = (double)t.QuadPart / frequencyToMicroseconds;
        t.QuadPart = microseconds;
        tv->tv_sec = t.QuadPart / 1000000;
        tv->tv_usec = t.QuadPart % 1000000;
        return (0);
    }
#else
    #include <pthread.h>
    #include <unistd.h>
#endif

#define CLOCKID CLOCK_REALTIME
#define SIG SIGRTMIN

void printHelloWorld() {
    printf("HelloWorld\n");
}

void subTime(struct timespec *t1, struct timespec *subtrahend) {
    // get the time difference between now and latestKick
    t1->tv_sec -= subtrahend->tv_sec;
    if (t1->tv_nsec < subtrahend->tv_nsec) {
        // e.g. t1 = 5s.10000ns, subtrahend = 3s.90000ns
        t1->tv_nsec = 1000000000 - (subtrahend->tv_nsec - t1->tv_nsec);
        t1->tv_sec += 1;
    }
    else {
        // e.g. t1 = 5s.50000ns, subtrahend = 3s.30000ns
        t1->tv_nsec -= subtrahend->tv_nsec;
    }
}

int compareTime(struct timespec* t1, struct timespec* t2) {
    if(t1->tv_sec < t2->tv_sec)
        return -1;
    if(t1->tv_sec > t2->tv_sec)
        return 1;
    if(t1->tv_nsec < t2->tv_nsec)
        return -1;
    if(t1->tv_nsec > t2->tv_nsec)
        return 1;
    return 0; // equal
}

void *timer (void *data) {
    SoftwareWatchdog* watchdog = data;
    pthread_mutex_lock(&(watchdog->mutex));

    struct timespec now, diff, remaining_sleeptime;
    remaining_sleeptime = watchdog->timeout;

    pthread_mutex_unlock(&(watchdog->mutex));

    while (watchdog->isEnabled) {

        // printf ("%s: remaining_sleeptime: %ds,%dns", watchdog->name, remaining_sleeptime); fflush(stdout);
        // printf ("%s: sleep start\n", watchdog->name); fflush(stdout);
        nanosleep(&remaining_sleeptime, &remaining_sleeptime);
        // printf ("%s: sleep end\n", watchdog->name); fflush(stdout);

        // get the current time
        clock_gettime(CLOCK_REALTIME, &(now));

        // get the time difference between now and latestKick
        pthread_mutex_lock(&(watchdog->mutex));
        diff = now;
        subTime(&diff, &(watchdog->latestKick));
        pthread_mutex_unlock(&(watchdog->mutex));

        if (watchdog->isEnabled && compareTime(&diff, &(watchdog->timeout)) > 0) { // the deadline has been missed
            // printf ("%s: calling timeout_cb\n", watchdog->name); fflush(stdout);
            watchdog->timeout_cb(watchdog);
            remaining_sleeptime = watchdog->timeout;
        }
        else {
            // printf ("%s: no need to call timeout_cb\n", watchdog->name); fflush(stdout);
            remaining_sleeptime = watchdog->timeout;
            subTime(&remaining_sleeptime, &diff);
        }
    }
}

int swd_setup(SoftwareWatchdog* watchdog, const char *name, timeout_cb_t timeout_cb, struct timespec timeout, void *userData){
    // TODO: check input parameters
    watchdog->name = strdup(name);
    watchdog->timeout_cb = timeout_cb;
    watchdog->timeout = timeout;
    watchdog->userData = userData;
    watchdog->isEnabled = false;
    pthread_mutex_init(&(watchdog->mutex), NULL);
}

int swd_enable(SoftwareWatchdog* watchdog) {
    if (watchdog->isEnabled) {
        // That's really bad and should not happen
        return -1;
    }
    // create and detach the timer
    swd_kick(watchdog); // kick it, to initialize the watchdog->latestKick
    watchdog->isEnabled = true;
    pthread_create(&(watchdog->thread_id), NULL, timer, (void*)watchdog);
    pthread_detach(watchdog->thread_id);
    // printf("%s: threadid: %d\n", watchdog->name, watchdog->thread_id); fflush(stdout);
    return 0;
}

void swd_disable(SoftwareWatchdog* watchdog) {
    watchdog->isEnabled = false;
}

void swd_kick(SoftwareWatchdog* watchdog) {
    pthread_mutex_lock(&(watchdog->mutex));
    clock_gettime(CLOCK_REALTIME, &(watchdog->latestKick));
    pthread_mutex_unlock(&(watchdog->mutex));
}