From 26f05b04d781782f5cc6518c685e954d5f9f264c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ilja=20Karta=C5=A1ov?= Date: Mon, 20 Jan 2020 09:14:13 +0100 Subject: [PATCH] Add context switching and basic scheduler --- kernel/aarch64/aarch64.S | 57 +++++++++- kernel/aarch64/aarch64.h | 3 + kernel/aarch64/aarch64.ld | 4 +- kernel/aarch64/aarch64_boot.S | 2 +- kernel/aarch64/aarch64_reg.h | 104 ++++++++++++++++++ kernel/drivers/soc/bcm2837/bcm2837.h | 1 + kernel/drivers/timer/timer.c | 47 +++++++- kernel/drivers/timer/timer.h | 4 + kernel/leos/irq.c | 6 +- kernel/leos/irq.h | 2 +- kernel/leos/leos.c | 25 ++++- kernel/leos/task.c | 159 ++++++++++++++++++++++++++- kernel/leos/task.h | 23 ++-- 13 files changed, 410 insertions(+), 27 deletions(-) diff --git a/kernel/aarch64/aarch64.S b/kernel/aarch64/aarch64.S index e416a0a..c758351 100644 --- a/kernel/aarch64/aarch64.S +++ b/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: diff --git a/kernel/aarch64/aarch64.h b/kernel/aarch64/aarch64.h index 21b90b6..2c2b8a0 100644 --- a/kernel/aarch64/aarch64.h +++ b/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 */ diff --git a/kernel/aarch64/aarch64.ld b/kernel/aarch64/aarch64.ld index da713e9..1c1c28e 100644 --- a/kernel/aarch64/aarch64.ld +++ b/kernel/aarch64/aarch64.ld @@ -10,6 +10,6 @@ SECTIONS bss_end = .; . = ALIGN(8); - . = . + 0x8000; - stack_ptr = . ; + . = . + 0x4000; + ld_stackPtr = . ; } diff --git a/kernel/aarch64/aarch64_boot.S b/kernel/aarch64/aarch64_boot.S index 74ccc7b..7926663 100644 --- a/kernel/aarch64/aarch64_boot.S +++ b/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: diff --git a/kernel/aarch64/aarch64_reg.h b/kernel/aarch64/aarch64_reg.h index 30a6ef4..8a6db7f 100644 --- a/kernel/aarch64/aarch64_reg.h +++ b/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 diff --git a/kernel/drivers/soc/bcm2837/bcm2837.h b/kernel/drivers/soc/bcm2837/bcm2837.h index 2dc5e5e..b7a0676 100644 --- a/kernel/drivers/soc/bcm2837/bcm2837.h +++ b/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) diff --git a/kernel/drivers/timer/timer.c b/kernel/drivers/timer/timer.c index 92d864e..e74481d 100644 --- a/kernel/drivers/timer/timer.c +++ b/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; +} diff --git a/kernel/drivers/timer/timer.h b/kernel/drivers/timer/timer.h index c75bbf0..e46692d 100644 --- a/kernel/drivers/timer/timer.h +++ b/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 */ diff --git a/kernel/leos/irq.c b/kernel/leos/irq.c index 0f735b7..6fe044f 100644 --- a/kernel/leos/irq.c +++ b/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(); diff --git a/kernel/leos/irq.h b/kernel/leos/irq.h index 63febca..613b288 100644 --- a/kernel/leos/irq.h +++ b/kernel/leos/irq.h @@ -21,7 +21,7 @@ void IRQ_init(void); -unsigned long +void * IRQ_onInterrupt(void); void diff --git a/kernel/leos/leos.c b/kernel/leos/leos.c index a45cc3e..bcf10ef 100644 --- a/kernel/leos/leos.c +++ b/kernel/leos/leos.c @@ -14,29 +14,46 @@ */ #include -#include #include #include +#include #include + 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(); + } +} diff --git a/kernel/leos/task.c b/kernel/leos/task.c index afb373b..c6f0d82 100644 --- a/kernel/leos/task.c +++ b/kernel/leos/task.c @@ -13,11 +13,166 @@ * @see https://lowenware.com/ */ +#include +#include +#include +#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; +} diff --git a/kernel/leos/task.h b/kernel/leos/task.h index be050ed..377d5fc 100644 --- a/kernel/leos/task.h +++ b/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 */