adapt TRAP function in main state pipeline to put KAIVB

into top bits

diff --git a/src/soc/fu/trap/main_stage.py b/src/soc/fu/trap/main_stage.py
index c47b396e7a6670a5de3ab3e773049c0136bd7b4d..3b9c7bed1a40da8d081a1bd0a87323f897462fce 100644 (file)
--- a/src/soc/fu/trap/main_stage.py
+++ b/src/soc/fu/trap/main_stage.py
@@ -74,8 +74,9 @@ class TrapMainStage(PipeModBase):
         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