Add context switching and basic scheduler

kernel/aarch64/aarch64.S

@@ -56,7 +56,7 @@ AArch64_memzero:
 /* Exceptions Vector Table
  * */
 	.macro SAVE_REGISTERS
-	sub sp, sp, 256
+	sub sp, sp, 272
 	stp x0, x1, [sp, 16 * 0]
 	stp x2, x3, [sp, 16 * 1]
 	stp x4, x5, [sp, 16 * 2]
@@ -102,7 +102,7 @@ AArch64_memzero:
 	ldp x4, x5, [sp, 16 * 2]
 	ldp x2, x3, [sp, 16 * 1]
 	ldp x0, x1, [sp, 16 * 0]
-	add sp, sp, 256
+	add sp, sp, 272
 	.endm
 
 	.macro VECTOR_ENTRY GOTO_LABEL
@@ -122,6 +122,59 @@ AArch64_memzero:
 die:
 	b die
 
+.globl AArch64_switchContext
+AArch64_switchContext:
+	/* Save current task context */
+	sub sp, sp, 272
+	stp x0, x1, [sp, 16 * 0]
+	stp x2, x3, [sp, 16 * 1]
+	stp x4, x5, [sp, 16 * 2]
+	stp x6, x7, [sp, 16 * 3]
+	stp x8, x9, [sp, 16 * 4]
+	stp x10, x11, [sp, 16 * 5]
+	stp x12, x13, [sp, 16 * 6]
+	stp x14, x15, [sp, 16 * 7]
+	stp x16, x17, [sp, 16 * 8]
+	stp x18, x19, [sp, 16 * 9]
+	stp x20, x21, [sp, 16 * 10]
+	stp x22, x23, [sp, 16 * 11]
+	stp x24, x25, [sp, 16 * 12]
+	stp x26, x27, [sp, 16 * 13]
+	stp x28, x29, [sp, 16 * 14]
+
+	mrs x23, NZCV
+	mrs x3, DAIF
+	orr x23, x23, x3
+	mrs x3, CurrentEL
+	orr x23, x23, x3
+
+	stp x30, x30, [sp, 16 * 15]
+	str x23, [sp, 16 * 16]
+	mov x2, sp
+	str x2, [x0]
+	/* Restore next task context */
+	ldr x2, [x1]
+	mov sp, x2
+	ldr x30, [sp, 16 * 15]
+	ldp x28, x29, [sp, 16 * 14]
+	ldp x26, x27, [sp, 16 * 13]
+	ldp x24, x25, [sp, 16 * 12]
+	ldp x22, x23, [sp, 16 * 11]
+	ldp x20, x21, [sp, 16 * 10]
+	ldp x18, x19, [sp, 16 * 9]
+	ldp x16, x17, [sp, 16 * 8]
+	ldp x14, x15, [sp, 16 * 7]
+	ldp x12, x13, [sp, 16 * 6]
+	ldp x10, x11, [sp, 16 * 5]
+	ldp x8, x9, [sp, 16 * 4]
+	ldp x6, x7, [sp, 16 * 3]
+	ldp x4, x5, [sp, 16 * 2]
+	ldp x2, x3, [sp, 16 * 1]
+	ldp x0, x1, [sp, 16 * 0]
+	add sp, sp, 272
+	ret
+
+
 .align 11
 .globl AArch64_vectors
 AArch64_vectors:

kernel/aarch64/aarch64.h

@@ -43,4 +43,7 @@ AArch64_disableIRQ(void);
 extern void
 AArch64_memzero(void *addr, unsigned long size);
 
+extern void
+AArch64_switchContext(void *currentTask, void *nextTask);
+
 #endif /* !AARCH64_H */

kernel/aarch64/aarch64.ld

@@ -10,6 +10,6 @@ SECTIONS
 	bss_end = .;
 
 	. = ALIGN(8);
-	. = . + 0x8000;
+	. = . + 0x4000;
-	stack_ptr = . ;
+	ld_stackPtr = . ;
 }

kernel/aarch64/aarch64_boot.S

@@ -31,7 +31,7 @@ set_el:
 	eret
 
 set_stack:
-	ldr x30, =stack_ptr  /* defined in aarch64.ld */
+	ldr x30, =ld_stackPtr  /* defined in aarch64.ld */
 	mov sp,  x30
 	bl skip
 skip:

kernel/aarch64/aarch64_reg.h

@@ -16,6 +16,110 @@
 #ifndef AARCH64_REG_H_C74FE7BF_C8A6_4718_867A_C125CBF326BD
 #define AARCH64_REG_H_C74FE7BF_C8A6_4718_867A_C125CBF326BD
 
+#define AARCH64_LOCAL_INT_ROUTING *((uint32_t *) 0x40000024)
+
+#define AARCH64_LOCAL_TIMER_STATCTL *((uint32_t *) 0x40000034)
+
+#define AARCH64_LOCAL_TIMER_RECLR *((uint32_t *) 0x40000038)
+
+/* Timers interrupt control registers 
+ * */
+#define AARCH64_CORE0_TIMER_IRQCTL *((uint32_t *) 0x40000040)
+#define AARCH64_CORE1_TIMER_IRQCTL *((uint32_t *) 0x40000044)
+#define AARCH64_CORE2_TIMER_IRQCTL *((uint32_t *) 0x40000048)
+#define AARCH64_CORE3_TIMER_IRQCTL *((uint32_t *) 0x4000004C)
+
+/* Timer interrupr routing 
+ * */
+#define AARCH64_TIMER0_IRQ 0x01
+#define AARCH64_TIMER1_IRQ 0x02
+#define AARCH64_TIMER2_IRQ 0x04
+#define AARCH64_TIMER3_IRQ 0x08
+#define AARCH64_TIMER0_FIQ 0x10
+#define AARCH64_TIMER1_FIQ 0x20
+#define AARCH64_TIMER2_FIQ 0x40
+#define AARCH64_TIMER3_FIQ 0x80
+
+/* Mailbox control registers 
+ * */
+#define AARCH64_CORE0_MBOX_IRQCTL *((uint32_t *) 0x40000050)
+#define AARCH64_CORE1_MBOX_IRQCTL *((uint32_t *) 0x40000054)
+#define AARCH64_CORE2_MBOX_IRQCTL *((uint32_t *) 0x40000058)
+#define AARCH64_CORE3_MBOX_IRQCTL *((uint32_t *) 0x4000005C)
+
+/* Mailbox interrupr routing 
+ * */
+#define AARCH64_MBOX0_IRQ 0x01
+#define AARCH64_MBOX1_IRQ 0x02
+#define AARCH64_MBOX2_IRQ 0x04
+#define AARCH64_MBOX3_IRQ 0x08
+#define AARCH64_MBOX0_FIQ 0x10
+#define AARCH64_MBOX1_FIQ 0x20
+#define AARCH64_MBOX2_FIQ 0x40
+#define AARCH64_MBOX3_FIQ 0x80
+
+/* Source registers for IRQ / FIQ
+ * */
+#define AARCH64_CORE0_IRQSRC *((uint32_t *) 0x40000060)
+#define AARCH64_CORE1_IRQSRC *((uint32_t *) 0x40000064)
+#define AARCH64_CORE2_IRQSRC *((uint32_t *) 0x40000068)
+#define AARCH64_CORE3_IRQSRC *((uint32_t *) 0x4000006C)
+#define AARCH64_CORE0_FIQSRC *((uint32_t *) 0x40000070)
+#define AARCH64_CORE1_FIQSRC *((uint32_t *) 0x40000074)
+#define AARCH64_CORE2_FIQSRC *((uint32_t *) 0x40000078)
+#define AARCH64_CORE3_FIQSRC *((uint32_t *) 0x4000007C)
+
+/* Interrupt source bits
+ * */
+#define AARCH64_TIMER0_INTSRC 0x0001
+#define AARCH64_TIMER1_INTSRC 0x0002
+#define AARCH64_TIMER2_INTSRC 0x0004
+#define AARCH64_TIMER3_INTSRC 0x0008
+#define AARCH64_MBOX0_INTSRC  0x0010
+#define AARCH64_MBOX1_INTSRC  0x0020
+#define AARCH64_MBOX2_INTSRC  0x0040
+#define AARCH64_MBOX3_INTSRC  0x0080
+#define AARCH64_GPU_INTSRC    0x0100
+#define AARCH64_PMU_INTSRC    0x0200
+
+/* Mailbox write-set registers (write only)
+ * */
+#define AARCH64_CORE0_MBOX0_SETREG *((uint32_t *) 0x40000080)
+#define AARCH64_CORE0_MBOX1_SETREG *((uint32_t *) 0x40000084)
+#define AARCH64_CORE0_MBOX2_SETREG *((uint32_t *) 0x40000088)
+#define AARCH64_CORE0_MBOX3_SETREG *((uint32_t *) 0x4000008C)
+#define AARCH64_CORE1_MBOX0_SETREG *((uint32_t *) 0x40000090)
+#define AARCH64_CORE1_MBOX1_SETREG *((uint32_t *) 0x40000094)
+#define AARCH64_CORE1_MBOX2_SETREG *((uint32_t *) 0x40000098)
+#define AARCH64_CORE1_MBOX3_SETREG *((uint32_t *) 0x4000009C)
+#define AARCH64_CORE2_MBOX0_SETREG *((uint32_t *) 0x400000A0)
+#define AARCH64_CORE2_MBOX1_SETREG *((uint32_t *) 0x400000A4)
+#define AARCH64_CORE2_MBOX2_SETREG *((uint32_t *) 0x400000A8)
+#define AARCH64_CORE2_MBOX3_SETREG *((uint32_t *) 0x400000AC)
+#define AARCH64_CORE3_MBOX0_SETREG *((uint32_t *) 0x400000B0)
+#define AARCH64_CORE3_MBOX1_SETREG *((uint32_t *) 0x400000B4)
+#define AARCH64_CORE3_MBOX2_SETREG *((uint32_t *) 0x400000B8)
+#define AARCH64_CORE3_MBOX3_SETREG *((uint32_t *) 0x400000BC)
+
+/* Mailbox write-clear registers (Read & Write)
+ * */
+#define AARCH64_CORE0_MBOX0_RDCLREG *((uint32_t *) 0x400000C0)
+#define AARCH64_CORE0_MBOX1_RDCLREG *((uint32_t *) 0x400000C4)
+#define AARCH64_CORE0_MBOX2_RDCLREG *((uint32_t *) 0x400000C8)
+#define AARCH64_CORE0_MBOX3_RDCLREG *((uint32_t *) 0x400000CC)
+#define AARCH64_CORE1_MBOX0_RDCLREG *((uint32_t *) 0x400000D0)
+#define AARCH64_CORE1_MBOX1_RDCLREG *((uint32_t *) 0x400000D4)
+#define AARCH64_CORE1_MBOX2_RDCLREG *((uint32_t *) 0x400000D8)
+#define AARCH64_CORE1_MBOX3_RDCLREG *((uint32_t *) 0x400000DC)
+#define AARCH64_CORE2_MBOX0_RDCLREG *((uint32_t *) 0x400000E0)
+#define AARCH64_CORE2_MBOX1_RDCLREG *((uint32_t *) 0x400000E4)
+#define AARCH64_CORE2_MBOX2_RDCLREG *((uint32_t *) 0x400000E8)
+#define AARCH64_CORE2_MBOX3_RDCLREG *((uint32_t *) 0x400000EC)
+#define AARCH64_CORE3_MBOX0_RDCLREG *((uint32_t *) 0x400000F0)
+#define AARCH64_CORE3_MBOX1_RDCLREG *((uint32_t *) 0x400000F4)
+#define AARCH64_CORE3_MBOX2_RDCLREG *((uint32_t *) 0x400000F8)
+#define AARCH64_CORE3_MBOX3_RDCLREG *((uint32_t *) 0x400000FC)
+
 /* Saved Program Status Register (SPSR)
  * Exception Level 3 (EL3)
  * See page 389 of AArch64-Reference-Manual

kernel/drivers/soc/bcm2837/bcm2837.h

@@ -62,6 +62,7 @@
 #define DISABLE_IRQS_2     (PERIPHERAL_BASE + 0x0000B220)
 #define DISABLE_BASIC_IRQS (PERIPHERAL_BASE + 0x0000B224)
 
+#define LOCAL_TIMER_IRQ 0
 /* SYSTEM TIMER */
 #define SYSTEM_TIMER_IRQ_0 (1 << 0)
 #define SYSTEM_TIMER_IRQ_1 (1 << 1)

kernel/drivers/timer/timer.c

@@ -25,10 +25,45 @@ unsigned int m_current = 0;
 #define CONFIG_ARM_TIMER 1
 #endif
 
+#define TIMER_ENABLE ((1 << 31) | (1 << 29) | (1 << 28))
+#define TIMER_RELOAD 1
+
+/*
+ * https://www.raspberrypi.org/forums/viewtopic.php?t=213393
+ * The sequence on a real PI3 is
+ * 1.) Route the local timer to a core register 0x40000024 (bits 0..2)
+ * QA7_rev3.4.pdf page 18 ... say write 0 which is core 0
+ *
+ * 2.) Setup timer status control register 0x40000034 (all 32 bits)
+ * QA7_rev3.4.pdf page 17 ... reload value 5000000 = like half sec, enable clock, enable interrupt
+ * You can play with clock prescalers etc later.
+ * 3.) Hit timer interrupt clear and reload register 0x40000038 (bits 30 & 31)
+ * QA7_rev3.4.pdf page 18 ... write 1 to both bits which clears irq signal and loads value from above
+ *
+ * 4.) Setup timer interrupt control register 0x40000040 (all bits ... zero all but the one bit set)
+ * QA7_rev3.4.pdf page 13 ... now this depends what mode Core0 leaves your bootstub in.
+ * If you did no EL changes in stub the core0 will still be in Hyp mode if like me you dropped it to SVC mode it is Non Secure
+ *
+ * If Core0 enters in Hyp mode ... set nCNTHPIRQ_IRQ bit 1
+ * If Core0 enters in Svc mode ... set nCNTPNSIRQ_IRQ bit 2
+ *
+ * 5.) Now you need to enable global interupts
+ * asm(" cpsie i")
+ * */
+
 void
 Timer_init(void)
 {
 #if CONFIG_ARM_TIMER == 1
+	/* Route local timer to a core0 */
+	AARCH64_LOCAL_INT_ROUTING &= ~0x03;
+	/* Set up timer status control register */
+	AARCH64_LOCAL_TIMER_STATCTL = TIMER_ENABLE | TIMER_RELOAD;
+	/* clear interrupt flag and reload timer */
+	AARCH64_LOCAL_TIMER_RECLR |= (1 << 31) | (1 << 30);
+	/* Set timer interrupt control register */
+	AARCH64_CORE0_TIMER_IRQCTL = (1 << 1); /* nCNTPNSIR1 - Non Secure*/
+
 #else
 	m_current = aarch64_get32r(TIMER_CLO);
 	m_current += m_interval;
@@ -39,15 +74,23 @@ Timer_init(void)
 void
 Timer_incFromISR(void)
 {
+	m_current++; /*= m_interval;*/
 #if CONFIG_ARM_TIMER == 1
-
+	/* clear interrupt flag and reload timer */
+	AARCH64_LOCAL_TIMER_RECLR |= (1 << 31) | (1 << 30);
 #else
-	m_current += m_interval;
 	aarch64_set32r(TIMER_C1, m_current);
 	aarch64_set32r(TIMER_CS, TIMER_CS_M1);
 #endif
+	/*
 	Log_putS("Timer: ");
 	Log_putI(m_current, 10);
 	Log_putS("\r\n");
+	*/
 }
 
+uint32_t
+Timer_getTicks(void)
+{
+	return m_current;
+}

kernel/drivers/timer/timer.h

@@ -16,10 +16,14 @@
 #ifndef TIMER_H_8D327261_47D6_4832_8DC5_31BF1614A21F
 #define TIMER_H_8D327261_47D6_4832_8DC5_31BF1614A21F
 
+
 void
 Timer_init(void);
 
 void
 Timer_incFromISR(void);
 
+uint32_t
+Timer_getTicks(void);
+
 #endif /* !TIMER_H */

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
+#ifndef CONFIG_IDLE_TASK_STACK_SIZE
-Task_scheduleFromISR(void)
+#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 {
+void
-	char name[CONFIG_TASK_MAX_NAME_LEN + 1];
+Task_initSheduler(void);
-	TaskCallback callback;
-	uint64_t *stack;
-	uint32_t stack_size;
-	uint32_t priority;
-};
 
 PID
-Task_create(struct Task *pTask, TaskCallback callback, uint64_t *stack
+Task_create(TaskCallback callback, void *arg);
-		, uint32_t stack_size);
 
-unsigned long
+void
+Task_yield(void);
+
+void
+Task_lockScheduler(void);
+
+void
+Task_unlockScheduler(void);
+
+void *
 Task_scheduleFromISR(void);
 
 #endif /* !TASK_H */