diff --git a/Makefile b/Makefile
index 56e088f..99331d5 100644
--- a/Makefile
+++ b/Makefile
@@ -31,7 +31,7 @@ BIN = $(CP) -O binary
 
 CFLAGS += -mabi=ilp32 -march=rv32imc -g $(INCLUDES)
 ASFLAGS += -mabi=ilp32 -march=rv32imc -g
-LDFLAGS += -nostartfiles -specs=nano.specs -lc -lm -lnosys
+LDFLAGS += -nostartfiles -specs=nosys.specs -lc -lm -lnosys
 
 
 LDFLAGS += -T$(LDSCRIPT) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map -Wl,--print-memory-usage
diff --git a/include/ibex_system.h b/include/ibex_system.h
index d3146f2..9d252f7 100644
--- a/include/ibex_system.h
+++ b/include/ibex_system.h
@@ -1,7 +1,19 @@
 #ifndef __IBEX_SYSTEM_H
 #define __IBEX_SYSTEM_H
 
+#include <stdint.h>
+
+#define TIMER_IRQn 7
+
+#define __csrr(csr, val) asm volatile("csrr %0, " csr "\n" : "=r"(val))
+#define __csrw(csr, val) asm volatile("csrw " csr ", %0\n" : : "r"(val))
+#define __csrs(csr, val) asm volatile("csrs " csr ", %0\n" : : "r"(val))
+#define __csrc(csr, val) asm volatile("csrc " csr ", %0\n" : : "r"(val))
+
 void __enable_irqs(void);
 void __disable_irqs(void);
+void __enable_irqn(int irq_number);
+void __enable_mcount(void);
+uint64_t __read_mcycle(void);
 
 #endif
\ No newline at end of file
diff --git a/include/soc_peripherals.h b/include/soc_peripherals.h
index ae6daa6..0d9e9f3 100644
--- a/include/soc_peripherals.h
+++ b/include/soc_peripherals.h
@@ -5,6 +5,8 @@
 #include "xilinx_gpio.h"
 #include "xilinx_timer.h"
 
+#include "ibex_system.h"
+
 // Peripheral defines
 #define UART0_BASE  0x40010000
 #define GPIO0_BASE  0x40000000
diff --git a/src/ibex_system.c b/src/ibex_system.c
index c984df0..86014b3 100644
--- a/src/ibex_system.c
+++ b/src/ibex_system.c
@@ -1,9 +1,37 @@
 #include "ibex_system.h"
 
-void __attribute__((naked)) __enable_irqs(void) {
-    asm volatile("csrs mstatus, %0\n" : : "r"(0x8));
+void __enable_irqs(void) {
+    __csrs("mstatus", 0x08);
 }
 
-void __attribute__((naked)) __disable_irqs(void) {
-    asm volatile("csrc mstatus, %0\n" : : "r"(0x08));
+void __disable_irqs(void) {
+    __csrc("mstatus", 0x08);
+}
+
+void __enable_irqn(int irq_number) {
+    __csrs("mie", (1U << irq_number));
+}
+
+void __enable_mcount(void) {
+    // CY bit in mcountinhibit
+    // RISC-V GCC has not implement this CSR name for now.
+    __csrc("0x320", 0x01);
+}
+
+void __disable_mcount(void) {
+    // Same as above.
+    __csrs("0x320", 0x01);
+}
+
+uint64_t __read_mcycle(void) {
+    uint32_t mcycleh = 0;
+    uint32_t mcycle = 0;
+    __csrr("mcycle", mcycle);
+    __csrr("mcycleh", mcycleh);
+
+    uint64_t ret = mcycleh << 31;
+    ret <<= 1;
+    ret |= mcycle;
+
+    return ret;
 }
\ No newline at end of file
diff --git a/src/main.c b/src/main.c
index 50e9fe7..4ddad8b 100644
--- a/src/main.c
+++ b/src/main.c
@@ -41,6 +41,7 @@ int main(int argc, char *argv[]) {
             previous_tick = GetTick();
             xilinx_gpio_toggle(&soc_gpio, 1);
             printf("SysTick: %lu\r\n", GetTick());
+            printf("MCycle: %llu\r\n", __read_mcycle());
         }
     }
     return 0;
diff --git a/src/system_init.c b/src/system_init.c
index 95c1382..36fa8d5 100644
--- a/src/system_init.c
+++ b/src/system_init.c
@@ -14,8 +14,10 @@ void System_Init(void) {
     SysTick = 0;
 
     // Enable interrupts
-    asm volatile("csrs mie, %0\n" : : "r"(0x80)); // Enable timer interrupt
-    asm volatile("csrs mstatus, %0\n" : : "r"(0x8)); // Enable global interrupt
+    __enable_irqn(TIMER_IRQn); // Enable timer interrupt
+    __enable_irqs();
+
+    __enable_mcount();
 
     // Setup systick timer
     TIMER0->TCSR0 = 0x12; // Generate mode, down count, auto-reload