1 files changed, 120 insertions, 75 deletions

diff --git a/libhw/rp2040_hwtimer.c b/libhw/rp2040_hwtimer.c
index b8255b8..6daa69d 100644
--- a/libhw/rp2040_hwtimer.c
+++ b/libhw/rp2040_hwtimer.c
@@ -1,120 +1,165 @@
-/* libhw/rp2040_hwtimer.c - Manage the RP2040's hardware timer
+/* libhw/rp2040_hwtimer.c - <libhw/generic/alarmclock.h> implementation for the RP2040's hardware timer
  *
  * Copyright (C) 2024  Luke T. Shumaker <lukeshu@lukeshu.com>
  * SPDX-License-Identifier: AGPL-3.0-or-later
  */
 
+#include <assert.h>
+
 #include <hardware/irq.h>   /* pico-sdk:hardware_irq */
 #include <hardware/timer.h> /* pico-sdk:hardware_timer */
 
 #include <libcr/coroutine.h>
+#include <libmisc/vcall.h>
+
+#define IMPLEMENTATION_FOR_LIBHW_GENERIC_ALARMCLOCK_H YES
+#include <libhw/generic/alarmclock.h>
+#include <libhw/rp2040_hwtimer.h>
+
+/* Init ***********************************************************************/
+
+struct rp2040_hwtimer {
+	implements_alarmclock;
+	enum rp2040_hwalarm_instance     alarm_num;
+	bool                             initialized;
+	struct alarmclock_trigger       *queue;
+};
+
+static uint64_t rp2040_hwtimer_get_time_ns(implements_alarmclock *self);
+static bool     rp2040_hwtimer_add_trigger(implements_alarmclock *self,
+                                           struct alarmclock_trigger *trigger,
+                                           uint64_t   fire_at_ns,
+                                           void       (*cb)(void *),
+                                           void      *cb_arg);
+static void     rp2040_hwtimer_del_trigger(implements_alarmclock *self,
+                                           struct alarmclock_trigger *trigger);
+
+static struct alarmclock_vtable rp2040_hwtimer_vtable = {
+	.get_time_ns = rp2040_hwtimer_get_time_ns,
+	.add_trigger = rp2040_hwtimer_add_trigger,
+	.del_trigger = rp2040_hwtimer_del_trigger,
+};
+
+static struct rp2040_hwtimer hwtimers[] = {
+	{ .vtable = &rp2040_hwtimer_vtable, .alarm_num = 0 },
+	{ .vtable = &rp2040_hwtimer_vtable, .alarm_num = 1 },
+	{ .vtable = &rp2040_hwtimer_vtable, .alarm_num = 2 },
+	{ .vtable = &rp2040_hwtimer_vtable, .alarm_num = 3 },
+};
+static_assert(sizeof(hwtimers)/sizeof(hwtimers[0]) == _RP2040_HWALARM_NUM);
+
+implements_alarmclock *rp2040_hwtimer(enum rp2040_hwalarm_instance alarm_num) {
+	assert(alarm_num < _RP2040_HWALARM_NUM);
+	return &hwtimers[alarm_num];
+}
 
-#define IMPLEMENTATION_FOR_LIBHW_RP2040_HWALARM_H YES
-#include <libhw/rp2040_hwalarm.h>
-#include <libhw/generic/bootclock.h>
+/* Main implementation ********************************************************/
 
-uint64_t bootclock_get_ns(void) {
-	return timer_time_us_64(timer_hw)*(NS_PER_S/US_PER_S);
+static uint64_t rp2040_hwtimer_get_time_ns(implements_alarmclock *) {
+	return timer_time_us_64(timer_hw) * (NS_PER_S/US_PER_S);
 }
 
-static struct {
-	bool                         initialized;
-	struct rp2040_hwalarm_trigger *queue;
-} alarms[_RP2040_HWALARM_NUM] = {0};
-
-static void rp2040_hwalarm_intrhandler(void) {
-    uint irq_num = __get_current_exception() - VTABLE_FIRST_IRQ;
-    uint alarm_num = TIMER_ALARM_NUM_FROM_IRQ(irq_num);
-    assert(alarm_num < _RP2040_HWALARM_NUM);
-
-    typeof(alarms[0]) *alarm = &alarms[alarm_num];
-
-    uint64_t now_us = timer_time_us_64(timer_hw);
-    while (alarm->queue && alarm->queue->fire_at_us <= now_us) {
-	    struct rp2040_hwalarm_trigger *trigger = alarm->queue;
-	    trigger->cb(trigger->cb_arg);
-	    alarm->queue = trigger->next;
-	    trigger->alarm = NULL;
-	    trigger->next = NULL;
-	    trigger->prev = NULL;
-	    now_us = timer_time_us_64(timer_hw);
-    }
-
-    hw_clear_bits(&timer_hw->intf, 1 << alarm_num);
-    if (alarm->queue)
-	    timer_hw->alarm[alarm_num] = (uint32_t)alarm->queue->fire_at_us;
-}
+#define NS_TO_US_ROUNDUP(x) ( ( (x) + (NS_PER_S/US_PER_S)-1) / (NS_PER_S/US_PER_S) )
 
-bool rp2040_hwalarm_trigger_init(struct rp2040_hwalarm_trigger *self,
-                                 enum rp2040_hwalarm_instance alarm_num,
-                                 uint64_t                     fire_at_ns,
-                                 void                         (*cb)(void *),
-                                 void                        *cb_arg) {
-	assert(self);
+static void rp2040_hwtimer_intrhandler(void) {
+	uint irq_num = __get_current_exception() - VTABLE_FIRST_IRQ;
+	enum rp2040_hwalarm_instance alarm_num = TIMER_ALARM_NUM_FROM_IRQ(irq_num);
 	assert(alarm_num < _RP2040_HWALARM_NUM);
+
+	struct rp2040_hwtimer *alarmclock = &hwtimers[alarm_num];
+
+	while (alarmclock->queue &&
+	       NS_TO_US_ROUNDUP(alarmclock->queue->fire_at_ns) <= timer_time_us_64(timer_hw)) {
+		struct alarmclock_trigger *trigger = alarmclock->queue;
+		trigger->cb(trigger->cb_arg);
+		alarmclock->queue = trigger->next;
+		trigger->alarmclock = NULL;
+		trigger->next = NULL;
+		trigger->prev = NULL;
+	}
+
+	hw_clear_bits(&timer_hw->intf, 1 << alarm_num);
+	if (alarmclock->queue)
+		timer_hw->alarm[alarm_num] = (uint32_t)NS_TO_US_ROUNDUP(alarmclock->queue->fire_at_ns);
+}
+
+static bool rp2040_hwtimer_add_trigger(implements_alarmclock     *_alarmclock,
+                                       struct alarmclock_trigger *trigger,
+                                       uint64_t                   fire_at_ns,
+                                       void                       (*cb)(void *),
+                                       void                      *cb_arg) {
+	struct rp2040_hwtimer *alarmclock =
+		VCALL_SELF(struct rp2040_hwtimer, implements_alarmclock, _alarmclock);
+	assert(alarmclock);
+	assert(trigger);
 	assert(fire_at_ns);
 	assert(cb);
 
-	uint64_t fire_at_us = (fire_at_ns + (NS_PER_S/US_PER_S) - 1)/(NS_PER_S/US_PER_S);
-
 	uint64_t now_us = timer_time_us_64(timer_hw);
-	if (fire_at_us > now_us && (fire_at_us - now_us) > UINT32_MAX)
+	if (NS_TO_US_ROUNDUP(fire_at_ns) > now_us &&
+	    (NS_TO_US_ROUNDUP(fire_at_ns) - now_us) > UINT32_MAX)
 		/* Too far in the future.  */
 		return true;
 
-	typeof(alarms[0]) *alarm = &alarms[alarm_num];
-
-	self->alarm      = alarm;
-	self->fire_at_us = fire_at_us;
-	self->cb         = cb;
-	self->cb_arg     = cb_arg;
+	trigger->alarmclock = alarmclock;
+	trigger->fire_at_ns = fire_at_ns;
+	trigger->cb         = cb;
+	trigger->cb_arg     = cb_arg;
 
 	cr_disable_interrupts();
-	struct rp2040_hwalarm_trigger **dst = &alarm->queue;
-	while (*dst && fire_at_us >= (*dst)->fire_at_us)
+	struct alarmclock_trigger **dst = &alarmclock->queue;
+	while (*dst && fire_at_ns >= (*dst)->fire_at_ns)
 		dst = &(*dst)->next;
-	self->next = *dst;
-	self->prev = *dst ? (*dst)->prev : NULL;
+	trigger->next = *dst;
+	trigger->prev = *dst ? (*dst)->prev : NULL;
 	if (*dst)
-		(*dst)->prev = self;
-	*dst = self;
-	if (!alarm->initialized) {
-		hw_set_bits(&timer_hw->inte, 1 << alarm_num);
-		irq_set_exclusive_handler(TIMER_ALARM_IRQ_NUM(timer_hw, alarm_num),
-		                          rp2040_hwalarm_intrhandler);
-		irq_set_enabled(TIMER_ALARM_IRQ_NUM(timer_hw, alarm_num), true);
-		alarm->initialized = true;
+		(*dst)->prev = trigger;
+	*dst = trigger;
+	if (!alarmclock->initialized) {
+		hw_set_bits(&timer_hw->inte, 1 << alarmclock->alarm_num);
+		irq_set_exclusive_handler(TIMER_ALARM_IRQ_NUM(timer_hw, alarmclock->alarm_num),
+		                          rp2040_hwtimer_intrhandler);
+		irq_set_enabled(TIMER_ALARM_IRQ_NUM(timer_hw, alarmclock->alarm_num), true);
+		alarmclock->initialized = true;
 	}
-	if (alarm->queue == self) {
+	if (alarmclock->queue == trigger) {
 		/* Force the interrupt handler to trigger as soon as
 		 * we enable interrupts.  This handles the case of
-		 * when fire_at_us is before when we called
+		 * when fire_at_ns is before when we called
 		 * cr_disable_interrupts().  We could check
 		 * timer_time_us_64() again after calling
 		 * cr_disable_interrupts() and do this conditionally,
 		 * but I don't think that would be any more efficient
 		 * than just letting the interrupt fire.  */
-		hw_set_bits(&timer_hw->intf, 1 << alarm_num);
+		hw_set_bits(&timer_hw->intf, 1 << alarmclock->alarm_num);
 	}
 	cr_enable_interrupts();
 
 	return false;
 }
 
-void rp2040_hwalarm_trigger_cancel(struct rp2040_hwalarm_trigger *self) {
-	assert(self);
+static void rp2040_hwtimer_del_trigger(implements_alarmclock     *_alarmclock,
+                                       struct alarmclock_trigger *trigger) {
+	struct rp2040_hwtimer *alarmclock =
+		VCALL_SELF(struct rp2040_hwtimer, implements_alarmclock, _alarmclock);
+	assert(alarmclock);
+	assert(trigger);
+
 	cr_disable_interrupts();
-	if (self->alarm) {
-		typeof(alarms[0]) *alarm = self->alarm;
-		if (!self->prev)
-			alarm->queue = self->next;
+	if (trigger->alarmclock == alarmclock) {
+		if (!trigger->prev)
+			alarmclock->queue = trigger->next;
 		else
-			self->prev->next = self->next;
-		if (self->next)
-			self->next->prev = self->prev;
-		self->alarm = NULL;
-		self->prev = NULL;
-		self->next = NULL;
+			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();
 }
+
+/* Globals ********************************************************************/
+
+const implements_alarmclock *bootclock = &hwtimers[0];