Update to work on real hardware

kernel/Makefile

@@ -1,6 +1,8 @@
 # environment
 
-include ../Makevars.mk
+ROOT = ../
+
+include ${ROOT}Makevars.mk
 
 # CFLAGS
 
@@ -65,5 +67,5 @@ $(BUILD_DIR)kernel.sym: $(BUILD_DIR)/kernel.elf
 $(BUILD_DIR)%.o: %
 	$(info compiling file: $<)
 	@mkdir -p $(dir ./$(BUILD_DIR)$<)
-	$(AARCH64_CC) $(INCS) $(CFLAGS) -c $< -o $@
+	$(AARCH64_CC) $(INCS) $(CFLAGS) $(CONFIG_CFLAGS) -c $< -o $@
 

kernel/aarch64/aarch64.S

@@ -125,13 +125,25 @@ die:
 .globl AArch64_startTask 
 AArch64_startTask:
 	bl Task_unlockScheduler
-	blr x1
+	mov x0, x20
+	blr x21
 	
 hang:
 	/* todo: implement task exit here */
 	WFE
 	b hang
 
+.globl AArch64_getPState
+AArch64_getPState:
+	mrs x23, NZCV
+	mrs x3, DAIF
+	orr x23, x23, x3
+	mrs x3, CurrentEL
+	lsr x3, x3, 0x02
+	/* set third bit: todo: find out its real meaning (registers width?) */
+	orr x3, x3, 0x04 
+	orr x0, x23, x3
+	ret
 
 .globl AArch64_switchContext
 AArch64_switchContext:
@@ -157,6 +169,7 @@ AArch64_switchContext:
 	mrs x3, DAIF
 	orr x23, x23, x3
 	mrs x3, CurrentEL
+	lsr x3, x3, 0x02
 	orr x23, x23, x3
 
 	stp x30, x30, [sp, 16 * 15]
@@ -227,9 +240,11 @@ el1h_sync:
 
 el1h_irq:
 	SAVE_REGISTERS
+	mov x0, sp
+	bl Task_setStackPointer
 	bl IRQ_onInterrupt
-	cbz x0, load_regs
-	mov sp, x0 /* task context must be switched */
+	bl Task_getStackPointer 
+	mov sp, x0
 load_regs:
 	LOAD_REGISTERS
 	eret

kernel/aarch64/aarch64.h

@@ -16,6 +16,7 @@
 #ifndef AARCH64_H_F3AA8888_CEB3_4C16_9835_C3F23B040BB5
 #define AARCH64_H_F3AA8888_CEB3_4C16_9835_C3F23B040BB5
 
+#include <leos/types.h>
 #include "aarch64_reg.h"
 #include "aarch64_irq.h"
 
@@ -46,6 +47,9 @@ AArch64_memzero(void *addr, unsigned long size);
 extern void
 AArch64_startTask(void *arg, void *callback);
 
+extern uint32_t
+AArch64_getPState(void);
+
 extern void
 AArch64_switchContext(void *currentTask, void *nextTask);
 

kernel/config.mk

@@ -0,0 +1,7 @@
+# Target architecture (AARCH64)
+CONFIG_ARCH = AARCH64
+
+# Use ARM local timer (1/0)
+CONFIG_ARM_TIMER = 0
+
+-include config.dev.mk

kernel/drivers/timer/timer.c

@@ -18,38 +18,15 @@
 #include <leos/log.h>
 #include "timer.h"
 
-const unsigned int m_interval = 2000000;
+#if CONFIG_ARM_TIMER == 1
+const unsigned int m_interval = 500000000;
+#else
+const unsigned int m_interval = 200000;
+#endif
 unsigned int m_current = 0;
 
-#ifndef CONFIG_ARM_TIMER
-#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")
- * */
+#define TIMER_RELOAD 5000000
 
 void
 Timer_init(void)
@@ -65,28 +42,24 @@ Timer_init(void)
 	AARCH64_CORE0_TIMER_IRQCTL = (1 << 1); /* nCNTPNSIR1 - Non Secure*/
 
 #else
-	m_current = aarch64_get32r(TIMER_CLO);
+	m_current = AArch64_getReg32(TIMER_CLO);
 	m_current += m_interval;
-	aarch64_set32r(TIMER_C1, m_current);
+	AArch64_setReg32(TIMER_C1, m_current);
 #endif
 }
 
 void
 Timer_incFromISR(void)
 {
-	m_current++; /*= m_interval;*/
+  m_current += m_interval;
 #if CONFIG_ARM_TIMER == 1
 	/* clear interrupt flag and reload timer */
+//	AARCH64_LOCAL_TIMER_STATCTL |= TIMER_RELOAD;
 	AARCH64_LOCAL_TIMER_RECLR |= (1 << 31) | (1 << 30);
 #else
-	aarch64_set32r(TIMER_C1, m_current);
-	aarch64_set32r(TIMER_CS, TIMER_CS_M1);
+	AArch64_setReg32(TIMER_C1, m_current);
+	AArch64_setReg32(TIMER_CS, TIMER_CS_M1);
 #endif
-	/*
-	Log_putS("--- tick  --- ");
-	Log_putI(m_current, 10);
-	Log_putS(" ---\r\n");
-	*/
 }
 
 uint32_t

kernel/leos/irq.c

@@ -39,11 +39,11 @@ static const char *m_types[] = {
 };
 
 
-static void *
+static void
 IRQ_onTimerInterrupt(void)
 {
 	Timer_incFromISR();
-	return Task_scheduleFromISR();
+	Task_scheduleFromISR();
 }
 
 void
@@ -52,13 +52,15 @@ IRQ_init()
 	AArch64_setReg32(ENABLE_IRQS_1, SYSTEM_TIMER_IRQ_1);
 }
 
-void *
+void
 IRQ_onInterrupt(void)
 {
 	unsigned int irq = AArch64_getReg32(IRQ_PENDING_1);
 
 	switch(irq) {
+#if CONFIG_ARM_TIMER == 1
 	case LOCAL_TIMER_IRQ:
+#endif
 	case SYSTEM_TIMER_IRQ_1:
 		return IRQ_onTimerInterrupt();
 
@@ -67,7 +69,6 @@ IRQ_onInterrupt(void)
 		Log_putU(irq, 16);
 		Log_putS("\n");
 	}
-	return 0; /* do not change stack pointer */
 }
 
 void

kernel/leos/irq.h

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

kernel/leos/leos.c

@@ -13,14 +13,18 @@
  * @see https://lowenware.com/
  */
 
-#include <leos/log.h>
 #include <leos/irq.h>
 #include <leos/leos.h>
+#include <leos/log.h>
+#include <leos/memory.h>
 #include <leos/task.h>
 #include <drivers/timer/timer.h>
 
 void
-Leos_demoTask(void *arg);
+Leos_demoTask1(void *arg);
+
+void
+Leos_demoTask2(void *arg);
 
 
 void
@@ -36,27 +40,38 @@ Leos_run(void)
 
 	IRQ_init();
 	Timer_init();
-	IRQ_enable();
+	Memory_init();
 
 	Task_initSheduler();
-	if (Task_create(Leos_demoTask, "Task 1") == -1)
+
+	IRQ_enable();
+
+	if (Task_create(Leos_demoTask1, "T1") == -1)
 		Log_putS("Task 1 was not created\r\n");
+		
+	if (Task_create(Leos_demoTask2, "T2") == -1)
+		Log_putS("Task 2 was not created\r\n");
+
 
 	for (;;) {
-		Log_putU(Timer_getTicks(), 10);
-		Log_putS(". Idle Task\r\n");
+		Log_putS("I");
 		Task_yield();
 	}
 }
 
 void
-Leos_demoTask(void *arg)
+Leos_demoTask2(void *arg)
 {
 	for (;;) {
-		Log_putU(Timer_getTicks(), 10);
-		Log_putS(". ");
-		Log_putS((const char *)arg);
-		Log_putS("\r\n");
+		Log_putS("B");
+		Task_yield();
+	}
+}
+void
+Leos_demoTask1(void *arg)
+{
+	for (;;) {
+		Log_putS("A");
 		Task_yield();
 	}
 }

kernel/leos/memory.c

@@ -15,19 +15,26 @@
  */
 
 #include <stdlib.h>
+#include <aarch64/aarch64.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;
+static unsigned long m_map[MEMORY_PAGE_COUNT / sizeof(unsigned long) / 8];
+static int m_ix;
+
+void
+Memory_init(void)
+{
+	m_ix = 0;
+	AArch64_memzero(m_map, sizeof(m_map));
+}
 
 void *
 Memory_getPage(void)
 {
 	int i, j;
 	unsigned long map;
-	
 
 	for (i = m_ix; i < sizeof(m_map) / sizeof(m_map[0]); i++) {
 		map = m_map[i];

kernel/leos/memory.h

@@ -16,6 +16,10 @@
 #ifndef MEMORY_H_C583E24E_55B0_49EF_99C9_5A36B04468AC
 #define MEMORY_H_C583E24E_55B0_49EF_99C9_5A36B04468AC
 
+
+void
+Memory_init(void);
+
 void *
 Memory_getPage(void);
 

kernel/leos/task.c

@@ -16,11 +16,13 @@
 #include <stdlib.h>
 #include <aarch64/aarch64.h>
 #include <drivers/soc/bcm2837/bcm2837.h>
+#include <drivers/timer/timer.h>
 #include "memory.h"
 #include "log.h"
 #include "task.h"
 
 #define TASK_STATE_RUNNING 0
+#define TASK_STATE_HALFCYCLE (1 << 8)
 
 #ifndef CONFIG_IDLE_TASK_STACK_SIZE
 #define CONFIG_IDLE_TASK_STACK_SIZE 0x4000
@@ -46,21 +48,8 @@ struct __attribute__((packed)) TaskContext {
 	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
-		, .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 *m_currentTask, *m_lastTask, m_idleTask;
+
 
 static uint32_t m_pid = 1;
 
@@ -70,7 +59,9 @@ scheduleNext(void)
 	struct Task *i, *next = NULL;
 	int32_t priority = -1;
 
-	Log_putS("scheduleNext\r\n ");
+	if (m_currentTask->counter)
+		m_currentTask->counter--;
+
 	for (;;) {
 		/* check urgent tasks */
 		for (i = &m_idleTask; i != NULL; i = i->next) {
@@ -81,10 +72,6 @@ scheduleNext(void)
 				priority = i->priority;
 				next = i;
 			}
-			if (!i->state && i->counter > priority) {
-				priority = i->priority;
-				next = i;
-			}
 		}
 
 		if (next) {
@@ -96,10 +83,6 @@ scheduleNext(void)
 		}
 	}
 
-	Log_putS("\t switch: ");
-	Log_putU((uint64_t)next->pid, 10);
-	Log_putS("\n");
-
 	return next;
 }
 
@@ -107,6 +90,24 @@ void
 Task_initSheduler(void)
 {
 	struct Task *idleTask = &m_idleTask;
+
+	m_pid = 1;
+	m_idleTask.sp = NULL;
+	m_idleTask.stackStart = NULL;
+	m_idleTask.stackSize = CONFIG_IDLE_TASK_STACK_SIZE;
+	m_idleTask.pid = 1;
+	m_idleTask.lock = 1;
+	m_idleTask.counter = 1;
+	m_idleTask.cycles = 1;
+	m_idleTask.priority = 0;
+	m_idleTask.state = TASK_STATE_RUNNING;
+	m_idleTask.name[0] = 'I';
+	m_idleTask.name[1] = 'D';
+	m_idleTask.name[2] = 'L';
+	m_idleTask.name[3] = 'E';
+	m_idleTask.name[4] = 0;
+	m_idleTask.next = NULL;
+
 	m_currentTask = idleTask;
 	m_lastTask = idleTask;
 	Task_unlockScheduler();
@@ -120,6 +121,12 @@ Task_create(TaskCallback callback, void *arg)
 
 	if (!task)
 		return -1;
+
+	Log_putS("Task @0x");
+	Log_putU((uint64_t) task, 16);
+	Log_putS(" - ");
+	Log_putU(((uint64_t) task) + MEMORY_PAGE_SIZE, 16);
+	Log_putS("\r\n");
 	
 	task->sp = (void *)task + MEMORY_PAGE_SIZE - 272;
 	task->stackStart = task->sp;
@@ -138,10 +145,11 @@ Task_create(TaskCallback callback, void *arg)
 	task->next = 0;
 	
 	ctx = (struct TaskContext *) task->sp;
-	ctx->x[0] = (uint64_t) arg;
-	ctx->x[1] = (uint64_t) callback;
+	ctx->x[20] = (uint64_t) arg;
+	ctx->x[21] = (uint64_t) callback;
 	ctx->x[30] = (uint64_t) AArch64_startTask;
 	ctx->elr_el1 = (uint64_t) AArch64_startTask;
+	ctx->spsr_el1 = AArch64_getPState();
 
 	Task_lockScheduler();
 	m_lastTask->next = task;
@@ -154,25 +162,41 @@ Task_create(TaskCallback callback, void *arg)
 void
 Task_yield(void)
 {
-	struct Task *next, *prev = m_currentTask;
+#if 1
+	uint64_t ticks = Timer_getTicks();
 
-	Log_putS("Task_yield()\r\n");
 	Task_lockScheduler();
+	Log_putS("^");
+	m_currentTask->counter = 0;
+	Task_unlockScheduler();
+	__asm__("WFE");
+
+	while (ticks == Timer_getTicks());
+#else
+	/* Task yield should be solved differently */
+	struct Task *next, *prev;
+
+	Task_lockScheduler();
+	Log_putS("^");
+	prev = m_currentTask;
 	prev->counter = 0;
 
 	next = scheduleNext();
 
 	if (next != prev) {
+		next->state |= TASK_STATE_HALFCYCLE;
 		m_currentTask = next;
+		Task_unlockScheduler();
 		AArch64_switchContext(prev, next);
-		/* newly created tasks never exit here, that is why in AArch64_startTask
-		 * there is own Task_unlockScheduler() call
+		/* `next` tasks never exits here, but in AArch64_startTask,
+		 * that is why there is an own Task_unlockScheduler() call
 		 * */
 	} else {
-		__asm__("WFE");
+		// __asm__("WFE");
 	}
 
 	Task_unlockScheduler();
+#endif
 }
 
 void
@@ -184,35 +208,55 @@ Task_lockScheduler(void)
 void
 Task_unlockScheduler(void)
 {
-	m_currentTask->lock--;
+	if (m_currentTask->lock)
+		m_currentTask->lock--;
 }
 
-void *
+void
 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");
+
+		if (m_currentTask->state & TASK_STATE_HALFCYCLE) {
+			m_currentTask->state &= ~TASK_STATE_HALFCYCLE;
+			return;
+		}
+
+		Log_putS("!");
 
 		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();
 	}
-#endif
-
-	return sp;
+}
+
+void *
+Task_getStackPointer(void)
+{
+	/*
+	Log_putS("gSP:");
+	Log_putU((uint64_t)m_currentTask->sp, 16);
+	Log_putS("\r\n");
+	*/
+	return m_currentTask->sp;
+}
+
+void
+Task_setStackPointer(void *sp)
+{
+	/*
+	Log_putS("sSP:");
+	Log_putU((uint64_t)sp, 16);
+	Log_putS("\r\n");
+	*/
+	m_currentTask->sp = sp;
 }

kernel/leos/task.h

@@ -39,7 +39,13 @@ Task_lockScheduler(void);
 void
 Task_unlockScheduler(void);
 
-void *
+void
 Task_scheduleFromISR(void);
 
+void *
+Task_getStackPointer(void);
+
+void
+Task_setStackPointer(void *sp);
+
 #endif /* !TASK_H */