diff --git a/context/mod.rs b/context/mod.rs
index 54877809c4ba230c38bb7cacc7ba727a46ae9a56..c7c02a88375d2220d2cd14c5d707a4f2cac8ee61 100644
--- a/context/mod.rs
+++ b/context/mod.rs
@@ -6,6 +6,7 @@ use core::mem;
 use core::sync::atomic::{AtomicUsize, ATOMIC_USIZE_INIT, Ordering};
 use spin::{Once, RwLock, RwLockReadGuard, RwLockWriteGuard};
 
+use arch;
 use arch::context::Context as ArchContext;
 use syscall::{Error, Result};
 
@@ -87,7 +88,8 @@ static CONTEXT_ID: AtomicUsize = ATOMIC_USIZE_INIT;
 pub fn init() {
     let mut contexts = contexts_mut();
     let context_lock = contexts.new_context().expect("could not initialize first context");
-    let context = context_lock.read();
+    let mut context = context_lock.write();
+    context.running = true;
     CONTEXT_ID.store(context.id, Ordering::SeqCst);
 }
 
@@ -108,8 +110,44 @@ pub fn contexts_mut() -> RwLockWriteGuard<'static, ContextList> {
 
 /// Switch to the next context
 /// Do not call this while holding locks!
-pub unsafe fn context_switch() {
-//    current.arch.switch_to(&mut next.arch);
+pub unsafe fn switch() {
+    use core::ops::DerefMut;
+
+    // Set the global lock to avoid the unsafe operations below from causing issues
+    while arch::context::CONTEXT_SWITCH_LOCK.compare_and_swap(false, true, Ordering::SeqCst) {
+        arch::interrupt::pause();
+    }
+
+    let from_ptr = if let Some(context_lock) = contexts().current() {
+        let mut context = context_lock.write();
+        context.deref_mut() as *mut Context
+    } else {
+        print!("NO FROM_PTR\n");
+        return;
+    };
+
+    let mut to_ptr = 0 as *mut Context;
+
+    for (_pid, context_lock) in contexts().map.iter() {
+        let mut context = context_lock.write();
+        if ! context.running {
+            to_ptr = context.deref_mut() as *mut Context;
+            break;
+        }
+    }
+
+    if to_ptr as usize == 0 {
+        print!("NO TO_PTR\n");
+        return;
+    }
+
+    unsafe {
+        (&mut *from_ptr).running = false;
+        (&mut *to_ptr).running = true;
+        CONTEXT_ID.store((&mut *to_ptr).id, Ordering::SeqCst);
+        
+        (&mut *from_ptr).arch.switch_to(&mut (&mut *to_ptr).arch);
+    }
 }
 
 /// A context, which identifies either a process or a thread
@@ -117,6 +155,8 @@ pub unsafe fn context_switch() {
 pub struct Context {
     /// The ID of this context
     pub id: usize,
+    /// Running or not
+    pub running: bool,
     /// The architecture specific context
     pub arch: ArchContext,
     /// Kernel stack
@@ -130,6 +170,7 @@ impl Context {
     pub fn new(id: usize) -> Context {
         Context {
             id: id,
+            running: false,
             arch: ArchContext::new(),
             kstack: None,
             files: Vec::new()
diff --git a/lib.rs b/lib.rs
index acdf02881614c22ba1dc48068faeb526a0605706..cc032657172ddf44a00f13a8cf99af2085f8e240 100644
--- a/lib.rs
+++ b/lib.rs
@@ -121,8 +121,10 @@ pub mod syscall;
 pub mod tests;
 
 pub extern fn context_test() {
-    print!("TEST\n");
+    print!("Test\n");
+    unsafe { context::switch(); }
 
+    print!("Test halt\n");
     loop {
         unsafe { interrupt::enable_and_halt(); }
     }
@@ -134,26 +136,14 @@ pub extern fn kmain() {
 
     print!("{}", format!("BSP: {:?}\n", syscall::getpid()));
 
-    let to_ptr = if let Ok(context_lock) = context::contexts_mut().spawn(context_test) {
+    if let Ok(context_lock) = context::contexts_mut().spawn(context_test) {
         print!("Spawned context\n");
-        let mut context = context_lock.write();
-        &mut context.arch as *mut arch::context::Context
-    } else {
-        0 as *mut arch::context::Context
-    };
-
-    let from_ptr = if let Some(context_lock) = context::contexts().current() {
-        let mut context = context_lock.write();
-        &mut context.arch as *mut arch::context::Context
-    } else {
-        0 as *mut arch::context::Context
-    };
-
-    if to_ptr as usize != 0 && from_ptr as usize != 0 {
-        print!("Switching\n");
-        unsafe { (&mut *from_ptr).switch_to(&mut *to_ptr); }
     }
 
+    print!("Main\n");
+    unsafe { context::switch(); }
+
+    print!("Main halt\n");
     loop {
         unsafe { interrupt::enable_and_halt(); }
     }