Initial implementation of transmit schedule

include/hexagram/schedule.h

@@ -4,6 +4,7 @@
 #include <time.h>
 #include <sys/types.h>
 #include <sys/time.h>
+#include <signal.h>
 
 #include <hexagram/table.h>
 
@@ -13,8 +14,14 @@ typedef struct _hexagram_schedule_slot {
 } hexagram_schedule_slot;
 
 typedef struct _hexagram_schedule {
+    struct sigevent ev;
+    struct itimerspec ts;
     timer_t timer;
-    size_t current;
+
+    size_t current,
+           count;
+
+    int error;
 } hexagram_schedule;
 
 hexagram_schedule *hexagram_schedule_create(hexagram_table_entry *table,
@@ -22,8 +29,12 @@ hexagram_schedule *hexagram_schedule_create(hexagram_table_entry *table,
 
 void hexagram_schedule_destroy(hexagram_schedule *schedule);
 
+void hexagram_schedule_reset(hexagram_schedule *schedule);
+
 int hexagram_schedule_run(hexagram_schedule *schedule);
 
 int hexagram_schedule_stop(hexagram_schedule *schedule);
 
+int hexagram_schedule_error(hexagram_schedule *schedule);
+
 #endif /* _HEXAGRAM_SCHEDULE_H */

include/hexagram/table.h

@@ -10,6 +10,4 @@ typedef struct _hexagram_table_entry {
     struct can_frame frame;
 } hexagram_table_entry;
 
-int hexagram_table_entry_send(hexagram_table_entry *entry);
-
 #endif /* _HEXAGRAM_TABLE_H */

src/Makefile

@@ -9,13 +9,13 @@ LDFLAGS		= $(shell pkg-config --libs cairo x11) -lXext
 
 HEADERS		= dict.h hash.h can.h capture.h pcapng.h module.h window.h \
 		  gauge.h tacho.h speedo.h thermo.h fuel.h mfd.h cluster.h \
-		  sim.h
+		  sim.h schedule.h table.h
 
 HEADERS_LOCAL	= util.h
 
 OBJS		= dict.o hash.o can.o capture.o pcapng.o module.o window.o \
 		  gauge.o tacho.o speedo.o thermo.o fuel.o mfd.o cluster.o \
-		  sim.o
+		  sim.o schedule.o
 
 VERSION_MAJOR	= 0
 VERSION_MINOR	= 0.1

src/schedule.c

@@ -1,25 +1,113 @@
 #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 _set_slot_interval(hexagram_schedule *schedule, size_t i) {
+    return _set_interval(schedule, _slot_interval_ns(schedule, i));
+}
+
+static void _schedule_notify(hexagram_schedule *schedule) {
+    time_t last = -1;
+
+    while (schedule->current < schedule->count) {
+        hexagram_schedule_slot *slot = _slot(schedule, schedule->current);
+        time_t delay = slot->interval_us;
+
+        if (hexagram_can_if_write(slot->entry->iface, &slot->entry->frame) < 0) {
+            schedule->error = errno;
+
+            break;
+        }
+
+        schedule->current++;
+
+        if (last >= 0 && last != delay) {
+            _set_slot_interval(schedule, schedule->current);
+
+            return;
+        }
+
+        last = delay;
+    }
+
+    schedule->current = 0;
+
+    _set_slot_interval(schedule, schedule->current);
+}
+
+static int _schedule_init(hexagram_schedule *schedule,
+                          hexagram_table_entry *table,
+                          size_t count) {
+    size_t i;
+
+    memset(schedule, '\0', sizeof(*schedule));
+
+    schedule->current = 0;
+    schedule->count   = count;
+
+    schedule->ev.sigev_notify          = SIGEV_THREAD;
+    schedule->ev.sigev_notify_function = _schedule_notify;
+    schedule->ev.sigev_value.sival_ptr = schedule;
+
+    if (timer_create(CLOCK_REALTIME,
+                     &schedule->ev,
+                     &schedule->timer) < 0) {
+        goto error_timer_create;
+    }
+
+    for (i=0; i<count; i++) {
+        hexagram_schedule_slot *slot = &((hexagram_schedule_slot *)(schedule + 1))[i];
+
+        slot->interval_us = table[i].freq_us;
+        slot->entry       = &table[i];
+    }
+
+    qsort(schedule + 1, count, sizeof(hexagram_schedule_slot), _slot_cmp);
+
+    return 0;
+
+error_timer_create:
+    return -1;
+}
+
 hexagram_schedule *hexagram_schedule_create(hexagram_table_entry *table,
                                             size_t count) {
     hexagram_schedule *schedule;
-    size_t i;
 
     if ((schedule = malloc(sizeof(*schedule) + count * sizeof(hexagram_schedule_slot))) == 0) {
         goto error_malloc_schedule;
     }
 
-    schedule->timer   = 0;
-    schedule->current = 0;
-
-    for (i=0; i<count; i++) {
-        hexagram_schedule_slot *slot = &((hexagram_schedule_slot *)(schedule + 1))[i];
-
-        slot->interval_us = 0;
-        slot->entry       = &table[i];
-    }
+    _schedule_init(schedule, table, count);
 
     return schedule;
 
@@ -34,3 +122,30 @@ void hexagram_schedule_destroy(hexagram_schedule *schedule) {
 
     free(schedule);
 }
+
+void hexagram_schedule_reset(hexagram_schedule *schedule) {
+    schedule->current = 0;
+    schedule->error   = 0;
+}
+
+int hexagram_schedule_run(hexagram_schedule *schedule) {
+    schedule->current = 0;
+    schedule->error   = 0;
+
+    return _set_slot_interval(schedule, schedule->current);
+}
+
+int hexagram_schedule_stop(hexagram_schedule *schedule) {
+    const struct itimerspec ts = {
+        .it_value.tv_sec     = 0,
+        .it_value.tv_nsec    = 0,
+        .it_interval.tv_sec  = 0,
+        .it_interval.tv_nsec = 0
+    };
+
+    return timer_settime(schedule->timer, 0, &ts, NULL);
+}
+
+int hexagram_schedule_error(hexagram_schedule *schedule) {
+    return schedule->error;
+}