Mutex Waiting Implemented

Svc 4 and 5

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