Mutex Waiting Implemented

Svc 4 and 5

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);
 }