5 files changed, 20 insertions, 21 deletions

diff --git a/src/cpu/irq.S b/src/cpu/irq.S
index c434deb..fbb6ab7 100644
--- a/src/cpu/irq.S
+++ b/src/cpu/irq.S
@@ -32,13 +32,15 @@ a_irq_handler:
 	cmp r0, #0
 	beq a_irq_handler.etimer
 	// Disable
-	bl disable_cntv
+	mov r0, #0
+	mcr p15, 0, r0, c14, c3, 1
 	b a_irq_handler.ttimer
 a_irq_handler.etimer:
 	// Enable
 	ldr r0, =cntfrq
 	bl write_cntv_tval
-	bl enable_cntv
+	mov r0, #1
+	mcr p15, 0, r0, c14, c3, 1
 	// Otherwise continue
 a_irq_handler.ttimer:
 	pop {r0}
diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index 1654828..874e764 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -29,10 +29,10 @@ void c_irq_handler(void)
 					if(timer_status == 0) {
 						cntfrq = read_cntfrq();
 						write_cntv_tval(cntfrq);
-						enable_cntv();
+						enablecntv();
 						write_cstring(&g_Drawer, "Enabled ", 0x00FF00);
 					} else {
-						disable_cntv();
+						disablecntv();
 						write_cstring(&g_Drawer, "Disabled", 0xFF0000);
 					}
 					g_Drawer.x = x;
diff --git a/src/sys/core.c b/src/sys/core.c
index 8e54f49..f75ec35 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -39,7 +39,7 @@ void sysinit(void)
 	// Route timer to core0 irq
 	routing_core0cntv_to_core0irq();
 	// Enable timer
-	enable_cntv();
+	enablecntv();
 
 	// Graphics Initialize
 	lfb_init();
diff --git a/src/util/time.c b/src/util/time.c
index f6d8f04..c09567f 100644
--- a/src/util/time.c
+++ b/src/util/time.c
@@ -12,20 +12,6 @@ unsigned long read_core0timer_pending(void)
 	return tmp;
 }
 
-void enable_cntv(void)
-{
-	unsigned long cntv_ctl;
-	cntv_ctl = 1;
-	asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL
-}
-
-void disable_cntv(void)
-{
-	unsigned long cntv_ctl;
-	cntv_ctl = 0;
-	asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL
-}
-
 unsigned long long read_cntvct(void)
 {
 	unsigned long long val;
diff --git a/src/util/time.h b/src/util/time.h
index aa74896..13875e2 100644
--- a/src/util/time.h
+++ b/src/util/time.h
@@ -3,12 +3,23 @@
 
 void routing_core0cntv_to_core0irq(void);
 unsigned long read_core0timer_pending(void);
-void enable_cntv(void);
-void disable_cntv(void);
 unsigned long long read_cntvct(void);
 unsigned long long read_cntvoff(void);
 unsigned long read_cntv_tval(void);
 void write_cntv_tval(unsigned long val);
 unsigned long read_cntfrq(void);
 
+static inline void enablecntv(void) {
+	unsigned long cntv_ctl;
+	cntv_ctl = 1;
+	asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL
+}
+
+static inline void disablecntv(void)
+{
+	unsigned long cntv_ctl;
+	cntv_ctl = 0;
+	asm volatile ("mcr p15, 0, %0, c14, c3, 1" :: "r"(cntv_ctl) ); // write CNTV_CTL
+}
+
 #endif