aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--include/cpu.h6
-rw-r--r--include/sys/schedule.h8
-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
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);
}