From 3908a7f5833f09fcd5383b9f15c830d71500f287 Mon Sep 17 00:00:00 2001
From: David Holm <dholmster@gmail.com>
Date: Thu, 3 Jan 2019 18:08:54 +0100
Subject: [PATCH] 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.
---
 main.cpp      | 23 ++-----------
 menu.h        |  2 ++
 scheduler.cpp | 94 +++++++++++++++++++++++++++++++++++++++++++++++++++
 scheduler.h   |  8 +++++
 4 files changed, 107 insertions(+), 20 deletions(-)
 create mode 100644 scheduler.cpp
 create mode 100644 scheduler.h

diff --git a/main.cpp b/main.cpp
index d0b0ca7..e9107a9 100644
--- a/main.cpp
+++ b/main.cpp
@@ -30,6 +30,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "user_io.h"
 #include "input.h"
 #include "fpga_io.h"
+#include "scheduler.h"
 
 const char *version = "$VER:HPS" VDATE;
 
@@ -75,26 +76,8 @@ int main(int argc, char *argv[])
 	FindStorage();
 	user_io_init((argc > 1) ? argv[1] : "");
 
-	while(1)
-	{
-		if(!is_fpga_ready(1))
-		{
-			printf("FPGA is not ready. JTAG uploading?\n");
-			printf("Waiting for FPGA to be ready...\n");
+	scheduler_init();
+	scheduler_run();
 
-			//enable reset in advance
-			fpga_core_reset(1);
-
-			while (!is_fpga_ready(0))
-			{
-				sleep(1);
-			}
-			reboot(0);
-		}
-
-		user_io_poll();
-		input_poll(0);
-		HandleUI();
-	}
 	return 0;
 }
diff --git a/menu.h b/menu.h
index 3705abe..bebf1f0 100644
--- a/menu.h
+++ b/menu.h
@@ -1,6 +1,8 @@
 #ifndef MENU_H
 #define MENU_H
 
+#include <inttypes.h>
+
 // UI strings, used by boot messages
 extern const char *config_memory_chip_msg[];
 extern const char *config_memory_slow_msg[];
diff --git a/scheduler.cpp b/scheduler.cpp
new file mode 100644
index 0000000..e504126
--- /dev/null
+++ b/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);
+}
diff --git a/scheduler.h b/scheduler.h
new file mode 100644
index 0000000..15207fb
--- /dev/null
+++ b/scheduler.h
@@ -0,0 +1,8 @@
+#ifndef SCHEDULER_H
+#define SCHEDULER_H
+
+void scheduler_init(void);
+void scheduler_run(void);
+void scheduler_yield(void);
+
+#endif