6 files changed, 77 insertions, 24 deletions

diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index 4142b28..d4c157f 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -14,7 +14,8 @@
 
 void handle_data(unsigned char);
 
-void c_irq_handler(void)
+static unsigned long counter = 0;
+unsigned long c_irq_handler(void)
 {
 	unsigned long source = load32(CORE0_IRQ_SOURCE);
 	// Check if GPU Interrupt
@@ -51,6 +52,7 @@ void c_irq_handler(void)
 				// Add task to handle the data
 				else {
 					add_thread(handle_data, (void*)data, 7);
+					return 1;
 				}
 			}
 		}
@@ -60,30 +62,40 @@ void c_irq_handler(void)
 			volatile unsigned long* timer_chi = (volatile unsigned long*)SYS_TIMER_CHI;
 			volatile unsigned long* nexttime = (volatile unsigned long*)SYS_TIMER_C0;
 			add_thread_without_duplicate(test_entry, 0, 2);
-			*nexttime = *timer_chi + 8000000;
+			*nexttime = *timer_chi + 4000000;
 			*timer_cs = SYS_TIMER_SC_M0;
+			status();
+			return 1;
 		}
 	}
 	// Check if CNTV triggered the interrupt
 	else if (source & (1 << 3)) {
+		// Reset the counter
+		write_cntv_tval(cntfrq/CPS);
+		counter++;
+		if (counter % 0x6000 == 0)
+			counter = 0;
 	}
-	return;
+	return 0;
 }
 
-static unsigned long counter = 0;
 unsigned long c_fiq_handler(void)
 {
 	unsigned long source = load32(CORE0_FIQ_SOURCE);
-	// Check if CNTV triggered the interrupt
-	if (source & (1 << 3)) {
-		// Reset the counter
-		write_cntv_tval(cntfrq/CPS);
-		counter++;
-		if (counter % 0x6000 == 0)
-			counter = 0;
-		if (counter % 0x40 == 0)
+	if (source & (1 << 8)) {
+		// Check if System Time Compare 0 Triggered the Interrupt
+		if (*(volatile unsigned long*)SYS_TIMER_CS & SYS_TIMER_SC_M0) {
+			volatile unsigned long* timer_cs = (volatile unsigned long*)SYS_TIMER_CS;
+			volatile unsigned long* timer_chi = (volatile unsigned long*)SYS_TIMER_CHI;
+			volatile unsigned long* nexttime = (volatile unsigned long*)SYS_TIMER_C0;
+			add_thread_without_duplicate(test_entry, 0, 2);
+			*nexttime = *timer_chi + 4000000;
+			*timer_cs = SYS_TIMER_SC_M0;
 			return 1;
-		return 0;
+		}
+	}
+	// Check if CNTV triggered the interrupt
+	else if (source & (1 << 3)) {
 	}
 	return 0;
 }
diff --git a/src/exceptions/irq.S b/src/exceptions/irq.S
index 2c1a6e8..a7e78bc 100644
--- a/src/exceptions/irq.S
+++ b/src/exceptions/irq.S
@@ -2,8 +2,27 @@
 .globl irq
 irq:
 	cpsid ai
-	push {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr}
+	stmfd sp!, {r0-r12,lr}
 	// Run IRQ handler
 	bl c_irq_handler
-	pop  {r0,r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12,lr}
+	cmp r0, #1
+	bne 1f
+	// Schedule if interrupted a thread
+	mrs r1, spsr
+	and r1, r1, #0x1f
+	cmp r1, #0x10
+	bne 1f
+	ldmfd sp!, {r0-r12,lr}
+	// Don't skip missed instruction upon return
+	sub lr, #4
+	push {r3}
+	// Store the instruction in a special area for
+	//  future processing
+	ldr r3, =irqlr
+	str lr, [r3, #0]
+	pop {r3}
+	cps #0x13
+	b schedule
+1:
+	ldmfd sp!, {r0-r12,lr}
 	subs pc, lr, #4
diff --git a/src/sys/core.c b/src/sys/core.c
index 9099a82..ef8d4b7 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -32,14 +32,14 @@ void sysinit(void)
 	// Enable UART GPU IRQ
 	store32(1<<25, IRQ_ENABLE2);
 	// Enable Timer
-	// Get the frequency
-	cntfrq = read_cntfrq();
-	// Clear cntv interrupt and set next 1 second timer
-	write_cntv_tval(cntfrq/100);
-	// Route timer to core0 fiq
-	routing_core0cntv_to_core0fiq();
-	// Enable timer
-	enablecntv();
+	//// Get the frequency
+	//cntfrq = read_cntfrq();
+	//// Clear cntv interrupt and set next 1 second timer
+	//write_cntv_tval(cntfrq/100);
+	//// Route timer to core0 fiq
+	//routing_core0cntv_to_core0fiq();
+	//// Enable timer
+	//enablecntv();
 	// Enable system timer
 	store32(SYS_TIMER_SC_M0, IRQ_ENABLE1);
 
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index 9184c76..8793d84 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -14,7 +14,7 @@ kernel_main:
 
 	// Intentional undefined instruction
 	// .word 0xf7f0a000
-	cpsie aif, #0x10
+	cpsie ai, #0x10
 	svc #2 // Start scheduling!
 2:
 	wfe
diff --git a/src/tests/test.S b/src/tests/test.S
index 035f8f6..e80b6be 100644
--- a/src/tests/test.S
+++ b/src/tests/test.S
@@ -2,9 +2,30 @@
 
 a.btest:
 	push {lr}
+	mov r0, #5
+	cmp r0, #4
 	pop {pc}
 
 .globl atest
 atest:
 	push {lr}
+	ldr r0, =a.btest
+	mov r1, #0
+	mov r2, #0
+	bl add_thread
+	mov r0, #5
+	subs r0, #5
+	svc #0
+	beq 1f
+	mov r0, #0
+	mov r1, #11
+	mov r2, #0x4E
+	mov r3, #0xFF0000
+	bl draw_cletter
+	pop {pc}
+1:	mov r0, #0
+	mov r1, #11
+	mov r2, #0x59
+	mov r3, #0xFF00
+	bl draw_cletter
 	pop {pc}
diff --git a/src/tests/test.c b/src/tests/test.c
index 0d72e0a..be37048 100644
--- a/src/tests/test.c
+++ b/src/tests/test.c
@@ -505,6 +505,7 @@ void qualitative_tests(void)
 	draw_string(0, y+2, "                   ");
 	draw_string(0, y+3, "                   ");
 	x = 0;
+	add_thread(atest, 0, 0);
 	add_thread(priority_inversion_test1, 0, 3);
 	add_thread(deadlock_test1, 0, 5);
 	add_thread(semaphore_test1, 0, 6);