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