bit more on TRAP handling (preparing priv instruction)

diff --git a/src/soc/decoder/power_decoder2.py b/src/soc/decoder/power_decoder2.py
index 5aca9d851a3f466e59c8b04eb5a477e4dd102ad7..7cb353b7a3ed9c7d2746363e4f71daa4af914d4f 100644 (file)
--- a/src/soc/decoder/power_decoder2.py
+++ b/src/soc/decoder/power_decoder2.py
@@ -26,7 +26,7 @@ TT_TRAP = 1<<2
 TT_ADDR = 1<<3
 
 
-def instr_is_privileged(m, op, insn):
+def instr_is_priv(m, op, insn):
     """determines if the instruction is privileged or not
     """
     comb = m.d.comb
@@ -656,15 +656,16 @@ class PowerDecode2(Elaboratable):
         comb += e.input_cr.eq(op.cr_in)   # condition reg comes in
         comb += e.output_cr.eq(op.cr_out) # condition reg goes in
 
-        with m.If(op.internal_op == InternalOp.OP_TRAP):
-            comb += e.traptype.eq(TT_TRAP) # request trap interrupt
-            comb += e.trapaddr.eq(0x70)    # addr=0x700 (strip first nibble)
-
         return m
 
         # privileged instruction
-        with m.If(instr_is_privileged(m, op.internal_op, e.insn) &
-                  msr[MSR_PR]):
+        with m.If(instr_is_priv(m, op.internal_op, e.insn) & msr[MSR_PR]):
+            # don't request registers RA/RT
+            comb += e.read_reg1.eq(0)
+            comb += e.read_reg2.eq(0)
+            comb += e.read_reg3.eq(0)
+            comb += e.write_reg.eq(0)
+            comb += e.write_ea.eq(0)
             # privileged instruction trap
             comb += op.internal_op.eq(InternalOp.OP_TRAP)
             comb += e.traptype.eq(TT_PRIV) # request privileged instruction

diff --git a/src/soc/fu/trap/main_stage.py b/src/soc/fu/trap/main_stage.py
index fe018d8f1de5ae69b1814d19346b3450392fd5e1..273b8e2003aa654f51b14ae315daf4e8fddd8d18 100644 (file)
--- a/src/soc/fu/trap/main_stage.py
+++ b/src/soc/fu/trap/main_stage.py
@@ -15,6 +15,7 @@ from soc.decoder.power_enums import InternalOp
 from soc.decoder.power_fields import DecodeFields
 from soc.decoder.power_fieldsn import SignalBitRange
 
+from soc.decoder.power_decoder2 import (TT_FP, TT_PRIV, TT_TRAP, TT_ADDR)
 
 # Listed in V3.0B Book III Chap 4.2.1
 # MSR bit numbers
@@ -115,6 +116,7 @@ class TrapMainStage(PipeModBase):
         a_i, b_i, cia_i, msr_i = self.i.a, self.i.b, self.i.cia, self.i.msr
         o, msr_o, nia_o = self.o.o, self.o.msr, self.o.nia
         srr0_o, srr1_o = self.o.srr0, self.o.srr1
+        traptype, trapaddr = op.traptype, op.trapaddr
 
         # take copy of D-Form TO field
         i_fields = self.fields.FormD
@@ -155,12 +157,13 @@ class TrapMainStage(PipeModBase):
 
         # They're in reverse bit order because POWER.
         # Check V3.0B Book 1, Appendix C.6 for chart
-        trap_bits = Signal(5)
+        trap_bits = Signal(5, reset_less=True)
         comb += trap_bits.eq(Cat(gt_u, lt_u, equal, gt_s, lt_s))
 
         # establish if the trap should go ahead (any tests requested in TO)
-        should_trap = Signal()
-        comb += should_trap.eq((trap_bits & to).any())
+        # or if traptype is set already
+        should_trap = Signal(reset_less=True)
+        comb += should_trap.eq((trap_bits & to).any() | traptype.any())
 
         # TODO: some #defines for the bits n stuff.
         with m.Switch(op):
@@ -169,9 +172,16 @@ class TrapMainStage(PipeModBase):
                 # trap instructions (tw, twi, td, tdi)
                 with m.If(should_trap):
                     # generate trap-type program interrupt
-                    self.trap(0x700, cia_i)
-                    # set bit 46 to say trap occurred (see 3.0B Book III 7.5.9)
-                    comb += srr1_o.data[PI_TRAP].eq(1)
+                    self.trap(trapaddr<<4, cia_i)
+                    with m.If(traptype == 0):
+                        # say trap occurred (see 3.0B Book III 7.5.9)
+                        comb += srr1_o.data[PI_TRAP].eq(1)
+                    with m.If(traptype & TT_PRIV):
+                        comb += srr1_o.data[PI_PRIV].eq(1)
+                    with m.If(traptype & TT_FP):
+                        comb += srr1_o.data[PI_FP].eq(1)
+                    with m.If(traptype & TT_ADDR):
+                        comb += srr1_o.data[PI_ADDR].eq(1)
 
             # move to MSR
             with m.Case(InternalOp.OP_MTMSR):