Got some scheduling

4 files changed, 71 insertions, 74 deletions

diff --git a/src/sys/core.c b/src/sys/core.c
index fc3d9e1..aa10bdf 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -16,6 +16,7 @@
 #include <util/time.h>
 
 void testlocal(void);
+void longlocal(void);
 
 // Initialize IRQs
 void sysinit(void)
@@ -68,48 +69,17 @@ void sysinit(void)
 	add_thread(testlocal, 0, 1);
 	add_thread(testlocal, 0, 3);
 	add_thread(testlocal, 0, 5);
+	add_thread(longlocal, 0, 5);
 	uart_scheduler();
 }
 
-void testlocal1(void)
-{
-	//unsigned long a = 5;
-	//struct Thread* t = scheduler.rthread_ll->data;
-	//uart_string("vRan Thread ");
-	//uart_10(t->data.pid);
-	//uart_string(" Pri. ");
-	//uart_10(t->data.priority);
-	//uart_string(" ...\n");
-	//add_thread(testlocal, 0);
-	//schedule();
-	//a += t->data.pid;
-	//uart_10(a);
-	//uart_string(" Done!\n");
-}
-
 void testlocal(void)
 {
-	//struct Thread* t = scheduler.rthread_ll->data;
-	//uart_string("Ran Thread ");
-	//uart_10(t->data.pid);
-	//uart_string(" Pri. ");
-	//uart_10(t->data.priority);
-	//uart_string(" ...\n");
-	////delay(0x80000000);
-	//if (t->data.pid == 5) {
-	//	add_thread(testlocal1, 1);
-	//	schedule();
-	//}
-	//if (t->data.pid == 3) {
-	//	// Example
-	//	/*
-	//		while (uart_tx_full) {
-	//			t->data.status = THREAD_WAITING;
-	//			schedule();
-	//		} // Will wait until uart_tx is not full
-	//	*/
-	//}
-	//uart_string("Done! ");
-	//uart_10(t->data.pid);
-	//uart_char('\n');
+	uart_string("Ran thread!\n");
+	if (scheduler.rthread->pid == 4) {
+		add_thread(testlocal, 0, 0);
+		//uart_scheduler();
+	}
+	uart_hexn((unsigned long)getsp());
+	uart_string("Exiting thread!\n");
 }
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index d9819a2..b76df62 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -3,24 +3,22 @@
 .globl kernel_main
 kernel_main:
 	bl sysinit
-	///https://wiki.osdev.org/ARM_Paging
-	// Query the ID_MMFR0 register
-	mrc p15, 0, r0, c0, c1, 4
-	bl uart_hexn
-	// Switch to user mode
+	// ///https://wiki.osdev.org/ARM_Paging
+	// // Query the ID_MMFR0 register
+	// mrc p15, 0, r0, c0, c1, 4
+	// bl uart_hexn
+	// // Switch to user mode
+	// cps #0x10
+	// // Intentional undefined instruction
+	// //.word 0xf7f0a000
+	// // This will fail in user mode
+	// mrc p15, 3, r0, c15, c0, 0
+	// // Supervisor Call #1 - Does nothing special
+	// svc #1
+	// mrs r0, cpsr
+	// bl uart_hexn
 	cps #0x10
-	// Intentional undefined instruction
-	//.word 0xf7f0a000
-	// This will fail in user mode
-	mrc p15, 3, r0, c15, c0, 0
-	// Supervisor Call #1 - Does nothing special
-	svc #1
-	mrs r0, cpsr
-	bl uart_hexn
-	// Supervisor Call #1 - Returns in supervisor mode
-	svc #0
-	mrs r0, cpsr
-	bl uart_hexn
+	svc #2 // Start scheduling!
 1:
 	wfe
 	b 1b
diff --git a/src/sys/schedule.S b/src/sys/schedule.S
index f80b707..6b6ef1c 100644
--- a/src/sys/schedule.S
+++ b/src/sys/schedule.S
@@ -12,7 +12,7 @@
 	ldr r2, [r3, #0] // struct Thread* rthread
 	str lr, [r2, #0] // svc_lr -> void* pc
 	str sp, [r2, #4] // svc_lr -> void* sp
-	cps #0x10 // Svc mode
+	cps #0x13 // Svc mode
 .endm
 
 .macro restore_ctx
@@ -25,7 +25,7 @@
 	// Restore Usr regs
 	ldmfd sp!, {lr}
 	ldmfd sp!, {r0-r12}
-	cps #0x10 // Svc mode
+	cps #0x13 // Svc mode
 .endm
 
 // Assumption: Enter in SVC mode
@@ -41,12 +41,34 @@ schedule:
 	mov r0, #0
 	str r0, [r1]
 1:
-	// bl get_next_thread // next_thread -> r0
+	bl next_thread // Thread* next -> r0
 	ldr r3, =scheduler
-	ldr r2, [r3, #0] // struct Thread* current
+	ldr r2, [r3, #0] // Thread* current
 	cmp r0, r2 // current = next?
 	beq 2f
 	str r0, [r3, #0] // next -> rthread
 2:
 	restore_ctx
-	movs pc, lr
+	subs pc, lr, #0
+
+.globl cleanup
+cleanup:
+	// roffset++
+	bl get_rthread_roffset
+	ldr r1, [r0, #0]
+	add r1, #1
+	str r1, [r0, #0]
+	// usrloop -> rthread
+	ldr r2, =usrloopthread
+	str r2, [r3, #0]
+	ldr sp, [r2, #4]
+	ldmfd sp!,{lr}
+	ldmfd sp!,{r0-r12}
+	ldr lr, =kernel_usr_task_loop
+	// svc sched
+	svc #2
+.globl kernel_usr_task_loop
+kernel_usr_task_loop:
+	wfe
+	//svc #2
+	b kernel_usr_task_loop
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index 119ad6b..5b311a9 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -3,15 +3,15 @@
 #include <sys/schedule.h>
 #include <util/mutex.h>
 
-void loop(void);
-void cleanup(void);
+extern void kernel_usr_task_loop(void);
+extern void cleanup(void);
 
 void init_scheduler(void)
 {
 	// Set rthread to usrloopthread - an infinitely running thread so that the pointer will never be null
-	usrloopthread.pc = (void*)loop;
+	usrloopthread.pc = (void*)kernel_usr_task_loop;
 	usrloopthread.sp = (void*)0x5FC8;
-	*(unsigned long**)usrloopthread.sp = (unsigned long*)loop;
+	*(unsigned long**)usrloopthread.sp = (unsigned long*)kernel_usr_task_loop;
 	usrloopthread.sp_base = (void*)0x6000;
 	usrloopthread.mptr = 0;
 	usrloopthread.pid = -1;
@@ -35,12 +35,6 @@ void init_scheduler(void)
 	nextpid = SCHED_PID + 1;
 }
 
-void loop(void)
-{
-	while(1)
-		asm volatile ("wfe");
-}
-
 void* get_stack(void)
 {
 	for (int i = 0; i < MAX_THREADS; i++) {
@@ -111,10 +105,10 @@ void uart_scheduler(void)
 			uart_char('\n');
 			unsigned long roffset = trb->roffset;
 			while (roffset != trb->woffset) {
-				uart_hex((unsigned long)&trb->queue[roffset]);
+				uart_hex((unsigned long)trb->queue[roffset]);
 				uart_char(' ');
 				memshow32((void*)trb->queue[roffset], 6);
-				memshow32((void*)trb->queue[roffset]->sp, 14);
+				//memshow32((void*)trb->queue[roffset]->sp, 14);
 				roffset++;
 				roffset %= TQUEUE_MAX;
 			}
@@ -124,6 +118,19 @@ void uart_scheduler(void)
 	uart_string("==============\n");
 }
 
-void cleanup(void)
+struct Thread* next_thread(void)
+{
+	struct Thread* next = &usrloopthread;
+	for (int p = 0; p < PRIORITIES; p++) {
+		struct ThreadRotBuffer* rb = &scheduler.thread_queues[p].ready;
+		if (rb->roffset == rb->woffset)
+			continue;
+		return rb->queue[rb->roffset];
+	}
+	return next;
+}
+
+void* get_rthread_roffset(void)
 {
+	return &scheduler.thread_queues[scheduler.rthread->priority].ready.roffset;
 }