Add TrapMainStage.trap() convenience function to set trap address and PC

to begin from on return

diff --git a/src/soc/fu/trap/main_stage.py b/src/soc/fu/trap/main_stage.py
index 5f4b95c97ef383cc891b5f3046f792f44ef69e64..650dc8a8602327f847bce7e79ec885abc387e1f8 100644 (file)
--- a/src/soc/fu/trap/main_stage.py
+++ b/src/soc/fu/trap/main_stage.py
@@ -73,6 +73,17 @@ class TrapMainStage(PipeModBase):
         self.fields = DecodeFields(SignalBitRange, [self.i.ctx.op.insn])
         self.fields.create_specs()
 
+    def trap(self, m, addr, trap_addr):
+        comb  = m.d.comb
+        nia_o, srr0_o = self.o.nia, self.o.srr0
+
+        comb += nia_o.data.eq(trap_addr)
+        comb += nia_o.ok.eq(1)
+
+        comb += srr0_o.data.eq(addr) # addr to begin from on return
+        comb += srro_o.ok.eq(1)
+
+
     def ispec(self):
         return TrapInputData(self.pspec)