Working on time

5 files changed, 83 insertions, 61 deletions

diff --git a/include/cpu.h b/include/cpu.h
index 202c6f5..43eeed3 100644
--- a/include/cpu.h
+++ b/include/cpu.h
@@ -80,6 +80,7 @@ static inline void* getirqstack(void)
 #define syscall_h_expand_and_quote(text)	syscall_h_quote(text)
 
 #define sys0(sys_n) asm volatile("svc #" syscall_h_expand_and_quote(sys_n) ::: "r0", "r1", "r2", "r3");
+#define sys0_32(sys_n,addr) asm volatile("svc #" syscall_h_expand_and_quote(sys_n) "\nmov r2, %0\nstr r0, [r2]" ::"r"(addr): "r0", "r1", "r2", "r3", "memory");
 #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) {\
@@ -87,14 +88,16 @@ static inline void* getirqstack(void)
 		asm volatile("svc #" syscall_h_expand_and_quote(sys_n) ::"r"(r0): "memory"); \
 		}
 
-#define SYS_YIELD       0
-#define SYS_TIME        1
-#define SYS_SCHED       2
-#define SYS_YIELD_HIGH  2
-#define SYS_ADD_THREAD  3
-#define SYS_LOCK        4
-#define SYS_UNLOCK      5
-#define SYS_SEMAPHORE_P 6
-#define SYS_SEMAPHORE_V 7
+#define SYS_YIELD        0
+#define SYS_TIME         1
+#define SYS_SCHED        2
+#define SYS_YIELD_HIGH   2
+#define SYS_ADD_THREAD   3
+#define SYS_LOCK         4
+#define SYS_UNLOCK       5
+#define SYS_SEMAPHORE_P  6
+#define SYS_SEMAPHORE_V  7
+#define SYS_SEMAPHORE_IV 8
+#define SYS_TIME_2       9
 
 #endif
diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index e3353e8..3bd28e6 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -80,20 +80,9 @@ unsigned long c_irq_handler(void)
 unsigned long c_fiq_handler(void)
 {
 	unsigned long source = load32(CORE0_FIQ_SOURCE);
-	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;
-		}
-	}
 	// Check if CNTV triggered the interrupt
-	else if (source & (1 << 3)) {
+	if (source & (1 << 3)) {
+		write_cntv_tval(cntfrq);
 	}
 	return 0;
 }
diff --git a/src/exceptions/svc.S b/src/exceptions/svc.S
index fdfe391..f833b5b 100644
--- a/src/exceptions/svc.S
+++ b/src/exceptions/svc.S
@@ -6,7 +6,7 @@ svc:
 	// Get the SVC Exception #
 	ldr r0, [lr, #-4]
 	bic r0, #0xFF000000
-	cmp r0, #7
+	cmp r0, #9
 	// Check it is within our defined SVC
 	bgt svc_exit
 	//// Jump to the appropriate Call
@@ -120,6 +120,10 @@ svc_000008: // Semaphore add #
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
 	b svc_exit
+svc_000009: // SYS_TIME_2
+	mrc p15, 0, r0, c14, c3, 0
+	str r0, [sp, #0]
+	b svc_exit
 svc_exit:
 	ldmfd sp!, {r0-r12,pc}^
 
@@ -133,3 +137,4 @@ svc_table:
 	.word svc_000006
 	.word svc_000007
 	.word svc_000008
+	.word svc_000009
diff --git a/src/sys/core.c b/src/sys/core.c
index ef8d4b7..d76b712 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -33,13 +33,13 @@ void sysinit(void)
 	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();
+	cntfrq = read_cntfrq();
+	// Clear cntv interrupt and set next 1 second timer
+	write_cntv_tval(cntfrq);
+	// 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/tests/test.c b/src/tests/test.c
index 1aaa4a9..1c44a82 100644
--- a/src/tests/test.c
+++ b/src/tests/test.c
@@ -41,9 +41,34 @@ void test_entry(void)
 
 	// Test 1: Tracing Time
 	dt = 0;
+	ti = 0;
+	tf = 0;
+	int t1, t2;
 	for(int i = 0; i < TEST_COUNT; i++) {
-		sys0_64(SYS_TIME, &ti);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &tf);
+		sys0_32(SYS_TIME_2, &ti);
+		dt += tf-ti;
+		if ((tf-ti) < TEST_BIN_COUNT) {
+			bins[(tf-ti)]++;
+		}
+	}
+	for (int i = 0; i < TEST_BIN_COUNT; i++) {
+		draw_hex32(tidx, y+6+i, i);
+		draw_string(tidx+9, y+6+i, TEST_STR_CLR);
+		draw_u10(tidx+9, y+6+i, bins[i]);
+		bins[i] = 0;
+	}
+	draw_string(tidx, y+5, "       ");
+	len = draw_u10(tidx, y+5, dt/TEST_COUNT);
+	draw_u10(tidx+len+1, y+5, dt%TEST_COUNT);
+	tidx += TEST_RESULT_WIDTH;
+	draw_hex32(0, y-1, nextpid);
+
+	// Test 1: Tracing Time
+	dt = 0;
+	for(int i = 0; i < TEST_COUNT; i++) {
+		sys0_32(SYS_TIME_2, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
 			bins[(tf-ti)]++;
@@ -63,9 +88,9 @@ void test_entry(void)
 	// Test 2: Yield Time
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		sys0(SYS_YIELD);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
 			bins[(tf-ti)]++;
@@ -85,9 +110,9 @@ void test_entry(void)
 	// Test 3: Add Thread, Lower Priority
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		add_thread(nooptest, 0, 3);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
 			bins[(tf-ti)]++;
@@ -107,9 +132,9 @@ void test_entry(void)
 	// Test 4: Add Thread, Higher Priority
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		add_thread(nooptest, 0, 0);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
 			bins[(tf-ti)]++;
@@ -129,9 +154,9 @@ void test_entry(void)
 	// Test 5: Create Mutex
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		struct Mutex* m = create_mutex(0);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		delete_mutex(m);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
@@ -153,9 +178,9 @@ void test_entry(void)
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
 		struct Mutex* m = create_mutex(0);
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		delete_mutex(m);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
 			bins[(tf-ti)]++;
@@ -176,9 +201,9 @@ void test_entry(void)
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
 		struct Mutex* m = create_mutex(0);
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		lock_mutex(m);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		delete_mutex(m);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
@@ -202,9 +227,9 @@ void test_entry(void)
 		struct Mutex* m = create_mutex(0);
 		add_thread(mutex_contention_helper, m, 2);
 		sys0(SYS_YIELD);
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		lock_mutex(m);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		delete_mutex(m);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
@@ -227,9 +252,9 @@ void test_entry(void)
 	for(int i = 0; i < TEST_COUNT; i++) {
 		struct Mutex* m = create_mutex(0);
 		lock_mutex(m);
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		unlock_mutex(m);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		delete_mutex(m);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
@@ -254,9 +279,9 @@ void test_entry(void)
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
 		sem = 1;
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		sys1(SYS_SEMAPHORE_P, &sem);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
 			bins[(tf-ti)]++;
@@ -277,9 +302,9 @@ void test_entry(void)
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
 		sem = 0;
-		sys0_64(SYS_TIME, &ti);
+		sys0_32(SYS_TIME_2, &tf);
 		sys1(SYS_SEMAPHORE_V, &sem);
-		sys0_64(SYS_TIME, &tf);
+		sys0_32(SYS_TIME_2, &ti);
 		dt += tf - ti;
 		if ((tf-ti) < TEST_BIN_COUNT)
 			bins[(tf-ti)]++;
@@ -300,12 +325,12 @@ void test_entry(void)
 	dt = 0;
 	for(int i = 0; i < TEST_COUNT; i++) {
 		sem = 1;
-		sys0_64(SYS_TIME, &ti);
+		//sys0_32(SYS_TIME_2, &tf);
 		sys1(SYS_SEMAPHORE_V, &sem);
-		sys0_64(SYS_TIME, &tf);
-		dt += tf - ti;
-		if ((tf-ti) < TEST_BIN_COUNT)
-			bins[(tf-ti)]++;
+		//sys0_32(SYS_TIME_2, &ti);
+		//dt += tf - ti;
+		//if ((tf-ti) < TEST_BIN_COUNT)
+		//	bins[(tf-ti)]++;
 	}
 	for (int i = 0; i < TEST_BIN_COUNT; i++) {
 		draw_hex32(tidx, y+6+i, i);
@@ -322,18 +347,18 @@ void test_entry(void)
 //	// Test 7: Tick Latency
 //#define DELAY_TIME 512000
 //	unsigned long center = 0;
-//	sys0_64(SYS_TIME, &ti);
+//	sys0_32(SYS_TIME_2, &tf);
 //	delay(DELAY_TIME);
-//	sys0_64(SYS_TIME, &tf);
+//	sys0_32(SYS_TIME_2, &ti);
 //	center = (tf - ti - 10);
 //	if (10 > (tf-ti))
 //		center = 0;
 //	dt = 0;
 //	unsigned long j = 0;
 //	for(int i = 0; i < TEST_COUNT; i++) {
-//		sys0_64(SYS_TIME, &ti);
+//		sys0_32(SYS_TIME_2, &tf);
 //		delay(DELAY_TIME);
-//		sys0_64(SYS_TIME, &tf);
+//		sys0_32(SYS_TIME_2, &ti);
 //		dt += tf - ti;
 //		if ((tf-ti-center) < TEST_BIN_COUNT)
 //			bins[(tf-ti)-center]++;