diff --git a/arch/x86_64/src/start.rs b/arch/x86_64/src/start.rs
index 33d52f0..566f696 100644
--- a/arch/x86_64/src/start.rs
+++ b/arch/x86_64/src/start.rs
@@ -207,7 +207,7 @@ pub unsafe fn usermode(ip: usize, sp: usize) -> ! {
 
         push rax # Push stack segment
         push rbx # Push stack pointer
-        mov rax, 0 # 3 << 12 | 1 << 9 # Set IOPL and interrupt enable flag
+        mov rax, 3 << 12 | 1 << 9 # Set IOPL and interrupt enable flag
         push rax # Push rflags
         mov rax, 0x23
         push rax # Push code segment
diff --git a/init/src/main.rs b/init/src/main.rs
index 0912780..dc90bf5 100644
--- a/init/src/main.rs
+++ b/init/src/main.rs
@@ -1,3 +1,8 @@
+use std::thread;
+
 pub fn main() {
     println!("Hello, World!");
+    loop {
+        thread::yield_now();
+    }
 }
diff --git a/kernel/context/mod.rs b/kernel/context/mod.rs
index e550b54..c0911cf 100644
--- a/kernel/context/mod.rs
+++ b/kernel/context/mod.rs
@@ -127,12 +127,11 @@ pub unsafe fn switch() {
         arch::interrupt::pause();
     }
 
-    let from_ptr = if let Some(context_lock) = contexts().current() {
+    let from_ptr = {
+        let contexts = contexts();
+        let context_lock = contexts.current().expect("context::switch: Not inside of context");
         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;
@@ -146,7 +145,9 @@ pub unsafe fn switch() {
     }
 
     if to_ptr as usize == 0 {
-        print!("NO TO_PTR\n");
+        // TODO: Sleep, wait for interrupt
+        // Unset global lock if no context found
+        arch::context::CONTEXT_SWITCH_LOCK.store(false, Ordering::SeqCst);
         return;
     }
 
diff --git a/kernel/syscall/mod.rs b/kernel/syscall/mod.rs
index dc3b387..6696ffd 100644
--- a/kernel/syscall/mod.rs
+++ b/kernel/syscall/mod.rs
@@ -30,6 +30,8 @@ pub enum Call {
     Exec = 11,
     /// Get process ID
     GetPid = 20,
+    /// Yield to scheduler
+    SchedYield = 158
 }
 
 /// Convert numbers to calls
@@ -44,6 +46,7 @@ impl Call {
             6 => Ok(Call::Close),
             11 => Ok(Call::Exec),
             20 => Ok(Call::GetPid),
+            158 => Ok(Call::SchedYield),
             _ => Err(Error::NoCall)
         }
     }
@@ -97,6 +100,7 @@ pub fn handle(a: usize, b: usize, c: usize, d: usize, e: usize, _f: usize) -> Re
         Call::Close => close(b),
         Call::Exec => exec(convert_slice(b as *const u8, c)?, convert_slice(d as *const [usize; 2], e)?),
         Call::GetPid => getpid(),
+        Call::SchedYield => sched_yield()
     }
 }
 
diff --git a/kernel/syscall/process.rs b/kernel/syscall/process.rs
index 9db0bfa..0149aed 100644
--- a/kernel/syscall/process.rs
+++ b/kernel/syscall/process.rs
@@ -28,3 +28,8 @@ pub fn getpid() -> Result<usize> {
     let context = context_lock.read();
     Ok(context.id)
 }
+
+pub fn sched_yield() -> Result<usize> {
+    unsafe { context::switch(); }
+    Ok(0)
+}