initial commit

23 files changed, 1273 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..bec1df1
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,4 @@
+#compiled files
+*.o
+*.elf
+*.hex
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..200e467
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,125 @@
+# CPU model
+MCU=atmega2560
+
+# CPU Frequency
+F_CPU=16000000L
+
+# Name of image to produce (can be arbitrary)
+TARGET=firmware
+
+# Serial port used for uploading
+SERIAL=/dev/ttyACM0
+BAUD=115200
+
+# Toolchain flags
+CC=avr-gcc
+CFLAGS= -std=gnu99 -O2 -Wall -finline-functions -ffunction-sections -fdata-sections -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -mmcu=$(MCU) -DF_CPU=$(F_CPU)
+LD=avr-gcc
+LDFLAGS= -O2 -Wl,--gc-sections,--relax -mmcu=$(MCU)
+AR=avr-ar
+AS=avr-as
+ASFLAGS=-mmcu=$(MCU)
+OC=avr-objcopy
+OCFLAGS=-O ihex -R .eeprom
+AVRDUDE=avrdude
+AVRDUDEFLAGS= -DV -p $(MCU) -P $(SERIAL) -c stk500v2 -b 115200
+AVRSIZE=avr-size
+CPP=avr-g++
+CPPFLAGS= -O2 -Wall -fno-exceptions -ffunction-sections -fdata-sections -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -mmcu=$(MCU) -DF_CPU=$(F_CPU)
+GDBINITFILE=gdb.conf
+
+MODULES=collection sched time tshield serial bug
+
+SOURCES=\
+	$(foreach module,$(MODULES),$(wildcard kernel/$(module)/*.S)) \
+	$(foreach module,$(MODULES),$(wildcard kernel/$(module)/*.c)) \
+	$(foreach module,$(MODULES),$(wildcard kernel/$(module)/mcu/$(MCU)/*.S)) \
+	$(foreach module,$(MODULES),$(wildcard kernel/$(module)/mcu/$(MCU)/*.c)) \
+	$(wildcard arduino/*.s) $(wildcard arduino/*.c) $(wildcard arduino/*.cpp) \
+	$(wildcard *.s) $(wildcard *.c) $(wildcard *.cpp)
+	
+OBJECTS=$(addsuffix .o, $(basename $(SOURCES)))
+INCLUDES=\
+	$(foreach module, $(MODULES), kernel/$(module)/include) \
+	$(foreach module, $(MODULES), kernel/$(module)/mcu/$(MCU)/include)
+
+ARDUINO_INCLUDES=arduino arduino/variants/mega
+
+
+# Rules
+all: target
+
+target: $(TARGET).hex
+
+$(TARGET).hex: $(TARGET).elf
+	$(OC) $(OCFLAGS) $< $@
+
+$(TARGET).elf: $(OBJECTS)
+	$(LD) $(LDFLAGS) -o $@ $^
+	
+# Arduino sources need special includes
+arduino/%.o: arduino/%.c
+	$(CC) $(CFLAGS) $(addprefix -I, $(ARDUINO_INCLUDES)) $(addprefix -I, $(INCLUDES)) -o $@ -c $<
+	
+arduino/%.o: arduino/%.cpp
+	$(CPP) $(CPPFLAGS) $(addprefix -I, $(ARDUINO_INCLUDES)) $(addprefix -I, $(INCLUDES)) -o $@ -c $<
+	
+# Kernel sources
+kernel/%.o: kernel/%.S
+	$(CC) $(CFLAGS) -o $@ -c $<
+	
+kernel/%.o: kernel/%.c
+	$(CC) $(CFLAGS) -I$(dir $<) $(addprefix -I, $(INCLUDES)) -o $@ -c $<
+	
+# Local sources may use arduino and therfore need special includes
+%.o: %.s
+	$(CC) $(CFLAGS) $(addprefix -I, $(ARDUINO_INCLUDES)) -I$(dir $<) $(addprefix -I, $(INCLUDES)) -o $@ -c $<
+	
+%.o: %.c
+	$(CC) $(CFLAGS) $(addprefix -I, $(ARDUINO_INCLUDES)) -I$(dir $<) $(addprefix -I, $(INCLUDES)) -o $@ -c $<
+	
+%.o: %.cpp
+	$(CPP) $(CPPFLAGS) $(addprefix -I, $(ARDUINO_INCLUDES)) -I$(dir $<) $(addprefix -I, $(INCLUDES)) -o $@ -c $<
+	
+
+# Utility rules
+upload: target
+	$(AVRDUDE) $(AVRDUDEFLAGS) -U flash:w:$(TARGET).hex:i
+	
+size: target
+	$(AVRSIZE) --format=avr --mcu=$(MCU) $(TARGET).elf
+	
+monitor:
+	cu -l $(SERIAL) -s $(BAUD) --parity=none -h
+
+clean:
+	@rm -f *.o
+	@for module in $(MODULES); do \
+		rm -f kernel/$$module/*.o; \
+		rm -f kernel/$$module/mcu/$(MCU)/*.o; \
+	done
+	@rm -f arduino/*.o
+	@rm -f $(TARGET).hex
+	@rm -f $(TARGET).elf
+	@rm -f $(GDBINITFILE)
+	
+# Debugging
+ddd: gdbinit
+	ddd --debugger "avr-gdb -x $(GDBINITFILE)"
+
+gdbserver: gdbinit
+#	run_avr -g -m $(MCU) -f $(F_CPU)
+	simulavr -p 1234 -d $(MCU) -F 16000000 -f $(TARGET).elf -g
+
+gdbinit: $(GDBINITFILE)
+
+$(GDBINITFILE): $(TARGET).elf
+	@echo "file $(TARGET).elf" > $(GDBINITFILE)
+	@echo "target remote localhost:1234" >> $(GDBINITFILE)
+	@echo "load" >> $(GDBINITFILE) 
+	@echo "break main.c:main" >> $(GDBINITFILE)
+	@echo
+	@echo "Use 'avr-gdb -x $(GDBINITFILE)'"
+
+	
+.PHONY: clean arduino
diff --git a/a.out b/a.out
new file mode 100755
index 0000000..ae9ca25
--- /dev/null
+++ b/a.out
diff --git a/kernel/bug/debug.c b/kernel/bug/debug.c
new file mode 100644
index 0000000..10a67d8
--- /dev/null
+++ b/kernel/bug/debug.c
@@ -0,0 +1,6 @@
+#include "bug/debug.h"
+#include "tshield/tshield.h"
+
+void debug_led(int led, int value) {
+  tshield_led(led, value);
+}
\ No newline at end of file
diff --git a/kernel/bug/include/bug/debug.h b/kernel/bug/include/bug/debug.h
new file mode 100644
index 0000000..937ff2c
--- /dev/null
+++ b/kernel/bug/include/bug/debug.h
@@ -0,0 +1,8 @@
+#ifndef DEBUG_H
+#define DEBUG_H
+
+#define DEBUG_LEDS 4
+
+void debug_led(int led, int value);
+
+#endif
\ No newline at end of file
diff --git a/kernel/bug/include/bug/panic.h b/kernel/bug/include/bug/panic.h
new file mode 100644
index 0000000..2a39076
--- /dev/null
+++ b/kernel/bug/include/bug/panic.h
@@ -0,0 +1,6 @@
+#ifndef PANIC_H
+#define PANIC_H
+
+void panic();
+
+#endif
\ No newline at end of file
diff --git a/kernel/bug/panic.c b/kernel/bug/panic.c
new file mode 100644
index 0000000..c82d0f0
--- /dev/null
+++ b/kernel/bug/panic.c
@@ -0,0 +1,23 @@
+#include "bug/panic.h"
+#include "bug/debug.h"
+#include <avr/interrupt.h>
+
+static inline void wait() {
+  for (volatile long i = 0; i < 20000; ++i) {};
+}
+
+void panic() {
+  cli();
+  while(1) {
+    for(int i = 0; i < DEBUG_LEDS; ++i) {
+      debug_led((i - 1) % DEBUG_LEDS, 0);
+      debug_led(i, 1);
+      wait();
+    }
+    for(int i = DEBUG_LEDS - 1; i >= 0; --i) {
+      debug_led((i + 1) % DEBUG_LEDS, 0);
+      debug_led(i, 1);
+      wait();
+    }
+  }
+}
\ No newline at end of file
diff --git a/kernel/collection/include/collection/list.h b/kernel/collection/include/collection/list.h
new file mode 100644
index 0000000..3a76885
--- /dev/null
+++ b/kernel/collection/include/collection/list.h
@@ -0,0 +1,244 @@
+#ifndef __LIST_H
+#define __LIST_H
+
+/* This file is from Linux Kernel (include/linux/list.h) 
+ * and modified by simply removing hardware prefetching of list items. 
+ * Here by copyright, credits attributed to wherever they belong.
+ * Kulesh Shanmugasundaram (kulesh [squiggly] isis.poly.edu)
+ */
+
+/*
+ * Simple doubly linked list implementation.
+ *
+ * Some of the internal functions ("__xxx") are useful when
+ * manipulating whole lists rather than single entries, as
+ * sometimes we already know the next/prev entries and we can
+ * generate better code by using them directly rather than
+ * using the generic single-entry routines.
+ */
+
+struct list_head {
+	struct list_head *next, *prev;
+};
+
+#define LIST_HEAD_INIT(name) { &(name), &(name) }
+
+#define LIST_HEAD(name) \
+	struct list_head name = LIST_HEAD_INIT(name)
+
+#define INIT_LIST_HEAD(ptr) do { \
+	(ptr)->next = (ptr); (ptr)->prev = (ptr); \
+} while (0)
+
+/*
+ * Insert a new entry between two known consecutive entries. 
+ *
+ * This is only for internal list manipulation where we know
+ * the prev/next entries already!
+ */
+static inline void __list_add(struct list_head *new,
+			      struct list_head *prev,
+			      struct list_head *next)
+{
+	next->prev = new;
+	new->next = next;
+	new->prev = prev;
+	prev->next = new;
+}
+
+/**
+ * list_add - add a new entry
+ * @new: new entry to be added
+ * @head: list head to add it after
+ *
+ * Insert a new entry after the specified head.
+ * This is good for implementing stacks.
+ */
+static inline void list_add(struct list_head *new, struct list_head *head)
+{
+	__list_add(new, head, head->next);
+}
+
+/**
+ * list_add_tail - add a new entry
+ * @new: new entry to be added
+ * @head: list head to add it before
+ *
+ * Insert a new entry before the specified head.
+ * This is useful for implementing queues.
+ */
+static inline void list_add_tail(struct list_head *new, struct list_head *head)
+{
+	__list_add(new, head->prev, head);
+}
+
+/*
+ * Delete a list entry by making the prev/next entries
+ * point to each other.
+ *
+ * This is only for internal list manipulation where we know
+ * the prev/next entries already!
+ */
+static inline void __list_del(struct list_head *prev, struct list_head *next)
+{
+	next->prev = prev;
+	prev->next = next;
+}
+
+/**
+ * list_del - deletes entry from list.
+ * @entry: the element to delete from the list.
+ * Note: list_empty on entry does not return true after this, the entry is in an undefined state.
+ */
+static inline void list_del(struct list_head *entry)
+{
+	__list_del(entry->prev, entry->next);
+	entry->next = (void *) 0;
+	entry->prev = (void *) 0;
+}
+
+/**
+ * list_del_init - deletes entry from list and reinitialize it.
+ * @entry: the element to delete from the list.
+ */
+static inline void list_del_init(struct list_head *entry)
+{
+	__list_del(entry->prev, entry->next);
+	INIT_LIST_HEAD(entry); 
+}
+
+/**
+ * list_move - delete from one list and add as another's head
+ * @list: the entry to move
+ * @head: the head that will precede our entry
+ */
+static inline void list_move(struct list_head *list, struct list_head *head)
+{
+        __list_del(list->prev, list->next);
+        list_add(list, head);
+}
+
+/**
+ * list_move_tail - delete from one list and add as another's tail
+ * @list: the entry to move
+ * @head: the head that will follow our entry
+ */
+static inline void list_move_tail(struct list_head *list,
+				  struct list_head *head)
+{
+        __list_del(list->prev, list->next);
+        list_add_tail(list, head);
+}
+
+/**
+ * list_empty - tests whether a list is empty
+ * @head: the list to test.
+ */
+static inline int list_empty(struct list_head *head)
+{
+	return head->next == head;
+}
+
+static inline void __list_splice(struct list_head *list,
+				 struct list_head *head)
+{
+	struct list_head *first = list->next;
+	struct list_head *last = list->prev;
+	struct list_head *at = head->next;
+
+	first->prev = head;
+	head->next = first;
+
+	last->next = at;
+	at->prev = last;
+}
+
+/**
+ * list_splice - join two lists
+ * @list: the new list to add.
+ * @head: the place to add it in the first list.
+ */
+static inline void list_splice(struct list_head *list, struct list_head *head)
+{
+	if (!list_empty(list))
+		__list_splice(list, head);
+}
+
+/**
+ * list_splice_init - join two lists and reinitialise the emptied list.
+ * @list: the new list to add.
+ * @head: the place to add it in the first list.
+ *
+ * The list at @list is reinitialised
+ */
+static inline void list_splice_init(struct list_head *list,
+				    struct list_head *head)
+{
+	if (!list_empty(list)) {
+		__list_splice(list, head);
+		INIT_LIST_HEAD(list);
+	}
+}
+
+/**
+ * list_entry - get the struct for this entry
+ * @ptr:	the &struct list_head pointer.
+ * @type:	the type of the struct this is embedded in.
+ * @member:	the name of the list_struct within the struct.
+ */
+#define list_entry(ptr, type, member) \
+	((type *)((char *)(ptr)-(unsigned long)(&((type *)0)->member)))
+
+/**
+ * list_for_each	-	iterate over a list
+ * @pos:	the &struct list_head to use as a loop counter.
+ * @head:	the head for your list.
+ */
+#define list_for_each(pos, head) \
+	for (pos = (head)->next; pos != (head); \
+        	pos = pos->next)
+/**
+ * list_for_each_prev	-	iterate over a list backwards
+ * @pos:	the &struct list_head to use as a loop counter.
+ * @head:	the head for your list.
+ */
+#define list_for_each_prev(pos, head) \
+	for (pos = (head)->prev; pos != (head); \
+        	pos = pos->prev)
+        	
+/**
+ * list_for_each_safe	-	iterate over a list safe against removal of list entry
+ * @pos:	the &struct list_head to use as a loop counter.
+ * @n:		another &struct list_head to use as temporary storage
+ * @head:	the head for your list.
+ */
+#define list_for_each_safe(pos, n, head) \
+	for (pos = (head)->next, n = pos->next; pos != (head); \
+		pos = n, n = pos->next)
+
+/**
+ * list_for_each_entry	-	iterate over list of given type
+ * @pos:	the type * to use as a loop counter.
+ * @head:	the head for your list.
+ * @member:	the name of the list_struct within the struct.
+ */
+#define list_for_each_entry(pos, head, member)				\
+	for (pos = list_entry((head)->next, typeof(*pos), member);	\
+	     &pos->member != (head); 					\
+	     pos = list_entry(pos->member.next, typeof(*pos), member))
+
+/**
+ * list_for_each_entry_safe - iterate over list of given type safe against removal of list entry
+ * @pos:	the type * to use as a loop counter.
+ * @n:		another type * to use as temporary storage
+ * @head:	the head for your list.
+ * @member:	the name of the list_struct within the struct.
+ */
+#define list_for_each_entry_safe(pos, n, head, member)			\
+	for (pos = list_entry((head)->next, typeof(*pos), member),	\
+		n = list_entry(pos->member.next, typeof(*pos), member);	\
+	     &pos->member != (head); 					\
+	     pos = n, n = list_entry(n->member.next, typeof(*n), member))
+
+
+#endif
diff --git a/kernel/collection/include/collection/rbuffer.h b/kernel/collection/include/collection/rbuffer.h
new file mode 100644
index 0000000..dbdd6ee
--- /dev/null
+++ b/kernel/collection/include/collection/rbuffer.h
@@ -0,0 +1,47 @@
+#ifndef RBUFFER_H
+#define RBUFFER_H
+
+#include <stddef.h>
+
+#define EEMPTY -1
+#define EFULL -2
+
+typedef char rbuffer_byte_t;
+
+struct rbuffer_t {
+  rbuffer_byte_t* address;
+  size_t size;
+  size_t head;
+  size_t tail;
+};
+
+#define RBUFFER_INIT(array, length) \
+  { \
+    .address = array, \
+    .size = length, \
+    .head=0, \
+    .tail=0 \
+  }
+
+void rbuffer_init(struct rbuffer_t* rb, rbuffer_byte_t* address, size_t size);
+
+/** Read data from ringbuffer. */
+static inline int rbuffer_read(struct rbuffer_t* const rb, rbuffer_byte_t* const data_out) {
+  if (rb->head == rb->tail) return EEMPTY;
+  *data_out = rb->address[rb->head];
+  rb->head = rb->head + 1;
+  if (rb->head >= rb->size) rb->head=0;
+  return 0;
+}
+
+static inline int rbuffer_write(struct rbuffer_t* const rb, rbuffer_byte_t data_in) {
+  size_t next_tail = rb->tail + 1;
+  if (next_tail >= rb->size) next_tail = 0;  
+  if (rb->head == next_tail) return EFULL;
+  
+  rb->address[rb->tail] = data_in;
+  rb->tail = next_tail;
+  return 0;
+}
+
+#endif
diff --git a/kernel/collection/rbuffer.c b/kernel/collection/rbuffer.c
new file mode 100644
index 0000000..ec11d13
--- /dev/null
+++ b/kernel/collection/rbuffer.c
@@ -0,0 +1,8 @@
+#include <collection/rbuffer.h>
+
+void rbuffer_init(struct rbuffer_t* rb, rbuffer_byte_t* address, size_t size) {
+  rb->address = address;
+  rb->size = size;
+  rb->head=0;
+  rb->tail=0;
+}
diff --git a/kernel/sched/idle.c b/kernel/sched/idle.c
new file mode 100644
index 0000000..18ccdd5
--- /dev/null
+++ b/kernel/sched/idle.c
@@ -0,0 +1,18 @@
+#include <avr/interrupt.h>
+#include <avr/sleep.h>
+#include <avr/power.h>
+#include "sched/idle.h"
+#include "tshield/tshield.h"
+
+void idle_entry(char args) {
+  while(1) {
+    tshield_led(TSHIELD_LED_IDLE,1);
+    set_sleep_mode(SLEEP_MODE_IDLE);
+    cli();
+    sleep_enable();
+    sei();
+    sleep_cpu();
+    sleep_disable();
+  };
+}
+
diff --git a/kernel/sched/include/sched/idle.h b/kernel/sched/include/sched/idle.h
new file mode 100644
index 0000000..3b6e40a
--- /dev/null
+++ b/kernel/sched/include/sched/idle.h
@@ -0,0 +1,6 @@
+#ifndef IDLE_H
+#define IDLE_H
+
+void idle_entry(char args);
+
+#endif
\ No newline at end of file
diff --git a/kernel/sched/include/sched/sched.h b/kernel/sched/include/sched/sched.h
new file mode 100644
index 0000000..d63b78c
--- /dev/null
+++ b/kernel/sched/include/sched/sched.h
@@ -0,0 +1,117 @@
+#ifndef SCHED_H
+#define SCHED_H
+
+#include "collection/list.h"
+#include "mcu/sched/context.h"
+
+/** Proposed default stack size, you may use this definition for declaring tasks. */
+#define STACK_SIZE 64
+
+/** Stack size to be allocated for the idle task. */
+#define IDLE_STACK_SIZE 64
+
+/**
+ * Task control block, contains runtime
+ * information of tasks.
+ */
+struct tcb_t {
+  /** Stack pointer of this task. (must be first in structure) */
+  char* volatile sp;
+
+  /** Lowest address of this task's memory (inclusive). */
+  char* mem_low;
+
+  /** Highest address of this task's memory (inclusive). */
+  char* mem_high;
+
+  /** Entry function of this task. */
+  void (*entry)(char);
+
+  /** Current wait queue that this task is in. */
+  struct list_head q;
+};
+
+/**
+ * Utility for declaring a task with statically allocated memory.
+ * Note: for a task to be scheduled, it must first be spawned (see spawn()).
+*/
+#define DECLARE_TASK(name, stack_size, entry_function) \
+  static char volatile declared_stack_##name[stack_size]; \
+  static struct tcb_t name = { \
+    .sp = 0, \
+    .mem_low = declared_stack_##name, \
+    .mem_high = declared_stack_##name + stack_size - 1, \
+    .entry = entry_function, \
+    .q = {} \
+  };
+
+/**
+ * Points to currently executing task. If no scheduling has been enabled,
+ * this points to NULL
+ */
+extern struct tcb_t* volatile current;
+
+/**
+ * Queue that contains all tasks that are ready to be run, awaiting their
+ * turn from the scheduler.
+ */
+extern struct list_head ready;
+
+/**
+ * Stack pointer for operations performed out of task context, including any
+ * calls made after SAVE_CONTEXT().
+ */
+extern char* volatile kstack;
+
+
+/** 
+ * Makes the current task sleep on a specific queue.
+ * This moves the current task to the given queue's tail.
+ */
+static inline void sleep_on(struct list_head* queue) {
+  list_move_tail(&current->q, 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);
+}
+
+/**
+ * Initializes the scheduler by setting up kstack, initializing the idle task
+ * and selecting the first task to run.
+ */
+void sched_init();
+
+/**
+ * Ticks the scheduler.
+ */
+inline void sched_tick() {
+  schedule(); //in a round-robin scheduler, scheduling is called after every tick
+}
+
+/** 
+ * Enters the scheduler, setting current to the next runnable task.
+ */
+void schedule();
+
+/**
+ * Initializes a given task and adds it to the ready queue.
+ */
+void spawn(struct tcb_t* const tcb, char args);
+
+/**
+ * Voluntarily yields control of the CPU to the scheduler.
+ */
+void yield() __attribute__ ( ( naked ) );
+
+void freeze() __attribute__ ( ( naked ) );
+
+
+#define ENTER_CRITICAL() cli()
+#define EXIT_CRITICAL() sei()
+
+#endif
diff --git a/kernel/sched/mcu/atmega2560/context.c b/kernel/sched/mcu/atmega2560/context.c
new file mode 100644
index 0000000..7a88b33
--- /dev/null
+++ b/kernel/sched/mcu/atmega2560/context.c
@@ -0,0 +1,96 @@
+#include "mcu/sched/context.h"
+
+char* init_stack(const char* const mem_low, const char* const mem_high, void (*entry)(char), char args) {
+  char* sp = (char*) mem_high;
+  unsigned long address = (unsigned long) entry;
+  
+  
+  *sp = (char) 0x1;
+  sp--;
+  *sp = (char) 0x2;
+  sp--;
+  *sp = (char) 0x1;
+  sp--;
+  *sp = (char) 0x2;
+  sp--;
+  
+	*sp = (char) ( address & (unsigned short) 0x00ff );
+	sp--;
+
+	*sp = (char) ( (address >> 8) & ( unsigned short ) 0x00ff );
+	sp--;
+  
+  *sp = (char) ( (address >> 16) & ( unsigned short ) 0x00ff );
+	sp--;
+
+	*sp = (char) 0x00; //r0
+	sp--;
+	*sp = (char) 0x80; //SREG, enable interrupts when task starts
+	sp--;
+	*sp = ( char ) 0x00; //r1
+	sp--;
+	*sp = ( char ) 0x00; //r2
+	sp--;
+	*sp = ( char ) 0x00; //r3
+	sp--;
+	*sp = ( char ) 0x00; //r4
+	sp--;
+	*sp = ( char ) 0x00; //r5
+	sp--;
+	*sp = ( char ) 0x00; //r6
+	sp--;
+	*sp = ( char ) 0x00; //r7
+	sp--;
+	*sp = ( char ) 0x00; //r8
+	sp--;
+	*sp = ( char ) 0x00; //r9
+	sp--;
+	*sp = ( char ) 0x00; //r10
+	sp--;
+	*sp = ( char ) 0x00; //r11
+	sp--;
+	*sp = ( char ) 0x00; //r12
+	sp--;
+	*sp = ( char ) 0x00; //r13
+	sp--;
+	*sp = ( char ) 0x00; //r14
+	sp--;
+	*sp = ( char ) 0x00; //r15
+	sp--;
+	*sp = ( char ) 0x00; //r16
+	sp--;
+	*sp = ( char ) 0x00; //r17
+	sp--;
+	*sp = ( char ) 0x00; //r18
+	sp--;
+	*sp = ( char ) 0x00; //r19
+	sp--;
+	*sp = ( char ) 0x00; //r20
+	sp--;
+  *sp = ( char ) 0x00; //r21
+	sp--;
+  *sp = ( char ) 0x00; //r22
+	sp--;
+  *sp = ( char ) 0x00; //r23
+	sp--;
+  
+  *sp = (char) ( args ); //place first argument in register 24
+	sp--;
+  
+  *sp = ( char ) 0x00; //r25
+	sp--;
+  *sp = ( char ) 0x00; //r26
+	sp--;
+  *sp = ( char ) 0x00; //r27
+	sp--;
+  *sp = ( char ) 0x00; //r28
+	sp--;
+  *sp = ( char ) 0x00; //r29
+	sp--;
+  *sp = ( char ) 0x00; //r30
+	sp--;
+  *sp = ( char ) 0x00; //r31
+	sp--;
+  
+  return sp;
+}
diff --git a/kernel/sched/mcu/atmega2560/include/mcu/sched/context.h b/kernel/sched/mcu/atmega2560/include/mcu/sched/context.h
new file mode 100644
index 0000000..9ff8d42
--- /dev/null
+++ b/kernel/sched/mcu/atmega2560/include/mcu/sched/context.h
@@ -0,0 +1,116 @@
+#ifndef CONTEXT_H
+#define CONTEXT_H
+
+#include <avr/interrupt.h>
+
+#define SAVE_CONTEXT() \
+  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" \
+    "push r2 \n\t" \
+    "push r3 \n\t" \
+    "push r4 \n\t" \
+    "push r5 \n\t" \
+    "push r6 \n\t" \
+    "push r7 \n\t" \
+    "push r8 \n\t" \
+    "push r9 \n\t" \
+    "push r10 \n\t" \
+    "push r11 \n\t" \
+    "push r12 \n\t" \
+    "push r13 \n\t" \
+    "push r14 \n\t" \
+    "push r15 \n\t" \
+    "push r16 \n\t" \
+    "push r17 \n\t" \
+    "push r18 \n\t" \
+    "push r19 \n\t" \
+    "push r20 \n\t" \
+    "push r21 \n\t" \
+    "push r22 \n\t" \
+    "push r23 \n\t" \
+    "push r24 \n\t" \
+    "push r25 \n\t" \
+    "push r26 \n\t" \
+    "push r27 \n\t" \
+    "push r28 \n\t" \
+    "push r29 \n\t" \
+    "push r30 \n\t" \
+    "push r31 \n\t" \
+    "lds r26, current \n\t" \
+    "lds r27, current +1 \n\t" \
+    "in r0, __SP_L__ \n\t" \
+    "st x+, r0 	\n\t" \
+    "in r0, __SP_H__ \n\t" \
+    "st x+, r0 \n\t" \
+    "lds r26, kstack \n\t" \
+    "lds r27, kstack + 1 \n\t" \
+    "ld r28, x+ \n\t" \
+    "out __SP_L__, r28 \n\t" \
+    "ld r29, x+ \n\t" \
+    "out __SP_H__, r29 \n\t" \
+  )
+  
+#define RESTORE_CONTEXT() \
+  asm volatile ( \
+    "lds r26, kstack \n\t" \
+    "lds r27, kstack +1 \n\t" \
+    "in r0, __SP_L__ \n\t" \
+    "st x+, r0  \n\t" \
+    "in r0, __SP_H__ \n\t" \
+    "st x+, r0  \n\t" \
+    "lds r26, current \n\t" \
+    "lds r27, current + 1 \n\t" \
+    "ld r28, x+ \n\t" \
+    "out __SP_L__, r28 \n\t" \
+    "ld r29, x+ \n\t" \
+    "out __SP_H__, r29 \n\t" \
+    "pop r31 \n\t" \
+    "pop r30 \n\t" \
+    "pop r29 \n\t" \
+    "pop r28 \n\t" \
+    "pop r27 \n\t" \
+    "pop r26 \n\t" \
+    "pop r25 \n\t" \
+    "pop r24 \n\t" \
+    "pop r23 \n\t" \
+    "pop r22 \n\t" \
+    "pop r21 \n\t" \
+    "pop r20 \n\t" \
+    "pop r19 \n\t" \
+    "pop r18 \n\t" \
+    "pop r17 \n\t" \
+    "pop r16 \n\t" \
+    "pop r15 \n\t" \
+    "pop r14 \n\t" \
+    "pop r13 \n\t" \
+    "pop r12 \n\t" \
+    "pop r11 \n\t" \
+    "pop r10 \n\t" \
+    "pop r9 \n\t" \
+    "pop r8 \n\t" \
+    "pop r7 \n\t" \
+    "pop r6 \n\t" \
+    "pop r5 \n\t" \
+    "pop r4 \n\t" \
+    "pop r3 \n\t" \
+    "pop r2 \n\t" \
+    "pop r1 \n\t" \
+    "pop r0 \n\t" \
+    "out __SREG__, r0 \n\t" \
+    "pop r0 \n\t" \
+  )
+
+char* init_stack(const char* const mem_low, const char* const mem_high, void (*entry)(char), char args);
+
+static inline void init_kstack(char **kstack) {
+  *kstack = (char*) SP;
+}
+
+#define RETURN() asm volatile("ret");
+
+#endif
diff --git a/kernel/sched/sched.c b/kernel/sched/sched.c
new file mode 100644
index 0000000..8406b8d
--- /dev/null
+++ b/kernel/sched/sched.c
@@ -0,0 +1,54 @@
+#include "sched/sched.h"
+#include "sched/idle.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);
+char* volatile kstack;
+
+static void init_idle() {
+  idle.sp = init_stack(idle.mem_low, idle.mem_high, idle.entry, 0);
+  INIT_LIST_HEAD(&idle.q);
+}
+
+void sched_init() {
+  init_kstack((char **) &kstack);
+  init_idle();
+  schedule();
+  sei();
+  RESTORE_CONTEXT();
+  RETURN();
+}
+
+void schedule() {
+  if(!list_empty(&ready)) {
+    current = list_entry(ready.next, struct tcb_t, q);
+    list_move_tail(&current->q, &ready);
+  } else {
+    current = &idle;
+  }
+}
+
+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) {
+  SAVE_CONTEXT();
+  schedule();
+  RESTORE_CONTEXT();
+  asm volatile ( "ret" );
+}
+
+void freeze() {
+  SAVE_CONTEXT();
+  list_del_init(&current->q);
+  schedule();
+  RESTORE_CONTEXT();
+  asm volatile ( "ret" );
+}
+
diff --git a/kernel/serial.disabled/include/serial/serial.h b/kernel/serial.disabled/include/serial/serial.h
new file mode 100644
index 0000000..c76fe45
--- /dev/null
+++ b/kernel/serial.disabled/include/serial/serial.h
@@ -0,0 +1,43 @@
+#ifndef SERIAL_PRIVATE_H
+#define SERIAL_PRIVATE_H
+
+#include <stddef.h>
+#include "collection/rbuffer.h"
+#include "collection/list.h"
+#include "serial/serial.h"
+
+#define USARTS 1
+#define SERIAL_BUFFER_SIZE 64
+
+struct serial_device_t {
+  volatile char __rx_buffer[SERIAL_BUFFER_SIZE];
+  volatile char __tx_buffer[SERIAL_BUFFER_SIZE];
+
+  struct rbuffer_t rx_buffer;
+  struct rbuffer_t tx_buffer;
+
+  struct list_head rx_q;
+  struct list_head tx_q;
+};
+
+#define SERIAL_DEVICE_INIT(name) \
+  { \
+    .rx_buffer = RBUFFER_INIT(name.__rx_buffer, SERIAL_BUFFER_SIZE), \
+    .tx_buffer = RBUFFER_INIT(name.__tx_buffer, SERIAL_BUFFER_SIZE), \
+    .rx_q = LIST_HEAD_INIT(name.rx_q), \
+    .tx_q = LIST_HEAD_INIT(name.tx_q) \
+  }
+
+void serial_init(unsigned long baud);
+
+size_t serial_read(char* const data, size_t max_size);
+
+void serial_write(const char* const data, size_t size);
+
+inline void serial_write_str(const char* const str) {
+  size_t length = 0;
+  for (length = 0; str[length] != 0; ++length){}
+  serial_write(str, length);
+}
+
+#endif
\ No newline at end of file
diff --git a/kernel/serial.disabled/serial.c b/kernel/serial.disabled/serial.c
new file mode 100644
index 0000000..1f66174
--- /dev/null
+++ b/kernel/serial.disabled/serial.c
@@ -0,0 +1,109 @@
+#include <avr/interrupt.h>
+#include "collection/list.h"
+#include "sched/sched.h"
+#include "sched/context.h"
+#include "bug/debug.h"
+#include "serial/serial.h"
+
+static struct serial_device_t serial = SERIAL_DEVICE_INIT(serial);
+
+void serial_init(unsigned long baud) {
+  UCSR0A |= (1 << U2X0);   //enable double speed transmission
+  uint16_t baud_setting = (F_CPU / 4 / baud - 1) / 2;
+  UBRR0H = baud_setting >> 8;
+  UBRR0L = baud_setting;
+  UCSR0B |= (1 << RXEN0) | (1 << TXEN0) | (1 << RXCIE0); // 8-bit, no parity, 1 stop bit
+  UCSR0B &= ~(1 << UDRIE0);
+
+  //EICRA = (EICRA & ~((1 << ISC00) | (1 << ISC01))) | (1 << ISC00);
+  //EIMSK |= (1 << INT0);
+}
+
+static inline int rbuffer_safe_read(struct rbuffer_t* const rb, char* d) {
+  ENTER_CRITICAL();
+  int ret = rbuffer_read(rb, d);
+  EXIT_CRITICAL();
+  return ret;
+}
+
+size_t serial_read(char* const data, size_t max_size) {
+
+  if (rbuffer_safe_read(&serial.rx_buffer, data) == 0) {
+    size_t i;
+    char d;
+    for (i = 1; i < max_size && (rbuffer_safe_read(&serial.rx_buffer, &d) == 0); ++i) {
+      data[i] = d;
+    }
+    return i;
+  } else {
+    ENTER_CRITICAL();
+    list_move_tail(&current->q, &serial.rx_q);
+    EXIT_CRITICAL();
+    yield();
+    return serial_read(data, max_size);
+  }
+}
+
+
+//called when byte is received
+ISR(USART0_RX_vect, ISR_NAKED) {
+  SAVE_CONTEXT();
+  char c = UDR0;
+  rbuffer_write(&serial.rx_buffer, c);
+  if (!list_empty(&serial.rx_q)) {
+    list_move_tail(serial.rx_q.next, &ready);
+  }
+  schedule();
+  RESTORE_CONTEXT();
+  asm volatile ("reti");
+}
+
+
+void serial_write(const char* const data, size_t size) {
+  for (size_t i = 0; i < size; ++i) {
+    ENTER_CRITICAL();
+    rbuffer_write(&serial.tx_buffer, data[i]);
+    EXIT_CRITICAL();
+    UCSR0B |= (1 << UDRIE0);
+  }
+}
+
+//called when data register is empty
+ISR(USART0_UDRE_vect) {
+  char c;
+  if (rbuffer_read(&serial.tx_buffer, &c) == 0) {
+    UDR0 = c;
+  } else {
+    UCSR0B &= ~(1 << UDRIE0); //buffer empty, disable interruot
+  }
+}
+/*
+void serial_read() {
+  ENTER_CRITICAL();
+  list_move_tail(&current->list, &serial_rx_q);
+  EXIT_CRITICAL();
+  yield();
+}
+*/
+/*
+void serial_write_str(const char* const str) {
+  for (size_t i = 0; str[i] != '\0'; ++i) {
+    ENTER_CRITICAL();
+    rbuffer_write(&serial_dev.tx_buffer, str[i]);
+    EXIT_CRITICAL();
+    UCSR0B |= (1 << UDRIE0);
+  }
+}*/
+
+
+/*
+ISR(INT3_vect, ISR_NAKED) {
+  SAVE_CONTEXT();
+  if (!list_empty(&serial_rx_q)) {
+    list_move_tail(serial_rx_q.next, &ready);
+  }
+  schedule();
+  RESTORE_CONTEXT();
+  asm volatile ("reti");
+}
+*/
diff --git a/kernel/time/include/time/timer.h b/kernel/time/include/time/timer.h
new file mode 100644
index 0000000..e59e374
--- /dev/null
+++ b/kernel/time/include/time/timer.h
@@ -0,0 +1,10 @@
+#ifndef TIMER_H
+#define TIMER_H
+
+#define HZ 100
+
+void timer_init();
+void timer_start();
+void timer_stop();
+
+#endif
diff --git a/kernel/time/mcu/atmega2560/timer.c b/kernel/time/mcu/atmega2560/timer.c
new file mode 100644
index 0000000..663df02
--- /dev/null
+++ b/kernel/time/mcu/atmega2560/timer.c
@@ -0,0 +1,31 @@
+#include <avr/interrupt.h>
+#include "sched/sched.h"
+#include "time/timer.h"
+
+void timer_init() {
+  cli();
+  TCCR3A = 0;
+  TCCR3B = 0;
+  TCCR3C = 0;
+
+  TCCR3B = (1 << WGM32); // turn on CTC mode:
+  TCCR3B |= (1 << CS32) | (0 << CS31) | (1 << CS30); // set to 1024 prescaler
+  OCR3A = 770;
+  sei();
+}
+
+void timer_start() {
+  TIMSK3 |= (1 << OCIE3A);
+}
+
+void timer_stop() {
+  TIMSK3 &= ~(1 << OCIE3A);
+}
+
+ISR(TIMER3_COMPA_vect, ISR_NAKED) {
+  SAVE_CONTEXT(); 
+  sched_tick();
+  sei();
+  RESTORE_CONTEXT();
+  asm volatile ("reti");
+}
diff --git a/kernel/tshield/include/tshield/tshield.h b/kernel/tshield/include/tshield/tshield.h
new file mode 100644
index 0000000..2624131
--- /dev/null
+++ b/kernel/tshield/include/tshield/tshield.h
@@ -0,0 +1,50 @@
+#ifndef TSHIElD_H
+#define TSHIELD_H
+
+/*
+ * Custom test shield
+ * Pin mappings
+ * 
+ * Leds (from left to right)
+ * =====
+ * Color   | Arduino pin | atmega2560
+ * ----------------------------------
+ * Green     D6            PH3
+ * Yellow    D7            PH4
+ * Red       D5            PE3
+ * Red       D4            PG5
+ * 
+ * 
+ * Buttons (from left to right)
+ * =====
+ * Number | Arduino pin | atmega2560
+ * ---------------------------------
+ * 0        D2            PE4 (INT4)
+ * 1        D8            PH5
+ * 2        D10           PB4
+ * 
+ * Outputs (from top to bottom)
+ * =====
+ * Type                        | Arduino pin | atmega2560
+ * ------------------------------------------------------
+ * Servo                         11, Vcc, GND  PB5
+ * Diode protected (max 200mA)   12            PB6
+ * 
+ */
+
+ #define TSHIELD_LED_IDLE 0
+ 
+void tshield_init();
+
+void tshield_test();
+
+unsigned char tshield_read();
+
+void tshield_led(unsigned char led, unsigned char value);
+
+void tshield_pp(unsigned char value);
+
+void tshield_servo(unsigned char angle);
+
+
+#endif
diff --git a/kernel/tshield/tshield.c b/kernel/tshield/tshield.c
new file mode 100644
index 0000000..ccf234d
--- /dev/null
+++ b/kernel/tshield/tshield.c
@@ -0,0 +1,115 @@
+#include <avr/io.h>
+#include "tshield/tshield.h"
+
+static void tshield_init_servo();
+
+void tshield_init() {
+  
+  DDRH |= (1 << 3) | (1 << 4); //leds
+  PORTH &= ~( (1 << 3) | (1 << 4));
+  DDRE |= (1 << 3);
+  PORTE &= ~(1 << 3);
+  DDRG |= (1 << 5);
+  PORTG &= ~(1 << 5);
+  
+  DDRE &= ~(1 << 4); // buttons
+  PORTE |= (1 << 4);
+  DDRH &= ~(1 << 5);
+  PORTH |= (1 << 5);
+  DDRB &= ~(1 << 4);
+  PORTB |= (1 << 4);
+  
+  DDRB |= (1 << 6);
+  PORTB &= ~(1 << 6);
+
+  tshield_init_servo();
+  
+}
+
+#define WAIT_VALUE 20000
+#define WAIT() \
+  for (volatile long x = 0; x < WAIT_VALUE; ++x){}
+  
+void tshield_test() {
+  while(1) {
+    PORTG |= (1 << 5);  
+    WAIT();
+    PORTG &= ~(1 << 5);
+    PORTE |= (1 << 3);
+    WAIT();
+    PORTE &= ~(1 << 3);
+    PORTH |=  (1 << 3);
+    WAIT();
+    PORTH &=  ~(1 << 3);
+    PORTH |=  (1 << 4);
+    WAIT();
+    PORTH &=  ~(1 << 4);
+  }
+  
+}
+
+unsigned char tshield_read() {
+  return ((PINE & (1 << 4) ? 0 : 1) << 2 ) |
+  ((PINH & (1 << 5) ? 0 : 1) << 1 ) |
+  (PINB & (1 << 4) ? 0 : 1);
+}
+
+void tshield_led(unsigned char led, unsigned char value) {
+  volatile unsigned char* port;
+  unsigned char bit;
+  switch (led) {
+    case 0:
+      port = &PORTG;
+      bit = 5;
+      break;
+    case 1:
+      port = &PORTE;
+      bit = 3;
+      break;
+    case 2:
+      port = &PORTH;
+      bit = 3;
+      break;
+    case 3:
+      port = &PORTH;
+      bit = 4;
+      break;
+    default:
+      return;
+  }
+
+  if (value) {
+    *port |= (1 << bit);
+  } else {
+    *port &= ~(1 << bit);
+  }
+
+}
+
+void tshield_pp(unsigned char value) {
+  if (value) {
+    PORTB |= (1 << 6);
+  } else {
+    PORTB &= ~(1 << 6);
+  }
+}
+
+void tshield_init_servo() {
+  
+  TCCR1A |= (1<<COM1A1); // non inverted pwm on channel A
+  TCCR1B |= (1<<CS11) | (1<<CS10); // prescaler 64
+
+  //fast pwm
+  TCCR1A |= (1<<WGM11);
+  TCCR1B|=(1<<WGM13) | (1<<WGM12);
+
+  ICR1=4999; //fPWM=50Hz (Period = 20ms Standard).
+
+  DDRB |= (1<< 5);
+}
+
+void tshield_servo(unsigned char angle) {
+  unsigned long k = (unsigned long) angle;
+  unsigned long counter = (450 * k) / 255 + 150;
+  OCR1A = (unsigned int) counter;
+}
\ No newline at end of file
diff --git a/main.c b/main.c
new file mode 100644
index 0000000..3312ad0
--- /dev/null
+++ b/main.c
@@ -0,0 +1,37 @@
+#include <stddef.h>
+#include <sched/sched.h>
+#include <time/timer.h>
+#include <bug/panic.h>
+#include <bug/debug.h>
+
+#define WAIT_CYCLES(cycles) for (volatile unsigned long i = 0; i < cycles; ++i) {}
+
+void blink( char id) {
+  while(1) {
+    debug_led(id,1);
+    //yield();
+    WAIT_CYCLES(20000);
+    debug_led(id,0);
+    //yield();
+    WAIT_CYCLES(20000);
+  }
+}
+
+DECLARE_TASK(task1, STACK_SIZE, blink);
+DECLARE_TASK(task2, STACK_SIZE, blink);
+DECLARE_TASK(task3, STACK_SIZE, blink);
+
+
+int main(int argc, char *argv[]) {
+  cli();
+  tshield_init();
+  timer_init();
+
+  spawn(&task1, 1);
+  spawn(&task2, 2);
+  spawn(&task3, 3);
+
+  timer_start();
+  sched_init();
+  while(1){}
+}