add signal for resetting trap internal state (kaivb cache)

[soc.git] / src / soc / fu / trap / main_stage.py

diff --git a/src/soc/fu/trap/main_stage.py b/src/soc/fu/trap/main_stage.py
index 9eb85887d14db0d472426ed6a1c78fbb6a42b492..8127e226e34afaf5cf87489bb86fa04a39a544e1 100644 (file)
--- a/src/soc/fu/trap/main_stage.py
+++ b/src/soc/fu/trap/main_stage.py
@@ -58,6 +58,8 @@ class TrapMainStage(PipeModBase):
         super().__init__(pspec, "main")
         self.fields = DecodeFields(SignalBitRange, [self.i.ctx.op.insn])
         self.fields.create_specs()
+        self.kaivb = Signal(64) # KAIVB SPR
+        self.state_reset = Signal() # raise high to reset KAIVB cache
 
     def trap(self, m, trap_addr, return_addr):
         """trap.  sets new PC, stores MSR and old PC in SRR1 and SRR0
@@ -67,12 +69,15 @@ class TrapMainStage(PipeModBase):
         msr_i = op.msr
         svstate_i = op.svstate
 
-        srr1_i = self.i.srr1
+        exc = LDSTException("trapexc")
+        comb += exc.eq(op.ldst_exc)
+        srr1_i = exc.srr1 # new SRR1 bits come from exception
         nia_o = self.o.nia
         svsrr0_o, srr0_o, srr1_o = self.o.svsrr0, self.o.srr0, self.o.srr1
 
-        # trap address
+        # trap address, including KAIVB override
         comb += nia_o.data.eq(trap_addr)
+        comb += nia_o.data[13:].eq(self.kaivb[13:])
         comb += nia_o.ok.eq(1)
 
         # addr to begin from on return
@@ -137,7 +142,7 @@ class TrapMainStage(PipeModBase):
 
     def elaborate(self, platform):
         m = Module()
-        comb = m.d.comb
+        comb, sync = m.d.comb, m.d.sync
         op = self.i.ctx.op
 
         # convenience variables
@@ -149,6 +154,10 @@ class TrapMainStage(PipeModBase):
         srr0_o, srr1_o, svsrr0_o = self.o.srr0, self.o.srr1, self.o.svsrr0
         traptype, trapaddr = op.traptype, op.trapaddr
 
+        # hard reset of KAIVB
+        with m.If(self.state_reset):
+            sync += self.kaivb.eq(0)
+
         # take copy of D-Form TO field
         i_fields = self.fields.FormD
         to = Signal(i_fields.TO[0:-1].shape())
@@ -199,6 +208,16 @@ class TrapMainStage(PipeModBase):
         # TODO: some #defines for the bits n stuff.
         with m.Switch(op.insn_type):
 
+            ##############
+            # KAIVB https://bugs.libre-soc.org/show_bug.cgi?id=859
+
+            with m.Case(MicrOp.OP_MTSPR):
+                sync += self.kaivb.eq(a_i)
+
+            with m.Case(MicrOp.OP_MFSPR):
+                comb += o.data.eq(self.kaivb)
+                comb += o.ok.eq(1)
+
             ###############
             # TDI/TWI/TD/TW.  v3.0B p90-91
 
@@ -251,7 +270,7 @@ class TrapMainStage(PipeModBase):
                 # L => bit 16 in LSB0, bit 15 in MSB0 order
                 L = self.fields.FormX.L1[0:1] # X-Form field L1
                 # start with copy of msr
-                comb += msr_o.eq(msr_i)
+                comb += msr_o.data.eq(msr_i)
                 with m.If(L):
                     # just update RI..EE
                     comb += msr_o.data[MSR.RI].eq(a_i[MSR.RI])
@@ -319,12 +338,12 @@ class TrapMainStage(PipeModBase):
                 if False: # XXX no - not doing hypervisor yet
                     with m.If(~self.i.ctx.op.insn[9]): # XXX BAD HACK! (hrfid)
                         with m.If(field(msr_i, 3)): # HV
-                            comb += field(msr_o, 51).eq(field(srr1_i, 51)) # ME
+                            comb += field(msr_o.data, 51).eq(field(srr1_i, 51)) # ME
                         with m.Else():
-                            comb += field(msr_o, 51).eq(field(msr_i, 51)) # ME
+                            comb += field(msr_o.data, 51).eq(field(msr_i, 51)) # ME
                 else:
                     # same as microwatt: treat MSR.ME rfid same as hrfid
-                    comb += field(msr_o, 51).eq(field(srr1_i, 51)) # ME
+                    comb += field(msr_o.data, 51).eq(field(srr1_i, 51)) # ME
 
                 # check problem state: if set, not permitted to set EE,IR,DR
                 msr_check_pr(m, srr1_i, msr_o.data)