aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/boot.S10
-rw-r--r--src/exceptions/svc.S41
-rw-r--r--src/sys/schedule.c45
-rw-r--r--src/tests/test.c45
4 files changed, 135 insertions, 6 deletions
diff --git a/src/boot.S b/src/boot.S
index e4da373..6b04301 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -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);
}