1 files changed, 86 insertions, 23 deletions

diff --git a/src/fiber.c b/src/fiber.c
index 0db2984..72da440 100644
--- a/src/fiber.c
+++ b/src/fiber.c
@@ -9,27 +9,36 @@
  *
  * See http://www.gnu.org/ for details.
  */
+
+#include <time.h>
 #include <errno.h>
+#include <unistd.h>
 #include <libtf/tf.h>
-#include TF_UCTX_H
+#include <libtf/heap.h>
 
-struct tf_scheduler {
-	struct tf_list_head	run_q;
-	struct tf_list_head	sleep_q;
-
-	struct tf_fiber *	active_fiber;
-	int			num_fibers;
+struct tf_fiber {
+	unsigned int		ref_count;
+	struct tf_list_node	queue_node;
+	struct tf_heap_node	heap_node;
+	char			data[TF_EMPTY_ARRAY];
 };
 
+#include TF_UCTX_H
+
 /* FIXME: should be in thread local storage */
-static struct tf_scheduler *__scheduler;
+struct tf_scheduler *__tf_scheduler;
 
 void *tf_fiber_create(tf_fiber_proc fiber_main, int private_size)
 {
-	struct tf_scheduler *sched = __scheduler;
+	struct tf_scheduler *sched = tf_get_scheduler();
 	struct tf_fiber *fiber;
 
+	if (tf_heap_prealloc(&sched->heap, sched->num_fibers + 1) < 0)
+		return NULL;
+
 	fiber = tf_uctx_create(fiber_main, private_size);
+	if (fiber == NULL)
+		return NULL;
 
 	/* The initial references for caller and scheduler */
 	*fiber = (struct tf_fiber) {
@@ -43,8 +52,9 @@ void *tf_fiber_create(tf_fiber_proc fiber_main, int private_size)
 	return fiber->data;
 }
 
-void __tf_fiber_destroy(struct tf_fiber *fiber)
+static void __tf_fiber_destroy(struct tf_fiber *fiber)
 {
+	tf_heap_delete(&fiber->heap_node, &tf_get_scheduler()->heap);
 	tf_uctx_destroy(fiber);
 }
 
@@ -62,17 +72,17 @@ void tf_fiber_put(void *data)
 		__tf_fiber_destroy(fiber);
 }
 
-static void run_fiber(void)
+static void update_time(struct tf_scheduler *sched)
 {
-	struct tf_scheduler *sched = __scheduler;
-	struct tf_fiber *schedf = container_of((void*) __scheduler, struct tf_fiber, data);
-	struct tf_fiber *f;
+	struct timespec ts;
 
-	if (tf_list_empty(&sched->run_q))
-		return;
+	clock_gettime(CLOCK_MONOTONIC, &ts);
+	sched->scheduler_time = ts.tv_sec * 1000 + ts.tv_nsec / 1000000;
+}
 
-	f = tf_list_first(&sched->run_q, struct tf_fiber, queue_node);
-	tf_list_del(&f->queue_node);
+static void run_fiber(struct tf_scheduler *sched, struct tf_fiber *f)
+{
+	struct tf_fiber *schedf = container_of((void*) tf_get_scheduler(), struct tf_fiber, data);
 
 	sched->active_fiber = f;
 	switch (tf_uctx_transfer(schedf, f, 1)) {
@@ -91,6 +101,31 @@ static void run_fiber(void)
 	}
 }
 
+static void process_heap(struct tf_scheduler *sched)
+{
+	struct tf_heap_node *node;
+	struct tf_fiber *f;
+	tf_mtime_t now = tf_mtime();
+
+	while (!tf_heap_empty(&sched->heap) &&
+	       tf_mtime_diff(now, tf_heap_get_value(&sched->heap)) > 0) {
+		node = tf_heap_get_node(&sched->heap);
+		f = container_of(node, struct tf_fiber, heap_node);
+		run_fiber(sched, f);
+	}
+}
+
+static void process_runq(struct tf_scheduler *sched)
+{
+	struct tf_fiber *f;
+
+	while (!tf_list_empty(&sched->run_q)) {
+		f = tf_list_first(&sched->run_q, struct tf_fiber, queue_node);
+		tf_list_del(&f->queue_node);
+		run_fiber(sched, f);
+	}
+}
+
 int tf_main(tf_fiber_proc main_fiber)
 {
 	struct tf_uctx *ctx = alloca(sizeof(struct tf_uctx) + sizeof(struct tf_scheduler));
@@ -102,20 +137,38 @@ int tf_main(tf_fiber_proc main_fiber)
 		.run_q = TF_LIST_HEAD_INITIALIZER(sched->run_q),
 		.sleep_q = TF_LIST_HEAD_INITIALIZER(sched->sleep_q),
 	};
-	__scheduler = sched;
+	__tf_scheduler = sched;
+	update_time(sched);
 	tf_fiber_put(tf_fiber_create(main_fiber, 0));
 	do {
-		run_fiber();
+		tf_mtime_diff_t timeout;
+
+		update_time(sched);
+		if (!tf_list_empty(&sched->run_q)) {
+			timeout = 0;
+		} else if (!tf_heap_empty(&sched->heap)) {
+			timeout = tf_mtime_diff(tf_heap_get_value(&sched->heap),
+						sched->scheduler_time);
+			if (timeout < 0)
+				timeout = 0;
+		} else
+			timeout = -1;
+
+		if (timeout > 0)
+			usleep(timeout * 1000);
+
+		process_heap(sched);
+		process_runq(sched);
 	} while (likely(sched->num_fibers));
-	__scheduler = NULL;
+	__tf_scheduler = NULL;
 
 	return 0;
 }
 
 int tf_schedule(int err)
 {
-	struct tf_scheduler *sched = __scheduler;
-	struct tf_fiber *schedf = container_of((void*) __scheduler, struct tf_fiber, data);
+	struct tf_scheduler *sched = tf_get_scheduler();
+	struct tf_fiber *schedf = container_of((void*) sched, struct tf_fiber, data);
 	struct tf_fiber *f = sched->active_fiber;
 	int r;
 
@@ -126,6 +179,16 @@ int tf_schedule(int err)
 	return r;
 }
 
+int tf_msleep(int milliseconds)
+{
+	struct tf_scheduler *sched = tf_get_scheduler();
+	struct tf_fiber *f = sched->active_fiber;
+
+	tf_heap_change(&f->heap_node, &sched->heap, tf_mtime() + milliseconds);
+
+	return tf_schedule(EIO);
+}
+
 void tf_kill(void *fiber)
 {
 }