host alarmclock

6 files changed, 132 insertions, 44 deletions

diff --git a/libhw/common_include/libhw/generic/alarmclock.h b/libhw/common_include/libhw/generic/alarmclock.h
index bf0d42f..a9d816b 100644
--- a/libhw/common_include/libhw/generic/alarmclock.h
+++ b/libhw/common_include/libhw/generic/alarmclock.h
@@ -87,7 +87,7 @@ static inline void alarmclock_sleep_for_s(implements_alarmclock *clock, uint64_t
 
 /* Globals ********************************************************************/
 
-extern const implements_alarmclock *bootclock;
+extern implements_alarmclock *bootclock;
 
 #define sleep_until_ns(t) alarmclock_sleep_until_ns(bootclock, t)
 #define sleep_for_ns(t)   alarmclock_sleep_for_ns(bootclock, t)
diff --git a/libhw/host_alarmclock.c b/libhw/host_alarmclock.c
index a89294c..26aca94 100644
--- a/libhw/host_alarmclock.c
+++ b/libhw/host_alarmclock.c
@@ -5,8 +5,12 @@
  */
 
 #include <assert.h>
-#include <time.h> /* for clock_gettime(), CLOCK_MONOTONIC, clockid_t, struct timespec */
+#include <errno.h>
+#include <error.h>
+#include <signal.h>
+#include <time.h>
 
+#include <libcr/coroutine.h>
 #include <libmisc/vcall.h>
 
 #define IMPLEMENTATION_FOR_LIBHW_GENERIC_ALARMCLOCK_H YES
@@ -14,6 +18,8 @@
 
 #include "host_sigrt.h"
 
+/* Types **********************************************************************/
+
 struct hostclock {
 	implements_alarmclock;
 	bool                             initialized;
@@ -22,26 +28,53 @@ struct hostclock {
 	struct alarmclock_trigger       *queue;
 };
 
-static uint64_t hostclock_get_ns(implements_alarmclock *_alarmclock) {
+/* Globals ********************************************************************/
+
+static uint64_t hostclock_get_time_ns(implements_alarmclock *self);
+static bool     hostclock_add_trigger(implements_alarmclock *self,
+                                      struct alarmclock_trigger *trigger,
+                                      uint64_t   fire_at_ns,
+                                      void       (*cb)(void *),
+                                      void      *cb_arg);
+static void     hostclock_del_trigger(implements_alarmclock *self,
+                                      struct alarmclock_trigger *trigger);
+
+static struct alarmclock_vtable hostclock_vtable = {
+	.get_time_ns = hostclock_get_time_ns,
+	.add_trigger = hostclock_add_trigger,
+	.del_trigger = hostclock_del_trigger,
+};
+
+static struct hostclock clock_monotonic = {
+	.vtable   = &hostclock_vtable,
+	.clock_id = CLOCK_MONOTONIC,
+};
+
+implements_alarmclock *bootclock = &clock_monotonic;
+
+/* Main implementation ********************************************************/
+
+#define UNUSED(name) /* name __attribute__ ((unused)) */
+
+static uint64_t hostclock_get_time_ns(implements_alarmclock *_alarmclock) {
 	struct hostclock *alarmclock =
 		VCALL_SELF(struct hostclock, implements_alarmclock, _alarmclock);
 	assert(alarmclock);
 
 	struct timespec ts;
-	int r;
 
-	r = clock_gettime(alarmclock->clock_id, &ts);
-	assert(r == 0);
+	if (clock_gettime(alarmclock->clock_id, &ts) != 0)
+		error(1, errno, "clock_gettime(%d)", (int)alarmclock->clock_id);
 
-	return (((uint64_t)ts.tv_sec) * NS_PER_S) + (uint64_t)ts.tv_nsec;
+	return ns_from_host_ns_time(ts);
 }
 
 static void hostclock_handle_sig_alarm(int UNUSED(sig), siginfo_t *info, void *UNUSED(ucontext)) {
 	struct hostclock *alarmclock = info->si_value.sival_ptr;
-	assert(alarmclock)
+	assert(alarmclock);
 
 	while (alarmclock->queue &&
-	       alarmclock->queue->fire_at_ns <= hostclock_get_ns(alarmclock)) {
+	       alarmclock->queue->fire_at_ns <= hostclock_get_time_ns(alarmclock)) {
 		struct alarmclock_trigger *trigger = alarmclock->queue;
 		trigger->cb(trigger->cb_arg);
 		alarmclock->queue = trigger->next;
@@ -51,37 +84,91 @@ static void hostclock_handle_sig_alarm(int UNUSED(sig), siginfo_t *info, void *U
 	}
 
 	if (alarmclock->queue) {
-		timer_settime(alarmclock
+		struct itimerspec alarmspec = {
+			.it_value    = ns_to_host_ns_time(alarmclock->queue->fire_at_ns),
+			.it_interval = {0},
+		};
+		if (timer_settime(alarmclock->timer_id, TIMER_ABSTIME, &alarmspec, NULL) != 0)
+			error(1, errno, "timer_settime");
 	}
-		timer_hw->alarm[alarm_num] = (uint32_t)NS_TO_US_ROUNDUP(alarmclock->queue->fire_at_ns);
 }
 
-static bool hostclock_add_trigger(implements_alarmclock *self,
+static bool hostclock_add_trigger(implements_alarmclock *_alarmclock,
                                       struct alarmclock_trigger *trigger,
                                       uint64_t   fire_at_ns,
                                       void       (*cb)(void *),
                                       void      *cb_arg) {
-	struct hostclock *clock =
-		VCALL_SELF(struct hostclock, implements_alarmclock, _clock);
-
+	struct hostclock *alarmclock =
+		VCALL_SELF(struct hostclock, implements_alarmclock, _alarmclock);
+	assert(alarmclock);
+	assert(trigger);
+	assert(fire_at_ns);
+	assert(cb);
+
+	trigger->alarmclock = alarmclock;
+	trigger->fire_at_ns = fire_at_ns;
+	trigger->cb         = cb;
+	trigger->cb_arg     = cb_arg;
+
+	cr_disable_interrupts();
+	struct alarmclock_trigger **dst = &alarmclock->queue;
+	while (*dst && fire_at_ns >= (*dst)->fire_at_ns)
+		dst = &(*dst)->next;
+	trigger->next = *dst;
+	trigger->prev = *dst ? (*dst)->prev : NULL;
+	if (*dst)
+		(*dst)->prev = trigger;
+	*dst = trigger;
+	if (!alarmclock->initialized) {
+		struct sigevent how_to_notify = {
+			.sigev_notify = SIGEV_SIGNAL,
+			.sigev_signo  = host_sigrt_alloc(),
+			.sigev_value  = {
+				.sival_ptr = alarmclock,
+			},
+		};
+		struct sigaction action = {
+			.sa_flags = SA_SIGINFO,
+			.sa_sigaction = hostclock_handle_sig_alarm,
+		};
+		if (sigaction(how_to_notify.sigev_signo, &action, NULL) != 0)
+			error(1, errno, "sigaction");
+		if (timer_create(alarmclock->clock_id, &how_to_notify, &alarmclock->timer_id) != 0)
+			error(1, errno, "timer_create(%d)", (int)alarmclock->clock_id);
+		alarmclock->initialized = true;
+	}
+	if (alarmclock->queue == trigger) {
+		struct itimerspec alarmspec = {
+			.it_value    = ns_to_host_ns_time(trigger->fire_at_ns),
+			.it_interval = {0},
+		};
+		if (timer_settime(alarmclock->timer_id, TIMER_ABSTIME, &alarmspec, NULL) != 0)
+			error(1, errno, "timer_settime");
+	}
+	cr_enable_interrupts();
 
+	return false;
 }
 
-static void hostclock_del_trigger(implements_alarmclock *self,
+static void hostclock_del_trigger(implements_alarmclock *_alarmclock,
                                       struct alarmclock_trigger *trigger) {
-}
-
-/* Globals ********************************************************************/
-
-static struct alarmclock_vtable hostclock_vtable = {
-	.get_time_ns = hostclock_get_time_ns,
-	.add_trigger = hostclock_add_trigger,
-	.del_trigger = hostclock_del_trigger,
-};
-
-static struct hostclock clock_monotonic = {
-	.vtable  = hostclock_vtable,
-	.clockid = CLOCK_MONOTONIC,
-};
+	struct hostclock *alarmclock =
+		VCALL_SELF(struct hostclock, implements_alarmclock, _alarmclock);
 
-const implements_alarmclock *bootclock = &clock_monotonic;
+	assert(alarmclock);
+	assert(trigger);
+
+	cr_disable_interrupts();
+	if (trigger->alarmclock == alarmclock) {
+		if (!trigger->prev)
+			alarmclock->queue = trigger->next;
+		else
+			trigger->prev->next = trigger->next;
+		if (trigger->next)
+			trigger->next->prev = trigger->prev;
+		trigger->alarmclock = NULL;
+		trigger->prev = NULL;
+		trigger->next = NULL;
+	}
+	cr_enable_interrupts();
+}
diff --git a/libhw/host_net.c b/libhw/host_net.c
index 97e7726..9d1ffcf 100644
--- a/libhw/host_net.c
+++ b/libhw/host_net.c
@@ -235,7 +235,7 @@ static ssize_t hostnet_tcp_read(implements_net_stream_conn *_conn, void *buf, si
 			return -NET_ETIMEDOUT;
 		args.timeout = ns_to_host_us_time(conn->read_deadline_ns-now_ns);
 	} else {
-		args.timeout = ns_to_host_us_time(0);
+		args.timeout = (host_us_time_t){0};
 	}
 
 	if (RUN_PTHREAD(hostnet_pthread_read, &args))
@@ -500,7 +500,7 @@ static ssize_t hostnet_udp_recvfrom(implements_net_packet_conn *_conn, void *buf
 			return -NET_ETIMEDOUT;
 		args.timeout = ns_to_host_us_time(conn->read_deadline_ns-now_ns);
 	} else {
-		args.timeout = ns_to_host_us_time(0);
+		args.timeout = (host_us_time_t){0};
 	}
 
 	if (RUN_PTHREAD(hostnet_pthread_recvfrom, &args))
diff --git a/libhw/host_sigrt.c b/libhw/host_sigrt.c
index 66041fc..6dcb963 100644
--- a/libhw/host_sigrt.c
+++ b/libhw/host_sigrt.c
@@ -9,7 +9,7 @@
 
 #include "host_sigrt.h"
 
-int host_alloc_sigrt(void) {
+int host_sigrt_alloc(void) {
 	static int next = 0;
 
 	if (!next)
diff --git a/libhw/host_sigrt.h b/libhw/host_sigrt.h
index fa5675b..e8be26a 100644
--- a/libhw/host_sigrt.h
+++ b/libhw/host_sigrt.h
@@ -35,13 +35,13 @@ static inline host_ns_time_t ns_to_host_ns_time(uint64_t time_ns) {
 }
 
 static inline uint64_t ns_from_host_us_time(host_us_time_t host_time) {
-	return (((uint64_t)ts.tv_sec) * NS_PER_S) +
-		((uint64_t)ts.tv_nsec * (NS_PER_S/US_PER_S));
+	return (((uint64_t)host_time.tv_sec) * NS_PER_S) +
+		((uint64_t)host_time.tv_usec * (NS_PER_S/US_PER_S));
 }
 
 static inline uint64_t ns_from_host_ns_time(host_ns_time_t host_time) {
-	return (((uint64_t)ts.tv_sec) * NS_PER_S) +
-		((uint64_t)ts.tv_nsec);
+	return (((uint64_t)host_time.tv_sec) * NS_PER_S) +
+		((uint64_t)host_time.tv_nsec);
 }
 
 #endif /* _LIBHW_HOST_SIGRT_H_ */
diff --git a/libhw/rp2040_hwtimer.c b/libhw/rp2040_hwtimer.c
index 6daa69d..54bdab3 100644
--- a/libhw/rp2040_hwtimer.c
+++ b/libhw/rp2040_hwtimer.c
@@ -16,7 +16,7 @@
 #include <libhw/generic/alarmclock.h>
 #include <libhw/rp2040_hwtimer.h>
 
-/* Init ***********************************************************************/
+/* Types **********************************************************************/
 
 struct rp2040_hwtimer {
 	implements_alarmclock;
@@ -25,6 +25,8 @@ struct rp2040_hwtimer {
 	struct alarmclock_trigger       *queue;
 };
 
+/* Globals ********************************************************************/
+
 static uint64_t rp2040_hwtimer_get_time_ns(implements_alarmclock *self);
 static bool     rp2040_hwtimer_add_trigger(implements_alarmclock *self,
                                            struct alarmclock_trigger *trigger,
@@ -48,12 +50,15 @@ static struct rp2040_hwtimer hwtimers[] = {
 };
 static_assert(sizeof(hwtimers)/sizeof(hwtimers[0]) == _RP2040_HWALARM_NUM);
 
+implements_alarmclock *bootclock = &hwtimers[0];
+
+/* Main implementation ********************************************************/
+
 implements_alarmclock *rp2040_hwtimer(enum rp2040_hwalarm_instance alarm_num) {
 	assert(alarm_num < _RP2040_HWALARM_NUM);
 	return &hwtimers[alarm_num];
 }
 
-/* Main implementation ********************************************************/
 
 static uint64_t rp2040_hwtimer_get_time_ns(implements_alarmclock *) {
 	return timer_time_us_64(timer_hw) * (NS_PER_S/US_PER_S);
@@ -159,7 +164,3 @@ static void rp2040_hwtimer_del_trigger(implements_alarmclock     *_alarmclock,
 	}
 	cr_enable_interrupts();
 }
-
-/* Globals ********************************************************************/
-
-const implements_alarmclock *bootclock = &hwtimers[0];