diff --git a/address/src/virt.rs b/address/src/virt.rs
index 031c7f6..6f11cb7 100644
--- a/address/src/virt.rs
+++ b/address/src/virt.rs
@@ -112,6 +112,11 @@ impl<T: AddressSpace> VirtualAddress<T> {
         self.0 as *mut U
     }
 
+    #[inline(always)]
+    pub fn as_ptr<U>(self) -> *const U {
+        self.0 as *const U
+    }
+
     #[inline(always)]
     pub unsafe fn as_mut<U>(self) -> &'static mut U {
         &mut *(self.0 as *mut U)
diff --git a/kernel/src/arch/exception.rs b/kernel/src/arch/exception.rs
index 3aa10fe..b80a30b 100644
--- a/kernel/src/arch/exception.rs
+++ b/kernel/src/arch/exception.rs
@@ -30,7 +30,7 @@ struct ExceptionContext {
 
 #[no_mangle]
 extern "C" fn exc_handler(context: ExceptionContext) -> ! {
-    debug!("Unhandled exception");
+    debug!("Unhandled exception\n");
 
     loop {}
 }
diff --git a/kernel/src/arch/mod.rs b/kernel/src/arch/mod.rs
index aa4b15a..67816e8 100644
--- a/kernel/src/arch/mod.rs
+++ b/kernel/src/arch/mod.rs
@@ -1 +1,18 @@
+use address::{PhysicalAddress, VirtualAddress};
+use crate::KernelSpace;
+
 pub mod exception;
+pub mod timer;
+
+pub unsafe fn mmio_write(addr: usize, value: u32) {
+    core::ptr::write_volatile(
+        VirtualAddress::<KernelSpace>::from(PhysicalAddress::from(addr)).as_mut_ptr(),
+        value,
+    );
+}
+
+pub unsafe fn mmio_read(addr: usize) -> u32 {
+    core::ptr::read_volatile(
+        VirtualAddress::<KernelSpace>::from(PhysicalAddress::from(addr)).as_mut_ptr(),
+    )
+}
diff --git a/kernel/src/arch/timer.rs b/kernel/src/arch/timer.rs
new file mode 100644
index 0000000..4c507b8
--- /dev/null
+++ b/kernel/src/arch/timer.rs
@@ -0,0 +1,41 @@
+use super::{mmio_write, mmio_read};
+use crate::cpu::get_cpu;
+
+pub struct LocalTimer;
+
+const CORE_TIMER_PSC: usize = 0x40000008;
+const CORE_TIMER_INTC: usize = 0x40000040;
+
+impl LocalTimer {
+    // XXX Only usable on core0 at this moment,
+    // as I didn't yet write a way to determine
+    // "ARM" core number
+    pub unsafe fn enable() {
+        let mut phys_core_id: usize;
+        llvm_asm!(r#"
+            mrs $0, mpidr_el1;
+            and $0, $0, #3
+        "#:"=r"(phys_core_id));
+
+        debug!("cpu{}: physical core id is {}\n", get_cpu().cpu_id, phys_core_id);
+
+        mmio_write(CORE_TIMER_PSC + 4 * phys_core_id, 0x80000000);
+        let tmp = mmio_read(CORE_TIMER_INTC + 4 * phys_core_id);
+        mmio_write(CORE_TIMER_INTC + 4 * phys_core_id, tmp | (1 << 1));
+
+        llvm_asm!(r#"
+            mov x0, #1
+            msr cntp_ctl_el0, x0
+        "#);
+    }
+
+    pub fn update(value: usize) {
+        unsafe {
+            llvm_asm!(r#"
+                mrs x0, cntpct_el0
+                add x0, x0, $0
+                msr cntp_cval_el0, x0
+            "#::"r"(value):"x0");
+        }
+    }
+}
diff --git a/kernel/src/arch/vectors.S b/kernel/src/arch/vectors.S
index 175ff95..6b4bc7c 100644
--- a/kernel/src/arch/vectors.S
+++ b/kernel/src/arch/vectors.S
@@ -39,7 +39,44 @@ vec_el1_sp_elx_sync:
 
 .p2align 7
 vec_el1_sp_elx_irq:
-    b .
+    sub sp, sp, #192
+    stp x0, x1, [sp, #0]
+    stp x2, x3, [sp, #16]
+    stp x4, x5, [sp, #32]
+    stp x6, x7, [sp, #48]
+    stp x8, x9, [sp, #64]
+    stp x10, x11, [sp, #80]
+    stp x12, x13, [sp, #96]
+    stp x14, x15, [sp, #112]
+    stp x16, x17, [sp, #128]
+    stp x18, x29, [sp, #144]
+    mrs x0, elr_el1
+    stp x30, x0, [sp, #160]
+
+    bl sched_yield
+
+    mrs x0, cntpct_el0
+    mov x1, #1 << 10
+    lsl x1, x1, #1
+    add x0, x0, x1
+    msr cntp_cval_el0, x0
+
+    ldp x30, x0, [sp, #160]
+    msr elr_el1, x0
+    ldp x0, x1, [sp, #0]
+    ldp x2, x3, [sp, #16]
+    ldp x4, x5, [sp, #32]
+    ldp x6, x7, [sp, #48]
+    ldp x8, x9, [sp, #64]
+    ldp x10, x11, [sp, #80]
+    ldp x12, x13, [sp, #96]
+    ldp x14, x15, [sp, #112]
+    ldp x16, x17, [sp, #128]
+    ldp x18, x29, [sp, #144]
+    add sp, sp, #192
+
+    eret
+
 .p2align 7
 vec_el1_sp_elx_fiq:
     b .
diff --git a/kernel/src/boot/entry.S b/kernel/src/boot/entry.S
index 960bb06..091c600 100644
--- a/kernel/src/boot/entry.S
+++ b/kernel/src/boot/entry.S
@@ -66,7 +66,8 @@ _entry_bsp:
     // orr x2, x2, #(0 << 2)   // MAIR[0]
     str x1, [x0]
 
-    mov x1, #(1 << 0)       // Present
+    mov x1, #1 << 30
+    orr x1, x1, #(1 << 0)       // Present
     orr x1, x1, #(1 << 10)  // Accessed
     orr x1, x1, #(3 << 8)   // Inner shareable
     orr x1, x1, #(1 << 2)   // MAIR[1]
@@ -104,6 +105,11 @@ _entry_ap:
     bne 1f
 
     // Leave EL2
+    mrs x0, cnthctl_el2
+    orr x0, x0, #((1 << 1) | (1 << 0))
+    msr cnthctl_el2, x0
+    msr cntvoff_el2, xzr
+
     adr x0, 1f
     msr elr_el2, x0
     // TODO mask DAIF?
diff --git a/kernel/src/cpu.rs b/kernel/src/cpu.rs
index 42377b3..3f9a289 100644
--- a/kernel/src/cpu.rs
+++ b/kernel/src/cpu.rs
@@ -5,15 +5,15 @@ use crate::{
 };
 use address::{PhysicalAddress, VirtualAddress};
 use core::mem::MaybeUninit;
-use core::sync::atomic::{Ordering, AtomicUsize};
+use core::sync::atomic::{AtomicUsize, Ordering};
 
 #[repr(C)]
 pub struct Cpu {
-    pub cpu_id: u32,        // 0x00
-    _pad0: u32,             // 0x04
-    stack_top: usize,       // 0x08
+    pub cpu_id: u32,  // 0x00
+    _pad0: u32,       // 0x04
+    stack_top: usize, // 0x08
     //
-    pub scheduler: Scheduler
+    pub scheduler: Scheduler,
 }
 
 const MAX_CPU: usize = 4;
@@ -43,6 +43,10 @@ extern "C" fn kernel_ap_main() -> ! {
     CPU_COUNT.fetch_add(1, Ordering::SeqCst);
 
     wakeup_single_ap();
+    unsafe {
+        llvm_asm!("msr daifset, #0xF;");
+        crate::arch::timer::LocalTimer::enable();
+    }
 
     proc::enter();
 }
@@ -74,7 +78,7 @@ pub fn wakeup_single_ap() {
             cpu_id: index as u32,
             _pad0: 0,
             stack_top: (stack_bottom + 4 * 4096).into(),
-            scheduler: Scheduler::new()
+            scheduler: Scheduler::new(),
         });
 
         let cpu_addr = VirtualAddress::<KernelSpace>::from(CPUS[index].as_mut_ptr() as u64);
@@ -96,7 +100,7 @@ pub fn bsp_init() {
             cpu_id: 0,
             _pad0: 0,
             stack_top: 0,
-            scheduler: Scheduler::new()
+            scheduler: Scheduler::new(),
         });
         set_cpu(CPUS[0].as_mut_ptr());
 
diff --git a/kernel/src/main.rs b/kernel/src/main.rs
index fada6f8..d0c72a0 100644
--- a/kernel/src/main.rs
+++ b/kernel/src/main.rs
@@ -50,6 +50,11 @@ extern "C" fn kernel_bsp_main() -> ! {
 
     debug!("BSP init finished\n");
 
+    unsafe {
+        llvm_asm!("msr daifset, #0xF;");
+        arch::timer::LocalTimer::enable();
+    }
+
     cpu::wakeup_single_ap();
     proc::enter();
 }
diff --git a/kernel/src/proc/context.S b/kernel/src/proc/context.S
index bdd9e84..ee61f1d 100644
--- a/kernel/src/proc/context.S
+++ b/kernel/src/proc/context.S
@@ -3,12 +3,14 @@
 .global context_switch
 
 context_enter_kernel:
-    ldp x0, xzr, [sp, #0]
-    ldp lr, x1, [sp, #16]
-    mov sp, x1
-    ret
+    mov x0, #5
+    msr spsr_el1, x0
+    ldp x0, x1, [sp]
+    msr elr_el1, x1
+    eret
 
 context_switch:
+    msr daifset, #0xF
     // Store old callee-saved regs on stack
     sub sp, sp, #96
 
@@ -23,6 +25,7 @@ context_switch:
     mov x19, sp
     str x19, [x1]
 context_switch_to:
+    msr daifset, #0xF
     // Load new stack
     ldr x0, [x0]
     mov sp, x0
diff --git a/kernel/src/proc/context.rs b/kernel/src/proc/context.rs
index 4f20864..a0b1d31 100644
--- a/kernel/src/proc/context.rs
+++ b/kernel/src/proc/context.rs
@@ -9,8 +9,8 @@ global_asm!(include_str!("context.S"));
 
 #[repr(C)]
 pub(super) struct Context {
-    kernel_sp: VirtualAddress<KernelSpace>,     // 0x00
-    cpu_id: u32,                                // 0x08
+    pub kernel_sp: VirtualAddress<KernelSpace>,     // 0x00
+    cpu_id: u32,                                    // 0x08
 }
 
 struct StackBuilder {
@@ -24,9 +24,9 @@ impl Context {
         let mut stack = unsafe { StackBuilder::new(kstack_phys.into(), 4096 * 4) };
         let stack_top = stack.sp;
 
-        stack.push(stack_top); // popped into stack register before ERET
+        debug!("Stack bounds: {:?}..{:?}\n", stack.sp, stack.bp);
+
         stack.push(entry); // ELR before ERET
-        stack.push(0usize); // padding
         stack.push(arg);
 
         stack.push(context_enter_kernel as usize); // x30 LR
diff --git a/kernel/src/proc/mod.rs b/kernel/src/proc/mod.rs
index dbf7333..f41dd52 100644
--- a/kernel/src/proc/mod.rs
+++ b/kernel/src/proc/mod.rs
@@ -28,20 +28,6 @@ impl Process {
             sched_next: null_mut(),
         }
     }
-
-    //pub unsafe fn enter_initial(&mut self) -> ! {
-    //    context_switch_to(&mut self.context);
-    //    panic!("This code should not run");
-    //}
-
-    //pub unsafe fn switch_to(&mut self, next: &mut Process) {
-    //    if self as *mut _ == next as *mut _ {
-    //        return;
-    //    }
-    //    // TODO &mut self.context argument can be eliminated
-    //    //      if extracted from Cpu struct in assembly
-    //    context_switch(&mut next.context, &mut self.context);
-    //}
 }
 
 impl Scheduler {
@@ -69,15 +55,18 @@ impl Scheduler {
         }
     }
 
-    unsafe fn enter_process(&mut self, proc: *mut Process) -> ! {
-        *self.current.lock() = proc;
-        context_switch_to(&mut (*proc).context);
-        panic!("This code should not run");
-    }
+    unsafe fn switch_to(&mut self, proc: *mut Process) {
+        let mut lock = self.current.lock();
+        let from = *lock;
+        *lock = proc;
 
-    unsafe fn switch_process(&mut self, from: *mut Process, to: *mut Process) {
-        *self.current.lock() = to;
-        context_switch(&mut (*to).context, &mut (*from).context);
+        if from.is_null() {
+            drop(lock);
+            context_switch_to(&mut (*proc).context);
+        } else {
+            drop(lock);
+            context_switch(&mut (*proc).context, &mut (*from).context);
+        }
     }
 
     unsafe fn enter(&mut self) -> ! {
@@ -89,7 +78,9 @@ impl Scheduler {
             self.idle.as_mut_ptr()
         };
         drop(lock);
-        self.enter_process(proc);
+        self.switch_to(proc);
+
+        panic!("This code should not run");
     }
 
     unsafe fn init_idle(&mut self) {
@@ -112,10 +103,11 @@ impl Scheduler {
         };
 
         assert!(!to.is_null());
+
         drop(queue_lock);
         drop(current_lock);
 
-        self.switch_process(from, to);
+        self.switch_to(to);
     }
 }
 
@@ -131,27 +123,24 @@ pub extern "C" fn sched_yield() {
     }
 }
 
-fn f0(arg: usize) {
+extern "C" fn f0(arg: usize) {
     loop {
         debug!("{}", arg);
-        unsafe {
-            sched_yield();
-        }
     }
 }
 
-static mut C0: MaybeUninit<Process> = MaybeUninit::uninit();
-static mut C1: MaybeUninit<Process> = MaybeUninit::uninit();
+static mut C: [MaybeUninit<Process>; 8] = MaybeUninit::uninit_array();
 
 pub fn enter() -> ! {
     unsafe {
         let cpu = get_cpu();
 
-        if cpu.cpu_id == 0 {
-            debug!("Setting up a task for cpu0\n");
-            C0.write(Process::new_kernel(f0 as usize, 0));
-            cpu.scheduler.queue(C0.assume_init_mut());
-        }
+        debug!("Setting up a task for cpu{}\n", cpu.cpu_id);
+        let id = cpu.cpu_id as usize;
+        C[id * 2].write(Process::new_kernel(f0 as usize, id * 2));
+        C[id * 2 + 1].write(Process::new_kernel(f0 as usize, id * 2 + 1));
+        cpu.scheduler.queue(C[id * 2 + 1].assume_init_mut());
+        cpu.scheduler.queue(C[id * 2].assume_init_mut());
 
         // Initialize the idle task
         cpu.scheduler.init_idle();