135 lines
3.7 KiB
C
135 lines
3.7 KiB
C
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <signal.h>
|
|
#include <errno.h>
|
|
|
|
#include <hexagram/schedule.h>
|
|
|
|
static int _slot_cmp(const void *item_a, const void *item_b) {
|
|
hexagram_schedule_slot *a = (hexagram_schedule_slot *)item_a,
|
|
*b = (hexagram_schedule_slot *)item_b;
|
|
|
|
return a->interval_us < b->interval_us? -1:
|
|
a->interval_us > b->interval_us? 1: 0;
|
|
}
|
|
|
|
static inline hexagram_schedule_slot *_slot(hexagram_schedule *schedule, size_t i) {
|
|
return &((hexagram_schedule_slot *)(schedule + 1))[i];
|
|
};
|
|
|
|
static inline time_t _slot_interval_ns(hexagram_schedule *schedule, size_t i) {
|
|
return 1000 * _slot(schedule, i)->interval_us;
|
|
}
|
|
|
|
static inline int _set_interval(hexagram_schedule *schedule, time_t int_nsec) {
|
|
schedule->ts.it_value.tv_sec = 0;
|
|
schedule->ts.it_value.tv_nsec = int_nsec;
|
|
schedule->ts.it_interval.tv_sec = 0;
|
|
schedule->ts.it_interval.tv_nsec = int_nsec;
|
|
|
|
return timer_settime(schedule->timer, 0, &schedule->ts, NULL);
|
|
}
|
|
|
|
static inline int _slot_set_interval(hexagram_schedule *schedule, size_t i) {
|
|
return _set_interval(schedule, _slot_interval_ns(schedule, i));
|
|
}
|
|
|
|
static void _ev_notify(union sigval sv) {
|
|
hexagram_schedule *schedule = sv.sival_ptr;
|
|
time_t last = -1;
|
|
|
|
while (schedule->current < schedule->count) {
|
|
hexagram_schedule_slot *slot = _slot(schedule, schedule->current);
|
|
hexagram_schedule_table_entry *entry = slot->entry;
|
|
time_t delay = slot->interval_us;
|
|
|
|
if (entry->handler(&entry->frame, schedule->ctx) < 0) {
|
|
schedule->error = errno;
|
|
|
|
break;
|
|
}
|
|
|
|
schedule->current++;
|
|
|
|
if (last >= 0 && last != delay) {
|
|
_slot_set_interval(schedule, schedule->current);
|
|
|
|
return;
|
|
}
|
|
|
|
last = delay;
|
|
}
|
|
|
|
schedule->current = 0;
|
|
|
|
_slot_set_interval(schedule, schedule->current);
|
|
}
|
|
|
|
hexagram_schedule *hexagram_schedule_create(hexagram_schedule_table_entry *table,
|
|
size_t count,
|
|
void *ctx) {
|
|
hexagram_schedule *schedule;
|
|
size_t i;
|
|
|
|
if ((schedule = malloc(sizeof(*schedule) + count * sizeof(hexagram_schedule_slot))) == 0) {
|
|
goto error_malloc_schedule;
|
|
}
|
|
|
|
memset(&schedule->ev, '\0', sizeof(struct sigevent));
|
|
|
|
schedule->ev.sigev_notify = SIGEV_THREAD;
|
|
schedule->ev.sigev_notify_function = _ev_notify;
|
|
schedule->ev.sigev_value.sival_ptr = schedule;
|
|
|
|
schedule->current = 0;
|
|
schedule->count = count;
|
|
schedule->table = table;
|
|
schedule->ctx = ctx;
|
|
schedule->error = 0;
|
|
|
|
if (timer_create(CLOCK_REALTIME, &schedule->ev, &schedule->timer) < 0) {
|
|
goto error_timer_create;
|
|
}
|
|
|
|
for (i=0; i<count; i++) {
|
|
hexagram_schedule_slot *slot = _slot(schedule, i);
|
|
|
|
slot->interval_us = table[i].interval_us;
|
|
slot->entry = &table[i];
|
|
}
|
|
|
|
qsort(schedule + 1, count, sizeof(hexagram_schedule_slot), _slot_cmp);
|
|
|
|
return schedule;
|
|
|
|
error_timer_create:
|
|
free(schedule);
|
|
|
|
error_malloc_schedule:
|
|
return NULL;
|
|
}
|
|
|
|
void hexagram_schedule_destroy(hexagram_schedule *schedule) {
|
|
if (schedule->timer) {
|
|
timer_delete(schedule->timer);
|
|
}
|
|
|
|
free(schedule);
|
|
}
|
|
|
|
void hexagram_schedule_reset(hexagram_schedule *schedule) {
|
|
schedule->current = 0;
|
|
schedule->error = 0;
|
|
}
|
|
|
|
int hexagram_schedule_run(hexagram_schedule *schedule) {
|
|
return _slot_set_interval(schedule, schedule->current);
|
|
}
|
|
|
|
int hexagram_schedule_stop(hexagram_schedule *schedule) {
|
|
return _set_interval(schedule, 0);
|
|
}
|
|
|
|
int hexagram_schedule_error(hexagram_schedule *schedule) {
|
|
return schedule->error;
|
|
}
|