diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 55c8d31ff3cdf8da145942d7e0dff413cdba9df7..54543f22db2b34dfdc7c6faaa8404900719bf70f 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -140,7 +140,7 @@ jobs:
         run: rustup show
       - uses: Swatinem/rust-cache@v2
       - name: Build dev profile
-        run: cargo build -Zbuild-std=std,panic_abort --target x86_64-unknown-hermit
+        run: cargo build -Zbuild-std=std,panic_abort --package rusty_demo --target x86_64-unknown-hermit
       - name: Download loader
         uses: dsaltares/fetch-gh-release-asset@1.1.1
         with:
@@ -160,7 +160,7 @@ jobs:
             -object memory-backend-file,id=mem,size=1G,mem-path=/dev/shm,share=on -numa node,memdev=mem \
             -initrd target/x86_64-unknown-hermit/debug/rusty_demo
       - name: Build release profile
-        run: cargo build -Zbuild-std=std,panic_abort --target x86_64-unknown-hermit --release
+        run: cargo build -Zbuild-std=std,panic_abort --package rusty_demo --target x86_64-unknown-hermit --release
       - name: Test release profile
         run: |
           virtiofsd --socket-path=./vhostqemu --shared-dir ./img --announce-submounts --sandbox none --seccomp none --inode-file-handles=never &
@@ -219,18 +219,18 @@ jobs:
           sudo apt-get install qemu-system-aarch64
       - uses: Swatinem/rust-cache@v2
       - name: Build dev profile
-        run: cargo build -Zbuild-std=std,panic_abort --target aarch64-unknown-hermit --package hello_world
+        run: cargo build -Zbuild-std=std,panic_abort --target aarch64-unknown-hermit --package rusty_demo
       - name: Test dev kernel
         run: |
           qemu-system-aarch64 -semihosting \
             -kernel rusty-loader-aarch64 -machine virt,gic-version=max \
             -m 512M -cpu max -smp 1 -display none -serial stdio -kernel rusty-loader-aarch64 \
-            -device guest-loader,addr=0x48000000,initrd=target/aarch64-unknown-hermit/debug/hello_world
+            -device guest-loader,addr=0x48000000,initrd=target/aarch64-unknown-hermit/debug/rusty_demo
       - name: Build release profile
-        run: cargo build -Zbuild-std=std,panic_abort --target aarch64-unknown-hermit --package hello_world --release
+        run: cargo build -Zbuild-std=std,panic_abort --target aarch64-unknown-hermit --package rusty_demo --release
       - name: Test release kernel
         run: |
           qemu-system-aarch64 -semihosting \
             -kernel rusty-loader-aarch64 -machine virt,gic-version=max \
             -m 512M -cpu max -smp 1 -display none -serial stdio -kernel rusty-loader-aarch64 \
-            -device guest-loader,addr=0x48000000,initrd=target/aarch64-unknown-hermit/release/hello_world
+            -device guest-loader,addr=0x48000000,initrd=target/aarch64-unknown-hermit/release/rusty_demo
diff --git a/Cargo.lock b/Cargo.lock
index 45ede66d216e1536b07aa36f162b4a016551c1fc..42077b7c5c1f7c63c0f4f6c5901ff2b1e041c645 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -44,6 +44,15 @@ version = "1.0.71"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9c7d0618f0e0b7e8ff11427422b64564d5fb0be1940354bfe2e0529b18a9d9b8"
 
+[[package]]
+name = "arm-gic"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d16805d71f41d1714a51ee34f40633c42980a2afaf8ae237bcba09d6e15494ef"
+dependencies = [
+ "bitflags 2.2.1",
+]
+
 [[package]]
 name = "async-task"
 version = "4.4.0"
@@ -345,6 +354,7 @@ dependencies = [
  "aarch64",
  "ahash",
  "align-address",
+ "arm-gic",
  "async-task",
  "bitflags 2.2.1",
  "crossbeam-utils",
diff --git a/Cargo.toml b/Cargo.toml
index 9872574c59847442a0c81cbf28762068cc46deba..ee0cb4f330d01db4d4d6005303872aab45563330 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -120,6 +120,7 @@ x86_64 = "0.14"
 hermit-dtb = { version = "0.1" }
 aarch64 = { version = "0.0.10", default-features = false }
 tock-registers = { version = "0.8.x", default-features = false }
+arm-gic = { version = "0.1" }
 
 [dev-dependencies]
 float-cmp = "0.9"
diff --git a/src/arch/aarch64/kernel/interrupts.rs b/src/arch/aarch64/kernel/interrupts.rs
index 48794923cb727d5587eb623dbb866908247dffb7..2829fa536f1defa07d3c5148dc837b0456dade86 100644
--- a/src/arch/aarch64/kernel/interrupts.rs
+++ b/src/arch/aarch64/kernel/interrupts.rs
@@ -1,8 +1,11 @@
+use alloc::vec::Vec;
 use core::arch::asm;
+use core::ptr;
 
 use aarch64::regs::*;
+use arm_gic::gicv3::{GicV3, IntId, SgiTarget};
 use hermit_dtb::Dtb;
-use hermit_sync::{InterruptTicketMutex, OnceCell};
+use hermit_sync::{without_interrupts, InterruptTicketMutex, OnceCell};
 use tock_registers::interfaces::Readable;
 
 use crate::arch::aarch64::kernel::boot_info;
@@ -13,64 +16,32 @@ use crate::arch::aarch64::mm::paging::{
 use crate::arch::aarch64::mm::{virtualmem, PhysAddr, VirtAddr};
 use crate::errno::EFAULT;
 use crate::scheduler::CoreId;
-use crate::sys_exit;
+use crate::{core_scheduler, sys_exit};
 
 pub const IST_SIZE: usize = 8 * BasePageSize::SIZE as usize;
 
-/*
- * GIC Distributor interface register offsets that are common to GICv3 & GICv2
- */
-
-const GICD_CTLR: usize = 0x0;
-const GICD_TYPER: usize = 0x4;
-const GICD_IIDR: usize = 0x8;
-const GICD_IGROUPR: usize = 0x80;
-const GICD_ISENABLER: usize = 0x100;
-const GICD_ICENABLER: usize = 0x180;
-const GICD_ISPENDR: usize = 0x200;
-const GICD_ICPENDR: usize = 0x280;
-const GICD_ISACTIVER: usize = 0x300;
-const GICD_ICACTIVER: usize = 0x380;
-const GICD_IPRIORITYR: usize = 0x400;
-const GICD_ITARGETSR: usize = 0x800;
-const GICD_ICFGR: usize = 0xC00;
-const GICD_NSACR: usize = 0xE00;
-const GICD_SGIR: usize = 0xF00;
-
-const GICD_CTLR_ENABLEGRP0: u32 = 1 << 0;
-const GICD_CTLR_ENABLEGRP1: u32 = 1 << 1;
-
-/* Physical CPU Interface registers */
-const GICC_CTLR: usize = 0x0;
-const GICC_PMR: usize = 0x4;
-const GICC_BPR: usize = 0x8;
-const GICC_IAR: usize = 0xC;
-const GICC_EOIR: usize = 0x10;
-const GICC_RPR: usize = 0x14;
-const GICC_HPPIR: usize = 0x18;
-const GICC_AHPPIR: usize = 0x28;
-const GICC_IIDR: usize = 0xFC;
-const GICC_DIR: usize = 0x1000;
-const GICC_PRIODROP: usize = GICC_EOIR;
-
-const GICC_CTLR_ENABLEGRP0: u32 = 1 << 0;
-const GICC_CTLR_ENABLEGRP1: u32 = 1 << 1;
-const GICC_CTLR_FIQEN: u32 = 1 << 3;
-const GICC_CTLR_ACKCTL: u32 = 1 << 2;
-
 /// maximum number of interrupt handlers
 const MAX_HANDLERS: usize = 256;
 
-static GICC_ADDRESS: OnceCell<VirtAddr> = OnceCell::new();
-static GICD_ADDRESS: OnceCell<VirtAddr> = OnceCell::new();
+/// Number of the timer interrupt
+static mut TIMER_INTERRUPT: u32 = 0;
+/// Possible interrupt handlers
+static mut INTERRUPT_HANDLERS: [Option<fn(state: &State)>; MAX_HANDLERS] = [None; MAX_HANDLERS];
+/// Driver for the Arm Generic Interrupt Controller version 3 (or 4).
+static mut GIC: OnceCell<GicV3> = OnceCell::new();
 
-/// Number of used supported interrupts
-static NR_IRQS: OnceCell<u32> = OnceCell::new();
-static mut INTERRUPT_HANDLERS: [fn(state: &State); MAX_HANDLERS] =
-	[default_interrupt_handler; MAX_HANDLERS];
+fn timer_handler(_state: &State) {
+	debug!("Handle timer interrupt");
 
-fn default_interrupt_handler(_state: &State) {
-	warn!("Entering default interrupt handler");
+	// disable timer
+	unsafe {
+		asm!(
+			"msr cntp_cval_el0, {disable}",
+			"msr cntp_ctl_el0, {disable}",
+			disable = in(reg) 0,
+			options(nostack, nomem),
+		);
+	}
 }
 
 /// Enable all interrupts
@@ -113,43 +84,67 @@ pub fn disable() {
 }
 
 pub fn irq_install_handler(irq_number: u32, handler: fn(state: &State)) {
-	info!("Install handler for interrupt {}", irq_number);
+	debug!("Install handler for interrupt {}", irq_number);
 	unsafe {
-		INTERRUPT_HANDLERS[irq_number as usize] = handler;
+		INTERRUPT_HANDLERS[irq_number as usize] = Some(handler);
 	}
 }
 
 #[no_mangle]
 pub extern "C" fn do_fiq(state: &State) {
-	info!("fiq");
-	let iar = gicc_read(GICC_IAR);
-	let vector: usize = iar as usize & 0x3ff;
+	if let Some(irqid) = GicV3::get_and_acknowledge_interrupt() {
+		let vector: usize = u32::from(irqid).try_into().unwrap();
 
-	info!("Receive fiq {}", vector);
+		debug!("Receive fiq {}", vector);
 
-	if vector < MAX_HANDLERS {
-		unsafe {
-			INTERRUPT_HANDLERS[vector](state);
+		if vector < MAX_HANDLERS {
+			unsafe {
+				if let Some(handler) = INTERRUPT_HANDLERS[vector] {
+					handler(state);
+				}
+			}
 		}
-	}
 
-	gicc_write(GICC_EOIR, iar.try_into().unwrap());
+		core_scheduler().handle_waiting_tasks();
+
+		GicV3::end_interrupt(irqid);
+
+		if unsafe { vector == TIMER_INTERRUPT.try_into().unwrap() } {
+			// a timer interrupt may have caused unblocking of tasks
+			core_scheduler().scheduler();
+		}
+	}
 }
 
 #[no_mangle]
-pub extern "C" fn do_irq(_state: &State) {
-	let iar = gicc_read(GICC_IAR);
-	let vector = iar & 0x3ff;
+pub extern "C" fn do_irq(state: &State) {
+	if let Some(irqid) = GicV3::get_and_acknowledge_interrupt() {
+		let vector: usize = u32::from(irqid).try_into().unwrap();
+
+		debug!("Receive interrupt {}", vector);
+
+		if vector < MAX_HANDLERS {
+			unsafe {
+				if let Some(handler) = INTERRUPT_HANDLERS[vector] {
+					handler(state);
+				}
+			}
+		}
+
+		core_scheduler().handle_waiting_tasks();
 
-	info!("Receive interrupt {}", vector);
+		GicV3::end_interrupt(irqid);
 
-	gicc_write(GICC_EOIR, iar);
+		if unsafe { vector == TIMER_INTERRUPT.try_into().unwrap() } {
+			// a timer interrupt may have caused unblocking of tasks
+			core_scheduler().scheduler();
+		}
+	}
 }
 
 #[no_mangle]
-pub extern "C" fn do_sync(state: &State) {
-	info!("{:#012x?}", state);
-	let iar = gicc_read(GICC_IAR);
+pub extern "C" fn do_sync(_state: &State) {
+	let irqid = GicV3::get_and_acknowledge_interrupt().unwrap();
 	let esr = ESR_EL1.get();
 	let ec = esr >> 26;
 	let iss = esr & 0xFFFFFF;
@@ -170,8 +165,7 @@ pub extern "C" fn do_sync(state: &State) {
 			error!("Table Base Register {:#x}", TTBR0_EL1.get());
 			error!("Exception Syndrome Register {:#x}", esr);
 
-			// send EOI
-			gicc_write(GICC_EOIR, iar);
+			GicV3::end_interrupt(irqid);
 			sys_exit(-EFAULT);
 		} else {
 			error!("Unknown exception");
@@ -185,133 +179,19 @@ pub extern "C" fn do_sync(state: &State) {
 
 #[no_mangle]
 pub extern "C" fn do_bad_mode(_state: &State, reason: u32) -> ! {
-	error!("Receive unhandled exception: {}\n", reason);
+	error!("Receive unhandled exception: {}", reason);
 
 	sys_exit(-EFAULT);
 }
 
 #[no_mangle]
 pub extern "C" fn do_error(_state: &State) -> ! {
-	error!("Receive error interrupt\n");
+	error!("Receive error interrupt");
 
 	sys_exit(-EFAULT);
 }
 
-#[inline]
-fn gicd_read(off: usize) -> u32 {
-	let value: u32;
-
-	// we have to use inline assembly to guarantee 32bit memory access
-	unsafe {
-		asm!("ldar {value:w}, [{addr}]",
-			value = out(reg) value,
-			addr = in(reg) (GICD_ADDRESS.get().unwrap().as_usize() + off),
-			options(nostack, readonly),
-		);
-	}
-
-	value
-}
-
-#[inline]
-fn gicd_write(off: usize, value: u32) {
-	// we have to use inline assembly to guarantee 32bit memory access
-	unsafe {
-		asm!("str {value:w}, [{addr}]",
-			value = in(reg) value,
-			addr = in(reg) (GICD_ADDRESS.get().unwrap().as_usize() + off),
-			options(nostack),
-		);
-	}
-}
-
-#[inline]
-fn gicc_read(off: usize) -> u32 {
-	let value: u32;
-
-	// we have to use inline assembly to guarantee 32bit memory access
-	unsafe {
-		asm!("ldar {value:w}, [{addr}]",
-			value = out(reg) value,
-			addr = in(reg) (GICC_ADDRESS.get().unwrap().as_usize() + off),
-			options(nostack, readonly),
-		);
-	}
-
-	value
-}
-
-#[inline]
-fn gicc_write(off: usize, value: u32) {
-	// we have to use inline assembly to guarantee 32bit memory access
-	unsafe {
-		asm!("str {value:w}, [{addr}]",
-			value = in(reg) value,
-			addr = in(reg) (GICC_ADDRESS.get().unwrap().as_usize() + off),
-			options(nostack),
-		);
-	}
-}
-
-/// Global enable forwarding interrupts from distributor to cpu interface
-fn gicd_enable() {
-	gicd_write(GICD_CTLR, GICD_CTLR_ENABLEGRP0 | GICD_CTLR_ENABLEGRP1);
-}
-
-/// Global disable forwarding interrupts from distributor to cpu interface
-fn gicd_disable() {
-	gicd_write(GICD_CTLR, 0);
-}
-
-/// Global enable signalling of interrupt from the cpu interface
-fn gicc_enable() {
-	gicc_write(
-		GICC_CTLR,
-		GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 | GICC_CTLR_FIQEN | GICC_CTLR_ACKCTL,
-	);
-}
-
-/// Global disable signalling of interrupt from the cpu interface
-fn gicc_disable() {
-	gicc_write(GICC_CTLR, 0);
-}
-
-fn gicc_set_priority(priority: u32) {
-	gicc_write(GICC_PMR, priority & 0xFF);
-}
-
-static MASK_LOCK: InterruptTicketMutex<()> = InterruptTicketMutex::new(());
-
-pub fn mask_interrupt(vector: u32) -> Result<(), ()> {
-	if vector < *NR_IRQS.get().unwrap() && vector < MAX_HANDLERS.try_into().unwrap() {
-		let _guard = MASK_LOCK.lock();
-
-		let regoff = GICD_ICENABLER + 4 * (vector as usize / 32);
-		gicd_write(regoff, 1 << (vector % 32));
-
-		Ok(())
-	} else {
-		Err(())
-	}
-}
-
-pub fn unmask_interrupt(vector: u32) -> Result<(), ()> {
-	if vector < *NR_IRQS.get().unwrap() && vector < MAX_HANDLERS.try_into().unwrap() {
-		let _guard = MASK_LOCK.lock();
-
-		let regoff = GICD_ISENABLER + 4 * (vector as usize / 32);
-		gicd_write(regoff, 1 << (vector % 32));
-		Ok(())
-	} else {
-		Err(())
-	}
-}
-
-pub fn set_oneshot_timer(wakeup_time: Option<u64>) {
-	todo!("set_oneshot_timer stub");
-}
-
-pub fn wakeup_core(core_to_wakeup: CoreId) {
+pub fn wakeup_core(_core_to_wakeup: CoreId) {
 	todo!("wakeup_core stub");
 }
 
@@ -344,7 +224,6 @@ pub fn init() {
 
 	let gicd_address =
 		virtualmem::allocate_aligned(gicd_size.try_into().unwrap(), 0x10000).unwrap();
-	GICD_ADDRESS.set(gicd_address).unwrap();
 	debug!("Mapping GIC Distributor interface to virtual address {gicd_address:p}",);
 
 	let mut flags = PageTableEntryFlags::empty();
@@ -358,7 +237,6 @@ pub fn init() {
 
 	let gicc_address =
 		virtualmem::allocate_aligned(gicc_size.try_into().unwrap(), 0x10000).unwrap();
-	GICC_ADDRESS.set(gicc_address).unwrap();
 	debug!("Mapping generic interrupt controller to virtual address {gicc_address:p}",);
 	paging::map::<BasePageSize>(
 		gicc_address,
@@ -367,39 +245,49 @@ pub fn init() {
 		flags,
 	);
 
-	gicc_disable();
-	gicd_disable();
-
-	let nr_irqs = ((gicd_read(GICD_TYPER) & 0x1f) + 1) * 32;
-	info!("Number of supported interrupts {}", nr_irqs);
-	NR_IRQS.set(nr_irqs).unwrap();
-
-	gicd_write(GICD_ICENABLER, 0xffff0000);
-	gicd_write(GICD_ISENABLER, 0x0000ffff);
-	gicd_write(GICD_ICPENDR, 0xffffffff);
-	gicd_write(GICD_IGROUPR, 0);
-
-	for i in 0..32 / 4 {
-		gicd_write(GICD_IPRIORITYR + i * 4, 0x80808080);
-	}
-
-	for i in 32 / 16..nr_irqs / 16 {
-		gicd_write(GICD_NSACR + i as usize * 4, 0xffffffff);
-	}
-
-	for i in 32 / 32..nr_irqs / 32 {
-		gicd_write(GICD_ICENABLER + i as usize * 4, 0xffffffff);
-		gicd_write(GICD_ICPENDR + i as usize * 4, 0xffffffff);
-		gicd_write(GICD_IGROUPR + i as usize * 4, 0);
+	GicV3::set_priority_mask(0xff);
+	let mut gic = unsafe { GicV3::new(gicd_address.as_mut_ptr(), gicc_address.as_mut_ptr()) };
+	gic.setup();
+
+	for node in dtb.enum_subnodes("/") {
+		let parts: Vec<_> = node.split('@').collect();
+
+		if let Some(compatible) = dtb.get_property(parts.first().unwrap(), "compatible") {
+			if core::str::from_utf8(compatible)
+				.unwrap()
+				.find("timer")
+				.is_some()
+			{
+				let irq_slice = dtb
+					.get_property(parts.first().unwrap(), "interrupts")
+					.unwrap();
+				/* Secure Phys IRQ */
+				let (_irqtype, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+				let (_irq, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+				let (_irqflags, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+				/* Non-secure Phys IRQ */
+				let (irqtype, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+				let (irq, irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+				let (irqflags, _irq_slice) = irq_slice.split_at(core::mem::size_of::<u32>());
+				let _irqtype = u32::from_be_bytes(irqtype.try_into().unwrap());
+				let irq = u32::from_be_bytes(irq.try_into().unwrap());
+				let _irqflags = u32::from_be_bytes(irqflags.try_into().unwrap());
+				unsafe {
+					TIMER_INTERRUPT = irq;
+				}
+
+				info!("Timer interrupt: {}", irq);
+				irq_install_handler(irq + 16, timer_handler);
+
+				// enable timer interrupt
+				let timer_irqid = IntId::ppi(irq);
+				gic.set_interrupt_priority(timer_irqid, 0x00);
+				gic.enable_interrupt(timer_irqid, true);
+			}
+		}
 	}
 
-	for i in 32 / 4..nr_irqs / 4 {
-		gicd_write(GICD_ITARGETSR + i as usize * 4, 0);
-		gicd_write(GICD_IPRIORITYR + i as usize * 4, 0x80808080);
+	unsafe {
+		GIC.set(gic).unwrap();
 	}
-
-	gicd_enable();
-
-	gicc_set_priority(0xF0);
-	gicc_enable();
 }
diff --git a/src/arch/aarch64/kernel/processor.rs b/src/arch/aarch64/kernel/processor.rs
index f94bdffd73f4b6aeed0bff8a22ae77eda5ee2f7e..997c953f81e31e459b74111c7997b7c6e381689b 100644
--- a/src/arch/aarch64/kernel/processor.rs
+++ b/src/arch/aarch64/kernel/processor.rs
@@ -4,13 +4,14 @@ use core::{fmt, str};
 
 use aarch64::regs::CNTFRQ_EL0;
 use hermit_dtb::Dtb;
-use hermit_sync::Lazy;
+use hermit_sync::{without_interrupts, Lazy};
 use qemu_exit::QEMUExit;
 use tock_registers::interfaces::Readable;
 
 use crate::arch::aarch64::kernel::boot_info;
 use crate::env;
 
+// System counter frequency in Hz
 static CPU_FREQUENCY: Lazy<CpuFrequency> = Lazy::new(|| {
 	let mut cpu_frequency = CpuFrequency::new();
 	unsafe {
@@ -36,27 +37,27 @@ impl fmt::Display for CpuFrequencySources {
 }
 
 struct CpuFrequency {
-	mhz: u16,
+	hz: u32,
 	source: CpuFrequencySources,
 }
 
 impl CpuFrequency {
 	const fn new() -> Self {
 		CpuFrequency {
-			mhz: 0,
+			hz: 0,
 			source: CpuFrequencySources::Invalid,
 		}
 	}
 
 	fn set_detected_cpu_frequency(
 		&mut self,
-		mhz: u16,
+		hz: u32,
 		source: CpuFrequencySources,
 	) -> Result<(), ()> {
 		//The clock frequency must never be set to zero, otherwise a division by zero will
 		//occur during runtime
-		if mhz > 0 {
-			self.mhz = mhz;
+		if hz > 0 {
+			self.hz = hz;
 			self.source = source;
 			Ok(())
 		} else {
@@ -66,12 +67,12 @@ impl CpuFrequency {
 
 	unsafe fn detect_from_cmdline(&mut self) -> Result<(), ()> {
 		let mhz = env::freq().ok_or(())?;
-		self.set_detected_cpu_frequency(mhz, CpuFrequencySources::CommandLine)
+		self.set_detected_cpu_frequency(u32::from(mhz) * 1000000, CpuFrequencySources::CommandLine)
 	}
 
 	unsafe fn detect_from_register(&mut self) -> Result<(), ()> {
-		let mhz = CNTFRQ_EL0.get() / 1000000;
-		self.set_detected_cpu_frequency(mhz.try_into().unwrap(), CpuFrequencySources::Register)
+		let hz = CNTFRQ_EL0.get() & 0xFFFFFFFF;
+		self.set_detected_cpu_frequency(hz.try_into().unwrap(), CpuFrequencySources::Register)
 	}
 
 	unsafe fn detect(&mut self) {
@@ -82,14 +83,14 @@ impl CpuFrequency {
 		}
 	}
 
-	fn get(&self) -> u16 {
-		self.mhz
+	fn get(&self) -> u32 {
+		self.hz
 	}
 }
 
 impl fmt::Display for CpuFrequency {
 	fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
-		write!(f, "{} MHz (from {})", self.mhz, self.source)
+		write!(f, "{} Hz (from {})", self.hz, self.source)
 	}
 }
 
@@ -150,19 +151,31 @@ pub fn shutdown() -> ! {
 	exit_handler.exit_success();
 }
 
+#[inline]
 pub fn get_timer_ticks() -> u64 {
 	// We simulate a timer with a 1 microsecond resolution by taking the CPU timestamp
 	// and dividing it by the CPU frequency in MHz.
-	get_timestamp() / u64::from(get_frequency())
+	(1000000 * get_timestamp()) / u64::from(CPU_FREQUENCY.get())
 }
 
+#[inline]
 pub fn get_frequency() -> u16 {
-	CPU_FREQUENCY.get()
+	(CPU_FREQUENCY.get() / 1000000).try_into().unwrap()
 }
 
 #[inline]
 pub fn get_timestamp() -> u64 {
-	0
+	let value: u64;
+
+	unsafe {
+		asm!(
+			"mrs {value}, cntpct_el0",
+			value = out(reg) value,
+			options(nostack),
+		);
+	}
+
+	value
 }
 
 #[inline]
@@ -232,6 +245,40 @@ pub fn detect_frequency() {
 	Lazy::force(&CPU_FREQUENCY);
 }
 
+#[inline]
+fn __set_oneshot_timer(wakeup_time: Option<u64>) {
+	if let Some(wt) = wakeup_time {
+		// wt is the absolute wakeup time in microseconds based on processor::get_timer_ticks.
+		let deadline = (wt * u64::from(CPU_FREQUENCY.get())) / 1000000;
+
+		unsafe {
+			asm!(
+				"msr cntp_cval_el0, {value}",
+				"msr cntp_ctl_el0, {enable}",
+				value = in(reg) deadline,
+				enable = in(reg) 1,
+				options(nostack, nomem),
+			);
+		}
+	} else {
+		// disable timer
+		unsafe {
+			asm!(
+				"msr cntp_cval_el0, {disable}",
+				"msr cntp_ctl_el0, {disable}",
+				disable = in(reg) 0,
+				options(nostack, nomem),
+			);
+		}
+	}
+}
+
+pub fn set_oneshot_timer(wakeup_time: Option<u64>) {
+	without_interrupts(|| {
+		__set_oneshot_timer(wakeup_time);
+	});
+}
+
 pub fn print_information() {
 	let dtb = unsafe {
 		Dtb::from_raw(boot_info().hardware_info.device_tree.unwrap().get() as *const u8)
@@ -244,6 +291,6 @@ pub fn print_information() {
 
 	infoheader!(" CPU INFORMATION ");
 	infoentry!("Processor compatiblity", str::from_utf8(reg).unwrap());
-	infoentry!("System frequency", *CPU_FREQUENCY);
+	infoentry!("Counter frequency", *CPU_FREQUENCY);
 	infofooter!();
 }
diff --git a/src/arch/aarch64/kernel/scheduler.rs b/src/arch/aarch64/kernel/scheduler.rs
index 7ac05c0ac2771007374dbc631d5f883d2a2426d3..f0c8f5f6fce255ef7051388f3b345a3333be225f 100644
--- a/src/arch/aarch64/kernel/scheduler.rs
+++ b/src/arch/aarch64/kernel/scheduler.rs
@@ -290,8 +290,6 @@ pub struct TaskTLS {
 
 impl TaskTLS {
 	fn from_environment() -> Option<Box<Self>> {
-		let tls_len = env::get_tls_memsz();
-
 		if env::get_tls_memsz() == 0 {
 			return None;
 		}
@@ -338,10 +336,6 @@ impl TaskTLS {
 	}
 }
 
-extern "C" fn leave_task() -> ! {
-	core_scheduler().exit(0)
-}
-
 #[cfg(not(target_os = "none"))]
 extern "C" fn task_start(_f: extern "C" fn(usize), _arg: usize, _user_stack: u64) -> ! {
 	unimplemented!()
diff --git a/src/arch/mod.rs b/src/arch/mod.rs
index 0e14d4f60d275daab68f66f65dd437fcd808a827..cf54d0389b55dc54515e95fe4e0e1301a18c5844 100644
--- a/src/arch/mod.rs
+++ b/src/arch/mod.rs
@@ -13,10 +13,12 @@ use crate::arch::aarch64::kernel::core_local::core_scheduler;
 #[cfg(target_arch = "aarch64")]
 pub use crate::arch::aarch64::kernel::interrupts;
 #[cfg(target_arch = "aarch64")]
-pub use crate::arch::aarch64::kernel::interrupts::{set_oneshot_timer, wakeup_core};
+pub use crate::arch::aarch64::kernel::interrupts::wakeup_core;
 #[cfg(target_arch = "aarch64")]
 pub use crate::arch::aarch64::kernel::processor;
 #[cfg(target_arch = "aarch64")]
+pub use crate::arch::aarch64::kernel::processor::set_oneshot_timer;
+#[cfg(target_arch = "aarch64")]
 pub use crate::arch::aarch64::kernel::scheduler;
 #[cfg(target_arch = "aarch64")]
 pub use crate::arch::aarch64::kernel::switch;