Add should_trap signal to trap output data

diff --git a/src/soc/fu/trap/main_stage.py b/src/soc/fu/trap/main_stage.py
index e5771b173374c7e33970f1ccf203ec833986ff13..7fcdc1779bc7f22ff239e36fb28237218b08d0f3 100644 (file)
--- a/src/soc/fu/trap/main_stage.py
+++ b/src/soc/fu/trap/main_stage.py
@@ -78,9 +78,14 @@ class LogicalMainStage(PipeModBase):
             
         with m.Switch(op):
             with m.Case(InternalOp.OP_TRAP):
-                pass
+                with m.If(should_trap):
+                    comb += self.o.nia.eq(0x700)
+                    comb += self.o.srr1.eq(self.i.msr)
+                    comb += self.o.srr1[63-46].eq(1)
+                    comb += self.o.srr0.eq(self.i.cia)
 
 
         comb += self.o.ctx.eq(self.i.ctx)
+        comb += self.o.should_trap.eq(should_trap)
 
         return m

diff --git a/src/soc/fu/trap/pipe_data.py b/src/soc/fu/trap/pipe_data.py
index 42548952834c6949fd48e1c6b52c55f4f7856b4a..751a183583e6af7bbcf645c1c36b0f555b59bc86 100644 (file)
--- a/src/soc/fu/trap/pipe_data.py
+++ b/src/soc/fu/trap/pipe_data.py
@@ -30,6 +30,7 @@ class TrapOutputData(IntegerData):
         self.msr = Signal(64, reset_less=True) # RB/immediate
         self.srr0 = Signal(64, reset_less=True) # RB/immediate
         self.srr1 = Signal(64, reset_less=True) # RB/immediate
+        self.should_trap = Signal(reset_less=True)
 
     def __iter__(self):
         yield from super().__iter__()
@@ -37,9 +38,11 @@ class TrapOutputData(IntegerData):
         yield self.msr
         yield self.srr0
         yield self.srr1
+        yield self.should_trap
 
     def eq(self, i):
         lst = super().eq(i)
         return lst + [
             self.nia.eq(i.nia), self.msr.eq(i.msr),
-            self.srr0.eq(i.srr0), self.srr1.eq(i.srr1)]
+            self.srr0.eq(i.srr0), self.srr1.eq(i.srr1),
+            self.should_trap.eq(i.should_trap)]