Add context switching and basic scheduler

This commit is contained in:
Ilja Kartašov 2020-01-20 09:14:13 +01:00
parent a93f257f2e
commit 26f05b04d7
13 changed files with 410 additions and 27 deletions

View File

@ -56,7 +56,7 @@ AArch64_memzero:
/* Exceptions Vector Table /* Exceptions Vector Table
* */ * */
.macro SAVE_REGISTERS .macro SAVE_REGISTERS
sub sp, sp, 256 sub sp, sp, 272
stp x0, x1, [sp, 16 * 0] stp x0, x1, [sp, 16 * 0]
stp x2, x3, [sp, 16 * 1] stp x2, x3, [sp, 16 * 1]
stp x4, x5, [sp, 16 * 2] stp x4, x5, [sp, 16 * 2]
@ -102,7 +102,7 @@ AArch64_memzero:
ldp x4, x5, [sp, 16 * 2] ldp x4, x5, [sp, 16 * 2]
ldp x2, x3, [sp, 16 * 1] ldp x2, x3, [sp, 16 * 1]
ldp x0, x1, [sp, 16 * 0] ldp x0, x1, [sp, 16 * 0]
add sp, sp, 256 add sp, sp, 272
.endm .endm
.macro VECTOR_ENTRY GOTO_LABEL .macro VECTOR_ENTRY GOTO_LABEL
@ -122,6 +122,59 @@ AArch64_memzero:
die: die:
b 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 .align 11
.globl AArch64_vectors .globl AArch64_vectors
AArch64_vectors: AArch64_vectors:

View File

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

View File

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

View File

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

View File

@ -16,6 +16,110 @@
#ifndef AARCH64_REG_H_C74FE7BF_C8A6_4718_867A_C125CBF326BD #ifndef AARCH64_REG_H_C74FE7BF_C8A6_4718_867A_C125CBF326BD
#define 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) /* Saved Program Status Register (SPSR)
* Exception Level 3 (EL3) * Exception Level 3 (EL3)
* See page 389 of AArch64-Reference-Manual * See page 389 of AArch64-Reference-Manual

View File

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

View File

@ -25,10 +25,45 @@ unsigned int m_current = 0;
#define CONFIG_ARM_TIMER 1 #define CONFIG_ARM_TIMER 1
#endif #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 void
Timer_init(void) Timer_init(void)
{ {
#if CONFIG_ARM_TIMER == 1 #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 #else
m_current = aarch64_get32r(TIMER_CLO); m_current = aarch64_get32r(TIMER_CLO);
m_current += m_interval; m_current += m_interval;
@ -39,15 +74,23 @@ Timer_init(void)
void void
Timer_incFromISR(void) Timer_incFromISR(void)
{ {
m_current++; /*= m_interval;*/
#if CONFIG_ARM_TIMER == 1 #if CONFIG_ARM_TIMER == 1
/* clear interrupt flag and reload timer */
AARCH64_LOCAL_TIMER_RECLR |= (1 << 31) | (1 << 30);
#else #else
m_current += m_interval;
aarch64_set32r(TIMER_C1, m_current); aarch64_set32r(TIMER_C1, m_current);
aarch64_set32r(TIMER_CS, TIMER_CS_M1); aarch64_set32r(TIMER_CS, TIMER_CS_M1);
#endif #endif
/*
Log_putS("Timer: "); Log_putS("Timer: ");
Log_putI(m_current, 10); Log_putI(m_current, 10);
Log_putS("\r\n"); Log_putS("\r\n");
*/
} }
uint32_t
Timer_getTicks(void)
{
return m_current;
}

View File

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

View File

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

View File

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

View File

@ -14,29 +14,46 @@
*/ */
#include <leos/log.h> #include <leos/log.h>
#include <leos/arch.h>
#include <leos/irq.h> #include <leos/irq.h>
#include <leos/leos.h> #include <leos/leos.h>
#include <leos/task.h>
#include <drivers/timer/timer.h> #include <drivers/timer/timer.h>
void void
Leos_run(void) Leos_run(void)
{ {
Log_init(); Log_init();
Log_putS("Starting Lowe OS (EL"); Log_putS("Starting LEOS (EL");
Log_putI(AArch64_getEL(), 10); Log_putI(AArch64_getEL(), 10);
Log_putS(")\r\n"); Log_putS(")\r\n");
AArch64_init(); AArch64_init();
Timer_init();
IRQ_init(); IRQ_init();
Timer_init();
IRQ_enable(); IRQ_enable();
Task_initSheduler();
Task_create(Leos_demoTask, "Task 1");
for (;;) { 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();
}
}

View File

@ -13,11 +13,166 @@
* @see https://lowenware.com/ * @see https://lowenware.com/
*/ */
#include <stdlib.h>
#include <aarch64/aarch64.h>
#include <drivers/soc/bcm2837/bcm2837.h>
#include "memory.h"
#include "task.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; 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;
}

View File

@ -24,19 +24,22 @@
typedef void (*TaskCallback)(void *p_ctx); 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 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); Task_scheduleFromISR(void);
#endif /* !TASK_H */ #endif /* !TASK_H */