diff --git a/fpga_io.cpp b/fpga_io.cpp
index 555eaf4..70fc114 100644
--- a/fpga_io.cpp
+++ b/fpga_io.cpp
@@ -657,6 +657,20 @@ void fpga_spi_en(uint32_t mask, uint32_t en)
 	fpga_gpo_write(en ? gpo | mask : gpo & ~mask);
 }
 
+void fpga_wait_to_reset()
+{
+	printf("FPGA is not ready. JTAG uploading?\n");
+	printf("Waiting for FPGA to be ready...\n");
+
+	fpga_core_reset(1);
+
+	while (!is_fpga_ready(0))
+	{
+		sleep(1);
+	}
+	reboot(0);
+}
+
 uint16_t fpga_spi(uint16_t word)
 {
 	uint32_t gpo = (fpga_gpo_read() & ~(0xFFFF | SSPI_STROBE)) | word;
@@ -671,6 +685,7 @@ uint16_t fpga_spi(uint16_t word)
 		if (gpi < 0)
 		{
 			printf("GPI[31]==1. FPGA is uninitialized?\n");
+			fpga_wait_to_reset();
 			return 0;
 		}
 	} while (!(gpi & SSPI_ACK));
@@ -683,6 +698,7 @@ uint16_t fpga_spi(uint16_t word)
 		if (gpi < 0)
 		{
 			printf("GPI[31]==1. FPGA is uninitialized?\n");
+			fpga_wait_to_reset();
 			return 0;
 		}
 	} while (gpi & SSPI_ACK);
diff --git a/fpga_io.h b/fpga_io.h
index 53a3f43..5448e24 100644
--- a/fpga_io.h
+++ b/fpga_io.h
@@ -39,4 +39,6 @@ void reboot(int cold);
 void app_restart(const char *path, const char *xml = 0);
 char *getappname();
 
+void fpga_wait_to_reset();
+
 #endif
diff --git a/main.cpp b/main.cpp
index 68cc7fb..f9109d2 100644
--- a/main.cpp
+++ b/main.cpp
@@ -76,13 +76,7 @@ int main(int argc, char *argv[])
 	{
 		if (!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);
+			fpga_wait_to_reset();
 		}
 
 		user_io_poll();
diff --git a/scheduler.cpp b/scheduler.cpp
index 094f563..26979d6 100644
--- a/scheduler.cpp
+++ b/scheduler.cpp
@@ -16,17 +16,7 @@ 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);
+		fpga_wait_to_reset();
 	}
 }