diff --git a/srv/task.c b/srv/task.c
index 4a2221d..0c40a82 100644
--- a/srv/task.c
+++ b/srv/task.c
@@ -6,3 +6,136 @@
  * see https://dunkels.com/adam/pt/index.html
  */
 
+//--includes--------------------------------------------------------------------
+
+#include "task.h"
+#include "../drv/stk.h"
+#include "error.h"
+
+
+//--local definitions-----------------------------------------------------------
+
+#define MAX_TASK_NB 10
+
+struct TaskContext {
+	Task function;
+	uint32_t delay_ms;
+	uint8_t state;
+	uint8_t priority;
+};
+
+void task_callback(void);
+
+
+//--local variables-------------------------------------------------------------
+
+static struct TaskContext task_list[MAX_TASK_NB];
+static uint8_t task_nb;
+
+
+//--public functions------------------------------------------------------------
+
+void task_start_scheduler(void)
+{
+	while (1) {
+		uint32_t delay = UINT32_MAX;
+		for (uint8_t priority = 0; priority < UINT8_MAX; ++priority) {
+
+			bool task_runned = false;
+			for (uint8_t i = 0; i < task_nb; ++i) {
+				if (task_list[i].function != nullptr) {
+					if (task_list[i].priority == priority) {
+						if (task_list[i].delay_ms == 0) {
+							task_runned = true;
+							struct TaskReturn ret
+								= task_list[i].function(task_list[i].state);
+							if (ret.state == _TASK_COUNT_EXIT) {
+								task_list[i].function = nullptr;
+							} else {
+								task_list[i].delay_ms = ret.delay_ms;
+								task_list[i].state = ret.state;
+							}
+						}
+						if (delay > task_list[i].delay_ms) {
+							delay = task_list[i].delay_ms;
+						}
+					}
+				}
+			}
+
+			if (task_runned) {
+				break;
+			}
+		}
+
+		if (delay > 0) {
+			stk_configure(delay * 1000, task_callback);
+			stk_start();
+			__asm("wfi");
+			stk_stop();
+			uint32_t stk_delay = stk_read();
+			if (stk_delay != 0) {
+				delay = stk_delay;
+			}
+
+			for (uint8_t i = 0; i < task_nb; ++i) {
+				if (task_list[i].function != nullptr) {
+					task_list[i].delay_ms -= delay;
+				}
+			}
+		}
+	}
+
+	//never returns
+}
+
+void task_start(Task task, uint8_t priority)
+{
+	for (uint8_t i = 0; i < MAX_TASK_NB; ++i) {
+		if (task_list[i].function == nullptr) {
+
+			task_list[i].function = task;
+			task_list[i].delay_ms = 0;
+			task_list[i].state = 0;
+			task_list[i].priority = priority;
+
+			++task_nb;
+			return;
+		}
+	}
+
+	error_trigger("task list is full");
+}
+
+void task_stop(Task task)
+{
+	for (uint8_t i = 0; i < task_nb; ++i) {
+		if (task_list[i].function == task) {
+			task_list[i].function(_TASK_COUNT_CLEANUP);
+			task_list[i].function = nullptr;
+
+			return;
+		}
+	}
+
+	error_trigger("task does not exist");
+}
+
+bool task_is_running(Task task)
+{
+	for (uint8_t i = 0; i < task_nb; ++i) {
+		if (task_list[i].function == task) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+
+
+//--local functions-------------------------------------------------------------
+
+void task_callback(void) {
+	stk_stop();
+}
diff --git a/srv/task.h b/srv/task.h
index f91b782..361da76 100644
--- a/srv/task.h
+++ b/srv/task.h
@@ -12,41 +12,85 @@
 //--includes--------------------------------------------------------------------
 
 #include <stdint.h>
+#include <stdbool.h>
+
+
+//--type definitions------------------------------------------------------------
+
+struct TaskReturn {
+	uint32_t delay_ms;
+	uint8_t state;
+};
+
+typedef struct TaskReturn(*Task)(uint8_t);
 
 
 //--functions-------------------------------------------------------------------
 
-#define TASK(fct_name) uint8_t fct_name(uint8_t __task_context)
+#define TASK(fct_name) struct TaskReturn fct_name(uint8_t __task_state)
 
-#define task_begin() do {     \
-	_TASK_COUNT_INIT;         \
-	switch (__task_context) { \
-	} while (0)
+#define TASK_ENTRY          \
+	_TASK_COUNT_INIT;       \
+	switch (__task_state) {
 
-#define task_end() do {      \
-	}                        \
-	return _TASK_COUNT_STOP; \
-	while (0)
+#define TASK_CLEANUP            \
+	case (_TASK_COUNT_CLEANUP): \
+		/* fall through */
 
-#define task_wait_until(cond) _task_wait_until(cond, _TASK_COUNT_INCR)
+#define TASK_EXIT                 \
+	}                             \
+	return (struct TaskReturn){0, _TASK_COUNT_EXIT};
+
+#define TASK_YIELD() _TASK_YIELD(_TASK_COUNT_INCR)
+#define TASK_SLEEP(delay_ms) _TASK_SLEEP(delay_ms, _TASK_COUNT_INCR)
+#define TASK_WAIT_UNTIL(cond) _TASK_WAIT_UNTIL(cond, _TASK_COUNT_INCR)
+#define TASK_EXECUTE(task) _TASK_EXECUTE(task, _TASK_COUNT_INCR)
+
+void task_start_scheduler(void);
+
+void task_start(Task task, uint8_t priority);
+void task_stop(Task task);
+bool task_is_running(Task task);
 
 
 //--internal_functions----------------------------------------------------------
 
 #define _TASK_COUNT_INIT enum { TASK_COUNTER_BASE = __COUNTER__ }
 #define _TASK_COUNT_INCR (uint8_t)(__COUNTER__ - TASK_COUNTER_BASE - 1)
-#define _TASK_COUNT_STOP UINT8_MAX
+#define _TASK_COUNT_EXIT UINT8_MAX
+#define _TASK_COUNT_CLEANUP (UINT8_MAX - 1)
 
-#define _task(fct_name)
+#define _TASK(fct_name)
 
-#define _task_wait_until(cond, count) do { \
-	case (count):                          \
-		if (!(cond)) {                     \
-			return count;                  \
-		}                                  \
-		/* fall through */                 \
-	while (0)
+#define _TASK_YIELD(count) do {           \
+	return (struct TaskReturn){0, count}; \
+	case (count):                         \
+		/* fall through */                \
+	} while (0)
 
+#define _TASK_SLEEP(delay_ms, count) do {        \
+	return (struct TaskReturn){delay_ms, count}; \
+	case (count):                                \
+		/* fall through */                       \
+	} while (0)
+
+#define _TASK_WAIT_UNTIL(cond, count) do {        \
+	case (count):                                 \
+		if (!(cond)) {                            \
+			return (struct TaskReturn){0, count}; \
+		}                                         \
+		/* fall through */                        \
+	} while (0)
+
+#define _TASK_EXECUTE(task, count) do {           \
+	task_start(Task task);                        \
+	return (struct TaskReturn){0, count);         \
+	case (count):                                 \
+		if (task_is_running(task)) {              \
+			return (struct TaskReturn){0, count}; \
+		}                                         \
+		/* fall through */                        \
+	} while (0)
 
 #endif //_task_h_