diff --git a/kernel/aarch64/aarch64.S b/kernel/aarch64/aarch64.S index c758351..4892a3e 100644 --- a/kernel/aarch64/aarch64.S +++ b/kernel/aarch64/aarch64.S @@ -122,6 +122,17 @@ AArch64_memzero: die: b die +.globl AArch64_startTask +AArch64_startTask: + bl Task_unlockScheduler + blr x1 + +hang: + /* todo: implement task exit here */ + WFE + b hang + + .globl AArch64_switchContext AArch64_switchContext: /* Save current task context */ diff --git a/kernel/aarch64/aarch64.h b/kernel/aarch64/aarch64.h index 2c2b8a0..03c1cb0 100644 --- a/kernel/aarch64/aarch64.h +++ b/kernel/aarch64/aarch64.h @@ -43,6 +43,9 @@ AArch64_disableIRQ(void); extern void AArch64_memzero(void *addr, unsigned long size); +extern void +AArch64_startTask(void *arg, void *callback); + extern void AArch64_switchContext(void *currentTask, void *nextTask); diff --git a/kernel/drivers/timer/timer.c b/kernel/drivers/timer/timer.c index e74481d..67047dc 100644 --- a/kernel/drivers/timer/timer.c +++ b/kernel/drivers/timer/timer.c @@ -18,7 +18,7 @@ #include #include "timer.h" -const unsigned int m_interval = 200000; +const unsigned int m_interval = 2000000; unsigned int m_current = 0; #ifndef CONFIG_ARM_TIMER @@ -83,9 +83,9 @@ Timer_incFromISR(void) aarch64_set32r(TIMER_CS, TIMER_CS_M1); #endif /* - Log_putS("Timer: "); + Log_putS("--- tick --- "); Log_putI(m_current, 10); - Log_putS("\r\n"); + Log_putS(" ---\r\n"); */ } diff --git a/kernel/drivers/timer/timer.h b/kernel/drivers/timer/timer.h index e46692d..65c338f 100644 --- a/kernel/drivers/timer/timer.h +++ b/kernel/drivers/timer/timer.h @@ -16,6 +16,7 @@ #ifndef TIMER_H_8D327261_47D6_4832_8DC5_31BF1614A21F #define TIMER_H_8D327261_47D6_4832_8DC5_31BF1614A21F +#include void Timer_init(void); diff --git a/kernel/leos/leos.c b/kernel/leos/leos.c index bcf10ef..3a7efc9 100644 --- a/kernel/leos/leos.c +++ b/kernel/leos/leos.c @@ -19,6 +19,8 @@ #include #include +void +Leos_demoTask(void *arg); void @@ -37,21 +39,22 @@ Leos_run(void) IRQ_enable(); Task_initSheduler(); - Task_create(Leos_demoTask, "Task 1"); + if (Task_create(Leos_demoTask, "Task 1") == -1) + Log_putS("Task 1 was not created\r\n"); for (;;) { Log_putU(Timer_getTicks(), 10); - Log_putS(". idle\r\n"); + Log_putS(". Idle Task\r\n"); Task_yield(); } } - void Leos_demoTask(void *arg) { for (;;) { Log_putU(Timer_getTicks(), 10); + Log_putS(". "); Log_putS((const char *)arg); Log_putS("\r\n"); Task_yield(); diff --git a/kernel/leos/memory.c b/kernel/leos/memory.c index f9b6a0f..bbd59ca 100644 --- a/kernel/leos/memory.c +++ b/kernel/leos/memory.c @@ -17,6 +17,7 @@ #include #include #include "memory.h" +#include "log.h" static unsigned long m_map[MEMORY_PAGE_COUNT / sizeof(unsigned long) / 8] = {0,}; static int m_ix = 0; @@ -33,12 +34,13 @@ Memory_getPage(void) for (j = 0; j < 8 * sizeof(map); j++) { if (!(map & (1 << j))) { m_map[i] |= (1 << j); - return (void *) ((i * sizeof(map) * 8 + j) * MEMORY_PAGE_SIZE); + return (void *) (MEMORY_LOW + (i * sizeof(map) * 8 + j) * MEMORY_PAGE_SIZE); } } m_ix = i; } + Log_putS("ENOMEM\r\n"); return NULL; } diff --git a/kernel/leos/task.c b/kernel/leos/task.c index c6f0d82..f389dc1 100644 --- a/kernel/leos/task.c +++ b/kernel/leos/task.c @@ -17,6 +17,7 @@ #include #include #include "memory.h" +#include "log.h" #include "task.h" #define TASK_STATE_RUNNING 0 @@ -28,6 +29,7 @@ struct Task { void *sp; void *stackStart; + uint32_t pid; uint32_t stackSize; uint32_t lock; uint32_t counter; @@ -38,12 +40,19 @@ struct Task { struct Task *next; }; +struct __attribute__((packed)) TaskContext { + uint64_t x[31]; + uint64_t elr_el1; + 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 @@ -53,12 +62,15 @@ static struct Task *m_currentTask = NULL , .next = NULL }; +static uint32_t m_pid = 1; + static struct Task * scheduleNext(void) { struct Task *i, *next = NULL; int32_t priority = -1; + Log_putS("scheduleNext\r\n "); for (;;) { /* check urgent tasks */ for (i = &m_idleTask; i != NULL; i = i->next) { @@ -84,6 +96,10 @@ scheduleNext(void) } } + Log_putS("\t switch: "); + Log_putU((uint64_t)next->pid, 10); + Log_putS("\n"); + return next; } @@ -92,13 +108,18 @@ Task_initSheduler(void) { struct Task *idleTask = &m_idleTask; m_currentTask = idleTask; - m_lastTask = m_currentTask; + m_lastTask = idleTask; + Task_unlockScheduler(); } PID Task_create(TaskCallback callback, void *arg) { + struct TaskContext *ctx; struct Task *task = Memory_getPage(); + + if (!task) + return -1; task->sp = (void *)task + MEMORY_PAGE_SIZE - 272; task->stackStart = task->sp; @@ -106,6 +127,7 @@ Task_create(TaskCallback callback, void *arg) task->lock = 1; task->counter = 1; task->cycles = 1; + task->pid = ++m_pid; task->priority = 0; task->state = TASK_STATE_RUNNING; task->name[0] = 'N'; @@ -114,27 +136,38 @@ Task_create(TaskCallback callback, void *arg) task->name[3] = 'E'; task->name[4] = 0; task->next = 0; + + ctx = (struct TaskContext *) task->sp; + ctx->x[0] = (uint64_t) arg; + ctx->x[1] = (uint64_t) callback; + ctx->x[30] = (uint64_t) AArch64_startTask; + ctx->elr_el1 = (uint64_t) AArch64_startTask; Task_lockScheduler(); m_lastTask->next = task; m_lastTask = task; Task_unlockScheduler(); - return 0; + + return task->pid; } void Task_yield(void) { - struct Task *next; + struct Task *next, *prev = m_currentTask; + Log_putS("Task_yield()\r\n"); Task_lockScheduler(); - m_currentTask->counter = 0; + prev->counter = 0; next = scheduleNext(); - if (next != m_currentTask) { - AArch64_switchContext(m_currentTask, next); + if (next != prev) { m_currentTask = next; + AArch64_switchContext(prev, next); + /* newly created tasks never exit here, that is why in AArch64_startTask + * there is own Task_unlockScheduler() call + * */ } else { __asm__("WFE"); } @@ -159,10 +192,16 @@ 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"); Task_lockScheduler(); + next = scheduleNext(); if (next != m_currentTask) { m_currentTask = next; @@ -173,6 +212,7 @@ Task_scheduleFromISR(void) * */ Task_unlockScheduler(); } +#endif return sp; }