1 files changed, 329 insertions, 0 deletions
diff --git a/c_src/linux/timer.c b/c_src/linux/timer.c
new file mode 100644
index 00000000..7d519a4d
--- /dev/null
+++ b/c_src/linux/timer.c
@@ -0,0 +1,329 @@
+
+#include <pthread.h>
+#include <signal.h>
+#include <time.h>
+
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/timer.h>
+
+/**
+ * timespec_add_ns - Adds nanoseconds to a timespec
+ * @a:		pointer to timespec to be incremented
+ * @ns:		unsigned nanoseconds value to be added
+ *
+ * This must always be inlined because its used from the x86-64 vdso,
+ * which cannot call other kernel functions.
+ */
+static struct timespec timespec_add_ns(struct timespec a, u64 ns)
+{
+	a.tv_nsec	+= ns;
+	a.tv_sec	+= a.tv_nsec / NSEC_PER_SEC;
+	a.tv_nsec	%= NSEC_PER_SEC;
+	return a;
+}
+
+#define DECLARE_HEAP(type)						\
+struct {								\
+	size_t size, used;						\
+	type *data;							\
+}
+
+#define heap_init(heap, _size)						\
+({									\
+	size_t _bytes;							\
+	(heap)->used = 0;						\
+	(heap)->size = (_size);						\
+	_bytes = (heap)->size * sizeof(*(heap)->data);			\
+	(heap)->data = malloc(_bytes);					\
+	(heap)->data;							\
+})
+
+#define heap_free(heap)							\
+do {									\
+	kvfree((heap)->data);						\
+	(heap)->data = NULL;						\
+} while (0)
+
+#define heap_swap(h, i, j)	swap((h)->data[i], (h)->data[j])
+
+#define heap_sift(h, i, cmp)						\
+do {									\
+	size_t _r, _j = i;						\
+									\
+	for (; _j * 2 + 1 < (h)->used; _j = _r) {			\
+		_r = _j * 2 + 1;					\
+		if (_r + 1 < (h)->used &&				\
+		    cmp((h)->data[_r], (h)->data[_r + 1]))		\
+			_r++;						\
+									\
+		if (cmp((h)->data[_r], (h)->data[_j]))			\
+			break;						\
+		heap_swap(h, _r, _j);					\
+	}								\
+} while (0)
+
+#define heap_sift_down(h, i, cmp)					\
+do {									\
+	while (i) {							\
+		size_t p = (i - 1) / 2;					\
+		if (cmp((h)->data[i], (h)->data[p]))			\
+			break;						\
+		heap_swap(h, i, p);					\
+		i = p;							\
+	}								\
+} while (0)
+
+#define heap_add(h, d, cmp)						\
+({									\
+	bool _r = !heap_full(h);					\
+	if (_r) {							\
+		size_t _i = (h)->used++;				\
+		(h)->data[_i] = d;					\
+									\
+		heap_sift_down(h, _i, cmp);				\
+		heap_sift(h, _i, cmp);					\
+	}								\
+	_r;								\
+})
+
+#define heap_del(h, i, cmp)						\
+do {									\
+	size_t _i = (i);						\
+									\
+	BUG_ON(_i >= (h)->used);					\
+	(h)->used--;							\
+	if ((_i) < (h)->used) {						\
+		heap_swap(h, _i, (h)->used);				\
+		heap_sift_down(h, _i, cmp);				\
+		heap_sift(h, _i, cmp);					\
+	}								\
+} while (0)
+
+#define heap_pop(h, d, cmp)						\
+({									\
+	bool _r = (h)->used;						\
+	if (_r) {							\
+		(d) = (h)->data[0];					\
+		heap_del(h, 0, cmp);					\
+	}								\
+	_r;								\
+})
+
+#define heap_peek(h)	((h)->used ? &(h)->data[0] : NULL)
+#define heap_full(h)	((h)->used == (h)->size)
+#define heap_empty(h)	((h)->used == 0)
+
+#define heap_resort(heap, cmp)						\
+do {									\
+	ssize_t _i;							\
+	for (_i = (ssize_t) (heap)->used / 2 -  1; _i >= 0; --_i)	\
+		heap_sift(heap, _i, cmp);				\
+} while (0)
+
+struct pending_timer {
+	struct timer_list	*timer;
+	unsigned long		expires;
+};
+
+static inline bool pending_timer_cmp(struct pending_timer a,
+				     struct pending_timer b)
+{
+	return a.expires < b.expires;
+}
+
+static DECLARE_HEAP(struct pending_timer) pending_timers;
+
+static pthread_mutex_t	timer_lock = PTHREAD_MUTEX_INITIALIZER;
+static pthread_cond_t	timer_cond = PTHREAD_COND_INITIALIZER;
+static pthread_cond_t	timer_running_cond = PTHREAD_COND_INITIALIZER;
+static unsigned long	timer_seq;
+
+static inline bool timer_running(void)
+{
+	return timer_seq & 1;
+}
+
+static ssize_t timer_idx(struct timer_list *timer)
+{
+	size_t i;
+
+	for (i = 0; i < pending_timers.used; i++)
+		if (pending_timers.data[i].timer == timer)
+			return i;
+
+	return -1;
+}
+
+int del_timer(struct timer_list *timer)
+{
+	ssize_t idx;
+
+	pthread_mutex_lock(&timer_lock);
+	idx = timer_idx(timer);
+	if (idx >= 0)
+		heap_del(&pending_timers, idx, pending_timer_cmp);
+
+	timer->pending = false;
+	pthread_mutex_unlock(&timer_lock);
+
+	return idx >= 0;
+}
+
+void flush_timers(void)
+{
+	unsigned long seq;
+
+	pthread_mutex_lock(&timer_lock);
+	seq = timer_seq;
+	while (timer_running() && seq == timer_seq)
+		pthread_cond_wait(&timer_running_cond, &timer_lock);
+
+	pthread_mutex_unlock(&timer_lock);
+}
+
+int del_timer_sync(struct timer_list *timer)
+{
+	unsigned long seq;
+	ssize_t idx;
+
+	pthread_mutex_lock(&timer_lock);
+	idx = timer_idx(timer);
+	if (idx >= 0)
+		heap_del(&pending_timers, idx, pending_timer_cmp);
+
+	timer->pending = false;
+
+	seq = timer_seq;
+	while (timer_running() && seq == timer_seq)
+		pthread_cond_wait(&timer_running_cond, &timer_lock);
+	pthread_mutex_unlock(&timer_lock);
+
+	return idx >= 0;
+}
+
+int mod_timer(struct timer_list *timer, unsigned long expires)
+{
+	ssize_t idx;
+
+	pthread_mutex_lock(&timer_lock);
+	timer->expires = expires;
+	timer->pending = true;
+	idx = timer_idx(timer);
+
+	if (idx >= 0 &&
+	    pending_timers.data[idx].expires == expires)
+		goto out;
+
+	if (idx >= 0) {
+		pending_timers.data[idx].expires = expires;
+
+		heap_sift_down(&pending_timers, idx, pending_timer_cmp);
+		heap_sift(&pending_timers, idx, pending_timer_cmp);
+	} else {
+		if (heap_full(&pending_timers)) {
+			pending_timers.size *= 2;
+			pending_timers.data =
+				realloc(pending_timers.data,
+					pending_timers.size *
+					sizeof(struct pending_timer));
+
+			BUG_ON(!pending_timers.data);
+		}
+
+		heap_add(&pending_timers,
+			 ((struct pending_timer) {
+				.timer = timer,
+				.expires = expires,
+			 }),
+			 pending_timer_cmp);
+	}
+
+	pthread_cond_signal(&timer_cond);
+out:
+	pthread_mutex_unlock(&timer_lock);
+
+	return idx >= 0;
+}
+
+static bool timer_thread_stop = false;
+
+static int timer_thread(void *arg)
+{
+	struct pending_timer *p;
+	struct timespec ts;
+	unsigned long now;
+	int ret;
+
+	pthread_mutex_lock(&timer_lock);
+
+	while (!timer_thread_stop) {
+		now = jiffies;
+		p = heap_peek(&pending_timers);
+
+		if (!p) {
+			pthread_cond_wait(&timer_cond, &timer_lock);
+			continue;
+		}
+
+		if (time_after_eq(now, p->expires)) {
+			struct timer_list *timer = p->timer;
+
+			heap_del(&pending_timers, 0, pending_timer_cmp);
+			BUG_ON(!timer_pending(timer));
+			timer->pending = false;
+
+			timer_seq++;
+			BUG_ON(!timer_running());
+
+			pthread_mutex_unlock(&timer_lock);
+			timer->function(timer);
+			pthread_mutex_lock(&timer_lock);
+
+			timer_seq++;
+			pthread_cond_broadcast(&timer_running_cond);
+			continue;
+		}
+
+
+		ret = clock_gettime(CLOCK_REALTIME, &ts);
+		BUG_ON(ret);
+
+		ts = timespec_add_ns(ts, jiffies_to_nsecs(p->expires - now));
+
+		pthread_cond_timedwait(&timer_cond, &timer_lock, &ts);
+	}
+
+	pthread_mutex_unlock(&timer_lock);
+
+	return 0;
+}
+
+struct task_struct *timer_task;
+
+__attribute__((constructor(103)))
+static void timers_init(void)
+{
+	heap_init(&pending_timers, 64);
+	BUG_ON(!pending_timers.data);
+
+	timer_task = kthread_run(timer_thread, NULL, "timers");
+	BUG_ON(IS_ERR(timer_task));
+}
+
+__attribute__((destructor(103)))
+static void timers_cleanup(void)
+{
+	get_task_struct(timer_task);
+
+	pthread_mutex_lock(&timer_lock);
+	timer_thread_stop = true;
+	pthread_cond_signal(&timer_cond);
+	pthread_mutex_unlock(&timer_lock);
+
+	int ret = kthread_stop(timer_task);
+	BUG_ON(ret);
+
+	put_task_struct(timer_task);
+	timer_task = NULL;
+}