1 files changed, 58 insertions, 0 deletions
diff --git a/coroutine.c b/coroutine.c
new file mode 100644
index 0000000..3228663
--- /dev/null
+++ b/coroutine.c
@@ -0,0 +1,58 @@
+/* coroutine.c - Simple coroutine and request/response implementation
+ *
+ * Copyright (C) 2024  Luke T. Shumaker <lukeshu@lukeshu.com>
+ * SPDX-Licence-Identifier: AGPL-3.0-or-later
+ */
+
+#include <stdlib.h> /* for malloc(pico_malloc) and realloc(pico_malloc) */
+
+#include "coroutine.h"
+
+static cid_t coroutine_table_len = 0;
+_cr_entry_t *_coroutine_table    = NULL;
+cid_t        _cur_cid            = 0;
+
+void coroutine_init(void) {
+	if (coroutine_table_len)
+		return;
+	coroutine_table_len = 1;
+	_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);
+	}
+	_coroutine_table[cid] = (_cr_entry_t){
+		.fn = fn,
+		.stack = stack,
+		.state = 0,
+		.blocked = false,
+	};
+	return cid;
+}
+
+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)
+		return;
+	_cur_cid = (start + shift) % coroutine_table_len;
+	_coroutine_table[_cur_cid].fn(_coroutine_table[_cur_cid].stack);
+}