diff --git a/kernel/src/kernel/fastpath.rs b/kernel/src/kernel/fastpath.rs
index c86ec85..31015aa 100644
--- a/kernel/src/kernel/fastpath.rs
+++ b/kernel/src/kernel/fastpath.rs
@@ -205,7 +205,7 @@ pub fn fastpath_restore(_badge: usize, _msgInfo: usize, cur_thread: *mut tcb_t)
 #[inline]
 #[no_mangle]
 pub fn fastpath_call(cptr: usize, msgInfo: usize) {
-	// sel4_common::println!("fastpath call");
+    // sel4_common::println!("fastpath call");
     let current = get_currenct_thread();
     let mut info = seL4_MessageInfo::from_word(msgInfo);
     let length = info.get_length() as usize;
diff --git a/kernel/src/syscall/invocation/decode/decode_sched_invocation.rs b/kernel/src/syscall/invocation/decode/decode_sched_invocation.rs
index cea6aff..3aacf1e 100644
--- a/kernel/src/syscall/invocation/decode/decode_sched_invocation.rs
+++ b/kernel/src/syscall/invocation/decode/decode_sched_invocation.rs
@@ -31,7 +31,8 @@ use crate::{
         get_syscall_arg,
         invocation::invoke_sched::{
             invokeSchedContext_Bind, invokeSchedContext_Consumed, invokeSchedContext_Unbind,
-            invokeSchedContext_UnbindObject, invokeSchedControl_ConfigureFlags,
+            invokeSchedContext_UnbindObject, invokeSchedContext_YieldTo,
+            invokeSchedControl_ConfigureFlags,
         },
     },
 };
@@ -61,7 +62,7 @@ pub fn decode_sched_context_invocation(
             set_thread_state(get_currenct_thread(), ThreadState::ThreadStateRestart);
             invokeSchedContext_Unbind(sc)
         }
-        MessageLabel::SchedContextYieldTo => decodeSchedContext_YieldTo(sc, buffer),
+        MessageLabel::SchedContextYieldTo => decodeSchedContext_YieldTo(sc),
         _ => {
             debug!("SchedContext invocation: Illegal operation attempted.");
             unsafe {
@@ -302,7 +303,45 @@ pub fn decodeSchedContext_Bind(sc: &mut sched_context) -> exception_t {
         }
     }
 }
-pub fn decodeSchedContext_YieldTo(sc: &mut sched_context, buffer: &seL4_IPCBuffer) -> exception_t {
-    unimplemented!("MCS yield to");
-    // TODO: MCS
+pub fn decodeSchedContext_YieldTo(sc: &mut sched_context) -> exception_t {
+    let thread = get_currenct_thread();
+
+    if sc.scTcb == 0 {
+        debug!("SchedContext_YieldTo: cannot yield to an inactive sched context");
+        unsafe {
+            current_syscall_error._type = seL4_IllegalOperation;
+        }
+        return exception_t::EXCEPTION_SYSCALL_ERROR;
+    }
+    if sc.scTcb == thread.get_ptr() {
+        debug!("SchedContext_YieldTo: cannot seL4_SchedContext_YieldTo on self");
+        unsafe {
+            current_syscall_error._type = seL4_IllegalOperation;
+        }
+        return exception_t::EXCEPTION_SYSCALL_ERROR;
+    }
+    if convert_to_mut_type_ref::<tcb_t>(sc.scTcb).tcbPriority > thread.tcbMCP {
+        debug!(
+            "SchedContext_YieldTo: insufficient mcp {} to yield to a thread with prio {}",
+            thread.tcbMCP,
+            convert_to_mut_type_ref::<tcb_t>(sc.scTcb).tcbPriority
+        );
+        unsafe {
+            current_syscall_error._type = seL4_IllegalOperation;
+        }
+        return exception_t::EXCEPTION_SYSCALL_ERROR;
+    }
+    assert!(thread.tcbYieldTo == 0);
+    if thread.tcbYieldTo != 0 {
+        debug!(
+            "SchedContext_YieldTo: cannot seL4_SchedContext_YieldTo to more than on SC at a time"
+        );
+        unsafe {
+            current_syscall_error._type = seL4_IllegalOperation;
+        }
+        return exception_t::EXCEPTION_SYSCALL_ERROR;
+    }
+
+    set_thread_state(thread, ThreadState::ThreadStateRestart);
+    return invokeSchedContext_YieldTo(sc);
 }
diff --git a/kernel/src/syscall/invocation/invoke_sched.rs b/kernel/src/syscall/invocation/invoke_sched.rs
index f175f36..5ca596f 100644
--- a/kernel/src/syscall/invocation/invoke_sched.rs
+++ b/kernel/src/syscall/invocation/invoke_sched.rs
@@ -5,7 +5,7 @@ use sel4_common::{
     utils::convert_to_mut_type_ref,
 };
 use sel4_task::{
-    checkBudget, commitTime, ksCurSC, ksCurThread, possible_switch_to,
+    checkBudget, commitTime, get_currenct_thread, ksCurSC, ksCurThread, possible_switch_to,
     reply::reply_t,
     rescheduleRequired,
     sched_context::{sched_context, MIN_REFILLS},
@@ -58,8 +58,30 @@ pub fn invokeSchedContext_Consumed(sc: &mut sched_context, buffer: &seL4_IPCBuff
     exception_t::EXCEPTION_NONE
 }
 pub fn invokeSchedContext_YieldTo(sc: &mut sched_context) -> exception_t {
-    // TODO: MCS
-    unimplemented!("invoke sched context yieldto");
+    if sc.scYieldFrom != 0 {
+        convert_to_mut_type_ref::<tcb_t>(sc.scYieldFrom).schedContext_completeYieldTo();
+        assert!(sc.scYieldFrom == 0);
+    }
+    sc.schedContext_resume();
+    let mut return_now = true;
+    let tcb = convert_to_mut_type_ref::<tcb_t>(sc.scTcb);
+    if tcb.is_schedulable() {
+        if tcb.tcbPriority < get_currenct_thread().tcbPriority {
+            tcb.sched_dequeue();
+            tcb.sched_enqueue();
+        } else {
+            get_currenct_thread().tcbYieldTo = sc.get_ptr();
+            sc.scYieldFrom = get_currenct_thread().get_ptr();
+            tcb.sched_dequeue();
+            get_currenct_thread().sched_enqueue();
+            tcb.sched_enqueue();
+            rescheduleRequired();
+            return_now = false;
+        }
+    }
+    if return_now == true {
+        sc.setConsumed();
+    }
     exception_t::EXCEPTION_NONE
 }
 pub fn invokeSchedControl_ConfigureFlags(
diff --git a/sel4_ipc/src/endpoint.rs b/sel4_ipc/src/endpoint.rs
index f21b1e7..9f31579 100644
--- a/sel4_ipc/src/endpoint.rs
+++ b/sel4_ipc/src/endpoint.rs
@@ -1,9 +1,9 @@
 use crate::transfer::Transfer;
+use sel4_common::arch::ArchReg;
+use sel4_common::structures_gen::endpoint;
 #[cfg(feature = "KERNEL_MCS")]
 use sel4_common::structures_gen::seL4_Fault_tag::seL4_Fault_NullFault;
-use sel4_common::structures_gen::endpoint;
 use sel4_common::utils::{convert_to_mut_type_ref, convert_to_option_mut_type_ref};
-use sel4_common::arch::ArchReg;
 #[cfg(feature = "KERNEL_MCS")]
 use sel4_task::{ksCurSC, reply::reply_t, sched_context::sched_context_t};
 use sel4_task::{
diff --git a/sel4_task/src/sched_context.rs b/sel4_task/src/sched_context.rs
index 6490b36..d722b1c 100644
--- a/sel4_task/src/sched_context.rs
+++ b/sel4_task/src/sched_context.rs
@@ -288,7 +288,7 @@ impl sched_context {
                     convert_to_mut_type_ref::<tcb_t>(self.scTcb)
                         .tcbState
                         .get_tcbQueued()
-                        != 0
+                        == 0
                 );
                 self.postpone();
             }