main, scheduler: Improve latency by coroutine scheduling

Schedules tasks in the main loop using coroutines so that long running
tasks, particularly relating to the UI, can yield execution in order to
call the poll functions at a tighter interval.

scheduler.cpp

@@ -0,0 +1,94 @@
+#include "scheduler.h"
+#include <stdio.h>
+#include "libco.h"
+#include "menu.h"
+#include "user_io.h"
+#include "input.h"
+#include "fpga_io.h"
+
+static cothread_t co_scheduler = nullptr;
+static cothread_t co_poll = nullptr;
+static cothread_t co_ui = nullptr;
+static cothread_t co_last = nullptr;
+
+static void scheduler_wait_fpga_ready(void)
+{
+	while (!is_fpga_ready(1))
+	{
+		printf("FPGA is not ready. JTAG uploading?\n");
+		printf("Waiting for FPGA to be ready...\n");
+
+		//enable reset in advance
+		fpga_core_reset(1);
+
+		while (!is_fpga_ready(0))
+		{
+			sleep(1);
+		}
+		reboot(0);
+	}
+}
+
+static void scheduler_co_poll(void)
+{
+	for (;;)
+	{
+		scheduler_wait_fpga_ready();
+
+		user_io_poll();
+		input_poll(0);
+
+		scheduler_yield();
+	}
+}
+
+static void scheduler_co_ui(void)
+{
+	for (;;)
+	{
+		HandleUI();
+
+		scheduler_yield();
+	}
+}
+
+static void scheduler_schedule(void)
+{
+	if (co_last == co_poll)
+	{
+		co_last = co_ui;
+		co_switch(co_ui);
+	}
+	else
+	{
+		co_last = co_poll;
+		co_switch(co_poll);
+	}
+}
+
+void scheduler_init(void)
+{
+	const unsigned int co_stack_size = 262144 * sizeof(void*);
+
+	co_poll = co_create(co_stack_size, scheduler_co_poll);
+	co_ui = co_create(co_stack_size, scheduler_co_ui);
+}
+
+void scheduler_run(void)
+{
+	co_scheduler = co_active();
+
+	for (;;)
+	{
+		scheduler_schedule();
+	}
+
+	co_delete(co_ui);
+	co_delete(co_poll);
+	co_delete(co_scheduler);
+}
+
+void scheduler_yield(void)
+{
+	co_switch(co_scheduler);
+}