implement sleeping

5 files changed, 50 insertions, 32 deletions

diff --git a/kernel/sched/idle.c b/kernel/sched/idle.c
index 85f35a3..360cb8d 100644
--- a/kernel/sched/idle.c
+++ b/kernel/sched/idle.c
@@ -2,16 +2,16 @@
 #include <avr/sleep.h>
 #include <avr/power.h>
 #include "sched/idle.h"
-#include "tshield/tshield.h"
 
 void idle_entry(char args) {
   while(1) {
-    //set_sleep_mode(SLEEP_MODE_IDLE);
-    //cli();
-    //sleep_enable();
-    //sei();
-    //sleep_cpu();
-    //sleep_disable();
+    debug_led(0,1);
+    set_sleep_mode(SLEEP_MODE_IDLE);
+    cli();
+    sleep_enable();
+    sei();
+    sleep_cpu();
+    sleep_disable();
   };
 }
 
diff --git a/kernel/sched/include/sched/sched.h b/kernel/sched/include/sched/sched.h
index 5e45328..8838f09 100644
--- a/kernel/sched/include/sched/sched.h
+++ b/kernel/sched/include/sched/sched.h
@@ -29,6 +29,9 @@ struct tcb_t {
 
   /** Current wait queue that this task is in. */
   struct list_head q;
+
+  long sleep_left;
+
 };
 
 /**
@@ -42,7 +45,8 @@ struct tcb_t {
     .mem_low = declared_stack_##name, \
     .mem_high = declared_stack_##name + stack_size - 1, \
     .entry = entry_function, \
-    .q = {} \
+    .q = {}, \
+    .sleep_left = 0 \
   };
 
 /**
@@ -92,9 +96,7 @@ void schedule();
 /**
  * Ticks the scheduler.
  */
-inline void sched_tick() {
-  schedule(); //in a round-robin scheduler, scheduling is called after every tick
-}
+void sched_tick();
 
 /**
  * Initializes a given task and adds it to the ready queue.
@@ -106,6 +108,8 @@ void spawn(struct tcb_t* const tcb, char args);
  */
 void yield() __attribute__ ( ( naked ) );
 
+void sleep(long ms) __attribute__ ( ( naked) );
+
 #define ENTER_CRITICAL() cli()
 #define EXIT_CRITICAL() sei()
 
diff --git a/kernel/sched/sched.c b/kernel/sched/sched.c
index 609c81b..3f4d003 100644
--- a/kernel/sched/sched.c
+++ b/kernel/sched/sched.c
@@ -1,11 +1,13 @@
 #include "sched/sched.h"
 #include "sched/idle.h"
+#include "time/timer.h"
 #include "mcu/sched/context.h"
 
 struct tcb_t* volatile current = 0;
 DECLARE_TASK(idle, IDLE_STACK_SIZE, idle_entry);
 
 struct list_head ready = LIST_HEAD_INIT(ready);
+static struct list_head sleeping = LIST_HEAD_INIT(sleeping);
 char* volatile kstack;
 
 static void init_idle() {
@@ -24,6 +26,11 @@ void sched_init() {
 
 void schedule() {
   if(!list_empty(&ready)) {
+
+    if (current == &idle) {
+      debug_led(0,0);
+    }
+
     current = list_entry(ready.next, struct tcb_t, q);
     list_move_tail(ready.next, &ready);
   } else {
@@ -31,6 +38,18 @@ void schedule() {
   }
 }
 
+void sched_tick() {
+  struct tcb_t* it;
+  struct tcb_t* tmp;
+  list_for_each_entry_safe(it, tmp, &sleeping, q) {
+    it->sleep_left -= 1000 / HZ;
+    if (it->sleep_left <= 0) {
+      list_move_tail(&it->q, &ready);
+    }
+  }
+  schedule(); //in a round-robin scheduler, scheduling is called after every tick
+}
+
 void wake_all(struct list_head* queue) {
   list_splice_init(queue, ready.prev);
   if (current == &idle) {
@@ -51,4 +70,15 @@ void yield(void) {
   RESTORE_CONTEXT();
   sei();
   RETURN();
+}
+
+void sleep(long ms) {
+  cli();
+  SAVE_CONTEXT();
+  current->sleep_left = ms;
+  sleep_on(&sleeping);
+  schedule();
+  RESTORE_CONTEXT();
+  sei();
+  RETURN();
 }
\ No newline at end of file
diff --git a/kernel/time/mcu/atmega2560/timer.c b/kernel/time/mcu/atmega2560/timer.c
index eece7a0..03f1ed5 100644
--- a/kernel/time/mcu/atmega2560/timer.c
+++ b/kernel/time/mcu/atmega2560/timer.c
@@ -11,7 +11,7 @@ void timer_init() {
   TCCR3B = (1 << WGM32); // turn on CTC mode:
   TCCR3B |= (1 << CS32) | (0 << CS31) | (1 << CS30); // set to 1024 prescaler
 
-  unsigned long int hz_counter = ((unsigned long int) F_CPU) / (2 * 1024 * ((unsigned long int) HZ)) - 1;
+  unsigned long int hz_counter = ((unsigned long int) F_CPU) / (1024 * ((unsigned long int) HZ)) - 1;
   OCR3A = hz_counter;
   sei();
 }
@@ -42,6 +42,5 @@ ISR(TIMER3_COMPA_vect, ISR_NAKED) {
   led13(on);
   sched_tick();
   RESTORE_CONTEXT();
-  sei();
   asm volatile ("reti");
 }
diff --git a/main.c b/main.c
index 95ee2f2..d1ceaa9 100644
--- a/main.c
+++ b/main.c
@@ -34,31 +34,16 @@ void wake() {
 void blink( char id) {
   while(1) {
     debug_led(id,1);
-    //yield();
-    WAIT_CYCLES(5000);
+    sleep(500);
     debug_led(id,0);
-    //yield();
-    WAIT_CYCLES(5000);
-    freeze();
+    sleep(500);
   }
 }
 
 void read(char id) {
+  sleep(5000);
   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);
-      }
-    }
+    
   }
 }