add the reply

kernel/src/kernel/fastpath.rs

@@ -4,9 +4,13 @@ use crate::{
     syscall::{slowpath, SysCall, SysReplyRecv},
 };
 use core::intrinsics::{likely, unlikely};
+#[cfg(feature = "KERNEL_MCS")]
+use sched_context::sched_context_t;
 use sel4_common::arch::msgRegister;
 use sel4_common::message_info::seL4_MessageInfo_func;
 use sel4_common::shared_types_bf_gen::seL4_MessageInfo;
+#[cfg(feature = "KERNEL_MCS")]
+use sel4_common::structures_gen::call_stack;
 use sel4_common::structures_gen::{
     cap, cap_cnode_cap, cap_null_cap, cap_page_table_cap, cap_reply_cap, cap_tag, endpoint,
     mdb_node, notification, seL4_Fault_tag, thread_state,
@@ -17,6 +21,8 @@ use sel4_common::{
 };
 use sel4_cspace::interface::*;
 use sel4_ipc::*;
+#[cfg(feature = "KERNEL_MCS")]
+use sel4_task::reply::reply_t;
 use sel4_task::*;
 use sel4_vspace::*;
 
@@ -239,16 +245,18 @@ pub fn fastpath_call(cptr: usize, msgInfo: usize) {
     if unlikely((ep_cap.get_capCanGrant() == 0) && (ep_cap.get_capCanGrantReply() == 0)) {
         slowpath(SysCall as usize);
     }
-    // #ifdef CONFIG_KERNEL_MCS
-    //     if (unlikely(dest->tcbSchedContext != NULL)) {
-    //         slowpath(SysCall);
-    //     }
-
-    //     reply_t *reply = thread_state_get_replyObject_np(dest->tcbState);
-    //     if (unlikely(reply == NULL)) {
-    //         slowpath(SysCall);
-    //     }
-    // #endif
+    #[cfg(feature = "KERNEL_MCS")]
+    {
+        if unlikely(dest.tcbSchedContext != 0) {
+            slowpath(SysCall as usize);
+        }
+        assert!(dest.tcbState.get_tcbQueued() == 0);
+        assert!(dest.tcbState.get_tcbInReleaseQueue() == 0);
+        let reply = dest.tcbState.get_replyObject();
+        if unlikely(reply == 0) {
+            slowpath(SysCall as usize);
+        }
+    }
     #[cfg(feature = "ENABLE_SMP")]
     if unlikely(get_currenct_thread().tcbAffinity != dest.tcbAffinity) {
         slowpath(SysCall as usize);
@@ -268,25 +276,31 @@ pub fn fastpath_call(cptr: usize, msgInfo: usize) {
 
     #[cfg(feature = "KERNEL_MCS")]
     {
-        // TODO: MCS
-        // #ifdef CONFIG_KERNEL_MCS
-        //     thread_state_ptr_set_replyObject_np(&dest->tcbState, 0);
-        //     thread_state_ptr_set_replyObject_np(&NODE_STATE(ksCurThread)->tcbState, REPLY_REF(reply));
-        //     reply->replyTCB = NODE_STATE(ksCurThread);
-
-        //     sched_context_t *sc = NODE_STATE(ksCurThread)->tcbSchedContext;
-        //     sc->scTcb = dest;
-        //     dest->tcbSchedContext = sc;
-        //     NODE_STATE(ksCurThread)->tcbSchedContext = NULL;
-
-        //     reply_t *old_caller = sc->scReply;
-        //     reply->replyPrev = call_stack_new(REPLY_REF(sc->scReply), false);
-        //     if (unlikely(old_caller)) {
-        //         old_caller->replyNext = call_stack_new(REPLY_REF(reply), false);
-        //     }
-        //     reply->replyNext = call_stack_new(SC_REF(sc), true);
-        //     sc->scReply = reply;
-        // #endif
+        let reply = dest.tcbState.get_replyObject();
+        assert!(dest.tcbState.get_tcbQueued() == 0);
+        assert!(dest.tcbState.get_tcbInReleaseQueue() == 0);
+        dest.tcbState.set_replyObject(0);
+
+        assert!(current.tcbState.get_tcbQueued() == 0);
+        assert!(current.tcbState.get_tcbInReleaseQueue() == 0);
+        current.tcbState.set_replyObject(reply);
+
+        convert_to_mut_type_ref::<reply_t>(reply as usize).replyTCB = unsafe { ksCurThread };
+
+        let sc = convert_to_mut_type_ref::<sched_context_t>(current.tcbSchedContext);
+        sc.scTcb = dest.get_ptr();
+        dest.tcbSchedContext = sc.get_ptr();
+        current.tcbSchedContext = 0;
+
+        let old_caller = convert_to_mut_type_ref::<reply_t>(sc.scReply);
+        convert_to_mut_type_ref::<reply_t>(reply as usize).replyPrev =
+            call_stack::new(sc.scReply as u64, 0);
+        if unlikely(old_caller.get_ptr() != 0) {
+            old_caller.replyNext = call_stack::new(reply, 0);
+        }
+        convert_to_mut_type_ref::<reply_t>(reply as usize).replyNext =
+            call_stack::new(sc.get_ptr() as u64, 1);
+        sc.scReply = reply as usize;
     }
     #[cfg(not(feature = "KERNEL_MCS"))]
     {
@@ -432,9 +446,6 @@ pub fn fastpath_reply_recv(cptr: usize, msgInfo: usize) {
 pub fn fastpath_reply_recv(cptr: usize, msgInfo: usize, reply: usize) {
     // debug!("enter fastpath_reply_recv");
 
-    use sel4_common::reply::reply_t;
-    use sel4_ipc::endpoint_func;
-    use sel4_task::sched_context::sched_context_t;
     let current = get_currenct_thread();
     let mut info = seL4_MessageInfo::from_word(msgInfo);
     let length = info.get_length() as usize;
@@ -479,20 +490,6 @@ pub fn fastpath_reply_recv(cptr: usize, msgInfo: usize, reply: usize) {
     if unlikely(ep.get_ep_state() == EPState::Send) {
         slowpath(SysReplyRecv as usize);
     }
-    // #ifdef CONFIG_KERNEL_MCS
-    //     /* Get the reply address */
-    //     reply_t *reply_ptr = REPLY_PTR(cap_reply_cap_get_capReplyPtr(reply_cap));
-    //     /* check that its valid and at the head of the call chain
-    //        and that the current thread's SC is going to be donated. */
-    //     if (unlikely(reply_ptr->replyTCB == NULL ||
-    //                  call_stack_get_isHead(reply_ptr->replyNext) == 0 ||
-    //                  SC_PTR(call_stack_get_callStackPtr(reply_ptr->replyNext)) != NODE_STATE(ksCurThread)->tcbSchedContext)) {
-    //         slowpath(SysReplyRecv);
-    //     }
-
-    //     /* Determine who the caller is. */
-    //     caller = reply_ptr->replyTCB;
-    // #endif
     /* Get the reply address */
     let reply_ptr = convert_to_mut_type_ref::<reply_t>(reply_cap.get_capReplyPtr() as usize);
     /* check that its valid and at the head of the call chain
@@ -526,18 +523,13 @@ pub fn fastpath_reply_recv(cptr: usize, msgInfo: usize, reply: usize) {
     if unlikely(caller.tcbSchedContext != 0) {
         slowpath(SysReplyRecv as usize);
     }
+    assert!(current.tcbState.get_replyObject() == 0);
 
     thread_state_ptr_mset_blockingObject_tsType(
         &mut current.tcbState,
         ep.get_ptr(),
         ThreadState::ThreadStateBlockedOnReceive as usize,
     );
-    // #ifdef CONFIG_KERNEL_MCS
-    //     /* unlink reply object from caller */
-    //     thread_state_ptr_set_replyObject_np(&caller->tcbState, 0);
-    //     /* set the reply object */
-    //     thread_state_ptr_set_replyObject_np(&NODE_STATE(ksCurThread)->tcbState, REPLY_REF(reply_ptr));
-    //     reply_ptr->replyTCB = NODE_STATE(ksCurThread);
     caller.tcbState.set_replyObject(0);
     current
         .tcbState

kernel/src/syscall/mod.rs

@@ -67,9 +67,11 @@ use sel4_common::structures_gen::{
 };
 use sel4_common::utils::{convert_to_mut_type_ref, ptr_to_mut};
 use sel4_ipc::{endpoint_func, notification_func, Transfer};
+#[cfg(feature = "KERNEL_MCS")]
+use sel4_task::mcs_preemption_point;
 use sel4_task::{
-    activateThread, get_currenct_thread, mcs_preemption_point, rescheduleRequired, schedule,
-    set_thread_state, tcb_t, ThreadState,
+    activateThread, get_currenct_thread, rescheduleRequired, schedule, set_thread_state, tcb_t,
+    ThreadState,
 };
 pub use utils::*;
 
@@ -345,11 +347,11 @@ pub fn handleTimeout(tptr: &mut tcb_t) {
 #[no_mangle]
 pub fn endTimeslice(can_timeout_fault: bool) {
     use sel4_common::structures_gen::seL4_Fault_Timeout;
-    use sel4_task::{ksCurSC, sched_context::sched_context_t};
+    use sel4_task::get_current_sc;
 
     unsafe {
         let thread = get_currenct_thread();
-        let sched_context = convert_to_mut_type_ref::<sched_context_t>(ksCurSC);
+        let sched_context = get_current_sc();
         if can_timeout_fault && !sched_context.is_round_robin() && thread.validTimeoutHandler() {
             current_fault = seL4_Fault_Timeout::new(sched_context.scBadge as u64).unsplay();
             handleTimeout(thread);

sel4_common/src/lib.rs

@@ -15,8 +15,6 @@ pub mod logging;
 pub mod message_info;
 pub mod object;
 pub mod platform;
-#[cfg(feature = "KERNEL_MCS")]
-pub mod reply;
 pub mod sel4_bitfield_types;
 pub mod sel4_config;
 pub mod shared_types_bf_gen;

sel4_ipc/src/endpoint.rs

@@ -227,6 +227,11 @@ impl endpoint_func for endpoint {
         can_grant_reply: bool,
         canDonate: bool,
     ) {
+        use sel4_common::{
+            structures_gen::seL4_Fault_tag, types_gen::seL4_Fault_tag::seL4_Fault_NullFault,
+        };
+        use sel4_task::{ksCurSC, reply::reply_t, sched_context::sched_context_t};
+
         match self.get_ep_state() {
             EPState::Idle | EPState::Send => {
                 if blocking {
@@ -265,7 +270,43 @@ impl endpoint_func for endpoint {
                 }
                 src_thread.do_ipc_transfer(dest_thread, Some(self), badge, can_grant);
 
-                // TODO: MCS
+                let reply = convert_to_mut_type_ref::<reply_t>(
+                    dest_thread.tcbState.get_replyObject() as usize,
+                );
+                if reply.get_ptr() != 0 {
+                    reply.unlink(dest_thread);
+                }
+                if do_call || src_thread.tcbFault.get_tag() != seL4_Fault_tag::seL4_Fault_NullFault
+                {
+                    if reply.get_ptr() != 0 && (can_grant || can_grant_reply) {
+                        reply.push(src_thread, dest_thread, canDonate);
+                    } else {
+                        set_thread_state(dest_thread, ThreadState::ThreadStateInactive);
+                    }
+                } else if canDonate && dest_thread.tcbSchedContext == 0 {
+                    convert_to_mut_type_ref::<sched_context_t>(src_thread.tcbSchedContext)
+                        .schedContext_donate(dest_thread);
+                }
+
+                assert!(
+                    dest_thread.tcbSchedContext == 0
+                        || convert_to_mut_type_ref::<sched_context_t>(dest_thread.tcbSchedContext)
+                            .refill_sufficient(0)
+                );
+                assert!(
+                    dest_thread.tcbSchedContext == 0
+                        || convert_to_mut_type_ref::<sched_context_t>(dest_thread.tcbSchedContext)
+                            .refill_ready()
+                );
+                set_thread_state(dest_thread, ThreadState::ThreadStateRunning);
+                if convert_to_mut_type_ref::<sched_context_t>(dest_thread.tcbSchedContext)
+                    .sc_sporadic()
+                    && dest_thread.tcbSchedContext != unsafe { ksCurSC }
+                {
+                    convert_to_mut_type_ref::<sched_context_t>(dest_thread.tcbSchedContext)
+                        .refill_unblock_check();
+                }
+                possible_switch_to(dest_thread);
             }
         }
     }

sel4_task/src/lib.rs

@@ -19,7 +19,8 @@ pub mod tcb;
 mod tcb_queue;
 mod thread_state;
 pub use ffi::*;
-pub use ffi::*;
+#[cfg(feature = "KERNEL_MCS")]
+pub mod reply;
 pub use scheduler::*;
 pub use structures::*;
 pub use tcb::*;

sel4_task/src/reply.rs

@@ -0,0 +1,36 @@
+use sel4_common::structures_gen::call_stack;
+
+use crate::{set_thread_state, tcb_t, ThreadState};
+
+pub type reply_t = reply;
+#[repr(C)]
+#[derive(Debug, Clone)]
+// TODO: MCS
+pub struct reply {
+    /// TCB pointed to by this reply object
+    pub replyTCB: usize,
+    pub replyPrev: call_stack,
+    pub replyNext: call_stack,
+    pub padding: usize,
+}
+impl reply {
+    pub fn get_ptr(&mut self) -> usize {
+        self as *const _ as usize
+    }
+    pub fn unlink(&mut self, tcb: &mut tcb_t) {
+        assert!(self.replyTCB == tcb.get_ptr());
+        assert!(tcb.tcbState.get_replyObject() as usize == self.get_ptr());
+        tcb.tcbState.set_replyObject(0);
+        self.replyTCB = 0;
+        set_thread_state(tcb, ThreadState::ThreadStateInactive);
+    }
+    pub fn push(&mut self, tcb_caller: &mut tcb_t, tcb_callee: &mut tcb_t, canDonate: bool) {
+        // TODO: MCS
+    }
+    pub fn pop(&mut self, tcb: &mut tcb_t) {
+        // TODO: MCS
+    }
+    pub fn remove(&mut self, tcb: &mut tcb_t) {
+        // TODO: MCS
+    }
+}

sel4_task/src/sched_context.rs

@@ -18,8 +18,8 @@ use sel4_common::{
 };
 
 use crate::{
-    get_currenct_thread, ksCurSC, ksCurTime, ksReprogram, ksSchedulerAction, rescheduleRequired,
-    tcb_t,
+    get_currenct_thread, get_current_sc, ksCurSC, ksCurTime, ksReprogram, ksSchedulerAction,
+    rescheduleRequired, tcb_t,
 };
 
 pub type sched_context_t = sched_context;
@@ -283,7 +283,7 @@ impl sched_context {
         to.tcbSchedContext = self.get_ptr()
     }
     pub fn schedContext_bindNtfn(&mut self, ntfn: &mut notification_t) {
-        ntfn.set_ntfnSchedContext(self as *mut _ as u64);
+        ntfn.set_ntfnSchedContext(self.get_ptr() as u64);
         self.scNotification = ntfn as *mut _ as usize;
     }
     pub fn schedContext_unbindNtfn(&mut self) {
@@ -314,7 +314,7 @@ impl sched_context {
 pub fn refill_budget_check(_usage: ticks_t) {
     unsafe {
         let mut usage = _usage;
-        let sc = convert_to_mut_type_ref::<sched_context_t>(ksCurSC);
+        let sc = get_current_sc();
         assert!(!sc.is_round_robin());
 
         while (*sc.refill_head()).rAmount <= usage && (*sc.refill_head()).rTime < MAX_RELEASE_TIME()