From 7753da66d7d72f441dadd76f357fd5ceb50e7c8f Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Thu, 24 Mar 2022 18:52:56 -0700
Subject: Modularize Userspace

---
 kernel/cpu/irq.c        |  8 ++++++++
 kernel/exceptions/svc.S | 15 ++++++++++++++-
 kernel/sys/core.c       | 25 ++++---------------------
 3 files changed, 26 insertions(+), 22 deletions(-)

(limited to 'kernel')

diff --git a/kernel/cpu/irq.c b/kernel/cpu/irq.c
index 0bc8221..ae29379 100644
--- a/kernel/cpu/irq.c
+++ b/kernel/cpu/irq.c
@@ -136,6 +136,10 @@ void subscribe_irq(unsigned long irq_num, void* handler, void* handler_info)
 			store32(SYS_TIMER_SC_M3, IRQ_ENABLE1);
 			*(volatile unsigned long*)SYS_TIMER_C3 = *(volatile unsigned long*)SYS_TIMER_CHI + *(unsigned long*)handler_info;
 			break;
+		case LOCAL_TIMER_IRQ:
+			store32(0x80, CORE0_TIMER_IRQCNTL);
+			sys0(SYS_ENABLE_CNTV);
+			break;
 	}
 }
 
@@ -161,5 +165,9 @@ void unsubscribe_irq(unsigned long irq_num)
 		case SYS_TIMER_3_IRQ:
 			store32(SYS_TIMER_SC_M3, IRQ_DISABLE1);
 			break;
+		case LOCAL_TIMER_IRQ:
+			store32(0x00, CORE0_TIMER_IRQCNTL);
+			sys0(SYS_DISABLE_CNTV);
+			break;
 	}
 }
diff --git a/kernel/exceptions/svc.S b/kernel/exceptions/svc.S
index a24bac9..75a0cf1 100644
--- a/kernel/exceptions/svc.S
+++ b/kernel/exceptions/svc.S
@@ -11,7 +11,7 @@ svc:
 	adrle r3, svc_table_1
 	ldrle pc, [r3, r0, LSL #2]
 	sub r0, #8
-	cmp r0, #7
+	cmp r0, #3
 	bgt svc_exit
 	//// Jump to the appropriate Call
 	adr r3, svc_table_2
@@ -128,6 +128,17 @@ svc_000009: // SYS_TIME_2
 	mrc p15, 0, r0, c9, c13, 0
 	str r0, [sp, #0]
 	b svc_exit
+svc_00000A: // SYS_CNTV_ENABLE
+	ldr r0, =cntfrq
+	ldr r0, [r0]
+	mcr p15, 0, r0, c14, c3, 0
+	mov r0, #1
+	mcr p15, 0, r0, c14, c3, 1
+	b svc_exit
+svc_00000B: // SYS_CNTV_DISABLE
+	mov r0, #0
+	mcr p15, 0, r0, c14, c3, 1
+	b svc_exit
 svc_exit:
 	ldmfd sp!, {r0-r12,pc}^
 
@@ -143,3 +154,5 @@ svc_table_1:
 svc_table_2:
 	.word svc_000008
 	.word svc_000009
+	.word svc_00000A
+	.word svc_00000B
diff --git a/kernel/sys/core.c b/kernel/sys/core.c
index 7f42d6e..4812b00 100644
--- a/kernel/sys/core.c
+++ b/kernel/sys/core.c
@@ -18,32 +18,15 @@
 // Initialize IRQs
 void sysinit(void)
 {
-	// Initialize System Globals
-	//stimeh = *(unsigned long*)SYS_TIMER_CHI;
-	//stimel = *(unsigned long*)SYS_TIMER_CLO;
-	//*(unsigned long*) SYS_TIMER_C0 = 2000000 + stimeh; // 2 second trigger
+	// Get the frequency
+	cntfrq = read_cntfrq();
+
+	// Initialize UART
 	uart_init();
-	///...
 
 	// Route GPU interrupts to Core 0
 	store32(0x00, GPU_INTERRUPTS_ROUTING);
 
-	//// Mask Overrun of UART0
-	//store32(1<<4, UART0_IMSC);
-	//// 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);
-	//// Route timer to core0 fiq
-	//routing_core0cntv_to_core0fiq();
-	//// Enable timer
-	//enablecntv();
-	//// Enable system timer
-	//store32(SYS_TIMER_SC_M0, IRQ_ENABLE1);
-
 	// Graphics Initialize
 	lfb_init();
 	lfb_showpicture();
-- 
cgit v1.2.1