diff --git a/kernel/src/syscall/invocation/invoke_tcb.rs b/kernel/src/syscall/invocation/invoke_tcb.rs
index 798732e..f46da80 100644
--- a/kernel/src/syscall/invocation/invoke_tcb.rs
+++ b/kernel/src/syscall/invocation/invoke_tcb.rs
@@ -98,7 +98,9 @@ pub fn invoke_tcb_write_registers(
 
     if resumeTarget != 0 {
         // cancel_ipc(dest);
-        dest.cancel_ipc();
+        if dest.is_stopped(){
+            dest.cancel_ipc();
+        }
         dest.restart();
     }
     if dest.is_current() {
diff --git a/sel4_task/src/tcb.rs b/sel4_task/src/tcb.rs
index eba1b4a..41b4e2b 100644
--- a/sel4_task/src/tcb.rs
+++ b/sel4_task/src/tcb.rs
@@ -24,7 +24,8 @@ use sel4_vspace::{
     setCurrentUserVSpaceRoot, ttbr_new,
 };
 use sel4_vspace::{pptr_t, set_vm_root};
-
+#[cfg(feature="KERNEL_MCS")]
+use crate::ksCurSC;
 use crate::prio_t;
 use crate::tcb_queue::tcb_queue_t;
 use sel4_common::sel4_config::*;
@@ -539,10 +540,15 @@ impl tcb_t {
         if self.is_stopped() {
             #[cfg(feature = "KERNEL_MCS")]
             {
-                // TODO: MCS
-                // #ifdef CONFIG_KERNEL_MCS
-                //         reply_remove_tcb(tptr);
-                // #else
+                // MCS
+                set_thread_state(self, ThreadState::ThreadStateRestart);
+                if convert_to_mut_type_ref::<sched_context_t>(self.tcbSchedContext).sc_sporadic() && self.tcbSchedContext != unsafe{ksCurSC}{
+                    convert_to_mut_type_ref::<sched_context_t>(self.tcbSchedContext).refill_unblock_check();
+                }
+                convert_to_mut_type_ref::<sched_context_t>(self.tcbSchedContext).schedContext_resume();
+                if self.is_schedulable(){
+                    possible_switch_to(self);
+                }
             }
             #[cfg(not(feature = "KERNEL_MCS"))]
             {