diff options
| -rw-r--r-- | include/cpu.h | 6 | ||||
| -rw-r--r-- | include/sys/schedule.h | 8 | ||||
| -rw-r--r-- | src/boot.S | 10 | ||||
| -rw-r--r-- | src/exceptions/svc.S | 41 | ||||
| -rw-r--r-- | src/sys/schedule.c | 45 | ||||
| -rw-r--r-- | src/tests/test.c | 45 |
6 files changed, 145 insertions, 10 deletions
diff --git a/include/cpu.h b/include/cpu.h index 19f0e56..c200016 100644 --- a/include/cpu.h +++ b/include/cpu.h @@ -81,7 +81,11 @@ static inline void* getirqstack(void) #define sys0(sys_n) asm volatile("svc #" syscall_h_expand_and_quote(sys_n) ::: "r0", "r1", "r2", "r3"); #define sys0_64(sys_n,addr) asm volatile("svc #" syscall_h_expand_and_quote(sys_n) "\nmov r2, %0\nstr r1, [r2]\nstr r0, [r2, #4]" ::"r"(addr): "r0", "r1", "r2", "r3", "memory"); -#define sys1(sys_n,arg0) asm volatile("svc #" syscall_h_expand_and_quote(sys_n) ::[r0]"r"(arg0): "r0", "r1", "r2", "r3"); +//#define sys1(sys_n,arg0) asm volatile("svc #" syscall_h_expand_and_quote(sys_n) ::[r0]"r"(arg0): "r0", "r1", "r2", "r3"); +#define sys1(sys_n,arg0) {\ + register long r0 asm ("r0") = (long) (arg0); \ + asm volatile("svc #" syscall_h_expand_and_quote(sys_n) ::"r"(r0): "memory"); \ + } #define SYS_YIELD 0 #define SYS_TIME 1 diff --git a/include/sys/schedule.h b/include/sys/schedule.h index cb784c5..336ab4a 100644 --- a/include/sys/schedule.h +++ b/include/sys/schedule.h @@ -27,12 +27,12 @@ struct Thread { void* pc; void* sp; // Store r0-r12,lr on stack unsigned long sp_base; - void* mptr; + unsigned long cpsr; unsigned long pid; unsigned char priority; unsigned char preempt; unsigned short status; - unsigned long cpsr; + void* mptr; }; struct ThreadRotBuffer { @@ -60,10 +60,12 @@ void add_thread(void* pc, void* arg, unsigned char priority); void draw_stacks(void); void uart_scheduler(void); struct Thread* next_thread(void); -/// TODO: ENSURE IRQ/ FIQ entry switches +/// TODO: ENSURE IRQ/ FIQ entry switches /// to user mode then calls the SVC call extern void schedule(void); extern void cleanup(void); void yield(void); +void sched_mutex_yield(void* m); +void sched_mutex_resurrect(void* m); #endif @@ -1,9 +1,9 @@ // To keep this in the first portion of the binary. .section ".text.boot" - + // Make _start global. .globl _start - + _start: reset: cpsid aif @@ -32,7 +32,7 @@ reset: ldr sp, =core0_sys_stack cps #0x13 // Setup sp in SVC mode. ldr sp, =core0_svc_stack - + // Clear out bss. ldr r4, =__bss_start ldr r9, =__bss_end @@ -41,10 +41,10 @@ reset: mov r7, #0 mov r8, #0 b 2f - + 1: // store multiple at r4. stmia r4!, {r5-r8} - + 2: // If we are still below bss_end, loop. cmp r4, r9 blo 1b diff --git a/src/exceptions/svc.S b/src/exceptions/svc.S index 48d03a5..fc4653a 100644 --- a/src/exceptions/svc.S +++ b/src/exceptions/svc.S @@ -5,8 +5,12 @@ svc: stmfd sp!, {r0-r12,lr} ldr r0, [lr, #-4] bic r0, #0xFF000000 - cmp r0, #3 + cmp r0, #5 bgt svc_exit + beq svc_000005 + cmp r0, #4 + beq svc_000004 + cmp r0, #3 beq svc_000003 cmp r0, #2 beq svc_000002 @@ -42,6 +46,41 @@ svc_000003: // Clean task stack mov r0, r2 bl kfree b svc_exit +svc_000004: // Lock Mutex (usr_r0 = struct Mutex*) + ldr r3, =scheduler + ldr r2, [r3, #0] // struct Thread* rthread + ldr r1, [r2, #0x10] // unsigned long pid + ldr r0, [sp, #0] // struct Mutex* m + add r0, #4 // Point to pid +1: clrex + ldrex r2, [r0, #0] + cmp r2, #0 + bne svc_000004_delay_mutex + strexeq r2, r1, [r0, #0] + teq r2, #0 + bne 1b + dmb + b svc_exit +svc_000004_delay_mutex: + // r0 = struct Mutex* m + sub r0, #4 + bl sched_mutex_yield + ldmfd sp!, {r0-r12,lr} + b schedule +svc_000005: // Release Mutex + ldr r0, [sp, #0] // struct Mutex* m + add r0, #4 + mov r1, #0 + dmb + str r1, [r0, #0] + dsb + sev + // TODO: Branch to scheduler to awake threads awaiting mutex + sub r0, #4 + bl sched_mutex_resurrect + ldmfd sp!, {r0-r12,lr} + b schedule + b svc_exit svc_exit: ldmfd sp!, {r0-r12,pc}^ diff --git a/src/sys/schedule.c b/src/sys/schedule.c index 2764108..99e0a77 100644 --- a/src/sys/schedule.c +++ b/src/sys/schedule.c @@ -169,5 +169,50 @@ void yield(void) trb->roffset += 1; trb->roffset %= TQUEUE_MAX; trb->queue[trb->woffset++] = rthread; + trb->woffset %= TQUEUE_MAX; +} + +void sched_mutex_yield(void* m) +{ + struct Thread* rthread = scheduler.rthread; + if (rthread == &usrloopthread) + return; + unsigned char priority = rthread->priority; + rthread->mptr = m; + struct ThreadRotBuffer* trb = &scheduler.thread_queues[priority].ready; + struct ThreadRotBuffer* trbm = &scheduler.thread_queues[priority].mwait; + trb->roffset += 1; trb->roffset %= TQUEUE_MAX; + trbm->queue[trbm->woffset++] = rthread; + trbm->woffset %= TQUEUE_MAX; +} + +void sched_mutex_resurrect(void* m) +{ + for (int p = 0; p < PRIORITIES; p++) { + struct ThreadRotBuffer* trbm = &scheduler.thread_queues[p].mwait; + unsigned long roffset = trbm->roffset; + while (roffset != trbm->woffset) { + if (trbm->queue[roffset]->mptr == m) { + trbm->queue[roffset]->mptr = 0; + struct ThreadRotBuffer* trb = &scheduler.thread_queues[p].ready; + trb->queue[trb->woffset++] = trbm->queue[roffset]; + trb->woffset %= TQUEUE_MAX; + // Copy all next backward to fill space + unsigned long coffset = roffset; + while (coffset != trbm->woffset) { + trbm->queue[coffset] = trbm->queue[(coffset+1)%TQUEUE_MAX]; + coffset++; + coffset %= TQUEUE_MAX; + } + if(trbm->woffset == 0) + trbm->woffset = TQUEUE_MAX-1; + else + trbm->woffset--; + return; + } + roffset++; + roffset %= TQUEUE_MAX; + } + } } diff --git a/src/tests/test.c b/src/tests/test.c index f6213ae..af906cc 100644 --- a/src/tests/test.c +++ b/src/tests/test.c @@ -4,8 +4,10 @@ #include <lib/kmem.h> #include <sys/core.h> #include <sys/schedule.h> +#include <util/mutex.h> extern void atest(void); +void btest(void); void test_entry(void) { @@ -27,4 +29,47 @@ void test_entry(void) dt += tf - ti; DRAW64(34, 19, dt/64); DRAW64(34+17, 19, dt%64); + btest(); +} + +static struct Mutex testm = {.addr = 0, .pid = 0}; +static int testi = 0; + +void ctest1(void) +{ + uart_string("1 Started\n"); + sys1(4, &testm); + uart_string("1 Finished\n"); +} + +void ctest2(void) +{ + uart_string("2 Started\n"); + sys1(4, &testm); + uart_string("2 Finished\n"); + sys1(5, &testm); +} + +void ctest3(void) +{ + uart_string("3 Started\n"); + sys1(5, &testm); + uart_string("3 Finished\n"); +} + +void btest(void) +{ + if (testi % 3 == 0) { + testi++; + ctest1(); + } + else if (testi % 3 == 1) { + testi++; + ctest2(); + } + else if (testi % 3 == 2) { + testi++; + ctest3(); + } + uart_hexn(&testm); } |