wip

1 files changed, 148 insertions, 63 deletions

diff --git a/coroutine.c b/coroutine.c
index 3062ad9..65971d4 100644
--- a/coroutine.c
+++ b/coroutine.c
@@ -4,79 +4,164 @@
  * SPDX-Licence-Identifier: AGPL-3.0-or-later
  */
 
-#include <stdio.h>
-#include <stdlib.h> /* for malloc(pico_malloc) and realloc(pico_malloc) */
+#include <stdint.h> /* for uint8_t */
+#include <string.h> /* for memset() */
+#include <stdio.h>  /* for fprintf(), stderr */
+#include <assert.h>
+#include <setjmp.h>
 
+#include "cfg_limits.h"
 #include "coroutine.h"
 
-static cid_t coroutine_table_len = 0;
-_cr_entry_t *_coroutine_table    = NULL;
-cid_t        _cur_cid            = 0;
+enum coroutine_state {
+	CR_ZERO = 0,
+	CR_GARBAGE,
+	CR_INITIALIZING,
+	CR_RUNNING,
+	CR_RUNNABLE,
+	CR_PAUSED,
+};
 
-void coroutine_init(void) {
-	printf("coroutine_table_len = %u\n", (int)coroutine_table_len);
-	if (coroutine_table_len) {
-		printf("return\n");
-		return;
+struct coroutine {
+	enum coroutine_state state;
+	jmp_buf              env;
+	uint8_t              stack[COROUTINE_STACK_SIZE];
+};
+
+static struct coroutine coroutine_table[COROUTINE_NUM] = {0};
+static cid_t            coroutine_running              = 0;
+static jmp_buf          scheduler_env;
+
+void coroutine_init(void) {}
+
+void *__sp;
+#define SP ({ asm("movq %%rsp,%0" : "=r"(__sp)); __sp; })
+
+static void print_addr(char *name, void *addr) {
+	if ((void*)&coroutine_table <= addr && addr < ((void*)&coroutine_table) + sizeof(coroutine_table)) {
+		for (size_t i = 0; i < COROUTINE_NUM; i++) {
+			if ((void*)&coroutine_table[i] <= addr && addr < ((void*)&coroutine_table[i]) + sizeof(coroutine_table[i])) {
+				printf("%s=%p (in coroutine_table[%zu] [%p, %p])\n", name, addr, i,
+				       (void*)&coroutine_table[i], ((void*)&coroutine_table[i]) + sizeof(coroutine_table[i]));
+				break;
+			}
+		}
+	} else {
+		printf("%s=%p (outside coroutine_table [%p, %p])\n", name, addr,
+		       (void*)&coroutine_table, ((void*)&coroutine_table) + sizeof(coroutine_table));
 	}
-	coroutine_table_len = 1;
-	printf("coroutine_table_len = %u\n", (int)coroutine_table_len);
-	_coroutine_table = malloc(sizeof _coroutine_table[0]);
-	_coroutine_table[0] = (_cr_entry_t){
-		.fn = (cr_fn_t)0xDEAD,
-		.stack = NULL,
-		.state = 0,
-		.blocked = true,
-	};
 }
 
-cid_t coroutine_add(cr_fn_t fn, void *stack) {
-	cid_t cid = 0;
-	for (cid_t i = 1; cid == 0 && i < coroutine_table_len; i++)
-		if (_coroutine_table[i].fn == NULL)
-			cid = i;
-	if (cid == 0) {
-		cid = coroutine_table_len++;
-		_coroutine_table = realloc(_coroutine_table, (sizeof _coroutine_table[0]) * coroutine_table_len);
+static void call_with_stack(void *stack, cr_fn_t fn, void *args) {
+	static void  *scheduler_sp = NULL;
+
+	/* I don't intend to run nThis only really exists for running on ARM-32, but being
+	 * able to run it on x86-64 is useful for debugging.  */
+#if __x86_64__
+#define STACK_GROWS_DOWNWARD 1
+	print_addr("sp", SP);
+	asm volatile ("movq %%rsp , %0\n\t"    /* scheduler_sp = sp */
+	              "movq %1    , %%rsp\n\t" /* sp = stack */
+	              "movq %3    , %%rdi\n\t" /* arg0 = args */
+	              "call *%2\n\t"           /* fn() */
+	              "movq %0    , %%rsp"     /* sp = scheduler_sp */
+	              :
+	              : /* %0 */"m"(scheduler_sp),
+	                /* %1 */"r"(stack),
+	                /* %2 */"r"(fn),
+	                /* %3 */"r"(args)
+	              :
+	              );
+#elif __arm__
+#define STACK_GROWS_DOWNWARD 1
+	asm volatile ("mov r0, coroutine_table[_cur_cid].arg"
+	              "mov _saved_stack sp"
+	              "mov sp, coroutine_table[_cur_cid].stack"
+	              "bl  coroutine_table[_cur_cid].fn"
+	              "mov _saved_stack sp");
+#else
+#error unsupported architecture
+#endif
+}
+
+cid_t coroutine_add(cr_fn_t fn, void *args) {
+	static cid_t last_created = 0;
+
+	cid_t child;
+	{
+		size_t idx_base = last_created;
+		for (size_t idx_shift = 0; idx_shift < COROUTINE_NUM; idx_shift++) {
+			child = ((idx_base + idx_shift) % COROUTINE_NUM) + 1;
+			if (coroutine_table[child-1].state == CR_ZERO || coroutine_table[child-1].state == CR_GARBAGE)
+				goto found;
+		}
+		return 0;
+	found:
+	}
+
+	if (coroutine_table[child-1].state == CR_GARBAGE)
+		memset(&coroutine_table[child-1], 0, sizeof(struct coroutine));
+	last_created = child;
+
+	cid_t parent = coroutine_running;
+	assert(parent == 0 || coroutine_table[parent-1].state == CR_RUNNING);
+	coroutine_running = child; 
+	coroutine_table[child-1].state = CR_INITIALIZING;
+	if (child == 2) {
+		print_addr("&child", &child);
 	}
-	_coroutine_table[cid] = (_cr_entry_t){
-		.fn = fn,
-		.stack = stack,
-		.state = 0,
-		.blocked = false,
-	};
-	printf("added coroutine %u\n", (int)cid);
-	return cid;
+	if (!setjmp(scheduler_env)) {
+		call_with_stack(coroutine_table[child-1].stack + (STACK_GROWS_DOWNWARD ? COROUTINE_STACK_SIZE : 0), fn, args);
+		assert(coroutine_table[child-1].state == CR_INITIALIZING);
+		coroutine_table[child-1].state = CR_RUNNABLE;
+	}
+	assert(coroutine_table[child-1].state == CR_RUNNABLE);
+	coroutine_running = parent;
+
+	return child;
 }
 
-void coroutine_task(void) {
-	cid_t start = (_cur_cid + 1) % coroutine_table_len;
-	cid_t shift;
-	for (shift = 0;
-	     shift < coroutine_table_len &&
-		     (_coroutine_table[(start+shift)%coroutine_table_len].fn == NULL ||
-		      _coroutine_table[(start+shift)%coroutine_table_len].blocked);
-	     shift++) {}
-	if (shift == coroutine_table_len) {
-		printf("idle\n");
-		return;
+void coroutine_main(void) {
+	bool ran;
+	for (coroutine_running = 1;; coroutine_running = (coroutine_running%COROUTINE_NUM)+1) {
+		if (coroutine_running == 1)
+			ran = false;
+		if (coroutine_table[coroutine_running-1].state == CR_RUNNABLE) {
+			ran = true;
+			coroutine_table[coroutine_running-1].state = CR_RUNNING;
+			if (!setjmp(scheduler_env)) {
+				longjmp(coroutine_table[coroutine_running-1].env, 1);
+				coroutine_table[coroutine_running-1].state = CR_GARBAGE;
+			}
+		}
+		if (coroutine_running == COROUTINE_NUM+1 && !ran) {
+			fprintf(stderr, "error: no runnable coroutines\n");
+			return;
+		}
 	}
-	_cur_cid = (start + shift) % coroutine_table_len;
-	printf("running %u\n", (int)_cur_cid);
-	if (_cur_cid == 0)
-		exit(1);
-	_coroutine_table[_cur_cid].fn(_coroutine_table[_cur_cid].stack);
 }
 
-// arm32
-mov r0, coroutine_table[_cur_cid].arg
-mov _saved_stack sp
-mov sp, coroutine_table[_cur_cid].stack
-bl  coroutine_table[_cur_cid].fn
-mov _saved_stack sp
-// x86-64
-movl coroutine_table[_cur_cid].arg %edi
-movq %rsp _saved_stack
-movq coroutine_table[_cur_cid].stack %rsp
-call coroutine_table[_cur_cid].fn
-movq _saved_stack %rsp
+bool _cr_begin(void) {
+	assert(coroutine_table[coroutine_running-1].state == CR_INITIALIZING);
+	return setjmp(coroutine_table[coroutine_running-1].env);
+}
+
+static inline void _cr_transition(enum coroutine_state state) {
+	assert(coroutine_table[coroutine_running-1].state == CR_RUNNING);
+	coroutine_table[coroutine_running-1].state = state;
+	if (state == CR_GARBAGE || !setjmp(coroutine_table[coroutine_running-1].env))
+		longjmp(scheduler_env, 1);
+}
+
+void cr_exit(void)            { _cr_transition(CR_GARBAGE); }
+void cr_yield(void)           { _cr_transition(CR_RUNNABLE); }
+void cr_pause_and_yield(void) { _cr_transition(CR_PAUSED); }
+
+void cr_unpause(cid_t cid) {
+	assert(coroutine_table[cid-1].state == CR_PAUSED);
+	coroutine_table[cid-1].state = CR_RUNNABLE;
+}
+
+cid_t cr_getcid(void) {
+	return coroutine_running;
+}