diff --git a/include/object/endpoint.h b/include/object/endpoint.h
index 13c430b92..8f129fe85 100644
--- a/include/object/endpoint.h
+++ b/include/object/endpoint.h
@@ -30,6 +30,8 @@ void sendIPC(bool_t blocking, bool_t do_call, word_t badge,
              bool_t canGrant, bool_t canGrantReply, bool_t canDonate, tcb_t *thread,
              endpoint_t *epptr);
 void receiveIPC(tcb_t *thread, cap_t cap, bool_t isBlocking, cap_t replyCPtr);
+void tcbEPAppend(tcb_t *thread, endpoint_t *epptr, bool_t isRecv);
+void tcbEPDequeue(tcb_t *thread, endpoint_t *epptr);
 void reorderEP(endpoint_t *epptr, tcb_t *thread);
 #else
 void sendIPC(bool_t blocking, bool_t do_call, word_t badge,
diff --git a/include/object/notification.h b/include/object/notification.h
index a2944abcc..6cbfdcb60 100644
--- a/include/object/notification.h
+++ b/include/object/notification.h
@@ -18,6 +18,8 @@ void unbindMaybeNotification(notification_t *ntfnPtr);
 void unbindNotification(tcb_t *tcb);
 void bindNotification(tcb_t *tcb, notification_t *ntfnPtr);
 #ifdef CONFIG_KERNEL_MCS
+void tcbNTFNAppend(tcb_t *thread, notification_t *ntfnPtr);
+void tcbNTFNDequeue(tcb_t *thread, notification_t *ntfnPtr);
 void reorderNTFN(notification_t *notification, tcb_t *thread);
 
 static inline void maybeReturnSchedContext(notification_t *ntfnPtr, tcb_t *tcb)
diff --git a/include/object/structures.h b/include/object/structures.h
index fe89f8471..340f24e60 100644
--- a/include/object/structures.h
+++ b/include/object/structures.h
@@ -290,12 +290,17 @@ struct tcb {
     word_t tcbAffinity;
 #endif /* ENABLE_SMP_SUPPORT */
 
-    /* Previous and next pointers for scheduler queues , 2 words */
+    /* Previous and next pointers for scheduler queues, 2 words
+     * also used for endpoint and notification queues in MCS */
     struct tcb *tcbSchedNext;
     struct tcb *tcbSchedPrev;
-    /* Previous and next pointers for endpoint and notification queues, 2 words */
+
+#ifndef CONFIG_KERNEL_MCS
+    /* Previous and next pointers for endpoint and notification queues, 2 words
+     * only for non-MCS configurations */
     struct tcb *tcbEPNext;
     struct tcb *tcbEPPrev;
+#endif
 
 #ifdef CONFIG_BENCHMARK_TRACK_UTILISATION
     /* 16 bytes (12 bytes aarch32) */
@@ -304,6 +309,12 @@ struct tcb {
 };
 typedef struct tcb tcb_t;
 
+/* To enable more uniform code shape between MCS and non-MCS configurations: */
+#ifdef CONFIG_KERNEL_MCS
+#define tcbEPNext tcbSchedNext
+#define tcbEPPrev tcbSchedPrev
+#endif
+
 #ifdef CONFIG_DEBUG_BUILD
 /* This debug_tcb object is inserted into the 'unused' region of a TCB object
    for debug build configurations. */
diff --git a/include/object/tcb.h b/include/object/tcb.h
index c2226a120..3ad0c8bf7 100644
--- a/include/object/tcb.h
+++ b/include/object/tcb.h
@@ -128,42 +128,62 @@ void remoteTCBStall(tcb_t *tcb);
 #define SCHED_APPEND_CURRENT_TCB    tcbSchedAppend(NODE_STATE(ksCurThread))
 
 #ifdef CONFIG_KERNEL_MCS
-/* Add TCB into the priority ordered endpoint queue */
-static inline tcb_queue_t tcbEPAppend(tcb_t *tcb, tcb_queue_t queue)
+
+static inline bool_t PURE higher_than_tcb_prio(tcb_t *tcb, prio_t priority)
 {
-    /* start at the back of the queue as FIFO is the common case */
-    tcb_t *before = queue.end;
-    tcb_t *after = NULL;
-
-    /* find a place to put the tcb */
-    while (unlikely(before != NULL && tcb->tcbPriority > before->tcbPriority)) {
-        after = before;
-        before = after->tcbEPPrev;
-    }
-
-    if (unlikely(before == NULL)) {
-        /* insert at head */
-        queue.head = tcb;
-    } else {
-        before->tcbEPNext = tcb;
-    }
-
-    if (likely(after == NULL)) {
-        /* insert at tail */
-        queue.end = tcb;
-    } else {
-        after->tcbEPPrev = tcb;
-    }
-
-    tcb->tcbEPNext = after;
-    tcb->tcbEPPrev = before;
-
-    return queue;
+    return tcb != NULL && priority > tcb->tcbPriority;
 }
 
-tcb_queue_t tcbEPDequeue(tcb_t *tcb, tcb_queue_t queue);
+/* Find the rightmost TCB in the given queue that has a priority which is
+   strictly greater than the given priority */
+static tcb_t *find_tcb_with_higher_prio(tcb_queue_t queue, prio_t priority)
+{
+    tcb_t *tcb = queue.end;
+
+    while (higher_than_tcb_prio(tcb, priority)) {
+        tcb = tcb->tcbSchedPrev;
+    }
+
+    return tcb;
+}
+
+/* Insert a TCB into a queue immediately after another item in the queue
+   (the queue must initially contain at least two items) */
+static inline void tcb_queue_insert_after(tcb_t *tcb, tcb_t *before)
+{
+    tcb_t *after;
+    after = before->tcbSchedNext;
+
+    tcb->tcbSchedPrev = before;
+    tcb->tcbSchedNext = after;
+
+    after->tcbSchedPrev = tcb;
+    before->tcbSchedNext = tcb;
+}
+
+/* Add TCB into the priority ordered endpoint or notification queue */
+static inline tcb_queue_t tcbAppend(tcb_t *tcb, tcb_queue_t queue)
+{
+    prio_t priority = tcb->tcbPriority;
+    tcb_queue_t new_queue = queue;
+
+    if (tcb_queue_empty(queue) || priority > queue.head->tcbPriority) {
+        new_queue = tcb_queue_prepend(queue, tcb);
+    } else {
+        if (queue.end->tcbPriority >= priority) {
+            new_queue = tcb_queue_append(queue, tcb);
+        } else {
+            tcb_t *before;
+            before = find_tcb_with_higher_prio(queue, priority);
+            tcb_queue_insert_after(tcb, before);
+        }
+    }
+
+    return new_queue;
+}
 
 #else
+
 tcb_queue_t tcbEPAppend(tcb_t *tcb, tcb_queue_t queue);
 tcb_queue_t tcbEPDequeue(tcb_t *tcb, tcb_queue_t queue);
 
diff --git a/src/fastpath/fastpath.c b/src/fastpath/fastpath.c
index 681d79cbc..1ded55b6e 100644
--- a/src/fastpath/fastpath.c
+++ b/src/fastpath/fastpath.c
@@ -465,7 +465,7 @@ void NORETURN fastpath_reply_recv(word_t cptr, word_t msgInfo)
     } else {
 #ifdef CONFIG_KERNEL_MCS
         /* Update queue. */
-        tcb_queue_t queue = tcbEPAppend(NODE_STATE(ksCurThread), ep_ptr_get_queue(ep_ptr));
+        tcb_queue_t queue = tcbAppend(NODE_STATE(ksCurThread), ep_ptr_get_queue(ep_ptr));
         endpoint_ptr_set_epQueue_head_np(ep_ptr, TCB_REF(queue.head));
         endpoint_ptr_mset_epQueue_tail_state(ep_ptr, TCB_REF(queue.end), EPState_Recv);
 #else
diff --git a/src/object/endpoint.c b/src/object/endpoint.c
index d0221a8a6..6e34398eb 100644
--- a/src/object/endpoint.c
+++ b/src/object/endpoint.c
@@ -28,7 +28,6 @@ void sendIPC(bool_t blocking, bool_t do_call, word_t badge,
     case EPState_Idle:
     case EPState_Send:
         if (blocking) {
-            tcb_queue_t queue;
 
             /* Set thread state to BlockedOnSend */
             thread_state_ptr_set_tsType(&thread->tcbState,
@@ -47,10 +46,16 @@ void sendIPC(bool_t blocking, bool_t do_call, word_t badge,
             scheduleTCB(thread);
 
             /* Place calling thread in endpoint queue */
+#ifdef CONFIG_KERNEL_MCS
+            tcbEPAppend(thread, epptr, EPState_Send);
+#else
+            tcb_queue_t queue;
             queue = ep_ptr_get_queue(epptr);
             queue = tcbEPAppend(thread, queue);
             endpoint_ptr_set_state(epptr, EPState_Send);
             ep_ptr_set_queue(epptr, queue);
+#endif /* CONFIG_KERNEL_MCS */
+
         }
         break;
 
@@ -66,12 +71,16 @@ void sendIPC(bool_t blocking, bool_t do_call, word_t badge,
         assert(dest);
 
         /* Dequeue the first TCB */
+#ifdef CONFIG_KERNEL_MCS
+        tcbEPDequeue(dest, epptr);
+#else
         queue = tcbEPDequeue(dest, queue);
         ep_ptr_set_queue(epptr, queue);
 
         if (!queue.head) {
             endpoint_ptr_set_state(epptr, EPState_Idle);
         }
+#endif /* CONFIG_KERNEL_MCS */
 
         /* Do the transfer */
         doIPCTransfer(thread, epptr, badge, canGrant, dest);
@@ -165,7 +174,6 @@ void receiveIPC(tcb_t *thread, cap_t cap, bool_t isBlocking)
         switch (endpoint_ptr_get_state(epptr)) {
         case EPState_Idle:
         case EPState_Recv: {
-            tcb_queue_t queue;
 
             if (isBlocking) {
                 /* Set thread state to BlockedOnReceive */
@@ -187,10 +195,15 @@ void receiveIPC(tcb_t *thread, cap_t cap, bool_t isBlocking)
 #endif
 
                 /* Place calling thread in endpoint queue */
+#ifdef CONFIG_KERNEL_MCS
+                tcbEPAppend(thread, epptr, EPState_Recv);
+#else
+                tcb_queue_t queue;
                 queue = ep_ptr_get_queue(epptr);
                 queue = tcbEPAppend(thread, queue);
                 endpoint_ptr_set_state(epptr, EPState_Recv);
                 ep_ptr_set_queue(epptr, queue);
+#endif /* CONFIG_KERNEL_MCS */
             } else {
                 doNBRecvFailedTransfer(thread);
             }
@@ -213,12 +226,16 @@ void receiveIPC(tcb_t *thread, cap_t cap, bool_t isBlocking)
             assert(sender);
 
             /* Dequeue the first TCB */
+#ifdef CONFIG_KERNEL_MCS
+            tcbEPDequeue(sender, epptr);
+#else
             queue = tcbEPDequeue(sender, queue);
             ep_ptr_set_queue(epptr, queue);
 
             if (!queue.head) {
                 endpoint_ptr_set_state(epptr, EPState_Idle);
             }
+#endif /* CONFIG_KERNEL_MCS */
 
             /* Get sender IPC details */
             badge = thread_state_ptr_get_blockingIPCBadge(&sender->tcbState);
@@ -321,7 +338,6 @@ void cancelIPC(tcb_t *tptr)
     case ThreadState_BlockedOnReceive: {
         /* blockedIPCCancel state */
         endpoint_t *epptr;
-        tcb_queue_t queue;
 
         epptr = EP_PTR(thread_state_ptr_get_blockingObject(state));
 
@@ -329,6 +345,10 @@ void cancelIPC(tcb_t *tptr)
         assert(endpoint_ptr_get_state(epptr) != EPState_Idle);
 
         /* Dequeue TCB */
+#ifdef CONFIG_KERNEL_MCS
+        tcbEPDequeue(tptr, epptr);
+#else
+        tcb_queue_t queue;
         queue = ep_ptr_get_queue(epptr);
         queue = tcbEPDequeue(tptr, queue);
         ep_ptr_set_queue(epptr, queue);
@@ -336,6 +356,7 @@ void cancelIPC(tcb_t *tptr)
         if (!queue.head) {
             endpoint_ptr_set_state(epptr, EPState_Idle);
         }
+#endif /* CONFIG_KERNEL_MCS */
 
 #ifdef CONFIG_KERNEL_MCS
         if (thread_state_ptr_get_tsType(state) == ThreadState_BlockedOnReceive) {
@@ -399,6 +420,18 @@ static inline void restart_thread_if_no_fault(tcb_t *thread)
         setThreadState(thread, ThreadState_Inactive);
     }
 }
+
+static inline void removeAndRestartEPQueuedThread(tcb_t *thread, endpoint_t *epptr)
+{
+    tcbEPDequeue(thread, epptr);
+    if (thread_state_get_tsType(thread->tcbState) == ThreadState_BlockedOnReceive) {
+        reply_t *reply = REPLY_PTR(thread_state_get_replyObject(thread->tcbState));
+        if (reply != NULL) {
+            reply_unlink(reply, thread);
+        }
+    }
+    restart_thread_if_no_fault(thread);
+}
 #endif
 
 void cancelAllIPC(endpoint_t *epptr)
@@ -408,6 +441,18 @@ void cancelAllIPC(endpoint_t *epptr)
         break;
 
     default: {
+        /* Clear the queue and set all blocked threads to restart */
+#ifdef CONFIG_KERNEL_MCS
+        tcb_queue_t queue;
+        tcb_t *thread, *next;
+
+        queue = ep_ptr_get_queue(epptr);
+
+        for (thread = queue.head; thread; thread = next) {
+            next = thread->tcbSchedNext;
+            removeAndRestartEPQueuedThread(thread, epptr);
+        }
+#else
         tcb_t *thread = TCB_PTR(endpoint_ptr_get_epQueue_head(epptr));
 
         /* Make endpoint idle */
@@ -415,21 +460,11 @@ void cancelAllIPC(endpoint_t *epptr)
         endpoint_ptr_set_epQueue_head(epptr, 0);
         endpoint_ptr_set_epQueue_tail(epptr, 0);
 
-        /* Set all blocked threads to restart */
         for (; thread; thread = thread->tcbEPNext) {
-#ifdef CONFIG_KERNEL_MCS
-            if (thread_state_get_tsType(thread->tcbState) == ThreadState_BlockedOnReceive) {
-                reply_t *reply = REPLY_PTR(thread_state_get_replyObject(thread->tcbState));
-                if (reply != NULL) {
-                    reply_unlink(reply, thread);
-                }
-            }
-            restart_thread_if_no_fault(thread);
-#else
             setThreadState(thread, ThreadState_Restart);
             SCHED_ENQUEUE(thread);
-#endif
         }
+#endif
 
         rescheduleRequired();
         break;
@@ -437,6 +472,20 @@ void cancelAllIPC(endpoint_t *epptr)
     }
 }
 
+#ifdef CONFIG_KERNEL_MCS
+static inline void removeAndRestartBadgedThread(tcb_t *thread, endpoint_t *epptr, word_t badge)
+{
+    word_t b = thread_state_ptr_get_blockingIPCBadge(&thread->tcbState);
+
+    /* senders do not have reply objects in their state, and we are only cancelling sends */
+    assert(thread_state_get_tsType(thread->tcbState) == ThreadState_BlockedOnSend);
+    if (b == badge) {
+        tcbEPDequeue(thread, epptr);
+        restart_thread_if_no_fault(thread);
+    }
+}
+#endif
+
 void cancelBadgedSends(endpoint_t *epptr, word_t badge)
 {
     switch (endpoint_ptr_get_state(epptr)) {
@@ -448,6 +497,12 @@ void cancelBadgedSends(endpoint_t *epptr, word_t badge)
         tcb_t *thread, *next;
         tcb_queue_t queue = ep_ptr_get_queue(epptr);
 
+#ifdef CONFIG_KERNEL_MCS
+        for (thread = queue.head; thread; thread = next) {
+            next = thread->tcbSchedNext;
+            removeAndRestartBadgedThread(thread, epptr, badge);
+        }
+#else
         /* this is a de-optimisation for verification
          * reasons. it allows the contents of the endpoint
          * queue to be ignored during the for loop. */
@@ -459,27 +514,20 @@ void cancelBadgedSends(endpoint_t *epptr, word_t badge)
             word_t b = thread_state_ptr_get_blockingIPCBadge(
                            &thread->tcbState);
             next = thread->tcbEPNext;
-#ifdef CONFIG_KERNEL_MCS
-            /* senders do not have reply objects in their state, and we are only cancelling sends */
-            assert(thread_state_get_tsType(thread->tcbState) == ThreadState_BlockedOnSend);
-            if (b == badge) {
-                restart_thread_if_no_fault(thread);
-                queue = tcbEPDequeue(thread, queue);
-            }
-#else
+
             if (b == badge) {
                 setThreadState(thread, ThreadState_Restart);
                 SCHED_ENQUEUE(thread);
                 queue = tcbEPDequeue(thread, queue);
             }
-#endif
+
         }
         ep_ptr_set_queue(epptr, queue);
 
         if (queue.head) {
             endpoint_ptr_set_state(epptr, EPState_Send);
         }
-
+#endif /* CONFIG_KERNEL_MCS */
         rescheduleRequired();
 
         break;
@@ -491,11 +539,39 @@ void cancelBadgedSends(endpoint_t *epptr, word_t badge)
 }
 
 #ifdef CONFIG_KERNEL_MCS
+void tcbEPAppend(tcb_t *thread, endpoint_t *epptr, endpoint_state_t ep_state)
+{
+    tcb_queue_t queue;
+    tcb_queue_t new_queue;
+
+    queue = ep_ptr_get_queue(epptr);
+    new_queue = tcbAppend(thread, queue);
+    ep_ptr_set_queue(epptr, new_queue);
+
+    /* Update the state of the endpoint with the state that was passed in. If the queue
+     * was previously non-empty this must be the same state the endpoint is currently in. */
+    endpoint_ptr_set_state(epptr, ep_state);
+}
+
+void tcbEPDequeue(tcb_t *thread, endpoint_t *epptr)
+{
+    tcb_queue_t queue;
+    tcb_queue_t new_queue;
+
+    queue = ep_ptr_get_queue(epptr);
+    new_queue = tcb_queue_remove(queue, thread);
+    ep_ptr_set_queue(epptr, new_queue);
+
+    if (tcb_queue_empty(new_queue)) {
+        endpoint_ptr_set_state(epptr, EPState_Idle);
+    }
+}
+
 void reorderEP(endpoint_t *epptr, tcb_t *thread)
 {
     tcb_queue_t queue = ep_ptr_get_queue(epptr);
-    queue = tcbEPDequeue(thread, queue);
-    queue = tcbEPAppend(thread, queue);
+    queue = tcb_queue_remove(queue, thread);
+    queue = tcbAppend(thread, queue);
     ep_ptr_set_queue(epptr, queue);
 }
 #endif
diff --git a/src/object/notification.c b/src/object/notification.c
index 30520319f..cf3a6a3c0 100644
--- a/src/object/notification.c
+++ b/src/object/notification.c
@@ -145,6 +145,9 @@ void sendSignal(notification_t *ntfnPtr, word_t badge)
         assert(dest);
 
         /* Dequeue TCB */
+#ifdef CONFIG_KERNEL_MCS
+        tcbNTFNDequeue(dest, ntfnPtr);
+#else
         ntfn_queue = tcbEPDequeue(dest, ntfn_queue);
         ntfn_ptr_set_queue(ntfnPtr, ntfn_queue);
 
@@ -152,6 +155,7 @@ void sendSignal(notification_t *ntfnPtr, word_t badge)
         if (!ntfn_queue.head) {
             notification_ptr_set_state(ntfnPtr, NtfnState_Idle);
         }
+#endif /* CONFIG_KERNEL_MCS */
 
         setThreadState(dest, ThreadState_Running);
         setRegister(dest, badgeRegister, badge);
@@ -196,7 +200,6 @@ void receiveSignal(tcb_t *thread, cap_t cap, bool_t isBlocking)
     switch (notification_ptr_get_state(ntfnPtr)) {
     case NtfnState_Idle:
     case NtfnState_Waiting: {
-        tcb_queue_t ntfn_queue;
 
         if (isBlocking) {
             /* Block thread on notification object */
@@ -207,11 +210,16 @@ void receiveSignal(tcb_t *thread, cap_t cap, bool_t isBlocking)
             scheduleTCB(thread);
 
             /* Enqueue TCB */
+#ifdef CONFIG_KERNEL_MCS
+            tcbNTFNAppend(thread, ntfnPtr);
+#else
+            tcb_queue_t ntfn_queue;
             ntfn_queue = ntfn_ptr_get_queue(ntfnPtr);
             ntfn_queue = tcbEPAppend(thread, ntfn_queue);
 
             notification_ptr_set_state(ntfnPtr, NtfnState_Waiting);
             ntfn_ptr_set_queue(ntfnPtr, ntfn_queue);
+#endif /* CONFIG_KERNEL_MCS */
 
 #ifdef CONFIG_KERNEL_MCS
             maybeReturnSchedContext(ntfnPtr, thread);
@@ -240,47 +248,66 @@ void receiveSignal(tcb_t *thread, cap_t cap, bool_t isBlocking)
     }
 }
 
+#ifdef CONFIG_KERNEL_MCS
+static inline void removeAndRestartNTFNQueuedThread(tcb_t *thread, notification_t *ntfnPtr)
+{
+    tcbNTFNDequeue(thread, ntfnPtr);
+    setThreadState(thread, ThreadState_Restart);
+    if (sc_sporadic(thread->tcbSchedContext)) {
+        /* We know that the thread can't have the current SC as its own SC at
+         * this point as it should still be associated with the current thread,
+         * or no thread. This check is added here to reduce the cost of proving
+         * this to be true as a short-term stop-gap. */
+        assert(thread->tcbSchedContext != NODE_STATE(ksCurSC));
+        if (thread->tcbSchedContext != NODE_STATE(ksCurSC)) {
+            refill_unblock_check(thread->tcbSchedContext);
+        }
+    }
+    possibleSwitchTo(thread);
+}
+#endif
+
 void cancelAllSignals(notification_t *ntfnPtr)
 {
     if (notification_ptr_get_state(ntfnPtr) == NtfnState_Waiting) {
+        /* Clear the queue and set all blocked threads to Restart */
+#ifdef CONFIG_KERNEL_MCS
+        tcb_queue_t queue;
+        tcb_t *thread, *next;
+
+        queue = ntfn_ptr_get_queue(ntfnPtr);
+
+        for (thread = queue.head; thread; thread = next) {
+            next = thread->tcbSchedNext;
+            removeAndRestartNTFNQueuedThread(thread, ntfnPtr);
+        }
+#else
         tcb_t *thread = TCB_PTR(notification_ptr_get_ntfnQueue_head(ntfnPtr));
 
         notification_ptr_set_state(ntfnPtr, NtfnState_Idle);
         notification_ptr_set_ntfnQueue_head(ntfnPtr, 0);
         notification_ptr_set_ntfnQueue_tail(ntfnPtr, 0);
 
-        /* Set all waiting threads to Restart */
         for (; thread; thread = thread->tcbEPNext) {
             setThreadState(thread, ThreadState_Restart);
-#ifdef CONFIG_KERNEL_MCS
-            if (sc_sporadic(thread->tcbSchedContext)) {
-                /* We know that the thread can't have the current SC
-                 * as its own SC as this point as it should still be
-                 * associated with the current thread, or no thread.
-                 * This check is added here to reduce the cost of
-                 * proving this to be true as a short-term stop-gap. */
-                assert(thread->tcbSchedContext != NODE_STATE(ksCurSC));
-                if (thread->tcbSchedContext != NODE_STATE(ksCurSC)) {
-                    refill_unblock_check(thread->tcbSchedContext);
-                }
-            }
-            possibleSwitchTo(thread);
-#else
             SCHED_ENQUEUE(thread);
-#endif
         }
+#endif /* CONFIG_KERNEL_MCS */
         rescheduleRequired();
     }
 }
 
 void cancelSignal(tcb_t *threadPtr, notification_t *ntfnPtr)
 {
-    tcb_queue_t ntfn_queue;
 
     /* Haskell error "cancelSignal: notification object must be in a waiting" state */
     assert(notification_ptr_get_state(ntfnPtr) == NtfnState_Waiting);
 
     /* Dequeue TCB */
+#ifdef CONFIG_KERNEL_MCS
+    tcbNTFNDequeue(threadPtr, ntfnPtr);
+#else
+    tcb_queue_t ntfn_queue;
     ntfn_queue = ntfn_ptr_get_queue(ntfnPtr);
     ntfn_queue = tcbEPDequeue(threadPtr, ntfn_queue);
     ntfn_ptr_set_queue(ntfnPtr, ntfn_queue);
@@ -289,6 +316,7 @@ void cancelSignal(tcb_t *threadPtr, notification_t *ntfnPtr)
     if (!ntfn_queue.head) {
         notification_ptr_set_state(ntfnPtr, NtfnState_Idle);
     }
+#endif /* CONFIG_KERNEL_MCS */
 
     /* Make thread inactive */
     setThreadState(threadPtr, ThreadState_Inactive);
@@ -356,11 +384,36 @@ void bindNotification(tcb_t *tcb, notification_t *ntfnPtr)
 }
 
 #ifdef CONFIG_KERNEL_MCS
+void tcbNTFNAppend(tcb_t *thread, notification_t *ntfnPtr)
+{
+    tcb_queue_t queue;
+    tcb_queue_t new_queue;
+
+    queue = ntfn_ptr_get_queue(ntfnPtr);
+    new_queue = tcbAppend(thread, queue);
+    ntfn_ptr_set_queue(ntfnPtr, new_queue);
+    notification_ptr_set_state(ntfnPtr, NtfnState_Waiting);
+}
+
+void tcbNTFNDequeue(tcb_t *thread, notification_t *ntfnPtr)
+{
+    tcb_queue_t queue;
+    tcb_queue_t new_queue;
+
+    queue = ntfn_ptr_get_queue(ntfnPtr);
+    new_queue = tcb_queue_remove(queue, thread);
+    ntfn_ptr_set_queue(ntfnPtr, new_queue);
+
+    if (tcb_queue_empty(new_queue)) {
+        notification_ptr_set_state(ntfnPtr, NtfnState_Idle);
+    }
+}
+
 void reorderNTFN(notification_t *ntfnPtr, tcb_t *thread)
 {
     tcb_queue_t queue = ntfn_ptr_get_queue(ntfnPtr);
-    queue = tcbEPDequeue(thread, queue);
-    queue = tcbEPAppend(thread, queue);
+    queue = tcb_queue_remove(queue, thread);
+    queue = tcbAppend(thread, queue);
     ntfn_ptr_set_queue(ntfnPtr, queue);
 }
 #endif
diff --git a/src/object/tcb.c b/src/object/tcb.c
index 534cc20f4..a7efb0d27 100644
--- a/src/object/tcb.c
+++ b/src/object/tcb.c
@@ -249,7 +249,6 @@ tcb_queue_t tcbEPAppend(tcb_t *tcb, tcb_queue_t queue)
 
     return queue;
 }
-#endif
 
 /* Remove TCB from an endpoint queue */
 tcb_queue_t tcbEPDequeue(tcb_t *tcb, tcb_queue_t queue)
@@ -268,6 +267,7 @@ tcb_queue_t tcbEPDequeue(tcb_t *tcb, tcb_queue_t queue)
 
     return queue;
 }
+#endif /* CONFIG_KERNEL_MCS */
 
 #ifdef CONFIG_KERNEL_MCS