From 4490254d3d7b7c84a61596bf8f73864829342abe Mon Sep 17 00:00:00 2001
From: Dolu1990 <charles.papon.90@gmail.com>
Date: Fri, 28 May 2021 13:35:52 +0200
Subject: [PATCH] Csr/Mmu ensure implement that SFENCE_VMA flush the next
 instructions SAT flush reworked a bit too

---
 .../scala/vexriscv/plugin/CsrPlugin.scala     | 19 +++++++++++++++++--
 .../scala/vexriscv/plugin/MmuPlugin.scala     | 19 +++++++++----------
 2 files changed, 26 insertions(+), 12 deletions(-)

diff --git a/src/main/scala/vexriscv/plugin/CsrPlugin.scala b/src/main/scala/vexriscv/plugin/CsrPlugin.scala
index 413c6c4..badb18f 100644
--- a/src/main/scala/vexriscv/plugin/CsrPlugin.scala
+++ b/src/main/scala/vexriscv/plugin/CsrPlugin.scala
@@ -474,6 +474,7 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
 
   object ENV_CTRL extends Stageable(EnvCtrlEnum())
   object IS_CSR extends Stageable(Bool)
+  object IS_SFENCE_VMA extends Stageable(Bool)
   object CSR_WRITE_OPCODE extends Stageable(Bool)
   object CSR_READ_OPCODE extends Stageable(Bool)
   object PIPELINED_CSR_READ extends Stageable(Bits(32 bits))
@@ -606,6 +607,11 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
 
     if(withExternalMhartid) externalMhartId = in UInt(mhartidWidth bits)
     if(utimeAccess != CsrAccess.NONE) utime = in UInt(64 bits) setName("utime")
+
+    if(supervisorGen) {
+      decoderService.addDefault(IS_SFENCE_VMA, False)
+      decoderService.add(SFENCE_VMA, List(IS_SFENCE_VMA -> True))
+    }
   }
 
   def inhibateInterrupts() : Unit = allowInterrupts := False
@@ -783,10 +789,15 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
         satpAccess(CSR.SATP, 31 -> satp.MODE, 22 -> satp.ASID, 0 -> satp.PPN)
 
 
-        val satpLogic = supervisorGen generate new Area {
+        val rescheduleLogic = supervisorGen generate new Area {
           redoInterface.valid := False
           redoInterface.payload := decode.input(PC)
-          duringWrite(CSR.SATP) {
+
+          val rescheduleNext = False
+          when(execute.arbitration.isValid && execute.input(IS_SFENCE_VMA)) { rescheduleNext := True }
+          duringWrite(CSR.SATP) { rescheduleNext := True }
+
+          when(rescheduleNext){
             redoInterface.valid := True
             execute.arbitration.flushNext := True
             decode.arbitration.haltByOther := True
@@ -1155,8 +1166,12 @@ class CsrPlugin(val config: CsrPluginConfig) extends Plugin[VexRiscv] with Excep
 
         when(arbitration.isValid && input(IS_CSR)) {
           if(!pipelineCsrRead) output(REGFILE_WRITE_DATA) := readData
+        }
+
+        when(arbitration.isValid && (input(IS_CSR) || (if(supervisorGen) input(IS_SFENCE_VMA) else False))) {
           arbitration.haltItself setWhen(blockedBySideEffects)
         }
+
         if(pipelineCsrRead){
           insert(PIPELINED_CSR_READ) := readData
           when(memory.arbitration.isValid && memory.input(IS_CSR)) {
diff --git a/src/main/scala/vexriscv/plugin/MmuPlugin.scala b/src/main/scala/vexriscv/plugin/MmuPlugin.scala
index 3f6635e..093f59a 100644
--- a/src/main/scala/vexriscv/plugin/MmuPlugin.scala
+++ b/src/main/scala/vexriscv/plugin/MmuPlugin.scala
@@ -53,13 +53,13 @@ class MmuPlugin(ioRange : UInt => Bool,
     port.bus
   }
 
-  object IS_SFENCE_VMA extends Stageable(Bool)
+  object IS_SFENCE_VMA2 extends Stageable(Bool)
   override def setup(pipeline: VexRiscv): Unit = {
     import Riscv._
     import pipeline.config._
     val decoderService = pipeline.service(classOf[DecoderService])
-    decoderService.addDefault(IS_SFENCE_VMA, False)
-    decoderService.add(SFENCE_VMA, List(IS_SFENCE_VMA -> True))
+    decoderService.addDefault(IS_SFENCE_VMA2, False)
+    decoderService.add(SFENCE_VMA, List(IS_SFENCE_VMA2 -> True))
 
 
     dBusAccess = pipeline.service(classOf[DBusAccessService]).newDBusAccess()
@@ -296,18 +296,17 @@ class MmuPlugin(ioRange : UInt => Bool,
       }
     }
 
-    val fenceStage = stages.last
+    val fenceStage = execute
+
+    //Both SFENCE_VMA and SATP reschedule the next instruction in the CsrPlugin itself with one extra cycle to ensure side effect propagation.
     fenceStage plug new Area{
       import fenceStage._
-      when(arbitration.isValid && input(IS_SFENCE_VMA)){
-        for(port <- core.ports; line <- port.cache) line.valid := False //Assume that the instruction already fetched into the pipeline are ok
+      when(arbitration.isValid && arbitration.isFiring && input(IS_SFENCE_VMA2)){
+        for(port <- core.ports; line <- port.cache) line.valid := False
       }
 
-      csrService.duringWrite(CSR.SATP){
+      csrService.onWrite(CSR.SATP){
         for(port <- core.ports; line <- port.cache) line.valid := False
-        core.ports.filter(_.handle.args.earlyRequireMmuLockup).foreach{p =>
-          p.dirty := True
-        }
       }
     }
   }