123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193 |
- // 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));
- }
|