Add context switching and basic scheduler

kernel/leos/irq.c

@@ -39,26 +39,26 @@ static const char *m_types[] = {
 };
 
 
-static unsigned long
+static void *
 IRQ_onTimerInterrupt(void)
 {
 	Timer_incFromISR();
 	return Task_scheduleFromISR();
 }
 
-
 void
 IRQ_init()
 {
 	AArch64_setReg32(ENABLE_IRQS_1, SYSTEM_TIMER_IRQ_1);
 }
 
-unsigned long
+void *
 IRQ_onInterrupt(void)
 {
 	unsigned int irq = AArch64_getReg32(IRQ_PENDING_1);
 
 	switch(irq) {
+	case LOCAL_TIMER_IRQ:
 	case SYSTEM_TIMER_IRQ_1:
 		return IRQ_onTimerInterrupt();
 

kernel/leos/irq.h

@@ -21,7 +21,7 @@
 void
 IRQ_init(void);
 
-unsigned long
+void *
 IRQ_onInterrupt(void);
 
 void

kernel/leos/leos.c

@@ -14,29 +14,46 @@
  */
 
 #include <leos/log.h>
-#include <leos/arch.h>
 #include <leos/irq.h>
 #include <leos/leos.h>
+#include <leos/task.h>
 #include <drivers/timer/timer.h>
 
 
+
 void
 Leos_run(void)
 {
 	Log_init();
 
-	Log_putS("Starting Lowe OS (EL");
+	Log_putS("Starting LEOS (EL");
 	Log_putI(AArch64_getEL(), 10);
 	Log_putS(")\r\n");
 
 	AArch64_init();
 
-	Timer_init();
 	IRQ_init();
+	Timer_init();
 	IRQ_enable();
 
+	Task_initSheduler();
+	Task_create(Leos_demoTask, "Task 1");
+
 	for (;;) {
-		__asm__("WFE");
+		Log_putU(Timer_getTicks(), 10);
+		Log_putS(". idle\r\n");
+		Task_yield();
 	}
 }
 
+
+void
+Leos_demoTask(void *arg)
+{
+	for (;;) {
+		Log_putU(Timer_getTicks(), 10);
+		Log_putS((const char *)arg);
+		Log_putS("\r\n");
+		Task_yield();
+	}
+}

kernel/leos/task.c

@@ -13,11 +13,166 @@
  * @see https://lowenware.com/
  */
 
+#include <stdlib.h>
+#include <aarch64/aarch64.h>
+#include <drivers/soc/bcm2837/bcm2837.h>
+#include "memory.h"
 #include "task.h"
 
+#define TASK_STATE_RUNNING 0
 
-unsigned long
-Task_scheduleFromISR(void)
+#ifndef CONFIG_IDLE_TASK_STACK_SIZE
+#define CONFIG_IDLE_TASK_STACK_SIZE 0x4000
+#endif
+
+struct Task {
+	void *sp;
+	void *stackStart;
+	uint32_t stackSize;
+	uint32_t lock;
+	uint32_t counter;
+	uint32_t cycles;
+	int32_t priority;
+	uint32_t state;
+	char name[CONFIG_TASK_MAX_NAME_LEN + 1];
+	struct Task *next;
+};
+
+static struct Task *m_currentTask = NULL
+	, *m_lastTask = NULL
+	, m_idleTask = {
+		.sp = NULL
+		, .stackStart = NULL
+		, .stackSize = CONFIG_IDLE_TASK_STACK_SIZE
+		, .lock = 1
+		, .counter = 1
+		, .cycles = 1
+		, .priority = 0
+		, .state = TASK_STATE_RUNNING
+		, .name = {'I', 'D', 'L', 'E', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
+		, .next = NULL
+	};
+
+static struct Task *
+scheduleNext(void)
 {
+	struct Task *i, *next = NULL;
+	int32_t priority = -1;
+
+	for (;;) {
+		/* check urgent tasks */
+		for (i = &m_idleTask; i != NULL; i = i->next) {
+			if (i->state != TASK_STATE_RUNNING)
+				continue;
+
+			if (i->priority > priority && i->counter) {
+				priority = i->priority;
+				next = i;
+			}
+			if (!i->state && i->counter > priority) {
+				priority = i->priority;
+				next = i;
+			}
+		}
+
+		if (next) {
+			break;
+		}
+
+		for (i = &m_idleTask; i != NULL; i = i->next) {
+			i->counter = i->cycles;
+		}
+	}
+
+	return next;
+}
+
+void
+Task_initSheduler(void)
+{
+	struct Task *idleTask = &m_idleTask;
+	m_currentTask = idleTask;
+	m_lastTask = m_currentTask;
+}
+
+PID
+Task_create(TaskCallback callback, void *arg)
+{
+	struct Task *task = Memory_getPage();
+	
+	task->sp = (void *)task + MEMORY_PAGE_SIZE - 272;
+	task->stackStart = task->sp;
+	task->stackSize = MEMORY_PAGE_SIZE - sizeof(*task);
+	task->lock = 1;
+	task->counter = 1;
+	task->cycles = 1;
+	task->priority = 0;
+	task->state = TASK_STATE_RUNNING;
+	task->name[0] = 'N';
+	task->name[1] = 'O';
+	task->name[2] = 'N';
+	task->name[3] = 'E';
+	task->name[4] = 0;
+	task->next = 0;
+
+	Task_lockScheduler();
+	m_lastTask->next = task;
+	m_lastTask = task;
+	Task_unlockScheduler();
 	return 0;
 }
+
+void
+Task_yield(void)
+{
+	struct Task *next;
+
+	Task_lockScheduler();
+	m_currentTask->counter = 0;
+
+	next = scheduleNext();
+
+	if (next != m_currentTask) {
+		AArch64_switchContext(m_currentTask, next);
+		m_currentTask = next;
+	} else {
+		__asm__("WFE");
+	}
+
+	Task_unlockScheduler();
+}
+
+void
+Task_lockScheduler(void)
+{
+	m_currentTask->lock++;
+}
+
+void
+Task_unlockScheduler(void)
+{
+	m_currentTask->lock--;
+}
+
+void *
+Task_scheduleFromISR(void)
+{
+	void *sp = NULL;
+
+	if (!m_currentTask->lock) {
+		struct Task *next;
+
+		Task_lockScheduler();
+		next = scheduleNext();
+		if (next != m_currentTask) {
+			m_currentTask = next;
+			sp = next->sp;
+		}
+		/* unlock call could be moved to aarch64.S interrupt handler in case of
+		 * issue
+		 * */
+		Task_unlockScheduler();
+	}
+
+	return sp;
+}

kernel/leos/task.h

@@ -24,19 +24,22 @@
 
 typedef void (*TaskCallback)(void *p_ctx);
 
-struct Task {
-	char name[CONFIG_TASK_MAX_NAME_LEN + 1];
-	TaskCallback callback;
-	uint64_t *stack;
-	uint32_t stack_size;
-	uint32_t priority;
-};
+void
+Task_initSheduler(void);
 
 PID
-Task_create(struct Task *pTask, TaskCallback callback, uint64_t *stack
-		, uint32_t stack_size);
+Task_create(TaskCallback callback, void *arg);
 
-unsigned long
+void
+Task_yield(void);
+
+void
+Task_lockScheduler(void);
+
+void
+Task_unlockScheduler(void);
+
+void *
 Task_scheduleFromISR(void);
 
 #endif /* !TASK_H */