1 files changed, 48 insertions, 2 deletions

diff --git a/main.c b/main.c
index e9fc28e..95ee2f2 100644
--- a/main.c
+++ b/main.c
@@ -4,23 +4,68 @@
 #include <bug/panic.h>
 #include <bug/debug.h>
 #include <tshield/tshield.h>
+#include <collection/list.h>
 
 #define WAIT_CYCLES(cycles) for (volatile unsigned long i = 0; i < cycles; ++i) {}
 
+static struct list_head frozen = LIST_HEAD_INIT(frozen);
+void freeze() __attribute__ ( ( naked ) );
+void freeze() {
+  cli();
+  SAVE_CONTEXT();
+  sleep_on(&frozen);
+  schedule();
+  RESTORE_CONTEXT();
+  sei();
+  asm volatile ( "ret" );
+}
+
+void wake() __attribute__ ( ( naked ) );
+void wake() {
+  cli();
+  SAVE_CONTEXT();
+  wake_all(&frozen);
+  RESTORE_CONTEXT();
+  sei();
+  asm volatile ( "ret" );
+}
+
+
 void blink( char id) {
   while(1) {
     debug_led(id,1);
     //yield();
-    WAIT_CYCLES(50000);
+    WAIT_CYCLES(5000);
     debug_led(id,0);
     //yield();
-    WAIT_CYCLES(50000);
+    WAIT_CYCLES(5000);
+    freeze();
+  }
+}
+
+void read(char id) {
+  while(1) {
+    debug_led(0, 0);
+    debug_led(3, 0);
+    unsigned char buttons = tshield_read();
+    if (buttons != 0) {
+      wake();
+      debug_led(0, 1);
+      unsigned int counter = 0;
+      if (counter > 0) {
+        debug_led(3, 1);
+      }
+      if (list_empty(&frozen)) {
+        debug_led(3, 1);
+      }
+    }
   }
 }
 
 DECLARE_TASK(task1, STACK_SIZE, blink);
 DECLARE_TASK(task2, STACK_SIZE, blink);
 DECLARE_TASK(task3, STACK_SIZE, blink);
+DECLARE_TASK(task4, STACK_SIZE, read);
 
 
 int main(int argc, char *argv[]) {
@@ -31,6 +76,7 @@ int main(int argc, char *argv[]) {
   spawn(&task1, 1);
   spawn(&task2, 2);
   spawn(&task3, 3);
+  spawn(&task4, 4);
 
   timer_start();
   sched_init();