feat(prototyper): add queue for fence event

feat(prototyper): add `ipi_type` for different msoft type

prototyper/src/clint.rs

@@ -10,8 +10,11 @@ use rustsbi::SbiRet;
 
 use crate::hsm::remote_hsm;
 use crate::riscv_spec::{current_hartid, stimecmp};
+use crate::trap_stack::ROOT_STACK;
 
 pub(crate) static SIFIVECLINT: AtomicPtr<SifiveClint> = AtomicPtr::new(null_mut());
+pub(crate) const IPI_TYPE_SSOFT: u8 = 1 << 0;
+pub(crate) const IPI_TYPE_FENCE: u8 = 1 << 1;
 
 pub(crate) fn init(base: usize) {
     SIFIVECLINT.store(base as _, Release);
@@ -94,13 +97,63 @@ impl<'a> rustsbi::Ipi for ClintDevice<'a> {
     fn send_ipi(&self, hart_mask: rustsbi::HartMask) -> SbiRet {
         for hart_id in 0..=self.max_hart_id {
             if hart_mask.has_bit(hart_id) && remote_hsm(hart_id).unwrap().allow_ipi() {
+                let old_ipi_type = set_ipi_type(hart_id, crate::clint::IPI_TYPE_SSOFT);
+                if old_ipi_type == 0 {
+                    unsafe {
+                        (*self.clint.load(Relaxed)).set_msip(hart_id);
+                    }
+                }
+            }
+        }
+        SbiRet::success(0)
+    }
+}
+
+impl<'a> ClintDevice<'a> {
+    #[inline]
+    pub fn send_ipi_by_fence(
+        &self,
+        hart_mask: rustsbi::HartMask,
+        ctx: crate::rfence::RFenceCTX,
+    ) -> SbiRet {
+        for hart_id in 0..=self.max_hart_id {
+            if hart_mask.has_bit(hart_id) && remote_hsm(hart_id).unwrap().allow_ipi() {
+                crate::rfence::remote_rfence(hart_id).unwrap().set(ctx);
+                crate::rfence::local_rfence().add();
                 if hart_id == current_hartid() {
-                    unsafe { riscv::register::mip::set_ssoft(); }
-                } else {
-                    unsafe { (*self.clint.load(Relaxed)).set_msip(hart_id); }
+                    continue;
+                }
+                let old_ipi_type = set_ipi_type(hart_id, crate::clint::IPI_TYPE_FENCE);
+                if old_ipi_type == 0 {
+                    unsafe {
+                        (*self.clint.load(Relaxed)).set_msip(hart_id);
+                    }
                 }
             }
         }
+        while crate::rfence::local_rfence().is_zero() == false {
+            crate::trap::rfence_signle_handler();
+        }
         SbiRet::success(0)
     }
 }
+
+pub fn set_ipi_type(hart_id: usize, event_id: u8) -> u8 {
+    unsafe {
+        ROOT_STACK
+            .get_unchecked_mut(hart_id)
+            .hart_context()
+            .ipi_type
+            .fetch_or(event_id, Relaxed)
+    }
+}
+
+pub fn get_and_reset_ipi_type() -> u8 {
+    unsafe {
+        ROOT_STACK
+            .get_unchecked_mut(current_hartid())
+            .hart_context()
+            .ipi_type
+            .swap(0, Relaxed)
+    }
+}

prototyper/src/fifo.rs

@@ -0,0 +1,65 @@
+use core::fmt::Debug;
+use core::mem::MaybeUninit;
+
+const FIFO_SIZE: usize = 16;
+
+pub enum FifoError {
+    EmptyFifo,
+    NoChange,
+}
+
+pub struct Fifo<T: Copy + Clone> {
+    data: [MaybeUninit<T>; FIFO_SIZE],
+    size: usize,
+    avil: usize,
+    tail: usize,
+}
+
+impl<T: Copy + Clone> Fifo<T> {
+    pub fn new() -> Fifo<T> {
+        let data: [MaybeUninit<T>; FIFO_SIZE] = [const { MaybeUninit::uninit() }; FIFO_SIZE];
+        Fifo {
+            data,
+            size: FIFO_SIZE,
+            avil: 0,
+            tail: 0,
+        }
+    }
+    pub fn is_full(&self) -> bool {
+        return self.avil == self.size;
+    }
+    pub fn is_empty(&self) -> bool {
+        return self.avil == 0;
+    }
+
+    pub fn push(&mut self, new_element: T) -> Result<(), FifoError> {
+        if self.is_full() {
+            return Err(FifoError::NoChange);
+        }
+        self.data[self.tail].write(new_element);
+        self.tail += 1;
+        self.avil += 1;
+        if self.tail >= self.size {
+            self.tail -= self.size;
+        }
+
+        Ok(())
+    }
+
+    pub fn pop(&mut self) -> Result<T, FifoError> {
+        if self.is_empty() {
+            return Err(FifoError::EmptyFifo);
+        }
+        let raw_head = self.tail as isize - self.avil as isize;
+        let head = if raw_head < 0 {
+            raw_head + self.size as isize
+        } else {
+            raw_head
+        } as usize;
+
+        self.avil -= 1;
+        let result = unsafe { self.data[head].assume_init_ref() }.clone();
+        unsafe { self.data[head].assume_init_drop() }
+        Ok(result)
+    }
+}

prototyper/src/hart_context.rs

@@ -4,14 +4,15 @@ use fast_trap::FlowContext;
 use crate::hsm::HsmCell;
 use crate::rfence::RFenceCell;
 use crate::NextStage;
-
+use core::sync::atomic::AtomicU8;
 
 /// 硬件线程上下文。
 pub(crate) struct HartContext {
     /// 陷入上下文。
     trap: FlowContext,
     pub hsm: HsmCell<NextStage>,
-    pub rfence: RFenceCell
+    pub rfence: RFenceCell,
+    pub ipi_type: AtomicU8,
 }
 
 impl HartContext {

prototyper/src/main.rs

@@ -13,6 +13,7 @@ mod console;
 mod dt;
 mod dynamic;
 mod fail;
+mod fifo;
 mod hart_context;
 mod hsm;
 mod reset;

prototyper/src/rfence.rs

@@ -1,20 +1,21 @@
-use rustsbi::Ipi;
 use rustsbi::{HartMask, SbiRet};
 use spin::Mutex;
 
 use crate::board::SBI;
-use crate::hsm::remote_hsm;
+use crate::fifo::{Fifo, FifoError};
 use crate::riscv_spec::current_hartid;
-use crate::trap::msoft_rfence_handler;
 use crate::trap_stack::NUM_HART_MAX;
 use crate::trap_stack::ROOT_STACK;
 
+use core::sync::atomic::{AtomicU32, Ordering};
+
 pub(crate) struct RFenceCell {
-    inner: Mutex<Option<RFenceCTX>>,
+    queue: Mutex<Fifo<(RFenceCTX, usize)>>, // (ctx, source_hart_id)
+    wait_sync_count: AtomicU32,
 }
 
 #[repr(C)]
-#[derive(Clone, Copy)]
+#[derive(Clone, Copy, Debug)]
 pub(crate) struct RFenceCTX {
     pub start_addr: usize,
     pub size: usize,
@@ -38,7 +39,8 @@ pub(crate) enum RFenceType {
 impl RFenceCell {
     pub fn new() -> Self {
         Self {
-            inner: Mutex::new(None),
+            queue: Mutex::new(Fifo::new()),
+            wait_sync_count: AtomicU32::new(0),
         }
     }
 
@@ -85,21 +87,54 @@ pub(crate) fn remote_rfence(hart_id: usize) -> Option<RemoteRFenceCell<'static>>
 
 #[allow(unused)]
 impl LocalRFenceCell<'_> {
-    pub fn clear(&self) {
-        *self.0.inner.lock() = None;
+    pub fn is_zero(&self) -> bool {
+        self.0.wait_sync_count.load(Ordering::Relaxed) == 0
+    }
+    pub fn add(&self) {
+        self.0.wait_sync_count.fetch_add(1, Ordering::Relaxed);
+    }
+
+    pub fn is_empty(&self) -> bool {
+        self.0.queue.lock().is_empty()
     }
-    pub fn get(&self) -> Option<RFenceCTX> {
-        (*self.0.inner.lock())
+    pub fn get(&self) -> Option<(RFenceCTX, usize)> {
+        match self.0.queue.lock().pop() {
+            Ok(res) => Some(res),
+            Err(_) => None,
+        }
     }
     pub fn set(&self, ctx: RFenceCTX) {
-        *self.0.inner.lock() = Some(ctx);
+        loop {
+            match self.0.queue.lock().push((ctx, current_hartid())) {
+                Ok(_) => break (),
+                Err(FifoError::NoChange) => {
+                    crate::trap::rfence_signle_handler();
+                    continue;
+                }
+                _ => panic!("Unable to push fence ops to fifo"),
+            }
+        }
     }
 }
 
 #[allow(unused)]
 impl RemoteRFenceCell<'_> {
     pub fn set(&self, ctx: RFenceCTX) {
-        *self.0.inner.lock() = Some(ctx);
+        // TODO: maybe deadlock
+        loop {
+            match self.0.queue.lock().push((ctx, current_hartid())) {
+                Ok(_) => break (),
+                Err(FifoError::NoChange) => {
+                    crate::trap::rfence_signle_handler();
+                    continue;
+                }
+                _ => panic!("Unable to push fence ops to fifo"),
+            }
+        }
+    }
+
+    pub fn sub(&self) {
+        self.0.wait_sync_count.fetch_sub(1, Ordering::Relaxed);
     }
 }
 
@@ -107,24 +142,14 @@ impl RemoteRFenceCell<'_> {
 pub(crate) struct RFence;
 
 fn remote_fence_process(rfence_ctx: RFenceCTX, hart_mask: HartMask) -> SbiRet {
-    let mut hart_mask_hsm = hart_mask_clear(hart_mask, current_hartid());
-    for hart_id in 0..=NUM_HART_MAX {
-        if hart_mask.has_bit(hart_id) {
-            if remote_hsm(hart_id).unwrap().allow_ipi() {
-                remote_rfence(hart_id).unwrap().set(rfence_ctx);
-            } else {
-                hart_mask_hsm = hart_mask_clear(hart_mask_hsm, hart_id);
-            }
-        }
-    }
     let sbi_ret = unsafe { SBI.assume_init_mut() }
         .clint
         .as_ref()
         .unwrap()
-        .send_ipi(hart_mask_hsm);
+        .send_ipi_by_fence(hart_mask, rfence_ctx);
 
     if hart_mask.has_bit(current_hartid()) {
-        msoft_rfence_handler();
+        crate::trap::rfence_handler();
     }
 
     sbi_ret

prototyper/src/trap.rs

@@ -204,24 +204,25 @@ pub extern "C" fn msoft_hanlder(ctx: &mut SupervisorContext) {
         }
         Err(rustsbi::spec::hsm::HART_STOP) => {
             clint::clear_msip();
-            unsafe { mie::set_msoft(); }
+            unsafe {
+                mie::set_msoft();
+            }
             riscv::asm::wfi();
         }
         // RFence
         _ => {
-            msoft_rfence_handler();
+            msoft_ipi_handler();
         }
     }
 }
 
-pub fn msoft_rfence_handler() {
+pub fn rfence_signle_handler() {
     let rfence_context = local_rfence().get();
     match rfence_context {
-        Some(ctx) => match ctx.op {
+        Some((ctx, id)) => match ctx.op {
             RFenceType::FenceI => unsafe {
                 asm!("fence.i");
-                local_rfence().clear();
-                clint::clear_msip();
+                crate::rfence::remote_rfence(id).unwrap().sub();
             },
             RFenceType::SFenceVma => {
                 // If the flush size is greater than the maximum limit then simply flush all
@@ -229,15 +230,18 @@ pub fn msoft_rfence_handler() {
                     || (ctx.size == usize::MAX)
                     || (ctx.size > TLB_FLUSH_LIMIT)
                 {
-                    unsafe { asm!("sfence.vma"); }
+                    unsafe {
+                        asm!("sfence.vma");
+                    }
                 } else {
                     for offset in (0..ctx.size).step_by(PAGE_SIZE) {
                         let addr = ctx.start_addr + offset;
-                        unsafe { asm!("sfence.vma {}", in(reg) addr); }
+                        unsafe {
+                            asm!("sfence.vma {}", in(reg) addr);
+                        }
                     }
                 }
-                local_rfence().clear();
-                clint::clear_msip();
+                crate::rfence::remote_rfence(id).unwrap().sub();
             }
             RFenceType::SFenceVmaAsid => {
                 let asid = ctx.asid;
@@ -246,29 +250,45 @@ pub fn msoft_rfence_handler() {
                     || (ctx.size == usize::MAX)
                     || (ctx.size > TLB_FLUSH_LIMIT)
                 {
-                    unsafe { asm!("sfence.vma {}, {}", in(reg) 0, in(reg) asid); }
+                    unsafe {
+                        asm!("sfence.vma {}, {}", in(reg) 0, in(reg) asid);
+                    }
                 } else {
                     for offset in (0..ctx.size).step_by(PAGE_SIZE) {
                         let addr = ctx.start_addr + offset;
-                        unsafe { asm!("sfence.vma {}, {}", in(reg) addr, in(reg) asid); }
+                        unsafe {
+                            asm!("sfence.vma {}, {}", in(reg) addr, in(reg) asid);
+                        }
                     }
                 }
-                local_rfence().clear();
-                clint::clear_msip();
+                crate::rfence::remote_rfence(id).unwrap().sub();
             }
             rfencetype => {
                 error!("Unsupported RFence Type: {:?}!", rfencetype);
-                local_rfence().clear();
-                clint::clear_msip();
             }
         },
-        None => {
-            clint::clear_msip();
-            unsafe {
-                riscv::register::mip::set_ssoft();
-            }
+        None => {}
+    }
+}
+
+pub fn rfence_handler() {
+    while local_rfence().is_empty() == false {
+        rfence_signle_handler();
+    }
+}
+
+pub fn msoft_ipi_handler() {
+    use crate::clint::get_and_reset_ipi_type;
+    clint::clear_msip();
+    let ipi_type = get_and_reset_ipi_type();
+    if (ipi_type & crate::clint::IPI_TYPE_SSOFT) != 0 {
+        unsafe {
+            riscv::register::mip::set_ssoft();
         }
     }
+    if (ipi_type & crate::clint::IPI_TYPE_FENCE) != 0 {
+        rfence_handler();
+    }
 }
 
 /// Fast trap
@@ -324,11 +344,19 @@ pub extern "C" fn fast_handler(
             } else {
                 match a7 {
                     legacy::LEGACY_CONSOLE_PUTCHAR => {
-                        ret.error = unsafe { SBI.assume_init_ref() }.uart16550.as_ref().unwrap().putchar(ctx.a0());
+                        ret.error = unsafe { SBI.assume_init_ref() }
+                            .uart16550
+                            .as_ref()
+                            .unwrap()
+                            .putchar(ctx.a0());
                         ret.value = a1;
                     }
                     legacy::LEGACY_CONSOLE_GETCHAR => {
-                        ret.error = unsafe { SBI.assume_init_ref() }.uart16550.as_ref().unwrap().getchar();
+                        ret.error = unsafe { SBI.assume_init_ref() }
+                            .uart16550
+                            .as_ref()
+                            .unwrap()
+                            .getchar();
                         ret.value = a1;
                     }
                     _ => {}