update the tcb enqueue, add the mcs ep append and handle unknown syscall

kernel/src/arch/aarch64/exception.rs

@@ -1,3 +1,5 @@
+use core::intrinsics::likely;
+
 use crate::arch::aarch64::consts::ARMDataAbort;
 use crate::arch::aarch64::consts::ARMPrefetchAbort;
 use crate::compatibility::lookupIPCBuffer;
@@ -25,6 +27,8 @@ use sel4_common::structures_gen::seL4_Fault_UnknownSyscall;
 use sel4_common::structures_gen::seL4_Fault_UserException;
 use sel4_common::structures_gen::seL4_Fault_VMFault;
 use sel4_common::utils::global_read;
+#[cfg(feature="KERNEL_MCS")]
+use sel4_task::{checkBudgetRestart,updateTimestamp};
 use sel4_task::{activateThread, get_currenct_thread, get_current_domain, schedule};
 
 use super::instruction::*;
@@ -90,10 +94,21 @@ pub fn handleUnknownSyscall(w: isize) -> exception_t {
         thread.tcbArch.set_register(Cap, current);
         return exception_t::EXCEPTION_NONE;
     }
+	#[cfg(not(feature="KERNEL_MCS"))]
     unsafe {
         current_fault = seL4_Fault_UnknownSyscall::new(w as u64).unsplay();
         handle_fault(get_currenct_thread());
     }
+	#[cfg(feature="KERNEL_MCS")]
+	{
+		updateTimestamp();
+		if likely(checkBudgetRestart()){
+			unsafe {
+				current_fault = seL4_Fault_UnknownSyscall::new(w as u64).unsplay();
+				handle_fault(get_currenct_thread());
+			}
+		}
+	}
     schedule();
     activateThread();
     exception_t::EXCEPTION_NONE

kernel/src/kernel/fastpath.rs

@@ -549,7 +549,7 @@ pub fn fastpath_reply_recv(cptr: usize, msgInfo: usize, reply: usize) {
         let mut queue = ep.get_queue();
         queue.ep_append(current);
         ep.set_epQueue_head(queue.head as u64);
-        endpoint_ptr_mset_epQueue_tail_state(ep as *mut endpoint, queue.head, EPState_Recv);
+        endpoint_ptr_mset_epQueue_tail_state(ep as *mut endpoint, queue.tail, EPState_Recv);
     } else {
         current.tcbEPPrev = 0;
         current.tcbEPNext = 0;

kernel/src/syscall/invocation/decode/mod.rs

@@ -47,6 +47,7 @@ pub fn decode_invocation(
     call: bool,
     buffer: &seL4_IPCBuffer,
 ) -> exception_t {
+	// sel4_common::println!("decode invocation {}", capability.get_tag());
     match capability.clone().splay() {
         cap_Splayed::null_cap(_) | cap_Splayed::zombie_cap(_) => {
             debug!(

sel4_ipc/src/endpoint.rs

@@ -305,7 +305,6 @@ impl endpoint_func for endpoint {
         can_grant_reply: bool,
         canDonate: bool,
     ) {
-        // sel4_common::println!("send ipc {}",self.get_ep_state() as usize);
         match self.get_ep_state() {
             EPState::Idle | EPState::Send => {
                 if blocking {
@@ -452,7 +451,6 @@ impl endpoint_func for endpoint {
         use sel4_common::structures_gen::{cap_tag::cap_reply_cap, notification_t, seL4_Fault_tag};
 
         use crate::notification_func;
-
         let mut replyptr: usize = 0;
         if reply_cap.clone().unsplay().get_tag() == cap_reply_cap {
             replyptr = reply_cap.get_capReplyPtr() as usize;
@@ -472,7 +470,7 @@ impl endpoint_func for endpoint {
         match self.get_ep_state() {
             EPState::Idle | EPState::Recv => {
                 if is_blocking {
-                    set_thread_state(thread, ThreadState::ThreadStateBlockedOnReceive);
+					thread.tcbState.set_tsType(ThreadState::ThreadStateBlockedOnReceive as u64);
                     thread.tcbState.set_blockingObject(self.get_ptr() as u64);
                     // MCS
                     thread.tcbState.set_replyObject(replyptr as u64);

sel4_task/src/tcb.rs

@@ -1,6 +1,6 @@
 #[cfg(feature = "KERNEL_MCS")]
 use crate::ksCurSC;
-use crate::prio_t;
+use crate::{ksReadyQueues, prio_t};
 use crate::tcb_queue::tcb_queue_t;
 #[cfg(feature = "KERNEL_MCS")]
 use crate::{ksReleaseHead, sched_context::sched_context_t};
@@ -279,14 +279,15 @@ impl tcb_t {
             let idx = ready_queues_index(dom, prio);
             let queue = self.get_sched_queue(idx);
             if queue.tail == 0 {
-                queue.head = self_ptr as usize;
+                queue.tail = self_ptr as usize;
                 addToBitmap(self.get_cpu(), dom, prio);
             } else {
-                convert_to_mut_type_ref::<tcb_t>(queue.tail).tcbSchedNext = self_ptr as usize;
+                convert_to_mut_type_ref::<tcb_t>(queue.head).tcbSchedPrev = self_ptr as usize;
             }
-            self.tcbSchedPrev = queue.tail;
-            self.tcbSchedNext = 0;
-            queue.tail = self_ptr as usize;
+            self.tcbSchedPrev = 0;
+            self.tcbSchedNext = queue.head;
+            queue.head = self_ptr as usize;
+			unsafe { ksReadyQueues[idx] = *queue; }
             self.tcbState.set_tcbQueued(1);
         }
 
@@ -346,7 +347,7 @@ impl tcb_t {
             } else {
                 queue.tail = self.tcbSchedPrev;
             }
-            // unsafe { ksReadyQueues[idx] = queue; }
+            unsafe { ksReadyQueues[idx] = *queue; }
             self.tcbState.set_tcbQueued(0);
         }
     }
@@ -384,7 +385,7 @@ impl tcb_t {
             self.tcbSchedPrev = queue.tail;
             self.tcbSchedNext = 0;
             queue.tail = self_ptr as usize;
-            // unsafe { ksReadyQueues[idx] = queue; }
+            unsafe { ksReadyQueues[idx] = *queue; }
 
             self.tcbState.set_tcbQueued(1);
         }

sel4_task/src/tcb_queue.rs

@@ -14,6 +14,7 @@ pub struct tcb_queue_t {
 
 impl tcb_queue_t {
     /// Append a tcb to the queue
+	#[cfg(not(feature="KERNEL_MCS"))]
     pub fn ep_append(&mut self, tcb: &mut tcb_t) {
         if self.head == 0 {
             self.head = tcb.get_ptr();
@@ -25,6 +26,36 @@ impl tcb_queue_t {
         tcb.tcbEPNext = 0;
         self.tail = tcb.get_ptr();
     }
+	#[cfg(feature="KERNEL_MCS")]
+    pub fn ep_append(&mut self, tcb: &mut tcb_t) {
+        use core::intrinsics::{likely, unlikely};
+
+		let mut before_ptr:usize = self.tail;
+		let mut after_ptr:usize = 0;
+
+
+		while unlikely(before_ptr != 0 && tcb.tcbPriority > convert_to_mut_type_ref::<tcb_t>(before_ptr).tcbPriority){
+			after_ptr= before_ptr;
+			before_ptr = convert_to_mut_type_ref::<tcb_t>(after_ptr).tcbEPPrev;
+		}
+        if unlikely( before_ptr == 0 ){
+			self.head = tcb.get_ptr();
+		}
+		else{
+			convert_to_mut_type_ref::<tcb_t>(before_ptr).tcbEPNext = tcb.get_ptr()
+		}
+
+
+		if likely(after_ptr == 0){
+			self.tail = tcb.get_ptr();
+		}
+		else {
+			convert_to_mut_type_ref::<tcb_t>(after_ptr).tcbEPPrev = tcb.get_ptr();
+		}
+
+		tcb.tcbEPNext = after_ptr;
+		tcb.tcbEPPrev = before_ptr;
+    }
 
     /// Dequeue a tcb from the queue
     pub fn ep_dequeue(&mut self, tcb: &mut tcb_t) {