fix freezing

10 files changed, 96 insertions, 32 deletions

diff --git a/Makefile b/Makefile
index b0fd7c0..a0b5e0e 100644
--- a/Makefile
+++ b/Makefile
@@ -92,6 +92,12 @@ upload: target
 monitor:
 	cu -l $(SERIAL) -s $(BAUD) --parity=none -h
 
+cycle:
+	@make clean && \
+	make > /dev/null && \
+	read -p "Press enter to proceed with upload..." nothing && \
+	make upload
+
 clean:
 	@rm -f *.o
 	@for module in $(MODULES); do \
@@ -122,4 +128,4 @@ $(GDBINITFILE): $(TARGET).elf
 	@echo "Use 'avr-gdb -x $(GDBINITFILE)'"
 
 	
-.PHONY: clean arduino
+.PHONY: clean cycle
diff --git a/kernel/sched/idle.c b/kernel/sched/idle.c
index d55d694..85f35a3 100644
--- a/kernel/sched/idle.c
+++ b/kernel/sched/idle.c
@@ -6,12 +6,12 @@
 
 void idle_entry(char args) {
   while(1) {
-    set_sleep_mode(SLEEP_MODE_IDLE);
-    cli();
-    sleep_enable();
-    sei();
-    sleep_cpu();
-    sleep_disable();
+    //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 a7f45ee..5e45328 100644
--- a/kernel/sched/include/sched/sched.h
+++ b/kernel/sched/include/sched/sched.h
@@ -76,9 +76,7 @@ static inline void sleep_on(struct list_head* queue) {
  * Wakes all tasks waiting in the given queue.
  * This moves all tasks contained in the queue to the ready queue.
  */
-static inline void wake_all(struct list_head* queue) {
-  list_splice_init(queue, ready.prev);
-}
+void wake_all(struct list_head* queue);
 
 /**
  * Initializes the scheduler by setting up kstack, initializing the idle task
@@ -108,9 +106,6 @@ void spawn(struct tcb_t* const tcb, char args);
  */
 void yield() __attribute__ ( ( naked ) );
 
-void freeze() __attribute__ ( ( naked ) );
-
-
 #define ENTER_CRITICAL() cli()
 #define EXIT_CRITICAL() sei()
 
diff --git a/kernel/sched/mcu/atmega2560/context.c b/kernel/sched/mcu/atmega2560/context.c
index ef7e172..10f16a9 100644
--- a/kernel/sched/mcu/atmega2560/context.c
+++ b/kernel/sched/mcu/atmega2560/context.c
@@ -4,6 +4,7 @@ char* init_stack(const char* const mem_low, const char* const mem_high, void (*e
   char* sp = (char*) mem_high;
   unsigned long address = (unsigned long) entry;
   
+  // pattern for debugging purposes
   *sp = (char) 0x1;
   sp--;
   *sp = (char) 0x2;
@@ -13,6 +14,7 @@ char* init_stack(const char* const mem_low, const char* const mem_high, void (*e
   *sp = (char) 0x2;
   sp--;
   
+  // put return address on stack
 	*sp = (char) ( address & (unsigned short) 0x00ff );
 	sp--;
 
@@ -22,6 +24,7 @@ char* init_stack(const char* const mem_low, const char* const mem_high, void (*e
   *sp = (char) ( (address >> 16) & ( unsigned short ) 0x00ff );
 	sp--;
 
+  // save registers
 	*sp = (char) 0x00; //r0
 	sp--;
 	*sp = (char) 0x80; //SREG, enable interrupts when task starts
diff --git a/kernel/sched/mcu/atmega2560/include/mcu/sched/context.h b/kernel/sched/mcu/atmega2560/include/mcu/sched/context.h
index d3b324c..437e7f3 100644
--- a/kernel/sched/mcu/atmega2560/include/mcu/sched/context.h
+++ b/kernel/sched/mcu/atmega2560/include/mcu/sched/context.h
@@ -13,7 +13,6 @@
   asm volatile ( \
     "push r0 \n\t" \
     "in r0, __SREG__ \n\t" \
-    "cli \n\t" \
     "push r0 \n\t" \
     "push r1 \n\t" \
     "clr r1 \n\t" \
diff --git a/kernel/sched/sched.c b/kernel/sched/sched.c
index bc3b30d..609c81b 100644
--- a/kernel/sched/sched.c
+++ b/kernel/sched/sched.c
@@ -14,8 +14,8 @@ static void init_idle() {
 }
 
 void sched_init() {
-  init_kstack((char **) &kstack);
   init_idle();
+  init_kstack((char **) &kstack);
   schedule();
   sei();
   RESTORE_CONTEXT();
@@ -25,30 +25,30 @@ void sched_init() {
 void schedule() {
   if(!list_empty(&ready)) {
     current = list_entry(ready.next, struct tcb_t, q);
-    list_move_tail(&current->q, &ready);
+    list_move_tail(ready.next, &ready);
   } else {
     current = &idle;
   }
 }
 
+void wake_all(struct list_head* queue) {
+  list_splice_init(queue, ready.prev);
+  if (current == &idle) {
+    schedule();
+  }
+}
+
 void spawn(struct tcb_t* const tcb, char args) {
   tcb->sp = init_stack(tcb->mem_low, tcb->mem_high, tcb->entry, args);
   INIT_LIST_HEAD(&tcb->q);
   list_add_tail(&tcb->q, &ready);
 }
 
-void yield() {
+void yield(void) {
+  cli();
   SAVE_CONTEXT();
   schedule();
   RESTORE_CONTEXT();
+  sei();
   RETURN();
-}
-
-void freeze() {
-  SAVE_CONTEXT();
-  list_del_init(&current->q);
-  schedule();
-  RESTORE_CONTEXT();
-  asm volatile ( "ret" );
-}
-
+}
\ No newline at end of file
diff --git a/kernel/time/include/time/timer.h b/kernel/time/include/time/timer.h
index 1fa64d5..eaec726 100644
--- a/kernel/time/include/time/timer.h
+++ b/kernel/time/include/time/timer.h
@@ -1,7 +1,7 @@
 #ifndef TIMER_H
 #define TIMER_H
 
-#define HZ 10
+#define HZ 50
 
 void timer_init();
 void timer_start();
diff --git a/kernel/time/mcu/atmega2560/timer.c b/kernel/time/mcu/atmega2560/timer.c
index e3ab6eb..eece7a0 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 int hz_counter = F_CPU / (2 * 1024 * HZ) - 1;
+  unsigned long int hz_counter = ((unsigned long int) F_CPU) / (2 * 1024 * ((unsigned long int) HZ)) - 1;
   OCR3A = hz_counter;
   sei();
 }
@@ -24,10 +24,24 @@ void timer_stop() {
   TIMSK3 &= ~(1 << OCIE3A);
 }
 
+#include <avr/io.h>
+void led13(int on) {
+  if (on) {
+    DDRB |= (1 << 7);
+    PORTB |= (1 << 7);
+  } else {
+    DDRB &= ~(1 << 7);
+    PORTB &= ~(1 << 7);
+  }
+}
+
 ISR(TIMER3_COMPA_vect, ISR_NAKED) {
   SAVE_CONTEXT();
+  static int on = 0;
+  on = !on;
+  led13(on);
   sched_tick();
-  sei();
   RESTORE_CONTEXT();
+  sei();
   asm volatile ("reti");
 }
diff --git a/kernel/tshield/mcu/atmega2560/tshield.c b/kernel/tshield/mcu/atmega2560/tshield.c
index 4c1e150..5502f83 100644
--- a/kernel/tshield/mcu/atmega2560/tshield.c
+++ b/kernel/tshield/mcu/atmega2560/tshield.c
@@ -1,4 +1,5 @@
 #include <avr/io.h>
+#include <avr/interrupt.h>
 #include "tshield/tshield.h"
 
 static void tshield_init_servo();
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();