Add task creation and fix Task_yield procedure

kernel/aarch64/aarch64.S

@@ -122,6 +122,17 @@ AArch64_memzero:
 die:
 	b die
 
+.globl AArch64_startTask 
+AArch64_startTask:
+	bl Task_unlockScheduler
+	blr x1
+	
+hang:
+	/* todo: implement task exit here */
+	WFE
+	b hang
+
+
 .globl AArch64_switchContext
 AArch64_switchContext:
 	/* Save current task context */

kernel/aarch64/aarch64.h

@@ -43,6 +43,9 @@ AArch64_disableIRQ(void);
 extern void
 AArch64_memzero(void *addr, unsigned long size);
 
+extern void
+AArch64_startTask(void *arg, void *callback);
+
 extern void
 AArch64_switchContext(void *currentTask, void *nextTask);
 

kernel/drivers/timer/timer.c

@@ -18,7 +18,7 @@
 #include <leos/log.h>
 #include "timer.h"
 
-const unsigned int m_interval = 200000;
+const unsigned int m_interval = 2000000;
 unsigned int m_current = 0;
 
 #ifndef CONFIG_ARM_TIMER
@@ -83,9 +83,9 @@ Timer_incFromISR(void)
 	aarch64_set32r(TIMER_CS, TIMER_CS_M1);
 #endif
 	/*
-	Log_putS("Timer: ");
+	Log_putS("--- tick  --- ");
 	Log_putI(m_current, 10);
-	Log_putS("\r\n");
+	Log_putS(" ---\r\n");
 	*/
 }
 

kernel/drivers/timer/timer.h

@@ -16,6 +16,7 @@
 #ifndef TIMER_H_8D327261_47D6_4832_8DC5_31BF1614A21F
 #define TIMER_H_8D327261_47D6_4832_8DC5_31BF1614A21F
 
+#include <leos/types.h>
 
 void
 Timer_init(void);

kernel/leos/leos.c

@@ -19,6 +19,8 @@
 #include <leos/task.h>
 #include <drivers/timer/timer.h>
 
+void
+Leos_demoTask(void *arg);
 
 
 void
@@ -37,21 +39,22 @@ Leos_run(void)
 	IRQ_enable();
 
 	Task_initSheduler();
-	Task_create(Leos_demoTask, "Task 1");
+	if (Task_create(Leos_demoTask, "Task 1") == -1)
+		Log_putS("Task 1 was not created\r\n");
 
 	for (;;) {
 		Log_putU(Timer_getTicks(), 10);
-		Log_putS(". idle\r\n");
+		Log_putS(". Idle Task\r\n");
 		Task_yield();
 	}
 }
 
-
 void
 Leos_demoTask(void *arg)
 {
 	for (;;) {
 		Log_putU(Timer_getTicks(), 10);
+		Log_putS(". ");
 		Log_putS((const char *)arg);
 		Log_putS("\r\n");
 		Task_yield();

kernel/leos/memory.c

@@ -17,6 +17,7 @@
 #include <stdlib.h>
 #include <drivers/soc/bcm2837/bcm2837.h>
 #include "memory.h"
+#include "log.h"
 
 static unsigned long m_map[MEMORY_PAGE_COUNT / sizeof(unsigned long) / 8] = {0,};
 static int m_ix = 0;
@@ -33,12 +34,13 @@ Memory_getPage(void)
 		for (j = 0; j < 8 * sizeof(map); j++) {
 			if (!(map & (1 << j))) {
 				m_map[i] |= (1 << j);
-				return (void *) ((i * sizeof(map) * 8 + j) * MEMORY_PAGE_SIZE);
+				return (void *) (MEMORY_LOW + (i * sizeof(map) * 8 + j) * MEMORY_PAGE_SIZE);
 			}
 		}
 		m_ix = i;
 	}
 
+	Log_putS("ENOMEM\r\n");
 	return NULL;
 }
 

kernel/leos/task.c

@@ -17,6 +17,7 @@
 #include <aarch64/aarch64.h>
 #include <drivers/soc/bcm2837/bcm2837.h>
 #include "memory.h"
+#include "log.h"
 #include "task.h"
 
 #define TASK_STATE_RUNNING 0
@@ -28,6 +29,7 @@
 struct Task {
 	void *sp;
 	void *stackStart;
+	uint32_t pid;
 	uint32_t stackSize;
 	uint32_t lock;
 	uint32_t counter;
@@ -38,12 +40,19 @@ struct Task {
 	struct Task *next;
 };
 
+struct __attribute__((packed)) TaskContext {
+	uint64_t x[31];
+	uint64_t elr_el1;
+	uint64_t spsr_el1;
+};
+
 static struct Task *m_currentTask = NULL
 	, *m_lastTask = NULL
 	, m_idleTask = {
 		.sp = NULL
 		, .stackStart = NULL
 		, .stackSize = CONFIG_IDLE_TASK_STACK_SIZE
+		, .pid = 1
 		, .lock = 1
 		, .counter = 1
 		, .cycles = 1
@@ -53,12 +62,15 @@ static struct Task *m_currentTask = NULL
 		, .next = NULL
 	};
 
+static uint32_t m_pid = 1;
+
 static struct Task *
 scheduleNext(void)
 {
 	struct Task *i, *next = NULL;
 	int32_t priority = -1;
 
+	Log_putS("scheduleNext\r\n ");
 	for (;;) {
 		/* check urgent tasks */
 		for (i = &m_idleTask; i != NULL; i = i->next) {
@@ -84,6 +96,10 @@ scheduleNext(void)
 		}
 	}
 
+	Log_putS("\t switch: ");
+	Log_putU((uint64_t)next->pid, 10);
+	Log_putS("\n");
+
 	return next;
 }
 
@@ -92,13 +108,18 @@ Task_initSheduler(void)
 {
 	struct Task *idleTask = &m_idleTask;
 	m_currentTask = idleTask;
-	m_lastTask = m_currentTask;
+	m_lastTask = idleTask;
+	Task_unlockScheduler();
 }
 
 PID
 Task_create(TaskCallback callback, void *arg)
 {
+	struct TaskContext *ctx;
 	struct Task *task = Memory_getPage();
+
+	if (!task)
+		return -1;
 	
 	task->sp = (void *)task + MEMORY_PAGE_SIZE - 272;
 	task->stackStart = task->sp;
@@ -106,6 +127,7 @@ Task_create(TaskCallback callback, void *arg)
 	task->lock = 1;
 	task->counter = 1;
 	task->cycles = 1;
+	task->pid = ++m_pid;
 	task->priority = 0;
 	task->state = TASK_STATE_RUNNING;
 	task->name[0] = 'N';
@@ -114,27 +136,38 @@ Task_create(TaskCallback callback, void *arg)
 	task->name[3] = 'E';
 	task->name[4] = 0;
 	task->next = 0;
+	
+	ctx = (struct TaskContext *) task->sp;
+	ctx->x[0] = (uint64_t) arg;
+	ctx->x[1] = (uint64_t) callback;
+	ctx->x[30] = (uint64_t) AArch64_startTask;
+	ctx->elr_el1 = (uint64_t) AArch64_startTask;
 
 	Task_lockScheduler();
 	m_lastTask->next = task;
 	m_lastTask = task;
 	Task_unlockScheduler();
-	return 0;
+
+	return task->pid;
 }
 
 void
 Task_yield(void)
 {
-	struct Task *next;
+	struct Task *next, *prev = m_currentTask;
 
+	Log_putS("Task_yield()\r\n");
 	Task_lockScheduler();
-	m_currentTask->counter = 0;
+	prev->counter = 0;
 
 	next = scheduleNext();
 
-	if (next != m_currentTask) {
+	if (next != prev) {
-		AArch64_switchContext(m_currentTask, next);
 		m_currentTask = next;
+		AArch64_switchContext(prev, next);
+		/* newly created tasks never exit here, that is why in AArch64_startTask
+		 * there is own Task_unlockScheduler() call
+		 * */
 	} else {
 		__asm__("WFE");
 	}
@@ -159,10 +192,16 @@ Task_scheduleFromISR(void)
 {
 	void *sp = NULL;
 
+#	if 0
+	if (m_currentTask->counter)
+		m_currentTask->counter--;
+
 	if (!m_currentTask->lock) {
 		struct Task *next;
+		Log_putS("Task_scheduleFromISR\r\n");
 
 		Task_lockScheduler();
+
 		next = scheduleNext();
 		if (next != m_currentTask) {
 			m_currentTask = next;
@@ -173,6 +212,7 @@ Task_scheduleFromISR(void)
 		 * */
 		Task_unlockScheduler();
 	}
+#endif
 
 	return sp;
 }