From bfe190d35a271052568df0397ac592e86082f3c8 Mon Sep 17 00:00:00 2001
From: Dolu1990 <charles.papon.90@gmail.com>
Date: Wed, 10 Jan 2024 19:21:01 +0100
Subject: [PATCH] add trapNextOnWrite

---
 .../scala/vexiiriscv/execute/CsrAccessPlugin.scala    | 11 +++++++++++
 src/main/scala/vexiiriscv/execute/CsrService.scala    |  1 +
 src/main/scala/vexiiriscv/memory/MmuPlugin.scala      |  6 +++---
 3 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/src/main/scala/vexiiriscv/execute/CsrAccessPlugin.scala b/src/main/scala/vexiiriscv/execute/CsrAccessPlugin.scala
index 13060853..13481176 100644
--- a/src/main/scala/vexiiriscv/execute/CsrAccessPlugin.scala
+++ b/src/main/scala/vexiiriscv/execute/CsrAccessPlugin.scala
@@ -133,6 +133,17 @@ class CsrAccessPlugin(layer : LaneLayer,
 
     csrLock.await()
 
+    val trapNextOnWriteFilter = CsrListFilter(trapNextOnWrite.flatMap{
+      case e : CsrListFilter => e.mapping
+    }.toList)
+
+    onDecode(trapNextOnWriteFilter) {
+      when(onDecodeWrite) {
+        onDecodeTrap()
+        onDecodeTrapCode := TrapReason.NEXT
+      }
+    }
+
 //    val useRamRead = spec.exists(_.isInstanceOf[CsrRamSpec])
 //    val useRamWrite = spec.exists(_.isInstanceOf[CsrRamSpec])
 //    val useRam = spec.exists(_.isInstanceOf[CsrRamSpec])
diff --git a/src/main/scala/vexiiriscv/execute/CsrService.scala b/src/main/scala/vexiiriscv/execute/CsrService.scala
index 46b3085b..c4dc4b4b 100644
--- a/src/main/scala/vexiiriscv/execute/CsrService.scala
+++ b/src/main/scala/vexiiriscv/execute/CsrService.scala
@@ -31,6 +31,7 @@ trait CsrService {
   val onReadingHartIdMap = mutable.LinkedHashMap[Int, Bool]()
   val isReadingHartIdCsrMap = mutable.LinkedHashMap[(Int, Any), Bool]()
   val onWritingHartIdMap = mutable.LinkedHashMap[Int, Bool]()
+  val trapNextOnWrite = mutable.LinkedHashSet[Any]()
 
   def onDecode(csrFilter : Any, priority : Int = 0)(body : => Unit) = spec += CsrOnDecode(csrFilter, priority, () => body)
   def onDecodeException() : Unit
diff --git a/src/main/scala/vexiiriscv/memory/MmuPlugin.scala b/src/main/scala/vexiiriscv/memory/MmuPlugin.scala
index b7bc1333..95056d0d 100644
--- a/src/main/scala/vexiiriscv/memory/MmuPlugin.scala
+++ b/src/main/scala/vexiiriscv/memory/MmuPlugin.scala
@@ -9,7 +9,7 @@ import spinal.lib.misc.pipeline._
 import vexiiriscv._
 import Global._
 import spinal.lib.misc.pipeline.{NodeBaseApi, Payload}
-import vexiiriscv.execute.{CsrAccessPlugin, CsrRamService}
+import vexiiriscv.execute.{CsrAccessPlugin, CsrListFilter, CsrRamService}
 import vexiiriscv.memory.AddressTranslationPortUsage.LOAD_STORE
 import vexiiriscv.misc.{PipelineBuilderPlugin, PrivilegedPlugin, TrapReason}
 import vexiiriscv.riscv.CSR
@@ -197,13 +197,13 @@ class MmuPlugin(var spec : MmuSpec,
     csr.writeCancel(CSR.SATP, satpModeWrite =/= 0 && satpModeWrite =/= spec.satpMode)
     //    csr.readWriteRam(CSR.SATP) not suported by writeCancel
 
-    //TODO !!!! MISS SPEC : Changes to the sstatus fields SUM and MXR take effect immediately, without the need to execute an SFENCE.VMA instruction.
     csr.onDecode(CSR.SATP){
       csr.onDecodeTrap()
       csr.onDecodeTrapCode := TrapReason.SFENCE_VMA
-//      invalidatePort.cmd.valid := True
     }
 
+    csr.trapNextOnWrite += CsrListFilter(List(CSR.MSTATUS, CSR.SSTATUS))
+
     csrLock.release()
     portsLock.await()