Got some scheduling

8 files changed, 123 insertions, 94 deletions

diff --git a/include/cpu/irq.h b/include/cpu/irq.h
index 807df46..03402ff 100644
--- a/include/cpu/irq.h
+++ b/include/cpu/irq.h
@@ -19,6 +19,6 @@ static inline void disablefiq(void)
 }
 
 void c_irq_handler(void);
-void c_fiq_handler(void);
+unsigned long c_fiq_handler(void);
 
 #endif
diff --git a/include/sys/schedule.h b/include/sys/schedule.h
index 4677aec..d5f3519 100644
--- a/include/sys/schedule.h
+++ b/include/sys/schedule.h
@@ -49,10 +49,11 @@ struct Scheduler {
 void init_scheduler(void);
 void add_thread(void* pc, void* arg, unsigned char priority);
 void uart_scheduler(void);
+struct Thread* next_thread(void);
 /// 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);
-// struct Thread* next_thread(void);
 
 #endif
diff --git a/src/boot.S b/src/boot.S
index 42e991a..27f6ddc 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -120,28 +120,34 @@ svc:
 	stmfd sp!, {r0-r12,lr}
 	ldr r0, [lr, #-4]
 	bic r0, #0xFF000000
-	push {r0}
-	ldr r0, =svc_msg
-	bl uart_string
-	ldr r0, [sp]
-	bl uart_hexn
-	pop {r0}
+	/// push {r0}
+	/// ldr r0, =svc_msg
+	/// bl uart_string
+	/// ldr r0, [sp]
+	/// bl uart_hexn
+	/// pop {r0}
 	// SVC #0 returns to supervisor mode
 	// TODO: Make supervisor mode return to a specific location
 	//  (rather than to a user location) such as the kernel loop
 	cmp r0, #0
+	bne 2f
+	cps #0x13
+	b   1f
+2:
+	cmp r0, #2
 	bne 1f
-	mrs r0, spsr
-	bic r0, #0x1f
-	// Return in supervisor mode
-	orr r0, #0x13
-	msr spsr, r0
+	ldmfd sp!, {r0-r12,lr}
+	bl schedule
 1:
 	ldmfd sp!, {r0-r12,pc}^
 io_halt_prefetch:
 	stmfd sp!, {r0-r12,lr}
+	push {lr}
 	ldr r0, =prefetch_msg
 	bl uart_string
+	pop {lr}
+	mov r0, lr
+	bl uart_hexn
 	ldmfd sp!, {r0-r12,pc}^
 io_halt_data:
 	stmfd sp!, {r0-r12,lr}
@@ -158,6 +164,17 @@ fiq:
 	//ldr r0, =fiq_msg
 	//bl uart_string
 	bl c_fiq_handler
+	cmp r0, #1
+	bne 1f
+	ldmfd sp!, {r0-r12,lr}
+	sub lr, #4
+	stmfd sp!, {r0-r12,lr}
+	ldr r3, =irqlr
+	str lr, [r3, #0]
+	ldmfd sp!, {r0-r12,lr}
+	cps #0x13
+	b schedule
+1:
 	ldmfd sp!, {r0-r12,lr}
 	subs pc, lr, #4
 
diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index 7a9bcaa..10f8583 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -68,7 +68,14 @@ void c_irq_handler(void)
 						} else if (data == 0x61) {
 							cmd[off] = (char) data;
 							off += 1;
-							_start(); // Trigger reset
+							//_start(); // Trigger reset
+							add_thread(localtest, 0, 0);
+							//heap_info_u();
+						} else if (data == 0x62) {
+							cmd[off] = (char) data;
+							off += 1;
+							//_start(); // Trigger reset
+							uart_scheduler();
 							//heap_info_u();
 						// Else output
 						} else {
@@ -115,7 +122,8 @@ void c_irq_handler(void)
 	return;
 }
 
-void c_fiq_handler(void)
+static unsigned long counter = 0;
+unsigned long c_fiq_handler(void)
 {
 	unsigned long source = load32(CORE0_FIQ_SOURCE);
 	if (source & (1 << 8)) {
@@ -199,7 +207,7 @@ void c_fiq_handler(void)
 				g_Drawer.y = 7;
 				write_string(&g_Drawer, "> ");
 				write_string(&g_Drawer, cmd);
-				return;
+				return 0;
 			}
 		} else if (*(unsigned long*)SYS_TIMER_CS == SYS_TIMER_SC_M0) {
 			volatile unsigned long* timer_cs = (unsigned long*)SYS_TIMER_CS;
@@ -211,18 +219,24 @@ void c_fiq_handler(void)
 		}
 	} else if (source & (1 << 3)) {
 		c_timer();
-		return;
+		if (counter++ >= 0x10) {
+			counter = 0;
+			//uart_scheduler();
+			return 1;
+		}
+		return 0;
 	}
-	return;
+	return 0;
 }
 
 void localtest(void)
 {
 	//struct Thread* t = scheduler.rthread_ll->data;
-	//uart_string("Running IRQ Task... ");
+	uart_string("Running IRQ Task...\n");
+	uart_scheduler();
 	//uart_10(t->data.pid);
 	//uart_char('\n');
-	//uart_string("Finished! ");
+	uart_string("Finished!\n");
 	//uart_10(t->data.pid);
 	//uart_char('\n');
 	//sched_info();
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;
 }