From 10f4200e58562d6070203afdddea1dc0c0eb5f88 Mon Sep 17 00:00:00 2001
From: Luke Kenneth Casson Leighton <lkcl@lkcl.net>
Date: Sun, 26 Jun 2022 12:57:33 +0100
Subject: [PATCH] adapt TRAP function in main state pipeline to put KAIVB into
 top bits

---
 src/soc/fu/trap/main_stage.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/soc/fu/trap/main_stage.py b/src/soc/fu/trap/main_stage.py
index c47b396e..3b9c7bed 100644
--- 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
-- 
2.30.2